diff --git a/GraspPlanning/ApproachMovementGenerator.cpp b/GraspPlanning/ApproachMovementGenerator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fd0947bdd5cbac338c25b7054b47e762cab65c7e --- /dev/null +++ b/GraspPlanning/ApproachMovementGenerator.cpp @@ -0,0 +1,103 @@ +#include "ApproachMovementGenerator.h" +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/SceneObject.h> +#include <VirtualRobot/Visualization/TriMeshModel.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/EndEffector/EndEffector.h> +#include <VirtualRobot/MathTools.h> +#include <iostream> +using namespace std; + +namespace GraspStudio +{ + +ApproachMovementGenerator::ApproachMovementGenerator( VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef ) + : object(object), eef(eef) +{ + name = "ApproachMovementGenerator"; + THROW_VR_EXCEPTION_IF(!object, "NULL object?!"); + THROW_VR_EXCEPTION_IF(!object->getCollisionModel(), "No collision model for object " << object->getName()); + THROW_VR_EXCEPTION_IF(!eef, "NULL eef?!"); + THROW_VR_EXCEPTION_IF(!eef->getGCP(), "Need a GraspCenterPoint Node defined in EEF " << eef->getName()); + + objectModel = object->getCollisionModel()->getTriMeshModel(); + THROW_VR_EXCEPTION_IF(!objectModel, "NULL trimeshmodel of object " << object->getName()); + THROW_VR_EXCEPTION_IF(objectModel->faces.size() == 0, "no faces in trimeshmodel of object " << object->getName()); + + eefRobot = eef->createEefRobot(eef->getName(), eef->getName()); + THROW_VR_EXCEPTION_IF(!eefRobot, "Failed cloning EEF " << eef->getName()); + + + eef_cloned = eefRobot->getEndEffector(eef->getName()); + THROW_VR_EXCEPTION_IF(!eef_cloned, "No EEF with name " << eef->getName() << " in cloned robot?!"); + THROW_VR_EXCEPTION_IF(!eef_cloned->getGCP(), "No GCP in EEF with name " << eef->getName()); +} + +ApproachMovementGenerator::~ApproachMovementGenerator() +{ +} + + +VirtualRobot::RobotPtr ApproachMovementGenerator::getEEFRobotClone() +{ + return eefRobot; +} + +bool ApproachMovementGenerator::setEEFPose(const Eigen::Matrix4f &pose) +{ + eefRobot->setGlobalPoseForRobotNode(eef_cloned->getGCP(),pose); + return true; +} + +bool ApproachMovementGenerator::updateEEFPose(const Eigen::Vector3f &deltaPosition) +{ + Eigen::Matrix4f deltaPose; + deltaPose.setIdentity(); + deltaPose.block(0,3,3,1)=deltaPosition; + return updateEEFPose(deltaPose); +} + +bool ApproachMovementGenerator::updateEEFPose(const Eigen::Matrix4f &deltaPose) +{ + Eigen::Matrix4f pose = eef_cloned->getGCP()->getGlobalPose(); + pose = deltaPose * pose; + return setEEFPose(pose); +} + +Eigen::Matrix4f ApproachMovementGenerator::getEEFPose() +{ + return eef_cloned->getGCP()->getGlobalPose(); +} + +bool ApproachMovementGenerator::setEEFToRandomApproachPose() +{ + Eigen::Matrix4f pose = createNewApproachPose(); + return setEEFPose(pose); +} + +std::string ApproachMovementGenerator::getGCPJoint() +{ + return eef_cloned->getGCP()->getName(); +} + +VirtualRobot::SceneObjectPtr ApproachMovementGenerator::getObject() +{ + return object; +} + +VirtualRobot::EndEffectorPtr ApproachMovementGenerator::getEEF() +{ + return eef_cloned; +} + +VirtualRobot::EndEffectorPtr ApproachMovementGenerator::getEEFOriginal() +{ + return eef; +} + +std::string ApproachMovementGenerator::getName() +{ + return name; +} + +} diff --git a/GraspPlanning/ApproachMovementGenerator.h b/GraspPlanning/ApproachMovementGenerator.h new file mode 100644 index 0000000000000000000000000000000000000000..06aab8f291bdb9f657ba3dc3550f62c21d364cbe --- /dev/null +++ b/GraspPlanning/ApproachMovementGenerator.h @@ -0,0 +1,99 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __APPROACH_MOVEMENT_GENERATOR_H__ +#define __APPROACH_MOVEMENT_GENERATOR_H__ + +#include "GraspStudio.h" +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/Robot.h> +#include <vector> +#include <string> +#include <Eigen/Core> + +namespace GraspStudio +{ + +/*! +* +* An interface for generating approach movements of an end-effector toward an object resulting in grasp hypothesis. +*/ +class GRASPSTUDIO_IMPORT_EXPORT ApproachMovementGenerator +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + To generate approach movements an object and an end effector has to be specified. + Internally a clone of the EEF is used. + */ + ApproachMovementGenerator(VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef); + + //! destructor + virtual ~ApproachMovementGenerator(); + + //! Creates a new pose for approaching + virtual Eigen::Matrix4f createNewApproachPose() = 0; + + //! Applies a random grasp hypothesis to the cloned EEF + virtual bool setEEFToRandomApproachPose(); + + //! This robot is moved around + VirtualRobot::RobotPtr getEEFRobotClone(); + + //! move EEF to pose (uses coord system of GCP) + bool setEEFPose(const Eigen::Matrix4f &pose); + + //! update pose of EEF (GCP) + bool updateEEFPose(const Eigen::Matrix4f &deltaPose); + bool updateEEFPose(const Eigen::Vector3f &deltaPosition); + + //! get pose of EEF (coord system of GCP) + Eigen::Matrix4f getEEFPose(); + + std::string getGCPJoint(); + + VirtualRobot::SceneObjectPtr getObject(); + + /*! + This is the cloned eef! + */ + VirtualRobot::EndEffectorPtr getEEF(); + VirtualRobot::EndEffectorPtr getEEFOriginal(); + + std::string getName(); +protected: + + VirtualRobot::SceneObjectPtr object; + VirtualRobot::TriMeshModelPtr objectModel; + VirtualRobot::EndEffectorPtr eef; + + //! This robot is moved around + VirtualRobot::RobotPtr eefRobot; + VirtualRobot::EndEffectorPtr eef_cloned; + + std::string name; +}; + +} + +#endif /* __APPROACH_MOVEMENT_GENERATOR_H__ */ diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.cpp b/GraspPlanning/ApproachMovementSurfaceNormal.cpp new file mode 100644 index 0000000000000000000000000000000000000000..55e7d559380510abade7ac78c14e94248f091d1e --- /dev/null +++ b/GraspPlanning/ApproachMovementSurfaceNormal.cpp @@ -0,0 +1,155 @@ +#include "ApproachMovementSurfaceNormal.h" +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/EndEffector/EndEffector.h> +#include <VirtualRobot/MathTools.h> +#include <VirtualRobot/SceneObjectSet.h> +#include <VirtualRobot/Visualization/TriMeshModel.h> +#include <VirtualRobot/CollisionDetection/CollisionChecker.h> + +#include <cstdio> +#include <cstring> +#include <iostream> + +using namespace std; + +namespace GraspStudio +{ + +ApproachMovementSurfaceNormal::ApproachMovementSurfaceNormal( VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef ) +:ApproachMovementGenerator( object, eef ) +{ + name = "ApproachMovementSurfaceNormal"; +} + +ApproachMovementSurfaceNormal::~ApproachMovementSurfaceNormal() +{ +} + +bool ApproachMovementSurfaceNormal::getPositionOnObject(Eigen::Vector3f &storePos, Eigen::Vector3f& storeApproachDir) +{ + if (!object || objectModel->faces.size()==0) + return false; + int nRandFace = rand() % objectModel->faces.size(); + int nVert1 = (objectModel->faces[nRandFace]).id1; + int nVert2 = (objectModel->faces[nRandFace]).id2; + int nVert3 = (objectModel->faces[nRandFace]).id3; + + storePos = VirtualRobot::MathTools::randomPointInTriangle(objectModel->vertices[nVert1],objectModel->vertices[nVert2],objectModel->vertices[nVert3]); + + //storePos = (objectModel->vertices[nVert1] + objectModel->vertices[nVert2] + objectModel->vertices[nVert3]) / 3.0f; + /*position(0) = (objectModel->vertices[nVert1].x + objectModel->vertices[nVert2].x + objectModel->vertices[nVert3].x) / 3.0f; + position(1) = (objectModel->vertices[nVert1].y + objectModel->vertices[nVert2].y + objectModel->vertices[nVert3].y) / 3.0f; + position(2) = (objectModel->vertices[nVert1].z + objectModel->vertices[nVert2].z + objectModel->vertices[nVert3].z) / 3.0f;*/ + + storeApproachDir = (objectModel->faces[nRandFace]).normal; + return true; +} + +Eigen::Matrix4f ApproachMovementSurfaceNormal::createNewApproachPose() +{ + // store current pose + Eigen::Matrix4f pose = getEEFPose(); + openHand(); + Eigen::Vector3f position; + Eigen::Vector3f approachDir; + if (!getPositionOnObject(position,approachDir)) + { + GRASPSTUDIO_ERROR << "no position on object?!" << endl; + return pose; + } + + + // set new pose + setEEFToApproachPose(position,approachDir); + + // move away until valid + moveEEFAway(approachDir,3.0f); + Eigen::Matrix4f poseB = getEEFPose(); + + // restore original pose + setEEFPose(pose); + + return poseB; +} + +bool ApproachMovementSurfaceNormal::setEEFToApproachPose(const Eigen::Vector3f &position, const Eigen::Vector3f &approachDir) +{ + VirtualRobot::RobotNodePtr graspNode = eef_cloned->getGCP(); + + // current pose + Eigen::Matrix4f pose = graspNode->getGlobalPose(); + + // target pose + Eigen::Matrix4f poseFinal = Eigen::Matrix4f::Identity(); + + // position + poseFinal.block(0,3,3,1) = position; + + //target orientation + Eigen::Vector3f z = approachDir; + while (z.norm()<1e-10) + z.setRandom(); + z.normalize(); + z *= -1.0f; + + Eigen::Vector3f y; + Eigen::Vector3f x; + + // create a random rotation around approach vector + bool bSuccess = false; + while (!bSuccess) + { + //random y dir vector + y.setRandom(); + if (y.norm()<1e-8) + continue; + y.normalize(); + + x = y.cross(z); + if (x.norm()<1e-8) + continue; + x.normalize(); + + + // now recalculate y again to obtain a correct base for a right handed coord system + y = z.cross(x); + if (y.norm()<1e-8) + continue; + y.normalize(); + + bSuccess = true; + } + poseFinal.block(0,0,3,1) = x; + poseFinal.block(0,1,3,1) = y; + poseFinal.block(0,2,3,1) = z; + + setEEFPose(poseFinal); + return true; +} + + +void ApproachMovementSurfaceNormal::openHand() +{ + if (eef_cloned) + { + eef_cloned->openActors(); + } +} + +void ApproachMovementSurfaceNormal::moveEEFAway(const Eigen::Vector3f &approachDir, float step, int maxLoops) +{ + VirtualRobot::SceneObjectSetPtr sos = eef_cloned->createSceneObjectSet(); + if (!sos) + return; + int loop = 0; + Eigen::Vector3f delta = approachDir * step; + while (loop < maxLoops && eef_cloned->getCollisionChecker()->checkCollision(object->getCollisionModel(),sos)) + { + updateEEFPose(delta); + loop++; + } +} + +} diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.h b/GraspPlanning/ApproachMovementSurfaceNormal.h new file mode 100644 index 0000000000000000000000000000000000000000..3ef8b4ea5c527fda32db12b130ba0a0bb8c856c9 --- /dev/null +++ b/GraspPlanning/ApproachMovementSurfaceNormal.h @@ -0,0 +1,73 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __APPROACH_MOVEMENT_GENERATOR_SURFACE_NORMAL_H__ +#define __APPROACH_MOVEMENT_GENERATOR_SURFACE_NORMAL_H__ + +#include "GraspStudio.h" +#include "ApproachMovementGenerator.h" +#include <VirtualRobot/SceneObject.h> +#include <vector> + +namespace GraspStudio +{ + /*! + * + * + * This class generates grasping configs by sampling a random surface position of the object and setting the EEF to a surface normal aligned position. + * The remaining free DoF (the rotation around the normal) is set randomly. Then the EEF is moved along the normal until + * a collision is detected or the GCP hits the object. + * If needed, the EEF is moved back until a collision-free pose is found. + * + * Internally the EEF is cloned. + * + */ +class GRASPSTUDIO_IMPORT_EXPORT ApproachMovementSurfaceNormal : public ApproachMovementGenerator +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + To generate approach movements an object and an end effector has to be specified. + */ + ApproachMovementSurfaceNormal(VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef); + //! destructor + virtual ~ApproachMovementSurfaceNormal(); + + //! Creates a new pose for approaching + virtual Eigen::Matrix4f createNewApproachPose(); + + //! Returns a position with normal on the surface of the object + bool getPositionOnObject(Eigen::Vector3f &storePos, Eigen::Vector3f& storeApproachDir); + + //! Sets EEF to a position so that the Z component of the GCP coord system is aligned with -approachDir + bool setEEFToApproachPose(const Eigen::Vector3f &position, const Eigen::Vector3f &approachDir); + + void moveEEFAway(const Eigen::Vector3f &approachDir, float step, int maxLoops = 1000); + +protected: + + void openHand(); +}; +} + +#endif /* __APPROACH_MOVEMENT_GENERATOR_SURFACE_NORMAL_H__ */ diff --git a/GraspPlanning/CMakeLists.txt b/GraspPlanning/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..0cdb23e57a0c8101b28fd09dc8a0d9edfc5f7ffa --- /dev/null +++ b/GraspPlanning/CMakeLists.txt @@ -0,0 +1,76 @@ +PROJECT ( GraspStudio ) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.2) +CMAKE_POLICY(VERSION 2.6) + +MESSAGE (STATUS "\n ***** CONFIGURING Simox project GraspStudio *****") +INCLUDE (config.cmake) + +########### QHULL ################# +OPTION(BUILD_qhull "if on builds qhull external library" ON) +IF(BUILD_qhull) + ADD_SUBDIRECTORY(ExternalDependencies/qhull-2003.1) +ENDIF(BUILD_qhull) +########### END QHULL ################# + +SET(SOURCES +ConvexHullGenerator.cpp +ContactConeGenerator.cpp +ApproachMovementGenerator.cpp +ApproachMovementSurfaceNormal.cpp +GraspPlanner/GraspPlanner.cpp +GraspPlanner/GenericGraspPlanner.cpp +GraspQuality/GraspQualityMeasure.cpp +GraspQuality/GraspQualityMeasureWrenchSpace.cpp +Visualization/ConvexHullVisualization.cpp +) + +SET(INCLUDES +GraspStudio.h +ConvexHullGenerator.h +ContactConeGenerator.h +ApproachMovementGenerator.h +ApproachMovementSurfaceNormal.h +GraspPlanner/GraspPlanner.h +GraspPlanner/GenericGraspPlanner.h +GraspQuality/GraspQualityMeasure.h +GraspQuality/GraspQualityMeasureWrenchSpace.h +Visualization/ConvexHullVisualization.h +${GRASPSTUDIO_SimoxDir}/VirtualRobot/definesVR.h +) + +if (SIMOX_USE_COIN_VISUALIZATION) + SET(SOURCES + ${SOURCES} + Visualization/CoinVisualization/CoinConvexHullVisualization.cpp + ) + + SET(INCLUDES + ${INCLUDES} + Visualization/CoinVisualization/CoinConvexHullVisualization.h + ) +endif (SIMOX_USE_COIN_VISUALIZATION) + + +INCLUDE_DIRECTORIES(${GRASPSTUDIO_VirtualRobotDir}/..) +INCLUDE_DIRECTORIES(${GRASPSTUDIO_DIR}) +INCLUDE_DIRECTORIES(${GRASPSTUDIO_DIR}/ExternalDependencies/qhull-2003.1/include) +LINK_DIRECTORIES (${VR_LIB_DIR}) + +ADD_LIBRARY (GraspStudio SHARED ${SOURCES} ${INCLUDES}) +#MESSAGE("VIRTUAL_ROBOT_ROBOT_LINK_LIBRARIES:" ${VIRTUAL_ROBOT_LINK_LIBRARIES}) +TARGET_LINK_LIBRARIES (GraspStudio ${COLLISIONDETECTION_LIB} ${VIRTUAL_ROBOT_LINK_LIBRARIES} VirtualRobot qhull) + +# include examples +ADD_SUBDIRECTORY(examples/) + +# include unit tests +#ADD_SUBDIRECTORY(tests/) + + +# .DLL path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${GRASPSTUDIO_BIN_DIR}) +# .so path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${GRASPSTUDIO_LIB_DIR}) +# .lib path (this is needed for setting the DLL-import library path on windows) +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${GRASPSTUDIO_LIB_DIR}) diff --git a/GraspPlanning/ContactConeGenerator.cpp b/GraspPlanning/ContactConeGenerator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8d7f1f3e9c368287d019528015c523e4c46c6484 --- /dev/null +++ b/GraspPlanning/ContactConeGenerator.cpp @@ -0,0 +1,144 @@ +// ************************************************************** +// Implementation of class ContactConeGenerator +// ************************************************************** +// Author: Niko Vahrenkamp, Martin Do +// Date: 27.10.2011 +// ************************************************************** + + +// ************************************************************** +// includes +// ************************************************************** + +#include "ContactConeGenerator.h" +#include <math.h> +#include <stdio.h> +#include <assert.h> +#include <iostream> +#include <sstream> +#include <fstream> +#include <string> + +using namespace std; +using namespace VirtualRobot; + +namespace GraspStudio +{ + +ContactConeGenerator::ContactConeGenerator(int coneSamples, float frictionCoeff, float unitForce) +{ + + this->unitForce = unitForce; + //Generation of generic friction cone discretized by an 8-sided polyhedron + this->frictionCoeff = frictionCoeff; + this->frictionConeAngle = atan(frictionCoeff); + this->frictionConeRad = 2*unitForce*sin((unitForce*2*frictionConeAngle) / (2*unitForce)); + this->frictionConeSamples = coneSamples; + for (int i = 0; i < frictionConeSamples; i++) + { + Eigen::Vector3f p; + p(0) = unitForce*(float)(cos(frictionConeAngle)*cos(i*2.0*M_PI/frictionConeSamples)); + p(1) = unitForce*(float)(cos(frictionConeAngle)*sin(i*2.0*M_PI/frictionConeSamples)); + p(2) = unitForce*(float)cos(frictionConeAngle); + frictionConeRimPoints.push_back(p); + } +} + +ContactConeGenerator::~ContactConeGenerator() +{ + frictionConeRimPoints.clear(); +} + + + +void ContactConeGenerator::computeConePoints( const VirtualRobot::MathTools::ContactPoint &point, std::vector<VirtualRobot::MathTools::ContactPoint> &storeConePoints ) +{ + //Rotate generic friction cone to align with object normals + + Eigen::Vector3f upRightNormal(0.0f,0.0f,1.0f); + MathTools::Quaternion objNormalRot = MathTools::getRotation(upRightNormal,point.n); + Eigen::Matrix4f objNormalTrafo = MathTools::quat2eigen4x4(objNormalRot); + + Eigen::Vector3f conePoint; + + /*SbVec3f conePoint; + SbVec3f objPoint(point.p(0),point.p(1),point.p(2)); + SbVec3f objNormal(point.n(0),point.n(1),point.n(2)); + SbRotation objNormalRot(objNormal,SbVec3f(0.0,0.0,1.0)); + SbMatrix objNormalTrafo; + objNormalTrafo.setRotate(objNormalRot);*/ + + float scaleFactor = 1.0f; + for (int i = 0; i < frictionConeSamples; i++) + { + VirtualRobot::MathTools::ContactPoint newConePoint; + Eigen::Vector3f conePointOrg = frictionConeRimPoints[i]*scaleFactor; + conePoint = MathTools::transformPosition(conePointOrg,objNormalTrafo); + + //SbVec3f conePointOrg((float)frictionConeRimPoints[i][0]*scaleFactor,(float)frictionConeRimPoints[i][1]*scaleFactor,(float)frictionConeRimPoints[i][2]*scaleFactor); + //objNormalTrafo.multMatrixVec(conePointOrg, conePoint); + newConePoint.p = conePoint + point.p; + newConePoint.n = conePoint; + newConePoint.n.normalize(); + + /*newConePoint.p(0) = conePoint[0]+objPoint[0]; + newConePoint.p(1) = conePoint[1]+objPoint[1]; + newConePoint.p(2) = conePoint[2]+objPoint[2]; + newConePoint.n(0) = conePoint[0]; + newConePoint.n(1) = conePoint[1]; + newConePoint.n(2) = conePoint[2];*/ + /*float l = sqrtf(conePoint[0]*conePoint[0] + conePoint[1]*conePoint[1] + conePoint[2]*conePoint[2]); + if (l>=1e-8) + { + l = 1.0f / l; + newConePoint.n(0) *= l; + newConePoint.n(1) *= l; + newConePoint.n(2) *= l; + }*/ + storeConePoints.push_back(newConePoint); + } +} + +void ContactConeGenerator::computeConePoints(const VirtualRobot::MathTools::ContactPoint &point, std::vector<Eigen::Vector3f> &storeConePoints) +{ + + //Rotate generic friction cone to align with object normals + + Eigen::Vector3f upRightNormal(0.0f,0.0f,1.0f); + MathTools::Quaternion objNormalRot = MathTools::getRotation(upRightNormal,point.n); // invert?! + Eigen::Matrix4f objNormalTrafo = MathTools::quat2eigen4x4(objNormalRot); + + Eigen::Vector3f conePoint; + + float scaleFactor = 1.0f; + for (int i = 0; i < frictionConeSamples; i++) + { + Eigen::Vector3f newConePoint; + Eigen::Vector3f conePointOrg = frictionConeRimPoints[i]*scaleFactor; + conePoint = MathTools::transformPosition(conePointOrg,objNormalTrafo); + newConePoint = conePoint + point.p; + storeConePoints.push_back(newConePoint); + } + /* + //Rotate generic friction cone to align with object normals + SbVec3f conePoint; + SbVec3f objPoint(point.p(0),point.p(1),point.p(2)); + SbVec3f objNormal(point.n(0),point.n(1),point.n(2)); + SbRotation objNormalRot(objNormal,SbVec3f(0.0,0.0,1.0)); + SbMatrix objNormalTrafo; + objNormalTrafo.setRotate(objNormalRot); + float scaleFactor = 1.0f; + for (int i = 0; i < frictionConeSamples; i++) + { + GraspStudio::Vec3D newConePoint; + SbVec3f conePointOrg((float)frictionConeRimPoints[i][0]*scaleFactor,(float)frictionConeRimPoints[i][1]*scaleFactor,(float)frictionConeRimPoints[i][2]*scaleFactor); + objNormalTrafo.multMatrixVec(conePointOrg, conePoint); + newConePoint.p(0) = conePoint[0]+objPoint[0]; + newConePoint.p(1) = conePoint[1]+objPoint[1]; + newConePoint.p(2) = conePoint[2]+objPoint[2]; + storeConePoints.push_back(newConePoint); + }*/ +} + +} // namespace + diff --git a/GraspPlanning/ContactConeGenerator.h b/GraspPlanning/ContactConeGenerator.h new file mode 100644 index 0000000000000000000000000000000000000000..fb7e140a424a3f510b2752df76180341c57b2b8e --- /dev/null +++ b/GraspPlanning/ContactConeGenerator.h @@ -0,0 +1,68 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __CONTACT_CONE_GENARTOR_H__ +#define __CONTACT_CONE_GENARTOR_H__ + +#include "GraspStudio.h" +#include <VirtualRobot/MathTools.h> +#include <vector> +#include <Eigen/Core> + +namespace GraspStudio +{ +/*! + \brief Creates approximated representations of contact cones. +*/ +class GRASPSTUDIO_IMPORT_EXPORT ContactConeGenerator +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + //! constructor + ContactConeGenerator(int coneSamples = 8, float frictionCoeff = 0.25f, float unitForce = 1.0f); + + //! destructor + ~ContactConeGenerator(); + + /*! + Computes the cone with normals. coneSamples computed points are appended to storeConePoints. + */ + void computeConePoints(const VirtualRobot::MathTools::ContactPoint &point, std::vector<VirtualRobot::MathTools::ContactPoint> &storeConePoints); + + //! Computes the cone points without normals. coneSamples computed points are appended to storeConePoints. + void computeConePoints(const VirtualRobot::MathTools::ContactPoint &point, std::vector<Eigen::Vector3f> &storeConePoints); + +private: + + //Friction cone relevant parameters + double unitForce; + double frictionCoeff; + double frictionConeAngle; + double frictionConeRad; + std::vector< Eigen::Vector3f > frictionConeRimPoints; + int frictionConeSamples; + +}; +} + +#endif /* __CONTACT_CONE_GENARTOR_H__ */ diff --git a/GraspPlanning/ConvexHullGenerator.cpp b/GraspPlanning/ConvexHullGenerator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..46b4946282b1edf5df42a43b216e51dbd9056c01 --- /dev/null +++ b/GraspPlanning/ConvexHullGenerator.cpp @@ -0,0 +1,769 @@ + +#include "ConvexHullGenerator.h" +#include <VirtualRobot/Visualization/TriMeshModel.h> +#include <math.h> +#include <iostream> +#include <float.h> + +//#define CONVEXHULL_DEBUG_OUTPUT + +using namespace std; +using namespace VirtualRobot; +using namespace VirtualRobot::MathTools; + +extern "C" +{ +#include <qhull/qhull_a.h> +} +namespace GraspStudio +{ +boost::mutex ConvexHullGenerator::qhull_mutex; + +bool ConvexHullGenerator::ConvertPoints(std::vector<Eigen::Vector3f> &points, double *storePointsQHull, bool lockMutex) +{ + if (lockMutex) + qhull_mutex.lock(); + for (int i=0;i<(int)points.size();i++) + { + storePointsQHull[i*3+0] = points[i][0]; + storePointsQHull[i*3+1] = points[i][1]; + storePointsQHull[i*3+2] = points[i][2]; + } + if (lockMutex) + qhull_mutex.unlock(); + return true; +} + +bool ConvexHullGenerator::ConvertPoints(std::vector<ContactPoint> &points, double *storePointsQHull, bool lockMutex) +{ + if (lockMutex) + qhull_mutex.lock(); + for (int i=0;i<(int)points.size();i++) + { + storePointsQHull[i*6+0] = points[i].p[0]; + storePointsQHull[i*6+1] = points[i].p[1]; + storePointsQHull[i*6+2] = points[i].p[2]; + storePointsQHull[i*6+3] = points[i].n[0]; + storePointsQHull[i*6+4] = points[i].n[1]; + storePointsQHull[i*6+5] = points[i].n[2]; + } + if (lockMutex) + qhull_mutex.unlock(); + return true; +} + + +VirtualRobot::MathTools::ConvexHull3DPtr ConvexHullGenerator::CreateConvexHull( VirtualRobot::TriMeshModelPtr pointsInput, bool lockMutex /*= true*/ ) +{ + if (lockMutex) + qhull_mutex.lock(); + VirtualRobot::MathTools::ConvexHull3DPtr r = CreateConvexHull(pointsInput->vertices,false); + if (lockMutex) + qhull_mutex.unlock(); + return r; +} + + + +VirtualRobot::MathTools::ConvexHull3DPtr ConvexHullGenerator::CreateConvexHull( std::vector<Eigen::Vector3f> &pointsInput, bool lockMutex /*= true*/ ) +{ + if (lockMutex) + qhull_mutex.lock(); + clock_t startT = clock(); + + ConvexHull3DPtr result(new ConvexHull3D()); + result->faces.clear(); + result->vertices.clear(); + int nPoints = (int)pointsInput.size(); + if (nPoints<4) + { + cout << __FUNCTION__ << "Error: Need at least 4 points (nr of points registered: " << nPoints << ")" << endl; + if (lockMutex) + qhull_mutex.unlock(); + return result; + } + int dim=3; /* dimension of points */ + int numpoints = nPoints; /* number of points */ + coordT *points = new coordT[(3)*nPoints]; /* array of coordinates for each point */ + boolT ismalloc= False; /* True if qhull should free points in qh_freeqhull() or reallocation */ + char flags[250]; /* option flags for qhull, see qh_opt.htm */ +#ifdef CONVEXHULL_DEBUG_OUTPUT + FILE *outfile= stdout; /* output from qh_produce_output()*/ + /* use NULL to skip qh_produce_output() */ +# else + FILE *outfile= NULL; /* output from qh_produce_output()*/ + /* use NULL to skip qh_produce_output() */ +#endif + FILE *errfile= stderr; /* error messages from qhull code */ + int exitcode; /* 0 if no error from qhull */ + facetT *facet; /* set by FORALLfacets */ + int curlong, totlong; /* memory remaining after qh_memfreeshort */ + vertexT *vertex, **vertexp; + + // qhull options: + // Tv: test result + // Tcv: test result + // Qt: triangulate result (not really?!) + // QJ: juggling input points, triangulated result + // FA: compute volume +#ifdef CONVEXHULL_DEBUG_OUTPUT + sprintf (flags, "qhull s Tcv Qt FA"); +#else + sprintf (flags, "qhull s Qt FA"); // QJ is faster than Qt (but the results seem to be less accurate, see www.qhull.org documentation) +#endif + + //cout << "QHULL input: nVertices: " << pointsInput.size() << endl; + ConvertPoints (pointsInput, points, false); + /*for (i=numpoints; i--; ) + rows[i]= points+dim*i; + qh_printmatrix (outfile, "input", rows, numpoints, dim);*/ + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, flags, outfile, errfile); + if (!exitcode) { /* if no error */ + facetT *facet_list = qh facet_list; + int convexNumFaces = qh num_facets; + int convexNumVert = qh_setsize(qh_facetvertices (facet_list, NULL, false)); + + qh_triangulate(); // need this for triangulated output! + int convexNumFaces2 = qh num_facets; + int convexNumVert2 = qh_setsize(qh_facetvertices (facet_list, NULL, false)); + /* + cout << "Numfacets1:" << convexNumFaces << endl; + cout << "Numvertices1:" << convexNumVert << endl; + cout << "Numfacets2:" << convexNumFaces2 << endl; + cout << "Numvertices2:" << convexNumVert2 << endl;*/ + /* 'qh facet_list' contains the convex hull */ + Eigen::Vector3f v[3]; + int nIds[3]; + TriangleFace f; + + int nFacets = 0; + //cout << "Volume: " << qh totvol << endl; + qh_getarea(qh facet_list); + //cout << "Volume: " << qh totvol << endl; + result->volume = qh totvol; + + FORALLfacets + { + + int c = 0; +#ifdef CONVEXHULL_DEBUG_OUTPUT + cout << "FACET " << nFacets << ":" << endl; +#endif + + FOREACHvertex_(facet->vertices) + { +#ifdef CONVEXHULL_DEBUG_OUTPUT + cout << vertex->point[0] << "," << vertex->point[1] << "," << vertex->point[2] << endl; +#endif + if (c<3) + { + v[c][0] = vertex->point[0]; + v[c][1] = vertex->point[1]; + v[c][2] = vertex->point[2]; + result->vertices.push_back(v[c]); + nIds[c] = (int)result->vertices.size() - 1; + c++; + } else + { + cout << __FUNCTION__ << ": Error, facet with more than 3 vertices not supported ... face nr:" << nFacets << endl; + } + } + f.id1 = nIds[0]; + f.id2 = nIds[1]; + f.id3 = nIds[2]; + f.normal[0] = facet->normal[0]; + f.normal[1] = facet->normal[1]; + f.normal[2] = facet->normal[2]; + //cout << "Normal: " << f.m_Normal.x << "," << f.m_Normal.y << "," << f.m_Normal.z << endl; + result->faces.push_back(f); + nFacets++; + } + /* + cout << "QHULL result: nVertices: " << storeResult.vertices.size() << endl; + cout << "QHULL result: nFactes: " << nFacets << endl; + */ + } + qh_freeqhull(!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (errfile, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); + + clock_t endT = clock(); + long timeMS = (long)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0); + //cout << __FUNCTION__ << ": Created convex hull in " << timeMS << " ms" << endl; + if (lockMutex) + qhull_mutex.unlock(); + return result; +} + +VirtualRobot::MathTools::ConvexHull6DPtr ConvexHullGenerator::CreateConvexHull(std::vector<ContactPoint> &pointsInput, bool lockMutex) +{ + if (lockMutex) + qhull_mutex.lock(); + clock_t startT = clock(); + + ConvexHull6DPtr result(new ConvexHull6D()); + + int nPoints = (int)pointsInput.size(); + if (nPoints<4) + { + cout << __FUNCTION__ << "Error: Need at least 4 points (nr of points registered: " << nPoints << ")" << endl; + if (lockMutex) + qhull_mutex.unlock(); + return result; + } + int dim=6; /* dimension of points */ + int numpoints = nPoints; /* number of points */ + coordT *points = new coordT[(6)*nPoints]; /* array of coordinates for each point */ + boolT ismalloc= False; /* True if qhull should free points in qh_freeqhull() or reallocation */ + char flags[250]; /* option flags for qhull, see qh_opt.htm */ +#ifdef CONVEXHULL_DEBUG_OUTPUT + FILE *outfile= stdout; /* output from qh_produce_output()*/ + /* use NULL to skip qh_produce_output() */ +# else + FILE *outfile= NULL; /* output from qh_produce_output()*/ + /* use NULL to skip qh_produce_output() */ +#endif + FILE *errfile= stderr; /* error messages from qhull code */ + int exitcode; /* 0 if no error from qhull */ + facetT *facet; /* set by FORALLfacets */ + int curlong, totlong; /* memory remaining after qh_memfreeshort */ + vertexT *vertex, **vertexp; + + // qhull options: + // Tv: test result + // Tcv: test result + // Qt: triangulated result? -> no, we need to triangulate the result with qh_triangulate() + // QJ: juggling input points, triangulated result (avoids errors with coplanar facets etc) + // FA: compute volume +#ifdef CONVEXHULL_DEBUG_OUTPUT + //sprintf (flags, "qhull s Tcv Qt FA"); + sprintf (flags, "qhull s QJ FA"); + std::cout.precision(2); +#else + //sprintf (flags, "qhull s Qt FA"); // some of the resulting facets are not triangulated ?! + sprintf (flags, "qhull QJ FA"); // QJ is faster than Qt (but the results seem to be less accurate, see www.qhull.org documentation) +#endif + + //cout << "QHULL input: nVertices: " << pointsInput.size() << endl; + //printVertices(pointsInput); + ConvertPoints (pointsInput, points, false); + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, + flags, outfile, errfile); + if (!exitcode) { /* if no error */ + facetT *facet_list = qh facet_list; + int convexNumFaces = qh num_facets; + int convexNumVert = qh_setsize(qh_facetvertices (facet_list, NULL, false)); + + qh_triangulate(); // need this for triangulated output! + int convexNumFaces2 = qh num_facets; + int convexNumVert2 = qh_setsize(qh_facetvertices (facet_list, NULL, false)); + double pCenter[6]; + for (int u=0;u<6;u++) + pCenter[u] = 0; + double pZero[6]; + for (int u=0;u<6;u++) + pZero[u] = 0; + int nVcertexCount = 0; + FORALLvertices + { + for (int u=0;u<6;u++) + pCenter[u] += vertex->point[u]; + nVcertexCount++; + } + if (nVcertexCount>0) + for (int u=0;u<6;u++) + pCenter[u] /= (float)nVcertexCount; + + result->center.p[0] = pCenter[0]; + result->center.p[1] = pCenter[1]; + result->center.p[2] = pCenter[2]; + result->center.n[0] = pCenter[3]; + result->center.n[1] = pCenter[4]; + result->center.n[2] = pCenter[5]; + + /* 'qh facet_list' contains the convex hull */ + ContactPoint v[6]; + int nIds[6]; + MathTools::TriangleFace6D f; + + int nFacets = 0; + qh_getarea(qh facet_list); + result->volume = qh totvol; + double p[6]; + p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = 0; + FORALLfacets + { + int c = 0; +#ifdef CONVEXHULL_DEBUG_OUTPUT + bool printInfo = false; + if (facet->offset>0) + printInfo = true; + if (printInfo) + { + cout << "FACET " << nFacets << ":" << endl; + cout << "Offset:" << facet->offset << endl; + } +#endif + + FOREACHvertex_(facet->vertices) + { +#ifdef CONVEXHULL_DEBUG_OUTPUT + if (printInfo) + { + cout << vertex->point[0] << "," << vertex->point[1] << "," << vertex->point[2] << "," << vertex->point[3] << "," << vertex->point[4] << "," << vertex->point[5] << endl; + } +#endif + if (c<6) + { + v[c].p[0] = vertex->point[0]; + v[c].p[1] = vertex->point[1]; + v[c].p[2] = vertex->point[2]; + v[c].n[0] = vertex->point[3]; + v[c].n[1] = vertex->point[4]; + v[c].n[2] = vertex->point[5]; + result->vertices.push_back(v[c]); + nIds[c] = (int)result->vertices.size() - 1; + c++; + } else + { + cout << __FUNCTION__ << ": Error, facet with more than 6 vertices not supported ... face nr:" << nFacets << endl; + } + } + f.id[0] = nIds[0]; + f.id[1] = nIds[1]; + f.id[2] = nIds[2]; + f.id[3] = nIds[3]; + f.id[4] = nIds[4]; + f.id[5] = nIds[5]; + f.normal.p[0] = facet->normal[0]; + f.normal.p[1] = facet->normal[1]; + f.normal.p[2] = facet->normal[2]; + f.normal.n[0] = facet->normal[3]; + f.normal.n[1] = facet->normal[4]; + f.normal.n[2] = facet->normal[5]; + f.offset = facet->offset; + double dist = qh_distnorm(6,pCenter,facet->normal,&(facet->offset)); + f.distNormCenter = dist; + qh_distplane(pCenter,facet,&dist); + f.distPlaneCenter = dist; + dist = qh_distnorm(6,pZero,facet->normal,&(facet->offset)); + f.distNormZero = dist; + qh_distplane(pZero,facet,&dist); + f.distPlaneZero = dist; +#ifdef CONVEXHULL_DEBUG_OUTPUT + if (printInfo) + { + + if (facet->center) + cout << "Center:" << facet->center[0] << "," << facet->center[1] << "," << facet->center[2] << "," << facet->center[3] << "," << facet->center[4] << "," << facet->center[5] << endl; + cout << "distPlaneZero: " << f.distPlaneZero << ", distNormZero:" << f.distNormZero << ", QHULL_OFFSET:" << facet->offset << endl; + //cout << "Normal: " << f.normal.x << "," << f.normal.y << "," << f.normal.z << "," << f.normal.nx << "," << f.normal.ny << "," << f.normal.nz << endl; + } +#endif + result->faces.push_back(f); + nFacets++; + } + /*cout << "QHULL result: created Vertices: " << storeResult.vertices.size() << endl; + cout << "QHULL result: created Faces: " << nFacets << endl; + cout << "QHULL result: nVertices: " << convexNumVert2 << endl; + cout << "QHULL result: nFactes: " << convexNumFaces2 << endl;*/ + } + + qh_freeqhull(!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (errfile, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); + + clock_t endT = clock(); + long timeMS = (long)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0); + //cout << __FUNCTION__ << ": Created 6D convex hull in " << timeMS << " ms" << endl; + if (lockMutex) + qhull_mutex.unlock(); + return result; +} + +/* +bool createPoints( SoSeparator *pInputIVModel, std::vector<Vec3D> &vStorePoints, bool lockMutex ) +{ + if (!pInputIVModel) + return false; + + if (lockMutex) + qhull_mutex.lock(); + vStorePoints.clear(); + SoCallbackAction ca; + ca.addTriangleCallback(SoShape::getClassTypeId(), &CConvexHullGenerator_triangleCB, &vStorePoints); + ca.apply(pInputIVModel); + if (lockMutex) + qhull_mutex.unlock(); + return true; +}*/ + +/* +bool createConvexHull(SoSeparator *pInputIVModel, ConvexHull3D &storeResult) +{ + qhull_mutex.lock(); + vector<Vec3D> points; + if (!CreatePoints(pInputIVModel,points,false)) + return false; + bool bRes = CreateConvexHull(points,storeResult,false); + qhull_mutex.unlock(); + return bRes; +}*/ + +/* +bool createIVModel( ConvexHull3D &convexHull, SoSeparator *pStoreResult, bool lockMutex ) +{ + if (!pStoreResult || convexHull.vertices.size()<=0 || convexHull.faces.size()<=0) + return false; + + if (lockMutex) + qhull_mutex.lock(); + + SoCoordinate3* pCoords = new SoCoordinate3(); + SoFaceSet* pFaceSet = new SoFaceSet(); + + int nFaces = (int)convexHull.faces.size(); + int nVertices = nFaces*3; + Face3D f; + Vec3D v1,v2,v3; + + SbVec3f *pVertexArray = new SbVec3f[nVertices]; + + int nVertexCount = 0; + + for (int i=0;i<nFaces;i++) + { + f = convexHull.faces.at(i); + v1 = convexHull.vertices.at(f.id[0]); + v2 = convexHull.vertices.at(f.id[1]); + v3 = convexHull.vertices.at(f.id[2]); + + bool bNeedFlip = GraspStudioHelpers::checkVerticeOrientation(v1,v2,v3,f.normal); + + // COUNTER CLOCKWISE + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + else + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z); + nVertexCount++; + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + else + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + nVertexCount++; + + } + pCoords->point.setValues(0,nVertices,pVertexArray); + long *nNumVertices = new long[nFaces]; + for (int i=0;i<nFaces;i++) + nNumVertices[i] = 3; + pFaceSet->numVertices.setValues(0,nFaces,(const int32_t*)nNumVertices); + + pStoreResult->addChild(pCoords); + pStoreResult->addChild(pFaceSet); + delete []pVertexArray; + delete []nNumVertices; + + if (lockMutex) + qhull_mutex.unlock(); + + return true; +}*/ + +/* +void addVertex(Vec3D &v1,Vec3D &v2,Vec3D &v3,Vec3D &normal,SbVec3f *pVertexArray, int& nVertexCount) +{ + bool bNeedFlip = GraspStudioHelpers::checkVerticeOrientation(v1,v2,v3,normal); + + // COUNTER CLOCKWISE + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + else + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + nVertexCount++; + + pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z); + nVertexCount++; + + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + else + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + nVertexCount++; +}*/ + +/* +bool createIVModel(ConvexHull6D &convHull, SoSeparator *pStoreResult, bool buseFirst3Coords) +{ + if (!pStoreResult || convHull.vertices.size()<=0 || convHull.faces.size()<=0) + return false; + + qhull_mutex.lock(); + Face6D f; + Vec3D v1,v2,v3,v4,v5,v6; + + + int nFaces = (int)convHull.faces.size(); + + + // project points to 3d, then create hull of these points to visualize it + std::vector<Vec3D> vProjectedPoints; + for (int i=0;i<nFaces;i++) + { + f = convHull.faces.at(i); + if (buseFirst3Coords) + { + v1.x = convHull.vertices.at(f.id[0]).x; + v1.y = convHull.vertices.at(f.id[0]).y; + v1.z = convHull.vertices.at(f.id[0]).z; + v2.x = convHull.vertices.at(f.id[1]).x; + v2.y = convHull.vertices.at(f.id[1]).y; + v2.z = convHull.vertices.at(f.id[1]).z; + v3.x = convHull.vertices.at(f.id[2]).x; + v3.y = convHull.vertices.at(f.id[2]).y; + v3.z = convHull.vertices.at(f.id[2]).z; + v4.x = convHull.vertices.at(f.id[3]).x; + v4.y = convHull.vertices.at(f.id[3]).y; + v4.z = convHull.vertices.at(f.id[3]).z; + v5.x = convHull.vertices.at(f.id[4]).x; + v5.y = convHull.vertices.at(f.id[4]).y; + v5.z = convHull.vertices.at(f.id[4]).z; + v6.x = convHull.vertices.at(f.id[5]).x; + v6.y = convHull.vertices.at(f.id[5]).y; + v6.z = convHull.vertices.at(f.id[5]).z; + } else + { + v1.x = convHull.vertices.at(f.id[0]).nx; + v1.y = convHull.vertices.at(f.id[0]).ny; + v1.z = convHull.vertices.at(f.id[0]).nz; + v2.x = convHull.vertices.at(f.id[1]).nx; + v2.y = convHull.vertices.at(f.id[1]).ny; + v2.z = convHull.vertices.at(f.id[1]).nz; + v3.x = convHull.vertices.at(f.id[2]).nx; + v3.y = convHull.vertices.at(f.id[2]).ny; + v3.z = convHull.vertices.at(f.id[2]).nz; + v4.x = convHull.vertices.at(f.id[3]).nx; + v4.y = convHull.vertices.at(f.id[3]).ny; + v4.z = convHull.vertices.at(f.id[3]).nz; + v5.x = convHull.vertices.at(f.id[4]).nx; + v5.y = convHull.vertices.at(f.id[4]).ny; + v5.z = convHull.vertices.at(f.id[4]).nz; + v6.x = convHull.vertices.at(f.id[5]).nx; + v6.y = convHull.vertices.at(f.id[5]).ny; + v6.z = convHull.vertices.at(f.id[5]).nz; + } + vProjectedPoints.push_back(v1); + vProjectedPoints.push_back(v2); + vProjectedPoints.push_back(v3); + vProjectedPoints.push_back(v4); + vProjectedPoints.push_back(v5); + vProjectedPoints.push_back(v6); + } + ConvexHull3D projectedHull; + if (!CreateConvexHull(vProjectedPoints, projectedHull, false)) + { + cout << __FUNCTION__ << " Could not create hull of projected points, aborting..." << endl; + qhull_mutex.unlock(); + return false; + } + bool bRes = CreateIVModel(projectedHull, pStoreResult, false); + qhull_mutex.unlock(); + return bRes; + / * + // creates 3d-projection of all 6d facets + int nVertices = nFaces*12; + SoCoordinate3* pCoords = new SoCoordinate3(); + SoFaceSet* pFaceSet = new SoFaceSet(); + Face6d f; + Vec3d v1,v2,v3,v4,v5,v6; + Vec3d normal; + + SbVec3f *pVertexArray = new SbVec3f[nVertices]; + + int nVertexCount = 0; + bool bNeedFlip = false; + + for (int i=0;i<nFaces;i++) + { + f = convHull.faces.at(i); + if (buseFirst3Coords) + { + v1.x = convHull.vertices.at(f.id[0]).x; + v1.y = convHull.vertices.at(f.id[0]).y; + v1.z = convHull.vertices.at(f.id[0]).z; + v2.x = convHull.vertices.at(f.id[1]).x; + v2.y = convHull.vertices.at(f.id[1]).y; + v2.z = convHull.vertices.at(f.id[1]).z; + v3.x = convHull.vertices.at(f.id[2]).x; + v3.y = convHull.vertices.at(f.id[2]).y; + v3.z = convHull.vertices.at(f.id[2]).z; + v4.x = convHull.vertices.at(f.id[3]).x; + v4.y = convHull.vertices.at(f.id[3]).y; + v4.z = convHull.vertices.at(f.id[3]).z; + v5.x = convHull.vertices.at(f.id[4]).x; + v5.y = convHull.vertices.at(f.id[4]).y; + v5.z = convHull.vertices.at(f.id[4]).z; + v6.x = convHull.vertices.at(f.id[5]).x; + v6.y = convHull.vertices.at(f.id[5]).y; + v6.z = convHull.vertices.at(f.id[5]).z; + normal.x = f.normal.x; + normal.y = f.normal.y; + normal.z = f.normal.z; + } else + { + v1.x = convHull.vertices.at(f.id[0]).nx; + v1.y = convHull.vertices.at(f.id[0]).ny; + v1.z = convHull.vertices.at(f.id[0]).nz; + v2.x = convHull.vertices.at(f.id[1]).nx; + v2.y = convHull.vertices.at(f.id[1]).ny; + v2.z = convHull.vertices.at(f.id[1]).nz; + v3.x = convHull.vertices.at(f.id[2]).nx; + v3.y = convHull.vertices.at(f.id[2]).ny; + v3.z = convHull.vertices.at(f.id[2]).nz; + v4.x = convHull.vertices.at(f.id[3]).nx; + v4.y = convHull.vertices.at(f.id[3]).ny; + v4.z = convHull.vertices.at(f.id[3]).nz; + v5.x = convHull.vertices.at(f.id[4]).nx; + v5.y = convHull.vertices.at(f.id[4]).ny; + v5.z = convHull.vertices.at(f.id[4]).nz; + v6.x = convHull.vertices.at(f.id[5]).nx; + v6.y = convHull.vertices.at(f.id[5]).ny; + v6.z = convHull.vertices.at(f.id[5]).nz; + normal.x = f.normal.nx; + normal.y = f.normal.ny; + normal.z = f.normal.nz; + } + bool bNeedFlip = GraspStudioHelpers::checkVerticeOrientation(v1,v2,v3,normal); + if (bNeedFlip) + { + pVertexArray[nVertexCount].setValue((float)v6.x,(float)v6.y,(float)v6.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v5.x,(float)v5.y,(float)v5.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v4.x,(float)v4.y,(float)v4.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + nVertexCount++; + } else + { + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v4.x,(float)v4.y,(float)v4.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v5.x,(float)v5.y,(float)v5.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v6.x,(float)v6.y,(float)v6.z); + nVertexCount++; + } + } + pCoords->point.setValues(0,nVertices,pVertexArray); + long *nNumVertices = new long[nFaces]; + for (int i=0;i<nFaces;i++) + nNumVertices[i] = 6; + pFaceSet->numVertices.setValues(0,nFaces,(const int32_t*)nNumVertices); + + pStoreResult->addChild(pCoords); + pStoreResult->addChild(pFaceSet); + delete []pVertexArray; + delete []nNumVertices; + + qhull_mutex.unlock(); + + return true; + * / +}*/ + +void ConvexHullGenerator::PrintVertices(std::vector<ContactPoint> &pointsInput) +{ + for (int i=0;i<(int)pointsInput.size();i++) + { + cout << "v" << i << ": " << pointsInput[i].p[0] << "," << pointsInput[i].p[1] << "," << pointsInput[i].p[2] << "," + << pointsInput[i].n[0] << "," << pointsInput[i].n[1] << "," << pointsInput[i].n[2] << endl; + } +} +void ConvexHullGenerator::PrintStatistics( VirtualRobot::MathTools::ConvexHull6DPtr convHull ) +{ + if (!convHull) + { + GRASPSTUDIO_ERROR << " Null data to print" << endl; + return; + } + float minValue[6]; + float maxValue[6]; + int i; + for (i=0;i<=5;i++) + { + minValue[i] = FLT_MAX; + maxValue[i] = -FLT_MAX; + } + for (i=0;i<(int)convHull->vertices.size();i++) + { + if (convHull->vertices[i].p[0] < minValue[0]) + minValue[0] = convHull->vertices[i].p[0]; + if (convHull->vertices[i].p[0] > maxValue[0]) + maxValue[0] = convHull->vertices[i].p[0]; + if (convHull->vertices[i].p[1] < minValue[1]) + minValue[1] = convHull->vertices[i].p[1]; + if (convHull->vertices[i].p[1] > maxValue[1]) + maxValue[1] = convHull->vertices[i].p[1]; + if (convHull->vertices[i].p[2] < minValue[2]) + minValue[2] = convHull->vertices[i].p[2]; + if (convHull->vertices[i].p[2] > maxValue[2]) + maxValue[2] = convHull->vertices[i].p[2]; + if (convHull->vertices[i].n[0] < minValue[3]) + minValue[3] = convHull->vertices[i].n[0]; + if (convHull->vertices[i].n[0] > maxValue[3]) + maxValue[3] = convHull->vertices[i].n[0]; + if (convHull->vertices[i].n[1] < minValue[4]) + minValue[4] = convHull->vertices[i].n[1]; + if (convHull->vertices[i].n[1] > maxValue[4]) + maxValue[4] = convHull->vertices[i].n[1]; + if (convHull->vertices[i].n[2] < minValue[5]) + minValue[5] = convHull->vertices[i].n[2]; + if (convHull->vertices[i].n[2] > maxValue[5]) + maxValue[5] = convHull->vertices[i].n[2]; + } + cout << "Conv Hull Bounds:" << endl; + cout << "\t\t x : " << minValue[0] << "," << maxValue[0] << endl; + cout << "\t\t y : " << minValue[1] << "," << maxValue[1] << endl; + cout << "\t\t z : " << minValue[2] << "," << maxValue[2] << endl; + cout << "\t\t nx: " << minValue[3] << "," << maxValue[3] << endl; + cout << "\t\t ny: " << minValue[4] << "," << maxValue[4] << endl; + cout << "\t\t nz: " << minValue[5] << "," << maxValue[5] << endl; +} + +bool ConvexHullGenerator::checkVerticeOrientation( const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const Eigen::Vector3f &v3, const Eigen::Vector3f &n ) +{ + Eigen::Vector3f tmp; + Eigen::Vector3f v1v2; + Eigen::Vector3f v1v3; + v1v2(0) = v2(0) - v1(0); + v1v2(1) = v2(1) - v1(1); + v1v2(2) = v2(2) - v1(2); + v1v3(0) = v3(0) - v1(0); + v1v3(1) = v3(1) - v1(1); + v1v3(2) = v3(2) - v1(2); + tmp = v1v2.cross(v1v3); + float tmpF = tmp.dot(n); + return (tmpF<0); + /*crossProduct(v1v2,v1v3,tmp); + float tmpF = dotProduct(tmp,n);*/ +} + + + +} // namespace diff --git a/GraspPlanning/ConvexHullGenerator.h b/GraspPlanning/ConvexHullGenerator.h new file mode 100644 index 0000000000000000000000000000000000000000..e3d6127abb59cbb6c81a25474ad609e9416f7b0e --- /dev/null +++ b/GraspPlanning/ConvexHullGenerator.h @@ -0,0 +1,69 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _CONVEX_HULL_GENERATOR_H_ +#define _CONVEX_HULL_GENERATOR_H_ + +#include "GraspStudio.h" +#include "VirtualRobot/MathTools.h" +#include <vector> +#include <boost/thread.hpp> + +namespace GraspStudio +{ +/*! +* This class can be used as an interface for qhull. +* A convex hull can be generated out of point arrays. +* This class is thread safe, which means that multiple threads +* are allowed to use the static methods of ConvexHullGenerator. +*/ +class GRASPSTUDIO_IMPORT_EXPORT ConvexHullGenerator +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /*! + Creates a convex hull of the points stored in pointsInput. + */ + static VirtualRobot::MathTools::ConvexHull3DPtr CreateConvexHull(std::vector<Eigen::Vector3f> &pointsInput, bool lockMutex = true); + static VirtualRobot::MathTools::ConvexHull3DPtr CreateConvexHull(VirtualRobot::TriMeshModelPtr pointsInput, bool lockMutex = true); + static VirtualRobot::MathTools::ConvexHull6DPtr CreateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint> &pointsInput, bool lockMutex = true); + + static void PrintStatistics(VirtualRobot::MathTools::ConvexHull6DPtr convHull); + + /*! + Convert points to qhull format + */ + static bool ConvertPoints(std::vector<Eigen::Vector3f> &points, double *storePointsQHull, bool lockMutex = true); + static bool ConvertPoints(std::vector<VirtualRobot::MathTools::ContactPoint> &points, double *storePointsQHull, bool lockMutex = true); + + static void PrintVertices(std::vector<VirtualRobot::MathTools::ContactPoint> &pointsInput); + + static bool checkVerticeOrientation (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const Eigen::Vector3f &v3, const Eigen::Vector3f &n); + +protected: + //! QHull is not thread safe, so protect qHull calls with a mutex + static boost::mutex qhull_mutex; +}; + +} + +#endif /* _CONVEX_HULL_GENERATOR_H_ */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/Announce.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/Announce.txt new file mode 100644 index 0000000000000000000000000000000000000000..5813e25f9b6f9cb9edee1a84a7f08a420653b15f --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/Announce.txt @@ -0,0 +1,50 @@ + + Qhull 2003.1 2003/12/30 + + http://www.qhull.org + http://savannah.gnu.org/projects/qhull/ + http://www6.uniovi.es/ftp/pub/mirrors/geom.umn.edu/software/ghindex.html + http://www.geomview.org + http://www.geom.uiuc.edu + +Qhull computes convex hulls, Delaunay triangulations, Voronoi diagrams, +furthest-site Voronoi diagrams, and halfspace intersections about a point. +It runs in 2-d, 3-d, 4-d, or higher. It implements the Quickhull algorithm +for computing convex hulls. Qhull handles round-off errors from floating +point arithmetic. It can approximate a convex hull. + +The program includes options for hull volume, facet area, partial hulls, +input transformations, randomization, tracing, multiple output formats, and +execution statistics. The program can be called from within your application. +You can view the results in 2-d, 3-d and 4-d with Geomview. + +To download Qhull: + http://www.qhull.org/download + http://savannah.nongnu.org/files/?group=qhull + +Download qhull-96.ps for: + + Barber, C. B., D.P. Dobkin, and H.T. Huhdanpaa, + "The Quickhull Algorithm for Convex Hulls," ACM + Trans. on Mathematical Software, Dec. 1996. + http://www.acm.org/pubs/citations/journals/toms/1996-22-4/p469-barber/ + http://citeseer.nj.nec.com/83502.html + +Abstract: + +The convex hull of a set of points is the smallest convex set that contains +the points. This article presents a practical convex hull algorithm that +combines the two-dimensional Quickhull Algorithm with the general dimension +Beneath-Beyond Algorithm. It is similar to the randomized, incremental +algorithms for convex hull and Delaunay triangulation. We provide empirical +evidence that the algorithm runs faster when the input contains non-extreme +points, and that it uses less memory. + +Computational geometry algorithms have traditionally assumed that input sets +are well behaved. When an algorithm is implemented with floating point +arithmetic, this assumption can lead to serious errors. We briefly describe +a solution to this problem when computing the convex hull in two, three, or +four dimensions. The output is a set of "thick" facets that contain all +possible exact convex hulls of the input. A variation is effective in five +or more dimensions. + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/CMakeLists.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..f6634507e92c0f151484d08367ace666f32236c4 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/CMakeLists.txt @@ -0,0 +1,34 @@ +PROJECT(qhull) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.0) +CMAKE_POLICY(VERSION 2.6) + +INCLUDE (${GRASPSTUDIO_DIR}/config.cmake) +SET (GRASPSTUDIO_VirtualRobotDir ${GRASPSTUDIO_SimoxDir}/VirtualRobot) +INCLUDE (${GRASPSTUDIO_VirtualRobotDir}/config.cmake) + +MESSAGE(STATUS "** qHull VirtualRobotDir: ${GRASPSTUDIO_VirtualRobotDir}") + + +# Specify sources and headers +FILE(GLOB SRCS ${PROJECT_SOURCE_DIR}/src/*.c) +FILE(GLOB INCS ${PROJECT_SOURCE_DIR}/src/*.h) + +INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/include ${PROJECT_SOURCE_DIR}/src) +ADD_LIBRARY(${PROJECT_NAME} STATIC ${SRCS} ${INCS}) +#SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${SIMOX_LIB_DIR}) + +# .DLL path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${GRASPSTUDIO_BIN_DIR}) +# .so path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${GRASPSTUDIO_LIB_DIR}) +# .lib path (this is needed for setting the DLL-import library path on windows) +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${GRASPSTUDIO_LIB_DIR}) + + +TARGET_LINK_LIBRARIES(${PROJECT_NAME}) +MESSAGE(${PROJECT_NAME} " will be placed into " ${GRASPSTUDIO_LIB_DIR}) + +MESSAGE("make install will install " ${PROJECT_NAME} " into " ${GRASPSTUDIO_INSTALL_LIB_DIR}) +INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${GRASPSTUDIO_INSTALL_LIB_DIR}) + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/CMakeLists.txt.bak b/GraspPlanning/ExternalDependencies/qhull-2003.1/CMakeLists.txt.bak new file mode 100644 index 0000000000000000000000000000000000000000..f65722629ebc5abb21622ab1fc6c95937caa76e8 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/CMakeLists.txt.bak @@ -0,0 +1,34 @@ +PROJECT(qhull) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.0) +CMAKE_POLICY(VERSION 2.6) + +INCLUDE (${GRASPSTUDIO_DIR}/config.cmake) +SET (GRASPSTUDIO_VirtualRobotDir ${GRASPSTUDIO_SimoxDir}/VirtualRobot) +INCLUDE (${GRASPSTUDIO_VirtualRobotDir}/config.cmake) + +MESSAGE(STATUS "** qHull VirtualRobotDir: ${GRASPSTUDIO_VirtualRobotDir}") + + +# Specify sources and headers +FILE(GLOB SRCS ${PROJECT_SOURCE_DIR}/src/*.c) +FILE(GLOB INCS ${PROJECT_SOURCE_DIR}/src/*.h) + +INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/include ${PROJECT_SOURCE_DIR}/src) +ADD_LIBRARY(${PROJECT_NAME} STATIC ${SRCS} ${INCS}) +#SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${SIMOX_LIB_DIR}) + +# .DLL path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${GRASPSTUDIO_BIN_DIR}) +# .so path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${GRASPSTUDIO_LIB_DIR}) +# .lib path (this is needed for setting the DLL-import library path on windows) +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${GRASPSTUDIO_LIB_DIR}) + + +TARGET_LINK_LIBRARIES(${PROJECT_NAME}) +MESSAGE(${PROJECT_NAME} " will be placed into " ${GRASPSTUDIO_LIB_DIR}) + +MESSAGE("make install will install " ${PROJECT_NAME} " into " ${SIMOX_INSTALL_LIB_DIR}) +INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${SIMOX_INSTALL_LIB_DIR}) + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/COPYING.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/COPYING.txt new file mode 100644 index 0000000000000000000000000000000000000000..7230a27a3269ee9455acf04de4793e5ab5e33d30 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/COPYING.txt @@ -0,0 +1,34 @@ + Qhull, Copyright (c) 1993-2003 + + The National Science and Technology Research Center for + Computation and Visualization of Geometric Structures + (The Geometry Center) + University of Minnesota + + email: qhull@qhull.org + +This software includes Qhull from The Geometry Center. Qhull is +copyrighted as noted above. Qhull is free software and may be obtained +via http from www.qhull.org. It may be freely copied, modified, +and redistributed under the following conditions: + +1. All copyright notices must remain intact in all files. + +2. A copy of this text file must be distributed along with any copies + of Qhull that you redistribute; this includes copies that you have + modified, or copies of programs or other software products that + include Qhull. + +3. If you modify Qhull, you must include a notice giving the + name of the person performing the modification, the date of + modification, and the reason for such modification. + +4. When distributing modified versions of Qhull, or other software + products that include Qhull, you must provide notice that the original + source code may be obtained as noted above. + +5. There is no warranty or other guarantee of fitness for Qhull, it is + provided solely "as is". Bug reports or fixes may be sent to + qhull_bug@qhull.org; the authors may or may not act on them as + they desire. + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/File_id.diz b/GraspPlanning/ExternalDependencies/qhull-2003.1/File_id.diz new file mode 100644 index 0000000000000000000000000000000000000000..765b146c6568a927132db0c75f28af79fb7ba0b4 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/File_id.diz @@ -0,0 +1,12 @@ +Qhull 2003.1 - Qhull computes convex hulls, +Delaunay triangulations, halfspace inter- +sections about a point, Voronoi diagrams, +furthest-site Delaunay triangulations, and +furthest-site Voronoi diagrams. Qhull +works with 2-d, 3-d, 4-d, 5-d, and higher +dimensions. It computes volumes, surface +areas, and approximations. It runs in a +DOS window under Windows 95/NT/XP. +www.qhull.org, freeware. + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/QHULL-GO.pif b/GraspPlanning/ExternalDependencies/qhull-2003.1/QHULL-GO.pif new file mode 100644 index 0000000000000000000000000000000000000000..a5414428207cfe796fb9a7fff6ae0790af968500 Binary files /dev/null and b/GraspPlanning/ExternalDependencies/qhull-2003.1/QHULL-GO.pif differ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/README.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/README.txt new file mode 100644 index 0000000000000000000000000000000000000000..4cbbdde6123dbf79dba3b3bd66b13ae149c9639d --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/README.txt @@ -0,0 +1,322 @@ +Name + + qhull, rbox 2003.1 2003/12/30 + +Convex hull, Delaunay triangulation, Voronoi diagrams, Halfspace intersection + + Documentation: + html/index.htm + + Available from: + <http://www.qhull.org> + <http://savannah.gnu.org/projects/qhull> + + Version 1 (simplicial only): + <http://www.qhull.org/download/qhull-1.0.tar.gz> + <http://www.qhull.org/download/qhull.sit.hqx> + + News and a paper: + <http://www.qhull.org/news> + <http://citeseer.nj.nec.com/83502.html> + +Purpose + + Qhull is a general dimension convex hull program that reads a set + of points from stdin, and outputs the smallest convex set that contains + the points to stdout. It also generates Delaunay triangulations, Voronoi + diagrams, furthest-site Voronoi diagrams, and halfspace intersections + about a point. + + Rbox is a useful tool in generating input for Qhull; it generates + hypercubes, diamonds, cones, circles, simplices, spirals, + lattices, and random points. + + Qhull produces graphical output for Geomview. This helps with + understanding the output. <http://www.geomview.org> + + +Environment requirements + + Qhull and rbox should run on all 32-bit and 64-bit computers. Use + an ANSI C or C++ compiler to compile the program. The software is + self-contained. + + Qhull is copyrighted software. Please read COPYING.txt and REGISTER.txt + before using or distributing Qhull. + +To contribute to Qhull + + Qhull is on Savannah at http://savannah.gnu.org/projects/qhull/ + +Qhull on Windows 95, 98, ME, NT, 2000, XP + + The zip file contains rbox.exe, qhull.exe, qconvex.exe, qdelaunay.exe, + qhalf.exe, qvoronoi.exe, documentation files, and source files. + + To install Qhull: + - Unzip the files into a directory. You may use WinZip32 <www.hotfiles.com> + - Click on QHULL-GO + + - In Windows 95, the DOS window needs improvement. + - Increase the size of the screen font to 8x12. + - If the text is too dim, fix the screen colors with shareware (e.g., crt.exe) + - If you use qhull a lot, consider using the Cygwin Unix shell (www.cygwin.com), + + To learn about Qhull: + - Execute 'qconvex' for a synopsis and examples. + - Execute 'rbox 10 | qconvex' to compute the convex hull of 10 random points. + - Execute 'rbox 10 | qconvex i TO file' to write results to 'file'. + - If an error occurs, Windows 95 sends the error to stdout instead of stderr + - use 'TO xxx' to send normal output to xxx and error output to stdout + - Browse the documentation: qhull\html\index.htm + +Compiling with cygwin on Windows NT, 2000, XP + - install cygwin [www.cygwin.com] with gcc, make, ar, and ln + - cd qhull/src + - make -f Makefile.txt + +Qhull on Unix (Debian) + + The gzip file, qhull.tar.gz, contains documentation and source files for + qhull and rbox. It should compile on all Unix systems, including Debian. + You may also use the source instructions below. + + To unpack the gzip file + - tar zxf qhull.tar.gz + - cd qhull + + Compile with the configure Makefile [R. Laboissiere]: + - ./configure + - make + +Compiling the source distribution + + The gzip file, qhull-src.tgz, contains documentation and source files for + qhull and rbox. + + To unpack the gzip file + - tar zxf qhull-src.tgz + - cd qhull + + Compiling with Makefile (i.e., Makefile.txt) + - cd src + - in Makefile, check the CC, CCOPTS1, PRINTMAN, and PRINTC defines + - the defaults are gcc and enscript + - CCOPTS1 should include the ANSI flag. It defines __STDC__ + - in user.h, check the definitions of qh_SECticks and qh_CPUclock. + - use '#define qh_CLOCKtype 2' for timing runs longer than 1 hour + - type: make + - this builds: qhull qconvex qdelaunay qhalf qvoronoi rbox libqhull.a + - type: make doc + - this prints the man page + - See also qhull/html/index.htm + - if your compiler reports many errors, it is probably not a ANSI C compiler + - you will need to set the -ansi switch or find another compiler + - if your compiler warns about missing prototypes for fprintf() etc. + - this is ok, your compiler should have these in stdio.h + - if your compiler warns about missing prototypes for memset() etc. + - include memory.h in qhull_a.h + - if your compiler is gcc-2.95.1, you need to set flag -fno-strict-aliasing. + - This flag is set by default for other versions [Karas, Krishnaswami] + - if your compiler reports "global.c: storage size of 'qh_qh' isn't known" + - delete the initializer "={0}" in global.c, stat.c and mem.c + - if your compiler warns about "stat.c: improper initializer" + - this is ok, the initializer is not used + - if you have trouble building libqhull.a with 'ar' + - try 'make -f Makefile.txt qhullx' + - if the code compiles, the qhull test case will automatically execute + - if an error occurs, there's an incompatibility between machines + - For gcc-2.95.1, you need to set flag -fno-strict-aliasing. + It is set by default for other versions of gcc [Karas, Krishnaswami] + - If you can, try a different compiler + - You can turn off the Qhull memory manager with qh_NOmem in mem.h + - You can turn off compiler optimization (-O2 in Makefile) + - If you find the source of the problem, please let us know + - if you have Geomview (www.geomview.org) + - try 'rbox 100 | qconvex G >a' and load 'a' into Geomview + - run 'q_eg' for Geomview examples of Qhull output (see qh-eg.htm) + - to install the programs and their man pages: + - define MANDIR and BINDIR + - type 'make install' + +Compiling on Windows 95, 98, NT, 2000, XP + + Qhull compiles as a console application in Visual C++ 5.0 at warning + level 3. + + Visual C++ quickstart for qhull.exe only: + - create a "Win32 console application" called "qhull" + - add the following files: + geom.c geom2.c global.c io.c mem.c merge.c poly.c poly2.c qhull.c + qset.c stat.c unix.c user.c + - create a "Win32 console application" called "rbox" + - add rbox.c + + Visual C++ quickstart for qhull library, qhull.exe, qconvex.exe, etc. + - To simplify setting up lots of projects, + - create a temporary "Win32 console application" called "source" + - add all .c files from .../src/... + - In Tools::Options::Tab + Set tab size to 8 and indent size to 2 + + - create a "Win32 console application" called "rbox" + - move rbox.c from "qhull source" + - for Project:Settings..., Link + you only need the default libraries + - build the project + + - create a "Win32 static library" called "library" + - move these files from "qhull source" + geom.c geom2.c global.c io.c mem.c merge.c poly.c poly2.c qhull.c + qset.c stat.c user.c + - set the library file (use the same for debug and release) + - build the project + + - create a "Win32 console application" called "qhull" + - Move unix.c from "qhull source" + - Add the library file created by "library" + - Qhull does not use other libraries + + - create a "Win32 console application" called "qconvex" + - Move qconvex.c from "qhull source" + - Copy the library file from "qhull" + + - do the same for qdelaun.c, qhalf, qvoronoi.c, user_eg.c, user_eg2.c + - delete "qhull sources" since it is no longer needed + - use Project:Settings to make any changes + - use batch build to rebuild everything + + Qhull compiles with Borland C++ 5.0 bcc32. A Makefile is included. + Execute 'make -f Mborland'. If you use the Borland IDE, set the ANSI + option in Options:Project:Compiler:Source:Language-compliance. + + Qhull compiles with Borland C++ 4.02 for Win32 and DOS Power Pack. + Use 'make -f Mborland -D_DPMI'. Qhull 1.0 compiles with Borland + C++ 4.02. For rbox 1.0, use "bcc32 -WX -w- -O2-e -erbox -lc rbox.c". + Use the same options for Qhull 1.0. [D. Zwick] + + Qhull compiles with Metrowerks C++ 1.7 with the ANSI option. + + If you turn on full warnings, the compiler will report a number of + unused variables, variables set but not used, and dead code. These are + intentional. For example, variables may be initialized (unnecessarily) + to prevent warnings about possible use of uninitialized variables. + +Compiling on the Power Macintosh + + Qhull compiles on the Power Macintosh with Metrowerk's C compiler. + It uses the SIOUX interface to read point coordinates and return output. + There is no graphical output. For project files, see 'Compiling for + Windows 95'. Instead of using SIOUX, Qhull may be embedded within an + application. + + Version 1 is available for Macintosh computers by download of qhull.sit.hqx + It reads point coordinates from a standard file and returns output + to a standard file. There is no graphical output. + + +Compiling on other machines + + Some users have reported problems with compiling Qhull under Irix 5.1. It + compiles under other versions of Irix. + + If you have troubles with the memory manager, you can turn it off by + defining qh_NOmem in mem.h. + + You may compile Qhull with a C++ compiler. + + +Distributed files + + README.txt // instructions for installing Qhull + REGISTER.txt // Qhull registration + COPYING.txt // copyright notice + QHULL-GO.pif // Windows icon for qhull-go.bat + Announce.txt // announcement + Changes.txt // change history for Qhull and rbox + File_id.diz // package descriptor + index.htm // Home page + html/qh-faq.htm // Frequently asked questions + html/qh-get.htm // Download page + html/index.htm // Manual + src/Makefile.txt // Makefile for Unix or cygwin 'make' + src/Mborland // Makefile for Borland C++/Win32 + src/Make-config.sh // Create Debian configure and automake + +src/ + rbox consists of: + rbox.exe // Win32 executable (.zip only) + rbox.htm // html manual + rbox.man // Unix man page + rbox.txt + rbox.c // source program + + qhull consists of: + qhull.exe // Win32 executables (.zip only) + qconvex.exe + qdelaunay.exe + qhalf.exe + qvoronoi.exe + qhull-go.bat // DOS window + qconvex.htm // html manuals + qdelaun.htm + qdelau_f.htm + qhalf.htm + qvoronoi.htm + qvoron_f.htm + qh-eg.htm + qh-impre.htm + qh-in.htm + index.htm + qh-opt*.htm + qh-quick.htm + qh--4d.gif,etc. // images for manual + qhull.man // Unix man page + qhull.txt + q_eg // shell script for Geomview examples + q_egtest // shell script for Geomview test examples + q_test // shell script to test qhull + + top-level source files: + src/index.htm // index to source files + qh-...htm // specific files + user.h // header file of user definable constants + qhull.h // header file for qhull + unix.c // Unix front end to qhull + qhull.c // Quickhull algorithm with partitioning + user.c // user re-definable functions + user_eg.c // example of incorporating qhull into a user program + user_eg2.c // more complex example + qhull_interface.cpp // call Qhull from C++ + + other source files: + qhull_a.h // include file for *.c + geom.c // geometric routines + geom2.c + geom.h + global.c // global variables + io.c // input-output routines + io.h + mem.c // memory routines, this is stand-alone code + mem.h + merge.c // merging of non-convex facets + merge.h + poly.c // polyhedron routines + poly2.c + poly.h + qset.c // set routines, this only depends on mem.c + qset.h + stat.c // statistics + stat.h + +Authors: + + C. Bradford Barber Hannu Huhdanpaa + bradb@qhull.org hannu@qhull.org + + The Geometry Center + University of Minnesota + + Qhull 1.0 was developed under NSF grants NSF/DMS-8920161 and + NSF-CCR-91-15793 750-7504 at the Geometry Center and Harvard + University. If you find Qhull useful, please let us know. diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/REGISTER.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/REGISTER.txt new file mode 100644 index 0000000000000000000000000000000000000000..4682e02669f1da0583867cdc91f2c82f1a07c676 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/REGISTER.txt @@ -0,0 +1,37 @@ +Dear User of Geometry Center Software: + +We would like to find out how you are using our software. Think of +Geometry Center software as a new kind of shareware: you share your +science and successes with us, and we share our software and support +with you. + +If you use Geometry Center software, please send us a note telling +us what you are doing with it. + +We need to know: + + (1) What you are working on - an abstract of your work would be + fine. + + (2) What Geometry Center software you use. + + (3) How that software has helped you, for example, by increasing + your productivity or allowing you to do things you could not do + before. In particular, if you feel that Geometry Center + software has had a direct bearing on your work, please tell us + about this. + +We encourage you to cite the use of any Geometry Center software you +have used in your publications. + +To cite Qhull, use + + Barber, C.B., Dobkin, D.P., and Huhdanpaa, H.T., "The Quickhull + algorithm for convex hulls," ACM Trans. on Mathematical Software, + Dec 1996. http://www.qhull.org + +Please send e-mail to + + qhull@qhull.org + +Thank you! diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/Qhull-go.bat b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/Qhull-go.bat new file mode 100644 index 0000000000000000000000000000000000000000..22792bde3209e7eb674ddac44ada9c6fe1ce256d --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/Qhull-go.bat @@ -0,0 +1,20 @@ +@echo off +echo. +echo ========= Qhull-go.bat, DOS window for Qhull and Rbox =========== +echo. +echo Use arrow keys to repeat and edit commands +echo. +echo Use properties button to increase size to 50 lines +echo. +echo Change font to 8x12 +echo. +echo Change screen colors with ansi.sys or crt.exe v4.0 (shareware) +echo. +echo For scrolling back, try peruse (PC Magazine utility) +echo. +echo If full-screen, use alt-enter to return to Windows +echo. +echo Type 'qconvex' for synopsis and examples. +echo. +doskey /insert +%comspec% \ No newline at end of file diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_eg b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_eg new file mode 100644 index 0000000000000000000000000000000000000000..e9d0cfb289a3a65b9173612222741c1ff5bb08d0 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_eg @@ -0,0 +1,58 @@ +#! /bin/sh +# writes examples to eg/ +# see html/qh_eg.htm +set -v +rbox c D3 | qconvex s G >eg/eg.01.cube +rbox c d G2.0 | qconvex s G >eg/eg.02.diamond.cube +rbox s 100 D3 | qconvex s G >eg/eg.03.sphere +rbox s 100 D2 | qconvex s G >eg/eg.04.circle +rbox 10 l | qconvex s G >eg/eg.05.spiral +rbox 1000 D2 | qconvex s C-0.03 Qc Gapcv >eg/eg.06.merge.square +rbox 1000 D3 | qconvex s G >eg/eg.07.box +rbox c G0.4 s 500 | qconvex s G >eg/eg.08a.cube.sphere +rbox d G0.6 s 500 | qconvex s G >eg/eg.08b.diamond.sphere +rbox 100 L3 G0.5 s | qconvex s G >eg/eg.09.lens +rbox 100 s P0.5,0.5,0.5 | qconvex s Ga QG0 >eg/eg.10a.sphere.visible +rbox 100 s P0.5,0.5,0.5 | qconvex s Ga QG-0 >eg/eg.10b.sphere.beyond +rbox 100 s P0.5,0.5,0.5 | qconvex s Ga QG0 PG >eg/eg.10c.sphere.horizon +rbox 100 s P0.5,0.5,0.5 | qconvex s Ga QV0 PgG >eg/eg.10d.sphere.cone +rbox 100 s P0.5,0.5,0.5 | qconvex s Ga >eg/eg.10e.sphere.new +rbox 100 s P0.5,0.5,0.5 | qhull s Ga QV0g Q0 >eg/eg.14.sphere.corner +rbox 500 W0 | qconvex s QR0 Qc Gvp >eg/eg.15a.surface +rbox 500 W0 | qconvex s QR0 Qt Qc Gvp >eg/eg.15b.triangle +rbox 500 W0 | qconvex s QR0 QJ5e-2 Qc Gvp >eg/eg.15c.joggle +echo 2 = rbox 6 r s D2, rbox 15 B0.3 W0.25, c G0.5 >eg/eg.data.17 +echo 25 >>eg/eg.data.17 +rbox 15 D2 B0.3 W0.25 c G0.5 | tail +3 >>eg/eg.data.17 +rbox 6 r s D2 B0.2 | tail +3 >>eg/eg.data.17 +qdelaunay s Qt <eg/eg.data.17 GnraD2 >eg/eg.17a.delaunay.2 +qdelaunay s <eg/eg.data.17 GnraD2 >eg/eg.17b.delaunay.2i +qdelaunay s <eg/eg.data.17 C-0 Ga >eg/eg.17c.delaunay.2-3 +qvoronoi s QJ <eg/eg.data.17 Gna >eg/eg.17d.voronoi.2 +qvoronoi s <eg/eg.data.17 Gna >eg/eg.17e.voronoi.2i +rbox c G0.1 d | qdelaunay Gt Qz >eg/eg.17f.delaunay.3 +rbox 10 D2 d | qdelaunay s Qu G >eg/eg.18a.furthest.2-3 +rbox 10 D2 d | qdelaunay s Qu Pd2 G >eg/eg.18b.furthest-up.2-3 +rbox 10 D2 d | qvoronoi s Qu Gv >eg/eg.18c.furthest.2 +rbox 10 D3 | qvoronoi s FQ QV5 p | qconvex s G >eg/eg.19.voronoi.region.3 +rbox r s 20 Z1 G0.2 | qconvex s QR1 G >eg/eg.20.cone +rbox 200 s | qconvex s Qc R0.014 Gpav >eg/eg.21b.roundoff.fixed +rbox 1000 s| qconvex s C0.01 Qc Gcrp >eg/eg.22a.merge.sphere.01 +rbox 1000 s| qconvex s C-0.01 Qc Gcrp >eg/eg.22b.merge.sphere.-01 +rbox 1000 s| qconvex s C0.05 Qc Gcrpv >eg/eg.22c.merge.sphere.05 +rbox 1000 s| qconvex s C-0.05 Qc Gcrpv >eg/eg.22d.merge.sphere.-05 +rbox 1000 | qconvex s Gcprvah C0.1 Qc >eg/eg.23.merge.cube +rbox 5000 D4 | qconvex s GD0v Pd0:0.5 C-0.02 C0.1 >eg/eg.24.merge.cube.4d-in-3d +rbox 5000 D4 | qconvex s s C-0.02 C0.1 Gh >eg/eg.30.4d.merge.cube +rbox 20 D3 | qdelaunay s G >eg/eg.31.4d.delaunay +rbox 30 s D4 | qconvex s G Pd0d1d2D3 >eg/eg.32.4d.octant +rbox 10 r s Z1 G0.3 | qconvex G >eg/eg.33a.cone +rbox 10 r s Z1 G0.3 | qconvex FQ FV n | qhalf G >eg/eg.33b.cone.dual +rbox 10 r s Z1 G0.3 | qconvex FQ FV n | qhalf FQ s Fp | qconvex G >eg/eg.33c.cone.halfspace + +echo ==the following should generate flipped and concave facets== >/dev/null +rbox 200 s | qhull Q0 s R0.014 Gav Po >eg/eg.21a.roundoff.errors +echo ==the preceeding should report flipped and concave facets== >/dev/null + + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_egtest b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_egtest new file mode 100644 index 0000000000000000000000000000000000000000..8948dcb1d6c426bcdfcfe727278e2f22b0a9ce00 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_egtest @@ -0,0 +1,33 @@ +#! /bin/sh +# writes examples to eg/ +set -v +rbox d D3 | qconvex s Gnrv Tc Tv >eg/eg.t01.spheres.3 +rbox d D2 | qconvex s Gnv Tc Tv >eg/eg.t02.spheres.2 +rbox d D3 | qconvex s Gnrp Tc Tv >eg/eg.t03.points.3 +rbox d D2 | qconvex s Gnp Tc Tv >eg/eg.t04.points.2 +rbox c D4 | qconvex s C0.05 GnpcD3 Pd3:0.5 Tc Tv >eg/eg.t05.centrum.points.4-3 +rbox d D3 | qconvex s Gnrc Tc Tv >eg/eg.t06.centrums.3.precise +rbox d D3 | qconvex s C0.05 Gnrc Tc Tv >eg/eg.t07.centrums.3 +rbox d D2 | qconvex s C0.05 Gc Tc Tv >eg/eg.t08.centrums.2 +rbox d D3 | qconvex s Gnha Tc Tv >eg/eg.t09.intersect.3 +rbox d D3 | qconvex s GaD0 Pd0 Tc Tv >eg/eg.t10.faces.3-2 +rbox d D3 | qconvex s GnrpD0 Pd0 Tc Tv >eg/eg.t11.points.3-2 +rbox d D3 | qconvex s C0.05 GnrcD0 Pd0 Tc Tv >eg/eg.t12.centrums.3-2 +rbox d D3 | qconvex s GnhaD0 Pd0 Tc Tv >eg/eg.t13.intersect.3-2 +rbox d D3 | qconvex s GnrvD0 Pd0 Tc Tv >eg/eg.t14.spheres.3-2 +rbox c D4 | qconvex s GvD0 Pd0:0.5 Tc Tv >eg/eg.t15.spheres.4-3 +rbox c D4 | qhull s Q0 C0 GpD0 Pd0:0.5 Tc Tv >eg/eg.t16.points.4-3 +rbox c D4 | qconvex s GahD0 Pd0:0.5 Tc Tv >eg/eg.t17.intersect.4-3 +rbox 100 s | qconvex s C-0.05 Qc Gicvprh Tc Tv >eg/eg.t18.imprecise.3 +rbox 30 s D4 | qconvex s GhD0 Pd0d1d2D3 Tc >eg/eg.t19.intersect.precise.4-3 +rbox 100 s P1,1,1 | qconvex s QG-0 Pgp Tc G >eg/eg.t20.notvisible +rbox 100 s | qconvex s QV-10 Pgp Tc G >eg/eg.t21.notvertex +rbox 100 r D2 P1,1 | qhull s Pd0:0.7 PD0:0.8 QgG0 G Tv >eg/eg.t22.split +rbox 100 D2 c G1.0 | qvoronoi s A-0.95 Gna Tv >eg/eg.t23.voronoi.imprecise +rbox 30 s D4 | qconvex s Gh Pd0d1d2D3 Tc >eg/eg.t24.intersect.precise.4d + +echo ==the following generates an error== >/dev/null +rbox 1000 D4 | qhull Q0 s Po R0.005 Ga Tc Tv >eg/eg.t25.neighbors.4d +echo ==the previous should generate an error== >/dev/null + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_test b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_test new file mode 100644 index 0000000000000000000000000000000000000000..bc1b3c280e74d2afc51e6578df4a09a22b922e52 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_test @@ -0,0 +1,385 @@ +#!/bin/sh +# +# NOTE: all tests duplicated in q_test.bat +set -v +echo === errors if 'user_eg' and 'user_eg2' not found === +echo === check user_eg ${d:-`date`} ===================== +user_eg "QR1 p n Qt" "v p" Fp +user_eg2 "QR1 p" "v p" Fp +echo === check front ends ${d:-`date`} ================== +qconvex - +qconvex . +qconvex +rbox c D3 | qconvex s n Qt +rbox c D2 | qconvex s i +rbox c D2 | qconvex o +rbox 1000 s | qconvex s Tv FA +rbox c d D2 | qconvex s Qc Fx +rbox y 1000 W0 | qconvex s n +rbox y 1000 W0 | qconvex s QJ +rbox d G1 D12 | qconvex QR0 FA +rbox c D6 | qconvex FA TF500 +rbox c P0 d D2 | qconvex p Fa Fc FP FI Fn FN FS Fv Fx +rbox c d D2 | qconvex s i QV0 +rbox c | qconvex Q0 +qvoronoi - +qvoronoi . +qvoronoi +rbox c P0 D2 | qvoronoi s o +rbox c P0 D2 | qvoronoi Fi Tv +rbox c P0 D2 | qvoronoi Fo +rbox c P0 D2 | qvoronoi Fv +rbox c P0 D2 | qvoronoi s Qu Qt Fv +rbox c P0 D2 | qvoronoi Qu Fo +rbox c G1 d D2 | qvoronoi s p +rbox c G1 d D2 | qvoronoi QJ p +rbox c P-0.1 P+0.1 P+0.1 D2 | qvoronoi s Fc FP FQ Fn FN +rbox P0 c D2 | qvoronoi s Fv QV0 +qdelaunay - +qdelaunay . +qdelaunay +rbox c P0 D2 | qdelaunay s o +rbox c P0 D2 | qdelaunay i +rbox c P0 D2 | qdelaunay Fv +rbox c P0 D2 | qdelaunay s Qu Qt Fv +rbox c G1 d D2 | qdelaunay s i +rbox c G1 d D2 | qhull d Qbb Ft +rbox c G1 d D2 | qhull d Qbb QJ s Ft +rbox M3,4 z 100 D2 | qdelaunay s +rbox c P-0.1 P+0.1 P+0.1 D2 | qdelaunay s Fx Fa Fc FP FQ Fn FN +rbox P0 P0 c D2 | qdelaunay s FP QV0 +qhalf - +qhalf . +qhalf +rbox d | qhull FQ n | qhalf s Qt H0,0,0 Fp +rbox c | qhull FQ FV n | qhalf s i +rbox c | qhull FQ FV n | qhalf o +rbox d D2 | qhull FQ n | qhalf s H0 Fc FP Fn FN FQ Fv Fx +echo === check quality of Qhull for ${d:-`hostname`} ${d:-`date`} +rbox 1000 W0 | qhull QR2 QJ s Fs Tv +rbox 1000 W0 | qhull QR2 s Fs Tv +rbox 1000 s | qhull C0.02 Qc Tv +rbox 500 s D4 | qhull C0.01 Qc Tv +rbox 1000 s | qhull C-0.02 Qc Tv +rbox 1000 s | qhull C-0.02 Qc Tv +rbox 1000 s D4 | qhull C-0.01 Qc Tv +rbox 200 s D5 | qhull C-0.01 Qx Qc Tv +rbox 100 s D6 | qhull C-0.001 Qx Qc Tv +rbox 1000 W1e-4 | qhull C-1e-6 Qc Tv +rbox 1000 W5e-4 D4 | qhull C-1e-5 Qc Tv +rbox 400 W1e-3 D5 | qhull C-1e-5 Qx Qc Tv +echo === check input format etc. ${d:-`date`} +qhull <<EOF +2 4 #;laskdjf +1 0 1 1 1 2 0 0 +EOF +qhull <<EOF +2 4 #;laskdjf +1 0 1 1 1 2 0 0 0 +EOF +qhull <<EOF +2 4 #;laskdjf +1 0 1 1 1 2 0 +EOF +qhull <<EOF +2 +4 #;laskdjf +1 #kjdfasdf +0 1 1 1 2 0 +EOF +qhull <<EOF +2 4 1 0 1 1 1 2 0 0 +EOF +qhull d Qz <<EOF +2 5 1 0 1 1 0 1 0 0 0 +EOF +qhull d Q8 Qz <<EOF +2 5 1 0 1 1 0 1 0 0 0 +EOF +rbox d h | qhull Fd FV n FD Tcv | qhull Fd H Fp Tcv +rbox 10 h | qhull Fd FD p Tcv | qhull Fd d Tcv +echo === check rbox ${d:-`date`} +rbox 3 n D2 +rbox 3 D2 +rbox 3 h D2 +rbox 3 z D2 +rbox 3 z h D2 +rbox 3 B10 D2 +rbox 3 z B10 D2 +rbox 4 L2 r D2 +rbox 8 L2 D2 +rbox 4 L4 r D3 +rbox 4 L4 s D5 W1e-3 +rbox y +rbox 10 M3,4 +rbox 10 L2 s D3 | qhull Tcv +rbox 10 L4 s W1e-3 D3 | qhull Tcv +rbox 10 L6 D3 | qhull Tcv +rbox 10 L1.1 s D4 | qhull Tcv +rbox y r 100 W0 O0.5 | qhull s p Tcv +rbox x r 100 W0 O0.5 | qhull s Tcv +rbox 12 D8 | qhull Tcv +rbox 12 D9 | qhull Tcv +rbox 1000 D4 | qhull s i A-0.97 C0.2 A0.7 Tcv +rbox 3 D2 | qhull Qb0B1:-2 p +rbox 100 r D2 | qhull Pd0:0.7 PD0:0.8 n Tcv +rbox 1000 s | qhull C0.05 Tcv +rbox 1000 s t | qhull Qm C0.05 Tcv +rbox 500 D2 | qhull n A-0.95 C0.1 Tcv +rbox 500 s P1,1,1 | qhull QgG0 Pp Tcv + +rbox d | qhull m +rbox d | qhull FM +rbox c D2 | qhull Tcv Q0 +rbox d D2 | qhull Tcv +rbox c D3 | qhull Tcv Q0 +rbox d D3 | qhull Tcv +rbox c D4 | qhull Tcv Q0 +rbox d D4 | qhull Tcv +rbox c D5 | qhull Tcv Q0 +rbox d D5 | qhull Tcv +rbox c D6 | qhull Tcv Q0 +rbox d D6 | qhull Tcv +rbox d D7 | qhull Tcv +rbox c D2 | qhull Tcv C-0 +rbox c D3 | qhull Tcv C-0 +rbox c D4 | qhull Tcv C-0 +rbox c D5 | qhull Tcv C-0 +rbox c D6 | qhull Tcv C-0 +rbox c D7 | qhull Tv C-0 +rbox 20 l D3 | qhull Tcv +rbox 100 s D2 | qhull Tcv +rbox 100 s D3 | qhull Tcv +rbox 100 s D4 | qhull Tcv +rbox 100 s c D4 | qhull Tcv +rbox 100 s d G1.5 D4 | qhull Tcv +rbox 100 s W1e-2 | qhull Tcv +rbox 100 | qhull Tcv +rbox 100 W1e-3 | qhull Tcv +rbox 100 r D2 | qhull Tcv +rbox 100 r s Z1 | qhull Tcv +rbox 100 r s Z1 G0.1 | qhull Tcv C-0 +rbox 100 s Z1 G0.1 | qhull Tcv +rbox 100 s Z1e-5 G0.1 | qhull Tc Pp + +echo === check qhull output formats ${d:-`date`} +rbox 5 r s D2 | qhull Tcv +rbox 5 r s D2 | qhull s +rbox 5 r s D2 | qhull s o +rbox 5 r s D2 | qhull f +rbox 5 r s D2 | qhull i +rbox 5 r s D2 | qhull m +rbox 5 r s D2 | qhull FM +rbox 5 r s D2 | qhull n +rbox 5 r s D2 | qhull p +rbox 5 r s D2 | qhull o +rbox 5 r s D2 | qhull Ft +rbox 5 r s D2 | qhull Fx +rbox 5 r s D2 | qhull p n i p p +rbox 10 D3 | qhull f Tcv +rbox 10 D3 | qhull i +rbox 10 D3 | qhull p +rbox 10 D3 | qhull o +rbox 10 D3 | qhull Fx +rbox 27 M1,0,1 | qhull Qc +rbox 50 D3 s | qhull C0.1 Qc Pd0d1d2 s p Tcv +rbox 10 D2 P0 P1e-15 | qhull d Qc FP s Tcv +rbox 100 s | qhull C-0.003 Qc FP s +rbox 100 s D2 | qhull C0.1 i Fx Tcv +rbox 4 s D3 | qhull Qc Ghipv Tcv +rbox 6 D4 | qhull f Tcv +rbox 6 D4 | qhull i +rbox 6 D4 | qhull p +rbox 6 D4 | qhull o +rbox 1000 s D2 | qhull FA Tcv +rbox 1000 s | qhull FA Tcv +rbox c D4 | qhull FA Tcv +rbox c D5 | qhull FA Tcv +rbox c D5 | qhull FA Qt Tcv +rbox 10 D2 | qhull d FA Tcv +rbox 10 D2 | qhull d Qu FA Tcv +rbox 10 D2 | qhull FA Tcv +rbox 10 c D2 | qhull Fx Tcv +rbox 1000 s | qhull FS Tcv +rbox 10 W0 D2 | qhull p Qc FcC Tcv +rbox 4 z h s D2 | qhull Fd s n FD Tcv +rbox 6 s D3 | qhull C-0.1 Qc FF s FQ Fi n Fo FQ FI Fm Fn FN FO FO FQ Fs FS FV Fv Tcv +rbox P0.5,0.5 P0.5,0.5 W0 5 D2 | qhull d FN Qc +rbox 10 D3 | qhull Fa PA5 +rbox 10 D3 | qhull Fa PF0.4 + +echo === test Qt ${d:-`date`} +rbox c | qhull Qt s o Tcv +rbox c | qhull Qt f i +rbox c | qhull Qt m FM n +rbox c | qhull Qt p o +rbox c | qhull Qt Fx +rbox c | qhull Qt FA s Fa +rbox 6 r s c G0.1 D2 | qhull Qt d FA Tcv +rbox 6 r s c G0.1 D2 | qhull d FA Tcv +rbox 6 r s c G0.1 D2 | qhull Qt v p Tcv +rbox c | qhull Qt C-0.1 Qc FF s FQ Fi n Fo FQ FI Fm Fn FN FO FO FQ Fs FS FV Fv Tcv +rbox 6 r s c G0.1 D2 P0.1,0.1 | qhull s FP d FO Qt +rbox 100 W0 | qhull Tv Q11 + +echo === test unbounded intersection ${d:-`date`} +rbox c | qhull PD0:0.5 n | qhull H0 Fp Tcv +rbox 1000 W1e-3 D3 | qhull PA8 Fa FS s n Tcv +rbox 1000 W1e-3 D3 | qhull C-0.01 PM10 Fm n Tcv Qc +rbox 1000 W1e-3 D3 | qhull C-0.01 PA8 PG n Tcv Qc +rbox 10 | qhull FO Tz TO q_test.log.1 +cat q_test.log.1 + +echo === check Delaunay/Voronoi ${d:-`date`} +rbox 10 D2 | qhull d Tcv +rbox 10 D2 | qhull d Tcv Qz +rbox 10 D3 | qhull d Tcv +rbox c | qhull d Qz Ft Tcv +rbox 10 s D2 c | qhull d Tcv +rbox 10 s D2 | qhull d Tcv Q8 Qz +rbox 10 D2 | qhull d Tcv p +rbox 10 D2 | qhull d Tcv i +rbox 10 D2 | qhull d Tcv o +rbox 10 D2 | qhull v Tcv o +rbox 10 D2 | qhull v Tcv p +rbox 10 D2 | qhull v Tcv G +rbox 10 D2 | qhull v Tcv Fv +rbox 10 D2 | qhull v Tcv Fi +rbox 10 D2 | qhull v Tcv Fo +rbox 10 D2 | qhull v Qu o Fv Fi Fo Tcv +rbox 10 D3 | qhull v Fv Tcv +rbox 10 D3 | qhull v Fi Tcv +rbox 10 D3 | qhull v Fo Tcv +rbox 10 D3 | qhull v Qu o Fv Fi Fo Tcv +rbox 5 D2 | qhull v f FnN o + +echo === check Halfspace ${d:-`date`} +rbox 100 s D4 | qhull FA FV n s Tcv | qhull H Fp Tcv | qhull FA Tcv +rbox d D3 | qhull FQ n s FD Tcv | qhull Fd H0.1,0.1 Fp FQ Tcv +rbox 5 r D2 | qhull s n Tcv | qhull H0 Fp Tcv + +echo === check qhull ${d:-`date`} +rbox 10 s D3 | qhull Tcv +rbox 10 s D3 | qhull f Pd0:0.5 Pd2 Tcv +rbox 10 s D3 | qhull f Tcv PD2:-0.5 +rbox 10 s D3 | qhull QR-1 +rbox 10 s D3 | qhull QR-40 +rbox 1000 D3 | qhull Tcvs +rbox 100 D3 | qhull T8 Tz TO q_test.log.1 +tail -10 q_test.log.1 +rm q_test.log.1 +rbox 100 s D3 | qhull TcvV-2 +rbox 100 s D3 | qhull TcvC2 +rbox 100 s D3 | qhull TcvV2 +rbox 100 s D3 | qhull T1cvV2P2 +rbox 100 s D3 | qhull TcvF100 +rbox 100 s D3 | qhull Qf Tcv +rbox 100 D3 | qhull Tcv +rbox 100 D3 | qhull Qs Tcv +rbox 100 D5 | qhull Qs Tcv +rbox 100 D3 | qhull Qr Tcv +rbox 100 D3 | qhull Qxv Tcv +rbox 100 D3 | qhull Qi f Pd0 Pd1 Pd2 Tcv +rbox c d | qhull Qc f Tcv +rbox c d | qhull Qc p Tcv +rbox 100 D3 | qhull QbB FO Tcv +rbox 1000 D2 B1e6 | qhull d Qbb FO Tcv +rbox 10 D3 | qhull QbB p Tcv +rbox 10 D3 | qhull Qbb p Tcv +rbox 10 D3 | qhull Qb0:-10B2:20 p Tcv +rbox 10 D3 | qhull Qb0:-10B2:20 p Tcv | qhull QbB p Tcv +rbox 10 D3 | qhull Qb1:0B1:0 d Tcv Q8 +rbox 10 D3 | qhull Qb1:0B1:0B2:0 d Tcv Q8 +rbox 10 D3 | qhull Qb1:0 d Tcv +rbox 10 D3 | qhull Qb1:0B1:0 Tcv +echo "== next command will error ${d:-`date`} ==" +rbox 10 D2 | qhull Qb1:1B1:1 Tcv +rbox 200 L20 D2 t | qhull FO Tcv C-0 +rbox 1000 L20 t | qhull FO Tcv C-0 +rbox 200 L20 D4 t | qhull FO Tcv C-0 +rbox 200 L20 D5 t | qhull FO Tcv Qx +rbox 1000 W1e-3 s D2 t | qhull d FO Tcv Qu Q0 +rbox 1000 W1e-3 s D2 t | qhull d FO Tcv Qu C-0 + +echo === check joggle and TRn ${d:-`date`} +rbox 100 W0 | qhull QJ1e-14 Qc TR100 Tv +rbox 100 W0 | qhull QJ1e-13 Qc TR100 Tv +rbox 100 W0 | qhull QJ1e-12 Qc TR100 Tv +rbox 100 W0 | qhull QJ1e-11 Qc TR100 Tv +rbox 100 W0 | qhull QJ1e-10 Qc TR100 Tv +rbox 100 | qhull d QJ Qb0:1e4 QB0:1e5 Qb1:1e4 QB1:1e6 Qb2:1e5 QB2:1e7 FO Tv + +echo === check precision options ${d:-`date`} +rbox 100 D3 s | qhull E0.01 Qx Tcv FO +rbox 100 D3 W1e-1 | qhull W1e-3 Tcv +rbox 100 D3 W1e-1 | qhull W1e-2 Tcv Q0 +rbox 100 D3 W1e-1 | qhull W1e-2 Tcv +rbox 100 D3 W1e-1 | qhull W1e-1 Tcv +rbox 15 D2 P0 P1e-14,1e-14 | qhull d Quc Tcv +rbox 15 D3 P0 P1e-12,1e-14,1e-14 | qhull d Qcu Tcv +rbox 1000 s D3 | qhull C-0.01 Tcv Qc +rbox 1000 s D3 | qhull C-0.01 V0 Qc Tcv +rbox 1000 s D3 | qhull C-0.01 U0 Qc Tcv +rbox 1000 s D3 | qhull C-0.01 V0 Qcm Tcv +rbox 1000 s D3 | qhull C-0.01 Qcm Tcv +rbox 1000 s D3 | qhull C-0.01 Q1 FO Tcv Qc +rbox 1000 s D3 | qhull C-0.01 Q2 FO Tcv Qc +rbox 1000 s D3 | qhull C-0.01 Q3 FO Tcv Qc +rbox 1000 s D3 | qhull C-0.01 Q4 FO Tcv Qc +echo === this may generate an error ${d:-`date`} +rbox 1000 s D3 | qhull C-0.01 Q5 FO Tcv +echo === this should generate an error ${d:-`date`} +rbox 1000 s D3 | qhull C-0.01 Q6 FO Po Tcv Qc +rbox 1000 s D3 | qhull C-0.01 Q7 FO Tcv Qc +rbox 1000 s D3 | qhull C-0.01 Qx Tcv Qc +rbox 100 s D3 t | qhull R1e-3 Tcv Qc +rbox 100 s D3 t | qhull R1e-2 Tcv Qc +rbox 500 s D3 t | qhull R0.05 A-1 Tcv Qc +rbox 100 W0 D3 t | qhull R1e-3 Tcv Qc +rbox 100 W0 D3 t | qhull R1e-3 Qx Tcv Qc +rbox 100 W0 D3 t | qhull R1e-2 Tcv Qc +rbox 100 W0 D3 t | qhull R1e-2 Qx Tcv Qc +rbox 500 W0 D3 t | qhull R0.05 A-1 Tcv Qc +rbox 500 W0 D3 t | qhull R0.05 Qx Tcv Qc +rbox 1000 W1e-20 t | qhull Tcv Qc +rbox 1000 W1e-20 D4 t | qhull Tcv Qc +rbox 500 W1e-20 D5 t | qhull Tv Qc +rbox 100 W1e-20 D6 t | qhull Tv Qc +rbox 50 W1e-20 D6 t | qhull Qv Tv Qc +rbox 10000 D4 t | qhull QR0 Qc C-0.01 A0.3 Tv +rbox 1000 D2 t | qhull d QR0 Qc C-1e-8 Qu Tv +rbox 300 D5 t |qhull A-0.999 Qx Qc Tcv +rbox 100 D6 t |qhull A-0.9999 Qx Qc Tcv +rbox 50 D7 t |qhull A-0.99999 Qx Qc Tcv W0.1 + +echo === check bad cases for Qhull. May cause errors ${d:-`date`} +rbox 1000 L100000 s G1e-6 t | qhull Tv +rbox 1000 L100000 s G1e-6 t | qhull Tv Q10 +rbox 1000 s Z1 G1e-13 t | qhull Tv +rbox 1000 s W1e-13 P0 t | qhull d Qbb Qc Tv +rbox 1000 s W1e-13 t | qhull d Tv +rbox 1000 s W1e-13 t D2 | qhull d Tv + +echo ======================================================= +echo ======================================================= +echo === The following commands may cause errors ${d:-`date`} +echo ======================================================= +echo ======================================================= +rbox c D7 | qhull Q0 Tcv +rbox 100 s D3 | qhull Q0 E1e-3 Tc Po +rbox 100 s D3 | qhull Q0 E1e-2 Tc Po +rbox 100 s D3 | qhull Q0 E1e-1 Tc Po +rbox 100 s D3 | qhull Q0 R1e-3 Tc Po +rbox 100 s D3 | qhull Q0 R1e-2 Tc Po +rbox 100 s D3 | qhull Q0 R0.05 Tc +rbox 100 s D3 | qhull Q0 R0.05 Tc Po +rbox 1000 W1e-7 | qhull Q0 Tc Po +rbox 50 s | qhull Q0 V0.05 W0.01 Tc Po +rbox 100 s D5 | qhull Q0 R1e-2 Tc Po +qhull +qhull . +qhull - +rbox +cat html/qhull.txt html/rbox.txt + +# end of q_test diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_test.bat b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_test.bat new file mode 100644 index 0000000000000000000000000000000000000000..81b679cfda0f7ebbe7e1da8b427c07b6d95d5a54 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/q_test.bat @@ -0,0 +1,666 @@ +echo q_test for Windows DOS box 2002/6/1 >q_test.x +echo === errors if 'user_eg' and 'user_eg2' not found === +echo === check user_eg === >>q_test.x +echo "user_eg 'QR1 p n Qt' 'v p' Fp" >>q_test.x +user_eg "QR1 p n Qt" "v p" Fp >>q_test.x +echo "user_eg2 'QR1 p' 'v p' Fp" >>q_test.x +user_eg2 "QR1 p" "v p" Fp >>q_test.x +echo === check front ends ========================================================== >>q_test.x +echo "qconvex -" >>q_test.x +qconvex - >>q_test.x +echo "qconvex ." >>q_test.x +qconvex . >>q_test.x +echo "qconvex" >>q_test.x +qconvex >>q_test.x +echo "rbox c D3 | qconvex s n Qt" >>q_test.x +rbox c D3 | qconvex s n Qt >>q_test.x +echo "rbox c D2 | qconvex i Qt " >>q_test.x +rbox c D2 | qconvex i Qt >>q_test.x +echo "rbox c D2 | qconvex o" >>q_test.x +rbox c D2 | qconvex o >>q_test.x +echo "rbox 1000 s | qconvex s Tv FA" >>q_test.x +rbox 1000 s | qconvex s Tv FA >>q_test.x +echo "rbox c d D2 | qconvex s Qc Fx" >>q_test.x +rbox c d D2 | qconvex s Qc Fx >>q_test.x +echo "rbox y 1000 W0 | qconvex s n " >>q_test.x +rbox y 1000 W0 | qconvex s n >>q_test.x +echo "rbox y 1000 W0 | qconvex s QJ" >>q_test.x +rbox y 1000 W0 | qconvex s QJ >>q_test.x +echo "rbox d G1 D12 | qconvex QR0 FA" >>q_test.x +rbox d G1 D12 | qconvex QR0 FA >>q_test.x +echo "rbox c D6 | qconvex FA TF500" >>q_test.x +rbox c D6 | qconvex FA TF500 >>q_test.x +echo "rbox c P0 d D2 | qconvex p Fa Fc FP FI Fn FN FS Fv Fx" >>q_test.x +rbox c P0 d D2 | qconvex p Fa Fc FP FI Fn FN FS Fv Fx >>q_test.x +echo "rbox c d D2 | qconvex s i QV0" >>q_test.x +rbox c d D2 | qconvex s i QV0 >>q_test.x +echo "rbox c | qconvex Q0" >>q_test.x +rbox c | qconvex Q0 >>q_test.x +echo "qvoronoi -" >>q_test.x +qvoronoi - >>q_test.x +echo "qvoronoi ." >>q_test.x +qvoronoi . >>q_test.x +echo "qvoronoi" >>q_test.x +qvoronoi >>q_test.x +echo "rbox c P0 D2 | qvoronoi s o" >>q_test.x +rbox c P0 D2 | qvoronoi s o >>q_test.x +echo "rbox c P0 D2 | qvoronoi Fi" >>q_test.x +rbox c P0 D2 | qvoronoi Fi >>q_test.x +echo "rbox c P0 D2 | qvoronoi Fo" >>q_test.x +rbox c P0 D2 | qvoronoi Fo >>q_test.x +echo "rbox c P0 D2 | qvoronoi Fv" >>q_test.x +rbox c P0 D2 | qvoronoi Fv >>q_test.x +echo "rbox c P0 D2 | qvoronoi s Qu Qt Fv" >>q_test.x +rbox c P0 D2 | qvoronoi s Qu Qt Fv >>q_test.x +echo "rbox c P0 D2 | qvoronoi Qu Fo" >>q_test.x +rbox c P0 D2 | qvoronoi Qu Fo >>q_test.x +echo "rbox c G1 d D2 | qvoronoi s p " >>q_test.x +rbox c G1 d D2 | qvoronoi s p >>q_test.x +echo "rbox c G1 d D2 | qvoronoi QJ p " >>q_test.x +rbox c G1 d D2 | qvoronoi QJ p >>q_test.x +echo "rbox c P-0.1 P+0.1 P+0.1 D2 | qvoronoi s Fc FP FQ Fn FN " >>q_test.x +rbox c P-0.1 P+0.1 P+0.1 D2 | qvoronoi s Fc FP FQ Fn FN >>q_test.x +echo "rbox P0 c D2 | qvoronoi s Fv QV0" >>q_test.x +rbox P0 c D2 | qvoronoi s Fv QV0 >>q_test.x +echo "qdelaunay -" >>q_test.x +qdelaunay - >>q_test.x +echo "qdelaunay ." >>q_test.x +qdelaunay . >>q_test.x +echo "qdelaunay" >>q_test.x +qdelaunay >>q_test.x +echo "rbox c P0 D2 | qdelaunay s o" >>q_test.x +rbox c P0 D2 | qdelaunay s o >>q_test.x +echo "rbox c P0 D2 | qdelaunay i" >>q_test.x +rbox c P0 D2 | qdelaunay i >>q_test.x +echo "rbox c P0 D2 | qdelaunay Fv" >>q_test.x +rbox c P0 D2 | qdelaunay Fv >>q_test.x +echo "rbox c P0 D2 | qdelaunay s Qu Qt Fv" >>q_test.x +rbox c P0 D2 | qdelaunay s Qu Qt Fv >>q_test.x +echo "rbox c G1 d D2 | qdelaunay s i" >>q_test.x +rbox c G1 d D2 | qdelaunay s i >>q_test.x +echo "rbox c G1 d D2 | qdelaunay Ft" >>q_test.x +rbox c G1 d D2 | qdelaunay Ft >>q_test.x +echo "rbox c G1 d D2 | qdelaunay QJ s Ft" >>q_test.x +rbox c G1 d D2 | qdelaunay QJ s Ft >>q_test.x +echo "rbox M3,4 z 100 D2 | qdelaunay s" >>q_test.x +rbox M3,4 z 100 D2 | qdelaunay s >>q_test.x +echo "rbox c P-0.1 P+0.1 P+0.1 D2 | qdelaunay s Fx Fa Fc FP FQ Fn FN" >>q_test.x +rbox c P-0.1 P+0.1 P+0.1 D2 | qdelaunay s Fx Fa Fc FP FQ Fn FN >>q_test.x +echo "rbox P0 P0 c D2 | qdelaunay s FP QV0" >>q_test.x +rbox P0 P0 c D2 | qdelaunay s FP QV0 >>q_test.x +echo "qhalf -" >>q_test.x +qhalf - >>q_test.x +echo "qhalf ." >>q_test.x +qhalf . >>q_test.x +echo "qhalf" >>q_test.x +qhalf >>q_test.x +echo "rbox d | qhull FQ n | qhalf s Qt H0,0,0 Fp" >>q_test.x +rbox d | qhull FQ n | qhalf s H0,0,0 Fp >>q_test.x +echo "rbox c | qhull FQ FV n | qhalf s i" >>q_test.x +rbox c | qhull FQ FV n | qhalf s i >>q_test.x +echo "rbox c | qhull FQ FV n | qhalf o" >>q_test.x +rbox c | qhull FQ FV n | qhalf o >>q_test.x +echo "rbox d D2 | qhull FQ n | qhalf s H0 Fc FP Fn FN FQ Fv Fx" >>q_test.x +rbox d D2 | qhull FQ n | qhalf s H0 Fc FP Fn FN FQ Fv Fx >>q_test.x + +echo === check quality of merges for ${d:-`hostname`} ${d:-`date`} >>q_test.x +echo "rbox 1000 W0 t | qhull QR2 QJ s Fs Tv" >>q_test.x +rbox 1000 W0 t | qhull QR2 QJ s Fs Tv >>q_test.x +echo "rbox 1000 W0 t | qhull QR2 s Fs Tv" >>q_test.x +rbox 1000 W0 t | qhull QR2 s Fs Tv >>q_test.x +echo "rbox 1000 s t | qhull C0.02 Qc Tv" >>q_test.x +rbox 1000 s t | qhull C0.02 Qc Tv >>q_test.x +echo "rbox 500 s D4 t | qhull C0.01 Qc Tv" >>q_test.x +rbox 500 s D4 t | qhull C0.01 Qc Tv >>q_test.x +echo "rbox 1000 s t | qhull C-0.02 Qc Tv" >>q_test.x +rbox 1000 s t | qhull C-0.02 Qc Tv >>q_test.x +echo "rbox 1000 s t | qhull C-0.02 Qc Tv" >>q_test.x +rbox 1000 s t | qhull C-0.02 Qc Tv >>q_test.x +echo "rbox 1000 s D4 t | qhull C-0.01 Qc Tv" >>q_test.x +rbox 1000 s D4 t | qhull C-0.01 Qc Tv >>q_test.x +echo "rbox 200 s D5 t | qhull C-0.01 Qx Qc Tv" >>q_test.x +rbox 200 s D5 t | qhull C-0.01 Qx Qc Tv >>q_test.x +echo "rbox 100 s D6 t | qhull C-0.001 Qx Qc Tv" >>q_test.x +rbox 100 s D6 t | qhull C-0.001 Qx Qc Tv >>q_test.x +echo "rbox 1000 W1e-4 t | qhull C-1e-6 Qc Tv" >>q_test.x +rbox 1000 W1e-4 t | qhull C-1e-6 Qc Tv >>q_test.x +echo "rbox 1000 W5e-4 D4 t | qhull C-1e-5 Qc Tv" >>q_test.x +rbox 1000 W5e-4 D4 t | qhull C-1e-5 Qc Tv >>q_test.x +echo "rbox 400 W1e-3 D5 t | qhull C-1e-5 Qx Qc Tv" >>q_test.x +rbox 400 W1e-3 D5 t | qhull C-1e-5 Qx Qc Tv >>q_test.x + +echo === check input format etc. ${d:-`date`} >>q_test.x +echo "rbox d h | qhull Fd FV n FD Tcv | qhull Fd H Fp Tcv" >>q_test.x +rbox d h | qhull Fd FV n FD Tcv | qhull Fd H Fp Tcv >>q_test.x +echo "rbox 10 h | qhull Fd FD p Tcv | qhull Fd d Tcv " >>q_test.x +rbox 10 h | qhull Fd FD p Tcv | qhull Fd d Tcv >>q_test.x + +echo === check rbox ${d:-`date`} >>q_test.x +echo "rbox 3 n D2" >>q_test.x +rbox 3 n D2 >>q_test.x +echo "rbox 3 D2" >>q_test.x +rbox 3 D2 >>q_test.x +echo "rbox 3 h D2" >>q_test.x +rbox 3 h D2 >>q_test.x +echo "rbox 3 z D2" >>q_test.x +rbox 3 z D2 >>q_test.x +echo "rbox 3 z h D2" >>q_test.x +rbox 3 z h D2 >>q_test.x +echo "rbox 3 B10 D2" >>q_test.x +rbox 3 B10 D2 >>q_test.x +echo "rbox 3 z B10 D2" >>q_test.x +rbox 3 z B10 D2 >>q_test.x +echo "rbox 4 L2 r D2" >>q_test.x +rbox 4 L2 r D2 >>q_test.x +echo "rbox 8 L2 D2" >>q_test.x +rbox 8 L2 D2 >>q_test.x +echo "rbox 4 L4 r D3" >>q_test.x +rbox 4 L4 r D3 >>q_test.x +echo "rbox y" >>q_test.x +rbox y >>q_test.x +echo "rbox 10 M3,4" >>q_test.x +rbox 10 M3,4 >>q_test.x +echo "rbox 4 L4 s D5 W1e-3 " >>q_test.x +rbox 4 L4 s D5 W1e-3 >>q_test.x +echo "rbox 10 L2 s D3 | qhull Tcv" >>q_test.x +rbox 10 L2 s D3 | qhull Tcv >>q_test.x +echo "rbox 10 L4 s W1e-3 D3 | qhull Tcv" >>q_test.x +rbox 10 L4 s W1e-3 D3 | qhull Tcv >>q_test.x +echo "rbox 10 L6 D3 | qhull Tcv" >>q_test.x +rbox 10 L6 D3 | qhull Tcv >>q_test.x +echo "rbox 10 L1.1 s D4 | qhull Tcv" >>q_test.x +rbox 10 L1.1 s D4 | qhull Tcv >>q_test.x +echo "rbox y r 100 W0 O0.5 | qhull s p Tcv" >>q_test.x +rbox y r 100 W0 O0.5 | qhull s p Tcv >>q_test.x +echo "rbox x r 100 W0 O0.5 | qhull s p Tcv" >>q_test.x +rbox x r 100 W0 O0.5 | qhull s p Tcv >>q_test.x +echo "rbox 12 D8 | qhull Tcv" >>q_test.x +rbox 12 D8 | qhull Tcv >>q_test.x +echo "rbox 12 D9 | qhull Tcv" >>q_test.x +rbox 12 D9 | qhull Tcv >>q_test.x +echo "rbox 1000 D4 | qhull s i A-0.97 C0.2 A0.7 Tcv" >>q_test.x +rbox 1000 D4 | qhull s i A-0.97 C0.2 A0.7 Tcv >>q_test.x +echo "rbox 3 D2 | qhull Qb0B1:-2 p " >>q_test.x +rbox 3 D2 | qhull Qb0B1:-2 p >>q_test.x +echo "rbox 100 r D2 | qhull Pd0:0.7 PD0:0.8 n Tcv" >>q_test.x +rbox 100 r D2 | qhull Pd0:0.7 PD0:0.8 n Tcv >>q_test.x +echo "rbox 1000 s | qhull C0.05 Tcv" >>q_test.x +rbox 1000 s | qhull C0.05 Tcv >>q_test.x +echo "rbox 1000 s t | qhull Qm C0.05 Tcv" >>q_test.x +rbox 1000 s t | qhull Qm C0.05 Tcv >>q_test.x +echo "rbox 500 D2 | qhull n A-0.95 C0.1 Tcv" >>q_test.x +rbox 500 D2 | qhull n A-0.95 C0.1 Tcv >>q_test.x +echo "rbox 500 s P1,1,1 | qhull QgG0 Pp Tcv" >>q_test.x +rbox 500 s P1,1,1 | qhull QgG0 Pp Tcv >>q_test.x + +echo "rbox d | qhull m" >>q_test.x +rbox d | qhull m >>q_test.x +echo "rbox d | qhull FM" >>q_test.x +rbox d | qhull FM >>q_test.x +echo "rbox c D2 | qhull Q0 Tcv" >>q_test.x +rbox c D2 | qhull Q0 Tcv >>q_test.x +echo "rbox d D2 | qhull Tcv" >>q_test.x +rbox d D2 | qhull Tcv >>q_test.x +echo "rbox c D3 | qhull Q0 Tcv" >>q_test.x +rbox c D3 | qhull Q0 Tcv >>q_test.x +echo "rbox d D3 | qhull Tcv" >>q_test.x +rbox d D3 | qhull Tcv >>q_test.x +echo "rbox c D4 | qhull Q0 Tcv" >>q_test.x +rbox c D4 | qhull Q0 Tcv >>q_test.x +echo "rbox d D4 | qhull Tcv" >>q_test.x +rbox d D4 | qhull Tcv >>q_test.x +echo "rbox c D5 | qhull Q0 Tcv" >>q_test.x +rbox c D5 | qhull Q0 Tcv >>q_test.x +echo "rbox d D5 | qhull Tcv" >>q_test.x +rbox d D5 | qhull Tcv >>q_test.x +echo "rbox c D6 | qhull Q0 Tcv" >>q_test.x +rbox c D6 | qhull Q0 Tcv >>q_test.x +echo "rbox d D6 | qhull Tcv" >>q_test.x +rbox d D6 | qhull Tcv >>q_test.x +echo "rbox d D7 | qhull Tcv" >>q_test.x +rbox d D7 | qhull Tcv >>q_test.x +echo "rbox c D2 | qhull Tcv C-0" >>q_test.x +rbox c D2 | qhull Tcv C-0 >>q_test.x +echo "rbox c D3 | qhull Tcv C-0" >>q_test.x +rbox c D3 | qhull Tcv C-0 >>q_test.x +echo "rbox c D4 | qhull Tcv C-0" >>q_test.x +rbox c D4 | qhull Tcv C-0 >>q_test.x +echo "rbox c D5 | qhull Tcv C-0" >>q_test.x +rbox c D5 | qhull Tcv C-0 >>q_test.x +echo "rbox c D6 | qhull Tcv C-0" >>q_test.x +rbox c D6 | qhull Tcv C-0 >>q_test.x +echo "rbox c D7 | qhull Tv C-0" >>q_test.x +rbox c D7 | qhull Tv C-0 >>q_test.x +echo "rbox 20 l D3 | qhull Tcv" >>q_test.x +rbox 20 l D3 | qhull Tcv >>q_test.x +echo "rbox 100 s D2 | qhull Tcv" >>q_test.x +rbox 100 s D2 | qhull Tcv >>q_test.x +echo "rbox 100 s D3 | qhull Tcv" >>q_test.x +rbox 100 s D3 | qhull Tcv >>q_test.x +echo "rbox 100 s D4 | qhull Tcv" >>q_test.x +rbox 100 s D4 | qhull Tcv >>q_test.x +echo "rbox 100 s c D4 | qhull Tcv" >>q_test.x +rbox 100 s c D4 | qhull Tcv >>q_test.x +echo "rbox 100 s d G1.5 D4 | qhull Tcv" >>q_test.x +rbox 100 s d G1.5 D4 | qhull Tcv >>q_test.x +echo "rbox 100 s W1e-2 | qhull Tcv" >>q_test.x +rbox 100 s W1e-2 | qhull Tcv >>q_test.x +echo "rbox 100 | qhull Tcv" >>q_test.x +rbox 100 | qhull Tcv >>q_test.x +echo "rbox 100 W1e-3 | qhull Tcv" >>q_test.x +rbox 100 W1e-3 | qhull Tcv >>q_test.x +echo "rbox 100 r D2 | qhull Tcv" >>q_test.x +rbox 100 r D2 | qhull Tcv >>q_test.x +echo "rbox 100 r s Z1 | qhull Tcv" >>q_test.x +rbox 100 r s Z1 | qhull Tcv >>q_test.x +echo "rbox 100 r s Z1 G0.1 | qhull Tcv C-0" >>q_test.x +rbox 100 r s Z1 G0.1 | qhull Tcv C-0 >>q_test.x +echo "rbox 100 s Z1 G0.1 | qhull Tcv " >>q_test.x +rbox 100 s Z1 G0.1 | qhull Tcv >>q_test.x +echo "rbox 100 s Z1e-5 G0.1 | qhull Tc Pp" >>q_test.x +rbox 100 s Z1e-5 G0.1 | qhull Tc Pp >>q_test.x + +echo === check qhull output formats ${d:-`date`} >>q_test.x +echo "rbox 5 r s D2 | qhull Tcv" >>q_test.x +rbox 5 r s D2 | qhull Tcv >>q_test.x +echo "rbox 5 r s D2 | qhull s " >>q_test.x +rbox 5 r s D2 | qhull s >>q_test.x +echo "rbox 5 r s D2 | qhull s o " >>q_test.x +rbox 5 r s D2 | qhull s o >>q_test.x +echo "rbox 5 r s D2 | qhull f" >>q_test.x +rbox 5 r s D2 | qhull f >>q_test.x +echo "rbox 5 r s D2 | qhull i " >>q_test.x +rbox 5 r s D2 | qhull i >>q_test.x +echo "rbox 5 r s D2 | qhull m " >>q_test.x +rbox 5 r s D2 | qhull m >>q_test.x +echo "rbox 5 r s D2 | qhull FM " >>q_test.x +rbox 5 r s D2 | qhull FM >>q_test.x +echo "rbox 5 r s D2 | qhull n " >>q_test.x +rbox 5 r s D2 | qhull n >>q_test.x +echo "rbox 5 r s D2 | qhull p " >>q_test.x +rbox 5 r s D2 | qhull p >>q_test.x +echo "rbox 5 r s D2 | qhull o " >>q_test.x +rbox 5 r s D2 | qhull o >>q_test.x +echo "rbox 5 r s D2 | qhull Fx" >>q_test.x +rbox 5 r s D2 | qhull Fx >>q_test.x +echo "rbox 5 r s D2 | qhull p n i p p" >>q_test.x +rbox 5 r s D2 | qhull p n i p p >>q_test.x +echo "rbox 10 D3 | qhull f Tcv" >>q_test.x +rbox 10 D3 | qhull f Tcv >>q_test.x +echo "rbox 10 D3 | qhull i" >>q_test.x +rbox 10 D3 | qhull i >>q_test.x +echo "rbox 10 D3 | qhull p " >>q_test.x +rbox 10 D3 | qhull p >>q_test.x +echo "rbox 10 D3 | qhull o " >>q_test.x +rbox 10 D3 | qhull o >>q_test.x +echo "rbox 10 D3 | qhull Fx " >>q_test.x +rbox 10 D3 | qhull Fx >>q_test.x +echo "rbox 27 M1,0,1 | qhull Qc" >>q_test.x +rbox 27 M1,0,1 | qhull Qc >>q_test.x +echo "rbox 50 D3 s | qhull C0.1 Qc Pd0d1d2 s p Tcv" >>q_test.x +rbox 50 D3 s | qhull C0.1 Qc Pd0d1d2 s p Tcv >>q_test.x +echo "rbox 10 D2 P0 P1e-15 | qhull d Qc FP s Tcv" >>q_test.x +rbox 10 D2 P0 P1e-15 | qhull d Qc FP s Tcv >>q_test.x +echo "rbox 100 s | qhull C-0.003 Qc FP s" >>q_test.x +rbox 100 s | qhull C-0.003 Qc FP s >>q_test.x +echo "rbox 100 s D2 | qhull C0.1 i Fx Tcv" >>q_test.x +rbox 100 s D2 | qhull C0.1 i Fx Tcv >>q_test.x +echo "rbox 4 s D3 | qhull Qc Ghipv Tcv " >>q_test.x +rbox 4 s D3 | qhull Qc Ghipv Tcv >>q_test.x +echo "rbox 6 D4 | qhull f Tcv" >>q_test.x +rbox 6 D4 | qhull f Tcv >>q_test.x +echo "rbox 6 D4 | qhull i" >>q_test.x +rbox 6 D4 | qhull i >>q_test.x +echo "rbox 6 D4 | qhull p " >>q_test.x +rbox 6 D4 | qhull p >>q_test.x +echo "rbox 6 D4 | qhull o" >>q_test.x +rbox 6 D4 | qhull o >>q_test.x +echo "rbox 1000 s D2 | qhull FA Tcv" >>q_test.x +rbox 1000 s D2 | qhull FA Tcv >>q_test.x +echo "rbox 1000 s | qhull FA Tcv" >>q_test.x +rbox 1000 s | qhull FA Tcv >>q_test.x +echo "rbox c D4 | qhull FA Tcv" >>q_test.x +rbox c D4 | qhull FA Tcv >>q_test.x +echo "rbox c D5 | qhull FA Tcv" >>q_test.x +rbox c D5 | qhull FA Tcv >>q_test.x +echo "rbox 10 D2 | qhull d FA Tcv" >>q_test.x +rbox 10 D2 | qhull d FA Tcv >>q_test.x +echo "rbox 10 D2 | qhull d Qu FA Tcv" >>q_test.x +rbox 10 D2 | qhull d Qu FA Tcv >>q_test.x +echo "rbox 10 D2 | qhull FA Tcv" >>q_test.x +rbox 10 D2 | qhull FA Tcv >>q_test.x +echo "rbox 10 c D2 | qhull Fx Tcv" >>q_test.x +rbox 10 c D2 | qhull Fx Tcv >>q_test.x +echo "rbox 1000 s | qhull FS Tcv" >>q_test.x +rbox 1000 s | qhull FS Tcv >>q_test.x +echo "rbox 10 W0 D2 | qhull p Qc FcC Tcv" >>q_test.x +rbox 10 W0 D2 | qhull p Qc FcC Tcv >>q_test.x +echo "rbox 4 z h s D2 | qhull Fd s n FD Tcv" >>q_test.x +rbox 4 z h s D2 | qhull Fd s n FD Tcv >>q_test.x +echo "rbox 6 s D3 | qhull C-0.1 Qc FF s FQ Fi n Fo FQ FI Fm Fn FN FO FO FQ Fs FS FV Fv Tcv" >>q_test.x +rbox 6 s D3 | qhull C-0.1 Qc FF s FQ Fi n Fo FQ FI Fm Fn FN FO FO FQ Fs FS FV Fv Tcv >>q_test.x +echo "rbox P0.5,0.5 P0.5,0.5 W0 5 D2 | qhull d FN Qc" >>q_test.x +rbox P0.5,0.5 P0.5,0.5 W0 5 D2 | qhull d FN Qc >>q_test.x +echo "rbox 10 D3 | qhull Fa PA5" >>q_test.x +rbox 10 D3 | qhull Fa PA5 >>q_test.x +echo "rbox 10 D3 | qhull Fa PF0.4" >>q_test.x +rbox 10 D3 | qhull Fa PF0.4 >>q_test.x + +echo === test Qt >>q_test.x +echo "rbox c | qhull Qt s o Tcv" >>q_test.x +rbox c | qhull Qt s o Tcv >>q_test.x +echo "rbox c | qhull Qt f i" >>q_test.x +rbox c | qhull Qt f i >>q_test.x +echo "rbox c | qhull Qt m FM n" >>q_test.x +rbox c | qhull Qt m FM n >>q_test.x +echo "rbox c | qhull Qt p o" >>q_test.x +rbox c | qhull Qt p o >>q_test.x +echo "rbox c | qhull Qt Fx" >>q_test.x +rbox c | qhull Qt Fx >>q_test.x +echo "rbox c | qhull Qt FA s Fa" >>q_test.x +rbox c | qhull Qt FA s Fa >>q_test.x +echo "rbox 6 r s c G0.1 D2 | qhull Qt d FA Tcv" >>q_test.x +rbox 6 r s c G0.1 D2 | qhull Qt d FA Tcv >>q_test.x +echo "rbox 6 r s c G0.1 D2 | qhull d FA Tcv" >>q_test.x +rbox 6 r s c G0.1 D2 | qhull d FA Tcv >>q_test.x +echo "rbox 6 r s c G0.1 D2 | qhull Qt v p Tcv" >>q_test.x +rbox 6 r s c G0.1 D2 | qhull Qt v p Tcv >>q_test.x +echo "rbox c | qhull Qt C-0.1 Qc FF s FQ Fi n Fo FQ FI Fm Fn FN FO FO FQ Fs FS FV Fv Tcv" >>q_test.x +rbox c | qhull Qt C-0.1 Qc FF s FQ Fi n Fo FQ FI Fm Fn FN FO FO FQ Fs FS FV Fv Tcv >>q_test.x +echo "rbox 6 r s c G0.1 D2 P0.1,0.1 | qhull s FP d FO Qt" >>q_test.x +rbox 6 r s c G0.1 D2 P0.1,0.1 | qhull s FP d FO Qt >>q_test.x +echo "RBOX 100 W0 | QHULL Tv Q11 FO" >>q_test.x +RBOX 100 W0 | QHULL Tv Q11 FO >>q_test.x + +echo === test unbounded intersection >>q_test.x +echo "rbox c | qhull PD0:0.5 n | qhull H0 Fp Tcv" >>q_test.x +rbox c | qhull PD0:0.5 n | qhull H0 Fp Tcv >>q_test.x +echo "rbox 1000 W1e-3 D3 | qhull PA8 Fa FS s n Tcv " >>q_test.x +rbox 1000 W1e-3 D3 | qhull PA8 Fa FS s n Tcv >>q_test.x +echo "rbox 1000 W1e-3 D3 | qhull C-0.01 PM10 Fm n Tcv Qc" >>q_test.x +rbox 1000 W1e-3 D3 | qhull C-0.01 PM10 Fm n Tcv Qc >>q_test.x +echo "rbox 1000 W1e-3 D3 | qhull C-0.01 PA8 PG n Tcv Qc" >>q_test.x +rbox 1000 W1e-3 D3 | qhull C-0.01 PA8 PG n Tcv Qc >>q_test.x +rbox 10 | qhull FO Tz TO q_test.log.1 +echo "type q_test.log.1" >>q_test.x +type q_test.log.1 >>q_test.x + +echo === check Delaunay/Voronoi ${d:-`date`} >>q_test.x +echo "rbox 10 D2 | qhull d Tcv" >>q_test.x +rbox 10 D2 | qhull d Tcv >>q_test.x +echo "rbox 10 D2 | qhull d Qz Tcv" >>q_test.x +rbox 10 D2 | qhull d Qz Tcv >>q_test.x +echo "rbox 10 D3 | qhull d Tcv" >>q_test.x +rbox 10 D3 | qhull d Tcv >>q_test.x +echo "rbox c | qhull d Qz Ft Tcv" >>q_test.x +rbox c | qhull d Qz Ft Tcv >>q_test.x +echo "rbox 10 s D2 c | qhull d Tcv" >>q_test.x +rbox 10 s D2 c | qhull d Tcv >>q_test.x +echo "rbox 10 s D2 | qhull d Tcv Qz Q8" >>q_test.x +rbox 10 s D2 | qhull d Tcv Qz Q8 >>q_test.x +echo "rbox 10 D2 | qhull d Tcv p" >>q_test.x +rbox 10 D2 | qhull d Tcv p >>q_test.x +echo "rbox 10 D2 | qhull d Tcv i" >>q_test.x +rbox 10 D2 | qhull d Tcv i >>q_test.x +echo "rbox 10 D2 | qhull d Tcv o" >>q_test.x +rbox 10 D2 | qhull d Tcv o >>q_test.x +echo "rbox 10 D2 | qhull v Tcv o" >>q_test.x +rbox 10 D2 | qhull v Tcv o >>q_test.x +echo "rbox 10 D2 | qhull v Tcv p" >>q_test.x +rbox 10 D2 | qhull v Tcv p >>q_test.x +echo "rbox 10 D2 | qhull v Tcv G" >>q_test.x +rbox 10 D2 | qhull v Tcv G >>q_test.x +echo "rbox 10 D2 | qhull v Tcv Fv" >>q_test.x +rbox 10 D2 | qhull v Tcv Fv >>q_test.x +echo "rbox 10 D2 | qhull v Tcv Fi" >>q_test.x +rbox 10 D2 | qhull v Tcv Fi >>q_test.x +echo "rbox 10 D2 | qhull v Tcv Fo" >>q_test.x +rbox 10 D2 | qhull v Tcv Fo >>q_test.x +echo "rbox 10 D2 | qhull v Qu Tcv o Fv Fi Fo" >>q_test.x +rbox 10 D2 | qhull v Qu Tcv o Fv Fi Fo >>q_test.x +echo "rbox 10 D3 | qhull v Tcv Fv" >>q_test.x +rbox 10 D3 | qhull v Tcv Fv >>q_test.x +echo "rbox 10 D3 | qhull v Tcv Fi" >>q_test.x +rbox 10 D3 | qhull v Tcv Fi >>q_test.x +echo "rbox 10 D3 | qhull v Tcv Fo" >>q_test.x +rbox 10 D3 | qhull v Tcv Fo >>q_test.x +echo "rbox 10 D3 | qhull v Qu Tcv o Fv Fi Fo" >>q_test.x +rbox 10 D3 | qhull v Qu Tcv o Fv Fi Fo >>q_test.x +echo "rbox 5 D2 | qhull v f FnN o" >>q_test.x +rbox 5 D2 | qhull v f FnN o >>q_test.x + +echo === check Halfspace ${d:-`date`} >>q_test.x +echo "rbox 100 s D4 | qhull FA FV n s Tcv | qhull H Fp Tcv | qhull FA Tcv" >>q_test.x +rbox 100 s D4 | qhull FA s Tcv >>q_test.x +rbox 100 s D4 | qhull FA FV n s Tcv | qhull H Fp Tcv | qhull FA Tcv >>q_test.x +echo "rbox d D3 | qhull FQ n s FD Tcv | qhull Fd H0.1,0.1 Fp FQ Tcv" >>q_test.x +rbox d D3 | qhull s >>q_test.x +rbox d D3 | qhull FQ n FD Tcv | qhull Fd H0.1,0.1 Fp FQ Tcv >>q_test.x +echo "rbox 5 r D2 | qhull s n Tcv | qhull H0 Fp Tcv" >>q_test.x +rbox 5 r D2 | qhull >>q_test.x +rbox 5 r D2 | qhull n Tcv | qhull H0 Fp Tcv >>q_test.x + +echo === check qhull ${d:-`date`} >>q_test.x +echo "rbox 10 s D3 | qhull Tcv" >>q_test.x +rbox 10 s D3 | qhull Tcv >>q_test.x +echo "rbox 10 s D3 | qhull f Pd0:0.5 Pd2 Tcv" >>q_test.x +rbox 10 s D3 | qhull f Pd0:0.5 Pd2 Tcv >>q_test.x +echo "rbox 10 s D3 | qhull f Tcv PD2:-0.5" >>q_test.x +rbox 10 s D3 | qhull f Tcv PD2:-0.5 >>q_test.x +echo "rbox 10 s D3 | qhull QR-1" >>q_test.x +rbox 10 s D3 | qhull QR-1 >>q_test.x +echo "rbox 10 s D3 | qhull QR-40" >>q_test.x +rbox 10 s D3 | qhull QR-40 >>q_test.x +echo "rbox 1000 D3 | qhull Tcvs" >>q_test.x +rbox 1000 D3 | qhull Tcvs >>q_test.x +echo "rbox 100 s D3 | qhull TcvV-2" >>q_test.x +rbox 100 s D3 | qhull TcvV-2 >>q_test.x +echo "rbox 100 s D3 | qhull TcvC2" >>q_test.x +rbox 100 s D3 | qhull TcvC2 >>q_test.x +echo "rbox 100 s D3 | qhull TcvV2" >>q_test.x +rbox 100 s D3 | qhull TcvV2 >>q_test.x +echo "rbox 100 s D3 | qhull T1cvV2P2" >>q_test.x +rbox 100 s D3 | qhull T1cvV2P2 >>q_test.x +echo "rbox 100 s D3 | qhull TcvF100" >>q_test.x +rbox 100 s D3 | qhull TcvF100 >>q_test.x +echo "rbox 100 s D3 | qhull Qf Tcv" >>q_test.x +rbox 100 s D3 | qhull Qf Tcv >>q_test.x +echo "rbox 100 D3 | qhull Tcv" >>q_test.x +rbox 100 D3 | qhull Tcv >>q_test.x +echo "rbox 100 D3 | qhull Qs Tcv" >>q_test.x +rbox 100 D3 | qhull Qs Tcv >>q_test.x +echo "rbox 100 D5 | qhull Qs Tcv" >>q_test.x +rbox 100 D5 | qhull Qs Tcv >>q_test.x +echo "rbox 100 D3 | qhull Qr Tcv" >>q_test.x +rbox 100 D3 | qhull Qr Tcv >>q_test.x +echo "rbox 100 D3 | qhull Qxv Tcv" >>q_test.x +rbox 100 D3 | qhull Qxv Tcv >>q_test.x +echo "rbox 100 D3 | qhull Qi f Pd0 Pd1 Pd2 Tcv" >>q_test.x +rbox 100 D3 | qhull Qi f Pd0 Pd1 Pd2 Tcv >>q_test.x +echo "rbox c d | qhull Qc f Tcv" >>q_test.x +rbox c d | qhull Qc f Tcv >>q_test.x +echo "rbox c d | qhull Qc p Tcv" >>q_test.x +rbox c d | qhull Qc p Tcv >>q_test.x +echo "rbox 100 D3 | qhull QbB FO Tcv" >>q_test.x +rbox 100 D3 | qhull QbB FO Tcv >>q_test.x +echo "rbox 1000 D2 B1e6 | qhull d Qbb FO Tcv" >>q_test.x +rbox 1000 D2 B1e6 | qhull d Qbb FO Tcv >>q_test.x +echo "rbox 10 D3 | qhull QbB p Tcv" >>q_test.x +rbox 10 D3 | qhull QbB p Tcv >>q_test.x +echo "rbox 10 D3 | qhull Qbb p Tcv" >>q_test.x +rbox 10 D3 | qhull Qbb p Tcv >>q_test.x +echo "rbox 10 D3 | qhull Qb0:-10B2:20 p Tcv" >>q_test.x +rbox 10 D3 | qhull Qb0:-10B2:20 p Tcv >>q_test.x +echo "rbox 10 D3 | qhull Qb0:-10B2:20 p Tcv | qhull QbB p Tcv" >>q_test.x +rbox 10 D3 | qhull Qb0:-10B2:20 p Tcv | qhull QbB p Tcv >>q_test.x +echo "rbox 10 D3 | qhull Qb1:0B1:0 d Tcv Q8" >>q_test.x +rbox 10 D3 | qhull Qb1:0B1:0 d Tcv Q8 >>q_test.x +echo "rbox 10 D3 | qhull Qb1:0B1:0B2:0 d Tcv Q8" >>q_test.x +rbox 10 D3 | qhull Qb1:0B1:0B2:0 d Tcv Q8 >>q_test.x +echo "rbox 10 D3 | qhull Qb1:0 d Tcv " >>q_test.x +rbox 10 D3 | qhull Qb1:0 d Tcv >>q_test.x +echo "rbox 10 D3 | qhull Qb1:0B1:0 Tcv" >>q_test.x +rbox 10 D3 | qhull Qb1:0B1:0 Tcv >>q_test.x +echo "== next command will error ==" >>q_test.x +echo "rbox 10 D2 | qhull Qb1:1B1:1 Tcv" >>q_test.x +rbox 10 D2 | qhull Qb1:1B1:1 Tcv >>q_test.x +echo "rbox 200 L20 D2 t | qhull FO Tcv C-0" >>q_test.x +rbox 200 L20 D2 t | qhull FO Tcv C-0 >>q_test.x +echo "rbox 1000 L20 t | qhull FO Tcv C-0" >>q_test.x +rbox 1000 L20 t | qhull FO Tcv C-0 >>q_test.x +echo "rbox 200 L20 D4 t | qhull FO Tcv C-0" >>q_test.x +rbox 200 L20 D4 t | qhull FO Tcv C-0 >>q_test.x +echo "rbox 200 L20 D5 t | qhull FO Tcv Qx" >>q_test.x +rbox 200 L20 D5 t | qhull FO Tcv Qx >>q_test.x +echo "rbox 1000 W1e-3 s D2 t | qhull d FO Tcv Qu Q0" >>q_test.x +rbox 1000 W1e-3 s D2 t | qhull d FO Tcv Qu Q0 >>q_test.x +echo "rbox 1000 W1e-3 s D2 t | qhull d FO Tcv Qu C-0" >>q_test.x +rbox 1000 W1e-3 s D2 t | qhull d FO Tcv Qu C-0 >>q_test.x + +echo === check joggle and TRn ${d:-`date`} >>q_test.x +echo "rbox 100 W0 | qhull QJ1e-14 Qc TR100 Tv" >>q_test.x +rbox 100 W0 | qhull QJ1e-14 Qc TR100 Tv >>q_test.x +echo "rbox 100 W0 | qhull QJ1e-13 Qc TR100 Tv" >>q_test.x +rbox 100 W0 | qhull QJ1e-13 Qc TR100 Tv >>q_test.x +echo "rbox 100 W0 | qhull QJ1e-12 Qc TR100 Tv" >>q_test.x +rbox 100 W0 | qhull QJ1e-12 Qc TR100 Tv >>q_test.x +echo "rbox 100 W0 | qhull QJ1e-11 Qc TR100 Tv" >>q_test.x +rbox 100 W0 | qhull QJ1e-11 Qc TR100 Tv >>q_test.x +echo "rbox 100 W0 | qhull QJ1e-10 Qc TR100 Tv" >>q_test.x +rbox 100 W0 | qhull QJ1e-10 Qc TR100 Tv >>q_test.x +echo "rbox 100 | qhull d QJ Qb0:1e4 QB0:1e5 Qb1:1e4 QB1:1e6 Qb2:1e5 QB2:1e7 FO Tv" >>q_test.x +rbox 100 | qhull d QJ Qb0:1e4 QB0:1e5 Qb1:1e4 QB1:1e6 Qb2:1e5 QB2:1e7 FO Tv >>q_test.x + +echo === check precision options ${d:-`date`} >>q_test.x +echo "rbox 100 D3 s | qhull E0.01 Qx Tcv FO" >>q_test.x +rbox 100 D3 s | qhull E0.01 Qx Tcv FO >>q_test.x +echo "rbox 100 D3 W1e-1 | qhull W1e-3 Tcv" >>q_test.x +rbox 100 D3 W1e-1 | qhull W1e-3 Tcv >>q_test.x +echo "rbox 100 D3 W1e-1 | qhull W1e-2 Tcv Q0" >>q_test.x +rbox 100 D3 W1e-1 | qhull W1e-2 Tcv Q0 >>q_test.x +echo "rbox 100 D3 W1e-1 | qhull W1e-2 Tcv" >>q_test.x +rbox 100 D3 W1e-1 | qhull W1e-2 Tcv >>q_test.x +echo "rbox 100 D3 W1e-1 | qhull W1e-1 Tcv" >>q_test.x +rbox 100 D3 W1e-1 | qhull W1e-1 Tcv >>q_test.x +echo "rbox 15 D2 P0 P1e-14,1e-14 | qhull d Quc Tcv" >>q_test.x +rbox 15 D2 P0 P1e-14,1e-14 | qhull d Quc Tcv >>q_test.x +echo "rbox 15 D3 P0 P1e-12,1e-14,1e-14 | qhull d Qcu Tcv" >>q_test.x +rbox 15 D3 P0 P1e-12,1e-14,1e-14 | qhull d Qcu Tcv >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Tcv Qc >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 V0 Qc Tcv" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 V0 Qc Tcv >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 U0 Qc Tcv" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 U0 Qc Tcv >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 V0 Qcm Tcv" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 V0 Qcm Tcv >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Qcm Tcv" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Qcm Tcv >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Q1 FO Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Q1 FO Tcv Qc >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Q2 FO Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Q2 FO Tcv Qc >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Q3 FO Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Q3 FO Tcv Qc >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Q4 FO Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Q4 FO Tcv Qc >>q_test.x +echo === this may generate an error ${d:-`date`} >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Q5 FO Tcv " >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Q5 FO Tcv >>q_test.x +echo === this should generate an error ${d:-`date`} >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Q6 FO Po Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Q6 FO Po Tcv Qc >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Q7 FO Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Q7 FO Tcv Qc >>q_test.x +echo "rbox 1000 s D3 | qhull C-0.01 Qx Tcv Qc" >>q_test.x +rbox 1000 s D3 | qhull C-0.01 Qx Tcv Qc >>q_test.x +echo "rbox 100 s D3 t | qhull R1e-3 Tcv Qc" >>q_test.x +rbox 100 s D3 t | qhull R1e-3 Tcv Qc >>q_test.x +echo "rbox 100 s D3 t | qhull R1e-2 Tcv Qc" >>q_test.x +rbox 100 s D3 t | qhull R1e-2 Tcv Qc >>q_test.x +echo "rbox 500 s D3 t | qhull R0.05 A-1 Tcv Qc" >>q_test.x +rbox 500 s D3 t | qhull R0.05 A-1 Tcv Qc >>q_test.x +echo "rbox 100 W0 D3 t | qhull R1e-3 Tcv Qc" >>q_test.x +rbox 100 W0 D3 t | qhull R1e-3 Tcv Qc >>q_test.x +echo "rbox 100 W0 D3 t | qhull R1e-3 Qx Tcv Qc" >>q_test.x +rbox 100 W0 D3 t | qhull R1e-3 Qx Tcv Qc >>q_test.x +echo "rbox 100 W0 D3 t | qhull R1e-2 Tcv Qc" >>q_test.x +rbox 100 W0 D3 t | qhull R1e-2 Tcv Qc >>q_test.x +echo "rbox 100 W0 D3 t | qhull R1e-2 Qx Tcv Qc" >>q_test.x +rbox 100 W0 D3 t | qhull R1e-2 Qx Tcv Qc >>q_test.x +echo "rbox 500 W0 D3 t | qhull R0.05 A-1 Tcv Qc" >>q_test.x +rbox 500 W0 D3 t | qhull R0.05 A-1 Tcv Qc >>q_test.x +echo "rbox 500 W0 D3 t | qhull R0.05 Qx Tcv Qc" >>q_test.x +rbox 500 W0 D3 t | qhull R0.05 Qx Tcv Qc >>q_test.x +echo "rbox 1000 W1e-20 t | qhull Tcv Qc" >>q_test.x +rbox 1000 W1e-20 t | qhull Tcv Qc >>q_test.x +echo "rbox 1000 W1e-20 D4 t | qhull Tcv Qc" >>q_test.x +rbox 1000 W1e-20 D4 t | qhull Tcv Qc >>q_test.x +echo "rbox 500 W1e-20 D5 t | qhull Tv Qc" >>q_test.x +rbox 500 W1e-20 D5 t | qhull Tv Qc >>q_test.x +echo "rbox 100 W1e-20 D6 t | qhull Tv Qc" >>q_test.x +rbox 100 W1e-20 D6 t | qhull Tv Qc >>q_test.x +echo "rbox 50 W1e-20 D6 t | qhull Qv Tv Qc" >>q_test.x +rbox 50 W1e-20 D6 t | qhull Qv Tv Qc >>q_test.x +echo "rbox 10000 D4 t | qhull QR0 Qc C-0.01 A0.3 Tv" >>q_test.x +rbox 10000 D4 t | qhull QR0 Qc C-0.01 A0.3 Tv >>q_test.x +echo "rbox 1000 D2 t | qhull d QR0 Qc C-1e-8 Qu Tv" >>q_test.x +rbox 1000 D2 t | qhull d QR0 Qc C-1e-8 Qu Tv >>q_test.x +echo "rbox 300 D5 t |qhull A-0.999 Qc Tcv" >>q_test.x +rbox 300 D5 t |qhull A-0.999 Qc Tcv >>q_test.x +echo "rbox 100 D6 t |qhull A-0.9999 Qc Tcv" >>q_test.x +rbox 100 D6 t |qhull A-0.9999 Qc Tcv >>q_test.x +echo "rbox 50 D7 t |qhull A-0.99999 Qc Tcv W0.1" >>q_test.x +rbox 50 D7 t |qhull A-0.99999 Qc Tcv W0.1 >>q_test.x + +echo === check bad cases for Qhull. May cause errors ${d:-`date`} >>q_test.x +echo "RBOX 1000 L100000 s G1e-6 t | QHULL Tv" >>q_test.x +RBOX 1000 L100000 s G1e-6 t | QHULL Tv >>q_test.x +echo "RBOX 1000 L100000 s G1e-6 t | QHULL Tv Q10" >>q_test.x +RBOX 1000 L100000 s G1e-6 t | QHULL Tv Q10 >>q_test.x +echo "rbox 1000 s Z1 G1e-13 t | qhull Tv" >>q_test.x +rbox 1000 s Z1 G1e-13 t | qhull Tv >>q_test.x +echo "RBOX 1000 s W1e-13 P0 t | QHULL d Qbb Qc Tv" >>q_test.x +RBOX 1000 s W1e-13 P0 t | QHULL d Qbb Qc Tv >>q_test.x +echo "RBOX 1000 s W1e-13 t | QHULL d Tv" >>q_test.x +RBOX 1000 s W1e-13 t | QHULL d Tv >>q_test.x +echo "RBOX 1000 s W1e-13 t D2 | QHULL d Tv" >>q_test.x +RBOX 1000 s W1e-13 t D2 | QHULL d Tv >>q_test.x + +echo ======================================================= >>q_test.x +echo ======================================================= >>q_test.x +echo === The following commands may cause errors =========== >>q_test.x +echo ======================================================= >>q_test.x +echo ======================================================= >>q_test.x +echo "rbox c D7 | qhull Q0 Tcv" >>q_test.x +rbox c D7 | qhull Q0 Tcv >>q_test.x +echo "rbox 100 s D3 | qhull Q0 E1e-3 Tc Po" >>q_test.x +rbox 100 s D3 | qhull Q0 E1e-3 Tc Po >>q_test.x +echo "rbox 100 s D3 | qhull Q0 E1e-2 Tc Po" >>q_test.x +rbox 100 s D3 | qhull Q0 E1e-2 Tc Po >>q_test.x +echo "rbox 100 s D3 | qhull Q0 E1e-1 Tc Po" >>q_test.x +rbox 100 s D3 | qhull Q0 E1e-1 Tc Po >>q_test.x +echo "rbox 100 s D3 | qhull Q0 R1e-3 Tc Po" >>q_test.x +rbox 100 s D3 | qhull Q0 R1e-3 Tc Po >>q_test.x +echo "rbox 100 s D3 | qhull Q0 R1e-2 Tc Po" >>q_test.x +rbox 100 s D3 | qhull Q0 R1e-2 Tc Po >>q_test.x +echo "rbox 100 s D3 | qhull Q0 R0.05 Tc" >>q_test.x +rbox 100 s D3 | qhull Q0 R0.05 Tc >>q_test.x +echo "rbox 100 s D3 | qhull Q0 R0.05 Tc Po" >>q_test.x +rbox 100 s D3 | qhull Q0 R0.05 Tc Po >>q_test.x +echo "rbox 1000 W1e-7 | qhull Q0 Tc Po" >>q_test.x +rbox 1000 W1e-7 | qhull Q0 Tc Po >>q_test.x +echo "rbox 50 s | qhull Q0 V0.05 W0.01 Tc Po" >>q_test.x +rbox 50 s | qhull Q0 V0.05 W0.01 Tc Po >>q_test.x +echo "rbox 100 s D5 | qhull Q0 R1e-2 Tc Po" >>q_test.x +rbox 100 s D5 | qhull Q0 R1e-2 Tc Po >>q_test.x +echo "qhull" >>q_test.x +qhull >>q_test.x +echo "qhull ." >>q_test.x +qhull . >>q_test.x +echo "qhull -" >>q_test.x +qhull - >>q_test.x +echo "rbox" >>q_test.x +rbox >>q_test.x + +echo "# end of q_test" >>q_test.x diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qconvex.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qconvex.c new file mode 100644 index 0000000000000000000000000000000000000000..dbd89339d9cbe86d931cefd6ab9ef1dc46978559 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qconvex.c @@ -0,0 +1,334 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qconvex.c + compute convex hulls using qhull + + see unix.c for full interface + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <ctype.h> +#include <math.h> +#include "qhull.h" +#include "mem.h" +#include "qset.h" + +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <console.h> +#include <Desk.h> + +#elif __cplusplus +extern "C" { + int isatty (int); +} + +#elif _MSC_VER +#include <io.h> +#define isatty _isatty + +#else +int isatty (int); /* returns 1 if stdin is a tty + if "Undefined symbol" this can be deleted along with call in main() */ +#endif + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt">-</a> + + qh_prompt + long prompt for qconvex + + notes: + restricted version of qhull.c + + see: + concise prompt below +*/ + +/* duplicated in qconvex.htm */ +char hidden_options[]=" d v H Qbb Qf Qg Qm Qr Qu Qv Qx Qz TR E V Fp Gt Q0 Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8 Q9 "; + +char qh_prompta[]= "\n\ +qconvex- compute the convex hull\n\ + http://www.qhull.org %s\n\ +\n\ +input (stdin):\n\ + first lines: dimension and number of points (or vice-versa).\n\ + other lines: point coordinates, best if one point per line\n\ + comments: start with a non-numeric character\n\ +\n\ +options:\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Qc - keep coplanar points with nearest facet\n\ + Qi - keep interior points with nearest facet\n\ +\n\ +Qhull control options:\n\ + Qbk:n - scale coord k so that low bound is n\n\ + QBk:n - scale coord k so that upper bound is n (QBk is %2.2g)\n\ + QbB - scale input to unit cube centered at the origin\n\ + Qbk:0Bk:0 - remove k-th coordinate from input\n\ + QJn - randomly joggle input in range [-n,n]\n\ + QRn - random rotation (n=seed, n=0 time, n=-1 time/no rotate)\n\ +%s%s%s%s"; /* split up qh_prompt for Visual C++ */ +char qh_promptb[]= "\ + Qs - search all points for the initial simplex\n\ + QGn - good facet if visible from point n, -n for not visible\n\ + QVn - good facet if it includes point n, -n if not\n\ +\n\ +"; +char qh_promptc[]= "\ +Trace options:\n\ + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events\n\ + Tc - check frequently during execution\n\ + Ts - print statistics\n\ + Tv - verify result: structure, convexity, and point inclusion\n\ + Tz - send all output to stdout\n\ + TFn - report summary when n or more facets created\n\ + TI file - input data from file, no spaces or single quotes\n\ + TO file - output results to file, may be enclosed in single quotes\n\ + TPn - turn on tracing when point n added to hull\n\ + TMn - turn on tracing at merge n\n\ + TWn - trace merge facets when width > n\n\ + TVn - stop qhull after adding point n, -n for before (see TCn)\n\ + TCn - stop qhull after building cone for point n (see TVn)\n\ +\n\ +Precision options:\n\ + Cn - radius of centrum (roundoff added). Merge facets if non-convex\n\ + An - cosine of maximum angle. Merge facets if cosine > n or non-convex\n\ + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge\n\ + Rn - randomly perturb computations by a factor of [1-n,1+n]\n\ + Un - max distance below plane for a new, coplanar point\n\ + Wn - min facet width for outside point (before roundoff)\n\ +\n\ +Output formats (may be combined; if none, produces a summary to stdout):\n\ + f - facet dump\n\ + G - Geomview output (see below)\n\ + i - vertices incident to each facet\n\ + m - Mathematica output (2-d and 3-d)\n\ + n - normals with offsets\n\ + o - OFF file format (dim, points and facets; Voronoi regions)\n\ + p - point coordinates \n\ + s - summary (stderr)\n\ +\n\ +"; +char qh_promptd[]= "\ +More formats:\n\ + Fa - area for each facet\n\ + FA - compute total area and volume for option 's'\n\ + Fc - count plus coplanar points for each facet\n\ + use 'Qc' (default) for coplanar and 'Qi' for interior\n\ + FC - centrum for each facet\n\ + Fd - use cdd format for input (homogeneous with offset first)\n\ + FD - use cdd format for numeric output (offset first)\n\ + FF - facet dump without ridges\n\ + Fi - inner plane for each facet\n\ + FI - ID for each facet\n\ + Fm - merge count for each facet (511 max)\n\ + Fn - count plus neighboring facets for each facet\n\ + FN - count plus neighboring facets for each point\n\ + Fo - outer plane (or max_outside) for each facet\n\ + FO - options and precision constants\n\ + FP - nearest vertex for each coplanar point\n\ + FQ - command used for qconvex\n\ + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets,\n\ + for output: #vertices, #facets,\n\ + #coplanar points, #non-simplicial facets\n\ + #real (2), max outer plane, min vertex\n\ + FS - sizes: #int (0) \n\ + #real(2) tot area, tot volume\n\ + Ft - triangulation with centrums for non-simplicial facets (OFF format)\n\ + Fv - count plus vertices for each facet\n\ + FV - average of vertices (a feasible point for 'H')\n\ + Fx - extreme points (in order for 2-d)\n\ +\n\ +"; +char qh_prompte[]= "\ +Geomview output (2-d, 3-d, and 4-d)\n\ + Ga - all points as dots\n\ + Gp - coplanar points and vertices as radii\n\ + Gv - vertices as spheres\n\ + Gi - inner planes only\n\ + Gn - no planes\n\ + Go - outer planes only\n\ + Gc - centrums\n\ + Gh - hyperplane intersections\n\ + Gr - ridges\n\ + GDn - drop dimension n in 3-d and 4-d output\n\ +\n\ +Print options:\n\ + PAn - keep n largest facets by area\n\ + Pdk:n - drop facet if normal[k] <= n (default 0.0)\n\ + PDk:n - drop facet if normal[k] >= n\n\ + Pg - print good facets (needs 'QGn' or 'QVn')\n\ + PFn - keep facets whose area is at least n\n\ + PG - print neighbors of good facets\n\ + PMn - keep n facets with most merges\n\ + Po - force output. If error, output neighborhood of facet\n\ + Pp - do not report precision problems\n\ +\n\ + . - list of all options\n\ + - - one line descriptions of all options\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt2">-</a> + + qh_prompt2 + synopsis for qhull +*/ +char qh_prompt2[]= "\n\ +qconvex- compute the convex hull. Qhull %s\n\ + input (stdin): dimension, number of points, point coordinates\n\ + comments start with a non-numeric character\n\ +\n\ +options (qconvex.htm):\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Tv - verify result: structure, convexity, and point inclusion\n\ + . - concise list of all options\n\ + - - one-line description of all options\n\ +\n\ +output options (subset):\n\ + s - summary of results (default)\n\ + i - vertices incident to each facet\n\ + n - normals with offsets\n\ + p - vertex coordinates (includes coplanar points if 'Qc')\n\ + Fx - extreme points (convex hull vertices)\n\ + FA - compute total area and volume\n\ + o - OFF format (dim, n, points, facets)\n\ + G - Geomview output (2-d, 3-d, and 4-d)\n\ + m - Mathematica output (2-d and 3-d)\n\ + QVn - print facets that include point n, -n if not\n\ + TO file- output results to file, may be enclosed in single quotes\n\ +\n\ +examples:\n\ + rbox c D2 | qconvex s n rbox c D2 | qconvex i\n\ + rbox c D2 | qconvex o rbox 1000 s | qconvex s Tv FA\n\ + rbox c d D2 | qconvex s Qc Fx rbox y 1000 W0 | qconvex s n\n\ + rbox y 1000 W0 | qconvex s QJ rbox d G1 D12 | qconvex QR0 FA Pp\n\ + rbox c D7 | qconvex FA TF1000\n\ +\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt3">-</a> + + qh_prompt3 + concise prompt for qhull +*/ +char qh_prompt3[]= "\n\ +Qhull %s.\n\ +Except for 'F.' and 'PG', upper-case options take an argument.\n\ +\n\ + incidences mathematica normals OFF_format points\n\ + summary facet_dump\n\ +\n\ + Farea FArea_total Fcoplanars FCentrums Fd_cdd_in\n\ + FD_cdd_out FFacet_xridge Finner FIDs Fmerges\n\ + Fneighbors FNeigh_vertex Fouter FOptions FPoint_near\n\ + FQhull Fsummary FSize Fvertices FVertex_ave\n\ + Fxtremes FMaple\n\ +\n\ + Gvertices Gpoints Gall_points Gno_planes Ginner\n\ + Gcentrums Ghyperplanes Gridges Gouter GDrop_dim\n\ +\n\ + PArea_keep Pdrop d0:0D0 PFacet_area_keep Pgood PGood_neighbors\n\ + PMerge_keep Poutput_forced Pprecision_not\n\ +\n\ + QbBound 0:0.5 QbB_scale_box Qcoplanar QGood_point Qinterior\n\ + QJoggle Qrandom QRotate Qsearch_1st Qtriangulate\n\ + QVertex_good\n\ +\n\ + T4_trace Tcheck_often Tstatistics Tverify Tz_stdout\n\ + TFacet_log TInput_file TPoint_trace TMerge_trace TOutput_file\n\ + TWide_trace TVertex_stop TCone_stop\n\ +\n\ + Angle_max Centrum_size Random_dist Ucoplanar_max Wide_outside\n\ +"; + +/*-<a href="qh-qhull.htm" + >-------------------------------</a><a name="main">-</a> + + main( argc, argv ) + processes the command line, calls qhull() to do the work, and exits + + design: + initializes data structures + reads points + finishes initialization + computes convex hull and other structures + checks the result + writes the output + frees memory +*/ +int main(int argc, char *argv[]) { + int curlong, totlong; /* used !qh_NOmem */ + int exitcode, numpoints, dim; + coordT *points; + boolT ismalloc; + +#if __MWERKS__ && __POWERPC__ + char inBuf[BUFSIZ], outBuf[BUFSIZ], errBuf[BUFSIZ]; + SIOUXSettings.showstatusline= false; + SIOUXSettings.tabspaces= 1; + SIOUXSettings.rows= 40; + if (setvbuf (stdin, inBuf, _IOFBF, sizeof(inBuf)) < 0 /* w/o, SIOUX I/O is slow*/ + || setvbuf (stdout, outBuf, _IOFBF, sizeof(outBuf)) < 0 + || (stdout != stderr && setvbuf (stderr, errBuf, _IOFBF, sizeof(errBuf)) < 0)) + fprintf (stderr, "qhull internal warning (main): could not change stdio to fully buffered.\n"); + argc= ccommand(&argv); +#endif + + if ((argc == 1) && isatty( 0 /*stdin*/)) { + fprintf(stdout, qh_prompt2, qh_version); + exit(qh_ERRnone); + } + if (argc > 1 && *argv[1] == '-' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompta, qh_version, qh_DEFAULTbox, + qh_promptb, qh_promptc, qh_promptd, qh_prompte); + exit(qh_ERRnone); + } + if (argc >1 && *argv[1] == '.' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompt3, qh_version); + exit(qh_ERRnone); + } + qh_init_A (stdin, stdout, stderr, argc, argv); /* sets qh qhull_command */ + exitcode= setjmp (qh errexit); /* simple statement for CRAY J916 */ + if (!exitcode) { + qh_checkflags (qh qhull_command, hidden_options); + qh_initflags (qh qhull_command); + points= qh_readpoints (&numpoints, &dim, &ismalloc); + if (dim >= 5) { + qh_option ("Qxact_merge", NULL, NULL); + qh MERGEexact= True; /* 'Qx' always */ + } + qh_init_B (points, numpoints, dim, ismalloc); + qh_qhull(); + qh_check_output(); + qh_produce_output(); + if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points(); + exitcode= qh_ERRnone; + } + qh NOerrexit= True; /* no more setjmp */ +#ifdef qh_NOmem + qh_freeqhull( True); +#else + qh_freeqhull( False); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); +#endif + return exitcode; +} /* main */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qdelaun.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qdelaun.c new file mode 100644 index 0000000000000000000000000000000000000000..68620fa1a9704f494cb214a6e7898df822d54cd3 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qdelaun.c @@ -0,0 +1,324 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qdelaun.c + compute Delaunay triangulations and furthest-point Delaunay + triangulations using qhull + + see unix.c for full interface + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <ctype.h> +#include <math.h> +#include "qhull.h" +#include "mem.h" +#include "qset.h" + +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <console.h> +#include <Desk.h> + +#elif __cplusplus +extern "C" { + int isatty (int); +} + +#elif _MSC_VER +#include <io.h> +#define isatty _isatty + +#else +int isatty (int); /* returns 1 if stdin is a tty + if "Undefined symbol" this can be deleted along with call in main() */ +#endif + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt">-</a> + + qh_prompt + long prompt for qhull + + notes: + restricted version of qhull.c + + see: + concise prompt below +*/ + +/* duplicated in qdelau_f.htm and qdelaun.htm */ +char hidden_options[]=" d n v H U Qb QB Qc Qf Qg Qi Qm Qr QR Qv Qx TR E V FC Fi Fo Ft Fp FV Q0 Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8 Q9 "; + +char qh_prompta[]= "\n\ +qdelaunay- compute the Delaunay triangulation\n\ + http://www.qhull.org %s\n\ +\n\ +input (stdin):\n\ + first lines: dimension and number of points (or vice-versa).\n\ + other lines: point coordinates, best if one point per line\n\ + comments: start with a non-numeric character\n\ +\n\ +options:\n\ + Qu - compute furthest-site Delaunay triangulation\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ +\n\ +Qhull control options:\n\ + QJn - randomly joggle input in range [-n,n]\n\ +%s%s%s%s"; /* split up qh_prompt for Visual C++ */ +char qh_promptb[]= "\ + Qs - search all points for the initial simplex\n\ + Qz - add point-at-infinity to Delaunay triangulation\n\ + QGn - print Delaunay region if visible from point n, -n if not\n\ + QVn - print Delaunay regions that include point n, -n if not\n\ +\n\ +"; +char qh_promptc[]= "\ +Trace options:\n\ + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events\n\ + Tc - check frequently during execution\n\ + Ts - print statistics\n\ + Tv - verify result: structure, convexity, and in-circle test\n\ + Tz - send all output to stdout\n\ + TFn - report summary when n or more facets created\n\ + TI file - input data from file, no spaces or single quotes\n\ + TO file - output results to file, may be enclosed in single quotes\n\ + TPn - turn on tracing when point n added to hull\n\ + TMn - turn on tracing at merge n\n\ + TWn - trace merge facets when width > n\n\ + TVn - stop qhull after adding point n, -n for before (see TCn)\n\ + TCn - stop qhull after building cone for point n (see TVn)\n\ +\n\ +Precision options:\n\ + Cn - radius of centrum (roundoff added). Merge facets if non-convex\n\ + An - cosine of maximum angle. Merge facets if cosine > n or non-convex\n\ + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge\n\ + Rn - randomly perturb computations by a factor of [1-n,1+n]\n\ + Wn - min facet width for outside point (before roundoff)\n\ +\n\ +Output formats (may be combined; if none, produces a summary to stdout):\n\ + f - facet dump\n\ + G - Geomview output (see below)\n\ + i - vertices incident to each Delaunay region\n\ + m - Mathematica output (2-d only, lifted to a paraboloid)\n\ + o - OFF format (dim, points, and facets as a paraboloid)\n\ + p - point coordinates (lifted to a paraboloid)\n\ + s - summary (stderr)\n\ +\n\ +"; +char qh_promptd[]= "\ +More formats:\n\ + Fa - area for each Delaunay region\n\ + FA - compute total area for option 's'\n\ + Fc - count plus coincident points for each Delaunay region\n\ + Fd - use cdd format for input (homogeneous with offset first)\n\ + FD - use cdd format for numeric output (offset first)\n\ + FF - facet dump without ridges\n\ + FI - ID of each Delaunay region\n\ + Fm - merge count for each Delaunay region (511 max)\n\ + FM - Maple output (2-d only, lifted to a paraboloid)\n\ + Fn - count plus neighboring region for each Delaunay region\n\ + FN - count plus neighboring region for each point\n\ + FO - options and precision constants\n\ + FP - nearest point and distance for each coincident point\n\ + FQ - command used for qdelaunay\n\ + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets,\n\ + for output: #vertices, #Delaunay regions,\n\ + #coincident points, #non-simplicial regions\n\ + #real (2), max outer plane, min vertex\n\ + FS - sizes: #int (0)\n\ + #real(2) tot area, 0\n\ + Fv - count plus vertices for each Delaunay region\n\ + Fx - extreme points of Delaunay triangulation (on convex hull)\n\ +\n\ +"; +char qh_prompte[]= "\ +Geomview options (2-d and 3-d)\n\ + Ga - all points as dots\n\ + Gp - coplanar points and vertices as radii\n\ + Gv - vertices as spheres\n\ + Gi - inner planes only\n\ + Gn - no planes\n\ + Go - outer planes only\n\ + Gc - centrums\n\ + Gh - hyperplane intersections\n\ + Gr - ridges\n\ + GDn - drop dimension n in 3-d and 4-d output\n\ + Gt - transparent outer ridges to view 3-d Delaunay\n\ +\n\ +Print options:\n\ + PAn - keep n largest Delaunay regions by area\n\ + Pdk:n - drop facet if normal[k] <= n (default 0.0)\n\ + PDk:n - drop facet if normal[k] >= n\n\ + Pg - print good Delaunay regions (needs 'QGn' or 'QVn')\n\ + PFn - keep Delaunay regions whose area is at least n\n\ + PG - print neighbors of good regions (needs 'QGn' or 'QVn')\n\ + PMn - keep n Delaunay regions with most merges\n\ + Po - force output. If error, output neighborhood of facet\n\ + Pp - do not report precision problems\n\ +\n\ + . - list of all options\n\ + - - one line descriptions of all options\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt2">-</a> + + qh_prompt2 + synopsis for qhull +*/ +char qh_prompt2[]= "\n\ +qdelaunay- compute the Delaunay triangulation. Qhull %s\n\ + input (stdin): dimension, number of points, point coordinates\n\ + comments start with a non-numeric character\n\ +\n\ +options (qdelaun.htm):\n\ + Qu - furthest-site Delaunay triangulation\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Tv - verify result: structure, convexity, and in-circle test\n\ + . - concise list of all options\n\ + - - one-line description of all options\n\ +\n\ +output options (subset):\n\ + s - summary of results (default)\n\ + i - vertices incident to each Delaunay region\n\ + Fx - extreme points (vertices of the convex hull)\n\ + o - OFF format (shows the points lifted to a paraboloid)\n\ + G - Geomview output (2-d and 3-d points lifted to a paraboloid)\n\ + m - Mathematica output (2-d inputs lifted to a paraboloid)\n\ + QVn - print Delaunay regions that include point n, -n if not\n\ + TO file- output results to file, may be enclosed in single quotes\n\ +\n\ +examples:\n\ + rbox c P0 D2 | qdelaunay s o rbox c P0 D2 | qdelaunay i\n\ + rbox c P0 D2 | qdelaunay Fv rbox c P0 D2 | qdelaunay s Qu Fv\n\ + rbox c G1 d D2 | qdelaunay s i rbox c G1 d D2 | qdelaunay Qt\n\ + rbox M3,4 z 100 D2 | qdelaunay s rbox M3,4 z 100 D2 | qdelaunay s Qt\n\ +\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt3">-</a> + + qh_prompt3 + concise prompt for qhull +*/ +char qh_prompt3[]= "\n\ +Qhull %s.\n\ +Except for 'F.' and 'PG', upper-case options take an argument.\n\ +\n\ + incidences mathematica OFF_format points_lifted summary\n\ + facet_dump\n\ +\n\ + Farea FArea_total Fcoincident Fd_cdd_in FD_cdd_out\n\ + FF_dump_xridge FIDs Fmerges Fneighbors FNeigh_vertex\n\ + FOptions FPoint_near FQdelaun Fsummary FSize\n\ + Fvertices Fxtremes FMaple\n\ +\n\ + Gvertices Gpoints Gall_points Gno_planes Ginner\n\ + Gcentrums Ghyperplanes Gridges Gouter GDrop_dim\n\ + Gtransparent\n\ +\n\ + PArea_keep Pdrop d0:0D0 Pgood PFacet_area_keep\n\ + PGood_neighbors PMerge_keep Poutput_forced Pprecision_not\n\ +\n\ + QGood_point QJoggle Qsearch_1st Qtriangulate QupperDelaunay\n\ + QVertex_good Qzinfinite\n\ +\n\ + T4_trace Tcheck_often Tstatistics Tverify Tz_stdout\n\ + TFacet_log TInput_file TPoint_trace TMerge_trace TOutput_file\n\ + TWide_trace TVertex_stop TCone_stop\n\ +\n\ + Angle_max Centrum_size Random_dist Wide_outside\n\ +"; + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="main">-</a> + + main( argc, argv ) + processes the command line, calls qhull() to do the work, and exits + + design: + initializes data structures + reads points + finishes initialization + computes convex hull and other structures + checks the result + writes the output + frees memory +*/ +int main(int argc, char *argv[]) { + int curlong, totlong; /* used !qh_NOmem */ + int exitcode, numpoints, dim; + coordT *points; + boolT ismalloc; + +#if __MWERKS__ && __POWERPC__ + char inBuf[BUFSIZ], outBuf[BUFSIZ], errBuf[BUFSIZ]; + SIOUXSettings.showstatusline= false; + SIOUXSettings.tabspaces= 1; + SIOUXSettings.rows= 40; + if (setvbuf (stdin, inBuf, _IOFBF, sizeof(inBuf)) < 0 /* w/o, SIOUX I/O is slow*/ + || setvbuf (stdout, outBuf, _IOFBF, sizeof(outBuf)) < 0 + || (stdout != stderr && setvbuf (stderr, errBuf, _IOFBF, sizeof(errBuf)) < 0)) + fprintf (stderr, "qhull internal warning (main): could not change stdio to fully buffered.\n"); + argc= ccommand(&argv); +#endif + + if ((argc == 1) && isatty( 0 /*stdin*/)) { + fprintf(stdout, qh_prompt2, qh_version); + exit(qh_ERRnone); + } + if (argc > 1 && *argv[1] == '-' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompta, qh_version, + qh_promptb, qh_promptc, qh_promptd, qh_prompte); + exit(qh_ERRnone); + } + if (argc >1 && *argv[1] == '.' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompt3, qh_version); + exit(qh_ERRnone); + } + qh_init_A (stdin, stdout, stderr, argc, argv); /* sets qh qhull_command */ + exitcode= setjmp (qh errexit); /* simple statement for CRAY J916 */ + if (!exitcode) { + qh_option ("delaunay Qbbound-last", NULL, NULL); + qh DELAUNAY= True; /* 'd' */ + qh SCALElast= True; /* 'Qbb' */ + qh KEEPcoplanar= True; /* 'Qc', to keep coplanars in 'p' */ + qh_checkflags (qh qhull_command, hidden_options); + qh_initflags (qh qhull_command); + points= qh_readpoints (&numpoints, &dim, &ismalloc); + if (dim >= 5) { + qh_option ("Qxact_merge", NULL, NULL); + qh MERGEexact= True; /* 'Qx' always */ + } + qh_init_B (points, numpoints, dim, ismalloc); + qh_qhull(); + qh_check_output(); + qh_produce_output(); + if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points(); + exitcode= qh_ERRnone; + } + qh NOerrexit= True; /* no more setjmp */ +#ifdef qh_NOmem + qh_freeqhull( True); +#else + qh_freeqhull( False); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); +#endif + return exitcode; +} /* main */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qhalf.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qhalf.c new file mode 100644 index 0000000000000000000000000000000000000000..0996c545e1263b4263f0b3d61c74620507383844 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qhalf.c @@ -0,0 +1,325 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qhalf.c + compute the intersection of halfspaces about a point + + see unix.c for full interface + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <ctype.h> +#include <math.h> +#include "qhull.h" +#include "mem.h" +#include "qset.h" + +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <console.h> +#include <Desk.h> + +#elif __cplusplus +extern "C" { + int isatty (int); +} + +#elif _MSC_VER +#include <io.h> +#define isatty _isatty + +#else +int isatty (int); /* returns 1 if stdin is a tty + if "Undefined symbol" this can be deleted along with call in main() */ +#endif + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt">-</a> + + qh_prompt + long prompt for qhull + + notes: + restricted version of qhull.c + + see: + concise prompt below +*/ + +/* duplicated in qhalf.htm */ +char hidden_options[]=" d n v Qbb QbB Qf Qg Qm Qr QR Qv Qx Qz TR E V Fa FA FC FD FS Ft FV Gt Q0 Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8 Q9 "; + +char qh_prompta[]= "\n\ +qhalf- compute the intersection of halfspaces about a point\n\ + http://www.qhull.org %s\n\ +\n\ +input (stdin):\n\ + optional interior point: dimension, 1, coordinates\n\ + first lines: dimension+1 and number of halfspaces\n\ + other lines: halfspace coefficients followed by offset\n\ + comments: start with a non-numeric character\n\ +\n\ +options:\n\ + Hn,n - specify coordinates of interior point\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Qc - keep coplanar halfspaces\n\ + Qi - keep other redundant halfspaces\n\ +\n\ +Qhull control options:\n\ + QJn - randomly joggle input in range [-n,n]\n\ +%s%s%s%s"; /* split up qh_prompt for Visual C++ */ +char qh_promptb[]= "\ + Qbk:0Bk:0 - remove k-th coordinate from input\n\ + Qs - search all halfspaces for the initial simplex\n\ + QGn - print intersection if visible to halfspace n, -n for not\n\ + QVn - print intersections for halfspace n, -n if not\n\ +\n\ +"; +char qh_promptc[]= "\ +Trace options:\n\ + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events\n\ + Tc - check frequently during execution\n\ + Ts - print statistics\n\ + Tv - verify result: structure, convexity, and redundancy\n\ + Tz - send all output to stdout\n\ + TFn - report summary when n or more facets created\n\ + TI file - input data from file, no spaces or single quotes\n\ + TO file - output results to file, may be enclosed in single quotes\n\ + TPn - turn on tracing when halfspace n added to intersection\n\ + TMn - turn on tracing at merge n\n\ + TWn - trace merge facets when width > n\n\ + TVn - stop qhull after adding halfspace n, -n for before (see TCn)\n\ + TCn - stop qhull after building cone for halfspace n (see TVn)\n\ +\n\ +Precision options:\n\ + Cn - radius of centrum (roundoff added). Merge facets if non-convex\n\ + An - cosine of maximum angle. Merge facets if cosine > n or non-convex\n\ + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge\n\ + Rn - randomly perturb computations by a factor of [1-n,1+n]\n\ + Un - max distance below plane for a new, coplanar halfspace\n\ + Wn - min facet width for outside halfspace (before roundoff)\n\ +\n\ +Output formats (may be combined; if none, produces a summary to stdout):\n\ + f - facet dump\n\ + G - Geomview output (dual convex hull)\n\ + i - non-redundant halfspaces incident to each intersection\n\ + m - Mathematica output (dual convex hull)\n\ + o - OFF format (dual convex hull: dimension, points, and facets)\n\ + p - vertex coordinates of dual convex hull (coplanars if 'Qc' or 'Qi')\n\ + s - summary (stderr)\n\ +\n\ +"; +char qh_promptd[]= "\ +More formats:\n\ + Fc - count plus redundant halfspaces for each intersection\n\ + - Qc (default) for coplanar and Qi for other redundant\n\ + Fd - use cdd format for input (homogeneous with offset first)\n\ + FF - facet dump without ridges\n\ + FI - ID of each intersection\n\ + Fm - merge count for each intersection (511 max)\n\ + FM - Maple output (dual convex hull)\n\ + Fn - count plus neighboring intersections for each intersection\n\ + FN - count plus intersections for each non-redundant halfspace\n\ + FO - options and precision constants\n\ + Fp - dim, count, and intersection coordinates\n\ + FP - nearest halfspace and distance for each redundant halfspace\n\ + FQ - command used for qhalf\n\ + Fs - summary: #int (8), dim, #halfspaces, #non-redundant, #intersections\n\ + for output: #non-redundant, #intersections, #coplanar\n\ + halfspaces, #non-simplicial intersections\n\ + #real (2), max outer plane, min vertex\n\ + Fv - count plus non-redundant halfspaces for each intersection\n\ + Fx - non-redundant halfspaces\n\ +\n\ +"; +char qh_prompte[]= "\ +Geomview output (2-d, 3-d and 4-d; dual convex hull)\n\ + Ga - all points (i.e., transformed halfspaces) as dots\n\ + Gp - coplanar points and vertices as radii\n\ + Gv - vertices (i.e., non-redundant halfspaces) as spheres\n\ + Gi - inner planes (i.e., halfspace intersections) only\n\ + Gn - no planes\n\ + Go - outer planes only\n\ + Gc - centrums\n\ + Gh - hyperplane intersections\n\ + Gr - ridges\n\ + GDn - drop dimension n in 3-d and 4-d output\n\ +\n\ +Print options:\n\ + PAn - keep n largest facets (i.e., intersections) by area\n\ + Pdk:n- drop facet if normal[k] <= n (default 0.0)\n\ + PDk:n- drop facet if normal[k] >= n\n\ + Pg - print good facets (needs 'QGn' or 'QVn')\n\ + PFn - keep facets whose area is at least n\n\ + PG - print neighbors of good facets\n\ + PMn - keep n facets with most merges\n\ + Po - force output. If error, output neighborhood of facet\n\ + Pp - do not report precision problems\n\ +\n\ + . - list of all options\n\ + - - one line descriptions of all options\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt2">-</a> + + qh_prompt2 + synopsis for qhull +*/ +char qh_prompt2[]= "\n\ +qhalf- halfspace intersection about a point. Qhull %s\n\ + input (stdin): [dim, 1, interior point], dim+1, n, coefficients+offset\n\ + comments start with a non-numeric character\n\ +\n\ +options (qhalf.htm):\n\ + Hn,n - specify coordinates of interior point\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Tv - verify result: structure, convexity, and redundancy\n\ + . - concise list of all options\n\ + - - one-line description of all options\n\ +\n\ +output options (subset):\n\ + s - summary of results (default)\n\ + Fp - intersection coordinates\n\ + Fv - non-redundant halfspaces incident to each intersection\n\ + Fx - non-redundant halfspaces\n\ + o - OFF file format (dual convex hull)\n\ + G - Geomview output (dual convex hull)\n\ + m - Mathematica output (dual convex hull)\n\ + QVn - print intersections for halfspace n, -n if not\n\ + TO file - output results to file, may be enclosed in single quotes\n\ +\n\ +examples:\n\ + rbox d | qconvex FQ n | qhalf s H0,0,0 Fp\n\ + rbox c | qconvex FQ FV n | qhalf s i\n\ + rbox c | qconvex FQ FV n | qhalf s o\n\ +\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt3">-</a> + + qh_prompt3 + concise prompt for qhull +*/ +char qh_prompt3[]= "\n\ +Qhull %s.\n\ +Except for 'F.' and 'PG', upper_case options take an argument.\n\ +\n\ + incidences Geomview mathematica OFF_format point_dual\n\ + summary facet_dump\n\ +\n\ + Fc_redundant Fd_cdd_in FF_dump_xridge FIDs Fmerges\n\ + Fneighbors FN_intersect FOptions Fp_coordinates FP_nearest\n\ + FQhalf Fsummary Fv_halfspace FMaple Fx_non_redundant\n\ +\n\ + Gvertices Gpoints Gall_points Gno_planes Ginner\n\ + Gcentrums Ghyperplanes Gridges Gouter GDrop_dim\n\ +\n\ + PArea_keep Pdrop d0:0D0 Pgood PFacet_area_keep\n\ + PGood_neighbors PMerge_keep Poutput_forced Pprecision_not\n\ +\n\ + Qbk:0Bk:0_drop Qcoplanar QG_half_good Qi_redundant QJoggle\n\ + Qsearch_1st Qtriangulate QVertex_good\n\ +\n\ + T4_trace Tcheck_often Tstatistics Tverify Tz_stdout\n\ + TFacet_log TInput_file TPoint_trace TMerge_trace TOutput_file\n\ + TWide_trace TVertex_stop TCone_stop\n\ +\n\ + Angle_max Centrum_size Random_dist Ucoplanar_max Wide_outside\n\ +"; + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="main">-</a> + + main( argc, argv ) + processes the command line, calls qhull() to do the work, and exits + + design: + initializes data structures + reads points + finishes initialization + computes convex hull and other structures + checks the result + writes the output + frees memory +*/ +int main(int argc, char *argv[]) { + int curlong, totlong; /* used !qh_NOmem */ + int exitcode, numpoints, dim; + coordT *points; + boolT ismalloc; + +#if __MWERKS__ && __POWERPC__ + char inBuf[BUFSIZ], outBuf[BUFSIZ], errBuf[BUFSIZ]; + SIOUXSettings.showstatusline= false; + SIOUXSettings.tabspaces= 1; + SIOUXSettings.rows= 40; + if (setvbuf (stdin, inBuf, _IOFBF, sizeof(inBuf)) < 0 /* w/o, SIOUX I/O is slow*/ + || setvbuf (stdout, outBuf, _IOFBF, sizeof(outBuf)) < 0 + || (stdout != stderr && setvbuf (stderr, errBuf, _IOFBF, sizeof(errBuf)) < 0)) + fprintf (stderr, "qhull internal warning (main): could not change stdio to fully buffered.\n"); + argc= ccommand(&argv); +#endif + + if ((argc == 1) && isatty( 0 /*stdin*/)) { + fprintf(stdout, qh_prompt2, qh_version); + exit(qh_ERRnone); + } + if (argc > 1 && *argv[1] == '-' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompta, qh_version, + qh_promptb, qh_promptc, qh_promptd, qh_prompte); + exit(qh_ERRnone); + } + if (argc >1 && *argv[1] == '.' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompt3, qh_version); + exit(qh_ERRnone); + } + qh_init_A (stdin, stdout, stderr, argc, argv); /* sets qh qhull_command */ + exitcode= setjmp (qh errexit); /* simple statement for CRAY J916 */ + if (!exitcode) { + qh_option ("Halfspace", NULL, NULL); + qh HALFspace= True; /* 'H' */ + qh_checkflags (qh qhull_command, hidden_options); + qh_initflags (qh qhull_command); + if (qh SCALEinput) { + fprintf(qh ferr, "\ +qhull error: options 'Qbk:n' and 'QBk:n' are not used with qhalf.\n\ + Use 'Qbk:0Bk:0 to drop dimension k.\n"); + qh_errexit(qh_ERRinput, NULL, NULL); + } + points= qh_readpoints (&numpoints, &dim, &ismalloc); + if (dim >= 5) { + qh_option ("Qxact_merge", NULL, NULL); + qh MERGEexact= True; /* 'Qx' always */ + } + qh_init_B (points, numpoints, dim, ismalloc); + qh_qhull(); + qh_check_output(); + qh_produce_output(); + if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points(); + exitcode= qh_ERRnone; + } + qh NOerrexit= True; /* no more setjmp */ +#ifdef qh_NOmem + qh_freeqhull( True); +#else + qh_freeqhull( False); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); +#endif + return exitcode; +} /* main */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qvoronoi.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qvoronoi.c new file mode 100644 index 0000000000000000000000000000000000000000..1a3aeb4cef437b6d6ff92228e063cfb5280200f1 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/qvoronoi.c @@ -0,0 +1,318 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qvoronoi.c + compute Voronoi diagrams and furthest-point Voronoi + diagrams using qhull + + see unix.c for full interface + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <ctype.h> +#include <math.h> +#include "qhull.h" +#include "mem.h" +#include "qset.h" + +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <console.h> +#include <Desk.h> + +#elif __cplusplus +extern "C" { + int isatty (int); +} + +#elif _MSC_VER +#include <io.h> +#define isatty _isatty + +#else +int isatty (int); /* returns 1 if stdin is a tty + if "Undefined symbol" this can be deleted along with call in main() */ +#endif + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt">-</a> + + qh_prompt + long prompt for qhull + + notes: + restricted version of qhull.c + + see: + concise prompt below +*/ + +/* duplicated in qvoron_f.htm and qvoronoi.htm */ +char hidden_options[]=" d n m v H U Qb QB Qc Qf Qg Qi Qm Qr QR Qv Qx TR E V Fa FA FC Fp FS Ft FV Pv Gt Q0 Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8 Q9 "; + +char qh_prompta[]= "\n\ +qvoronoi- compute the Voronoi diagram\n\ + http://www.qhull.org %s\n\ +\n\ +input (stdin):\n\ + first lines: dimension and number of points (or vice-versa).\n\ + other lines: point coordinates, best if one point per line\n\ + comments: start with a non-numeric character\n\ +\n\ +options:\n\ + Qu - compute furthest-site Voronoi diagram\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ +\n\ +Qhull control options:\n\ + Qz - add point-at-infinity to Voronoi diagram\n\ + QJn - randomly joggle input in range [-n,n]\n\ +%s%s%s%s"; /* split up qh_prompt for Visual C++ */ +char qh_promptb[]= "\ + Qs - search all points for the initial simplex\n\ + QGn - Voronoi vertices if visible from point n, -n if not\n\ + QVn - Voronoi vertices for input point n, -n if not\n\ +\n\ +"; +char qh_promptc[]= "\ +Trace options:\n\ + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events\n\ + Tc - check frequently during execution\n\ + Ts - statistics\n\ + Tv - verify result: structure, convexity, and in-circle test\n\ + Tz - send all output to stdout\n\ + TFn - report summary when n or more facets created\n\ + TI file - input data from file, no spaces or single quotes\n\ + TO file - output results to file, may be enclosed in single quotes\n\ + TPn - turn on tracing when point n added to hull\n\ + TMn - turn on tracing at merge n\n\ + TWn - trace merge facets when width > n\n\ + TVn - stop qhull after adding point n, -n for before (see TCn)\n\ + TCn - stop qhull after building cone for point n (see TVn)\n\ +\n\ +Precision options:\n\ + Cn - radius of centrum (roundoff added). Merge facets if non-convex\n\ + An - cosine of maximum angle. Merge facets if cosine > n or non-convex\n\ + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge\n\ + Rn - randomly perturb computations by a factor of [1-n,1+n]\n\ + Wn - min facet width for non-coincident point (before roundoff)\n\ +\n\ +Output formats (may be combined; if none, produces a summary to stdout):\n\ + s - summary to stderr\n\ + p - Voronoi vertices\n\ + o - OFF format (dim, Voronoi vertices, and Voronoi regions)\n\ + i - Delaunay regions (use 'Pp' to avoid warning)\n\ + f - facet dump\n\ +\n\ +"; +char qh_promptd[]= "\ +More formats:\n\ + Fc - count plus coincident points (by Voronoi vertex)\n\ + Fd - use cdd format for input (homogeneous with offset first)\n\ + FD - use cdd format for output (offset first)\n\ + FF - facet dump without ridges\n\ + Fi - separating hyperplanes for bounded Voronoi regions\n\ + FI - ID for each Voronoi vertex\n\ + Fm - merge count for each Voronoi vertex (511 max)\n\ + Fn - count plus neighboring Voronoi vertices for each Voronoi vertex\n\ + FN - count and Voronoi vertices for each Voronoi region\n\ + Fo - separating hyperplanes for unbounded Voronoi regions\n\ + FO - options and precision constants\n\ + FP - nearest point and distance for each coincident point\n\ + FQ - command used for qvoronoi\n\ + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets,\n\ + for output: #Voronoi regions, #Voronoi vertices,\n\ + #coincident points, #non-simplicial regions\n\ + #real (2), max outer plane and min vertex\n\ + Fv - Voronoi diagram as Voronoi vertices between adjacent input sites\n\ + Fx - extreme points of Delaunay triangulation (on convex hull)\n\ +\n\ +"; +char qh_prompte[]= "\ +Geomview options (2-d only)\n\ + Ga - all points as dots\n\ + Gp - coplanar points and vertices as radii\n\ + Gv - vertices as spheres\n\ + Gi - inner planes only\n\ + Gn - no planes\n\ + Go - outer planes only\n\ + Gc - centrums\n\ + Gh - hyperplane intersections\n\ + Gr - ridges\n\ + GDn - drop dimension n in 3-d and 4-d output\n\ +\n\ +Print options:\n\ + PAn - keep n largest Voronoi vertices by 'area'\n\ + Pdk:n - drop facet if normal[k] <= n (default 0.0)\n\ + PDk:n - drop facet if normal[k] >= n\n\ + Pg - print good Voronoi vertices (needs 'QGn' or 'QVn')\n\ + PFn - keep Voronoi vertices whose 'area' is at least n\n\ + PG - print neighbors of good Voronoi vertices\n\ + PMn - keep n Voronoi vertices with most merges\n\ + Po - force output. If error, output neighborhood of facet\n\ + Pp - do not report precision problems\n\ +\n\ + . - list of all options\n\ + - - one line descriptions of all options\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt2">-</a> + + qh_prompt2 + synopsis for qhull +*/ +char qh_prompt2[]= "\n\ +qvoronoi- compute the Voronoi diagram. Qhull %s\n\ + input (stdin): dimension, number of points, point coordinates\n\ + comments start with a non-numeric character\n\ +\n\ +options (qvoronoi.htm):\n\ + Qu - compute furthest-site Voronoi diagram\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Tv - verify result: structure, convexity, and in-circle test\n\ + . - concise list of all options\n\ + - - one-line description of all options\n\ +\n\ +output options (subset):\n\ + s - summary of results (default)\n\ + p - Voronoi vertices\n\ + o - OFF file format (dim, Voronoi vertices, and Voronoi regions)\n\ + FN - count and Voronoi vertices for each Voronoi region\n\ + Fv - Voronoi diagram as Voronoi vertices between adjacent input sites\n\ + Fi - separating hyperplanes for bounded regions, 'Fo' for unbounded\n\ + G - Geomview output (2-d only)\n\ + QVn - Voronoi vertices for input point n, -n if not\n\ + TO file- output results to file, may be enclosed in single quotes\n\ +\n\ +examples:\n\ +rbox c P0 D2 | qvoronoi s o rbox c P0 D2 | qvoronoi Fi\n\ +rbox c P0 D2 | qvoronoi Fo rbox c P0 D2 | qvoronoi Fv\n\ +rbox c P0 D2 | qvoronoi s Qu Fv rbox c P0 D2 | qvoronoi Qu Fo\n\ +rbox c G1 d D2 | qvoronoi s p rbox c G1 d D2 | qvoronoi QJ s p\n\ +rbox c P0 D2 | qvoronoi s Fv QV0\n\ +\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt3">-</a> + + qh_prompt3 + concise prompt for qhull +*/ +char qh_prompt3[]= "\n\ +Qhull %s.\n\ +Except for 'F.' and 'PG', upper-case options take an argument.\n\ +\n\ + OFF_format p_vertices i_delaunay summary facet_dump\n\ +\n\ + Fcoincident Fd_cdd_in FD_cdd_out FF-dump-xridge Fi_bounded\n\ + Fxtremes Fmerges Fneighbors FNeigh_region FOptions\n\ + Fo_unbounded FPoint_near FQvoronoi Fsummary Fvoronoi\n\ + FIDs\n\ +\n\ + Gvertices Gpoints Gall_points Gno_planes Ginner\n\ + Gcentrums Ghyperplanes Gridges Gouter GDrop_dim\n\ +\n\ + PArea_keep Pdrop d0:0D0 Pgood PFacet_area_keep\n\ + PGood_neighbors PMerge_keep Poutput_forced Pprecision_not\n\ +\n\ + QG_vertex_good QJoggle Qsearch_1st Qtriangulate Qupper_voronoi\n\ + QV_point_good Qzinfinite\n\ +\n\ + T4_trace Tcheck_often Tstatistics Tverify Tz_stdout\n\ + TFacet_log TInput_file TPoint_trace TMerge_trace TOutput_file\n\ + TWide_trace TVertex_stop TCone_stop\n\ +\n\ + Angle_max Centrum_size Random_dist Wide_outside\n\ +"; + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="main">-</a> + + main( argc, argv ) + processes the command line, calls qhull() to do the work, and exits + + design: + initializes data structures + reads points + finishes initialization + computes convex hull and other structures + checks the result + writes the output + frees memory +*/ +int main(int argc, char *argv[]) { + int curlong, totlong; /* used !qh_NOmem */ + int exitcode, numpoints, dim; + coordT *points; + boolT ismalloc; + +#if __MWERKS__ && __POWERPC__ + char inBuf[BUFSIZ], outBuf[BUFSIZ], errBuf[BUFSIZ]; + SIOUXSettings.showstatusline= false; + SIOUXSettings.tabspaces= 1; + SIOUXSettings.rows= 40; + if (setvbuf (stdin, inBuf, _IOFBF, sizeof(inBuf)) < 0 /* w/o, SIOUX I/O is slow*/ + || setvbuf (stdout, outBuf, _IOFBF, sizeof(outBuf)) < 0 + || (stdout != stderr && setvbuf (stderr, errBuf, _IOFBF, sizeof(errBuf)) < 0)) + fprintf (stderr, "qhull internal warning (main): could not change stdio to fully buffered.\n"); + argc= ccommand(&argv); +#endif + + if ((argc == 1) && isatty( 0 /*stdin*/)) { + fprintf(stdout, qh_prompt2, qh_version); + exit(qh_ERRnone); + } + if (argc > 1 && *argv[1] == '-' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompta, qh_version, + qh_promptb, qh_promptc, qh_promptd, qh_prompte); + exit(qh_ERRnone); + } + if (argc >1 && *argv[1] == '.' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompt3, qh_version); + exit(qh_ERRnone); + } + qh_init_A (stdin, stdout, stderr, argc, argv); /* sets qh qhull_command */ + exitcode= setjmp (qh errexit); /* simple statement for CRAY J916 */ + if (!exitcode) { + qh_option ("voronoi _bbound-last _coplanar-keep", NULL, NULL); + qh DELAUNAY= True; /* 'v' */ + qh VORONOI= True; + qh SCALElast= True; /* 'Qbb' */ + qh_checkflags (qh qhull_command, hidden_options); + qh_initflags (qh qhull_command); + points= qh_readpoints (&numpoints, &dim, &ismalloc); + if (dim >= 5) { + qh_option ("_merge-exact", NULL, NULL); + qh MERGEexact= True; /* 'Qx' always */ + } + qh_init_B (points, numpoints, dim, ismalloc); + qh_qhull(); + qh_check_output(); + qh_produce_output(); + if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points(); + exitcode= qh_ERRnone; + } + qh NOerrexit= True; /* no more setjmp */ +#ifdef qh_NOmem + qh_freeqhull( True); +#else + qh_freeqhull( False); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); +#endif + return exitcode; +} /* main */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/rbox.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/rbox.c new file mode 100644 index 0000000000000000000000000000000000000000..1c288bddc96bb7214908ce94ca0c498169e51de1 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/rbox.c @@ -0,0 +1,788 @@ +/*<html><pre> -<a href="index.htm#TOC" + >-------------------------------</a><a name="TOP">-</a> + + rbox.c + Generate input points for qhull. + + notes: + 50 points generated for 'rbox D4' + + This code needs a full rewrite. It needs separate procedures for each + distribution with common, helper procedures. + + WARNING: + incorrect range if qh_RANDOMmax is defined wrong (user.h) +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <ctype.h> +#include <math.h> +#include <limits.h> +#include <time.h> + +#include "user.h" +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <console.h> +#include <Desk.h> +#endif + +#ifdef _MSC_VER /* Microsoft Visual C++ */ +#pragma warning( disable : 4244) /* conversion from double to int */ +#endif + +#define MINVALUE 0.8 +#define MAXdim 200 +#define PI 3.1415926535897932384 +#define DEFAULTzbox 1e6 + +char prompt[]= "\n\ +-rbox- generate various point distributions. Default is random in cube.\n\ +\n\ +args (any order, space separated): Version: 2001/06/24\n\ + 3000 number of random points in cube, lens, spiral, sphere or grid\n\ + D3 dimension 3-d\n\ + c add a unit cube to the output ('c G2.0' sets size)\n\ + d add a unit diamond to the output ('d G2.0' sets size)\n\ + l generate a regular 3-d spiral\n\ + r generate a regular polygon, ('r s Z1 G0.1' makes a cone)\n\ + s generate cospherical points\n\ + x generate random points in simplex, may use 'r' or 'Wn'\n\ + y same as 'x', plus simplex\n\ + Pn,m,r add point [n,m,r] first, pads with 0\n\ +\n\ + Ln lens distribution of radius n. Also 's', 'r', 'G', 'W'.\n\ + Mn,m,r lattice (Mesh) rotated by [n,-m,0], [m,n,0], [0,0,r], ...\n\ + '27 M1,0,1' is {0,1,2} x {0,1,2} x {0,1,2}. Try 'M3,4 z'.\n\ + W0.1 random distribution within 0.1 of the cube's or sphere's surface\n\ + Z0.5 s random points in a 0.5 disk projected to a sphere\n\ + Z0.5 s G0.6 same as Z0.5 within a 0.6 gap\n\ +\n\ + Bn bounding box coordinates, default %2.2g\n\ + h output as homogeneous coordinates for cdd\n\ + n remove command line from the first line of output\n\ + On offset coordinates by n\n\ + t use time as the random number seed (default is command line)\n\ + tn use n as the random number seed\n\ + z print integer coordinates, default 'Bn' is %2.2g\n\ +"; + +/* ------------------------------ prototypes ----------------*/ +int roundi( double a); +void out1( double a); +void out2n( double a, double b); +void out3n( double a, double b, double c); +int qh_rand( void); +void qh_srand( int seed); + + +/* ------------------------------ globals -------------------*/ + + FILE *fp; + int isinteger= 0; + double out_offset= 0.0; + + +/*-------------------------------------------- +-rbox- main procedure of rbox application +*/ +int main(int argc, char **argv) { + int i,j,k; + int gendim; + int cubesize, diamondsize, seed=0, count, apex; + int dim=3 , numpoints= 0, totpoints, addpoints=0; + int issphere=0, isaxis=0, iscdd= 0, islens= 0, isregular=0, iswidth=0, addcube=0; + int isgap=0, isspiral=0, NOcommand= 0, adddiamond=0, istime=0; + int isbox=0, issimplex=0, issimplex2=0, ismesh=0; + double width=0.0, gap=0.0, radius= 0.0; + double coord[MAXdim], offset, meshm=3.0, meshn=4.0, meshr=5.0; + double *simplex, *simplexp; + int nthroot, mult[MAXdim]; + double norm, factor, randr, rangap, lensangle= 0, lensbase= 1; + double anglediff, angle, x, y, cube= 0.0, diamond= 0.0; + double box= qh_DEFAULTbox; /* scale all numbers before output */ + double randmax= qh_RANDOMmax; + char command[200], *s, seedbuf[200]; + time_t timedata; + +#if __MWERKS__ && __POWERPC__ + char inBuf[BUFSIZ], outBuf[BUFSIZ], errBuf[BUFSIZ]; + SIOUXSettings.showstatusline= False; + SIOUXSettings.tabspaces= 1; + SIOUXSettings.rows= 40; + if (setvbuf (stdin, inBuf, _IOFBF, sizeof(inBuf)) < 0 /* w/o, SIOUX I/O is slow*/ + || setvbuf (stdout, outBuf, _IOFBF, sizeof(outBuf)) < 0 + || (stdout != stderr && setvbuf (stderr, errBuf, _IOFBF, sizeof(errBuf)) < 0)) + fprintf ( stderr, "qhull internal warning (main): could not change stdio to fully buffered.\n"); + argc= ccommand(&argv); +#endif + if (argc == 1) { + printf (prompt, box, DEFAULTzbox); + exit(1); + } + if ((s = strrchr( argv[0], '\\'))) /* Borland gives full path */ + strcpy (command, s+1); + else + strcpy (command, argv[0]); + if ((s= strstr (command, ".EXE")) + || (s= strstr (command, ".exe"))) + *s= '\0'; + /* ============= read flags =============== */ + for (i=1; i < argc; i++) { + if (strlen (command) + strlen(argv[i]) + 1 < sizeof(command) ) { + strcat (command, " "); + strcat (command, argv[i]); + } + if (isdigit (argv[i][0])) { + numpoints= atoi (argv[i]); + continue; + } + if (argv[i][0] == '-') + (argv[i])++; + switch (argv[i][0]) { + case 'c': + addcube= 1; + if (i+1 < argc && argv[i+1][0] == 'G') + cube= (double) atof (&argv[++i][1]); + break; + case 'd': + adddiamond= 1; + if (i+1 < argc && argv[i+1][0] == 'G') + diamond= (double) atof (&argv[++i][1]); + break; + case 'h': + iscdd= 1; + break; + case 'l': + isspiral= 1; + break; + case 'n': + NOcommand= 1; + break; + case 'r': + isregular= 1; + break; + case 's': + issphere= 1; + break; + case 't': + istime= 1; + if (isdigit (argv[i][1])) + seed= atoi (&argv[i][1]); + else { + seed= time (&timedata); + sprintf (seedbuf, "%d", seed); + strcat (command, seedbuf); + } + break; + case 'x': + issimplex= 1; + break; + case 'y': + issimplex2= 1; + break; + case 'z': + isinteger= 1; + break; + case 'B': + box= (double) atof (&argv[i][1]); + isbox= 1; + break; + case 'D': + dim= atoi (&argv[i][1]); + if (dim < 1 + || dim > MAXdim) { + fprintf (stderr, "rbox error: dim %d too large or too small\n", dim); + exit (1); + } + break; + case 'G': + if (argv[i][1]) + gap= (double) atof (&argv[i][1]); + else + gap= 0.5; + isgap= 1; + break; + case 'L': + if (argv[i][1]) + radius= (double) atof (&argv[i][1]); + else + radius= 10; + islens= 1; + break; + case 'M': + ismesh= 1; + s= argv[i]+1; + if (*s) + meshn= strtod (s, &s); + if (*s == ',') + meshm= strtod (++s, &s); + else + meshm= 0.0; + if (*s == ',') + meshr= strtod (++s, &s); + else + meshr= sqrt (meshn*meshn + meshm*meshm); + if (*s) { + fprintf (stderr, "rbox warning: assuming 'M3,4,5' since mesh args are not integers or reals\n"); + meshn= 3.0, meshm=4.0, meshr=5.0; + } + break; + case 'O': + out_offset= (double) atof (&argv[i][1]); + break; + case 'P': + addpoints++; + break; + case 'W': + width= (double) atof (&argv[i][1]); + iswidth= 1; + break; + case 'Z': + if (argv[i][1]) + radius= (double) atof (&argv[i][1]); + else + radius= 1.0; + isaxis= 1; + break; + default: + fprintf (stderr, "rbox warning: unknown flag %s.\nExecute 'rbox' without arguments for documentation.\n", argv[i]); + } + } + /* ============= defaults, constants, and sizes =============== */ + if (isinteger && !isbox) + box= DEFAULTzbox; + if (addcube) { + cubesize= floor(ldexp(1.0,dim)+0.5); + if (cube == 0.0) + cube= box; + }else + cubesize= 0; + if (adddiamond) { + diamondsize= 2*dim; + if (diamond == 0.0) + diamond= box; + }else + diamondsize= 0; + if (islens) { + if (isaxis) { + fprintf (stderr, "rbox error: can not combine 'Ln' with 'Zn'\n"); + exit(1); + } + if (radius <= 1.0) { + fprintf (stderr, "rbox error: lens radius %.2g should be greater than 1.0\n", + radius); + exit(1); + } + lensangle= asin (1.0/radius); + lensbase= radius * cos (lensangle); + } + if (!numpoints) { + if (issimplex2) + ; /* ok */ + else if (isregular + issimplex + islens + issphere + isaxis + isspiral + iswidth + ismesh) { + fprintf (stderr, "rbox error: missing count\n"); + exit(1); + }else if (adddiamond + addcube + addpoints) + ; /* ok */ + else { + numpoints= 50; /* ./rbox D4 is the test case */ + issphere= 1; + } + } + if ((issimplex + islens + isspiral + ismesh > 1) + || (issimplex + issphere + isspiral + ismesh > 1)) { + fprintf (stderr, "rbox error: can only specify one of 'l', 's', 'x', 'Ln', or 'Mn,m,r' ('Ln s' is ok).\n"); + exit(1); + } + fp= stdout; + /* ============= print header with total points =============== */ + if (issimplex || ismesh) + totpoints= numpoints; + else if (issimplex2) + totpoints= numpoints+dim+1; + else if (isregular) { + totpoints= numpoints; + if (dim == 2) { + if (islens) + totpoints += numpoints - 2; + }else if (dim == 3) { + if (islens) + totpoints += 2 * numpoints; + else if (isgap) + totpoints += 1 + numpoints; + else + totpoints += 2; + } + }else + totpoints= numpoints + isaxis; + totpoints += cubesize + diamondsize + addpoints; + if (iscdd) + fprintf(fp, "%s\nbegin\n %d %d %s\n", + NOcommand ? "" : command, + totpoints, dim+1, + isinteger ? "integer" : "real"); + else if (NOcommand) + fprintf(fp, "%d\n%d\n", dim, totpoints); + else + fprintf(fp, "%d %s\n%d\n", dim, command, totpoints); + /* ============= seed randoms =============== */ + if (istime == 0) { + for (s=command; *s; s++) { + if (issimplex2 && *s == 'y') /* make 'y' same seed as 'x' */ + i= 'x'; + else + i= *s; + seed= 11*seed + i; + } + } /* else, seed explicitly set to n or to time */ + qh_RANDOMseed_(seed); + /* ============= explicit points =============== */ + for (i=1; i < argc; i++) { + if (argv[i][0] == 'P') { + s= argv[i]+1; + count= 0; + if (iscdd) + out1( 1.0); + while (*s) { + out1( strtod (s, &s)); + count++; + if (*s) { + if (*s++ != ',') { + fprintf (stderr, "rbox error: missing comma after coordinate in %s\n\n", argv[i]); + exit (1); + } + } + } + if (count < dim) { + for (k= dim-count; k--; ) + out1( 0.0); + }else if (count > dim) { + fprintf (stderr, "rbox error: %d coordinates instead of %d coordinates in %s\n\n", + count, dim, argv[i]); + exit (1); + } + fprintf (fp, "\n"); + } + } + /* ============= simplex distribution =============== */ + if (issimplex+issimplex2) { + if (!(simplex= malloc( dim * (dim+1) * sizeof(double)))) { + fprintf (stderr, "insufficient memory for simplex\n"); + exit(0); + } + simplexp= simplex; + if (isregular) { + for (i= 0; i<dim; i++) { + for (k= 0; k<dim; k++) + *(simplexp++)= i==k ? 1.0 : 0.0; + } + for (k= 0; k<dim; k++) + *(simplexp++)= -1.0; + }else { + for (i= 0; i<dim+1; i++) { + for (k= 0; k<dim; k++) { + randr= qh_RANDOMint; + *(simplexp++)= 2.0 * randr/randmax - 1.0; + } + } + } + if (issimplex2) { + simplexp= simplex; + for (i= 0; i<dim+1; i++) { + if (iscdd) + out1( 1.0); + for (k= 0; k<dim; k++) + out1( *(simplexp++) * box); + fprintf (fp, "\n"); + } + } + for (j= 0; j<numpoints; j++) { + if (iswidth) + apex= qh_RANDOMint % (dim+1); + else + apex= -1; + for (k= 0; k<dim; k++) + coord[k]= 0.0; + norm= 0.0; + for (i= 0; i<dim+1; i++) { + randr= qh_RANDOMint; + factor= randr/randmax; + if (i == apex) + factor *= width; + norm += factor; + for (k= 0; k<dim; k++) { + simplexp= simplex + i*dim + k; + coord[k] += factor * (*simplexp); + } + } + for (k= 0; k<dim; k++) + coord[k] /= norm; + if (iscdd) + out1( 1.0); + for (k=0; k < dim; k++) + out1( coord[k] * box); + fprintf (fp, "\n"); + } + isregular= 0; /* continue with isbox */ + numpoints= 0; + } + /* ============= mesh distribution =============== */ + if (ismesh) { + nthroot= pow (numpoints, 1.0/dim) + 0.99999; + for (k= dim; k--; ) + mult[k]= 0; + for (i= 0; i < numpoints; i++) { + for (k= 0; k < dim; k++) { + if (k == 0) + out1( mult[0] * meshn + mult[1] * (-meshm)); + else if (k == 1) + out1( mult[0] * meshm + mult[1] * meshn); + else + out1( mult[k] * meshr ); + } + fprintf (fp, "\n"); + for (k= 0; k < dim; k++) { + if (++mult[k] < nthroot) + break; + mult[k]= 0; + } + } + } + + /* ============= regular points for 's' =============== */ + else if (isregular && !islens) { + if (dim != 2 && dim != 3) { + fprintf(stderr, "rbox error: regular points can be used only in 2-d and 3-d\n\n"); + exit(1); + } + if (!isaxis || radius == 0.0) { + isaxis= 1; + radius= 1.0; + } + if (dim == 3) { + if (iscdd) + out1( 1.0); + out3n( 0.0, 0.0, -box); + if (!isgap) { + if (iscdd) + out1( 1.0); + out3n( 0.0, 0.0, box); + } + } + angle= 0.0; + anglediff= 2.0 * PI/numpoints; + for (i=0; i < numpoints; i++) { + angle += anglediff; + x= radius * cos (angle); + y= radius * sin (angle); + if (dim == 2) { + if (iscdd) + out1( 1.0); + out2n( x*box, y*box); + }else { + norm= sqrt (1.0 + x*x + y*y); + if (iscdd) + out1( 1.0); + out3n( box*x/norm, box*y/norm, box/norm); + if (isgap) { + x *= 1-gap; + y *= 1-gap; + norm= sqrt (1.0 + x*x + y*y); + if (iscdd) + out1( 1.0); + out3n( box*x/norm, box*y/norm, box/norm); + } + } + } + } + /* ============= regular points for 'r Ln D2' =============== */ + else if (isregular && islens && dim == 2) { + double cos_0; + + angle= lensangle; + anglediff= 2 * lensangle/(numpoints - 1); + cos_0= cos (lensangle); + for (i=0; i < numpoints; i++, angle -= anglediff) { + x= radius * sin (angle); + y= radius * (cos (angle) - cos_0); + if (iscdd) + out1( 1.0); + out2n( x*box, y*box); + if (i != 0 && i != numpoints - 1) { + if (iscdd) + out1( 1.0); + out2n( x*box, -y*box); + } + } + } + /* ============= regular points for 'r Ln D3' =============== */ + else if (isregular && islens && dim != 2) { + if (dim != 3) { + fprintf(stderr, "rbox error: regular points can be used only in 2-d and 3-d\n\n"); + exit(1); + } + angle= 0.0; + anglediff= 2* PI/numpoints; + if (!isgap) { + isgap= 1; + gap= 0.5; + } + offset= sqrt (radius * radius - (1-gap)*(1-gap)) - lensbase; + for (i=0; i < numpoints; i++, angle += anglediff) { + x= cos (angle); + y= sin (angle); + if (iscdd) + out1( 1.0); + out3n( box*x, box*y, 0); + x *= 1-gap; + y *= 1-gap; + if (iscdd) + out1( 1.0); + out3n( box*x, box*y, box * offset); + if (iscdd) + out1( 1.0); + out3n( box*x, box*y, -box * offset); + } + } + /* ============= apex of 'Zn' distribution + gendim =============== */ + else { + if (isaxis) { + gendim= dim-1; + if (iscdd) + out1( 1.0); + for (j=0; j < gendim; j++) + out1( 0.0); + out1( -box); + fprintf (fp, "\n"); + }else if (islens) + gendim= dim-1; + else + gendim= dim; + /* ============= generate random point in unit cube =============== */ + for (i=0; i < numpoints; i++) { + norm= 0.0; + for (j=0; j < gendim; j++) { + randr= qh_RANDOMint; + coord[j]= 2.0 * randr/randmax - 1.0; + norm += coord[j] * coord[j]; + } + norm= sqrt (norm); + /* ============= dim-1 point of 'Zn' distribution ========== */ + if (isaxis) { + if (!isgap) { + isgap= 1; + gap= 1.0; + } + randr= qh_RANDOMint; + rangap= 1.0 - gap * randr/randmax; + factor= radius * rangap / norm; + for (j=0; j<gendim; j++) + coord[j]= factor * coord[j]; + /* ============= dim-1 point of 'Ln s' distribution =========== */ + }else if (islens && issphere) { + if (!isgap) { + isgap= 1; + gap= 1.0; + } + randr= qh_RANDOMint; + rangap= 1.0 - gap * randr/randmax; + factor= rangap / norm; + for (j=0; j<gendim; j++) + coord[j]= factor * coord[j]; + /* ============= dim-1 point of 'Ln' distribution ========== */ + }else if (islens && !issphere) { + if (!isgap) { + isgap= 1; + gap= 1.0; + } + j= qh_RANDOMint % gendim; + if (coord[j] < 0) + coord[j]= -1.0 - coord[j] * gap; + else + coord[j]= 1.0 - coord[j] * gap; + /* ============= point of 'l' distribution =============== */ + }else if (isspiral) { + if (dim != 3) { + fprintf(stderr, "rbox error: spiral distribution is available only in 3d\n\n"); + exit(1); + } + coord[0]= cos(2*PI*i/(numpoints - 1)); + coord[1]= sin(2*PI*i/(numpoints - 1)); + coord[2]= 2.0*(double)i/(double)(numpoints-1) - 1.0; + /* ============= point of 's' distribution =============== */ + }else if (issphere) { + factor= 1.0/norm; + if (iswidth) { + randr= qh_RANDOMint; + factor *= 1.0 - width * randr/randmax; + } + for (j=0; j<dim; j++) + coord[j]= factor * coord[j]; + } + /* ============= project 'Zn s' point in to sphere =============== */ + if (isaxis && issphere) { + coord[dim-1]= 1.0; + norm= 1.0; + for (j=0; j<gendim; j++) + norm += coord[j] * coord[j]; + norm= sqrt (norm); + for (j=0; j<dim; j++) + coord[j]= coord[j] / norm; + if (iswidth) { + randr= qh_RANDOMint; + coord[dim-1] *= 1 - width * randr/randmax; + } + /* ============= project 'Zn' point onto cube =============== */ + }else if (isaxis && !issphere) { /* not very interesting */ + randr= qh_RANDOMint; + coord[dim-1]= 2.0 * randr/randmax - 1.0; + /* ============= project 'Ln' point out to sphere =============== */ + }else if (islens) { + coord[dim-1]= lensbase; + for (j=0, norm= 0; j<dim; j++) + norm += coord[j] * coord[j]; + norm= sqrt (norm); + for (j=0; j<dim; j++) + coord[j]= coord[j] * radius/ norm; + coord[dim-1] -= lensbase; + if (iswidth) { + randr= qh_RANDOMint; + coord[dim-1] *= 1 - width * randr/randmax; + } + if (qh_RANDOMint > randmax/2) + coord[dim-1]= -coord[dim-1]; + /* ============= project 'Wn' point toward boundary =============== */ + }else if (iswidth && !issphere) { + j= qh_RANDOMint % gendim; + if (coord[j] < 0) + coord[j]= -1.0 - coord[j] * width; + else + coord[j]= 1.0 - coord[j] * width; + } + /* ============= write point =============== */ + if (iscdd) + out1( 1.0); + for (k=0; k < dim; k++) + out1( coord[k] * box); + fprintf (fp, "\n"); + } + } + /* ============= write cube vertices =============== */ + if (addcube) { + for (j=0; j<cubesize; j++) { + if (iscdd) + out1( 1.0); + for (k=dim-1; k>=0; k--) { + if (j & ( 1 << k)) + out1( cube); + else + out1( -cube); + } + fprintf (fp, "\n"); + } + } + /* ============= write diamond vertices =============== */ + if (adddiamond) { + for (j=0; j<diamondsize; j++) { + if (iscdd) + out1( 1.0); + for (k=dim-1; k>=0; k--) { + if (j/2 != k) + out1( 0.0); + else if (j & 0x1) + out1( diamond); + else + out1( -diamond); + } + fprintf (fp, "\n"); + } + } + if (iscdd) + fprintf (fp, "end\nhull\n"); + return 0; + } /* rbox */ + +/*------------------------------------------------ +-outxxx - output functions +*/ +int roundi( double a) { + if (a < 0.0) { + if (a - 0.5 < INT_MIN) { + fprintf(stderr, "rbox input error: coordinate %2.2g is too large. Reduce 'Bn'\n", a); + exit (1); + } + return a - 0.5; + }else { + if (a + 0.5 > INT_MAX) { + fprintf(stderr, "rbox input error: coordinate %2.2g is too large. Reduce 'Bn'\n", a); + exit (1); + } + return a + 0.5; + } +} /* roundi */ + +void out1(double a) { + + if (isinteger) + fprintf(fp, "%d ", roundi( a+out_offset)); + else + fprintf(fp, qh_REAL_1, a+out_offset); +} /* out1 */ + +void out2n( double a, double b) { + + if (isinteger) + fprintf(fp, "%d %d\n", roundi(a+out_offset), roundi(b+out_offset)); + else + fprintf(fp, qh_REAL_2n, a+out_offset, b+out_offset); +} /* out2n */ + +void out3n( double a, double b, double c) { + + if (isinteger) + fprintf(fp, "%d %d %d\n", roundi(a+out_offset), roundi(b+out_offset), roundi(c+out_offset)); + else + fprintf(fp, qh_REAL_3n, a+out_offset, b+out_offset, c+out_offset); +} /* out3n */ + +/*------------------------------------------------- +-rand & srand- generate pseudo-random number between 1 and 2^31 -2 + from Park & Miller's minimimal standard random number generator + Communications of the ACM, 31:1192-1201, 1988. +notes: + does not use 0 or 2^31 -1 + this is silently enforced by qh_srand() + copied from geom2.c +*/ +static int seed = 1; /* global static */ + +int qh_rand( void) { +#define qh_rand_a 16807 +#define qh_rand_m 2147483647 +#define qh_rand_q 127773 /* m div a */ +#define qh_rand_r 2836 /* m mod a */ + int lo, hi, test; + + hi = seed / qh_rand_q; /* seed div q */ + lo = seed % qh_rand_q; /* seed mod q */ + test = qh_rand_a * lo - qh_rand_r * hi; + if (test > 0) + seed= test; + else + seed= test + qh_rand_m; + return seed; +} /* rand */ + +void qh_srand( int newseed) { + if (newseed < 1) + seed= 1; + else if (newseed >= qh_rand_m) + seed= qh_rand_m - 1; + else + seed= newseed; +} /* qh_srand */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/unix.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/unix.c new file mode 100644 index 0000000000000000000000000000000000000000..9f06fcefb3ab432ac50b502ba0624ec61e2b61de --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/unix.c @@ -0,0 +1,377 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + unix.c + command line interface to qhull + includes SIOUX interface for Macintoshes + + see qh-qhull.htm + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <ctype.h> +#include <math.h> +#include "qhull.h" +#include "mem.h" +#include "qset.h" + +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <console.h> +#include <Desk.h> + +#elif __cplusplus +extern "C" { + int isatty (int); +} + +#elif _MSC_VER +#include <io.h> +#define isatty _isatty + +#else +int isatty (int); /* returns 1 if stdin is a tty + if "Undefined symbol" this can be deleted along with call in main() */ +#endif + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt">-</a> + + qh_prompt + long prompt for qhull + + see: + concise prompt below +*/ +char qh_prompta[]= "\n\ +qhull- compute convex hulls and related structures.\n\ + http://www.qhull.org %s\n\ +\n\ +input (stdin):\n\ + first lines: dimension and number of points (or vice-versa).\n\ + other lines: point coordinates, best if one point per line\n\ + comments: start with a non-numeric character\n\ + halfspaces: use dim plus one and put offset after coefficients.\n\ + May be preceeded by a single interior point ('H').\n\ +\n\ +options:\n\ + d - Delaunay triangulation by lifting points to a paraboloid\n\ + d Qu - furthest-site Delaunay triangulation (upper convex hull)\n\ + v - Voronoi diagram (dual of the Delaunay triangulation)\n\ + v Qu - furthest-site Voronoi diagram\n\ + Hn,n,... - halfspace intersection about point [n,n,0,...]\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Qc - keep coplanar points with nearest facet\n\ + Qi - keep interior points with nearest facet\n\ +\n\ +Qhull control options:\n\ + Qbk:n - scale coord k so that low bound is n\n\ + QBk:n - scale coord k so that upper bound is n (QBk is %2.2g)\n\ + QbB - scale input to unit cube centered at the origin\n\ + Qbb - scale last coordinate to [0,m] for Delaunay triangulations\n\ + Qbk:0Bk:0 - remove k-th coordinate from input\n\ + QJn - randomly joggle input in range [-n,n]\n\ + QRn - random rotation (n=seed, n=0 time, n=-1 time/no rotate)\n\ +%s%s%s%s"; /* split up qh_prompt for Visual C++ */ +char qh_promptb[]= "\ + Qf - partition point to furthest outside facet\n\ + Qg - only build good facets (needs 'QGn', 'QVn', or 'PdD')\n\ + Qm - only process points that would increase max_outside\n\ + Qr - process random outside points instead of furthest ones\n\ + Qs - search all points for the initial simplex\n\ + Qu - for 'd' or 'v', compute upper hull without point at-infinity\n\ + returns furthest-site Delaunay triangulation\n\ + Qv - test vertex neighbors for convexity\n\ + Qx - exact pre-merges (skips coplanar and angle-coplanar facets)\n\ + Qz - add point-at-infinity to Delaunay triangulation\n\ + QGn - good facet if visible from point n, -n for not visible\n\ + QVn - good facet if it includes point n, -n if not\n\ + Q0 - turn off default premerge with 'C-0'/'Qx'\n\ + Q1 - sort merges by type instead of angle\n\ + Q2 - merge all non-convex at once instead of independent sets\n\ + Q3 - do not merge redundant vertices\n\ + Q4 - avoid old->new merges\n\ + Q5 - do not correct outer planes at end of qhull\n\ + Q6 - do not pre-merge concave or coplanar facets\n\ + Q7 - depth-first processing instead of breadth-first\n\ + Q8 - do not process near-inside points\n\ + Q9 - process furthest of furthest points\n\ + Q10 - no special processing for narrow distributions\n\ + Q11 - copy normals and recompute centrums for tricoplanar facets\n\ +\n\ +"; +char qh_promptc[]= "\ +Topts- Trace options:\n\ + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events\n\ + Tc - check frequently during execution\n\ + Ts - print statistics\n\ + Tv - verify result: structure, convexity, and point inclusion\n\ + Tz - send all output to stdout\n\ + TFn - report summary when n or more facets created\n\ + TI file - input data from file, no spaces or single quotes\n\ + TO file - output results to file, may be enclosed in single quotes\n\ + TPn - turn on tracing when point n added to hull\n\ + TMn - turn on tracing at merge n\n\ + TWn - trace merge facets when width > n\n\ + TRn - rerun qhull n times. Use with 'QJn'\n\ + TVn - stop qhull after adding point n, -n for before (see TCn)\n\ + TCn - stop qhull after building cone for point n (see TVn)\n\ +\n\ +Precision options:\n\ + Cn - radius of centrum (roundoff added). Merge facets if non-convex\n\ + An - cosine of maximum angle. Merge facets if cosine > n or non-convex\n\ + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge\n\ + En - max roundoff error for distance computation\n\ + Rn - randomly perturb computations by a factor of [1-n,1+n]\n\ + Vn - min distance above plane for a visible facet (default 3C-n or En)\n\ + Un - max distance below plane for a new, coplanar point (default Vn)\n\ + Wn - min facet width for outside point (before roundoff, default 2Vn)\n\ +\n\ +Output formats (may be combined; if none, produces a summary to stdout):\n\ + f - facet dump\n\ + G - Geomview output (see below)\n\ + i - vertices incident to each facet\n\ + m - Mathematica output (2-d and 3-d)\n\ + o - OFF format (dim, points and facets; Voronoi regions)\n\ + n - normals with offsets\n\ + p - vertex coordinates or Voronoi vertices (coplanar points if 'Qc')\n\ + s - summary (stderr)\n\ +\n\ +"; +char qh_promptd[]= "\ +More formats:\n\ + Fa - area for each facet\n\ + FA - compute total area and volume for option 's'\n\ + Fc - count plus coplanar points for each facet\n\ + use 'Qc' (default) for coplanar and 'Qi' for interior\n\ + FC - centrum or Voronoi center for each facet\n\ + Fd - use cdd format for input (homogeneous with offset first)\n\ + FD - use cdd format for numeric output (offset first)\n\ + FF - facet dump without ridges\n\ + Fi - inner plane for each facet\n\ + for 'v', separating hyperplanes for bounded Voronoi regions\n\ + FI - ID of each facet\n\ + Fm - merge count for each facet (511 max)\n\ + FM - Maple output (2-d and 3-d)\n\ + Fn - count plus neighboring facets for each facet\n\ + FN - count plus neighboring facets for each point\n\ + Fo - outer plane (or max_outside) for each facet\n\ + for 'v', separating hyperplanes for unbounded Voronoi regions\n\ + FO - options and precision constants\n\ + Fp - dim, count, and intersection coordinates (halfspace only)\n\ + FP - nearest vertex and distance for each coplanar point\n\ + FQ - command used for qhull\n\ + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets,\n\ + output: #vertices, #facets, #coplanars, #nonsimplicial\n\ + #real (2), max outer plane, min vertex\n\ + FS - sizes: #int (0)\n\ + #real(2) tot area, tot volume\n\ + Ft - triangulation with centrums for non-simplicial facets (OFF format)\n\ + Fv - count plus vertices for each facet\n\ + for 'v', Voronoi diagram as Voronoi vertices for pairs of sites\n\ + FV - average of vertices (a feasible point for 'H')\n\ + Fx - extreme points (in order for 2-d)\n\ +\n\ +"; +char qh_prompte[]= "\ +Geomview options (2-d, 3-d, and 4-d; 2-d Voronoi)\n\ + Ga - all points as dots\n\ + Gp - coplanar points and vertices as radii\n\ + Gv - vertices as spheres\n\ + Gi - inner planes only\n\ + Gn - no planes\n\ + Go - outer planes only\n\ + Gc - centrums\n\ + Gh - hyperplane intersections\n\ + Gr - ridges\n\ + GDn - drop dimension n in 3-d and 4-d output\n\ + Gt - for 3-d 'd', transparent outer ridges\n\ +\n\ +Print options:\n\ + PAn - keep n largest facets by area\n\ + Pdk:n - drop facet if normal[k] <= n (default 0.0)\n\ + PDk:n - drop facet if normal[k] >= n\n\ + Pg - print good facets (needs 'QGn' or 'QVn')\n\ + PFn - keep facets whose area is at least n\n\ + PG - print neighbors of good facets\n\ + PMn - keep n facets with most merges\n\ + Po - force output. If error, output neighborhood of facet\n\ + Pp - do not report precision problems\n\ +\n\ + . - list of all options\n\ + - - one line descriptions of all options\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt2">-</a> + + qh_prompt2 + synopsis for qhull +*/ +char qh_prompt2[]= "\n\ +qhull- compute convex hulls and related structures. Qhull %s\n\ + input (stdin): dimension, n, point coordinates\n\ + comments start with a non-numeric character\n\ + halfspace: use dim+1 and put offsets after coefficients\n\ +\n\ +options (qh-quick.htm):\n\ + d - Delaunay triangulation by lifting points to a paraboloid\n\ + d Qu - furthest-site Delaunay triangulation (upper convex hull)\n\ + v - Voronoi diagram as the dual of the Delaunay triangulation\n\ + v Qu - furthest-site Voronoi diagram\n\ + H1,1 - Halfspace intersection about [1,1,0,...] via polar duality\n\ + Qt - triangulated output\n\ + QJ - joggled input instead of merged facets\n\ + Tv - verify result: structure, convexity, and point inclusion\n\ + . - concise list of all options\n\ + - - one-line description of all options\n\ +\n\ +Output options (subset):\n\ + s - summary of results (default)\n\ + i - vertices incident to each facet\n\ + n - normals with offsets\n\ + p - vertex coordinates (if 'Qc', includes coplanar points)\n\ + if 'v', Voronoi vertices\n\ + Fp - halfspace intersections\n\ + Fx - extreme points (convex hull vertices)\n\ + FA - compute total area and volume\n\ + o - OFF format (if 'v', outputs Voronoi regions)\n\ + G - Geomview output (2-d, 3-d and 4-d)\n\ + m - Mathematica output (2-d and 3-d)\n\ + QVn - print facets that include point n, -n if not\n\ + TO file- output results to file, may be enclosed in single quotes\n\ +\n\ +examples:\n\ + rbox c d D2 | qhull Qc s f Fx | more rbox 1000 s | qhull Tv s FA\n\ + rbox 10 D2 | qhull d QJ s i TO result rbox 10 D2 | qhull v Qbb Qt p\n\ + rbox 10 D2 | qhull d Qu QJ m rbox 10 D2 | qhull v Qu QJ o\n\ + rbox c | qhull n rbox c | qhull FV n | qhull H Fp\n\ + rbox d D12 | qhull QR0 FA rbox c D7 | qhull FA TF1000\n\ + rbox y 1000 W0 | qhull rbox 10 | qhull v QJ o Fv\n\ +\n\ +"; +/* for opts, don't assign 'e' or 'E' to a flag (already used for exponent) */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="prompt3">-</a> + + qh_prompt3 + concise prompt for qhull +*/ +char qh_prompt3[]= "\n\ +Qhull %s.\n\ +Except for 'F.' and 'PG', upper-case options take an argument.\n\ +\n\ + delaunay voronoi Geomview Halfspace facet_dump\n\ + incidences mathematica normals OFF_format points\n\ + summary\n\ +\n\ + Farea FArea-total Fcoplanars FCentrums Fd-cdd-in\n\ + FD-cdd-out FF-dump-xridge Finner FIDs Fmerges\n\ + Fneighbors FNeigh-vertex Fouter FOptions Fpoint-intersect\n\ + FPoint_near FQhull Fsummary FSize Ftriangles\n\ + Fvertices Fvoronoi FVertex-ave Fxtremes FMaple\n\ +\n\ + Gvertices Gpoints Gall_points Gno_planes Ginner\n\ + Gcentrums Ghyperplanes Gridges Gouter GDrop_dim\n\ + Gtransparent\n\ +\n\ + PArea-keep Pdrop d0:0D0 Pgood PFacet_area_keep\n\ + PGood_neighbors PMerge-keep Poutput_forced Pprecision_not\n\ +\n\ + QbBound 0:0.5 Qbk:0Bk:0_drop QbB-scale-box Qbb-scale-last Qcoplanar\n\ + Qfurthest Qgood_only QGood_point Qinterior Qmax_out\n\ + QJoggle Qrandom QRotate Qsearch_1st Qtriangulate\n\ + QupperDelaunay QVertex_good Qvneighbors Qxact_merge Qzinfinite\n\ +\n\ + Q0_no_premerge Q1_no_angle Q2_no_independ Q3_no_redundant Q4_no_old\n\ + Q5_no_check_out Q6_no_concave Q7_depth_first Q8_no_near_in Q9_pick_furthest\n\ + Q10_no_narrow Q11_trinormals\n\ +\n\ + T4_trace Tcheck_often Tstatistics Tverify Tz_stdout\n\ + TFacet_log TInput_file TPoint_trace TMerge_trace TOutput_file\n\ + TRerun TWide_trace TVertex_stop TCone_stop\n\ +\n\ + Angle_max Centrum_size Error_round Random_dist Visible_min\n\ + Ucoplanar_max Wide_outside\n\ +"; + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="main">-</a> + + main( argc, argv ) + processes the command line, calls qhull() to do the work, and exits + + design: + initializes data structures + reads points + finishes initialization + computes convex hull and other structures + checks the result + writes the output + frees memory +*/ +int main(int argc, char *argv[]) { + int curlong, totlong; /* used !qh_NOmem */ + int exitcode, numpoints, dim; + coordT *points; + boolT ismalloc; + +#if __MWERKS__ && __POWERPC__ + char inBuf[BUFSIZ], outBuf[BUFSIZ], errBuf[BUFSIZ]; + SIOUXSettings.showstatusline= false; + SIOUXSettings.tabspaces= 1; + SIOUXSettings.rows= 40; + if (setvbuf (stdin, inBuf, _IOFBF, sizeof(inBuf)) < 0 /* w/o, SIOUX I/O is slow*/ + || setvbuf (stdout, outBuf, _IOFBF, sizeof(outBuf)) < 0 + || (stdout != stderr && setvbuf (stderr, errBuf, _IOFBF, sizeof(errBuf)) < 0)) + fprintf (stderr, "qhull internal warning (main): could not change stdio to fully buffered.\n"); + argc= ccommand(&argv); +#endif + + if ((argc == 1) && isatty( 0 /*stdin*/)) { + fprintf(stdout, qh_prompt2, qh_version); + exit(qh_ERRnone); + } + if (argc > 1 && *argv[1] == '-' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompta, qh_version, qh_DEFAULTbox, + qh_promptb, qh_promptc, qh_promptd, qh_prompte); + exit(qh_ERRnone); + } + if (argc >1 && *argv[1] == '.' && !*(argv[1]+1)) { + fprintf(stdout, qh_prompt3, qh_version); + exit(qh_ERRnone); + } + qh_init_A (stdin, stdout, stderr, argc, argv); /* sets qh qhull_command */ + exitcode= setjmp (qh errexit); /* simple statement for CRAY J916 */ + if (!exitcode) { + qh_initflags (qh qhull_command); + points= qh_readpoints (&numpoints, &dim, &ismalloc); + qh_init_B (points, numpoints, dim, ismalloc); + qh_qhull(); + qh_check_output(); + qh_produce_output(); + if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points(); + exitcode= qh_ERRnone; + } + qh NOerrexit= True; /* no more setjmp */ +#ifdef qh_NOmem + qh_freeqhull( True); +#else + qh_freeqhull( False); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); +#endif + return exitcode; +} /* main */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/user_eg.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/user_eg.c new file mode 100644 index 0000000000000000000000000000000000000000..9f2ac81d781955721a83f4762bac571e6f13d7a8 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/user_eg.c @@ -0,0 +1,317 @@ +/*<html><pre> -<a href="qh-user.htm" + >-------------------------------</a><a name="TOP">-</a> + + user_eg.c + sample code for calling qhull() from an application + + call with: + + user_eg "cube/diamond options" "delaunay options" "halfspace options" + + for example: + + user_eg # return summaries + + user_eg "n" "o" "Fp" # return normals, OFF, points + + user_eg "n Qt" "o" "Fp" # triangulated cube + + user_eg "QR0 p" "QR0 v p" "QR0 Fp" # rotate input and return points + # 'v' returns Voronoi + # transform is rotated for halfspaces + + main() makes three runs of qhull. + + 1) compute the convex hull of a cube + + 2a) compute the Delaunay triangulation of random points + + 2b) find the Delaunay triangle closest to a point. + + 3) compute the halfspace intersection of a diamond + + notes: + + For another example, see main() in unix.c and user_eg2.c. + These examples, call qh_qhull() directly. They allow + tighter control on the code loaded with Qhull. + + For a simple C++ example, see qhull_interface.cpp + + Summaries are sent to stderr if other output formats are used + + compiled by 'make user_eg' + + see qhull.h for data structures, macros, and user-callable functions. +*/ + +#include "qhull_a.h" + +/*------------------------------------------------- +-internal function prototypes +*/ +void print_summary (void); +void makecube (coordT *points, int numpoints, int dim); +void makeDelaunay (coordT *points, int numpoints, int dim, int seed); +void findDelaunay (int dim); +void makehalf (coordT *points, int numpoints, int dim); + +/*------------------------------------------------- +-print_summary() +*/ +void print_summary (void) { + facetT *facet; + int k; + + printf ("\n%d vertices and %d facets with normals:\n", + qh num_vertices, qh num_facets); + FORALLfacets { + for (k=0; k < qh hull_dim; k++) + printf ("%6.2g ", facet->normal[k]); + printf ("\n"); + } +} + +/*-------------------------------------------------- +-makecube- set points to vertices of cube + points is numpoints X dim +*/ +void makecube (coordT *points, int numpoints, int dim) { + int j,k; + coordT *point; + + for (j=0; j<numpoints; j++) { + point= points + j*dim; + for (k=dim; k--; ) { + if (j & ( 1 << k)) + point[k]= 1.0; + else + point[k]= -1.0; + } + } +} /*.makecube.*/ + +/*-------------------------------------------------- +-makeDelaunay- set points for dim Delaunay triangulation of random points + points is numpoints X dim. +notes: + makeDelaunay() in user_eg2.c uses qh_setdelaunay() to project points in place. +*/ +void makeDelaunay (coordT *points, int numpoints, int dim, int seed) { + int j,k; + coordT *point, realr; + + printf ("seed: %d\n", seed); + qh_RANDOMseed_( seed); + for (j=0; j<numpoints; j++) { + point= points + j*dim; + for (k= 0; k < dim; k++) { + realr= qh_RANDOMint; + point[k]= 2.0 * realr/(qh_RANDOMmax+1) - 1.0; + } + } +} /*.makeDelaunay.*/ + +/*-------------------------------------------------- +-findDelaunay- find Delaunay triangle for [0.5,0.5,...] + assumes dim < 100 +notes: + calls qh_setdelaunay() to project the point to a parabaloid +warning: + This is not implemented for tricoplanar facets ('Qt'), + See <a href="../html/qh-in.htm#findfacet">locate a facet with qh_findbestfacet()</a> +*/ +void findDelaunay (int dim) { + int k; + coordT point[ 100]; + boolT isoutside; + realT bestdist; + facetT *facet; + vertexT *vertex, **vertexp; + + for (k= 0; k < dim; k++) + point[k]= 0.5; + qh_setdelaunay (dim+1, 1, point); + facet= qh_findbestfacet (point, qh_ALL, &bestdist, &isoutside); + if (facet->tricoplanar) { + fprintf(stderr, "findDelaunay: not implemented for triangulated, non-simplicial Delaunay regions (tricoplanar facet, f%d).\n", + facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + FOREACHvertex_(facet->vertices) { + for (k=0; k < dim; k++) + printf ("%5.2f ", vertex->point[k]); + printf ("\n"); + } +} /*.findDelaunay.*/ + +/*-------------------------------------------------- +-makehalf- set points to halfspaces for a (dim)-dimensional diamond + points is numpoints X dim+1 + + each halfspace consists of dim coefficients followed by an offset +*/ +void makehalf (coordT *points, int numpoints, int dim) { + int j,k; + coordT *point; + + for (j=0; j<numpoints; j++) { + point= points + j*(dim+1); + point[dim]= -1.0; /* offset */ + for (k=dim; k--; ) { + if (j & ( 1 << k)) + point[k]= 1.0; + else + point[k]= -1.0; + } + } +} /*.makehalf.*/ + +#define DIM 3 /* dimension of points, must be < 31 for SIZEcube */ +#define SIZEcube (1<<DIM) +#define SIZEdiamond (2*DIM) +#define TOTpoints (SIZEcube + SIZEdiamond) + +/*-------------------------------------------------- +-main- derived from call_qhull in user.c + + see program header + + this contains three runs of Qhull for convex hull, Delaunay + triangulation or Voronoi vertices, and halfspace intersection + +*/ +int main (int argc, char *argv[]) { + int dim= DIM; /* dimension of points */ + int numpoints; /* number of points */ + coordT points[(DIM+1)*TOTpoints]; /* array of coordinates for each point */ + coordT *rows[TOTpoints]; + boolT ismalloc= False; /* True if qhull should free points in qh_freeqhull() or reallocation */ + char flags[250]; /* option flags for qhull, see qh_opt.htm */ + FILE *outfile= stdout; /* output from qh_produce_output() + use NULL to skip qh_produce_output() */ + FILE *errfile= stderr; /* error messages from qhull code */ + int exitcode; /* 0 if no error from qhull */ + facetT *facet; /* set by FORALLfacets */ + int curlong, totlong; /* memory remaining after qh_memfreeshort */ + int i; + + printf ("This is the output from user_eg.c\n\n\ +It shows how qhull() may be called from an application. It is not part\n\ +of qhull itself. If it appears accidently, please remove user_eg.c from\n\ +your project.\n\n"); + + /* + Run 1: convex hull + */ + printf( "\ncompute convex hull of cube after rotating input\n"); + sprintf (flags, "qhull s Tcv %s", argc >= 2 ? argv[1] : ""); + numpoints= SIZEcube; + makecube (points, numpoints, DIM); + for (i=numpoints; i--; ) + rows[i]= points+dim*i; + qh_printmatrix (outfile, "input", rows, numpoints, dim); + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, + flags, outfile, errfile); + if (!exitcode) { /* if no error */ + /* 'qh facet_list' contains the convex hull */ + print_summary(); + FORALLfacets { + /* ... your code ... */ + } + } + qh_freeqhull(!qh_ALL); /* free long memory */ + qh_memfreeshort (&curlong, &totlong); /* free short memory and memory allocator */ + if (curlong || totlong) + fprintf (errfile, "qhull internal warning (user_eg, #1): did not free %d bytes of long memory (%d pieces)\n", totlong, curlong); + + /* + Run 2: Delaunay triangulation + */ + + printf( "\ncompute %d-d Delaunay triangulation\n", dim); + sprintf (flags, "qhull s d Tcv %s", argc >= 3 ? argv[2] : ""); + numpoints= SIZEcube; + makeDelaunay (points, numpoints, dim, time(NULL)); + for (i=numpoints; i--; ) + rows[i]= points+dim*i; + qh_printmatrix (outfile, "input", rows, numpoints, dim); + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, + flags, outfile, errfile); + if (!exitcode) { /* if no error */ + /* 'qh facet_list' contains the convex hull */ + /* If you want a Voronoi diagram ('v') and do not request output (i.e., outfile=NULL), + call qh_setvoronoi_all() after qh_new_qhull(). */ + print_summary(); + FORALLfacets { + /* ... your code ... */ + } + printf( "\nfind %d-d Delaunay triangle closest to [0.5, 0.5, ...]\n", dim); + exitcode= setjmp (qh errexit); + if (!exitcode) { + /* Trap Qhull errors in findDelaunay(). Without the setjmp(), Qhull + will exit() after reporting an error */ + qh NOerrexit= False; + findDelaunay (DIM); + } + qh NOerrexit= True; + } +#if qh_QHpointer /* see user.h */ + { + qhT *oldqhA, *oldqhB; + coordT pointsB[DIM*TOTpoints]; /* array of coordinates for each point */ + + + printf( "\nsave first triangulation and compute a new triangulation\n"); + oldqhA= qh_save_qhull(); + sprintf (flags, "qhull s d Tcv %s", argc >= 3 ? argv[2] : ""); + numpoints= SIZEcube; + makeDelaunay (pointsB, numpoints, dim, time(NULL)+1); + for (i=numpoints; i--; ) + rows[i]= pointsB+dim*i; + qh_printmatrix (outfile, "input", rows, numpoints, dim); + exitcode= qh_new_qhull (dim, numpoints, pointsB, ismalloc, + flags, outfile, errfile); + if (!exitcode) + print_summary(); + printf( "\nsave second triangulation and restore first one\n"); + oldqhB= qh_save_qhull(); + qh_restore_qhull (&oldqhA); + print_summary(); + printf( "\nfree first triangulation and restore second one.\n"); + qh_freeqhull (qh_ALL); /* free short and long memory used by first call */ + /* do not use qh_memfreeshort */ + qh_restore_qhull (&oldqhB); + print_summary(); + } +#endif + qh_freeqhull(!qh_ALL); /* free long memory */ + qh_memfreeshort (&curlong, &totlong); /* free short memory and memory allocator */ + if (curlong || totlong) + fprintf (errfile, "qhull internal warning (user_eg, #2): did not free %d bytes of long memory (%d pieces)\n", totlong, curlong); + + /* + Run 3: halfspace intersection about the origin + */ + printf( "\ncompute halfspace intersection about the origin for a diamond\n"); + sprintf (flags, "qhull H0 s Tcv %s", argc >= 4 ? argv[3] : "Fp"); + numpoints= SIZEcube; + makehalf (points, numpoints, dim); + for (i=numpoints; i--; ) + rows[i]= points+(dim+1)*i; + qh_printmatrix (outfile, "input as halfspace coefficients + offsets", rows, numpoints, dim+1); + /* use qh_sethalfspace_all to transform the halfspaces yourself. + If so, set 'qh feasible_point and do not use option 'Hn,...' [it would retransform the halfspaces] + */ + exitcode= qh_new_qhull (dim+1, numpoints, points, ismalloc, + flags, outfile, errfile); + if (!exitcode) + print_summary(); + qh_freeqhull (!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) /* could also check previous runs */ + fprintf (stderr, "qhull internal warning (user_eg, #3): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); + return exitcode; +} /* main */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/user_eg2.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/user_eg2.c new file mode 100644 index 0000000000000000000000000000000000000000..28ecbce467423160cd933c0165683c1c09108e36 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/eg/user_eg2.c @@ -0,0 +1,542 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + user_eg2.c + + sample code for calling qhull() from an application. + + See user_eg.c for a simpler method using qh_new_qhull(). + The method used here and in unix.c gives you additional + control over Qhull. + + call with: + + user_eg2 "triangulated cube/diamond options" "delaunay options" "halfspace options" + + for example: + + user_eg2 # return summaries + + user_eg2 "n" "o" "Fp" # return normals, OFF, points + + user_eg2 "QR0 p" "QR0 v p" "QR0 Fp" # rotate input and return points + # 'v' returns Voronoi + # transform is rotated for halfspaces + + main() makes three runs of qhull. + + 1) compute the convex hull of a cube, and incrementally add a diamond + + 2a) compute the Delaunay triangulation of random points, and add points. + + 2b) find the Delaunay triangle closest to a point. + + 3) compute the halfspace intersection of a diamond, and add a cube + + notes: + + summaries are sent to stderr if other output formats are used + + derived from unix.c and compiled by 'make user_eg2' + + see qhull.h for data structures, macros, and user-callable functions. + + If you want to control all output to stdio and input to stdin, + set the #if below to "1" and delete all lines that contain "io.c". + This prevents the loading of io.o. Qhull will + still write to 'qh ferr' (stderr) for error reporting and tracing. + + Defining #if 1, also prevents user.o from being loaded. +*/ + +#include "qhull_a.h" + +/*------------------------------------------------- +-internal function prototypes +*/ +void print_summary (void); +void makecube (coordT *points, int numpoints, int dim); +void adddiamond (coordT *points, int numpoints, int numnew, int dim); +void makeDelaunay (coordT *points, int numpoints, int dim); +void addDelaunay (coordT *points, int numpoints, int numnew, int dim); +void findDelaunay (int dim); +void makehalf (coordT *points, int numpoints, int dim); +void addhalf (coordT *points, int numpoints, int numnew, int dim, coordT *feasible); + +/*------------------------------------------------- +-print_summary() +*/ +void print_summary (void) { + facetT *facet; + int k; + + printf ("\n%d vertices and %d facets with normals:\n", + qh num_vertices, qh num_facets); + FORALLfacets { + for (k=0; k < qh hull_dim; k++) + printf ("%6.2g ", facet->normal[k]); + printf ("\n"); + } +} + +/*-------------------------------------------------- +-makecube- set points to vertices of cube + points is numpoints X dim +*/ +void makecube (coordT *points, int numpoints, int dim) { + int j,k; + coordT *point; + + for (j=0; j<numpoints; j++) { + point= points + j*dim; + for (k=dim; k--; ) { + if (j & ( 1 << k)) + point[k]= 1.0; + else + point[k]= -1.0; + } + } +} /*.makecube.*/ + +/*-------------------------------------------------- +-adddiamond- add diamond to convex hull + points is numpoints+numnew X dim. + +notes: + qh_addpoint() does not make a copy of the point coordinates. + + For inside points and some outside points, qh_findbestfacet performs + an exhaustive search for a visible facet. Algorithms that retain + previously constructed hulls should be faster for on-line construction + of the convex hull. +*/ +void adddiamond (coordT *points, int numpoints, int numnew, int dim) { + int j,k; + coordT *point; + facetT *facet; + boolT isoutside; + realT bestdist; + + for (j= 0; j < numnew ; j++) { + point= points + (numpoints+j)*dim; + if (points == qh first_point) /* in case of 'QRn' */ + qh num_points= numpoints+j+1; + /* qh num_points sets the size of the points array. You may + allocate the points elsewhere. If so, qh_addpoint records + the point's address in qh other_points + */ + for (k=dim; k--; ) { + if (j/2 == k) + point[k]= (j & 1) ? 2.0 : -2.0; + else + point[k]= 0.0; + } + facet= qh_findbestfacet (point, !qh_ALL, &bestdist, &isoutside); + if (isoutside) { + if (!qh_addpoint (point, facet, False)) + break; /* user requested an early exit with 'TVn' or 'TCn' */ + } + printf ("%d vertices and %d facets\n", + qh num_vertices, qh num_facets); + /* qh_produce_output(); */ + } + if (qh DOcheckmax) + qh_check_maxout(); + else if (qh KEEPnearinside) + qh_nearcoplanar(); +} /*.adddiamond.*/ + +/*-------------------------------------------------- +-makeDelaunay- set points for dim-1 Delaunay triangulation of random points + points is numpoints X dim. Each point is projected to a paraboloid. +*/ +void makeDelaunay (coordT *points, int numpoints, int dim) { + int j,k, seed; + coordT *point, realr; + + seed= time(NULL); + printf ("seed: %d\n", seed); + qh_RANDOMseed_( seed); + for (j=0; j<numpoints; j++) { + point= points + j*dim; + for (k= 0; k < dim-1; k++) { + realr= qh_RANDOMint; + point[k]= 2.0 * realr/(qh_RANDOMmax+1) - 1.0; + } + } + qh_setdelaunay (dim, numpoints, points); +} /*.makeDelaunay.*/ + +/*-------------------------------------------------- +-addDelaunay- add points to dim-1 Delaunay triangulation + points is numpoints+numnew X dim. Each point is projected to a paraboloid. +notes: + qh_addpoint() does not make a copy of the point coordinates. + + Since qh_addpoint() is not given a visible facet, it performs a directed + search of all facets. Algorithms that retain previously + constructed hulls may be faster. +*/ +void addDelaunay (coordT *points, int numpoints, int numnew, int dim) { + int j,k; + coordT *point, realr; + facetT *facet; + realT bestdist; + boolT isoutside; + + for (j= 0; j < numnew ; j++) { + point= points + (numpoints+j)*dim; + if (points == qh first_point) /* in case of 'QRn' */ + qh num_points= numpoints+j+1; + /* qh num_points sets the size of the points array. You may + allocate the point elsewhere. If so, qh_addpoint records + the point's address in qh other_points + */ + for (k= 0; k < dim-1; k++) { + realr= qh_RANDOMint; + point[k]= 2.0 * realr/(qh_RANDOMmax+1) - 1.0; + } + qh_setdelaunay (dim, 1, point); + facet= qh_findbestfacet (point, !qh_ALL, &bestdist, &isoutside); + if (isoutside) { + if (!qh_addpoint (point, facet, False)) + break; /* user requested an early exit with 'TVn' or 'TCn' */ + } + qh_printpoint (stdout, "added point", point); + printf ("%d points, %d extra points, %d vertices, and %d facets in total\n", + qh num_points, qh_setsize (qh other_points), + qh num_vertices, qh num_facets); + + /* qh_produce_output(); */ + } + if (qh DOcheckmax) + qh_check_maxout(); + else if (qh KEEPnearinside) + qh_nearcoplanar(); +} /*.addDelaunay.*/ + +/*-------------------------------------------------- +-findDelaunay- find Delaunay triangle for [0.5,0.5,...] + assumes dim < 100 +notes: + calls qh_setdelaunay() to project the point to a parabaloid +warning: + This is not implemented for tricoplanar facets ('Qt'), + See <a href="../html/qh-in.htm#findfacet">locate a facet with qh_findbestfacet()</a> +*/ +void findDelaunay (int dim) { + int k; + coordT point[ 100]; + boolT isoutside; + realT bestdist; + facetT *facet; + vertexT *vertex, **vertexp; + + for (k= 0; k < dim-1; k++) + point[k]= 0.5; + qh_setdelaunay (dim, 1, point); + facet= qh_findbestfacet (point, qh_ALL, &bestdist, &isoutside); + if (facet->tricoplanar) { + fprintf(stderr, "findDelaunay: not implemented for triangulated, non-simplicial Delaunay regions (tricoplanar facet, f%d).\n", + facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + FOREACHvertex_(facet->vertices) { + for (k=0; k < dim-1; k++) + printf ("%5.2f ", vertex->point[k]); + printf ("\n"); + } +} /*.findDelaunay.*/ + +/*-------------------------------------------------- +-makehalf- set points to halfspaces for a (dim)-d diamond + points is numpoints X dim+1 + + each halfspace consists of dim coefficients followed by an offset +*/ +void makehalf (coordT *points, int numpoints, int dim) { + int j,k; + coordT *point; + + for (j=0; j<numpoints; j++) { + point= points + j*(dim+1); + point[dim]= -1.0; /* offset */ + for (k=dim; k--; ) { + if (j & ( 1 << k)) + point[k]= 1.0; + else + point[k]= -1.0; + } + } +} /*.makehalf.*/ + +/*-------------------------------------------------- +-addhalf- add halfspaces for a (dim)-d cube to the intersection + points is numpoints+numnew X dim+1 +notes: + assumes dim < 100. + + For makehalf(), points is the initial set of halfspaces with offsets. + It is transformed by qh_sethalfspace_all into a + (dim)-d set of newpoints. Qhull computed the convex hull of newpoints - + this is equivalent to the halfspace intersection of the + orginal halfspaces. + + For addhalf(), the remainder of points stores the transforms of + the added halfspaces. Qhull computes the convex hull of newpoints + and the added points. qh_addpoint() does not make a copy of these points. + + Since halfspace intersection is equivalent to a convex hull, + qh_findbestfacet may perform an exhaustive search + for a visible facet. Algorithms that retain previously constructed + intersections should be faster for on-line construction. +*/ +void addhalf (coordT *points, int numpoints, int numnew, int dim, coordT *feasible) { + int j,k; + coordT *point, normal[100], offset, *next; + facetT *facet; + boolT isoutside; + realT bestdist; + + for (j= 0; j < numnew ; j++) { + offset= -1.0; + for (k=dim; k--; ) { + if (j/2 == k) { + normal[k]= sqrt (dim); /* to normalize as in makehalf */ + if (j & 1) + normal[k]= -normal[k]; + }else + normal[k]= 0.0; + } + point= points + (numpoints+j)* (dim+1); /* does not use point[dim] */ + qh_sethalfspace (dim, point, &next, normal, &offset, feasible); + facet= qh_findbestfacet (point, !qh_ALL, &bestdist, &isoutside); + if (isoutside) { + if (!qh_addpoint (point, facet, False)) + break; /* user requested an early exit with 'TVn' or 'TCn' */ + } + qh_printpoint (stdout, "added offset -1 and normal", normal); + printf ("%d points, %d extra points, %d vertices, and %d facets in total\n", + qh num_points, qh_setsize (qh other_points), + qh num_vertices, qh num_facets); + /* qh_produce_output(); */ + } + if (qh DOcheckmax) + qh_check_maxout(); + else if (qh KEEPnearinside) + qh_nearcoplanar(); +} /*.addhalf.*/ + +#define DIM 3 /* dimension of points, must be < 31 for SIZEcube */ +#define SIZEcube (1<<DIM) +#define SIZEdiamond (2*DIM) +#define TOTpoints (SIZEcube + SIZEdiamond) + +/*-------------------------------------------------- +-main- derived from call_qhull in user.c + + see program header + + this contains three runs of Qhull for convex hull, Delaunay + triangulation or Voronoi vertices, and halfspace intersection + +*/ +int main (int argc, char *argv[]) { + boolT ismalloc; + int curlong, totlong, exitcode; + char options [2000]; + + printf ("This is the output from user_eg2.c\n\n\ +It shows how qhull() may be called from an application. It is not part\n\ +of qhull itself. If it appears accidently, please remove user_eg2.c from\n\ +your project.\n\n"); + ismalloc= False; /* True if qh_freeqhull should 'free(array)' */ + /* + Run 1: convex hull + */ + qh_init_A (stdin, stdout, stderr, 0, NULL); + exitcode= setjmp (qh errexit); + if (!exitcode) { + coordT array[TOTpoints][DIM]; + + strcat (qh rbox_command, "user_eg cube"); + sprintf (options, "qhull s Tcv Q11 %s ", argc >= 2 ? argv[1] : ""); + qh_initflags (options); + printf( "\ncompute triangulated convex hull of cube after rotating input\n"); + makecube (array[0], SIZEcube, DIM); + qh_init_B (array[0], SIZEcube, DIM, ismalloc); + qh_qhull(); + qh_check_output(); + qh_triangulate(); /* requires option 'Q11' if want to add points */ + print_summary (); + if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points (); + printf( "\nadd points in a diamond\n"); + adddiamond (array[0], SIZEcube, SIZEdiamond, DIM); + qh_check_output(); + print_summary (); + qh_produce_output(); /* delete this line to help avoid io.c */ + if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points (); + } + qh NOerrexit= True; + qh_freeqhull (!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + /* + Run 2: Delaunay triangulation + */ + qh_init_A (stdin, stdout, stderr, 0, NULL); + exitcode= setjmp (qh errexit); + if (!exitcode) { + coordT array[TOTpoints][DIM]; + + strcat (qh rbox_command, "user_eg Delaunay"); + sprintf (options, "qhull s d Tcv %s", argc >= 3 ? argv[2] : ""); + qh_initflags (options); + printf( "\ncompute %d-d Delaunay triangulation\n", DIM-1); + makeDelaunay (array[0], SIZEcube, DIM); + /* Instead of makeDelaunay with qh_setdelaunay, you may + produce a 2-d array of points, set DIM to 2, and set + qh PROJECTdelaunay to True. qh_init_B will call + qh_projectinput to project the points to the paraboloid + and add a point "at-infinity". + */ + qh_init_B (array[0], SIZEcube, DIM, ismalloc); + qh_qhull(); + /* If you want Voronoi ('v') without qh_produce_output(), call + qh_setvoronoi_all() after qh_qhull() */ + qh_check_output(); + print_summary (); + qh_produce_output(); /* delete this line to help avoid io.c */ + if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points (); + printf( "\nadd points to triangulation\n"); + addDelaunay (array[0], SIZEcube, SIZEdiamond, DIM); + qh_check_output(); + qh_produce_output(); /* delete this line to help avoid io.c */ + if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points (); + printf( "\nfind Delaunay triangle closest to [0.5, 0.5, ...]\n"); + findDelaunay (DIM); + } + qh NOerrexit= True; + qh_freeqhull (!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + /* + Run 3: halfspace intersection + */ + qh_init_A (stdin, stdout, stderr, 0, NULL); + exitcode= setjmp (qh errexit); + if (!exitcode) { + coordT array[TOTpoints][DIM+1]; /* +1 for halfspace offset */ + pointT *points; + + strcat (qh rbox_command, "user_eg halfspaces"); + sprintf (options, "qhull H0 s Tcv %s", argc >= 4 ? argv[3] : ""); + qh_initflags (options); + printf( "\ncompute halfspace intersection about the origin for a diamond\n"); + makehalf (array[0], SIZEcube, DIM); + qh_setfeasible (DIM); /* from io.c, sets qh feasible_point from 'Hn,n' */ + /* you may malloc and set qh feasible_point directly. It is only used for + option 'Fp' */ + points= qh_sethalfspace_all ( DIM+1, SIZEcube, array[0], qh feasible_point); + qh_init_B (points, SIZEcube, DIM, True); /* qh_freeqhull frees points */ + qh_qhull(); + qh_check_output(); + qh_produce_output(); /* delete this line to help avoid io.c */ + if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points (); + printf( "\nadd halfspaces for cube to intersection\n"); + addhalf (array[0], SIZEcube, SIZEdiamond, DIM, qh feasible_point); + qh_check_output(); + qh_produce_output(); /* delete this line to help avoid io.c */ + if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points (); + } + qh NOerrexit= True; + qh NOerrexit= True; + qh_freeqhull (!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) /* could also check previous runs */ + fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); + return exitcode; +} /* main */ + +#if 1 /* use 1 to prevent loading of io.o and user.o */ +/*------------------------------------------- +-errexit- return exitcode to system after an error + assumes exitcode non-zero + prints useful information + see qh_errexit2() in qhull.c for 2 facets +*/ +void qh_errexit(int exitcode, facetT *facet, ridgeT *ridge) { + + if (qh ERREXITcalled) { + fprintf (qh ferr, "qhull error while processing previous error. Exit program\n"); + exit(1); + } + qh ERREXITcalled= True; + if (!qh QHULLfinished) + qh hulltime= (unsigned)clock() - qh hulltime; + fprintf (qh ferr, "\nWhile executing: %s | %s\n", qh rbox_command, qh qhull_command); + fprintf(qh ferr, "Options selected:\n%s\n", qh qhull_options); + if (qh furthest_id >= 0) { + fprintf(qh ferr, "\nLast point added to hull was p%d", qh furthest_id); + if (zzval_(Ztotmerge)) + fprintf(qh ferr, " Last merge was #%d.", zzval_(Ztotmerge)); + if (qh QHULLfinished) + fprintf(qh ferr, "\nQhull has finished constructing the hull."); + else if (qh POSTmerging) + fprintf(qh ferr, "\nQhull has started post-merging"); + fprintf(qh ferr, "\n\n"); + } + if (qh NOerrexit) { + fprintf (qh ferr, "qhull error while ending program. Exit program\n"); + exit(1); + } + if (!exitcode) + exitcode= qh_ERRqhull; + qh NOerrexit= True; + longjmp(qh errexit, exitcode); +} /* errexit */ + + +/*------------------------------------------- +-errprint- prints out the information of the erroneous object + any parameter may be NULL, also prints neighbors and geomview output +*/ +void qh_errprint(char *string, facetT *atfacet, facetT *otherfacet, ridgeT *atridge, vertexT *atvertex) { + + fprintf (qh ferr, "%s facets f%d f%d ridge r%d vertex v%d\n", + string, getid_(atfacet), getid_(otherfacet), getid_(atridge), + getid_(atvertex)); +} /* errprint */ + + +void qh_printfacetlist(facetT *facetlist, setT *facets, boolT printall) { + facetT *facet, **facetp; + + /* remove these calls to help avoid io.c */ + qh_printbegin (qh ferr, qh_PRINTfacets, facetlist, facets, printall);/*io.c*/ + FORALLfacet_(facetlist) /*io.c*/ + qh_printafacet(qh ferr, qh_PRINTfacets, facet, printall); /*io.c*/ + FOREACHfacet_(facets) /*io.c*/ + qh_printafacet(qh ferr, qh_PRINTfacets, facet, printall); /*io.c*/ + qh_printend (qh ferr, qh_PRINTfacets, facetlist, facets, printall); /*io.c*/ + + FORALLfacet_(facetlist) + fprintf( qh ferr, "facet f%d\n", facet->id); +} /* printfacetlist */ + + + +/*----------------------------------------- +-user_memsizes- allocate up to 10 additional, quick allocation sizes +*/ +void qh_user_memsizes (void) { + + /* qh_memsize (size); */ +} /* user_memsizes */ + +#endif diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/index.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/index.htm new file mode 100644 index 0000000000000000000000000000000000000000..354579bc0afe2ab7af4b8005ea2e90cb157508fe --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/index.htm @@ -0,0 +1,777 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<meta name="GENERATOR" content="Microsoft FrontPage 2.0"> +<title>Qhull manual</title> +<!-- Navigation links +NOTE -- verify all links by 'grep href=' 'grep name=' add # 'sort /+7' + index.htm +--> +</head> + +<body> + +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b><a +href="http://www.qhull.org/news">News</a> about Qhull<br> +<b>Up:</b> <a href="http://www.qhull.org/html/qh-faq.htm">FAQ</a> about Qhull<br> +<b>To:</b> <a href="#TOC">Qhull manual: Table of Contents</a> +(please wait while loading) <br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/fixed.html"><img +src="qh--rand.gif" alt="[random-fixed]" align="middle" +width="100" height="100"></a> Qhull manual </h1> + +<p>Qhull is a general dimension code for computing convex hulls, +Delaunay triangulations, halfspace intersections about a point, Voronoi +diagrams, furthest-site Delaunay triangulations, and +furthest-site Voronoi diagrams. These structures have +applications in science, engineering, statistics, and +mathematics. See <a +href="http://www.ifor.math.ethz.ch/staff/fukuda/polyfaq/polyfaq.html">Fukuda's +introduction</a> to convex hulls, Delaunay triangulations, +Voronoi diagrams, and linear programming. For a detailed +introduction, see O'Rourke [<a href="#orou94">'94</a>], <i>Computational +Geometry in C</i>. +</p> + +<p>There are six programs. Except for rbox, they use +the same code. +<blockquote> +<ul> +<li><a href="qconvex.htm">qconvex</a> -- convex hulls +<li><a href="qdelaun.htm">qdelaunay</a> -- Delaunay triangulations and + furthest-site Delaunay triangulations +<li><a href="qhalf.htm">qhalf</a> -- halfspace intersections about a point +<li><a href="qhull.htm">qhull</a> -- all structures with additional options +<li><a href="qvoronoi.htm">qvoronoi</a> -- Voronoi diagrams and + furthest-site Voronoi diagrams +<li><a href="rbox.htm">rbox</a> -- generate point distributions for qhull +</ul> +</blockquote> + +<p>Qhull implements the Quickhull algorithm for computing the +convex hull. Qhull includes options +for hull volume, facet area, multiple output formats, and +graphical output. It can approximate a convex hull. </p> + +<p>Qhull handles roundoff errors from floating point +arithmetic. It generates a convex hull with "thick" facets. +A facet's outer plane is clearly above all of the points; +its inner plane is clearly below the facet's vertices. Any +exact convex hull must lie between the inner and outer plane. + +<p>Qhull uses merged facets, triangulated output, or joggled +input. Triangulated output triangulates non-simplicial, merged +facets. Joggled input also +guarantees simplicial output, but it +is less accurate than merged facets. For merged facets, Qhull +reports the maximum outer and inner plane. + +<p><i>Brad Barber, Cambridge MA, 2003/12/30</i></p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h2><a href="#TOP">»</a><a name="TOC">Qhull manual: Table of +Contents </a></h2> + +<ul> + <li><a href="#when">When</a> to use Qhull + <ul> + <li><a href="http://www.qhull.org/news">News</a> for Qhull + with new features and reported bugs. + <li><a href="http://www.qhull.org">Home</a> for Qhull with additional URLs + (<a href=index.htm>local copy</a>) + <li><a href="http://www.qhull.org/html/qh-faq.htm">FAQ</a> for Qhull (<a href="qh-faq.htm">local copy</a>) + <li><a href="http://www.qhull.org/download">Download</a> Qhull (<a href=qh-get.htm>local copy</a>) + <li><a href="qh-quick.htm#programs">Quick</a> reference for Qhull and its <a href="qh-quick.htm#options">options</a> + <p> + <li><a href="../COPYING.txt">COPYING.txt</a> - copyright notice<br> + <li><a href="../REGISTER.txt">REGISTER.txt</a> - registration<br> + <li><a href="../README.txt">README.txt</a> - installation + instructions<br> + <li><a href="../src/Changes.txt">Changes.txt</a> - change history <br> + <li><a href="qhull.txt">qhull.txt</a> - Unix manual page + </ul> + <p> + <li><a href="#description">Description</a> of Qhull + <ul> + <li><a href="#definition">de</a>finition • <a + href="#input">in</a>put • <a href="#output">ou</a>tput + • <a href="#algorithm">al</a>gorithm • <a + href="#structure">da</a>ta structure </li> + <li><a href="qh-impre.htm">Imprecision</a> in Qhull</li> + <LI><a href="qh-impre.htm#joggle">Merged facets</a> or joggled input + <li><a href="#geomview">Geomview</a>, Qhull's graphical + viewer</li> + <li><a href="qh-eg.htm">Examples</a> of Qhull using Geomview</li> + </ul> + <p> + <li><a href=qh-quick.htm#programs>Qhull programs</a> + <ul> + <li><a href="qconvex.htm">qconvex</a> -- convex hulls + <li><a href="qdelaun.htm">qdelaunay</a> -- Delaunay triangulations and + furthest-site Delaunay triangulations + <li><a href="qhalf.htm">qhalf</a> -- halfspace intersections about a point + <li><a href="qhull.htm">qhull</a> -- all structures with additional options + <li><a href="qvoronoi.htm">qvoronoi</a> -- Voronoi diagrams and + furthest-site Voronoi diagrams + <li><a href="rbox.htm">rbox</a> -- generate point distributions for qhull + </ul> + <p> + <li>Related URLs + <ul> + + <li><a href="news:comp.graphics.algorithms">Newsgroup</a>: + comp.graphics.algorithms + <li><a + href="http://exaflop.org/docs/cgafaq/">FAQ</a> for computer graphics algorithms and + <a href="http://exaflop.org/docs/cgafaq/cga6.html">geometric</a> structures. + <li>Amenta's <a href="http://www.geom.uiuc.edu/software/cglist">Directory + of Computational Geometry Software </a></li> + <li>Erickson's <a + href="http://compgeom.cs.uiuc.edu/~jeffe/compgeom/code.html">Computational + Geometry Software</a> </li> + <li>Fukuda's <a + href="http://www.ifor.math.ethz.ch/staff/fukuda/polyfaq/polyfaq.html"> + introduction</a> to convex hulls, Delaunay triangulations, + Voronoi diagrams, and linear programming. + <li>Stony Brook's <a + href="http://www.cs.sunysb.edu/~algorith/major_section/1.6.shtml">Algorithm Repository</a> on computational geometry. + </li> + </ul> + <p> + <li><a href="qh-quick.htm#options">Qhull options</a><ul> + <li><a href="qh-opto.htm#output">Output</a> formats</li> + <li><a href="qh-optf.htm#format">Additional</a> I/O + formats</li> + <li><a href="qh-optg.htm#geomview">Geomview</a> + output options</li> + <li><a href="qh-optp.htm#print">Print</a> options</li> + <li><a href="qh-optq.htm#qhull">Qhull</a> control + options</li> + <li><a href="qh-optc.htm#prec">Precision</a> options</li> + <li><a href="qh-optt.htm#trace">Trace</a> options</li> + </ul> + </li> + <p> + <li><a href="qh-in.htm">Qhull internals</a><ul> + <li><a href="qh-in.htm#performance">Performance</a> + of Qhull</li> + <li><a href="qh-in.htm#library">Calling</a> Qhull + from your program</li> + <li><a href="qh-in.htm#enhance">Enhancements</a> to + Qhull</li> + <li><a href="../src/index.htm">Qhull functions, macros, and + data structures</a> </li> + </ul> + </li> + <p> + <li><a href="#bugs">What to do</a> if something goes wrong</li> + <li><a href="#email">Email</a></li> + <li><a href="#authors">Authors</a></li> + <li><a href="#ref">References</a></li> + <li><a href="#acknowledge">Acknowledgments</a></li> +</ul> +<h2><a href="#TOC">»</a><a name="when">When to use Qhull</a></h2> +<blockquote> + +<p>Qhull constructs convex hulls, Delaunay triangulations, +halfspace intersections about a point, Voronoi diagrams, furthest-site Delaunay +triangulations, and furthest-site Voronoi diagrams.</p> + +<p>For convex hulls and halfspace intersections, Qhull may be used +for 2-d upto 8-d. For Voronoi diagrams and Delaunay triangulations, Qhull may be +used for 2-d upto 7-d. In higher dimensions, the size of the output +grows rapidly and Qhull does not work well with virtual memory. +If <i>n</i> is the size of +the input and <i>d</i> is the dimension (d>=3), the size of the output +and execution time +grows by <i>n^(floor(d/2)</i> +[see <a href=qh-in.htm#performance>Performance</a>]. For example, do +not try to build a 16-d convex hull of 1000 points. It will +have on the order of 1,000,000,000,000,000,000,000,000 facets. + +<p>On a 600 MHz Pentium 3, Qhull computes the 2-d convex hull of +300,000 cocircular points in 11 seconds. It computes the +2-d Delaunay triangulation and 3-d convex hull of 120,000 points +in 12 seconds. It computes the +3-d Delaunay triangulation and 4-d convex hull of 40,000 points +in 18 seconds. It computes the +4-d Delaunay triangulation and 5-d convex hull of 6,000 points +in 12 seconds. It computes the +5-d Delaunay triangulation and 6-d convex hull of 1,000 points +in 12 seconds. It computes the +6-d Delaunay triangulation and 7-d convex hull of 300 points +in 15 seconds. It computes the +7-d Delaunay triangulation and 8-d convex hull of 120 points +in 15 seconds. It computes the +8-d Delaunay triangulation and 9-d convex hull of 70 points +in 15 seconds. It computes the +9-d Delaunay triangulation and 10-d convex hull of 50 points +in 17 seconds. The 10-d convex hull of 50 points has about 90,000 facets. + +<!-- duplicated in index.htm and html/index.htm --> +<p>Qhull does <i>not</i> support constrained Delaunay +triangulations, triangulation of non-convex surfaces, mesh +generation of non-convex objects, or medium-sized inputs in 9-D +and higher. </p> + +<p>This is a big package with many options. It is one of the +fastest available. It is the only 3-d code that handles precision +problems due to floating point arithmetic. For example, it +implements the identity function for extreme points (see <a +href="qh-impre.htm">Imprecision in Qhull</a>). </p> + +<p>If you need a short code for convex hull, Delaunay +triangulation, or Voronoi volumes consider Clarkson's <a +href="http://netlib.bell-labs.com/netlib/voronoi/hull.html">hull +program</a>. If you need 2-d Delaunay triangulations consider +Shewchuk's <a href="http://www.cs.cmu.edu/~quake/triangle.html">triangle +program</a>. It is much faster than Qhull and it allows +constraints. Both programs use exact arithmetic. They are in <a +href="ftp://netlib.bell-labs.com/netlib/voronoi">ftp://netlib.bell-labs.com/netlib/voronoi</a>. +Qhull <a +href="http://www.qhull.org/download">version +1.0</a> may also meet your needs. It detects precision problems, +but does not handle them.</p> + +<p><a href=http://www.algorithmic-solutions.com/enleda.htm>Leda</a> is a +library for writing computational +geometry programs and other combinatorial algorithms. It +includes routines for computing 3-d convex +hulls, 2-d Delaunay triangulations, and 3-d Delaunay triangulations. +It provides rational arithmetic and graphical output. It runs on most +platforms. + +<p>If your problem is in high dimensions with a few, +non-simplicial facets, try Fukuda's <a +href="http://www.cs.mcgill.ca/~fukuda/soft/cdd_home/cdd.html">cdd</a>. +It is much faster than Qhull for these distributions. </p> + +<p>Custom software for 2-d and 3-d convex hulls may be faster +than Qhull. Custom software should use less memory. Qhull uses +general-dimension data structures and code. The data structures +support non-simplicial facets.</p> + +<p>Qhull is not suitable for mesh generation or triangulation of +arbitrary surfaces. You may use Qhull if the surface is convex or +completely visible from an interior point (e.g., a star-shaped +polyhedron). First, project each site to a sphere that is +centered at the interior point. Then, compute the convex hull of +the projected sites. The facets of the convex hull correspond to +a triangulation of the surface. For mesh generation of arbitrary +surfaces, see <a +href="http://www-users.informatik.rwth-aachen.de/~roberts/meshgeneration.html">Schneiders' +Finite Element Mesh Generation</a>.</p> + +<p>Qhull is not suitable for constrained Delaunay triangulations. +With a lot of work, you can write a program that uses Qhull to +add constraints by adding additional points to the triangulation.</p> + +<p>Qhull is not suitable for the subdivision of arbitrary +objects. Use <tt>qdelaunay</tt> to subdivide a convex object.</p> + +</blockquote> +<h2><a href="#TOC">»</a><a name="description">Description of +Qhull </a></h2> +<blockquote> + +<h3><a href="#TOC">»</a><a name="definition">definition</a></h3> +<blockquote> + +<p>The <i>convex hull</i> of a point set <i>P</i> is the smallest +convex set that contains <i>P</i>. If <i>P</i> is finite, the +convex hull defines a matrix <i>A</i> and a vector <i>b</i> such +that for all <i>x</i> in <i>P</i>, <i>Ax+b <= [0,...]</i>. </p> + +<p>Qhull computes the convex hull in 2-d, 3-d, 4-d, and higher +dimensions. Qhull represents a convex hull as a list of facets. +Each facet has a set of vertices, a set of neighboring facets, +and a halfspace. A halfspace is defined by a unit normal and an +offset (i.e., a row of <i>A</i> and an element of <i>b</i>). </p> + +<p>Qhull accounts for round-off error. It returns +"thick" facets defined by two parallel hyperplanes. The +outer planes contain all input points. The inner planes exclude +all output vertices. See <a href="qh-impre.htm#imprecise">Imprecise +convex hulls</a>.</p> + +<p>Qhull may be used for the Delaunay triangulation or the +Voronoi diagram of a set of points. It may be used for the +intersection of halfspaces. </p> + +</blockquote> +<h3><a href="#TOC">»</a><a name="input">input format</a></h3> +<blockquote> + +<p>The input data on <tt>stdin</tt> consists of:</p> + +<ul> + <li>first line contains the dimension</li> + <li>second line contains the number of input points</li> + <li>remaining lines contain point coordinates</li> +</ul> + +<p>For example: </p> + +<pre> + 3 #sample 3-d input + 5 + 0.4 -0.5 1.0 + 1000 -1e-5 -100 + 0.3 0.2 0.1 + 1.0 1.0 1.0 + 0 0 0 +</pre> + +<p>Input may be entered by hand. End the input with a control-D +(^D) character. </p> + +<p>To input data from a file, use I/O redirection or '<a +href="qh-optt.htm#TI">TI file</a>'. The filename may not +include spaces or quotes.</p> + +<p>A comment starts with a non-numeric character and continues to +the end of line. The first comment is reported in summaries and +statistics. With multiple <tt>qhull</tt> commands, use option '<a +href="qh-optf.htm#FQ">FQ</a>' to place a comment in the output.</p> + +<p>The dimension and number of points can be reversed. Comments +and line breaks are ignored. Error reporting is better if there +is one point per line.</p> + +</blockquote> +<h3><a href="#TOC">»</a><a name="option">option format</a></h3> +<blockquote> + +<p>Use options to specify the output formats and control +Qhull. The <tt>qhull</tt> program takes all options. The +other programs use a subset of the options. They disallow +experimental and inappropriate options. + +<blockquote> +<ul> +<li> +qconvex == qhull +<li> +qdelaunay == qhull d Qbb +<li> +qhalf == qhull H +<li> +qvoronoi == qhull v Qbb +</ul> +</blockquote> + +<p>Single letters are used for output formats and precision +constants. The other options are grouped into menus for formats +('<a href="qh-optf.htm#format">F</a>'), Geomview ('<a +href="qh-optg.htm#geomview">G </a>'), printing ('<a +href="qh-optp.htm#print">P</a>'), Qhull control ('<a +href="qh-optq.htm#qhull">Q </a>'), and tracing ('<a +href="qh-optt.htm#trace">T</a>'). The menu options may be listed +together (e.g., 'GrD3' for 'Gr' and 'GD3'). Options may be in any +order. Capitalized options take a numeric argument (except for '<a +href="qh-optp.htm#PG">PG</a>' and '<a href="qh-optf.htm#format">F</a>' +options). Use option '<a href="qh-optf.htm#FO">FO</a>' to print +the selected options.</p> + +<p>Qhull uses zero-relative indexing. If there are <i>n</i> +points, the index of the first point is <i>0</i> and the index of +the last point is <i>n-1</i>.</p> + +<p>The default options are:</p> + +<ul> + <li>summary output ('<a href="qh-opto.htm#s">s</a>') </li> + <li>merged facets ('<a href="qh-optc.htm#C0">C-0</a>' in 2-d, + 3-d, 4-d; '<a href="qh-optq.htm#Qx">Qx</a>' in 5-d and + up)</li> +</ul> + +<p>Except for bounding box +('<a href="qh-optq.htm#Qbk">Qbk:n</a>', etc.), drop facets +('<a href="qh-optp.htm#Pdk">Pdk:n</a>', etc.), and +Qhull command ('<a href="qh-optf.htm#FQ">FQ</a>'), only the last +occurence of an option counts. +Bounding box and drop facets may be repeated for each dimension. +Option 'FQ' may be repeated any number of times. + +<p>The Unix <tt>tcsh</tt> and <tt>ksh </tt>shells make it easy to +try out different options. In Windows 95, use a DOS window with <tt>doskey</tt> +and a window scroller (e.g., <tt>peruse</tt>). </p> + +</blockquote> +<h3><a href="#TOC">»</a><a name="output">output format</a></h3> +<blockquote> + +<p>To write the results to a file, use I/O redirection or '<a +href="qh-optt.htm#TO">TO file</a>'. Windows 95 users should use +'TO file' or the console. If a filename is surrounded by single quotes, +it may include spaces. +</p> + +<p>The default output option is a short summary ('<a +href="qh-opto.htm#s">s</a>') to <tt>stdout</tt>. There are many +others (see <a href="qh-opto.htm">output</a> and <a +href="qh-optf.htm">formats</a>). You can list vertex incidences, +vertices and facets, vertex coordinates, or facet normals. You +can view Qhull objects with Geomview, Mathematica, or Maple. You can +print the internal data structures. You can call Qhull from your +application (see <a href="qh-in.htm#library">Qhull library</a>).</p> + +<p>For example, 'qhull <a href="qh-opto.htm#o">o</a>' lists the +vertices and facets of the convex hull. </p> + +<p>Error messages and additional summaries ('<a +href="qh-opto.htm#s">s</a>') go to <tt>stderr</tt>. Unless +redirected, <tt>stderr</tt> is the console.</p> + +</blockquote> +<h3><a href="#TOC">»</a><a name="algorithm">algorithm</a></h3> +<blockquote> + +<p>Qhull implements the Quickhull algorithm for convex hull +[Barber et al. <a href="#bar-dob96">'96</a>]. This algorithm +combines the 2-d Quickhull algorithm with the <em>n</em>-d +beneath-beyond algorithm [c.f., Preparata & Shamos <a +href="#pre-sha85">'85</a>]. It is similar to the randomized +algorithms of Clarkson and others [Clarkson & Shor <a +href="#cla-sho89">'89</a>; Clarkson et al. <a href="#cla-meh93">'93</a>; +Mulmuley <a href="#mulm94">'94</a>]. For a demonstration, see <a +href="qh-eg.htm#how">How Qhull adds a point</a>. The main +advantages of Quickhull are output sensitive performance (in +terms of the number of extreme points), reduced space +requirements, and floating-point error handling. </p> + +</blockquote> +<h3><a href="#TOC">»</a><a name="structure">data structures</a></h3> +<blockquote> + +<p>Qhull produces the following data structures for dimension <i>d</i>: +</p> + +<ul> + <li>A <em>coordinate</em> is a real number in floating point + format. </li> + <li>A <em>point</em> is an array of <i>d</i> coordinates. + With option '<a href="qh-optq.htm#QJn">QJ</a>', the + coordinates are joggled by a small amount. </li> + <li>A <em>vertex</em> is an input point. </li> + <li>A <em>hyperplane</em> is <i>d</i> normal coefficients and + an offset. The length of the normal is one. The + hyperplane defines a halfspace. If <i>V</i> is a normal, <i>b</i> + is an offset, and <i>x</i> is a point inside the convex + hull, then <i>Vx+b <0</i>.</li> + <li>An <em>outer plane</em> is a positive + offset from a hyperplane. When Qhull is done, all points + will be below all outer planes.</li> + <li>An <em>inner plane</em> is a negative + offset from a hyperplane. When Qhull is done, all + vertices will be above the corresponding inner planes.</li> + <li>An <em>orientation</em> is either 'top' or 'bottom'. It is the + topological equivalent of a hyperplane's geometric + orientation. </li> + <li>A <em>simplicial facet</em> is a set of + <i>d</i> neighboring facets, a set of <i>d</i> vertices, a + hyperplane equation, an inner plane, an outer plane, and + an orientation. For example in 3-d, a simplicial facet is + a triangle. </li> + <li>A <em>centrum</em> is a point on a facet's hyperplane. A + centrum is the average of a facet's vertices. Neighboring + facets are <em>convex</em> if each centrum is below the + neighbor facet's hyperplane. </li> + <li>A <em>ridge</em> is a set of <i>d-1</i> vertices, two + neighboring facets, and an orientation. For example in + 3-d, a ridge is a line segment. </li> + <li>A <em>non-simplicial facet</em> is a set of ridges, a + hyperplane equation, a centrum, an outer plane, and an + inner plane. The ridges determine a set of neighboring + facets, a set of vertices, and an orientation. Qhull + produces a non-simplicial facet when it merges two facets + together. For example, a cube has six non-simplicial + facets. </li> +</ul> + +<p>For examples, use option '<a href="qh-opto.htm#f">f</a>'. See <a +href="../src/qh-poly.htm">polyhedron operations</a> for further +design documentation. </p> + +</blockquote> +<h3><a href="#TOC">»</a>Imprecision in Qhull</h3> +<blockquote> + +<p>See <a href="qh-impre.htm">Imprecision in Qhull</a>.</p> + +</blockquote> +<h3><a href="#TOC">»</a><a name="geomview">Geomview, Qhull's +graphical viewer</a></h3> +<blockquote> + +<p><a href="http://www.geomview.org">Geomview</a> +is an interactive geometry viewing program for Linux, SGI workstations, +Sun workstations, AIX workstations, NeXT workstations, and X-windows. +It is an +<a href=http://sourceforge.net/projects/geomview>open source project</a> +under SourceForge. +Besides a 3-d viewer, it includes a 4-d viewer, an n-d viewer and +many features for viewing mathematical objects. You may need to +ftp <tt>ndview</tt> from the <tt>newpieces</tt> directory. </p> + +</blockquote> +<h3><a href="#TOC">»</a>Description of Qhull examples</h3> +<blockquote> + +<p>See <a href="qh-eg.htm">Examples</a>. Some of the examples +have <a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/welcome.html">pictures +</a>.</p> + +</blockquote> +</blockquote> +<h2><a href="#TOC">»</a>Options for using Qhull </h2> +<blockquote> + +<p>See <a href="qh-quick.htm#options">Options</a>.</p> + +</blockquote> +<h2><a href="#TOC">»</a>Qhull internals </h2> +<blockquote> + +<p>See <a href="qh-in.htm">Internals</a>.</p> + +</blockquote> +<h2><a href="#TOC">»</a><a name="bugs">What to do if something +goes wrong</a></h2> +<blockquote> + +<p>Please report bugs to <a href=mailto:qhull_bug@qhull.org>qhull_bug@qhull.org</a> +</a>. Please report if Qhull crashes. Please report if Qhull +generates an "internal error". Please report if Qhull +produces a poor approximate hull in 2-d, 3-d or 4-d. Please +report documentation errors. Please report missing or incorrect +links.</p> + +<p>If you do not understand something, try a small example. The <a +href="rbox.htm">rbox</a> program is an easy way to generate +test cases. The <a href="#geomview">Geomview</a> program helps to +visualize the output from Qhull.</p> + +<p>If Qhull does not compile, it is due to an incompatibility +between your system and ours. The first thing to check is that +your compiler is ANSI standard. Qhull produces a compiler error +if __STDC__ is not defined. You may need to set a flag (e.g., +'-A' or '-ansi').</p> + +<p>If Qhull compiles but crashes on the test case (rbox D4), +there's still incompatibility between your system and ours. +Sometimes it is due to memory management. This can be turned off +with qh_NOmem in mem.h. Please let us know if you figure out how +to fix these problems. </p> + +<p>If you doubt the output from Qhull, add option '<a +href="qh-optt.htm#Tv">Tv</a>'. It checks that every point is +inside the outer planes of the convex hull. It checks that every +facet is convex with its neighbors. It checks the topology of the +convex hull.</p> + +<p>Qhull should work on all inputs. It may report precision +errors if you turn off merged facets with option '<a +href="qh-optq.htm#Q0">Q0</a>'. This can get as bad as facets with +flipped orientation or two facets with the same vertices. You'll +get a long help message if you run into such a case. They are +easy to generate with <tt>rbox</tt>.</p> + +<p>If you do find a problem, try to simplify it before reporting +the error. Try different size inputs to locate the smallest one +that causes an error. You're welcome to hunt through the code +using the execution trace ('<a href="qh-optt.htm#Tn">T4</a>') as +a guide. This is especially true if you're incorporating Qhull +into your own program. </p> + +<p>When you report an error, please attach a data set to the end +of your message. Include the options that you used with Qhull, +the results of option '<a href="qh-optf.htm#FO">FO</a>', and any +messages generated by Qhull. This allows me to see the error for +myself. Qhull is maintained part-time. </p> + +</blockquote> +<h2><a href="#TOC">»</a><a name="email">Email</a></h2> +<blockquote> + +<p>Please send correspondence to Brad Barber at <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +and report bugs to <a href=mailto:qhull_bug@qhull.org>qhull_bug@qhull.org</a> +</a>. Let me know how you use Qhull. If you mention it in a +paper, please send a reference and abstract.</p> + +<p>If you would like to get Qhull announcements (e.g., a new +version) and news (any bugs that get fixed, etc.), let us know +and we will add you to our mailing list. If you would like to +communicate with other Qhull users, I will add you to the +qhull_users alias. For Internet news about geometric algorithms +and convex hulls, look at comp.graphics.algorithms and +sci.math.num-analysis. For Qhull news look at <a +href="http://www.qhull.org/news">qhull-news.html</a>.</p> + +</blockquote> +<h2><a href="#TOC">»</a><a name="authors">Authors</a></h2> +<blockquote> + +<pre> C. Bradford Barber Hannu Huhdanpaa + bradb@qhull.org hannu@qhull.org + + c/o The Geometry Center + University of Minnesota + 400 Lind Hall + 207 Church Street S.E. + Minneapolis, MN 55455 +</pre> + +</blockquote> +<h2><a href="#TOC">»</a><a name="acknowledge">Acknowledgments</a></h2> +<blockquote> + +<p>A special thanks to David Dobkin for his guidance. A special +thanks to Albert Marden, Victor Milenkovic, the Geometry Center, +and Harvard University for supporting this work.</p> + +<p>A special thanks to Mark Phillips, Robert Miner, and Stuart Levy for running the Geometry + Center web site long after the Geometry Center closed. + Stuart moved the web site to the University of Illinois at Champaign-Urbana. +Mark and Robert are founders of <a href=http://www.geomtech.com>Geometry Technologies</a>. +Mark, Stuart, and Tamara Munzner are the original authors of <a href=http://www.geomview.org>Geomview</a>. + +<p>A special thanks to <a href="http://www.endocardial.com/">Endocardial +Solutions, Inc.</a> of St. Paul, Minnesota for their support of the +internal documentation (<a href=../src/index.htm>src/index.htm</a>). They use Qhull to build 3-d models of +heart chambers.</p> + +<p>Qhull 1.0 was developed under National Science Foundation +grants NSF/DMS-8920161 and NSF-CCR-91-15793 750-7504. If you find +it useful, please let us know.</p> + +<p>The Geometry Center was supported by grant DMS-8920161 from the +National Science Foundation, by grant DOE/DE-FG02-92ER25137 from +the Department of Energy, by the University of Minnesota, and by +Minnesota Technology, Inc.</p> + +</blockquote> +<h2><a href="#TOC">»</a><a name="ref">References</a></h2> +<blockquote> + +<p><a name="aure91">Aurenhammer</a>, F., "Voronoi diagrams +-- A survey of a fundamental geometric data structure," <i>ACM +Computing Surveys</i>, 1991, 23:345-405. </p> + +<p><a name="bar-dob96">Barber</a>, C. B., D.P. Dobkin, and H.T. +Huhdanpaa, "The Quickhull Algorithm for Convex Hulls," <i>ACM +Transactions on Mathematical Software</i>, Vol. 22, No. 4 (Dec. +1996), p. 469-483 [<a +href="http://www.acm.org/pubs/citations/journals/toms/1996-22-4/p469-barber/">http://www.acm.org</a>; +<a href="http://citeseer.nj.nec.com/83502.html">http://citeseer.nj.nec.com</a>]. +</p> + +<p><a name="cla-sho89">Clarkson</a>, K.L. and P.W. Shor, +"Applications of random sampling in computational geometry, +II", <i>Discrete Computational Geometry</i>, 4:387-421, 1989</p> + +<p><a name="cla-meh93">Clarkson</a>, K.L., K. Mehlhorn, and R. +Seidel, "Four results on randomized incremental +construction," <em>Computational Geometry: Theory and +Applications</em>, vol. 3, p. 185-211, 1993.</p> + +<p><a name="devi01">Devillers</a>, et. al., +"Walking in a triangulation," <i>ACM Symposium on +Computational Geometry</i>, June 3-5,2001, Medford MA. + +<p><a name="dob-kir90">Dobkin</a>, D.P. and D.G. Kirkpatrick, +"Determining the separation of preprocessed polyhedra--a +unified approach," in <i>Proc. 17th Inter. Colloq. Automata +Lang. Program.</i>, in <i>Lecture Notes in Computer Science</i>, +Springer-Verlag, 443:400-413, 1990. </p> + +<p><a name="edel01">Edelsbrunner</a>, H, <i>Geometry and Topology for Mesh Generation</i>, +Cambridge University Press, 2001. + +<p><a name=gart99>Gartner, B.</a>, "Fast and robust smallest enclosing balls", <i>Algorithms - ESA '99</i>, LNCS 1643. + +<p><a name="fort93">Fortune, S.</a>, "Computational +geometry," in R. Martin, editor, <i>Directions in Geometric +Computation</i>, Information Geometers, 47 Stockers Avenue, +Winchester, SO22 5LB, UK, ISBN 1-874728-02-X, 1993.</p> + +<p><a name="mile93">Milenkovic, V.</a>, "Robust polygon +modeling," Computer-Aided Design, vol. 25, p. 546-566, +September 1993. </p> + +<p><a name="muck96">Mucke</a>, E.P., I. Saias, B. Zhu, <i>Fast +randomized point location without preprocessing in Two- and +Three-dimensional Delaunay Triangulations</i>, ACM Symposium on +Computational Geometry, p. 274-283, 1996 [<a +href="http://www.geom.uiuc.edu/software/cglist/GeomDir/">GeomDir</a>]. +</p> + +<p><a name="mulm94">Mulmuley</a>, K., <i>Computational Geometry, +An Introduction Through Randomized Algorithms</i>, Prentice-Hall, +NJ, 1994.</p> + +<p><a name="orou94">O'Rourke</a>, J., <i>Computational Geometry +in C</i>, Cambridge University Press, 1994.</p> + +<p><a name="pre-sha85">Preparata</a>, F. and M. Shamos, <i>Computational +Geometry</i>, Springer-Verlag, New York, 1985.</p> + +</blockquote> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b><a +href="http://www.qhull.org/news">News</a> about Qhull<br> +<b>Up:</b> <a href="http://www.qhull.org/html/qh-faq.htm">FAQ</a> about Qhull<br> +<b>To:</b> <a href="#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>Dn:</b> <a href="qh-impre.htm">Imprecision in Qhull</a><br> +<b>Dn:</b> <a href="qh-eg.htm">Description of Qhull examples</a><br> +<b>Dn:</b> <a href="qh-in.htm">Qhull internals</a><br> +<b>Dn:</b> <a href="../src/index.htm">Qhull functions, macros, and data +structures</a> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qconvex.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qconvex.htm new file mode 100644 index 0000000000000000000000000000000000000000..9811cab40cfdb4cc82f44c334fa88f12b69d8c62 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qconvex.htm @@ -0,0 +1,628 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>qconvex -- convex hull</title> +</head> + +<body> +<!-- Navigation links --> +<a name="TOP"><b>Up</b></a><b>:</b> +<a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a> -- Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/cone.html"><img +src="qh--cone.gif" alt="[cone]" align="middle" width="100" +height="100"></a>qconvex -- convex hull</h1> + +<p>The convex hull of a set of points is the smallest convex set +containing the points. See the detailed introduction by O'Rourke +[<a href="index.htm#orou94">'94</a>]. See <a +href="index.htm#description">Description of Qhull</a> and <a +href="qh-eg.htm#how">How Qhull adds a point</a>.</p> + +<blockquote> +<dl> + <dt><b>Example:</b> rbox 10 D3 | qconvex <a + href="qh-opto.htm#s">s</a> <a href="qh-opto.htm#o">o</a> <a + href="qh-optt.htm#TO">TO result</a></dt> + <dd>Compute the 3-d convex hull of 10 random points. Write a + summary to the console and the points and facets to + 'result'.</dd> + + <dt> </dt> + <dt><b>Example:</b> rbox c | qconvex <a + href="qh-opto.htm#n">n</a></dt> + <dd>Print the normals for each facet of a cube.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox c | qconvex <a + href="qh-opto.htm#i">i</a> <a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>Print the triangulated facets of a cube.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox y 500 W0 | qconvex</dt> + <dd>Compute the convex hull of a simplex with 500 + points on its surface.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox x W1e-12 1000 | qconvex + <a href="qh-optq.htm#QR">QR0</a></dt> + <dd>Compute the convex hull of 1000 points near the + surface of a randomly rotated simplex. Report + the maximum thickness of a facet.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox 1000 s | qconvex <a + href="qh-opto.htm#s">s</a> <a + href="qh-optf.htm#FA">FA</a> </dt> + <dd>Compute the convex hull of 1000 cospherical + points. Verify the results and print a summary + with the total area and volume.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox d D12 | qconvex <a + href="qh-optq.htm#QRn">QR0</a> <a + href="qh-optf.htm#FA">FA</a></dt> + <dd>Compute the convex hull of a 12-d diamond. + Randomly rotate the input. Note the large number + of facets and the small volume.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox c D7 | qconvex <a + href="qh-optf.htm#FA">FA</a> <a + href="qh-optt.htm#TFn">TF1000</a></dt> + <dd>Compute the convex hull of the 7-d hypercube. + Report on progress every 1000 facets. Computing + the convex hull of the 9-d hypercube takes too + much time and space. </dd> + <dt> </dt> + <dt><b>Example:</b> rbox c d D2 | qconvex <a + href="qh-optq.htm#Qc">Qc</a> <a + href="qh-opto.htm#s">s</a> <a + href="qh-opto.htm#f">f</a> <a + href="qh-optf.htm#Fx">Fx</a> | more</dt> + <dd>Dump all fields of all facets for a square and a + diamond. Also print a summary and a list of + vertices. Note the coplanar points.</dd> + <dt> </dt> +</dl> +</blockquote> + +<p>Except for rbox, all of the qhull programs compute a convex hull. + +<p>By default, Qhull merges coplanar facets. For example, the convex +hull of a cube's vertices has six facets. + +<p>If you use '<a href="qh-optq.htm#Qt">Qt</a>' (triangulated output), +all facets will be simplicial (e.g., triangles in 2-d). For the cube +example, it will have 12 facets. Some facets may be +degenerate and have zero area. + +<p>If you use '<a href="qh-optq.htm#QJn">QJ</a>' (joggled input), +all facets will be simplicial. The corresponding vertices will be +slightly perturbed and identical points will be joggled apart. +Joggled input is less accurate that triangulated +output.See <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>The output for 4-d convex hulls may be confusing if the convex +hull contains non-simplicial facets (e.g., a hypercube). See +<a href=qh-faq.htm#extra>Why +are there extra points in a 4-d or higher convex hull?</a><br> +</p> +</p> + +<p>The 'qconvex' program is equivalent to +'<a href=qhull.htm#outputs>qhull</a>' in 2-d to 4-d, and +'<a href=qhull.htm#outputs>qhull</a> <a href=qh-optq.htm#Qx>Qx</a>' +in 5-d and higher. It disables the following Qhull +<a href=qh-quick.htm#options>options</a>: <i>d v H Qbb Qf Qg Qm +Qr Qu Qv Qx Qz TR E V Fp Gt Q0,etc</i>. + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h3><a href="#TOP">»</a><a name="synopsis">qconvex synopsis</a></h3> +<pre> +qconvex- compute the convex hull. + input (stdin): dimension, number of points, point coordinates + comments start with a non-numeric character + +options (qconvex.htm): + Qt - triangulated output + QJ - joggle input instead of merging facets + Tv - verify result: structure, convexity, and point inclusion + . - concise list of all options + - - one-line description of all options + +output options (subset): + s - summary of results (default) + i - vertices incident to each facet + n - normals with offsets + p - vertex coordinates (includes coplanar points if 'Qc') + Fx - extreme points (convex hull vertices) + FA - compute total area and volume + o - OFF format (dim, n, points, facets) + G - Geomview output (2-d, 3-d, and 4-d) + m - Mathematica output (2-d and 3-d) + QVn - print facets that include point n, -n if not + TO file- output results to file, may be enclosed in single quotes + +examples: + rbox c D2 | qconvex s n rbox c D2 | qconvex i + rbox c D2 | qconvex o rbox 1000 s | qconvex s Tv FA + rbox c d D2 | qconvex s Qc Fx rbox y 1000 W0 | qconvex s n + rbox y 1000 W0 | qconvex s QJ rbox d G1 D12 | qconvex QR0 FA Pp + rbox c D7 | qconvex FA TF1000 +</pre> + +<h3><a href="#TOP">»</a><a name="input">qconvex +input</a></h3> +<blockquote> + +<p>The input data on <tt>stdin</tt> consists of:</p> +<ul> + <li>dimension + <li>number of points</li> + <li>point coordinates</li> +</ul> + +<p>Use I/O redirection (e.g., qconvex < data.txt), a pipe (e.g., rbox 10 | qconvex), +or the '<a href=qh-optt.htm#TI>TI</a>' option (e.g., qconvex TI data.txt). + +<p>Comments start with a non-numeric character. Error reporting is +simpler if there is one point per line. Dimension +and number of points may be reversed. + +<p>Here is the input for computing the convex +hull of the unit cube. The output is the normals, one +per facet.</p> + +<blockquote> + <p>rbox c > data </p> + <pre> +3 RBOX c +8 + -0.5 -0.5 -0.5 + -0.5 -0.5 0.5 + -0.5 0.5 -0.5 + -0.5 0.5 0.5 + 0.5 -0.5 -0.5 + 0.5 -0.5 0.5 + 0.5 0.5 -0.5 + 0.5 0.5 0.5 +</pre> + <p>qconvex s n < data</p> + <pre> + +Convex hull of 8 points in 3-d: + + Number of vertices: 8 + Number of facets: 6 + Number of non-simplicial facets: 6 + +Statistics for: RBOX c | QCONVEX s n + + Number of points processed: 8 + Number of hyperplanes created: 11 + Number of distance tests for qhull: 35 + Number of merged facets: 6 + Number of distance tests for merging: 84 + CPU seconds to compute hull (after input): 0.081 + +4 +6 + 0 0 -1 -0.5 + 0 -1 0 -0.5 + 1 0 0 -0.5 + -1 0 0 -0.5 + 0 1 0 -0.5 + 0 0 1 -0.5 +</pre> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="outputs">qconvex outputs</a></h3> +<blockquote> + +<p>These options control the output of qconvex. They may be used +individually or together.</p> +<blockquote> +<dl compact> + <dt> </dt> + <dd><b>Vertices</b></dd> + <dt><a href="qh-optf.htm#Fx">Fx</a></dt> + <dd>list extreme points (i.e., vertices). The first line is the number of + extreme points. Each point is listed, one per line. The cube example + has eight vertices.</dd> + <dt><a href="qh-optf.htm#Fv">Fv</a></dt> + <dd>list vertices for each facet. The first line is the number of facets. + Each remaining line starts with the number of vertices. For the cube example, + each facet has four vertices.</dd> + <dt><a href="qh-opto.htm#i">i</a></dt> + <dd>list vertices for each facet. The first line is the number of facets. The + remaining lines list the vertices for each facet. In 3-d and + higher, report cospherical sites by adding extra points.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Coordinates</b></dd> + <dt><a href="qh-opto.htm#o">o</a></dt> + <dd>print vertices and facets of the convex hull in OFF format. The + first line is the dimension. The second line is the number of + vertices, facets, and ridges. The vertex + coordinates are next, followed by the facets. Each facet starts with + the number of vertices. The cube example has four vertices per facet.</dd> + <dt><a href="qh-optf.htm#Ft">Ft</a></dt> + <dd>print a triangulation of the convex hull in OFF format. The first line + is the dimension. The second line is the number of vertices and added points, + followed by the number of facets and the number of ridges. + The vertex coordinates are next, followed by the centrum coordinates. There is + one centrum for each non-simplicial facet. + The cube example has six centrums, one per square. + Each facet starts with the number of vertices or centrums. + In the cube example, each facet uses two vertices and one centrum.</dd> + <dt><a href="qh-opto.htm#p">p</a></dt> + <dd>print vertex coordinates. The first line is the dimension and the second + line is the number of vertices. The following lines are the coordinates of each + vertex. The cube example has eight vertices.</dd> + <dt><a href="qh-optq.htm#Qc">Qc</a> <a href="qh-opto.htm#p">p</a></dt> + <dd>print coordinates of vertices and coplanar points. The first line is the dimension. + The second line is the number of vertices and coplanar points. The coordinates + are next, one line per point. Use '<a href="qh-optq.htm#Qc">Qc</a> <a href="qh-optq.htm#Qi">Qi</a> p' + to print the coordinates of all points.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Facets</b></dd> + <dt><a href="qh-optf.htm#Fn">Fn</a></dt> + <dd>list neighboring facets for each facet. The first line is the + number of facets. Each remaining line starts with the number of + neighboring facets. The cube example has four neighbors per facet.</dd> + <dt><a href="qh-optf.htm#FN">FN</a></dt> + <dd>list neighboring facets for each point. The first line is the + total number of points. Each remaining line starts with the number of + neighboring facets. Each vertex of the cube example has three neighboring + facets. Use '<a href="qh-optq.htm#Qc">Qc</a> <a href="qh-optq.htm#Qi">Qi</a> FN' + to include coplanar and interior points. </dd> + <dt><a href="qh-optf.htm#Fa">Fa</a></dt> + <dd>print area for each facet. The first line is the number of facets. + Facet area follows, one line per facet. For the cube example, each facet has area one.</dd> + <dt><a href="qh-optf.htm#FI">FI</a></dt> + <dd>list facet IDs. The first line is the number of + facets. The IDs follow, one per line.</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>Coplanar and interior points</b></dd> + <dt><a href="qh-optf.htm#Fc">Fc</a></dt> + <dd>list coplanar points for each facet. The first line is the number + of facets. The remaining lines start with the number of coplanar points. + A coplanar point is assigned to one facet.</dd> + <dt><a href="qh-optq.htm#Qi">Qi</a> <a href="qh-optf.htm#Fc">Fc</a></dt> + <dd>list interior points for each facet. The first line is the number + of facets. The remaining lines start with the number of interior points. + A coplanar point is assigned to one facet.</dd> + <dt><a href="qh-optf.htm#FP">FP</a></dt> + <dd>print distance to nearest vertex for coplanar points. The first line is the + number of coplanar points. Each remaining line starts with the point ID of + a vertex, followed by the point ID of a coplanar point, its facet, and distance. + Use '<a href="qh-optq.htm#Qc">Qc</a> <a href="qh-optq.htm#Qi">Qi</a> + <a href="qh-optf.htm#FP">FP</a>' for coplanar and interior points.</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>Hyperplanes</b></dd> + <dt><a href="qh-opto.htm#n">n</a></dt> + <dd>print hyperplane for each facet. The first line is the dimension. The + second line is the number of facets. Each remaining line is the hyperplane's + coefficients followed by its offset.</dd> + <dt><a href="qh-optf.htm#Fo">Fo</a></dt> + <dd>print outer plane for each facet. The output plane is above all points. + The first line is the dimension. The + second line is the number of facets. Each remaining line is the outer plane's + coefficients followed by its offset.</dd> + <dt><a href="qh-optf.htm#Fi">Fi</a></dt> + <dd>print inner plane for each facet. The inner plane of a facet is + below its vertices. + The first line is the dimension. The + second line is the number of facets. Each remaining line is the inner plane's + coefficients followed by its offset.</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="qh-opto.htm#s">s</a></dt> + <dd>print summary for the convex hull. Use '<a + href="qh-optf.htm#Fs">Fs</a>' and '<a + href="qh-optf.htm#FS">FS</a>' if you need numeric data.</dd> + <dt><a href="qh-optf.htm#FA">FA</a></dt> + <dd>compute total area and volume for '<a + href="qh-opto.htm#s">s</a>' and '<a href="qh-optf.htm#FS">FS</a>'</dd> + <dt><a href="qh-opto.htm#m">m</a></dt> + <dd>Mathematica output for the convex hull in 2-d or 3-d.</dd> + <dt><a href="qh-optf.htm#FM">FM</a></dt> + <dd>Maple output for the convex hull in 2-d or 3-d.</dd> + <dt><a href="qh-optg.htm#G">G</a></dt> + <dd>Geomview output for the convex hull in 2-d, 3-d, or 4-d.</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>Scaling and rotation</b></dd> + <dt><a href="qh-optq.htm#Qbk">Qbk:n</a></dt> + <dd>scale k'th coordinate to lower bound.</dd> + <dt><a href="qh-optq.htm#QBk">QBk:n</a></dt> + <dd>scale k'th coordinate to upper bound.</dd> + <dt><a href="qh-optq.htm#QbB">QbB</a></dt> + <dd>scale input to unit cube centered at the origin.</dd> + <dt><a href="qh-optq.htm#QRn">QRn</a></dt> + <dd>randomly rotate the input with a random seed of n. If n=0, the + seed is the time. If n=-1, use time for the random seed, but do + not rotate the input.</dd> + <dt><a href="qh-optq.htm#Qb0">Qbk:0Bk:0</a></dt> + <dd>remove k'th coordinate from input. This computes the + convex hull in one lower dimension.</dd> +</dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="controls">qconvex controls</a></h3> +<blockquote> + +<p>These options provide additional control:</p> + +<blockquote> +<dl compact> + <dt><a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>triangulated output. Qhull triangulates non-simplicial facets. It may produce + degenerate facets of zero area.</dd> + <dt><a href="qh-optq.htm#QJn">QJ</a></dt> + <dd>joggle the input instead of merging facets. This guarantees simplicial facets + (e.g., triangles in 3-d). It is less accurate than triangulated output ('Qt').</dd> + <dt><a href="qh-optq.htm#Qc">Qc</a></dt> + <dd>keep coplanar points</dd> + <dt><a href="qh-optq.htm#Qi">Qi</a></dt> + <dd>keep interior points</dd> + <dt><a href="qh-opto.htm#f">f </a></dt> + <dd>facet dump. Print the data structure for each facet.</dd> + <dt><a href="qh-optq.htm#QVn">QVn</a></dt> + <dd>select facets containing point <em>n</em> as a vertex,</dd> + <dt><a href="qh-optq.htm#QGn">QGn</a></dt> + <dd>select facets that are visible from point <em>n</em> + (marked 'good'). Use <em>-n</em> for the remainder.</dd> + <dt><a href="qh-optp.htm#PDk">PDk:0</a></dt> + <dd>select facets with a negative coordinate for dimension <i>k</i></dd> + <dt><a href="qh-optt.htm#TFn">TFn</a></dt> + <dd>report progress after constructing <em>n</em> facets</dd> + <dt><a href="qh-optt.htm#Tv">Tv</a></dt> + <dd>verify result</dd> + <dt><a href="qh-optt.htm#TO">TI file</a></dt> + <dd>input data from file. The filename may not use spaces or quotes.</dd> + <dt><a href="qh-optt.htm#TO">TO file</a></dt> + <dd>output results to file. Use single quotes if the filename + contains spaces (e.g., <tt>TO 'file with spaces.txt'</tt></dd> + <dt><a href="qh-optq.htm#Qs">Qs</a></dt> + <dd>search all points for the initial simplex. If Qhull can + not construct an initial simplex, it reports a +descriptive message. Usually, the point set is degenerate and one +or more dimensions should be removed ('<a href="qh-optq.htm#Qb0">Qbk:0Bk:0</a>'). +If not, use option 'Qs'. It performs an exhaustive search for the +best initial simplex. This is expensive is high dimensions.</dd> +</dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="graphics">qconvex graphics</a></h3> +<blockquote> + +<p>Display 2-d, 3-d, and 4-d convex hulls with Geomview ('<a +href="qh-optg.htm#G">G</a>').</p> + +<p>Display 2-d and 3-d convex hulls with Mathematica ('<a +href="qh-opto.htm#m">m</a>').</p> + +<p>To view 4-d convex hulls in 3-d, use '<a +href="qh-optp.htm#Pdk">Pd0d1d2d3</a>' to select the positive +octant and '<a href="qh-optg.htm#GDn">GrD2</a>' to drop dimension +2. </p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="notes">qconvex notes</a></h3> +<blockquote> + +<p>Qhull always computes a convex hull. The +convex hull may be used for other geometric structures. The +general technique is to transform the structure into an +equivalent convex hull problem. For example, the Delaunay +triangulation is equivalent to the convex hull of the input sites +after lifting the points to a paraboloid.</p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="conventions">qconvex +conventions</a></h3> +<blockquote> + +<p>The following terminology is used for convex hulls in Qhull. +See <a href="index.htm#structure">Qhull's data structures</a>.</p> + +<ul> + <li><em>point</em> - <em>d</em> coordinates</li> + <li><em>vertex</em> - extreme point of the input set</li> + <li><em>ridge</em> - <i>d-1</i> vertices between two + neighboring facets</li> + <li><em>hyperplane</em> - halfspace defined by a unit normal + and offset</li> + <li><em>coplanar point</em> - a nearly incident point to a + hyperplane</li> + <li><em>centrum</em> - a point on the hyperplane for testing + convexity</li> + <li><em>facet</em> - a facet with vertices, ridges, coplanar + points, neighboring facets, and hyperplane</li> + <li><em>simplicial facet</em> - a facet with <em>d</em> + vertices, <em>d</em> ridges, and <em>d</em> neighbors</li> + <li><em>non-simplicial facet</em> - a facet with more than <em>d</em> + vertices</li> + <li><em>good facet</em> - a facet selected by '<a + href="qh-optq.htm#QVn">QVn</a>', etc.</li> +</ul> +</blockquote> +<h3><a href="#TOP">»</a><a name="options">qconvex options</a></h3> + +<pre> +qconvex- compute the convex hull + http://www.qhull.org + +input (stdin): + first lines: dimension and number of points (or vice-versa). + other lines: point coordinates, best if one point per line + comments: start with a non-numeric character + +options: + Qt - triangulated output + QJ - joggle input instead of merging facets + Qc - keep coplanar points with nearest facet + Qi - keep interior points with nearest facet + +Qhull control options: + Qbk:n - scale coord k so that low bound is n + QBk:n - scale coord k so that upper bound is n (QBk is 0.5) + QbB - scale input to unit cube centered at the origin + Qbk:0Bk:0 - remove k-th coordinate from input + QJn - randomly joggle input in range [-n,n] + QRn - random rotation (n=seed, n=0 time, n=-1 time/no rotate) + Qs - search all points for the initial simplex + QGn - good facet if visible from point n, -n for not visible + QVn - good facet if it includes point n, -n if not + +Trace options: + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events + Tc - check frequently during execution + Ts - print statistics + Tv - verify result: structure, convexity, and point inclusion + Tz - send all output to stdout + TFn - report summary when n or more facets created + TI file - input data from file, no spaces or single quotes + TO file - output results to file, may be enclosed in single quotes + TPn - turn on tracing when point n added to hull + TMn - turn on tracing at merge n + TWn - trace merge facets when width > n + TVn - stop qhull after adding point n, -n for before (see TCn) + TCn - stop qhull after building cone for point n (see TVn) + +Precision options: + Cn - radius of centrum (roundoff added). Merge facets if non-convex + An - cosine of maximum angle. Merge facets if cosine > n or non-convex + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge + Rn - randomly perturb computations by a factor of [1-n,1+n] + Un - max distance below plane for a new, coplanar point + Wn - min facet width for outside point (before roundoff) + +Output formats (may be combined; if none, produces a summary to stdout): + f - facet dump + G - Geomview output (see below) + i - vertices incident to each facet + m - Mathematica output (2-d and 3-d) + n - normals with offsets + o - OFF file format (dim, points and facets; Voronoi regions) + p - point coordinates + s - summary (stderr) + +More formats: + Fa - area for each facet + FA - compute total area and volume for option 's' + Fc - count plus coplanar points for each facet + use 'Qc' (default) for coplanar and 'Qi' for interior + FC - centrum for each facet + Fd - use cdd format for input (homogeneous with offset first) + FD - use cdd format for numeric output (offset first) + FF - facet dump without ridges + Fi - inner plane for each facet + FI - ID for each facet + Fm - merge count for each facet (511 max) + FM - Maple output (2-d and 3-d) + Fn - count plus neighboring facets for each facet + FN - count plus neighboring facets for each point + Fo - outer plane (or max_outside) for each facet + FO - options and precision constants + FP - nearest vertex for each coplanar point + FQ - command used for qconvex + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets, + for output: #vertices, #facets, + #coplanar points, #non-simplicial facets + #real (2), max outer plane, min vertex + FS - sizes: #int (0) + #real(2) tot area, tot volume + Ft - triangulation with centrums for non-simplicial facets (OFF format) + Fv - count plus vertices for each facet + FV - average of vertices (a feasible point for 'H') + Fx - extreme points (in order for 2-d) + +Geomview output (2-d, 3-d, and 4-d) + Ga - all points as dots + Gp - coplanar points and vertices as radii + Gv - vertices as spheres + Gi - inner planes only + Gn - no planes + Go - outer planes only + Gc - centrums + Gh - hyperplane intersections + Gr - ridges + GDn - drop dimension n in 3-d and 4-d output + +Print options: + PAn - keep n largest facets by area + Pdk:n - drop facet if normal[k] <= n (default 0.0) + PDk:n - drop facet if normal[k] >= n + Pg - print good facets (needs 'QGn' or 'QVn') + PFn - keep facets whose area is at least n + PG - print neighbors of good facets + PMn - keep n facets with most merges + Po - force output. If error, output neighborhood of facet + Pp - do not report precision problems + + . - list of all options + - - one line descriptions of all options + +</pre> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +•<a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qdelau_f.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qdelau_f.htm new file mode 100644 index 0000000000000000000000000000000000000000..c07fa335a5be15f9c24a964a3bf8c83fe98a878e --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qdelau_f.htm @@ -0,0 +1,414 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>qdelaunay Qu -- furthest-site Delaunay triangulation</title> +</head> + +<body> +<!-- Navigation links --> +<a name="TOP"><b>Up</b></a><b>:</b> +<a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a>qdelaunay Qu -- furthest-site Delaunay triangulation</h1> + +<p>The furthest-site Delaunay triangulation corresponds to the upper facets of the <a href="qdelaun.htm">Delaunay construction</a>. +Its vertices are the +extreme points of the input sites. +It is the dual of the <a +href="qvoron_f.htm">furthest-site Voronoi diagram</a>. + +<blockquote> +<dl> + <dt><b>Example:</b> rbox 10 D2 | qdelaunay <a + href="qh-optq.htm#Qu">Qu</a> <a + href="qh-optq.htm#Qt">Qt</a> <a href="qh-opto.htm#s">s</a> + <a href="qh-opto.htm#i">i</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d, furthest-site Delaunay triangulation of 10 random + points. Triangulate the output. + Write a summary to the console and the regions to + 'result'.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox 10 D2 | qdelaunay <a + href="qh-optq.htm#Qu">Qu</a> <a + href="qh-optq.htm#QJn">QJ</a> <a href="qh-opto.htm#s">s</a> + <a href="qh-opto.htm#i">i</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d, furthest-site Delaunay triangulation of 10 random + points. Joggle the input to guarantee triangular output. + Write a summary to the console and the regions to + 'result'.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox r y c G1 D2 | qdelaunay <a + href="qh-optq.htm#Qu">Qu</a> <a href="qh-opto.htm#s">s</a> + <a href="qh-optf.htm#Fv">Fv</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d, furthest-site Delaunay triangulation of a triangle inside + a square. + Write a summary to the console and unoriented regions to 'result'. + Merge regions for cocircular input sites (e.g., the square). + The square is the only furthest-site + Delaunay region.</dd> +</dl> +</blockquote> + +<p>As with the Delaunay triangulation, Qhull computes the +furthest-site Delaunay triangulation by lifting the input sites to a +paraboloid. The lower facets correspond to the Delaunay +triangulation while the upper facets correspond to the +furthest-site triangulation. Neither triangulation includes +"vertical" facets (i.e., facets whose last hyperplane +coefficient is nearly zero). Vertical facets correspond to input +sites that are coplanar to the convex hull of the input. An +example is points on the boundary of a lattice.</p> + +<p>By default, qdelaunay merges cocircular and cospherical regions. +For example, the furthest-site Delaunay triangulation of a square inside a diamond +('rbox D2 c d G4 | qdelaunay Qu') consists of one region (the diamond). + +<p>If you use '<a href="qh-optq.htm#Qt">Qt</a>' (triangulated output), +all furthest-site Delaunay regions will be simplicial (e.g., triangles in 2-d). +Some regions may be +degenerate and have zero area. + +<p>If you use '<a href="qh-optq.htm#QJn">QJ</a>' (joggled input), all furthest-site +Delaunay regions +will be simplicial (e.g., triangles in 2-d). Joggled input +is less accurate than triangulated output ('Qt'). See <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>The output for 3-d, furthest-site Delaunay triangulations may be confusing if the +input contains cospherical data. See the FAQ item +<a href=qh-faq.htm#extra>Why +are there extra points in a 4-d or higher convex hull?</a> +Avoid these problems with triangulated output ('<a href="qh-optq.htm#Qt">Qt</a>') or +joggled input ('<a href="qh-optq.htm#QJn">QJ</a>'). +</p> + +<p>The 'qdelaunay' program is equivalent to +'<a href=qhull.htm#outputs>qhull d</a> <a href=qh-optq.htm#Qbb>Qbb</a>' in 2-d to 3-d, and +'<a href=qhull.htm#outputs>qhull d</a> <a href=qh-optq.htm#Qbb>Qbb</a> <a href=qh-optq.htm#Qx>Qx</a>' +in 4-d and higher. It disables the following Qhull +<a href=qh-quick.htm#options>options</a>: <i>d n v H U Qb QB Qc Qf Qg Qi +Qm Qr QR Qv Qx TR E V FC Fi Fo Fp FV Q0,etc</i>. + + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h3><a href="#TOP">»</a><a name="synopsis">furthest-site qdelaunay synopsis</a></h3> +<blockquote> + +See <a href="qdelaun.htm#synopsis">qdelaunay synopsis</a>. The same +program is used for both constructions. Use option '<a href="qh-optq.htm#Qu">Qu</a>' +for furthest-site Delaunay triangulations. + +</blockquote> +<h3><a href="#TOP">»</a><a name="input">furthest-site qdelaunay +input</a></h3> + +<blockquote> +<p>The input data on <tt>stdin</tt> consists of:</p> +<ul> + <li>dimension + <li>number of points</li> + <li>point coordinates</li> +</ul> + +<p>Use I/O redirection (e.g., qdelaunay Qu < data.txt), a pipe (e.g., rbox 10 | qdelaunay Qu), +or the '<a href=qh-optt.htm#TI>TI</a>' option (e.g., qdelaunay Qu TI data.txt). + +<p>For example, this is a square containing four random points. +Its furthest-site Delaunay +triangulation contains one square. +<p> +<blockquote> +<tt>rbox c 4 D2 > data</tt> +<blockquote><pre> +2 RBOX c 4 D2 +8 +-0.4999921736307369 -0.3684622117955817 +0.2556053225468894 -0.0413498678629751 +0.0327672376602583 -0.2810408135699488 +-0.452955383763607 0.17886471718444 + -0.5 -0.5 + -0.5 0.5 + 0.5 -0.5 + 0.5 0.5 +</pre></blockquote> + +<p><tt>qdelaunay Qu i < data</tt> +<blockquote><pre> + +Furthest-site Delaunay triangulation by the convex hull of 8 points in 3-d: + + Number of input sites: 8 + Number of Delaunay regions: 1 + Number of non-simplicial Delaunay regions: 1 + +Statistics for: RBOX c 4 D2 | QDELAUNAY s Qu i + + Number of points processed: 8 + Number of hyperplanes created: 20 + Number of facets in hull: 11 + Number of distance tests for qhull: 34 + Number of merged facets: 1 + Number of distance tests for merging: 107 + CPU seconds to compute hull (after input): 0.02 + +1 +7 6 4 5 +</pre></blockquote> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="outputs">furthest-site qdelaunay +outputs</a></h3> +<blockquote> + +<p>These options control the output of furthest-site Delaunay triangulations:</p> +<blockquote> + +<dl compact> + <dd><b>furthest-site Delaunay regions</b></dd> + <dt><a href="qh-opto.htm#i">i</a></dt> + <dd>list input sites for each furthest-site Delaunay region. The first line is the number of regions. The + remaining lines list the input sites for each region. The regions are + oriented. In 3-d and + higher, report cospherical sites by adding extra points. For the points-in-square example, + the square is the only furthest-site Delaunay region.</dd> + <dt><a href="qh-optf.htm#Fv">Fv</a></dt> + <dd>list input sites for each furthest-site Delaunay region. The first line is the number of regions. + Each remaining line starts with the number of input sites. The regions + are unoriented. For the points-in-square example, + the square is the only furthest-site Delaunay region.</dd> + <dt><a href="qh-optf.htm#Ft">Ft</a></dt> + <dd>print a triangulation of the furthest-site Delaunay regions in OFF format. The first line + is the dimension. The second line is the number of input sites and added points, + followed by the number of simplices and the number of ridges. + The input coordinates are next, followed by the centrum coordinates. There is + one centrum for each non-simplicial furthest-site Delaunay region. Each remaining line starts + with dimension+1. The + simplices are oriented. + For the points-in-square example, the square has a centrum at the + origin. It splits the square into four triangular regions.</dd> + <dt><a href="qh-optf.htm#Fn">Fn</a></dt> + <dd>list neighboring regions for each furthest-site Delaunay region. The first line is the + number of regions. Each remaining line starts with the number of + neighboring regions. Negative indices (e.g., <em>-1</em>) indicate regions + outside of the furthest-site Delaunay triangulation. + For the points-in-square example, the four neighboring regions + are outside of the triangulation. They belong to the regular + Delaunay triangulation.</dd> + <dt><a href="qh-optf.htm#FN">FN</a></dt> + <dd>list the furthest-site Delaunay regions for each input site. The first line is the + total number of input sites. Each remaining line starts with the number of + furthest-site Delaunay regions. Negative indices (e.g., <em>-1</em>) indicate regions + outside of the furthest-site Delaunay triangulation. + For the points-in-square example, the four random points belong to no region + while the square's vertices belong to region <em>0</em> and three + regions outside of the furthest-site Delaunay triangulation.</dd> + <dt><a href="qh-optf.htm#Fa">Fa</a></dt> + <dd>print area for each furthest-site Delaunay region. The first line is the number of regions. + The areas follow, one line per region. For the points-in-square example, the + square has unit area. </dd> + + <dt> </dt> + <dt> </dt> + <dd><b>Input sites</b></dd> + <dt><a href="qh-optf.htm#Fx">Fx</a></dt> + <dd>list extreme points of the input sites. These points are vertices of the furthest-point + Delaunay triangulation. They are on the + boundary of the convex hull. The first line is the number of + extreme points. Each point is listed, one per line. The points-in-square example + has four extreme points.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="qh-optf.htm#FA">FA</a></dt> + <dd>compute total area for '<a href="qh-opto.htm#s">s</a>' + and '<a href="qh-optf.htm#FS">FS</a>'. This is the + same as the area of the convex hull.</dd> + <dt><a href="qh-opto.htm#o">o</a></dt> + <dd>print upper facets of the corresponding convex hull (a + paraboloid)</dd> + <dt><a href="qh-opto.htm#m">m</a></dt> + <dd>Mathematica output for the upper facets of the paraboloid (2-d triangulations).</dd> + <dt><a href="qh-optf.htm#FM">FM</a></dt> + <dd>Maple output for the upper facets of the paraboloid (2-d triangulations).</dd> + <dt><a href="qh-optg.htm#G">G</a></dt> + <dd>Geomview output for the paraboloid (2-d or 3-d triangulations).</dd> + <dt><a href="qh-opto.htm#s">s</a></dt> + <dd>print summary for the furthest-site Delaunay triangulation. Use '<a + href="qh-optf.htm#Fs">Fs</a>' and '<a + href="qh-optf.htm#FS">FS</a>' for numeric data.</dd> +</dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="controls">furthest-site qdelaunay +controls</a></h3> +<blockquote> + +<p>These options provide additional control:</p> +<blockquote> + +<dl compact> + <dt><a href="qh-optq.htm#Qu">Qu</a></dt> + <dd>must be used for furthest-site Delaunay triangulation.</dd> + <dt><a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>triangulated output. Qhull triangulates non-simplicial facets. It may produce + degenerate facets of zero area.</dd> + <dt><a href="qh-optq.htm#QJn">QJ</a></dt> + <dd>joggle the input to avoid cospherical and coincident + sites. It is less accurate than triangulated output ('Qt').</dd> + <dt><a href="qh-optq.htm#QVn">QVn</a></dt> + <dd>select facets adjacent to input site <em>n</em> (marked + 'good').</dd> + <dt><a href="qh-optt.htm#Tv">Tv</a></dt> + <dd>verify result.</dd> + <dt><a href="qh-optt.htm#TO">TI file</a></dt> + <dd>input data from file. The filename may not use spaces or quotes.</dd> + <dt><a href="qh-optt.htm#TO">TO file</a></dt> + <dd>output results to file. Use single quotes if the filename + contains spaces (e.g., <tt>TO 'file with spaces.txt'</tt></dd> + <dt><a href="qh-optt.htm#TFn">TFn</a></dt> + <dd>report progress after constructing <em>n</em> facets</dd> + <dt><a href="qh-optp.htm#PDk">PDk:1</a></dt> + <dd>include upper and lower facets in the output. Set <em>k</em> + to the last dimension (e.g., 'PD2:1' for 2-d inputs). </dd> + <dt><a href="qh-opto.htm#f">f</a></dt> + <dd>facet dump. Print the data structure for each facet (i.e., furthest-site Delaunay region).</dd> +</dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="graphics">furthest-site qdelaunay +graphics</a></h3> +<blockquote> + +See <a href="qdelaun.htm#graphics">Delaunay graphics</a>. +They are the same except for Mathematica and Maple output. + +</blockquote> +<h3><a href="#TOP">»</a><a name="notes">furthest-site +qdelaunay notes</a></h3> +<blockquote> + +<p>The furthest-site Delaunay triangulation does not +record coincident input sites. Use <tt>qdelaunay</tt> instead. + +<p><tt>qdelaunay Qu</tt> does not work for purely cocircular +or cospherical points (e.g., rbox c | qdelaunay Qu). Instead, +use <tt>qdelaunay Qz</tt> -- when all points are vertices of the convex +hull of the input sites, the Delaunay triangulation is the same +as the furthest-site Delaunay triangulation. + +<p>A non-simplicial, furthest-site Delaunay region indicates nearly cocircular or +cospherical input sites. To avoid non-simplicial regions triangulate +the output ('<a href="qh-optq.htm#Qt">Qt</a>') or joggle +the input ('<a href="qh-optq.htm#QJn">QJ</a>'). Joggled input +is less accurate than triangulated output. +You may also triangulate +non-simplicial regions with option '<a +href="qh-optf.htm#Ft">Ft</a>'. It adds +the centrum to non-simplicial regions. Alternatively, use an <a +href="qh-impre.htm#exact">exact arithmetic code</a>.</p> + +<p>Furthest-site Delaunay triangulations do not include facets that are +coplanar with the convex hull of the input sites. A facet is +coplanar if the last coefficient of its normal is +nearly zero (see <a href="../src/user.h#ZEROdelaunay">qh_ZEROdelaunay</a>). + +</blockquote> +<h3><a href="#TOP">»</a><a name="conventions">furthest-site qdelaunay conventions</a></h3> +<blockquote> + +<p>The following terminology is used for furthest-site Delaunay +triangulations in Qhull. The underlying structure is the upper +facets of a convex hull in one higher dimension. See <a +href="qconvex.htm#conventions">convex hull conventions</a>, <a +href="qdelaun.htm#conventions">Delaunay conventions</a>, +and <a href="index.htm#structure">Qhull's data structures</a></p> +<blockquote> +<ul> + <li><em>input site</em> - a point in the input (one dimension + lower than a point on the convex hull)</li> + <li><em>point</em> - <i>d+1</i> coordinates. The last + coordinate is the sum of the squares of the input site's + coordinates</li> + <li><em>vertex</em> - a point on the paraboloid. It + corresponds to a unique input site. </li> + <li><em>furthest-site Delaunay facet</em> - an upper facet of the + paraboloid. The last coefficient of its normal is + clearly positive.</li> + <li><em>furthest-site Delaunay region</em> - a furthest-site Delaunay + facet projected to the input sites</li> + <li><em>non-simplicial facet</em> - more than <em>d</em> + points are cocircular or cospherical</li> + <li><em>good facet</em> - a furthest-site Delaunay facet with optional + restrictions by '<a href="qh-optq.htm#QVn">QVn</a>', etc.</li> +</ul> +</blockquote> +</blockquote> +<h3><a href="#TOP">»</a><a name="options">furthest-site qdelaunay options</a></h3> +<blockquote> + +See <a href="qdelaun.htm#options">qdelaunay options</a>. The same +program is used for both constructions. Use option '<a href="qh-optq.htm#Qu">Qu</a>' +for furthest-site Delaunay triangulations. + +</blockquote> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qdelaun.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qdelaun.htm new file mode 100644 index 0000000000000000000000000000000000000000..807554af7def557e06b395ec5b7b75f0128b141e --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qdelaun.htm @@ -0,0 +1,624 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>qdelaunay -- Delaunay triangulation</title> +</head> + +<body> +<!-- Navigation links --> +<a name="TOP"><b>Up</b></a><b>:</b> +<a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a>qdelaunay -- Delaunay triangulation</h1> + +<p>The Delaunay triangulation is the triangulation with empty +circumspheres. It has many useful properties and applications. +See the survey article by Aurenhammer [<a +href="index.htm#aure91">'91</a>] and the detailed introduction +by O'Rourke [<a href="index.htm#orou94">'94</a>]. </p> + +<blockquote> +<dl> + <dt><b>Example:</b> rbox r y c G0.1 D2 | qdelaunay <a href="qh-opto.htm#s">s</a> + <a href="qh-optf.htm#Fv">Fv</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d Delaunay triangulation of a triangle and + a small square. + Write a summary to the console and unoriented regions to 'result'. + Merge regions for cocircular input sites (i.e., the + square).</dd> + <dt> </dt> + <dt><b>Example:</b> rbox r y c G0.1 D2 | qdelaunay <a href="qh-opto.htm#s">s</a> + <a href="qh-optf.htm#Fv">Fv</a> <a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>Compute the 2-d Delaunay triangulation of a triangle and + a small square. Write a summary and unoriented + regions to the console. Produce triangulated output.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox 10 D2 | qdelaunay <a + href="qh-optq.htm#QJn">QJ</a> <a href="qh-opto.htm#s">s</a> + <a href="qh-opto.htm#i">i</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d Delaunay triangulation of 10 random + points. Joggle the input to guarantee triangular output. + Write a summary to the console and the regions to + 'result'.</dd> +</dl> +</blockquote> + +<p>Qhull computes the Delaunay triangulation by computing a +convex hull. It lifts the input sites to a paraboloid by adding +the sum of the squares of the coordinates. It scales the height +of the paraboloid to improve numeric precision ('<a href=qh-optq.htm#Qbb>Qbb</a>'). +It computes the convex +hull of the lifted sites, and projects the lower convex hull to +the input. + +<p>Each region of the Delaunay triangulation +corresponds to a facet of the lower half of the convex hull. +Facets of the upper half of the convex hull correspond to the <a +href="qdelau_f.htm">furthest-site Delaunay triangulation</a>. +See the examples, <a href="qh-eg.htm#delaunay">Delaunay and +Voronoi diagrams</a>.</p> + +<p>See <a href="http://www.qhull.org/html/qh-faq.htm#TOC">Qhull FAQ</a> - Delaunay and +Voronoi diagram questions.</p> + +<p>By default, qdelaunay merges cocircular and cospherical regions. +For example, the Delaunay triangulation of a square inside a diamond +('rbox D2 c d G4 | qdelaunay') contains one region for the square. +It identifies coincident points. + +<p>If you use '<a href="qh-optq.htm#Qt">Qt</a>' (triangulated output), +all Delaunay regions will be simplicial (e.g., triangles in 2-d). +Some regions may be +degenerate and have zero area. Triangulated output identifies coincident +points. + +<p>If you use '<a href="qh-optq.htm#QJn">QJ</a>' (joggled input), all Delaunay regions +will be simplicial (e.g., triangles in 2-d). Coincident points will +create small regions since the points are joggled apart. Joggled input +is less accurate than triangulated output ('Qt'). See <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>The output for 3-d Delaunay triangulations may be confusing if the +input contains cospherical data. See the FAQ item +<a href=qh-faq.htm#extra>Why +are there extra points in a 4-d or higher convex hull?</a> +Avoid these problems with triangulated output ('<a href="qh-optq.htm#Qt">Qt</a>') or +joggled input ('<a href="qh-optq.htm#QJn">QJ</a>'). +</p> + +<p>The 'qdelaunay' program is equivalent to +'<a href=qhull.htm#outputs>qhull d</a> <a href=qh-optq.htm#Qbb>Qbb</a>' in 2-d to 3-d, and +'<a href=qhull.htm#outputs>qhull d</a> <a href=qh-optq.htm#Qbb>Qbb</a> <a href=qh-optq.htm#Qx>Qx</a>' +in 4-d and higher. It disables the following Qhull +<a href=qh-quick.htm#options>options</a>: <i>d n v H U Qb QB Qc Qf Qg Qi +Qm Qr QR Qv Qx TR E V FC Fi Fo Fp Ft FV Q0,etc</i>. + + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h3><a href="#TOP">»</a><a name="synopsis">qdelaunay synopsis</a></h3> + +<pre> +qdelaunay- compute the Delaunay triangulation. + input (stdin): dimension, number of points, point coordinates + comments start with a non-numeric character + +options (qdelaun.htm): + Qt - triangulated output + QJ - joggle input instead of merging facets + Qu - furthest-site Delaunay triangulation + Tv - verify result: structure, convexity, and in-circle test + . - concise list of all options + - - one-line description of all options + +output options (subset): + s - summary of results (default) + i - vertices incident to each Delaunay region + Fx - extreme points (vertices of the convex hull) + o - OFF format (shows the points lifted to a paraboloid) + G - Geomview output (2-d and 3-d points lifted to a paraboloid) + m - Mathematica output (2-d inputs lifted to a paraboloid) + QVn - print Delaunay regions that include point n, -n if not + TO file- output results to file, may be enclosed in single quotes + +examples: + rbox c P0 D2 | qdelaunay s o rbox c P0 D2 | qdelaunay i + rbox c P0 D3 | qdelaunay Fv Qt rbox c P0 D2 | qdelaunay s Qu Fv + rbox c G1 d D2 | qdelaunay s i rbox c G1 d D2 | qdelaunay s i Qt + rbox M3,4 z 100 D2 | qdelaunay s rbox M3,4 z 100 D2 | qdelaunay s Qt +</pre> + + +<h3><a href="#TOP">»</a><a name="input">qdelaunay +input</a></h3> + +<blockquote> +<p>The input data on <tt>stdin</tt> consists of:</p> +<ul> + <li>dimension + <li>number of points</li> + <li>point coordinates</li> +</ul> + +<p>Use I/O redirection (e.g., qdelaunay < data.txt), a pipe (e.g., rbox 10 | qdelaunay), +or the '<a href=qh-optt.htm#TI>TI</a>' option (e.g., qdelaunay TI data.txt). + +<p>For example, this is four cocircular points inside a square. Its Delaunay +triangulation contains 8 triangles and one four-sided +figure. +<p> +<blockquote> +<tt>rbox s 4 W0 c G1 D2 > data</tt> +<blockquote><pre> +2 RBOX s 4 W0 c D2 +8 +-0.4941988586954018 -0.07594397977563715 +-0.06448037284989526 0.4958248496365813 +0.4911154367094632 0.09383830681375946 +-0.348353580869097 -0.3586778257652367 + -1 -1 + -1 1 + 1 -1 + 1 1 +</pre></blockquote> + +<p><tt>qdelaunay s i < data</tt> +<blockquote><pre> + +Delaunay triangulation by the convex hull of 8 points in 3-d + + Number of input sites: 8 + Number of Delaunay regions: 9 + Number of non-simplicial Delaunay regions: 1 + +Statistics for: RBOX s 4 W0 c D2 | QDELAUNAY s i + + Number of points processed: 8 + Number of hyperplanes created: 18 + Number of facets in hull: 10 + Number of distance tests for qhull: 33 + Number of merged facets: 2 + Number of distance tests for merging: 102 + CPU seconds to compute hull (after input): 0.028 + +9 +1 7 5 +6 3 4 +2 3 6 +7 2 6 +2 7 1 +0 5 4 +3 0 4 +0 1 5 +1 0 3 2 +</pre></blockquote> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="outputs">qdelaunay +outputs</a></h3> +<blockquote> + +<p>These options control the output of Delaunay triangulations:</p> +<blockquote> + +<dl compact> + <dd><b>Delaunay regions</b></dd> + <dt><a href="qh-opto.htm#i">i</a></dt> + <dd>list input sites for each Delaunay region. The first line is the number of regions. The + remaining lines list the input sites for each region. The regions are + oriented. In 3-d and + higher, report cospherical sites by adding extra points. Use triangulated + output ('<a href="qh-optq.htm#Qt">Qt</a>') to avoid non-simpicial regions. For the circle-in-square example, + eight Delaunay regions are triangular and the ninth has four input sites.</dd> + <dt><a href="qh-optf.htm#Fv">Fv</a></dt> + <dd>list input sites for each Delaunay region. The first line is the number of regions. + Each remaining line starts with the number of input sites. The regions + are unoriented. For the circle-in-square example, + eight Delaunay regions are triangular and the ninth has four input sites.</dd> + <dt><a href="qh-optf.htm#Fn">Fn</a></dt> + <dd>list neighboring regions for each Delaunay region. The first line is the + number of regions. Each remaining line starts with the number of + neighboring regions. Negative indices (e.g., <em>-1</em>) indicate regions + outside of the Delaunay triangulation. + For the circle-in-square example, the four regions on the square are neighbors to + the region-at-infinity.</dd> + <dt><a href="qh-optf.htm#FN">FN</a></dt> + <dd>list the Delaunay regions for each input site. The first line is the + total number of input sites. Each remaining line starts with the number of + Delaunay regions. Negative indices (e.g., <em>-1</em>) indicate regions + outside of the Delaunay triangulation. + For the circle-in-square example, each point on the circle belongs to four + Delaunay regions. Use '<a href="qh-optq.htm#Qc">Qc</a> FN' + to include coincident input sites and deleted vertices. </dd> + <dt><a href="qh-optf.htm#Fa">Fa</a></dt> + <dd>print area for each Delaunay region. The first line is the number of regions. + The areas follow, one line per region. For the circle-in-square example, the + cocircular region has area 0.4. </dd> + <dt> </dt> + <dt> </dt> + <dd><b>Input sites</b></dd> + <dt><a href="qh-optf.htm#Fc">Fc</a></dt> + <dd>list coincident input sites for each Delaunay region. + The first line is the number of regions. The remaining lines start with + the number of coincident sites and deleted vertices. Deleted vertices + indicate highly degenerate input (see'<A href="qh-optf.htm#Fs">Fs</a>'). + A coincident site is assigned to one Delaunay + region. Do not use '<a href="qh-optq.htm#QJn">QJ</a>' with 'Fc'; the joggle will separate + coincident sites.</dd> + <dt><a href="qh-optf.htm#FP">FP</a></dt> + <dd>print coincident input sites with distance to + nearest site (i.e., vertex). The first line is the + number of coincident sites. Each remaining line starts with the point ID of + an input site, followed by the point ID of a coincident point, its region, and distance. + Includes deleted vertices which + indicate highly degenerate input (see'<A href="qh-optf.htm#Fs">Fs</a>'). + Do not use '<a href="qh-optq.htm#QJn">QJ</a>' with 'FP'; the joggle will separate + coincident sites.</dd> + <dt><a href="qh-optf.htm#Fx">Fx</a></dt> + <dd>list extreme points of the input sites. These points are on the + boundary of the convex hull. The first line is the number of + extreme points. Each point is listed, one per line. The circle-in-square example + has four extreme points.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="qh-optf.htm#FA">FA</a></dt> + <dd>compute total area for '<a href="qh-opto.htm#s">s</a>' + and '<a href="qh-optf.htm#FS">FS</a>'</dd> + <dt><a href="qh-opto.htm#o">o</a></dt> + <dd>print lower facets of the corresponding convex hull (a + paraboloid)</dd> + <dt><a href="qh-opto.htm#m">m</a></dt> + <dd>Mathematica output for the lower facets of the paraboloid (2-d triangulations).</dd> + <dt><a href="qh-optf.htm#FM">FM</a></dt> + <dd>Maple output for the lower facets of the paraboloid (2-d triangulations).</dd> + <dt><a href="qh-optg.htm#G">G</a></dt> + <dd>Geomview output for the paraboloid (2-d or 3-d triangulations).</dd> + <dt><a href="qh-opto.htm#s">s</a></dt> + <dd>print summary for the Delaunay triangulation. Use '<a + href="qh-optf.htm#Fs">Fs</a>' and '<a + href="qh-optf.htm#FS">FS</a>' for numeric data.</dd> +</dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="controls">qdelaunay +controls</a></h3> +<blockquote> + +<p>These options provide additional control:</p> +<blockquote> + +<dl compact> + <dt><a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>triangulated output. Qhull triangulates non-simplicial facets. It may produce +degenerate facets of zero area.</dd> + <dt><a href="qh-optq.htm#QJn">QJ</a></dt> + <dd>joggle the input to avoid cospherical and coincident + sites. It is less accurate than triangulated output ('Qt').</dd> + <dt><a href="qh-optq.htm#Qu">Qu</a></dt> + <dd>compute the <a href="qdelau_f.htm">furthest-site Delaunay triangulation</a>.</dd> + <dt><a href="qh-optq.htm#Qz">Qz</a></dt> + <dd>add a point above the paraboloid to reduce precision + errors. Use it for nearly cocircular/cospherical input + (e.g., 'rbox c | qdelaunay Qz'). The point is printed for + options '<a href="qh-optf.htm#Ft">Ft</a>' and '<a + href="qh-opto.htm#o">o</a>'.</dd> + <dt><a href="qh-optq.htm#QVn">QVn</a></dt> + <dd>select facets adjacent to input site <em>n</em> (marked + 'good').</dd> + <dt><a href="qh-optt.htm#Tv">Tv</a></dt> + <dd>verify result.</dd> + <dt><a href="qh-optt.htm#TO">TI file</a></dt> + <dd>input data from file. The filename may not use spaces or quotes.</dd> + <dt><a href="qh-optt.htm#TO">TO file</a></dt> + <dd>output results to file. Use single quotes if the filename + contains spaces (e.g., <tt>TO 'file with spaces.txt'</tt></dd> + <dt><a href="qh-optt.htm#TFn">TFn</a></dt> + <dd>report progress after constructing <em>n</em> facets</dd> + <dt><a href="qh-optp.htm#PDk">PDk:1</a></dt> + <dd>include upper and lower facets in the output. Set <em>k</em> + to the last dimension (e.g., 'PD2:1' for 2-d inputs). </dd> + <dt><a href="qh-opto.htm#f">f</a></dt> + <dd>facet dump. Print the data structure for each facet (i.e., Delaunay region).</dd> +</dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="graphics">qdelaunay +graphics</a></h3> +<blockquote> + +<p>For 2-d and 3-d Delaunay triangulations, Geomview ('qdelaunay <a +href="qh-optg.htm#G">G</a>') displays the corresponding convex +hull (a paraboloid). </p> + +<p>To view a 2-d Delaunay triangulation, use 'qdelaunay <a +href="qh-optg.htm#GDn">GrD2</a>' to drop the last dimension. This +is the same as viewing the hull without perspective (see +Geomview's 'cameras' menu). </p> + +<p>To view a 3-d Delaunay triangulation, use 'qdelaunay <a +href="qh-optg.htm#GDn">GrD3</a>' to drop the last dimension. You +may see extra edges. These are interior edges that Geomview moves +towards the viewer (see 'lines closer' in Geomview's camera +options). Use option '<a href="qh-optg.htm#Gt">Gt</a>' to make +the outer ridges transparent in 3-d. See <a +href="qh-eg.htm#delaunay">Delaunay and Voronoi examples</a>.</p> + +<p>For 2-d Delaunay triangulations, Mathematica ('<a +href="qh-opto.htm#m">m</a>') and Maple ('<a +href="qh-optf.htm#FM">FM</a>') output displays the lower facets of the corresponding convex +hull (a paraboloid). </p> + +<p>For 2-d, furthest-site Delaunay triangulations, Maple and Mathematica output ('<a +href="qh-optq.htm#Qu">Qu</a> <a +href="qh-opto.htm#m">m</a>') displays the upper facets of the corresponding convex +hull (a paraboloid). </p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="notes">qdelaunay +notes</a></h3> +<blockquote> + +<p>You can simplify the Delaunay triangulation by enclosing the input +sites in a large square or cube. This is particularly recommended +for cocircular or cospherical input data. + +<p>A non-simplicial Delaunay region indicates nearly cocircular or +cospherical input sites. To avoid non-simplicial regions either triangulate +the output ('<a href="qh-optq.htm#Qt">Qt</a>') or joggle +the input ('<a href="qh-optq.htm#QJn">QJ</a>'). Triangulated output +is more accurate than joggled input. Alternatively, use an <a +href="qh-impre.htm#exact">exact arithmetic code</a>.</p> + +<p>Delaunay triangulations do not include facets that are +coplanar with the convex hull of the input sites. A facet is +coplanar if the last coefficient of its normal is +nearly zero (see <a href="../src/user.h#ZEROdelaunay">qh_ZEROdelaunay</a>). + +<p>See <a href=qh-impre.htm#delaunay>Imprecision issues :: Delaunay triangulations</a> +for a discussion of precision issues. Deleted vertices indicate +highly degenerate input. They are listed in the summary output and +option '<a href="qh-optf.htm#Fs">Fs</a>'.</p> + +<p>To compute the Delaunay triangulation of points on a sphere, +compute their convex hull. If the sphere is the unit sphere at +the origin, the facet normals are the Voronoi vertices of the +input. The points may be restricted to a hemisphere. [S. Fortune] +</p> + +<p>The 3-d Delaunay triangulation of regular points on a half +spiral (e.g., 'rbox 100 l | qdelaunay') has quadratic size, while the Delaunay triangulation +of random 3-d points is +approximately linear for reasonably sized point sets. + +<p>With the <a href="qh-in.htm#library">Qhull library</a>, you +can use <tt>qh_findbestfacet</tt> in <tt>poly2.c</tt> to locate the facet +that contains a point. You should first lift the point to the +paraboloid (i.e., the last coordinate is the sum of the squares +of the point's coordinates -- <tt>qh_setdelaunay</tt>). Do not use options +'<a href="qh-optq.htm#Qbb">Qbb</a>', '<a href="qh-optq.htm#QbB">QbB</a>', +'<a href="qh-optq.htm#Qbk">Qbk:n</a>', or '<a +href="qh-optq.htm#QBk">QBk:n</a>' since these scale the last +coordinate. </p> + +<p>If a point is interior to the convex hull of the input set, it +is interior to the adjacent vertices of the Delaunay +triangulation. This is demonstrated by the following pipe for +point 0: + +<pre> + qdelaunay <data s FQ QV0 p | qconvex s Qb3:0B3:0 p +</pre> + +<p>The first call to qdelaunay returns the neighboring points of +point 0 in the Delaunay triangulation. The second call to qconvex +returns the vertices of the convex hull of these points (after +dropping the lifted coordinate). If point 0 is interior to the +original point set, it is interior to the reduced point set. </p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="conventions">qdelaunay conventions</a></h3> +<blockquote> + +<p>The following terminology is used for Delaunay triangulations +in Qhull for dimension <i>d</i>. The underlying structure is the +lower facets of a convex hull in dimension <i>d+1</i>. For +further information, see <a href="index.htm#structure">data +structures</a> and <a href="qconvex.htm#conventions">convex hull +conventions</a>.</p> +<blockquote> +<ul> + <li><em>input site</em> - a point in the input (one dimension + lower than a point on the convex hull)</li> + <li><em>point</em> - a point has <i>d+1</i> coordinates. The + last coordinate is the sum of the squares of the input + site's coordinates</li> + <li><em>coplanar point</em> - a <em>coincident</em> + input site or a deleted vertex. Deleted vertices + indicate highly degenerate input.</li> + <li><em>vertex</em> - a point on the paraboloid. It + corresponds to a unique input site. </li> + <li><em>point-at-infinity</em> - a point added above the + paraboloid by option '<a href="qh-optq.htm#Qz">Qz</a>'</li> + <li><em>lower facet</em> - a facet corresponding to a + Delaunay region. The last coefficient of its normal is + clearly negative.</li> + <li><em>upper facet</em> - a facet corresponding to a + furthest-site Delaunay region. The last coefficient of + its normal is clearly positive. </li> + <li><em>Delaunay region</em> - a + lower facet projected to the input sites</li> + <li><em>upper Delaunay region</em> - an upper facet projected + to the input sites</li> + <li><em>non-simplicial facet</em> - more than <em>d</em> + input sites are cocircular or cospherical</li> + <li><em>good facet</em> - a Delaunay region with optional + restrictions by '<a href="qh-optq.htm#QVn">QVn</a>', etc.</li> +</ul> +</blockquote> +</blockquote> +<h3><a href="#TOP">»</a><a name="options">qdelaunay options</a></h3> + +<pre> +qdelaunay- compute the Delaunay triangulation + http://www.qhull.org + +input (stdin): + first lines: dimension and number of points (or vice-versa). + other lines: point coordinates, best if one point per line + comments: start with a non-numeric character + +options: + Qt - triangulated output + QJ - joggle input instead of merging facets + Qu - compute furthest-site Delaunay triangulation + +Qhull control options: + QJn - randomly joggle input in range [-n,n] + Qs - search all points for the initial simplex + Qz - add point-at-infinity to Delaunay triangulation + QGn - print Delaunay region if visible from point n, -n if not + QVn - print Delaunay regions that include point n, -n if not + +Trace options: + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events + Tc - check frequently during execution + Ts - print statistics + Tv - verify result: structure, convexity, and in-circle test + Tz - send all output to stdout + TFn - report summary when n or more facets created + TI file - input data from file, no spaces or single quotes + TO file - output results to file, may be enclosed in single quotes + TPn - turn on tracing when point n added to hull + TMn - turn on tracing at merge n + TWn - trace merge facets when width > n + TVn - stop qhull after adding point n, -n for before (see TCn) + TCn - stop qhull after building cone for point n (see TVn) + +Precision options: + Cn - radius of centrum (roundoff added). Merge facets if non-convex + An - cosine of maximum angle. Merge facets if cosine > n or non-convex + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge + Rn - randomly perturb computations by a factor of [1-n,1+n] + Wn - min facet width for outside point (before roundoff) + +Output formats (may be combined; if none, produces a summary to stdout): + f - facet dump + G - Geomview output (see below) + i - vertices incident to each Delaunay region + m - Mathematica output (2-d only, lifted to a paraboloid) + o - OFF format (dim, points, and facets as a paraboloid) + p - point coordinates (lifted to a paraboloid) + s - summary (stderr) + +More formats: + Fa - area for each Delaunay region + FA - compute total area for option 's' + Fc - count plus coincident points for each Delaunay region + Fd - use cdd format for input (homogeneous with offset first) + FD - use cdd format for numeric output (offset first) + FF - facet dump without ridges + FI - ID of each Delaunay region + Fm - merge count for each Delaunay region (511 max) + FM - Maple output (2-d only, lifted to a paraboloid) + Fn - count plus neighboring region for each Delaunay region + FN - count plus neighboring region for each point + FO - options and precision constants + FP - nearest point and distance for each coincident point + FQ - command used for qdelaunay + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets, + for output: #vertices, #Delaunay regions, + #coincident points, #non-simplicial regions + #real (2), max outer plane, min vertex + FS - sizes: #int (0) + #real(2) tot area, 0 + Fv - count plus vertices for each Delaunay region + Fx - extreme points of Delaunay triangulation (on convex hull) + +Geomview options (2-d and 3-d) + Ga - all points as dots + Gp - coplanar points and vertices as radii + Gv - vertices as spheres + Gi - inner planes only + Gn - no planes + Go - outer planes only + Gc - centrums + Gh - hyperplane intersections + Gr - ridges + GDn - drop dimension n in 3-d and 4-d output + Gt - transparent outer ridges to view 3-d Delaunay + +Print options: + PAn - keep n largest Delaunay regions by area + Pdk:n - drop facet if normal[k] <= n (default 0.0) + PDk:n - drop facet if normal[k] >= n + Pg - print good Delaunay regions (needs 'QGn' or 'QVn') + PFn - keep Delaunay regions whose area is at least n + PG - print neighbors of good regions (needs 'QGn' or 'QVn') + PMn - keep n Delaunay regions with most merges + Po - force output. If error, output neighborhood of facet + Pp - do not report precision problems + + . - list of all options + - - one line descriptions of all options +</pre> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--4d.gif b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--4d.gif new file mode 100644 index 0000000000000000000000000000000000000000..08be18c8a5a1c89f07da6abb451283cbf176d5b4 Binary files /dev/null and b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--4d.gif differ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--cone.gif b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--cone.gif new file mode 100644 index 0000000000000000000000000000000000000000..78470ee8f020a5a669645452aada93bc50918cca Binary files /dev/null and b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--cone.gif differ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--dt.gif b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--dt.gif new file mode 100644 index 0000000000000000000000000000000000000000..b6d4e26727ce648be6ed6d4c8e5a49a367064f20 Binary files /dev/null and b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--dt.gif differ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--geom.gif b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--geom.gif new file mode 100644 index 0000000000000000000000000000000000000000..9c70b54991a4574015671ec0e0b29102c5346737 Binary files /dev/null and b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--geom.gif differ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--half.gif b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--half.gif new file mode 100644 index 0000000000000000000000000000000000000000..80a4d0883b5e139a93fe5df8bcdcdd4f94d63137 Binary files /dev/null and b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--half.gif differ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--rand.gif b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--rand.gif new file mode 100644 index 0000000000000000000000000000000000000000..4d4e4daee8cb9f758248405028b0f87d8006931d Binary files /dev/null and b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh--rand.gif differ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-eg.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-eg.htm new file mode 100644 index 0000000000000000000000000000000000000000..bec9dda74de64a63767fe59eb5a6c20268b8b543 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-eg.htm @@ -0,0 +1,691 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Examples of Qhull</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a href="http://www.qhull.org">Home +page</a> for Qhull <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To: </b><a href="#TOC">Qhull examples: Table of Contents</a> (please wait +while loading)<br> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/half.html"><img +src="qh--half.gif" alt="[halfspace]" align="middle" width="100" +height="100"></a> Examples of Qhull</h1> + +<p>This section of the Qhull manual will introduce you to Qhull +and its options. Each example is a file for viewing with <a +href="index.htm#geomview">Geomview</a>. You will need to +use a Unix computer with a copy of Geomview. +<p> +If you are not running Unix, you can view <a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/welcome.html">pictures</a> +for some of the examples. To understand Qhull without Geomview, try the +examples in <a href="qh-quick.htm#programs">Programs</a> and +<a href="qh-quick.htm#programs">Programs/input</a>. You can also try small +examples that you compute by hand. Use <a href="rbox.htm">rbox</a> +to generate examples. +<p> +To generate the Geomview examples, execute the shell script <tt>eg/q_eg</tt>. +It uses <tt>rbox</tt>. The shell script <tt>eg/q_egtest</tt> generates +test examples, and <tt>eg/q_test</tt> exercises the code. If you +find yourself viewing the inside of a 3-d example, use Geomview's +normalization option on the 'obscure' menu.</p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h2><a href="#TOP">»</a><a name="TOC">Qhull examples: Table of +Contents </a></h2> + +<ul> + <li><a href="#2d">2-d and 3-d examples</a></li> + <li><a href="#how">How Qhull adds a point</a></li> + <li><a href="#joggle">Triangulated output or joggled input</a></li> + <li><a href="#delaunay">Delaunay and Voronoi diagrams</a></li> + <li><a href="#merge">Facet merging for imprecision</a></li> + <li><a href="#4d">4-d objects</a></li> + <li><a href="#half">Halfspace intersections</a></li> +</ul> + +<hr> +<ul> + <li><a href="#TOC">»</a><a name="2d">2-d and 3-d examples</a><ul> + <li><a href="#01">eg.01.cube</a></li> + <li><a href="#02">eg.02.diamond.cube</a></li> + <li><a href="#03">eg.03.sphere</a></li> + <li><a href="#04">eg.04.circle</a></li> + <li><a href="#05">eg.05.spiral</a></li> + <li><a href="#06">eg.06.merge.square</a></li> + <li><a href="#07">eg.07.box</a></li> + <li><a href="#08a">eg.08a.cube.sphere</a></li> + <li><a href="#08b">eg.08b.diamond.sphere</a></li> + <li><a href="#09">eg.09.lens</a></li> + </ul> + </li> + <li><a href="#TOC">»</a><a name="how">How Qhull adds a point</a><ul> + <li><a href="#10a">eg.10a.sphere.visible</a></li> + <li><a href="#10b">eg.10b.sphere.beyond</a></li> + <li><a href="#10c">eg.10c.sphere.horizon</a></li> + <li><a href="#10d">eg.10d.sphere.cone</a></li> + <li><a href="#10e">eg.10e.sphere.new</a></li> + <li><a href="#14">eg.14.sphere.corner</a></li> + </ul> + </li> + <li><a href="#TOC">»</a> <a name="joggle">Triangulated output or joggled input</a> + <ul> + <li><a href="#15a">eg.15a.surface</a></li> + <li><a href="#15b">eg.15b.triangle</a></li> + <li><a href="#15c">eg.15c.joggle</a></li> + </ul> + <li><a href="#TOC">»</a><a name="delaunay"> Delaunay and + Voronoi diagrams</a><ul> + <li><a href="#17a">eg.17a.delaunay.2</a></li> + <li><a href="#17b">eg.17b.delaunay.2i</a></li> + <li><a href="#17c">eg.17c.delaunay.2-3</a></li> + <li><a href="#17d">eg.17d.voronoi.2</a></li> + <li><a href="#17e">eg.17e.voronoi.2i</a></li> + <li><a href="#17f">eg.17f.delaunay.3</a></li> + <li><a href="#18a">eg.18a.furthest.2-3</a></li> + <li><a href="#18b">eg.18b.furthest-up.2-3</a></li> + <li><a href="#18c">eg.18c.furthest.2</a></li> + <li><a href="#19">eg.19.voronoi.region.3</a></li> + </ul> + </li> + <li><a href="#TOC">»</a><a name="merge">Facet merging for + imprecision </a><ul> + <li><a href="#20">eg.20.cone</a></li> + <li><a href="#21a">eg.21a.roundoff.errors</a></li> + <li><a href="#21b">eg.21b.roundoff.fixed</a></li> + <li><a href="#22a">eg.22a.merge.sphere.01</a></li> + <li><a href="#22b">eg.22b.merge.sphere.-01</a></li> + <li><a href="#22c">eg.22c.merge.sphere.05</a></li> + <li><a href="#22d">eg.22d.merge.sphere.-05</a></li> + <li><a href="#23">eg.23.merge.cube</a></li> + </ul> + </li> + <li><a href="#TOC">»</a><a name="4d">4-d objects</a><ul> + <li><a href="#24">eg.24.merge.cube.4d-in-3d</a></li> + <li><a href="#30">eg.30.4d.merge.cube</a></li> + <li><a href="#31">eg.31.4d.delaunay</a></li> + <li><a href="#32">eg.32.4d.octant</a></li> + </ul> + </li> + <li><a href="#TOC">»</a><a name="half">Halfspace + intersections</a><ul> + <li><a href="#33a">eg.33a.cone</a></li> + <li><a href="#33b">eg.33b.cone.dual</a></li> + <li><a href="#33c">eg.33c.cone.halfspace</a></li> + </ul> + </li> +</ul> + +<hr> + +<h2><a href="#TOC">»</a>2-d and 3-d examples</h2> + +<h3><a href="#2d">»</a><a name="01">rbox c D3 | qconvex G +>eg.01.cube </a></h3> + +<p>The first example is a cube in 3-d. The color of each facet +indicates its normal. For example, normal [0,0,1] along the Z +axis is (r=0.5, g=0.5, b=1.0). With the 'Dn' option in <tt>rbox</tt>, +you can generate hypercubes in any dimension. Above 7-d the +number of intermediate facets grows rapidly. Use '<a +href="qh-optt.htm#TFn">TFn</a>' to track qconvex's progress. Note +that each facet is a square that qconvex merged from coplanar +triangles.</p> + +<h3><a href="#2d">»</a><a name="02">rbox c d G3.0 | qconvex G +>eg.02.diamond.cube </a></h3> + +<p>The second example is a cube plus a diamond ('d') scaled by <tt>rbox</tt>'s +'G' option. In higher dimensions, diamonds are much simpler than +hypercubes. </p> + +<h3><a href="#2d">»</a><a name="03">rbox s 100 D3 | qconvex G +>eg.03.sphere </a></h3> + +<p>The <tt>rbox s</tt> option generates random points and +projects them to the d-sphere. All points should be on the convex +hull. Notice that random points look more clustered than you +might expect. You can get a smoother distribution by merging +facets and printing the vertices, e.g.,<i> rbox 1000 s | qconvex +A-0.95 p | qconvex G >eg.99</i>.</p> + +<h3><a href="#2d">»</a><a name="04">rbox s 100 D2 | qconvex G +>eg.04.circle </a></h3> + +<p>In 2-d, there are many ways to generate a convex hull. One of +the earliest algorithms, and one of the fastest, is the 2-d +Quickhull algorithm [c.f., Preparata & Shamos <a +href="index.htm#pre-sha85">'85</a>]. It was the model for +Qhull.</p> + +<h3><a href="#2d">»</a><a name="05">rbox 10 l | qconvex G +>eg.05.spiral </a></h3> + +<p>One rotation of a spiral.</p> + +<h3><a href="#2d">»</a><a name="06">rbox 1000 D2 | qconvex C-0.03 +Qc Gapcv >eg.06.merge.square</a></h3> + +<p>This demonstrates how Qhull handles precision errors. Option '<a +href="qh-optc.htm#Cn">C-0.03</a>' requires a clearly convex angle +between adjacent facets. Otherwise, Qhull merges the facets. </p> + +<p>This is the convex hull of random points in a square. The +facets have thickness because they must be outside all points and +must include their vertices. The colored lines represent the +original points and the spheres represent the vertices. Floating +in the middle of each facet is the centrum. Each centrum is at +least 0.03 below the planes of its neighbors. This guarantees +that the facets are convex.</p> + +<h3><a href="#2d">»</a><a name="07">rbox 1000 D3 | qconvex G +>eg.07.box </a></h3> + +<p>Here's the same distribution but in 3-d with Qhull handling +machine roundoff errors. Note the large number of facets. </p> + +<h3><a href="#2d">»</a><a name="08a">rbox c G0.4 s 500 | qconvex G +>eg.08a.cube.sphere </a></h3> + +<p>The sphere is just barely poking out of the cube. Try the same +distribution with randomization turned on ('<a +href="qh-optq.htm#Qr">Qr</a>'). This turns Qhull into a +randomized incremental algorithm. To compare Qhull and +randomization, look at the number of hyperplanes created and the +number of points partitioned. Don't compare CPU times since Qhull's +implementation of randomization is inefficient. The number of +hyperplanes and partitionings indicate the dominant costs for +Qhull. With randomization, you'll notice that the number of +facets created is larger than before. This is especially true as +you increase the number of points. It is because the randomized +algorithm builds most of the sphere before it adds the cube's +vertices.</p> + +<h3><a href="#2d">»</a><a name="08b">rbox d G0.6 s 500 | qconvex G +>eg.08b.diamond.sphere </a></h3> + +<p>This is a combination of the diamond distribution and the +sphere.</p> + +<h3><a href="#2d">»</a><a name="09">rbox 100 L3 G0.5 s | qconvex +G >eg.09.lens </a></h3> + +<p>Each half of the lens distribution lies on a sphere of radius +three. A directed search for the furthest facet below a point +(e.g., qh_findbest in <tt>geom.c</tt>) may fail if started from +an arbitrary facet. For example, if the first facet is on the +opposite side of the lens, a directed search will report that the +point is inside the convex hull even though it is outside. This +problem occurs whenever the curvature of the convex hull is less +than a sphere centered at the test point. </p> + +<p>To prevent this problem, Qhull does not use directed search +all the time. When Qhull processes a point on the edge of the +lens, it partitions the remaining points with an exhaustive +search instead of a directed search (see qh_findbestnew in <tt>geom2.c</tt>). +</p> + +<h2><a href="#TOC">»</a>How Qhull adds a point</h2> + +<h3><a href="#how">»</a><a name="10a">rbox 100 s P0.5,0.5,0.5 | +qconvex Ga QG0 >eg.10a.sphere.visible</a></h3> + +<p>The next 4 examples show how Qhull adds a point. The point +[0.5,0.5,0.5] is at one corner of the bounding box. Qhull adds a +point using the beneath-beyond algorithm. First Qhull finds all +of the facets that are visible from the point. Qhull will replace +these facets with new facets.</p> + +<h3><a href="#how">»</a><a name="10b">rbox 100 s +P0.5,0.5,0.5|qconvex Ga QG-0 >eg.10b.sphere.beyond </a></h3> + +<p>These are the facets that are not visible from the point. +Qhull will keep these facets.</p> + +<h3><a href="#how">»</a><a name="10c">rbox 100 s P0.5,0.5,0.5 | +qconvex PG Ga QG0 >eg.10c.sphere.horizon </a></h3> + +<p>These facets are the horizon facets; they border the visible +facets. The inside edges are the horizon ridges. Each horizon +ridge will form the base for a new facet.</p> + +<h3><a href="#how">»</a><a name="10d">rbox 100 s P0.5,0.5,0.5 | +qconvex Ga QV0 PgG >eg.10d.sphere.cone </a></h3> + +<p>This is the cone of points from the new point to the horizon +facets. Try combining this image with <tt>eg.10c.sphere.horizon</tt> +and <tt>eg.10a.sphere.visible</tt>. +</p> + +<h3><a href="#how">»</a><a name="10e">rbox 100 s P0.5,0.5,0.5 | +qconvex Ga >eg.10e.sphere.new</a></h3> + +<p>This is the convex hull after [0.5,0.5,0.5] has been added. +Note that in actual practice, the above sequence would never +happen. Unlike the randomized algorithms, Qhull always processes +a point that is furthest in an outside set. A point like +[0.5,0.5,0.5] would be one of the first points processed.</p> + +<h3><a href="#how">»</a><a name="14">rbox 100 s P0.5,0.5,0.5 | +qhull Ga QV0g Q0 >eg.14.sphere.corner</a></h3> + +<p>The '<a href="qh-optq.htm#QVn">QVn</a>', '<a +href="qh-optq.htm#QGn">QGn </a>' and '<a href="qh-optp.htm#Pdk">Pdk</a>' +options define good facets for Qhull. In this case '<a +href="qh-optq.htm#QVn">QV0</a>' defines the 0'th point +[0.5,0.5,0.5] as the good vertex, and '<a href="qh-optq.htm#Qg">Qg</a>' +tells Qhull to only build facets that might be part of a good +facet. This technique reduces output size in low dimensions. It +does not work with facet merging.</p> + +<h2><a href="#TOC">»</a>Triangulated output or joggled input</h2> + +<h3><a href="#joggle">»</a><a name="15a">rbox 500 W0 | qconvex QR0 Qc Gvp >eg.15a.surface</a></h3> + +<p>This is the convex hull of 500 points on the surface of +a cube. Note the large, non-simplicial facet for each face. +Qhull merges non-convex facets. + +<p>If the facets were not merged, Qhull +would report precision problems. For example, turn off facet merging +with option '<a href="qh-optq.htm#Q0">Q0</a>'. Qhull may report concave +facets, flipped facets, or other precision errors: +<blockquote> +rbox 500 W0 | qhull QR0 Q0 +</blockquote> + +<p> +<h3><a href="#joggle">»</a><a name="15b">rbox 500 W0 | qconvex QR0 Qt Qc Gvp >eg.15b.triangle</a></h3> + +<p>Like the previous examples, this is the convex hull of 500 points on the +surface of a cube. Option '<a href="qh-optq.htm#Qt">Qt</a>' triangulates the +non-simplicial facets. Triangulated output is +particularly helpful for Delaunay triangulations. + +<p> +<h3><a href="#joggle">»</a><a name="15c">rbox 500 W0 | qconvex QR0 QJ5e-2 Qc Gvp >eg.15c.joggle</a></h3> + +<p>This is the convex hull of 500 joggled points on the surface of +a cube. The option '<a href="qh-optq.htm#QJn">QJ5e-2</a>' +sets a very large joggle to make the effect visible. Notice +that all of the facets are triangles. If you rotate the cube, +you'll see red-yellow lines for coplanar points. +<p> +With option '<a href="qh-optq.htm#QJn">QJ</a>', Qhull joggles the +input to avoid precision problems. It adds a small random number +to each input coordinate. If a precision +error occurs, it increases the joggle and tries again. It repeats +this process until no precision problems occur. +<p> +Joggled input is a simple solution to precision problems in +computational geometry. Qhull can also merge facets to handle +precision problems. See <a href="qh-impre.htm#joggle">Merged facets or joggled input</a>. + +<h2><a href="#TOC">»</a>Delaunay and Voronoi diagrams</h2> + +<h3><a href="#delaunay">»</a><a name="17a">qdelaunay Qt +<eg.data.17 GnraD2 >eg.17a.delaunay.2</a></h3> + +<p> +The input file, <tt>eg.data.17</tt>, consists of a square, 15 random +points within the outside half of the square, and 6 co-circular +points centered on the square. + +<p>The Delaunay triangulation is the triangulation with empty +circumcircles. The input for this example is unusual because it +includes six co-circular points. Every triangular subset of these +points has the same circumcircle. Option '<a href="qh-optq.htm#Qt">Qt</a>' +triangulates the co-circular facet.</p> + +<h3><a href="#delaunay">»</a><a name="17b">qdelaunay <eg.data.17 +GnraD2 >eg.17b.delaunay.2i</a></h3> + +<p>This is the same example without triangulated output ('<a href="qh-optq.htm#Qt">Qt</a>'). qdelaunay +merges the non-unique Delaunay triangles into a hexagon.</p> + +<h3><a href="#delaunay">»</a><a name="17c">qdelaunay <eg.data.17 +Ga >eg.17c.delaunay.2-3 </a></h3> + +<p>This is how Qhull generated both diagrams. Use Geomview's +'obscure' menu to turn off normalization, and Geomview's +'cameras' menu to turn off perspective. Then load this <a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html">object</a> +with one of the previous diagrams.</p> + +<p>The points are lifted to a paraboloid by summing the squares +of each coordinate. These are the light blue points. Then the +convex hull is taken. That's what you see here. If you look up +the Z-axis, you'll see that points and edges coincide.</p> + +<h3><a href="#delaunay">»</a><a name="17d">qvoronoi QJ +<eg.data.17 Gna >eg.17d.voronoi.2</a></h3> + +<p>The Voronoi diagram is the dual of the Delaunay triangulation. +Here you see the original sites and the Voronoi vertices. +Notice the each +vertex is equidistant from three sites. The edges indicate the +Voronoi region for a site. Qhull does not draw the unbounded +edges. Instead, it draws extra edges to close the unbounded +Voronoi regions. You may find it helpful to enclose the input +points in a square. You can compute the unbounded +rays from option '<a href="qh-optf.htm#Fo2">Fo</a>'. +</p> + +<p>Instead +of triangulated output ('<a href="qh-optq.htm#Qt">Qt</a>'), this +example uses joggled input ('<a href="qh-optq.htm#QJn">QJ</a>'). +Normally, you should use neither 'QJ' nor 'Qt' for Voronoi diagrams. + +<h3><a href="#delaunay">»</a><a name="17e">qvoronoi <eg.data.17 +Gna >eg.17e.voronoi.2i </a></h3> + +<p>This looks the same as the previous diagrams, but take a look +at the data. Run 'qvoronoi p <eg/eg.data.17'. This prints +the Voronoi vertices. + +<p>With 'QJ', there are four nearly identical Voronoi vertices +within 10^-11 of the origin. Option 'QJ' joggled the input. After the joggle, +the cocircular +input sites are no longer cocircular. The corresponding Voronoi vertices are +similar but not identical. + +<p>This example does not use options 'Qt' or 'QJ'. The cocircular +input sites define one Voronoi vertex near the origin. </p> + +<p>Option 'Qt' would triangulate the corresponding Delaunay region into +four triangles. Each triangle is assigned the same Voronoi vertex.</p> + +<h3><a href="#delaunay">»</a><a name="17f"> rbox c G0.1 d | +qdelaunay Gt Qz <eg.17f.delaunay.3 </a></h3> + +<p>This is the 3-d Delaunay triangulation of a small cube inside +a prism. Since the outside ridges are transparent, it shows the +interior of the outermost facets. If you slice open the +triangulation with Geomview's ginsu, you will see that the innermost +facet is a cube. Note the use of '<a href="qh-optq.htm#Qz">Qz</a>' +to add a point "at infinity". This avoids a degenerate +input due to cospherical points.</p> + +<h3><a href="#delaunay">»</a><a name="18a">rbox 10 D2 d | qdelaunay +Qu G >eg.18a.furthest.2-3 </a></h3> + +<p>The furthest-site Voronoi diagram contains Voronoi regions for +points that are <i>furthest </i>from an input site. It is the +dual of the furthest-site Delaunay triangulation. You can +determine the furthest-site Delaunay triangulation from the +convex hull of the lifted points (<a href="#17c">eg.17c.delaunay.2-3</a>). +The upper convex hull (blue) generates the furthest-site Delaunay +triangulation. </p> + +<h3><a href="#delaunay">»</a><a name="18b">rbox 10 D2 d | qdelaunay +Qu Pd2 G >eg.18b.furthest-up.2-3</a></h3> + +<p>This is the upper convex hull of the preceding example. The +furthest-site Delaunay triangulation is the projection of the +upper convex hull back to the input points. The furthest-site +Voronoi vertices are the circumcenters of the furthest-site +Delaunay triangles. </p> + +<h3><a href="#delaunay">»</a><a name="18c">rbox 10 D2 d | qvoronoi +Qu Gv >eg.18c.furthest.2</a></h3> + +<p>This shows an incomplete furthest-site Voronoi diagram. It +only shows regions with more than two vertices. The regions are +artificially truncated. The actual regions are unbounded. You can +print the regions' vertices with 'qvoronoi Qu <a +href="qh-opto.htm#o">o</a>'. </p> + +<p>Use Geomview's 'obscure' menu to turn off normalization, and +Geomview's 'cameras' menu to turn off perspective. Then load this +with the upper convex hull.</p> + +<h3><a href="#delaunay">»</a><a name="19">rbox 10 D3 | qvoronoi QV5 +p | qconvex G >eg.19.voronoi.region.3 </a></h3> + +<p>This shows the Voronoi region for input site 5 of a 3-d +Voronoi diagram.</p> + +<h2><a href="#TOC">»</a>Facet merging for imprecision</h2> + +<h3><a href="#merge">»</a><a name="20">rbox r s 20 Z1 G0.2 | +qconvex G >eg.20.cone </a></h3> + +<p>There are two things unusual about this <a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/cone.html">cone</a>. +One is the large flat disk at one end and the other is the +rectangles about the middle. That's how the points were +generated, and if those points were exact, this is the correct +hull. But <tt>rbox</tt> used floating point arithmetic to +generate the data. So the precise convex hull should have been +triangles instead of rectangles. By requiring convexity, Qhull +has recovered the original design.</p> + +<h3><a href="#merge">»</a><a name="21a">rbox 200 s | qhull Q0 +R0.01 Gav Po >eg.21a.roundoff.errors </a></h3> + +<p>This is the convex hull of 200 cospherical points with +precision errors ignored ('<a href="qh-optq.htm#Q0">Q0</a>'). To +demonstrate the effect of roundoff error, we've added a random +perturbation ('<a href="qh-optc.htm#Rn">R0.01</a>') to every +distance and hyperplane calculation. Qhull, like all other convex +hull algorithms with floating point arithmetic, makes +inconsistent decisions and generates wildly wrong results. In +this case, one or more facets are flipped over. These facets have +the wrong color. You can also turn on 'normals' in Geomview's +appearances menu and turn off 'facing normals'. There should be +some white lines pointing in the wrong direction. These +correspond to flipped facets. </p> + +<p>Different machines may not produce this picture. If your +machine generated a long error message, decrease the number of +points or the random perturbation ('<a href="qh-optc.htm#Rn">R0.01</a>'). +If it did not report flipped facets, increase the number of +points or perturbation.</p> + +<h3><a href="#merge">»</a><a name="21b">rbox 200 s | qconvex Qc +R0.01 Gpav >eg.21b.roundoff.fixed </a></h3> + +<p>Qhull handles the random perturbations and returns an +imprecise <a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/fixed.html">sphere</a>. +In this case, the output is a weak approximation to the points. +This is because a random perturbation of '<a +href="qh-optc.htm#Rn">R0.01 </a>' is equivalent to losing all but +1.8 digits of precision. The outer planes float above the points +because Qhull needs to allow for the maximum roundoff error. </p> +<p> +If you start with a smaller random perturbation, you +can use joggle ('<a href="qh-optq.htm#QJn">QJn</a>') to avoid +precision problems. You need to set <i>n</i> significantly +larger than the random perturbation. For example, try +'rbox 200 s | qconvex Qc R1e-4 QJ1e-1'. + +<h3><a href="#merge">»</a><a name="22a">rbox 1000 s| qconvex C0.01 +Qc Gcrp >eg.22a.merge.sphere.01</a></h3> + +<h3><a href="#merge">»</a><a name="22b">rbox 1000 s| qconvex +C-0.01 Qc Gcrp >eg.22b.merge.sphere.-01</a></h3> + +<h3><a href="#merge">»</a><a name="22c">rbox 1000 s| qconvex C0.05 +Qc Gcrpv >eg.22c.merge.sphere.05</a></h3> + +<h3><a href="#merge">»</a><a name="22d">rbox 1000 s| qconvex +C-0.05 Qc Gcrpv >eg.22d.merge.sphere.-05</a></h3> + +<p>The next four examples compare post-merging and pre-merging ('<a +href="qh-optc.htm#Cn2">Cn</a>' vs. '<a href="qh-optc.htm#Cn">C-n</a>'). +Qhull uses '-' as a flag to indicate pre-merging.</p> + +<p>Post-merging happens after the convex hull is built. During +post-merging, Qhull repeatedly merges an independent set of +non-convex facets. For a given set of parameters, the result is +about as good as one can hope for.</p> + +<p>Pre-merging does the same thing as post-merging, except that +it happens after adding each point to the convex hull. With +pre-merging, Qhull guarantees a convex hull, but the facets are +wider than those from post-merging. If a pre-merge option is not +specified, Qhull handles machine round-off errors.</p> + +<p>You may see coplanar points appearing slightly outside +the facets of the last example. This is becomes Geomview moves +line segments forward toward the viewer. You can avoid the +effect by setting 'lines closer' to '0' in Geomview's camera menu. + +<h3><a href="#merge">»</a><a name="23">rbox 1000 | qconvex s +Gcprvah C0.1 Qc >eg.23.merge.cube</a></h3> + +<p>Here's the 3-d imprecise cube with all of the Geomview +options. There's spheres for the vertices, radii for the coplanar +points, dots for the interior points, hyperplane intersections, +centrums, and inner and outer planes. The radii are shorter than +the spheres because this uses post-merging ('<a href="qh-optc.htm#Cn2">C0.1</a>') +instead of pre-merging. + +<h2><a href="#TOC">»</a>4-d objects</h2> + +<p>With Qhull and Geomview you can develop an intuitive sense of +4-d surfaces. When you get into trouble, think of viewing the +surface of a 3-d sphere in a 2-d plane.</p> + +<h3><a href="#4d">»</a><a name="24">rbox 5000 D4 | qconvex s GD0v +Pd0:0.5 C-0.02 C0.1 >eg.24.merge.cube.4d-in-3d</a></h3> + +<p>Here's one facet of the imprecise cube in 4-d. It's projected +into 3-d (the '<a href="qh-optg.htm#GDn">GDn</a>' option drops +dimension n). Each ridge consists of two triangles between this +facet and the neighboring facet. In this case, Geomview displays +the topological ridges, i.e., as triangles between three +vertices. That is why the cube looks lopsided.</p> + +<h3><a href="#4d">»</a><a name="30">rbox 5000 D4 | qconvex s +C-0.02 C0.1 Gh >eg.30.4d.merge.cube </a></h3> + +<p><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/4dcube.html">Here</a> +is the equivalent in 4-d of the imprecise <a href="#06">square</a> +and imprecise <a href="#23">cube</a>. It's the imprecise convex +hull of 5000 random points in a hypercube. It's a full 4-d object +so Geomview's <tt>ginsu </tt>does not work. If you view it in +Geomview, you'll be inside the hypercube. To view 4-d objects +directly, either load the <tt>4dview</tt> module or the <tt>ndview +</tt>module. For <tt>4dview</tt>, you must have started Geomview +in the same directory as the object. For <tt>ndview</tt>, +initialize a set of windows with the prefab menu, and load the +object through Geomview. The <tt>4dview</tt> module includes an +option for slicing along any hyperplane. If you do this in the x, +y, or z plane, you'll see the inside of a hypercube.</p> + +<p>The '<a href="qh-optg.htm#Gh">Gh</a>' option prints the +geometric intersections between adjacent facets. Note the strong +convexity constraint for post-merging ('<a href="qh-optc.htm#Cn2">C0.1</a>'). +It deletes the small facets.</p> + +<h3><a href="#4d">»</a><a name="31">rbox 20 D3 | qdelaunay G +>eg.31.4d.delaunay </a></h3> + +<p>The Delaunay triangulation of 3-d sites corresponds to a 4-d +convex hull. You can't see 4-d directly but each facet is a 3-d +object that you can project to 3-d. This is exactly the same as +projecting a 2-d facet of a soccer ball onto a plane.</p> + +<p>Here we see all of the facets together. You can use Geomview's +<tt>ndview</tt> to look at the object from several directions. +Try turning on edges in the appearance menu. You'll notice that +some edges seem to disappear. That's because the object is +actually two sets of overlapping facets.</p> + +<p>You can slice the object apart using Geomview's <tt>4dview</tt>. +With <tt>4dview</tt>, try slicing along the w axis to get a +single set of facets and then slice along the x axis to look +inside. Another interesting picture is to slice away the equator +in the w dimension.</p> + +<h3><a href="#4d">»</a><a name="32">rbox 30 s D4 | qconvex s G +Pd0d1d2D3</a></h3> + +<p>This is the positive octant of the convex hull of 30 4-d +points. When looking at 4-d, it's easier to look at just a few +facets at once. If you picked a facet that was directly above +you, then that facet looks exactly the same in 3-d as it looks in +4-d. If you pick several facets, then you need to imagine that +the space of a facet is rotated relative to its neighbors. Try +Geomview's <tt>ndview</tt> on this example.</p> + +<h2><a href="#TOC">»</a>Halfspace intersections</h2> + +<h3><a href="#half">»</a><a name="33a">rbox 10 r s Z1 G0.3 | +qconvex G >eg.33a.cone </a></h3> + +<h3><a href="#half">»</a><a name="33b">rbox 10 r s Z1 G0.3 | +qconvex FV n | qhalf G >eg.33b.cone.dual</a></h3> + +<h3><a href="#half">»</a><a name="33c">rbox 10 r s Z1 G0.3 | +qconvex FV n | qhalf Fp | qconvex G >eg.33c.cone.halfspace</a></h3> + +<p>These examples illustrate halfspace intersection. The first +picture is the convex hull of two 20-gons plus an apex. The +second picture is the dual of the first. Try loading <a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/half.html">both</a> +at once. Each vertex of the second picture corresponds to a facet +or halfspace of the first. The vertices with four edges +correspond to a facet with four neighbors. Similarly the facets +correspond to vertices. A facet's normal coefficients divided by +its negative offset is the vertice's coordinates. The coordinates +are the intersection of the original halfspaces. </p> + +<p>The third picture shows how Qhull can go back and forth +between equivalent representations. It starts with a cone, +generates the halfspaces that define each facet of the cone, and +then intersects these halfspaces. Except for roundoff error, the +third picture is a duplicate of the first. </p> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home +page for Qhull</a> <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual: Table of Contents</a><br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To: </b><a href="#TOC">Qhull examples: Table of Contents</a> (please wait +while loading)<br> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-faq.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-faq.htm new file mode 100644 index 0000000000000000000000000000000000000000..5e52d10ab0861651d6e8afecc94aa531884ae64d --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-faq.htm @@ -0,0 +1,1503 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<meta name="GENERATOR" content="Microsoft FrontPage 2.0"> +<title>Qhull FAQ</title> +<!-- Navigation links +NOTE -- verify all links by 'grep href=' 'grep name=' add # 'sort /+7' +<base href> does not work since #TOC is relative to base instead of doc +--> +</head> + +<body> + +<p><a name="TOP"><b>Up:</b></a> <a + href="http://www.qhull.org">Home page</a> for Qhull +(http://www.qhull.org)<br> +<b>Up:</b> <A href="index.htm#TOC">Qhull manual</A>: Table of Contents<br> +<b>To:</b> <A href="qh-quick.htm#programs">Programs</A> +• <A href="qh-quick.htm#options">Options</A> +• <A href="qh-opto.htm#output">Output</A> +• <A href="qh-optf.htm#format">Formats</A> +• <A href="qh-optg.htm#geomview">Geomview</A> +• <A href="qh-optp.htm#print">Print</A> +• <A href="qh-optq.htm#qhull">Qhull</A> +• <A href="qh-optc.htm#prec">Precision</A> +• <A href="qh-optt.htm#trace">Trace</A> <br> +<b>To:</b> <A href="#TOC">FAQ: Table of Contents</A> (please +wait while loading) <br> + +<hr> +<!-- Main text of document --> +<h1><a + href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/4dcube.html"><IMG + align=middle alt ="[4-d cube]" + height=100 src="qh--4d.gif" width=100 ></a> Frequently Asked Questions about Qhull</h1><!-- +<p><i>This is the most recent FAQ. It was updated Sept. 13, 1999.</i> +--> +<p>If your question does not appear here, see: </p> + +<ul> + <li><a href="http://www.qhull.org/news">News</a> about Qhull + <li><A href="index.htm#TOC">Qhull manual:</A> table of contents + <li><A href="../README.txt">Installation</A> instructions for Qhull and rbox + + <li><A href="mailto:qhull@qhull.org">Send e-mail</A> to + qhull@qhull.org + <li><A href="mailto:qhull_bug@qhull.org">Report bugs</A> + to qhull_bug@qhull.org </li> +</ul> + +<p>Qhull is a general dimension code for computing convex hulls, +Delaunay triangulations, halfspace intersections about a point, +Voronoi diagrams, furthest-site Delaunay triangulations, and +furthest-site Voronoi diagrams. These structures have +applications in science, engineering, statistics, and +mathematics. For a detailed introduction, see O'Rourke [<A + href="index.htm#orou94" >'94</A>], <i>Computational Geometry in C</i>. +</p> + +<p>There are separate programs for each application of +Qhull. These programs disable experimental and inappropriate +options. If you prefer, you may use Qhull directly. All programs +run the same code. + +<p>Version 3.1 added triangulated output ('<A href="qh-optq.htm#Qt">Qt</A>'). +It should be used for Delaunay triangulations instead of +using joggled input ('<A href="qh-optq.htm#QJn">QJ</A>'). + +<p><i>Brad Barber, Cambridge MA, +2003/12/30 <!-- +--> </i></p> + +<p><b>Copyright © 1998-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h2><A href="#TOP">»</A><a name="TOC">FAQ: Table of Contents </a></h2> + +<p>Within each category, the most recently asked questions are +first. +<ul> + <li>Startup questions <ul> + <li><A href="#console">How</A> do I run Qhull from Windows? + <li><A href="#input">How</A> do I enter points for Qhull? + <li><A href="#learn">How</A> do I learn to use Qhull?</li> + </ul> + <li>Convex hull questions<ul> + <li><A href="#area">How</A> do I report just the area and volume of a + convex hull? + <li><A href="#extra">Why</A> are there extra points in a 4-d or higher + convex hull? + <li><A href="#dup">How</A> do I report duplicate + vertices? </li> + </ul> + <li>Delaunay and Voronoi diagram questions<ul> + <li><a href="#maxsphere">How</a> do I get the radii of the empty + spheres for each Voronoi vertex? + + <li><A href="#flat">How</A> do I get rid of nearly flat Delaunay + triangles? + + <li><A href="#square">What</A> is the Voronoi diagram of a square? + + <li><A href="#vclosest">How</A> do I find the Delaunay triangle or Voronoi + region that is closest to a point? + + <li><A href="#mesh">How</A> do I compute the Delaunay triangulation of a + non-convex object? + + <li><A href="#mesh">How</A> do I mesh a volume from a set of triangulated + surface points? + + <li><A href="#constrained">Can</A> Qhull produce a triangular mesh for an + object? + + <li><A href="#dridges">For</A> 3-d Delaunay triangulations, how do I + report the triangles of each tetrahedron? + <li><A href="#vsphere">How</A> do I construct the Voronoi diagram of + cospherical points? + <li><A href="#3dd">How</A> do I construct a 3-d Delaunay triangulation? + <li><A href="#2d">How</A> do I get the triangles for a 2-d Delaunay + triangulation and the vertices of its Voronoi diagram? + <li><A href="#rays">Can</A> Qhull compute the unbounded rays of the + Voronoi diagram? + <li><A href="#big">Can </A>Qhull triangulate a + hundred 16-d points?</li> + </ul> + <li>Approximation questions<ul> + <li><A href="#simplex">How</A> do I approximate data + with a simplex?</li> + </ul> + <li>Halfspace questions<ul> + <li><A href="#halfspace">How</A> do I compute the + intersection of halfspaces with Qhull?</li> + </ul> + <li><a name="library">Qhull library</a> questions<ul> + <li><A href="#math">Is</A> Qhull available for Mathematica, Matlab, or + Maple? + + <li><A href="#ridges">Why</A> are there too few ridges? + <li><A href="#call">Can</A> Qhull use coordinates without placing them in + a data file? + <li><A href="#size">How</A> large are Qhull's data structures? + <li><A href="#inc">Can</A> Qhull construct convex hulls and Delaunay + triangulations one point at a time? + <li><A href="#ridges2">How</A> do I visit the ridges of a Delaunay + triangulation? + <li><A href="#listd">How</A> do I visit the Delaunay facets? + <LI><A + href="#outside">When</A> is a point outside or inside a facet? + <li><A href="#closest">How</A> do I find the facet that is closest to a + point? + <li><A href="#vclosest">How</A> do I find the Delaunay triangle or Voronoi + region that is closest to a point? + <li><A href="#vertices">How</A> do I list the vertices? + <li><A href="#test">How</A> do I test code that uses the Qhull library? + <li><A href="#orient">When</A> I compute a plane + equation from a facet, I sometimes get an + outward-pointing normal and sometimes an + inward-pointing normal</li> + </ul> + </li> +</ul> + +<hr> + +<h2><A href="#TOC">»</A><a name="startup">Startup</a> questions</h2> + +<h4><A href="#TOC">»</A><a name="console">How</a> do I run Qhull +from Windows?</h4><blockquote> + +<p>Qhull is a console program. You will first need a DOS window +(i.e., a "DOS prompt"). You can double click on +'eg\Qhull-go.bat'. It loads 'doskey' to simplify rerunning qhull +and rbox. </p> + +<blockquote><ul> + <li>Type 'qconvex', 'qdelaunay', 'qhalf', 'qvoronoi, + 'qhull', and 'rbox' for a synopsis of each program. + + <li>Type 'rbox c D2 | qconvex s i' to compute the + convex hull of a square. + + <li>Type 'rbox c D2 | qconvex s i TO results.txt' to + write the results to the file 'results.txt'. A summary is still printed on + the the console. + + <li>Type 'rbox c D2' to see the input format for + qconvex. + + <li>Type 'qconvex < data.txt s i TO results.txt' to + read input data from 'data.txt'. + + <li>If you want to enter data by hand, type 'qconvex s i TO + results.txt' to read input data from the console. Type in + the numbers and end with a ctrl-D. </li> +</ul></blockquote> + +</blockquote><h4><A href="#TOC">»</A><a name="input">How</a> do I enter +points for Qhull?</h4><blockquote> + +<p>Qhull takes its data from standard input. For example, create +a file named 'data.txt' with the following contents: </p> + +<blockquote> + <pre> +2 #sample 2-d input +5 #number of points +1 2 #coordinates of points +-1.1 3 +3 2.2 +4 5 +-10 -10 +</pre> +</blockquote> + +<p>Then call qconvex with 'qconvex < data.txt'. It will print a +summary of the convex hull. Use 'qconvex < data.txt o' to print +the vertices and edges. See also <A href="index.htm#input">input +format</A>. </p> + +<p>You can generate sample data with rbox, e.g., 'rbox 10' +generates 10 random points in 3-d. Use a pipe ('|') to run rbox +and qhull together, e.g., </p> + +<blockquote> + <p>rbox c | qconvex o </p> +</blockquote> + +<p>computes the convex hull of a cube. </p> + +</blockquote><h4><A href="#TOC">»</A><a name="learn">How</a> do I learn to +use Qhull?</h4><blockquote> + +<p>First read: </p> + +<ul> + <li><A href="index.htm">Introduction</A> to Qhull + <li><A href="index.htm#when">When</A> to use Qhull + <li><A href="qconvex.htm">qconvex</A> -- convex hull + <li><A href="qdelaun.htm">qdelaunay</A> -- Delaunay triangulation + <li><A href="qhalf.htm">qhalf</A> -- half-space intersection about a point + + <li><A href="qvoronoi.htm">qvoronoi</A> -- Voronoi diagram + <li><A href="rbox.htm">Rbox</A>, for sample inputs + <li><A href="qh-eg.htm">Examples</A> of Qhull</li> +</ul> + +<p>Look at Qhull's on-line documentation: </p> + +<ul> + <li>'qconvex' gives a synopsis of qconvex and its options + + <li>'rbox' lists all of the options for generating point + sets + <li>'qconvex - | more' lists the options for qconvex + <li>'qconvex .' gives a concise list of options + <li>'qdelaunay', 'qhalf', 'qvoronoi', and 'qhull' also have a synopsis and option list</li> +</ul> + +<p>Then try out the Qhull programs on small examples. </p> + +<ul> + <li>'rbox c' lists the vertices of a cube + <li>'rbox c | qconvex' is the convex hull of a cube + <li>'rbox c | qconvex o' lists the vertices and facets of + a cube + <li>'rbox c | qconvex Qt o' triangulates the cube + <li>'rbox c | qconvex QJ o' joggles the input and + triangulates the cube + <li>'rbox c D2 | qconvex' generates the convex hull of a + square + <li>'rbox c D4 | qconvex' generates the convex hull of a + hypercube + <li>'rbox 6 s D2 | qconvex p Fx' lists 6 random points in + a circle and lists the vertices of their convex hull in order + <li>'rbox c D2 c G2 | qdelaunay' computes the Delaunay + triangulation of two embedded squares. It merges the cospherical facets. + <li>'rbox c D2 c G2 | qdelaunay Qt' computes the Delaunay + triangulation of two embedded squares. It triangulates the cospherical facets. + <li>'rbox c D2 c G2 | qvoronoi o' computes the + corresponding Voronoi vertices and regions. + <li>'rbox c D2 c G2 | qvoronio Fv' shows the Voronoi diagram + for the previous example. Each line is one edge of the + diagram. The first number is 4, the next two numbers list + a pair of input sites, and the last two numbers list the + corresponding pair of Voronoi vertices. </li> +</ul> + +<p>Install <a href="http://www.geomview.org">Geomview</a> +if you are running SGI Irix, Solaris, SunOS, Linux, HP, IBM +RS/6000, DEC Alpha, or Next. You can then visualize the output of +Qhull. Qhull comes with Geomview <A href="qh-eg.htm">examples</A>. +</p> + +<p>Then try Qhull with a small example of your application. Work +out the results by hand. Then experiment with Qhull's options to +find the ones that you need. </p> + +<p>You will need to decide how Qhull should handle precision +problems. It can triangulate the output ('<A + href="qh-optq.htm#Qt" >Qt</A>'), joggle the input ('<A + href="qh-optq.htm#QJn" >QJ</A>'), or merge facets (the default). </p> + +<ul> + <li>With joggle, Qhull produces simplicial (i.e., + triangular) output by joggling the input. After joggle, + no points are cocircular or cospherical. + <li>With facet merging, Qhull produces a better + approximation and does not modify the input. + <li>With triangulated output, Qhull merges facets and triangulates + the result.</li> + <li>See <A href="qh-impre.htm#joggle">Merged facets or joggled input</A>. </li> +</ul> + +</blockquote> +<h2><A href="#TOC">»</A><a name="convex">Convex hull questions</a></h2> + +<h4><A href="#TOC">»</A><a name="area">How</a> do I report just the area + and volume of a convex hull?</h4><blockquote> + +Use option 'FS'. For example, + +<blockquote><pre> +C:\qhull>rbox 10 | qconvex FS +0 +2 2.192915621644613 0.2027867899638665 + +C:\qhull>rbox 10 | qconvex FA + +Convex hull of 10 points in 3-d: + + Number of vertices: 10 + Number of facets: 16 + +Statistics for: RBOX 10 | QCONVEX FA + + Number of points processed: 10 + Number of hyperplanes created: 28 + Number of distance tests for qhull: 44 + CPU seconds to compute hull (after input): 0 + Total facet area: 2.1929156 + Total volume: 0.20278679 +</pre></blockquote> + +</blockquote><h4><A href="#TOC">»</A><a name="extra">Why</a> are there extra +points in a 4-d or higher convex hull?</h4><blockquote> + +<p>You may see extra points if you use options '<A + href="qh-opto.htm#i" >i</A>' or '<A href="qh-optf.htm#Ft">Ft</A>' + without using triangulated output ('<A href="qh-optq.htm#Qt">Qt</A>'). +The extra points occur when a facet is non-simplicial (i.e., a +facet with more than <i>d</i> vertices). For example, Qhull +reports the following for one facet of the convex hull of a hypercube. +Option 'Pd0:0.5' returns the facet along the positive-x axis: </p> + +<blockquote> + <pre> +rbox c D4 | qconvex i Pd0:0.5 +12 +17 13 14 15 +17 13 12 14 +17 11 13 15 +17 14 11 15 +17 10 11 14 +17 14 12 8 +17 12 13 8 +17 10 14 8 +17 11 10 8 +17 13 9 8 +17 9 11 8 +17 11 9 13 +</pre> +</blockquote> + +<p>The 4-d hypercube has 16 vertices; so point "17" was +added by qconvex. Qhull adds the point in order to report a +simplicial decomposition of the facet. The point corresponds to +the "centrum" which Qhull computes to test for +convexity. </p> + +<p>Triangulate the output ('<A href="qh-optq.htm#Qt">Qt</A>') to avoid the extra points. +Since the hypercube is 4-d, each simplicial facet is a tetrahedron. +<blockquote> +<pre> +C:\qhull3.1>rbox c D4 | qconvex i Pd0:0.5 Qt +9 +9 13 14 15 +12 9 13 14 +9 11 13 15 +11 9 14 15 +9 10 11 14 +12 9 14 8 +9 12 13 8 +9 10 14 8 +10 9 11 8 +</pre> +</blockquote> + +<p>Use the '<A href="qh-optf.htm#Fv">Fv</A>' option to print the +vertices of simplicial and non-simplicial facets. For example, +here is the same hypercube facet with option 'Fv' instead of 'i': +</p> + +<blockquote> + <pre> +C:\qhull>rbox c D4 | qconvex Pd0:0.5 Fv +1 +8 9 10 12 11 13 14 15 8 +</pre> +</blockquote> + +<p>The coordinates of the extra point are printed with the '<A + href="qh-optf.htm#Ft" >Ft</A>' option. </p> + +<blockquote> + <pre> +rbox c D4 | qconvex Pd0:0.5 Ft +4 +17 12 3 + -0.5 -0.5 -0.5 -0.5 + -0.5 -0.5 -0.5 0.5 + -0.5 -0.5 0.5 -0.5 + -0.5 -0.5 0.5 0.5 + -0.5 0.5 -0.5 -0.5 + -0.5 0.5 -0.5 0.5 + -0.5 0.5 0.5 -0.5 + -0.5 0.5 0.5 0.5 + 0.5 -0.5 -0.5 -0.5 + 0.5 -0.5 -0.5 0.5 + 0.5 -0.5 0.5 -0.5 + 0.5 -0.5 0.5 0.5 + 0.5 0.5 -0.5 -0.5 + 0.5 0.5 -0.5 0.5 + 0.5 0.5 0.5 -0.5 + 0.5 0.5 0.5 0.5 + 0.5 0 0 0 +4 16 13 14 15 +4 16 13 12 14 +4 16 11 13 15 +4 16 14 11 15 +4 16 10 11 14 +4 16 14 12 8 +4 16 12 13 8 +4 16 10 14 8 +4 16 11 10 8 +4 16 13 9 8 +4 16 9 11 8 +4 16 11 9 13 +</pre> +</blockquote> + +</blockquote><h4><A href="#TOC">»</A><a name="dup">How</a> do I report +duplicate vertices?</h4><blockquote> + +<p>There's no direct way. You can use option +'<A href="qh-optf.htm#FP">FP</A>' to +report the distance to the nearest vertex for coplanar input +points. Select the minimum distance for a duplicated vertex, and +locate all input sites less than this distance. </p> + +<p>For Delaunay triangulations, all coplanar points are nearly +incident to a vertex. If you want a report of coincident input +sites, do not use option '<A href="qh-optq.htm#QJn">QJ</A>'. By +adding a small random quantity to each input coordinate, it +prevents coincident input sites. </p> + +</blockquote> +<h2><A href="#TOC">»</A><a name="delaunay">Delaunay and Voronoi +diagram questions</a></h2> + +<h4><A href="#TOC">»</A><a name="maxsphere">How</a> do I get the radii of the empty + spheres for each Voronoi vertex?</h4><blockquote> + +Use option '<A href="qh-optf.htm#Fi">Fi</A>' to list each bisector (i.e. Delaunay ridge). Then compute the +minimum distance for each Voronoi vertex. + +<p>There's other ways to get the same information. Let me know if you +find a better method. + +<h4><A href="#TOC">»</A><a name="flat">How</a> do I get rid of +nearly flat Delaunay triangles?</h4><blockquote> + +<p>Nearly flat triangles occur when boundary points are nearly +collinear or coplanar. They also occur for nearly coincident +points. Both events can easily occur when using joggle. For example +(rbox 10 W0 D2 | qdelaunay QJ Fa) lists the areas of the Delaunay +triangles of 10 points on the boundary of a square. Some of +these triangles are nearly flat. This occurs when one point +is joggled inside of two other points. In this case, nearly flat +triangles do not occur with triangulated output (rbox 10 W0 D2 | qdelaunay Qt Fa). + + +<p>Another example, (rbox c P0 P0 D2 | qdelaunay QJ Fa), computes the +areas of the Delaunay triangles for the unit square and two +instances of the origin. Four of the triangles have an area +of 0.25 while two have an area of 2.0e-11. The later are due to +the duplicated origin. With triangulated output (rbox c P0 P0 D2 | qdelaunay Qt Fa) +there are four triangles of equal area. + +<p>Nearly flat triangles also occur without using joggle. For +example, (rbox c P0 P0,0.4999999999 | qdelaunay Fa), computes +the areas of the Delaunay triangles for the unit square, +a nearly collinear point, and the origin. One triangle has an +area of 3.3e-11. + +<p>Unfortunately, none of Qhull's merging options remove nearly +flat Delaunay triangles due to nearly collinear or coplanar boundary +points. +The merging options concern the empty circumsphere +property of Delaunay triangles. This is independent of the area of +the Delaunay triangles. Qhull does handle nearly coincident points. + +<p>You can handle collinear or coplanar boundary points by +enclosing the points in a box. For example, +(rbox c P0 P0,0.4999999999 c G1 | qdelaunay Fa), surrounds the +previous points with [(1,1), (1,-1), (-1,-1), (-1, 1)]. +Its Delaunay triangulation does not include a +nearly flat triangle. The box also simplifies the graphical +output from Qhull. + +<p>Without joggle, Qhull lists coincident points as "coplanar" +points. For example, (rbox c P0 P0 D2 | qdelaunay Fa), ignores +the duplicated origin and lists four triangles of size 0.25. +Use 'Fc' to list the coincident points (e.g., +rbox c P0 P0 D2 | qdelaunay Fc). + +<p>There is no easy way to determine coincident points with joggle. +Joggle removes all coincident, cocircular, and cospherical points +before running Qhull. Instead use facet merging (the default) +or triangulated output ('<A href="qh-optq.htm#Qt">Qt</A>'). + + +</blockquote> +<h4><A href="#TOC">»</A><a name="square">What</a> is the Voronoi diagram + of a square?</h4><blockquote> + +<p> +Consider a square, +<blockquote><pre> +C:\qhull>rbox c D2 +2 RBOX c D2 +4 + -0.5 -0.5 + -0.5 0.5 + 0.5 -0.5 + 0.5 0.5 +</pre></blockquote> + +<p>There's two ways to compute the Voronoi diagram: with facet merging +or with joggle. With facet merging, the +result is: + +<blockquote><pre> +C:\qhull>rbox c D2 | qvoronoi Qz + +Voronoi diagram by the convex hull of 5 points in 3-d: + + Number of Voronoi regions and at-infinity: 5 + Number of Voronoi vertices: 1 + Number of facets in hull: 5 + +Statistics for: RBOX c D2 | QVORONOI Qz + + Number of points processed: 5 + Number of hyperplanes created: 7 + Number of distance tests for qhull: 8 + Number of merged facets: 1 + Number of distance tests for merging: 29 + CPU seconds to compute hull (after input): 0 + +C:\qhull>rbox c D2 | qvoronoi Qz o +2 +2 5 1 +-10.101 -10.101 + 0 0 +2 0 1 +2 0 1 +2 0 1 +2 0 1 +0 + +C:\qhull>rbox c D2 | qvoronoi Qz Fv +4 +4 0 1 0 1 +4 0 2 0 1 +4 1 3 0 1 +4 2 3 0 1 +</pre></blockquote> + +<p>There is one Voronoi vertex at the origin and rays from the origin +along each of the coordinate axes. +The last line '4 2 3 0 1' means that there is +a ray that bisects input points #2 and #3 from infinity (vertex 0) to +the origin (vertex 1). +Option 'Qz' adds an artificial point since the input is cocircular. +Coordinates -10.101 indicate the +vertex at infinity. + +<p>With triangulated output, the Voronoi vertex is +duplicated: + +<blockquote><pre> +C:\qhull3.1>rbox c D2 | qvoronoi Qt Qz + +Voronoi diagram by the convex hull of 5 points in 3-d: + + Number of Voronoi regions and at-infinity: 5 + Number of Voronoi vertices: 2 + Number of triangulated facets: 1 + +Statistics for: RBOX c D2 | QVORONOI Qt Qz + + Number of points processed: 5 + Number of hyperplanes created: 7 + Number of facets in hull: 6 + Number of distance tests for qhull: 8 + Number of distance tests for merging: 33 + Number of distance tests for checking: 30 + Number of merged facets: 1 + CPU seconds to compute hull (after input): 0.05 + +C:\qhull3.1>rbox c D2 | qvoronoi Qt Qz o +2 +3 5 1 +-10.101 -10.101 + 0 0 + 0 0 +3 2 0 1 +2 1 0 +2 2 0 +3 2 0 1 +0 + +C:\qhull3.1>rbox c D2 | qvoronoi Qt Qz Fv +4 +4 0 2 0 2 +4 0 1 0 1 +4 1 3 0 1 +4 2 3 0 2 +</pre></blockquote> + + +<p>With joggle, the input is no longer cocircular and the Voronoi vertex is +split into two: + +<blockquote><pre> +C:\qhull>rbox c D2 | qvoronoi Qt Qz + +C:\qhull>rbox c D2 | qvoronoi QJ o +2 +3 4 1 +-10.101 -10.101 +-4.71511718558304e-012 -1.775812830118184e-011 +9.020340030474472e-012 -4.02267108512433e-012 +2 0 1 +3 2 1 0 +3 2 0 1 +2 2 0 + +C:\qhull>rbox c D2 | qvoronoi QJ Fv +5 +4 0 2 0 1 +4 0 1 0 1 +4 1 2 1 2 +4 1 3 0 2 +4 2 3 0 2 +</pre></blockquote> + +<p>Note that the Voronoi diagram includes the same rays as + before plus a short edge between the two vertices.</p> + + +</blockquote><h4><A href="#TOC">»</A><a name="mesh">How</a> do I compute +the Delaunay triangulation of a non-convex object?</h4><blockquote> + +<p>A similar question is +"How do I mesh a volume from a set of triangulated surface points?" + +<p>This is an instance of the constrained Delaunay Triangulation +problem. Qhull does not handle constraints. The boundary of the +Delaunay triangulation is always convex. But if the input set +contains enough points, the triangulation will include the +boundary. The number of points needed depends on the input. + +<p>Shewchuk has developed a theory of constrained Delaunay triangulations. +See his +<a href="http://www.cs.cmu.edu/~jrs/jrspapers.html#cdt">paper</a> at the +1998 Computational Geometry Conference. Using these ideas, constraints +could be added to Qhull. They would have many applications. + +<p>There is a large literature on mesh generation and many commercial +offerings. For pointers see +<a href="http://www.andrew.cmu.edu/user/sowen/mesh.html">Owen's Meshing +Research Corner</a> and +<a href="http://www-users.informatik.rwth-aachen.de/~roberts/meshgeneration.html">Schneiders' +Finite Element Mesh Generation page</a>.</p> + +</blockquote><h4><A href="#TOC">»</A><a name="constrained">Can</a> Qhull +produce a triangular mesh for an object?</h4><blockquote> + +<p>Yes for convex objects, no for non-convex objects. For +non-convex objects, it triangulates the concavities. Unless the +object has many points on its surface, triangles may cross the +surface. </p> + +</blockquote><h4><A href="#TOC">»</A><a name="dridges">For</a> 3-d Delaunay +triangulations, how do I report the triangles of each +tetrahedron?</h4><blockquote> + +<p>For points in general position, a 3-d Delaunay triangulation +generates tetrahedron. Each face of a tetrahedron is a triangle. +For example, the 3-d Delaunay triangulation of random points on +the surface of a cube, is a cellular structure of tetrahedron. </p> + +<p>Use triangulated output ('qdelaunay Qt i') or joggled input ('qdelaunay QJ i') +to generate the Delaunay triangulation. +Option 'i' reports each tetrahedron. The triangles are +every combination of 3 vertices. Each triangle is a +"ridge" of the Delaunay triangulation. </p> + +<p>For example, </p> + +<pre> + rbox 10 | qdelaunay Qt i + 14 + 9 5 8 7 + 0 9 8 7 + 5 3 8 7 + 3 0 8 7 + 5 4 8 1 + 4 6 8 1 + 2 9 5 8 + 4 2 5 8 + 4 2 9 5 + 6 2 4 8 + 9 2 0 8 + 2 6 0 8 + 2 4 9 1 + 2 6 4 1 +</pre> + +<p>is the Delaunay triangulation of 10 random points. Ridge 9-5-8 +occurs twice. Once for tetrahedron 9 5 8 7 and the other for +tetrahedron 2 9 5 8. </p> + +<p>You can also use the Qhull library to generate the triangles. +See "<A href="#ridges2">How</A> do I visit the ridges of a +Delaunay triangulation?"</p> + +</blockquote><h4><A href="#TOC">»</A><a name="vsphere">How</a> do I construct +the Voronoi diagram of cospherical points?</h4><blockquote> + +<p>Three-d terrain data can be approximated with cospherical +points. The Delaunay triangulation of cospherical points is the +same as their convex hull. If the points lie on the unit sphere, +the facet normals are the Voronoi vertices [via S. Fortune]. </p> + +<p>For example, consider the points {[1,0,0], [-1,0,0], [0,1,0], +...}. Their convex hull is: </p> + +<pre> +rbox d G1 | qconvex o +3 +6 8 12 + 0 0 -1 + 0 0 1 + 0 -1 0 + 0 1 0 + -1 0 0 + 1 0 0 +3 3 1 4 +3 1 3 5 +3 0 3 4 +3 3 0 5 +3 2 1 5 +3 1 2 4 +3 2 0 4 +3 0 2 5 +</pre> + +<p>The facet normals are: </p> + +<pre> +rbox d G1 | qconvex n +4 +8 +-0.5773502691896258 0.5773502691896258 0.5773502691896258 -0.5773502691896258 + 0.5773502691896258 0.5773502691896258 0.5773502691896258 -0.5773502691896258 +-0.5773502691896258 0.5773502691896258 -0.5773502691896258 -0.5773502691896258 + 0.5773502691896258 0.5773502691896258 -0.5773502691896258 -0.5773502691896258 + 0.5773502691896258 -0.5773502691896258 0.5773502691896258 -0.5773502691896258 +-0.5773502691896258 -0.5773502691896258 0.5773502691896258 -0.5773502691896258 +-0.5773502691896258 -0.5773502691896258 -0.5773502691896258 -0.5773502691896258 + 0.5773502691896258 -0.5773502691896258 -0.5773502691896258 -0.5773502691896258 +</pre> + +<p>If you drop the offset from each line (the last number), each +line is the Voronoi vertex for the corresponding facet. The +neighboring facets for each point define the Voronoi region for +each point. For example: </p> + +<pre> +rbox d G1 | qconvex FN +6 +4 7 3 2 6 +4 5 0 1 4 +4 7 4 5 6 +4 3 1 0 2 +4 6 2 0 5 +4 7 3 1 4 +</pre> + +<p>The Voronoi vertices {7, 3, 2, 6} define the Voronoi region +for point 0. Point 0 is [0,0,-1]. Its Voronoi vertices are </p> + +<pre> +-0.5773502691896258 0.5773502691896258 -0.5773502691896258 + 0.5773502691896258 0.5773502691896258 -0.5773502691896258 +-0.5773502691896258 -0.5773502691896258 -0.5773502691896258 + 0.5773502691896258 -0.5773502691896258 -0.5773502691896258 +</pre> + +<p>In this case, the Voronoi vertices are oriented, but in +general they are unordered. </p> + +<p>By taking the dual of the Delaunay triangulation, you can +construct the Voronoi diagram. For cospherical points, the convex +hull vertices for each facet, define the input sites for each +Voronoi vertex. In 3-d, the input sites are oriented. For +example: </p> + +<pre> +rbox d G1 | qconvex i +8 +3 1 4 +1 3 5 +0 3 4 +3 0 5 +2 1 5 +1 2 4 +2 0 4 +0 2 5 +</pre> + +<p>The convex hull vertices for facet 0 are {3, 1, 4}. So Voronoi +vertex 0 (i.e., [-0.577, 0.577, 0.577]) is the Voronoi vertex for +input sites {3, 1, 4} (i.e., {[0,1,0], [0,0,1], [-1,0,0]}). </p> + +</blockquote><h4><A href="#TOC">»</A><a name="3dd">How</a> do I construct a +3-d Delaunay triangulation?</h4><blockquote> + +<p>For 3-d Delaunay triangulations with cospherical input sites, +use triangulated output ('<A href="qh-optq.htm#Qt">Qt</A>') or +joggled input ('<A href="qh-optq.htm#QJn">QJ</A>'). Otherwise +option 'i' will +triangulate non-simplicial facets by adding a point to the facet. + +<p>If you want non-simplicial output for cospherical sites, use +option +'<A href="qh-optf.htm#Fv">Fv</A>' or '<A href="qh-opto.htm#o">o</A>'. +For option 'o', ignore the last coordinate. It is the lifted +coordinate for the corresponding convex hull in 4-d. + +<p>The following example is a cube +inside a tetrahedron. The 8-vertex facet is the cube. Ignore the +last coordinates. </p> + +<blockquote> + <pre> +C:\qhull>rbox r y c G0.1 | qdelaunay Fv +4 +12 20 44 + 0.5 0 0 0.3055555555555555 + 0 0.5 0 0.3055555555555555 + 0 0 0.5 0.3055555555555555 + -0.5 -0.5 -0.5 0.9999999999999999 + -0.1 -0.1 -0.1 -6.938893903907228e-018 + -0.1 -0.1 0.1 -6.938893903907228e-018 + -0.1 0.1 -0.1 -6.938893903907228e-018 + -0.1 0.1 0.1 -6.938893903907228e-018 + 0.1 -0.1 -0.1 -6.938893903907228e-018 + 0.1 -0.1 0.1 -6.938893903907228e-018 + 0.1 0.1 -0.1 -6.938893903907228e-018 + 0.1 0.1 0.1 -6.938893903907228e-018 +4 2 11 1 0 +4 10 1 0 3 +4 11 10 1 0 +4 2 9 0 3 +4 9 11 2 0 +4 7 2 1 3 +4 11 7 2 1 +4 8 10 0 3 +4 9 8 0 3 +5 8 9 10 11 0 +4 10 6 1 3 +4 6 7 1 3 +5 6 8 10 4 3 +5 6 7 10 11 1 +4 5 9 2 3 +4 7 5 2 3 +5 5 8 9 4 3 +5 5 6 7 4 3 +8 5 6 8 7 9 10 11 4 +5 5 7 9 11 2 +</pre> +</blockquote> + +<p>If you want simplicial output use options +'<A href="qh-optq.htm#Qt">Qt</A> <A + href="qh-optf.htm#Ft" >i</A>' or +'<A href="qh-optq.htm#QJn">QJ</A> <A + href="qh-optf.htm#Ft" >i</A>', e.g., +</p> + +<blockquote> + <pre> +rbox r y c G0.1 | qdelaunay Qt i +31 +2 11 1 0 +11 10 1 0 +9 11 2 0 +11 7 2 1 +8 10 0 3 +9 8 0 3 +10 6 1 3 +6 7 1 3 +5 9 2 3 +7 5 2 3 +9 8 10 11 +8 10 11 0 +9 8 11 0 +6 8 10 4 +8 6 10 3 +6 8 4 3 +6 7 10 11 +10 6 11 1 +6 7 11 1 +8 5 4 3 +5 8 9 3 +5 6 4 3 +6 5 7 3 +5 9 10 11 +8 5 9 10 +7 5 10 11 +5 6 7 10 +8 5 10 4 +5 6 10 4 +5 9 11 2 +7 5 11 2 +</pre> +</blockquote> + +</blockquote><h4><A href="#TOC">»</A><a name="2d">How</a> do I get the +triangles for a 2-d Delaunay triangulation and the vertices of +its Voronoi diagram?</h4><blockquote> + +<p>To compute the Delaunay triangles indexed by the indices of +the input sites, use </p> + +<blockquote> + <p>rbox 10 D2 | qdelaunay Qt i </p> +</blockquote> + +<p>To compute the Voronoi vertices and the Voronoi region for +each input site, use </p> + +<blockquote> + <p>rbox 10 D2 | qvoronoi o </p> +</blockquote> + +<p>To compute each edge ("ridge") of the Voronoi +diagram for each pair of adjacent input sites, use</p> + +<blockquote> + <p>rbox 10 D2 | qvoronoi Fv </p> +</blockquote> + +<p>To compute the area and volume of the Voronoi region for input site 5, +use </p> + +<blockquote> + <p>rbox 10 D2 | qvoronoi QV5 p | qconvex s FS </p> +</blockquote> + +<p>To compute the lines ("hyperplanes") that define the +Voronoi region for input site 5, use </p> + +<blockquote> + <p>rbox 10 D2 | qvoronoi QV5 p | qconvex n </p> +</blockquote> +or +<blockquote> + <p>rbox 10 D2 | qvoronoi QV5 Fi Fo</p> +</blockquote> + +<p>To list the extreme points of the input sites use </p> + +<blockquote> + <p>rbox 10 D2 | qdelaunay Fx </p> +</blockquote> + +<p>You will get the same point ids with </p> + +<blockquote> + <p>rbox 10 D2 | qconvex Fx </p> +</blockquote> + +</blockquote><h4><A href="#TOC">»</A><a name="rays">Can</a> Qhull compute the +unbounded rays of the Voronoi diagram?</h4><blockquote> + +<p>Use '<A href="qh-optf.htm#Fo2">Fo</A>' to compute the separating +hyperplanes for unbounded Voronoi regions. The corresponding ray +goes to infinity from the Voronoi vertices. If you enclose the +input sites in a large enough box, the outermost bounded regions +will represent the unbounded regions of the original points. </p> + +<p>If you do not box the input sites, you can identify the +unbounded regions. They list '0' as a vertex. Vertex 0 represents +"infinity". Each unbounded ray includes vertex 0 in +option '<A href="qh-optf.htm#Fv2">Fv</A>. See <A + href="qvoronoi.htm#graphics" >Voronoi graphics</A> and <A + href="qvoronoi.htm#notes" >Voronoi notes</A>. </p> + +</blockquote><h4><A href="#TOC">»</A><a name="big">Can </a>Qhull triangulate +a hundred 16-d points?</h4><blockquote> + +<p>No. This is an immense structure. A triangulation of 19, 16-d +points has 43 simplices. If you add one point at a time, the +triangulation increased as follows: 43, 189, 523, 1289, 2830, +6071, 11410, 20487. The last triangulation for 26 points used 13 +megabytes of memory. When Qhull uses virtual memory, it becomes +too slow to use. </p> + +</blockquote> +<h2><A href="#TOC">»</A>Approximation questions</h2> + +<h4><A href="#TOC">»</A><a name="simplex">How</a> do I +approximate data with a simplex</h4><blockquote> + +<p>Qhull may be used to help select a simplex that approximates a +data set. It will take experimentation. Geomview will help to +visualize the results. This task may be difficult to do in 5-d +and higher. Use rbox options 'x' and 'y' to produce random +distributions within a simplex. Your methods work if you can +recover the simplex. </p> + +<p>Use Qhull's precision options to get a first approximation to +the hull, say with 10 to 50 facets. For example, try 'C0.05' to +remove small facets after constructing the hull. Use 'W0.05' to +ignore points within 0.05 of a facet. Use 'PA5' to print the five +largest facets by area. </p> + +<p>Then use other methods to fit a simplex to this data. Remove +outlying vertices with few nearby points. Look for large facets +in different quadrants. You can use option 'Pd0d1d2' to print all +the facets in a quadrant. </p> + +<p>In 4-d and higher, use the outer planes (option 'Fo' or +'facet->maxoutside') since the hyperplane of an approximate +facet may be below many of the input points. </p> + +<p>For example, consider fitting a cube to 1000 uniformly random +points in the unit cube. In this case, the first try was good: </p> + +<blockquote> + <pre> +rbox 1000 | qconvex W0.05 C0.05 PA6 Fo +4 +6 +0.35715408374381 0.08706467018177928 -0.9299788727015564 -0.5985514741284483 +0.995841591359023 -0.02512604712761577 0.08756829720435189 -0.5258834069202866 +0.02448099521570909 -0.02685210459017302 0.9993396046151313 -0.5158104982631999 +-0.9990223929415094 -0.01261133513150079 0.04236994958247349 -0.509218270408407 +-0.0128069014364698 -0.9998380680115362 0.01264203427283151 -0.5002512653670584 +0.01120895057872914 0.01803671994177704 -0.9997744926535512 -0.5056824072956361 +</pre> +</blockquote> + +</blockquote> +<h2><A href="#TOC">»</A>Halfspace questions</h2> + +<h4><A href="#TOC">»</A><a name="halfspace">How</a> do I compute the + intersection of halfspaces with Qhull?</h4><blockquote> + +<p>Qhull computes the halfspace intersection about a point. The +point must be inside all of the halfspaces. Given a point, a +duality turns a halfspace intersection problem into a convex +hull problem. + +<p>Use linear programming if you +do not know a point in the interior of the halfspaces. +See the <A href="qhalf.htm#notes">manual</A>. You will need + a linear programming code. This may require a fair amount of work to + implement.</p> + + + +</blockquote> +<h2><A href="#TOC">»</A><a name="library">Qhull library +questions</a></h2> + +<h4><A href="#TOC">»</A><a name="math">Is</a> Qhull available for Mathematica, Matlab, or Maple?</h4><blockquote> + +<p>Z. You of <a href="http://www.mathworks.com">MathWorks</a> added qhull to MATLAB 6. +See functions <a href="http://www.mathworks.com/access/helpdesk/help/techdoc/ref/convhulln.shtml" + >convhulln</a>, + <a href="http://www.mathworks.com/access/helpdesk/help/techdoc/ref/delaunayn.shtml" + >delaunayn</a>, + <a href="http://www.mathworks.com/access/helpdesk/help/techdoc/ref/griddata3.shtml" + >griddata3</a>, + <a href="http://www.mathworks.com/access/helpdesk/help/techdoc/ref/griddatan.shtml" + >griddatan</a>, + <a href="http://www.mathworks.com/access/helpdesk/help/techdoc/ref/tsearch.shtml" + >tsearch</a>, + <a href="http://www.mathworks.com/access/helpdesk/help/techdoc/ref/tsearchn.shtml" + >tsearchn</a>, and + <a href="http://www.mathworks.com/access/helpdesk/help/techdoc/ref/voronoin.shtml" + >voronoin</a>. V. Brumberg update MATLAB R14 for Qhull 2003.1 and triangulated output. + +<p>See <a href="http://www.mathsource.com/Content/Enhancements/Geometry/0211-251" + >qh-math</a> +for a Delaunay interface to Mathematica. It includes projects for CodeWarrior +on the Macintosh and Visual C++ on Win32 PCs. + +<p>See Mathematica ('<a +href="qh-opto.htm#m">m</a>') and Maple ('<a +href="qh-optf.htm#FM">FM</a>') output options. + +<p></p> +</blockquote><h4><A href="#TOC">»</A><a name="ridges">Why</a> are there too few ridges?</h4><blockquote> + +The following sample code may produce fewer ridges than expected: + +<blockquote><pre> + facetT *facetp; + ridgeT *ridge, **ridgep; + + FORALLfacets { + printf("facet f%d\n", facet->id); + FOREACHridge_(facet->ridges) { + printf(" ridge r%d between f%d and f%d\n", ridge->id, ridge->top->id, ridge->bottom->id); + } + } +</pre></blockquote> + +<p> Qhull does not create ridges for simplicial facets. +Instead it computes ridges from facet->neighbors. To make ridges for a +simplicial facet, use qh_makeridges() in merge.c. Usefacet->visit_id to visit +each ridge once (instead of twice). For example, + +<blockquote><pre> + facetT *facet, *neighbor; + ridgeT *ridge, **ridgep; + + qh visit_id++; + FORALLfacets { + printf("facet f%d\n", facet->id); + qh_makeridges(facet); + facet->visitId= qh visit_id; + FOREACHridge_(facet->ridges) { + neighbor= otherfacet_(ridge, visible); + if (neighbor->visitid != qh visit_id) + printf(" ridge r%d between f%d and f%d\n", ridge->id, ridge->top->id, ridge->bottom->id); + } + } +</pre></blockquote> + +</blockquote><h4><A href="#TOC">»</A><a name="call">Can</a> Qhull use coordinates without placing + them in a data file?</h4><blockquote> + +<p>Use qh_call_qhull(). See user_eg.c for an example. +See the manual for an introduction to the Qhull library. + +<p>Start with a small example for which you know the + answer.</p> + + +</blockquote><h4><A href="#TOC">»</A><a name="size">How</a> large are Qhull's data structures?</h4><blockquote> + +<p>Qhull uses a general-dimension data structure. +The size depends on the dimension. Use option 'Ts' to print +out the memory statistics [e.g., 'rbox D2 10 | qconvex Ts']. + + +</blockquote><h4><A href="#TOC">»</A><a name="inc">Can</a> Qhull construct +convex hulls and Delaunay triangulations one point at a time?</h4><blockquote> + +<p>The Qhull library may be used to construct convex hulls and +Delaunay triangulations one point at a time. It may not be used +for deleting points or moving points. </p> + +<p>Qhull is designed for batch processing. Neither Clarkson's +randomized incremental algorithm nor Qhull are designed for +on-line operation. For many applications, it is better to +reconstruct the convex hull or Delaunay triangulation from +scratch for each new point. </p> + +<p>With random point sets and on-line processing, Clarkson's +algorithm should run faster than Qhull. Clarkson uses the +intermediate facets to reject new, interior points, while Qhull, +when used on-line, visits every facet to reject such points. If +used on-line for n points, Clarkson may take O(n) times as much +memory as the average off-line case, while Qhull's space +requirement does not change. </p> + +<p>If you triangulate the output before adding all the points +(option 'Qt' and procedure qh_triangulate), you must set +option '<A href="qh-optq.htm#Q11">Q11</A>'. It duplicates the +normals of triangulated facets and recomputes the centrums. +This should be avoided for regular use since triangulated facets +are not clearly convex with their neighbors. It appears to +work most of the time, but fails for cases that Qhull normally +handles well [see the test call to qh_triangulate in qh_addpoint]. + +</blockquote><h4><A href="#TOC">»</A><a name="ridges2">How</a> do I visit the +ridges of a Delaunay triangulation?</h4><blockquote> + +<p>To visit the ridges of a Delaunay triangulation, visit each +facet. Each ridge will appear twice since it belongs to two +facets. In pseudo-code: </p> + +<pre> + for each facet of the triangulation + if the facet is Delaunay (i.e., part of the lower convex hull) + for each ridge of the facet + if the ridge's neighboring facet has not been visited + ... process a ridge of the Delaunay triangulation ... +</pre> + +<p>In undebugged, C code: </p> + +<pre> + qh visit_id++; + FORALLfacets_(facetlist) + if (!facet->upperdelaunay) { + facet->visitid= qh visit_id; + qh_makeridges(facet); + FOREACHridge_(facet->ridges) { + neighbor= otherfacet_(ridge, facet); + if (neighbor->visitid != qh visit_id) { + /* Print ridge here with facet-id and neighbor-id */ + /*fprintf(fp, "f%d\tf%d\t",facet->id,neighbor->ID);*/ + FOREACHvertex_(ridge->vertices) + fprintf(fp,"%d ",qh_pointid (vertex->point) ); + qh_printfacetNvertex_simplicial (fp, facet, format); + fprintf(fp," "); + if(neighbor->upperdelaunay) + fprintf(fp," -1 -1 -1 -1 "); + else + qh_printfacetNvertex_simplicial (fp, neighbor, format); + fprintf(fp,"\n"); + } + } + } + } +</pre> + +<p>Qhull should be redesigned as a class library, or at least as +an API. It currently provides everything needed, but the +programmer has to do a lot of work. Hopefully someone will write +C++ wrapper classes or a Python module for Qhull. </p> + +</blockquote><h4><A href="#TOC">»</A><a name="listd">How</a> do I visit the +Delaunay regions?</h4><blockquote> + +<p>Qhull constructs a Delaunay triangulation by lifting the +input sites to a paraboloid. The Delaunay triangulation +corresponds to the lower convex hull of the lifted points. To +visit each facet of the lower convex hull, use: </p> + +<pre> + facetT *facet; + + ... + FORALLfacets { + if (!facet->upperdelaunay) { + ... only facets for Delaunay regions ... + } + } +</pre> + +</blockquote><h4><A href="#TOC">»</A><a name="outside">When</a> is a point +outside or inside a facet?</h4><blockquote> + +<p>A point is outside of a facet if it is clearly outside the +facet's outer plane. The outer plane is defined by an offset +(facet->maxoutside) from the facet's hyperplane. </p> + +<pre> + facetT *facet; + pointT *point; + realT dist; + + ... + qh_distplane(point, facet, &dist); + if (dist > facet->maxoutside + 2 * qh DISTround) { + /* point is clearly outside of facet */ + } +</pre> + +<p>A point is inside of a facet if it is clearly inside the +facet's inner plane. The inner plane is computed as the maximum +distance of a vertex to the facet. It may be computed for an +individual facet, or you may use the maximum over all facets. For +example: </p> + +<pre> + facetT *facet; + pointT *point; + realT dist; + + ... + qh_distplane(point, facet, &dist); + if (dist < qh min_vertex - 2 * qh DISTround) { + /* point is clearly inside of facet */ + } +</pre> + +<p>Both tests include two qh.DISTrounds because the computation +of the furthest point from a facet may be off by qh.DISTround and +the computation of the current distance to the facet may be off +by qh.DISTround. </p> + +</blockquote><h4><A href="#TOC">»</A><a name="closest">How</a> do I find the +facet that is closest to a point?</h4><blockquote> + +<p>Use qh_findbestfacet(). For example, </p> + +<pre> + coordT point[ DIM ]; + boolT isoutside; + realT bestdist; + facetT *facet; + + ... set coordinates for point ... + + facet= qh_findbestfacet (point, qh_ALL, &bestdist, &isoutside); + + /* 'facet' is the closest facet to 'point' */ +</pre> + +<p>qh_findbestfacet() performs a directed search for the facet +furthest below the point. If the point lies inside this facet, +qh_findbestfacet() performs an exhaustive search of all facets. +An exhaustive search may be needed because a facet on the far +side of a lens-shaped distribution may be closer to a point than +all of the facet's neighbors. The exhaustive search may be +skipped for spherical distributions. </p> + +<p>Also see, "<A href="#vclosest">How</A> do I find the +Delaunay triangle that is closest to a +point?" </p> + +</blockquote><h4><A href="#TOC">»</A><a name="vclosest">How</a> do I find the +Delaunay triangle or Voronoi region that is closest to a point?</h4><blockquote> + +<p>A Delaunay triangulation subdivides the plane, or in general +dimension, subdivides space. Given a point, how do you determine +the subdivision containing the point? Or, given a set of points, +how do you determine the subdivision containing each point of the set? +Efficiency is important -- an exhaustive search of the subdivision +is too slow. + +<p>First compute the Delaunay triangle with qh_call_qhull(). +Lift the point to the paraboloid by summing the squares of the +coordinates. Use qh_findbestfacet() [poly2.c] to find the closest Delaunay +triangle. Determine the closest vertex to find the corresponding +Voronoi region. Do not use options +'<A href="qh-optq.htm#Qbb">Qbb</A>', '<A href="qh-optq.htm#QbB">QbB</A>', +'<A href="qh-optq.htm#Qbk">Qbk:n</A>', or '<A + href="qh-optq.htm#QBk" >QBk:n</A>' since these scale the last +coordinate. Optimizations of qh_findbestfacet() should +be possible for Delaunay triangulations.</p> + +<p>You first need to lift the point to the paraboloid (i.e., the +last coordinate is the sum of the squares of the point's coordinates). +The +routine, qh_setdelaunay() [geom2.c], lifts an array of points to the +paraboloid. The following excerpt is from findclosest() in +user_eg.c. </p> + +<pre> + coordT point[ DIM + 1]; /* one extra coordinate for lifting the point */ + boolT isoutside; + realT bestdist; + facetT *facet; + + ... set coordinates for point[] ... + + qh_setdelaunay (DIM+1, 1, point); + facet= qh_findbestfacet (point, qh_ALL, &bestdist, &isoutside); + /* 'facet' is the closest Delaunay triangle to 'point' */ +</pre> + +<p>The returned facet either contains the point or it is the +closest Delaunay triangle along the convex hull of the input set. + +<p>Point location is an active research area in Computational +Geometry. For a practical approach, see Mucke, et al, "Fast randomized +point location without preprocessing in two- and +three-dimensional Delaunay triangulations," <i>Computational +Geometry '96</i>, p. 274-283, May 1996. +For an introduction to planar point location see [O'Rourke '93]. +Also see, "<A + href="#closest" >How</A> do I find the facet that is closest to a +point?" </p> + +<p>To locate the closest Voronoi region, determine the closest +vertex of the closest Delaunay triangle. </p> + +<pre> + realT dist, bestdist= REALmax; + vertexT *bestvertex= NULL, *vertex, **vertexp; + + /* 'facet' is the closest Delaunay triangle to 'point' */ + + FOREACHvertex_( facet->vertices ) { + dist= qh_pointdist( point, vertex->point, DIM ); + if (dist < bestdist) { + bestdist= dist; + bestvertex= vertex; + } + } + /* 'bestvertex' represents the Voronoi region closest to 'point'. The corresponding + input site is 'bestvertex->point' */ +</pre> + +</blockquote><h4><A href="#TOC">»</A><a name="vertices">How</a> do I list the +vertices?</h4><blockquote> + +<p>To list the vertices (i.e., extreme points) of the convex hull +use </p> + +<blockquote> + <pre> + vertexT *vertex; + + FORALLvertices { + ... + // vertex->point is the coordinates of the vertex + // qh_pointid (vertex->point) is the point ID of the vertex + ... + } + </pre> +</blockquote> + +</blockquote><h4><A href="#TOC">»</A><a name="test">How</a> do I test code +that uses the Qhull library?</h4><blockquote> + +<p>Compare the output from your program with the output from the +Qhull program. Use option 'T1' or 'T4' to trace what Qhull is +doing. Prepare a <i>small</i> example for which you know the +output. Run the example through the Qhull program and your code. +Compare the trace outputs. If you do everything right, the two +trace outputs should be almost the same. The trace output will +also guide you to the functions that you need to review. </p> + +</blockquote><h4><A href="#TOC">»</A><a name="orient">When</a> I compute a +plane equation from a facet, I sometimes get an outward-pointing +normal and sometimes an inward-pointing normal</h4><blockquote> + +<p>Qhull orients simplicial facets, and prints oriented output +for 'i', 'Ft', and other options. The orientation depends on <i>both</i> +the vertex order and the flag facet->toporient.</p> + +<p>Qhull does not orient + non-simplicial facets. Instead it orients the facet's ridges. These are + printed with the 'Qt' and 'Ft' option. The facet's hyperplane is oriented. </p> + +</blockquote> +<hr><!-- Navigation links --> + +<p><a><b>Up:</b> </a><a + href="http://www.qhull.org">Home page for Qhull</a><br> +<b>Up:</b> <A href="index.htm#TOC">Qhull manual</A>: Table of Contents <br> +<b>To:</b> <A href="qh-quick.htm#programs">Programs</A> +• <A href="qh-quick.htm#options">Options</A> +• <A href="qh-opto.htm#output">Output</A> +• <A href="qh-optf.htm#format">Formats</A> +• <A href="qh-optg.htm#geomview">Geomview</A> +• <A href="qh-optp.htm#print">Print</A> +• <A href="qh-optq.htm#qhull">Qhull</A> +• <A href="qh-optc.htm#prec">Precision</A> +• <A href="qh-optt.htm#trace">Trace</A> <br> +<b>To:</b> <A href="#TOC">FAQ: Table of Contents</A><br><!-- GC common information --> + +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><IMG align=middle + height=40 src="qh--geom.gif" width=40 ></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: +Sept. 25, 1995 --- <!-- hhmts start -->Last modified: see top +<!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-get.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-get.htm new file mode 100644 index 0000000000000000000000000000000000000000..2fd374e69272ddbaedd4234c39cdc88c2bbe87bb --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-get.htm @@ -0,0 +1,125 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull Downloads</title> +</head> + +<body> +<!-- Navigation links --> +<p><b>Up:</b> <a href="http://www.geom.uiuc.edu/software/download"><i>Downloadable Software from the +Geometry Center </i></a><br> +</p> + +<hr> + +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/cone.html"><img +src="../html/qh--cone.gif" alt="[CONE]" align="middle" +width="100" height="100"></a> Qhull Downloads</h1> + +<ul> + <li><a href="http://www.qhull.org">Qhull Home Page</a> <p>Qhull + computes convex hulls, Delaunay triangulations, halfspace + intersections about a point, Voronoi diagrams, furthest-site Delaunay + triangulations, and furthest-site Voronoi diagrams. It + runs in 2-d, 3-d, 4-d, and higher dimensions. It + implements the Quickhull algorithm for computing the + convex hull. Qhull handles roundoff errors from floating + point arithmetic. It can approximate a convex hull. </p> + + <p>Visit <a href="http://www.qhull.org/news">Qhull News</a> + for news, bug reports, change history, and users.</p> + </li> + <li><a + href="http://www.qhull.org/download/qhull-2003.1.zip">Download: + Qhull 2003.1 for Windows</a> (1.2 Mbytes) <p>Type: console programs for + Windows 95, 98, ME, 2000, NT, XP</p> + <p>Includes executables, documentation, sources files, and a cygwin Makefile. It runs in a + DOS window.</p> + </li> + <li><a href="http://www.qhull.org/download/qhull-2003.1-src.tgz">Download: + source for Qhull 2003.1</a> (600K) <p>Type: C source code for + 32-bit and 64-bit architectures </p> + <p>Includes documentation, source files, and a simple Makefile</p> + </li> + <li><a href=http://savannah.nongnu.org/files/?group=qhull>Download: Qhull 2002.1 for Unix</a> + <p>Type: C source code for Unix systems (Debian configure)</b> + + <p>Includes documentation, source files, and a configure Makefile. + Includes Debian configuration files. Includes downloads of Qhull's current and previous + versions.</p> + + <!--- + <p>B. Pearlmutter created a +<a href=http://packages.debian.org/stable/math/qhull-bin.html>Debian build</a> of Qhull 3.1 +and upgraded it to 2002.1. + --> + + </li> + <li><a href="http://www.qhull.org/download/qhull-2002.1-1mdk.i686.rpm">Download: + Qhull version 2002.1 i686 rpm</a> (643K) <p>Type: rpm build for Mandrake 8.2 and RedHat 7.3</p> + + <p>For other Linux systems, use the <a href="http://www.qhull.org/download/qhull-2002.1-1mdk.src.rpm">src rpm</a> [L. Mazet] + </li> + + <li><a href=http://savannah.gnu.org/projects/qhull/>Qhull@Savannah</a> + <p>Type: CVS repository</b> + + <p>CVS repositiory of Qhull sources, documentation, and Makefiles. + </p> + </li> + <li><a href=http://www6.uniovi.es/ftp/pub/mirrors/geom.umn.edu/software/ghindex.html>Spanish + mirror site</a> <p>Download Qhull's current and + previous versions from <a href=http://www.etsimo.uniovi.es/derechos.html>Servidor + WWW de la Escuela de Minas</a> of the Universidad de Oviedo.</p> + </li> + <li><a + href="http://citeseer.nj.nec.com/83502.html">Download: + Article about Qhull</a> (210K) <p>Type: various formats on CiteSeer</p> + <p>Barber, C.B., Dobkin, D.P., and Huhdanpaa, H.T., + "The Quickhull algorithm for convex hulls," <i>ACM + Transactions on Mathematical Software</i>, vol. 22, pp. + 469-483, Dec 1996 [<a + href="http://www.acm.org/pubs/citations/journals/toms/1996-22-4/p469-barber/">abstract</a>].</p> + </li> + <li><a + href="http://www.qhull.org/download/qhull-1.0.tar.gz">Download: + Qhull version 1.0</a> (92K) <p>Type: C source code for + 32-bit architectures </p> + <p>Version 1.0 is a fifth the size of version 2.4. It + computes convex hulls and Delaunay triangulations. If a + precision error occurs, it stops with an error message. + It reports an initialization error for inputs made with + 0/1 coordinates. </p> + <p>Version 1.0 compiles on a PC with Borland C++ 4.02 for + Win32 and DOS Power Pack. The options for rbox are + "bcc32 -WX -w- -O2-e -erbox -lc rbox.c". The + options for qhull are the same. [D. Zwick] </p> + </li> + <li><a + href="http://www.qhull.org/download/qhull-1.0.sit.hqx">Download: + Qhull version 1.0 (Mac)</a> (180K) <p>Type: C source + code, packaged for Macintosh w/68881 </p> + <p>The Macintosh implementation of Qhull 1.0 reads input + from a file and returns the results to a file. The code + is compiled for a 68881 floating point coprocessor. The + package includes Think C projects for Qhull and the rbox + input generator. </p> + </li> +</ul> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.geom.uiuc.edu/software/download"><i>Downloadable Software from the +Geometry Center </i></a><br> +<!-- GC common information --></p> + +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="../html/qh--geom.gif" alt="[HOME]" +align="middle"></a> <i>The Geometry Center Home Page</i> </p> + +<p>Comments to: <a href="mailto:qhull@qhull.org">qhull@qhull.org</a><br> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-impre.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-impre.htm new file mode 100644 index 0000000000000000000000000000000000000000..5f1ef5bd8021fa93047d1b8f4c59a75406676f31 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-impre.htm @@ -0,0 +1,778 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Imprecision in Qhull</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a href="http://www.qhull.org">Home +page</a> for Qhull <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of +Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To: </b><a href="#TOC">Qhull imprecision</a>: Table of Contents +(please wait while loading) + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/4dcube.html"><img +src="qh--4d.gif" alt="[4-d cube]" align="middle" width="100" +height="100"></a> Imprecision in Qhull</h1> + +<p>This section of the Qhull manual discusses the problems caused +by coplanar points and why Qhull uses options '<a +href="qh-optc.htm#C0">C-0</a>' or '<a href="qh-optq.htm#Qx">Qx</a>' +by default. If you ignore precision issues with option '<a +href="qh-optq.htm#Q0">Q0</a>', the output from Qhull can be +arbitrarily bad. Qhull avoids precision problems if you merge facets (the default) or joggle +the input ('<a +href="qh-optq.htm#QJn">QJ</a>'). </p> + +<p>Use option '<a href="qh-optt.htm#Tv">Tv</a>' to verify the +output from Qhull. It verifies that adjacent facets are clearly +convex. It verifies that all points are on or below all facets. </p> + +<p>Qhull automatically tests for convexity if it detects +precision errors while constructing the hull. </p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h2><a href="#TOP">»</a><a name="TOC">Qhull +imprecision: Table of Contents </a></h2> + +<ul> + <li><a href="#prec">Precision problems</a></li> + <li><a href="#joggle">Merged facets or joggled input</a></li> + <li><a href="#delaunay">Delaunay triangulations</a></li> + <li><a href="#imprecise">Merged facets</a></li> + <li><a href="#how">How Qhull merges facets</a></li> + <li><a href="#limit">Limitations of merged facets</a></li> + <li><a href="#injoggle">Joggled input</a></li> + <li><a href="#exact">Exact arithmetic</a></li> + <li><a href="#approximate">Approximating a convex hull</a></li> +</ul> + +<hr> + +<h2><a href="#TOC">»</a><a name="prec">Precision problems</a></h2> + +<p>Since Qhull uses floating point arithmetic, roundoff error +occurs with each calculation. This causes problems for +geometric algorithms. Other floating point codes for convex +hulls, Delaunay triangulations, and Voronoi diagrams also suffer +from these problems. Qhull handles most of them.</p> + +<p>There are several kinds of precision errors:</p> + +<ul> + <li>Representation error occurs when there are not enough + digits to represent a number, e.g., 1/3.</li> + <li>Measurement error occurs when the input coordinates are + from measurements. </li> + <li>Roundoff error occurs when a calculation is rounded to a + fixed number of digits, e.g., a floating point + calculation.</li> + <li>Approximation error occurs when the user wants an + approximate result because the exact result contains too + much detail.</li> +</ul> + +<p>Under imprecision, calculations may return erroneous results. +For example, roundoff error can turn a small, positive number +into a small, negative number. See Milenkovic [<a +href="index.htm#mile93">'93</a>] for a discussion of <em>strict +robust geometry</em>. Qhull does not meet Milenkovic's criterion +for accuracy. Qhull's error bound is empirical instead of +theoretical.</p> + +<p>Qhull 1.0 checked for precision errors but did not handle +them. The output could contain concave facets, facets with +inverted orientation ("flipped" facets), more than two +facets adjacent to a ridge, and two facets with exactly the same +set of vertices.</p> + +<p>Qhull 2.4 and later automatically handles errors due to +machine round-off. Option '<a href="qh-optc.htm#C0">C-0</a>' or '<a +href="qh-optq.htm#Qx">Qx</a>' is set by default. In 5-d and +higher, the output is clearly convex but an input point could be +outside of the hull. This may be corrected by using option '<a +href="qh-optc.htm#C0">C-0</a>', but then the output may contain +wide facets.</p> + +<p>Qhull 2.5 and later provides option '<a href="qh-optq.htm#QJn">QJ</a>' +to joggled input. Each input coordinate is modified by a +small, random quantity. If a precision error occurs, a larger +modification is tried. When no precision errors occur, Qhull is +done. </p> + +<p>Qhull 3.1 and later provides option '<a href="qh-optq.htm#Qt">Qt</a>' +for triangulated output. This removes the need for +joggled input ('<a href="qh-optq.htm#QJn">QJ</a>'). +Non-simplicial facets are triangulated. +The facets may have zero area. +Triangulated output is particularly useful for Delaunay triangulations.</p> + +<p>By handling round-off errors, Qhull can provide a variety of +output formats. For example, it can return the halfspace that +defines each facet ('<a href="qh-opto.htm#n">n</a>'). The +halfspaces include roundoff error. If the halfspaces were exact, +their intersection would return the original extreme points. With +imprecise halfspaces and exact arithmetic, nearly incident points +may be returned for an original extreme point. By handling +roundoff error, Qhull returns one intersection point for each of +the original extreme points. Qhull may split or merge an extreme +point, but this appears to be unlikely.</p> + +<p>The following pipe implements the identity function for +extreme points (with roundoff): +<blockquote> + qconvex <a href="qh-optf.htm#FV">FV</a> <a + href="qh-opto.htm#n">n</a> | qhalf <a href="qh-optf.htm#Fp">Fp</a> +</blockquote> + +<p>Bernd Gartner published his +<a href=http://www.inf.ethz.ch/personal/gaertner/miniball.html>Miniball</a> +algorithm ["Fast and robust smallest enclosing balls", <i>Algorithms - ESA '99</i>, LNCS 1643]. +It uses floating point arithmetic and a carefully designed primitive operation. +It is practical to 20-D or higher, and identifies at least two points on the +convex hull of the input set. Like Qhull, it is an incremental algorithm that +processes points furthest from the intermediate result and ignores +points that are close to the intermediate result. + +<h2><a href="#TOC">»</a><a name="joggle">Merged facets or joggled input</a></h2> + +<p>This section discusses the choice between merged facets and joggled input. +By default, Qhull uses merged facets to handle +precision problems. With option '<a href="qh-optq.htm#QJn">QJ</a>', +the input is joggled. See <a href="qh-eg.htm#joggle">examples</a> +of joggled input and triangulated output. +<ul> +<li>Use merged facets (the default) +when you want non-simplicial output (e.g., the faces of a cube). +<li>Use merged facets and triangulated output ('<a href="qh-optq.htm#Qt">Qt</a>') when +you want simplicial output and coplanar facets (e.g., triangles for a Delaunay triangulation). +<li>Use joggled input ('<a href="qh-optq.htm#QJn">QJ</a>') when you need clearly-convex, +simplicial output. +</ul> + +<p>The choice between merged facets and joggled input depends on +the application. Both run about the same speed. Joggled input may +be faster if the initial joggle is sufficiently large to avoid +precision errors. + +<p>Most applications should used merged facets +with triangulated output. </p> + +<p>Use merged facets (the +default, '<a href="qh-optc.htm#C0">C-0</a>') +or triangulated output ('<a href="qh-optq.htm#Qt">Qt</a>') if </p> + +<ul> + <li>Your application supports non-simplicial facets, or + it allows degenerate, simplicial facets (option '<a href="qh-optq.htm#Qt">Qt</a>'). </li> + <li>You do not want the input modified. </li> + <li>You want to set additional options for approximating the + hull. </li> + <li>You use single precision arithmetic (<a href="../src/user.h#realT">realT</a>). + </li> +</ul> + +<p>Use joggled input ('<a href="qh-optq.htm#QJn">QJ</a>') if </p> + +<ul> + <li>Your application needs clearly convex, simplicial output</li> + <li>Your application supports perturbed input points and narrow triangles.</li> + <li>Seven significant digits is sufficient accuracy.</li> +</ul> + +<p>You may use both techniques or combine joggle with +post-merging ('<a href="qh-optc.htm#Cn2">Cn</a>'). </p> + +<p>Other researchers have used techniques similar to joggled +input. Sullivan and Beichel [ref?] randomly perturb the input +before computing the Delaunay triangulation. Corkum and Wyllie +[news://comp.graphics, 1990] randomly rotate a polytope before +testing point inclusion. Edelsbrunner and Mucke [Symp. Comp. +Geo., 1988] and Yap [J. Comp. Sys. Sci., 1990] symbolically +perturb the input to remove singularities. </p> + +<p>Merged facets ('<a href="qh-optc.htm#C0">C-0</a>') handles +precision problems directly. If a precision problem occurs, Qhull +merges one of the offending facets into one of its neighbors. +Since all precision problems in Qhull are associated with one or +more facets, Qhull will either fix the problem or attempt to merge the +last remaining facets. </p> + +<h2><a href="#TOC">»</a><a name="delaunay">Delaunay +triangulations </a></h2> + +<p>Programs that use Delaunay triangulations traditionally assume +a triangulated input. By default, <a href=qdelaun.htm>qdelaunay</a> +merges regions with cocircular or cospherical input sites. +If you want a simplicial triangulation +use triangulated output ('<a href="qh-optq.htm#Qt">Qt</a>') or joggled +input ('<a href="qh-optq.htm#QJn">QJ</a>'). + +<p>For Delaunay triangulations, triangulated +output should produce good results. All points are within roundoff error of +a paraboloid. If two points are nearly incident, one will be a +coplanar point. So all points are clearly separated and convex. +If qhull reports deleted vertices, the triangulation +may contain serious precision faults. Deleted vertices are reported +in the summary ('<a href="qh-opto.htm#s">s</a>', '<a href="qh-optf.htm#Fs">Fs</a>'</p> + +<p>You should use option '<a href="qh-optq.htm#Qbb">Qbb</a>' with Delaunay +triangulations. It scales the last coordinate and may reduce +roundoff error. It is automatically set for <a href=qdelaun.htm>qdelaunay</a>, +<a href=qvoronoi.htm>qvoronoi</a>, and option '<a +href="qh-optq.htm#QJn">QJ</a>'.</p> + +<p>Edelsbrunner, H, <i>Geometry and Topology for Mesh Generation</i>, Cambridge University Press, 2001. +Good mathematical treatise on Delaunay triangulation and mesh generation for 2-d +and 3-d surfaces. The chapter on surface simplification is +particularly interesting. It is similar to facet merging in Qhull. + +<p>Veron and Leon published an algorithm for shape preserving polyhedral +simplification with bounded error [<i>Computers and Graphics</i>, 22.5:565-585, 1998]. +It remove nodes using front propagation and multiple remeshing. + +<h2><a href="#TOC">»</a><a name="imprecise">Merged facets </a></h2> + +<p>Qhull detects precision +problems when computing distances. A precision problem occurs if +the distance computation is less than the maximum roundoff error. +Qhull treats the result of a hyperplane computation as if it +were exact.</p> + +<p>Qhull handles precision problems by merging non-convex facets. +The result of merging two facets is a thick facet defined by an <i>inner +plane</i> and an <i>outer plane</i>. The inner and outer planes +are offsets from the facet's hyperplane. The inner plane is +clearly below the facet's vertices. At the end of Qhull, the +outer planes are clearly above all input points. Any exact convex +hull must lie between the inner and outer planes.</p> + +<p>Qhull tests for convexity by computing a point for each facet. +This point is called the facet's <i>centrum</i>. It is the +arithmetic center of the facet's vertices projected to the +facet's hyperplane. For simplicial facets with <em>d</em> +vertices, the centrum is equivalent to the centroid or center of +gravity. </p> + +<p>Two neighboring facets are convex if each centrum is clearly +below the other hyperplane. The '<a href="qh-optc.htm#Cn2">Cn</a>' +or '<a href="qh-optc.htm#Cn">C-n</a>' options sets the centrum's +radius to <i>n </i>. A centrum is clearly below a hyperplane if +the computed distance from the centrum to the hyperplane is +greater than the centrum's radius plus two maximum roundoff +errors. Two are required because the centrum can be the maximum +roundoff error above its hyperplane and the distance computation +can be high by the maximum roundoff error.</p> + +<p>With the '<a href="qh-optc.htm#Cn">C-n</a>' or '<a +href="qh-optc.htm#An">A-n </a>' options, Qhull merges non-convex +facets while constructing the hull. The remaining facets are +clearly convex. With the '<a href="qh-optq.htm#Qx">Qx </a>' +option, Qhull merges coplanar facets after constructing the hull. +While constructing the hull, it merges coplanar horizon facets, +flipped facets, concave facets and duplicated ridges. With '<a +href="qh-optq.htm#Qx">Qx</a>', coplanar points may be missed, but +it appears to be unlikely.</p> + +<p>If the user sets the '<a href="qh-optc.htm#An2">An</a>' or '<a +href="qh-optc.htm#An">A-n</a>' option, then all neighboring +facets are clearly convex and the maximum (exact) cosine of an +angle is <i>n</i>.</p> + +<p>If '<a href="qh-optc.htm#C0">C-0</a>' or '<a +href="qh-optq.htm#Qx">Qx</a>' is used without other precision +options (default), Qhull tests vertices instead of centrums for +adjacent simplices. In 3-d, if simplex <i>abc</i> is adjacent to +simplex <i>bcd</i>, Qhull tests that vertex <i>a</i> is clearly +below simplex <i>bcd </i>, and vertex <i>d</i> is clearly below +simplex <i>abc</i>. When building the hull, Qhull tests vertices +if the horizon is simplicial and no merges occur. </p> + +<h2><a href="#TOC">»</a><a name="how">How Qhull merges facets</a></h2> + +<p>If two facets are not clearly convex, then Qhull removes one +or the other facet by merging the facet into a neighbor. It +selects the merge which minimizes the distance from the +neighboring hyperplane to the facet's vertices. Qhull also +performs merges when a facet has fewer than <i>d</i> neighbors (called a +degenerate facet), when a facet's vertices are included in a +neighboring facet's vertices (called a redundant facet), when a +facet's orientation is flipped, or when a ridge occurs between +more than two facets.</p> + +<p>Qhull performs merges in a series of passes sorted by merge +angle. Each pass merges those facets which haven't already been +merged in that pass. After a pass, Qhull checks for redundant +vertices. For example, if a vertex has only two neighbors in 3-d, +the vertex is redundant and Qhull merges it into an adjacent +vertex.</p> + +<p>Merging two simplicial facets creates a non-simplicial facet +of <em>d+1</em> vertices. Additional merges create larger facets. +When merging facet A into facet B, Qhull retains facet B's +hyperplane. It merges the vertices, neighbors, and ridges of both +facets. It recomputes the centrum if a wide merge has not +occurred (qh_WIDEcoplanar) and the number of extra vertices is +smaller than a constant (qh_MAXnewcentrum).</p> + + +<h2><a href="#TOC">»</a><a name="limit">Limitations of merged +facets</a></h2> + +<ul> +<li><b>Uneven dimensions</b> -- +If one coordinate has a larger absolute value than other +coordinates, it may dominate the effect of roundoff errors on +distance computations. You may use option '<a +href="qh-optq.htm#QbB">QbB</a>' to scale points to the unit cube. +For Delaunay triangulations and Voronoi diagrams, <a href=qdelaun.htm>qdelaunay</a> +and <a href=qvoronoi.htm>qvoronoi</a> always set +option '<a href="qh-optq.htm#Qbb">Qbb</a>'. It scales the last +coordinate to [0,m] where <i>m</i> is the maximum width of the +other coordinates. Option '<a href="qh-optq.htm#Qbb">Qbb</a>' is +needed for Delaunay triangulations of integer coordinates +and nearly cocircular points. + +<p>For example, compare +<pre> + rbox 1000 W0 t | qconvex Qb2:-1e-14B2:1e-14 +</pre> +with +<pre> + rbox 1000 W0 t | qconvex +</pre> +The distributions are the same but the first is compressed to a 2e-14 slab. +<p> +<li><b>Post-merging of coplanar facets</b> -- In 5-d and higher, option '<a href="qh-optq.htm#Qx">Qx</a>' +(default) delays merging of coplanar facets until post-merging. +This may allow "dents" to occur in the intermediate +convex hulls. A point may be poorly partitioned and force a poor +approximation. See option '<a href="qh-optq.htm#Qx">Qx</a>' for +further discussion.</p> + +<p>This is difficult to produce in 5-d and higher. Option '<a href="qh-optq.htm#Q6">Q6</a>' turns off merging of concave +facets. This is similar to 'Qx'. It may lead to serious precision errors, +for example, +<pre> + rbox 10000 W1e-13 | qhull Q6 Tv +</pre> + +<p> +<li><b>Maximum facet width</b> -- +Qhull reports the maximum outer plane and inner planes (if +more than roundoff error apart). There is no upper bound +for either figure. This is an area for further research. Qhull +does a good job of post-merging in all dimensions. Qhull does a +good job of pre-merging in 2-d, 3-d, and 4-d. With the '<a +href="qh-optq.htm#Qx">Qx</a>' option, it does a good job in +higher dimensions. In 5-d and higher, Qhull does poorly at +detecting redundant vertices. </p> + +<p>In the summary ('<a href="qh-opto.htm#s">s</a>'), look at the +ratio between the maximum facet width and the maximum width of a +single merge, e.g., "(3.4x)". Qhull usually reports a +ratio of four or lower in 3-d and six or lower in 4-d. If it +reports a ratio greater than 10, this may indicate an +implementation error. Narrow distributions (see following) may +produce wide facets. + +<p>For example, if special processing for narrow distributions is +turned off ('<a href="qh-optq.htm#Q10">Q10</a>'), qhull may produce +a wide facet:</p> +<pre> + RBOX 1000 L100000 s G1e-16 t1002074964 | QHULL Tv Q10 +</pre> + +<p> +<li><b>Narrow distribution</b> -- In 3-d, a narrow distribution may result in a poor +approximation. For example, if you do not use qdelaunay nor option +'<a href="qh-optq.htm#Qbb">Qbb</a>', the furthest-site +Delaunay triangulation of nearly cocircular points may produce a poor +approximation: +<pre> + RBOX s 5000 W1e-13 D2 t1002151341 | QHULL d Qt + RBOX 1000 s W1e-13 t1002231672 | QHULL d Tv +</pre> + +<p>During +construction of the hull, a point may be above two +facets with opposite orientations that span the input +set. Even though the point may be nearly coplanar with both +facets, and can be distant from the precise convex +hull of the input sites. Additional facets leave the point distant from +a facet. To fix this problem, add option '<a href="qh-optq.htm#Qbb">Qbb</a>' +(it scales the last coordinate). Option '<a href="qh-optq.htm#Qbb">Qbb</a>' +is automatically set for <a href=qdelaun.htm>qdelaunay</a> and <a href=qvoronoi.htm>qvoronoi</a>. + +<p>Qhull generates a warning if the initial simplex is narrow. +For narrow distributions, Qhull changes how it processes coplanar +points -- it does not make a point coplanar until the hull is +finished. +Use option '<a href="qh-optq.htm#Q10">Q10</a>' to try Qhull without +special processing for narrow distributions. +For example, special processing is needed for: +<pre> + RBOX 1000 L100000 s G1e-16 t1002074964 | QHULL Tv Q10 +</pre> + +<p>You may turn off the warning message by reducing +qh_WARNnarrow in <tt>user.h</tt> or by setting option +'<a href="qh-optp.htm#Pp">Pp</a>'. </p> + +<p>Similar problems occur for distributions with a large flat facet surrounded +with many small facet at a sharp angle to the large facet. +Qhull 3.1 fixes most of these problems, but a poor approximation can occur. +A point may be left outside of the convex hull ('<a href="qh-optt.htm#Tv">Tv</a>'). +Examples include +the furthest-site Delaunay triangulation of nearly cocircular points plus the origin, and the convex hull of a cone of nearly cocircular points. The width of the band is 10^-13. +<pre> + rbox s 1000 W1e-13 P0 D2 t996799242 | qhull d Tv + rbox 1000 s Z1 G1e-13 t1002152123 | qhull Tv + RBOX 1000 s Z1 G1e-13 t1002231668 | QHULL Tv +</pre> + +<p> +<li><b>Quadratic running time</b> -- If the output contains large, non-simplicial +facets, the running time for Qhull may be quadratic in the size of the triangulated +output. For example, <tt>RBOX 1000 s W1e-13 c G2 | QHULL d</tt> is 4 times +faster for 500 points. The convex hull contains two large nearly spherical facets and +many nearly coplanar facets. Each new point retriangulates the spherical facet and repartitions the remaining points into all of the nearly coplanar facets. +In this case, quadratic running time is avoided if you use qdelaunay, +add option '<a href="qh-optq.htm#Qbb">Qbb</a>', +or add the origin ('P0') to the input. +<p> +<li><b>Facet with zero-area</b> -- +It is possible for a zero-area facet to be convex with its +neighbors. This can occur if the hyperplanes of neighboring +facets are above the facet's centrum, and the facet's hyperplane +is above the neighboring centrums. Qhull computes the facet's +hyperplane so that it passes through the facet's vertices. The +vertices can be collinear. </p> + +<p> +<li><b>No more facets</b> -- Qhull reports an error if there are <em>d+1</em> facets left +and two of the facets are not clearly convex. This typically +occurs when the convexity constraints are too strong or the input +points are degenerate. The former is more likely in 5-d and +higher -- especially with option '<a href="qh-optc.htm#Cn">C-n</a>'.</p> + +<p> +<li><b>Deleted cone</b> -- Lots of merging can end up deleting all +of the new facets for a point. This is a rare event that has +only been seen while debugging the code. + +<p> +<li><b>Triangulated output leads to precision problems</b> -- With sufficient +merging, the ridges of a non-simplicial facet may have serious topological +and geometric problems. A ridge may be between more than two +neighboring facets. If so, their triangulation ('<a href="qh-optq.htm#Qt">Qt</a>') +will fail since two facets have the same vertex set. Furthermore, +a triangulated facet may have flipped orientation compared to its +neighbors.</li> + +<p>The triangulation process detects degenerate facets with +only two neighbors. These are marked degenerate. They have +zero area. + +<p> +<li><b>Coplanar points</b> -- +Option '<a href="qh-optq.htm#Qc">Qc</a>' is determined by +qh_check_maxout() after constructing the hull. Qhull needs to +retain all possible coplanar points in the facets' coplanar sets. +This depends on qh_RATIOnearInside in <tt>user.h.</tt> +Furthermore, the cutoff for a coplanar point is arbitrarily set +at the minimum vertex. If coplanar points are important to your +application, remove the interior points by hand (set '<a +href="qh-optq.htm#Qc">Qc</a> <a href="qh-optq.htm#Qi">Qi</a>') or +make qh_RATIOnearInside sufficiently large.</p> + +<p> +<li><b>Maximum roundoff error</b> -- Qhull computes the maximum roundoff error from the maximum +coordinates of the point set. Usually the maximum roundoff error +is a reasonable choice for all distance computations. The maximum +roundoff error could be computed separately for each point or for +each distance computation. This is expensive and it conflicts +with option '<a href="qh-optc.htm#Cn">C-n</a>'. + +<p> +<li><b>All flipped or upper Delaunay</b> -- When a lot of merging occurs for +Delaunay triangulations, a new point may lead to no good facets. For example, +try a strong convexity constraint: +<pre> + rbox 1000 s t993602376 | qdelaunay C-1e-3 +</pre> + +</ul> + +<h2><a href="#TOC">»</a><a name="injoggle">Joggled input</a></h2> + +<p>Joggled input is a simple work-around for precision problems +in computational geometry ["joggle: to shake or jar +slightly," Amer. Heritage Dictionary]. Other names are +<i>jostled input</i> or <i>random perturbation</i>. +Qhull joggles the +input by modifying each coordinate by a small random quantity. If +a precision problem occurs, Qhull joggles the input with a larger +quantity and the algorithm is restarted. This process continues +until no precision problems occur. Unless all inputs incur +precision problems, Qhull will terminate. Qhull adjusts the inner +and outer planes to account for the joggled input. </p> + +<p>Neither joggle nor merged facets has an upper bound for the width of the output +facets, but both methods work well in practice. Joggled input is +easier to justify. Precision errors occur when the points are +nearly singular. For example, four points may be coplanar or +three points may be collinear. Consider a line and an incident +point. A precision error occurs if the point is within some +epsilon of the line. Now joggle the point away from the line by a +small, uniformly distributed, random quantity. If the point is +changed by more than epsilon, the precision error is avoided. The +probability of this event depends on the maximum joggle. Once the +maximum joggle is larger than epsilon, doubling the maximum +joggle will halve the probability of a precision error. </p> + +<p>With actual data, an analysis would need to account for each +point changing independently and other computations. It is easier +to determine the probabilities empirically ('<a href="qh-optt.htm#TRn">TRn</a>') . For example, consider +computing the convex hull of the unit cube centered on the +origin. The arithmetic has 16 significant decimal digits. </p> + +<blockquote> + <p><b>Convex hull of unit cube</b> </p> + <table border="1"> + <tr> + <th align="left">joggle</th> + <th align="right">error prob. </th> + </tr> + <tr> + <td align="right">1.0e-15</td> + <td align="center">0.983 </td> + </tr> + <tr> + <td align="right">2.0e-15</td> + <td align="center">0.830 </td> + </tr> + <tr> + <td align="right">4.0e-15</td> + <td align="center">0.561 </td> + </tr> + <tr> + <td align="right">8.0e-15</td> + <td align="center">0.325 </td> + </tr> + <tr> + <td align="right">1.6e-14</td> + <td align="center">0.185 </td> + </tr> + <tr> + <td align="right">3.2e-14</td> + <td align="center">0.099 </td> + </tr> + <tr> + <td align="right">6.4e-14</td> + <td align="center">0.051 </td> + </tr> + <tr> + <td align="right">1.3e-13</td> + <td align="center">0.025 </td> + </tr> + <tr> + <td align="right">2.6e-13</td> + <td align="center">0.010 </td> + </tr> + <tr> + <td align="right">5.1e-13</td> + <td align="center">0.004 </td> + </tr> + <tr> + <td align="right">1.0e-12</td> + <td align="center">0.002 </td> + </tr> + <tr> + <td align="right">2.0e-12</td> + <td align="center">0.001 </td> + </tr> + </table> +</blockquote> + +<p>A larger joggle is needed for multiple points. Since the +number of potential singularities increases, the probability of +one or more precision errors increases. Here is an example. </p> + +<blockquote> + <p><b>Convex hull of 1000 points on unit cube</b> </p> + <table border="1"> + <tr> + <th align="left">joggle</th> + <th align="right">error prob. </th> + </tr> + <tr> + <td align="right">1.0e-12</td> + <td align="center">0.870 </td> + </tr> + <tr> + <td align="right">2.0e-12</td> + <td align="center">0.700 </td> + </tr> + <tr> + <td align="right">4.0e-12</td> + <td align="center">0.450 </td> + </tr> + <tr> + <td align="right">8.0e-12</td> + <td align="center">0.250 </td> + </tr> + <tr> + <td align="right">1.6e-11</td> + <td align="center">0.110 </td> + </tr> + <tr> + <td align="right">3.2e-11</td> + <td align="center">0.065 </td> + </tr> + <tr> + <td align="right">6.4e-11</td> + <td align="center">0.030 </td> + </tr> + <tr> + <td align="right">1.3e-10</td> + <td align="center">0.010 </td> + </tr> + <tr> + <td align="right">2.6e-10</td> + <td align="center">0.008 </td> + </tr> + <tr> + <td align="right">5.1e-09</td> + <td align="center">0.003 </td> + </tr> + </table> +</blockquote> + +<p>Other distributions behave similarly. No distribution should +behave significantly worse. In Euclidean space, the probability +measure of all singularities is zero. With floating point +numbers, the probability of a singularity is non-zero. With +sufficient digits, the probability of a singularity is extremely +small for random data. For a sufficiently large joggle, all data +is nearly random data. </p> + +<p>Qhull uses an initial joggle of 30,000 times the maximum +roundoff error for a distance computation. This avoids most +potential singularities. If a failure occurs, Qhull retries at +the initial joggle (in case bad luck occurred). If it occurs +again, Qhull increases the joggle by ten-fold and tries again. +This process repeats until the joggle is a hundredth of the width +of the input points. Qhull reports an error after 100 attempts. +This should never happen with double-precision arithmetic. Once +the probability of success is non-zero, the probability of +success increases about ten-fold at each iteration. The +probability of repeated failures becomes extremely small. </p> + +<p>Merged facets produces a significantly better approximation. +Empirically, the maximum separation between inner and outer +facets is about 30 times the maximum roundoff error for a +distance computation. This is about 2,000 times better than +joggled input. Most applications though will not notice the +difference. </p> + +<h2><a href="#TOC">»</a><a name="exact">Exact arithmetic</a></h2> + +<p>Exact arithmetic may be used instead of floating point. +Singularities such as coplanar points can either be handled +directly or the input can be symbolically perturbed. Using exact +arithmetic is slower than using floating point arithmetic and the +output may take more space. Chaining a sequence of operations +increases the time and space required. Some operations are +difficult to do.</p> + +<p>Clarkson's <a +href="http://netlib.bell-labs.com/netlib/voronoi/hull.html">hull +program</a> and Shewchuk's <a +href="http://www.cs.cmu.edu/~quake/triangle.html">triangle +program</a> are practical implementations of exact arithmetic.</p> + +<p>Clarkson limits the input precision to about fifteen digits. +This reduces the number of nearly singular computations. When a +determinant is nearly singular, he uses exact arithmetic to +compute a precise result.</p> + +<h2><a href="#TOC">»</a><a name="approximate">Approximating a +convex hull</a></h2> + +<p>Qhull may be used for approximating a convex hull. This is +particularly valuable in 5-d and higher where hulls can be +immense. You can use '<a href="qh-optq.htm#Qx">Qx</a> <a +href="qh-optc.htm#Cn">C-n</a>' to merge facets as the hull is +being constructed. Then use '<a href="qh-optc.htm#Cn2">Cn</a>' +and/or '<a href="qh-optc.htm#An2">An</a>' to merge small facets +during post-processing. You can print the <i>n</i> largest facets +with option '<a href="qh-optp.htm#PAn">PAn</a>'. You can print +facets whose area is at least <i>n</i> with option '<a +href="qh-optp.htm#PFn">PFn</a>'. You can output the outer planes +and an interior point with '<a href="qh-optf.htm#FV">FV</a> <a +href="qh-optf.htm#Fo">Fo</a>' and then compute their intersection +with 'qhalf'. </p> + +<p>To approximate a convex hull in 6-d and higher, use +post-merging with '<a href="qh-optc.htm#Wn">Wn</a>' (e.g., qhull +W1e-1 C1e-2 TF2000). Pre-merging with a convexity constraint +(e.g., qhull Qx C-1e-2) often produces a poor approximation or +terminates with a simplex. Option '<a href="qh-optq.htm#QbB">QbB</a>' +may help to spread out the data.</p> + +<p>You will need to experiment to determine a satisfactory set of +options. Use <a href="rbox.htm">rbox</a> to generate test sets +quickly and <a href="index.htm#geomview">Geomview</a> to view +the results. You will probably want to write your own driver for +Qhull using the Qhull library. For example, you could select the +largest facet in each quadrant.</p> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home +page</a> for Qhull <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of +Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#TOC">Qhull imprecision: Table of Contents</a> + +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-in.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-in.htm new file mode 100644 index 0000000000000000000000000000000000000000..482c82b73263e96a3c25e481d26c075205dc09d5 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-in.htm @@ -0,0 +1,620 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull internals</title> +<!-- Navigation links --> +</head> + +<body> + +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page for Qhull</a> +<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual: Table of +Contents</a><br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#TOC">Qhull internals</a>: Table of Contents +(please wait while loading) <br> +<b>Dn:</b> <a href="../src/index.htm">Qhull functions</a>, macros, and data +structures +</p> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/4dcube.html"><img +src="qh--4d.gif" alt="[4-d cube]" align="middle" width="100" +height="100"></a> Qhull internals</h1> + +<p>This section discusses the internals of Qhull. </p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h2><a href="#TOP">»</a><a name="TOC">Qhull internals: Table of +Contents </a></h2> + +<ul> + <li><a href="#performance">Performance</a> of Qhull</li> + <li><a href="#library">Calling Qhull</a> from your program + <ul> + <li><a href="#constrained">Constrained Delaunay</a> + triangulation</li> + <li><a href="#dids">Delaunay triangulations</a> and point indices</li> + <li><a href="#findfacet">Locate facet</a> with + qh_findbestfacet()</li> + <li><a href="#inc">On-line construction</a> with + qh_addpoint()</li> + <li><a href="#mem">Sets and quick memory</a> allocation</li> + <li><a href="#tricoplanar">Tricoplanar facets</a> and option 'Qt'</li> + <li><a href="#vneighbor">Vertex neighbors</a> of a vertex</li> + <li><a href="#vertices">Voronoi vertices</a> of a region</li> + <li><a href="#ridge">Voronoi vertices</a> of a ridge</li> + </ul> + </li> + <li><a href="#enhance">Enhancements</a> to Qhull</li> + <li><a href="../src/index.htm">Qhull functions</a>, macros, and data + structures </li> +</ul> + +<hr> + +<h2><a href="#TOC">»</a><a name="performance">Performance of +Qhull </a></h2> + +<p>Empirically, Qhull's performance is balanced in the sense that +the average case happens on average. This may always be true if +the precision of the input is limited to at most <i>O(log n)</i> +bits. Empirically, the maximum number of vertices occurs at the +end of constructing the hull. </p> + +<p>Let <i>n</i> be the number of input points, <i>v</i> be the +number of output vertices, and <i>f_v </i>be the maximum number +of facets for a convex hull of <i>v</i> vertices. If both +conditions hold, Qhull runs in <i>O(n log v)</i> in 2-d and 3-d +and <i>O(n f_v/v)</i> otherwise. The function <i>f_v</i> +increases rapidly with dimension. It is <em>O(v^floor(d/2) / +floor(d/2)!)</em>.</p> + +<p>The time complexity for merging is unknown. Options '<a +href="qh-optc.htm#C0">C-0</a>' and '<a href="qh-optq.htm#Qx">Qx</a>' +(defaults) handle precision problems due to floating-point +arithmetic. They are optimized for simplicial outputs. </p> + +<p>When running large data sets, you should monitor Qhull's +performance with the '<a href="qh-optt.htm#TFn">TFn</a>' option. +The time per facet is approximately constant. In high-d with many +merged facets, the size of the ridge sets grows rapidly. For +example the product of 8-d simplices contains 18 facets and +500,000 ridges. This will increase the time needed per facet. </p> + +<p>As dimension increases, the number of facets and ridges in a +convex hull grows rapidly for the same number of vertices. For +example, the convex hull of 300 cospherical points in 6-d has +30,000 facets. </p> + +<p>If Qhull appears to stop processing facets, check the memory +usage of Qhull. If more than 5-10% of Qhull is in virtual memory, +its performance will degrade rapidly. </p> + +<p>When building hulls in 20-d and higher, you can follow the +progress of Qhull with option '<a href="qh-optt.htm#Tn">T1</a>'. +It reports each major event in processing a point. </p> + +<p>To reduce memory requirements, recompile Qhull for +single-precision reals (REALfloat in <tt>user.h</tt>). +Single-precision does not work with joggle ('<a +href="qh-optq.htm#QJn">QJ</a>'). Check qh_MEMalign in <tt>user.h</tt> +and the match between free list sizes and data structure sizes +(see the end of the statistics report from '<a +href="qh-optt.htm#Ts">Ts</a>'). If free list sizes do not match, +you may be able to use a smaller qh_MEMalign. Setting +qh_COMPUTEfurthest saves a small amount of memory, as does +clearing qh_MAXoutside (both in <tt>user.h</tt>).</p> + +<p>Shewchuk is working on a 3-d version of his triangle +program. It is optimized for 3-d simplicial Delaunay triangulation +and uses less memory than Qhull.</p> + +<p>To reduce the size of the Qhull executable, consider +qh_NOtrace and qh_KEEPstatistics 0 in <tt>user.h</tt>. By +changing <tt>user.c </tt>you can also remove the input/output +code in <tt>io.c</tt>. If you don't need facet merging, then +version 1.01 of Qhull is much smaller. It contains some bugs that +prevent Qhull from initializing in simple test cases. It is +slower in high dimensions.</p> + +<p>The precision options, '<a href="qh-optc.htm#Vn">Vn</a>', '<a +href="qh-optc.htm#Wn">Wn</a>', '<a href="qh-optc.htm#Un">Un</a>'. +'<a href="qh-optc.htm#An">A-n</a>', '<a href="qh-optc.htm#Cn">C-n</a>', +'<a href="qh-optc.htm#An2">An</a>', '<a href="qh-optc.htm#Cn2">Cn</a>', +and '<a href="qh-optq.htm#Qx">Qx</a>', may have large effects on +Qhull performance. You will need to experiment to find the best +combination for your application. </p> + +<p>The verify option ('<a href="qh-optt.htm#Tv">Tv</a>') checks +every point after the hull is complete. If facet merging is used, +it checks that every point is inside every facet. This can take a +very long time if there are many points and many facets. You can +interrupt the verify without losing your output. If facet merging +is not used and there are many points and facets, Qhull uses a +directed search instead of an exhaustive search. This should be +fast enough for most point sets. Directed search is not used for +facet merging because directed search was already used for +updating the facets' outer planes.</p> + +<p>The check-frequently option ('<a href="qh-optt.htm#Tc">Tc</a>') +becomes expensive as the dimension increases. The verify option +('<a href="qh-optt.htm#Tv">Tv</a>') performs many of the same +checks before outputting the results.</p> + +<p>Options '<a href="qh-optq.htm#Q0">Q0</a>' (no pre-merging), '<a +href="qh-optq.htm#Q3">Q3</a>' (no checks for redundant vertices), +'<a href="qh-optq.htm#Q5">Q5</a>' (no updates for outer planes), +and '<a href="qh-optq.htm#Q8">Q8</a>' (no near-interior points) +increase Qhull's speed. The corresponding operations may not be +needed in your application.</p> + +<p>In 2-d and 3-d, a partial hull may be faster to produce. +Option '<a href="qh-optq.htm#QGn">QgGn</a>' only builds facets +visible to point n. Option '<a href="qh-optq.htm#QVn">QgVn</a>' +only builds facets that contain point n. In higher-dimensions, +this does not reduce the number of facets.</p> + +<p><tt>User.h</tt> includes a number of performance-related +constants. Changes may improve Qhull performance on your data +sets. To understand their effect on performance, you will need to +read the corresponding code. </p> + +<p>GNU <tt>gprof</tt> reports that the dominate cost for 3-d +convex hull of cosperical points is qh_distplane(), mainly called +from qh_findbestnew(). The dominate cost for 3-d Delaunay triangulation +is creating new facets in qh_addpoint(), while qh_distplane() remains +the most expensive function. + +</p> +<h2><a href="#TOC">»</a><a name="library">Calling Qhull from +your program</a></h2> + +<p><b>Warning:</b> Qhull was not designed for calling from other +programs. There is neither API nor Qhull classes. +It can be done, but it takes work and head scratching. +You will need to understand the data structures and read the code. +Most users will find it easier to call Qhull as an external +command. + +<p>For examples of calling Qhull, see GNU Octave's +<a href=http://octave.sourceforge.net/index/analysis.html#Geometry>computational geometry code</a>, +and Qhull's +<a href=../src/user_eg.c>user_eg.c</a>, +<a href=../src/user_eg2.c>user_eg2.c</a>, +<a href=../src/user.c>user.c</a>, and +<a href=../src/qhull_interface.cpp>qhull_interface.cpp</a>. +Qhull's programs use the same library: +<a href=../src/unix.c>unix.c</a>, +<a href=../src/qconvex.c>qconvex.c</a>, +<a href=../src/qdelaun.c>qdelaun.c</a>, +<a href=../src/qhalf.c>qhalf.c</a>, and +<a href=../src/qvoronoi.c>qvoronoi.c</a>. + +<p>The <a href=http://www.boost.org/libs/graph/doc/table_of_contents.html>BGL</a> +Boost Graph Library [aka GGCL] provides C++ classes for graph data structures +and algorithms [Dr. Dobb's 9/00 p. 29-38; OOPSLA '99 p. 399-414]. It is modelled after the +Standard Template Library. It would provide a good interface to Qhull. +If you are interested in adapting BGL to Qhull, please contact +<a href="mailto:bradb@qhull.org">bradb@qhull.org</a>. + +<p>See <a href="../src/index.htm">Qhull functions, macros, and data +structures</a> for internal documentation of Qhull. The +documentation provides an overview and index. To use the library +you will need to read and understand the code. For most users, it +is better to write data to a file, call the qhull program, and +read the results from the output file.</p> + +<p>When you read the code, be aware of the macros "qh" +and "qhstat", e.g., "qh hull_dim". They are +defined in <tt>qhull.h</tt>. They allow the global data +structures to be pre-allocated (faster access) or dynamically +allocated (allows multiple copies). </p> + +<p>Qhull's <tt>Makefile</tt> produces a library, <tt>libqhull.a</tt>, +for inclusion in your programs. First review <tt>qhull.h</tt>. +This defines the data structures used by Qhull and provides +prototypes for the top-level functions. +Most users will only need qhull.h in their programs. For +example, the Qhull program is defined with <tt>qhull.h</tt> and <tt>unix.c</tt>. +To access all functions, use <tt>qhull_a.h</tt>. Include the file +with "<tt>#include <qhull/qhull_a.h>".</tt> This +avoids potential name conflicts.</p> + +<p>If you use the Qhull library, you are on your own as far as +bugs go. Start with small examples for which you know the output. +If you get a bug, try to duplicate it with the Qhull program. The +'<a href="qh-optt.htm#Tc">Tc</a>' option will catch many problems +as they occur. When an error occurs, use '<a +href="qh-optt.htm#Tn">T4</a> <a href="qh-optt.htm#TPn">TPn</a>' +to trace from the last point added to the hull. Compare your +trace with the trace output from the Qhull program.</p> + +<p>Errors in the Qhull library are more likely than errors in the +Qhull program. These are usually due to feature interactions that +do not occur in the Qhull program. Please report all errors that +you find in the Qhull library. Please include suggestions for +improvement. </p> + +<h3><a href="#TOC">»</a><a name="mem">sets and quick memory +allocation</a></h3> + +<p>You can use <tt>mem.c</tt> and<tt> qset.c</tt> individually. <tt>Mem.c +</tt>implements quick-fit memory allocation. It is faster than +malloc/free in applications that allocate and deallocate lots of +memory. </p> + +<p><tt>Qset.c</tt> implements sets and related collections. It's +the inner loop of Qhull, so speed is more important than +abstraction. Set iteration is particularly fast. <tt>qset.c</tt> +just includes the functions needed for Qhull. </p> + +<h3><a href="#TOC">»</a><a name="dids">Delaunay triangulations +and point indices</a></h3> + +<p>Here some unchecked code to print the point indices of each +Delaunay triangle. Use option 'QJ' if you want to avoid +non-simplicial facets. Note that upper Delaunay regions are +skipped. These facets correspond to the furthest-site Delaunay +triangulation. </p> + +<blockquote> + <pre> + facetT *facet; + vertexT *vertex, **vertexp; + + FORALLfacets { + if (!facet->upperdelaunay) { + printf ("%d", qh_setsize (facet->vertices); + FOREACHvertex_(facet->vertices) + printf (" %d", qh_pointid (vertex->point)); + printf ("\n"); + } + } + +</pre> +</blockquote> + +<h3><a href="#TOC">»</a><a name="findfacet">locate a facet with +qh_findbestfacet()</a></h3> + +<p>The routine qh_findbestfacet in <tt>poly2.c</tt> is +particularly useful. It uses a directed search to locate the +facet that is furthest below a point. For Delaunay +triangulations, this facet is the Delaunay triangle that contains +the lifted point. For convex hulls, the distance of a point to +the convex hull is either the distance to this facet or the +distance to a subface of the facet.</p> + +<blockquote> +<p><b>Warning:</b> If triangulated output ('<a href=qh-optq.htm#Qt>Qt</a>') and +the best facet is triangulated, qh_findbestfacet() returns one of +the corresponding 'tricoplanar' facets. The actual best facet may be a different +tricoplanar facet. +<p> +See qh_nearvertex() in poly2.c for sample code to visit each +tricoplanar facet. To identify the correct tricoplanar facet, +see Devillers, et. al., [<a href="index.htm#devi01">'01</a>] +and Mucke, et al [<a href="index.htm#muck96">'96</a>]. If you +implement this test in general dimension, please notify +<a href="mailto:qhull@qhull.org">qhull@qhull.org</a>. +</blockquote> + +<p>qh_findbestfacet performs an exhaustive search if its directed +search returns a facet that is above the point. This occurs when +the point is inside the hull or if the curvature of the convex +hull is less than the curvature of a sphere centered at the point +(e.g., a point near a lens-shaped convex hull). When the later +occurs, the distance function is bimodal and a directed search +may return a facet on the far side of the convex hull. </p> + +<p>Algorithms that retain the previously constructed hulls +usually avoid an exhaustive search for the best facet. You may +use a hierarchical decomposition of the convex hull [Dobkin and +Kirkpatrick <a href="index.htm#dob-kir90">'90</a>]. </p> + +<p>To use qh_findbestfacet with Delaunay triangulations, lift the +point to a paraboloid by summing the squares of its coordinates +(see qh_setdelaunay in geom2.c). Do not scale the input with +options 'Qbk', 'QBk', 'QbB' or 'Qbb'. See Mucke, et al [<a +href="index.htm#muck96">'96</a>] for a good point location +algorithm.</p> + +<p>The intersection of a ray with the convex hull may be found by +locating the facet closest to a distant point on the ray. +Intersecting the ray with the facet's hyperplane gives a new +point to test. </p> + +<h3><a href="#TOC">»</a><a name="inc">on-line construction with +qh_addpoint()</a></h3> + +<p>The Qhull library may be used for the on-line construction of +convex hulls, Delaunay triangulations, and halfspace +intersections about a point. It may be slower than implementations that retain +intermediate convex hulls (e.g., Clarkson's <a +href="http://netlib.bell-labs.com/netlib/voronoi/hull.html">hull +program</a>). These implementations always use a directed search. +For the on-line construction of convex hulls and halfspace +intersections, Qhull may use an exhaustive search +(qh_findbestfacet). </p> + +<p>You may use qh_findbestfacet and qh_addpoint (<tt>qhull.c</tt>) to add a point to +a convex hull. Do not modify the point's coordinates since +qh_addpoint does not make a copy of the coordinates. For Delaunay +triangulations, you need to lift the point to a paraboloid by +summing the squares of the coordinates (see qh_setdelaunay in +geom2.c). Do not scale the input with options 'Qbk', 'QBk', 'QbB' +or 'Qbb'. Do not deallocate the point's coordinates. You need to +provide a facet that is below the point (<a href="#findfacet">qh_findbestfacet</a>). +</p> + +<p>You can not delete points. Another limitation is that Qhull +uses the initial set of points to determine the maximum roundoff +error (via the upper and lower bounds for each coordinate). </p> + +<p>For many applications, it is better to rebuild the hull from +scratch for each new point. This is especially true if the point +set is small or if many points are added at a time.</p> + +<p>Calling qh_addpoint from your program may be slower than +recomputing the convex hull with qh_qhull. This is especially +true if the added points are not appended to the qh first_point +array. In this case, Qhull must search a set to determine a +point's ID. [R. Weber] </p> + +<p>See user_eg.c for examples of the on-line construction of +convex hulls, Delaunay triangulations, and halfspace +intersections. The outline is: </p> + +<blockquote> + <pre> +initialize qhull with an initial set of points +qh_qhull(); + +for each additional point p + append p to the end of the point array or allocate p separately + lift p to the paraboloid by calling qh_setdelaunay + facet= qh_findbestfacet (p, !qh_ALL, &bestdist, &isoutside); + if (isoutside) + if (!qh_addpoint (point, facet, False)) + break; /* user requested an early exit with 'TVn' or 'TCn' */ + +call qh_check_maxout() to compute outer planes +terminate qhull</pre> +</blockquote> + +<h3><a href="#TOC">»</a><a name="constrained">Constrained +Delaunay triangulation </a></h3> + +<p>With a fair amount of work, Qhull is suitable for constrained +Delaunay triangulation. See Shewchuk, ACM Symposium on +Computational Geometry, Minneapolis 1998.</p> + +<p>Here's a quick way to add a constraint to a Delaunay +triangulation: subdivide the constraint into pieces shorter than +the minimum feature separation. You will need an independent +check of the constraint in the output since the minimum feature +separation may be incorrect. [H. Geron] </p> + +<h3><A href="#TOC">»</A><a name="tricoplanar">Tricoplanar facets and option 'Qt'</h3> + +<p>Option '<a href=qh-optq.htm#Qt>Qt</a>' triangulates non-simplicial +facets (e.g., a square facet in 3-d or a cubical facet in 4-d). +All facets share the same apex (i.e., the first vertex in facet->vertices). +For each triangulated facet, Qhull +sets facet->tricoplanar true and copies facet->center, facet->normal, facet->offset, and facet->maxoutside. One of +the facets owns facet->normal; its facet->keepcentrum is true. +If facet->isarea is false, facet->triowner points to the owning +facet. + +<p>Qhull sets facet->degenerate if the facet's vertices belong +to the same ridge of the non-simplicial facet. + +<p>To visit each tricoplanar facet of a non-simplicial facet, +either visit all neighbors of the apex or recursively visit +all neighbors of a tricoplanar facet. The tricoplanar facets +will have the same facet->center.</p> + +<p>See <a href=../src/io.c#detvridge>qh_detvridge</a> for an example of ignoring tricoplanar facets.</p> + +<h3><a href="#TOC">»</a><a name="vertices">Voronoi vertices of a +region</a></h3> + +<p>The following code iterates over all Voronoi vertices for each +Voronoi region. Qhull computes Voronoi vertices from the convex +hull that corresponds to a Delaunay triangulation. An input site +corresponds to a vertex of the convex hull and a Voronoi vertex +corresponds to an adjacent facet. A facet is +"upperdelaunay" if it corresponds to a Voronoi vertex +"at-infinity". Qhull uses qh_printvoronoi in <tt>io.c</tt> +for '<a href=qvoronoi.htm>qvoronoi</a> <a href="qh-opto.htm#o">o'</a> </p> + +<blockquote> + <pre> +/* please review this code for correctness */ +qh_setvoronoi_all(); +FORALLvertices { + site_id = qh_pointid (vertex->point); + if (qh hull_dim == 3) + qh_order_vertexneighbors(vertex); + infinity_seen = 0; + FOREACHneighbor_(vertex) { + if (neighbor->upperdelaunay) { + if (!infinity_seen) { + infinity_seen = 1; + ... process a Voronoi vertex "at infinity" ... + } + }else { + voronoi_vertex = neighbor->center; + ... your code goes here ... + } + } +} +</pre> +</blockquote> + +<h3><a href="#TOC">»</a><a name="ridge">Voronoi vertices of a +ridge</a></h3> + +<p>Qhull uses qh_printvdiagram() in io.c to print the ridges of a +Voronoi diagram for option '<a href="qh-optf.htm#Fv2">Fv</a>'. +The helper function qh_eachvoronoi() does the real work. It calls +the callback 'printvridge' for each ridge of the Voronoi diagram. +</p> + +<p>You may call qh_printvdiagram2(), qh_eachvoronoi(), or +qh_eachvoronoi_all() with your own function. If you do not need +the total number of ridges, you can skip the first call to +qh_printvdiagram2(). See qh_printvridge() and qh_printvnorm() in +io.c for examples. </p> + +<h3><a href="#TOC">»</a><a name="vneighbor">vertex neighbors of +a vertex</a></h3> + +<p>To visit all of the vertices that share an edge with a vertex: +</p> + +<ul> + <li>Generate neighbors for each vertex with + qh_vertexneighbors in <tt>poly2.c</tt>. </li> + <li>For simplicial facets, visit the vertices of each + neighbor </li> + <li>For non-simplicial facets, <ul> + <li>Generate ridges for neighbors with qh_makeridges + in <tt>merge.c</tt>. </li> + <li>Generate ridges for a vertex with qh_vertexridges + in <tt>merge.c</tt>. </li> + <li>Visit the vertices of these ridges. </li> + </ul> + </li> +</ul> + +<p>For non-simplicial facets, the ridges form a simplicial +decomposition of the (d-2)-faces between each pair of facets -- +if you need 1-faces, you probably need to generate the full face +graph of the convex hull. </p> + +<h2><a href="#TOC">»</a><a name="enhance">Enhancements to Qhull </a></h2> + +<p>There are many ways in which Qhull can be improved. </p> + +<pre>Here is a partial list: + - fix finddelaunay() in user_eg.c for tricoplanar facets + - write a BGL, C++ interface to Qhull + http://www.boost.org/libs/graph/doc/table_of_contents.html + - change qh_save_qhull to swap the qhT structure instead of using pointers + - change error handling and tracing to be independent of 'qh ferr' + - determine the maximum width for a given set of parameters + - prove that directed search locates all coplanar facets + - in high-d merging, can a loop of facets become disconnected? + - find a way to improve inner hulls in 5-d and higher + - determine the best policy for facet visibility ('<a +href="qh-optc.htm#Vn">Vn</a>') + - determine the limitations of '<a href="qh-optq.htm#Qg">Qg</a>' + +Precision improvements: + - For 'Qt', resolve cross-linked, butterfly ridges. + May allow retriangulation in qh_addpoint(). + - for Delaunay triangulations ('d' or 'v') under joggled input ('QJ'), + remove vertical facets whose lowest vertex may be coplanar with convex hull + - review use of 'Qbb' with 'd QJ'. Is MAXabs_coord better than MAXwidth? + - check Sugihara and Iri's better in-sphere test [Canadian + Conf. on Comp. Geo., 1989; Univ. of Tokyo RMI 89-05] + - replace centrum with center of mass and facet area + - handle numeric overflow in qh_normalize and elsewhere + - merge flipped facets into non-flipped neighbors. + currently they merge into best neighbor (appears ok) + - determine min norm for Cramer's rule (qh_sethyperplane_det). It looks high. + - improve facet width for very narrow distributions + +New features: + - implement Matlab's tsearch() using Qhull + - compute volume of Voronoi regions. You need to determine the dual face + graph in all dimensions [see Clarkson's hull program] + - compute alpha shapes [see Clarkson's hull program] + - implement deletion of Delaunay vertices + see Devillers, ACM Symposium on Computational Geometry, Minneapolis 1999. + - compute largest empty circle [see O'Rourke, chapter 5.5.3] [Hase] + - list redundant (i.e., coincident) vertices [Spitz] + - implement Mucke, et al, ['96] for point location in Delaunay triangulations + - implement convex hull of moving points + - implement constrained Delaunay diagrams + see Shewchuk, ACM Symposium on Computational Geometry, Minneapolis 1998. + - estimate outer volume of hull + - automatically determine lower dimensional hulls + - allow "color" data for input points + need to insert a coordinate for Delaunay triangulations + +Input/output improvements: + - Support the VTK Visualization Toolkit, http://www.kitware.com/vtk.html + - generate output data array for Qhull library [Gautier] + - need improved DOS window with screen fonts, scrollbar, cut/paste + - generate Geomview output for Voronoi ridges and unbounded rays + - generate Geomview output for halfspace intersection + - generate Geomview display of furthest-site Voronoi diagram + - use '<a href="qh-optg.htm#GDn">GDn</a>' to view 5-d facets in 4-d + - convert Geomview output for other 3-d viewers + - add interactive output option to avoid recomputing a hull + - orient vertex neighbors for '<a href="qh-optf.htm#Fv">Fv</a>' in 3-d and 2-d + - track total number of ridges for summary and logging + +Performance improvements: + - optimize Qhull for 2-d Delaunay triangulations + - use O'Rourke's <a href="index.htm#orou94">'94</a> vertex->duplicate_edge + - add bucketing + - better to specialize all of the code (ca. 2-3x faster w/o merging) + - use updated LU decomposition to speed up hyperplane construction + - [Gill et al. 1974, Math. Comp. 28:505-35] + - construct hyperplanes from the corresponding horizon/visible facets + - for merging in high d, do not use vertex->neighbors + +</pre> + +<p>Please let us know about your applications and improvements. </p> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home +page for Qhull</a> <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual: Table of +Contents</a><br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#TOC">Qhull internals</a>: Table of Contents <br> +<b>Dn:</b> <a href="../src/index.htm">Qhull functions</a>, macros, and data +structures <!-- GC common information --> + +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see changes.txt <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optc.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optc.htm new file mode 100644 index 0000000000000000000000000000000000000000..ce22064ea09bb1c18c3860e0e1fe2910e7e44500 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optc.htm @@ -0,0 +1,289 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull precision options</title> +</head> + +<body> +<!-- Navigation links --> +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a> Qhull precision options</h1> + +This section lists the precision options for Qhull. These options are +indicated by an upper-case letter followed by a number. + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<p><a href="index.htm#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +<a name="prec">•</a> <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<h2>Precision options</h2> + +<p>Most users will not need to set these options. They are best +used for <a href="qh-impre.htm#approximate">approximating</a> a +convex hull. They may also be used for testing Qhull's handling +of precision errors.</p> + +<p>By default, Qhull uses options '<a href="#C0">C-0</a>' for +2-d, 3-d and 4-d, and '<a href="qh-optq.htm#Qx">Qx</a>' for 5-d +and higher. These options use facet merging to handle precision +errors. You may also use joggled input '<a href="qh-optq.htm#QJn">QJ</a>' +to avoid precision problems. +For more information see <a +href="qh-impre.htm">Imprecision in Qhull</a>.</p> + +<dl compact> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="#Cn2">Cn</a></dt> + <dd>centrum radius for post-merging</dd> + <dt><a href="#Cn">C-n</a></dt> + <dd>centrum radius for pre-merging</dd> + <dt><a href="#An2">An</a></dt> + <dd>cosine of maximum angle for post-merging</dd> + <dt><a href="#An">A-n</a></dt> + <dd>cosine of maximum angle for pre-merging</dd> + <dt><a href="qh-optq.htm#Qx">Qx</a></dt> + <dd>exact pre-merges (allows coplanar facets)</dd> + <dt><a href="#C0">C-0</a></dt> + <dd>handle all precision errors</dd> + <dt><a href="#Wn">Wn</a></dt> + <dd>min distance above plane for outside points</dd> +</dl> + +<dl compact> + <dt> </dt> + <dd><b>Experimental</b></dd> + <dt><a href="#Un">Un</a></dt> + <dd>max distance below plane for a new, coplanar point</dd> + <dt><a href="#En">En</a></dt> + <dd>max roundoff error for distance computation</dd> + <dt><a href="#Vn">Vn</a></dt> + <dd>min distance above plane for a visible facet</dd> + <dt><a href="#Rn">Rn</a></dt> + <dd>randomly perturb computations by a factor of [1-n,1+n]</dd> +</dl> + +<dl compact> +</dl> + +<hr> + +<h3><a href="#prec">»</a><a name="An">A-n - cosine of maximum +angle for pre-merging.</a></h3> + +<p>Pre-merging occurs while Qhull constructs the hull. It is +indicated by '<a href="#Cn">C-n</a>', 'A-n', or '<a +href="qh-optq.htm#Qx">Qx</a>'.</p> + +<p>If the angle between a pair of facet normals is greater than <i>n</i>, +Qhull merges one of the facets into a neighbor. It selects the +facet that is closest to a neighboring facet.</p> + +<p>For example, option 'A-0.99' merges facets during the +construction of the hull. If the cosine of the angle between +facets is greater than 0.99, one or the other facet is merged. +Qhull accounts for the maximum roundoff error.</p> + +<p>If 'A-n' is set without '<a href="#Cn">C-n</a>', then '<a +href="#C0">C-0</a>' is automatically set. </p> + +<p>In 5-d and higher, you should set '<a href="qh-optq.htm#Qx">Qx</a>' +along with 'A-n'. It skips merges of coplanar facets until after +the hull is constructed and before '<a href="#An2">An</a>' and '<a +href="#Cn2">Cn</a>' are checked. </p> + +<h3><a href="#prec">»</a><a name="An2">An - cosine of maximum angle for +post-merging.</a></h3> + +<p>Post merging occurs after the hull is constructed. For +example, option 'A0.99' merges a facet if the cosine of the angle +between facets is greater than 0.99. Qhull accounts for the +maximum roundoff error.</p> + +<p>If 'An' is set without '<a href="#Cn2">Cn</a>', then '<a +href="#Cn2">C0</a>' is automatically set. </p> + +<h3><a href="#prec">»</a><a name="C0">C-0 - handle all precision +errors </a></h3> + +<p>Qhull handles precision errors by merging facets. The 'C-0' +option handles all precision errors in 2-d, 3-d, and 4-d. It is +set by default. It may be used in higher dimensions, but +sometimes the facet width grows rapidly. It is usually better to +use '<a href="qh-optq.htm#Qx">Qx</a>' in 5-d and higher. +Use '<a href="qh-optq.htm#QJn">QJ</a>' to joggle the input +instead of merging facets. +Use '<a +href="qh-optq.htm#Q0">Q0</a>' to turn both options off.</p> + +<p>Qhull optimizes 'C-0' ("_zero-centrum") by testing +vertices instead of centrums for adjacent simplices. This may be +slower in higher dimensions if merges decrease the number of +processed points. The optimization may be turned off by setting a +small value such as 'C-1e-30'. See <a href="qh-impre.htm">How +Qhull handles imprecision</a>.</p> + +<h3><a href="#prec">»</a><a name="Cn">C-n - centrum radius for +pre-merging</a></h3> + +<p>Pre-merging occurs while Qhull constructs the hull. It is +indicated by 'C-n', '<a href="#An">A-n</a>', or '<a +href="qh-optq.htm#Qx">Qx</a>'.</p> + +<p>The <i>centrum</i> of a facet is a point on the facet for +testing facet convexity. It is the average of the vertices +projected to the facet's hyperplane. Two adjacent facets are +convex if each centrum is clearly below the other facet. </p> + +<p>If adjacent facets are non-convex, one of the facets is merged +into a neighboring facet. Qhull merges the facet that is closest +to a neighboring facet. </p> + +<p>For option 'C-n', <i>n</i> is the centrum radius. For example, +'C-0.001' merges facets whenever the centrum is less than 0.001 +from a neighboring hyperplane. Qhull accounts for roundoff error +when testing the centrum.</p> + +<p>In 5-d and higher, you should set '<a href="qh-optq.htm#Qx">Qx</a>' +along with 'C-n'. It skips merges of coplanar facets until after +the hull is constructed and before '<a href="#An2">An</a>' and '<a +href="#Cn2">Cn</a>' are checked. </p> + +<h3><a href="#prec">»</a><a name="Cn2">Cn - centrum radius for +post-merging</a></h3> + +<p>Post-merging occurs after Qhull constructs the hull. It is +indicated by '<a href="#Cn2">Cn</a>' or '<a href="#An2">An</a>'. </p> + +<p>For option '<a href="#Cn2">Cn</a>', <i>n</i> is the centrum +radius. For example, 'C0.001' merges facets when the centrum is +less than 0.001 from a neighboring hyperplane. Qhull accounts for +roundoff error when testing the centrum.</p> + +<p>Both pre-merging and post-merging may be defined. If only +post-merging is used ('<a href="qh-optq.htm#Q0">Q0</a>' with +'Cn'), Qhull may fail to produce a hull due to precision errors +during the hull's construction.</p> + +<h3><a href="#prec">»</a><a name="En">En - max roundoff error +for distance computations</a></h3> + +<p>This allows the user to change the maximum roundoff error +computed by Qhull. The value computed by Qhull may be overly +pessimistic. If 'En' is set too small, then the output may not be +convex. The statistic "max. distance of a new vertex to a +facet" (from option '<a href="qh-optt.htm#Ts">Ts</a>') is a +reasonable upper bound for the actual roundoff error. </p> + +<h3><a href="#prec">»</a><a name="Rn">Rn - randomly perturb +computations </a></h3> + +<p>This option perturbs every distance, hyperplane, and angle +computation by up to <i>(+/- n * max_coord)</i>. It simulates the +effect of roundoff errors. Unless '<a href="#En">En</a>' is +explicitly set, it is adjusted for 'Rn'. The command 'qhull Rn' +will generate a convex hull despite the perturbations. See the <a +href="qh-eg.htm#merge">Examples </a>section for an example.</p> + +<p>Options 'Rn C-n' have the effect of '<a href="#Wn">W2n</a>' +and '<a href="#Cn">C-2n</a>'. To use time as the random number +seed, use option '<a href="qh-optq.htm#QRn">QR-1</a>'.</p> + +<h3><a href="#prec">»</a><a name="Un">Un - max distance for a +new, coplanar point </a></h3> + +<p>This allows the user to set coplanarity. When pre-merging ('<a +href="#Cn">C-n </a>', '<a href="#An">A-n</a>' or '<a +href="qh-optq.htm#Qx">Qx</a>'), Qhull merges a new point into any +coplanar facets. The default value for 'Un' is '<a href="#Vn">Vn</a>'.</p> + +<h3><a href="#prec">»</a><a name="Vn">Vn - min distance for a +visible facet </a></h3> + +<p>This allows the user to set facet visibility. When adding a +point to the convex hull, Qhull determines all facets that are +visible from the point. A facet is visible if the distance from +the point to the facet is greater than 'Vn'.</p> + +<p>Without merging, the default value for 'Vn' is the roundoff +error ('<a href="#En">En</a>'). With merging, the default value +is the pre-merge centrum ('<a href="#Cn">C-n</a>') in 2-d or 3-d, +or three times that in other dimensions. If the outside width is +specified with option '<a href="#Wn">Wn </a>', the maximum, +default value for 'Vn' is '<a href="#Wn">Wn</a>'.</p> + +<p>Qhull warns if 'Vn' is greater than '<a href="#Wn">Wn</a>' and +furthest outside ('<a href="qh-optq.htm#Qf">Qf</a>') is not +selected; this combination usually results in flipped facets +(i.e., reversed normals).</p> + +<h3><a href="#prec">»</a><a name="Wn">Wn - min distance above +plane for outside points</a></h3> + +<p>Points are added to the convex hull only if they are clearly +outside of a facet. A point is outside of a facet if its distance +to the facet is greater than 'Wn'. Without pre-merging, the +default value for 'Wn' is '<a href="#En">En </a>'. If the user +specifies pre-merging and does not set 'Wn', than 'Wn' is set to +the maximum of '<a href="#Cn">C-n</a>' and <i>maxcoord*(1 - </i><a +href="#An"><i>A-n</i></a><i>)</i>.</p> + +<p>This option is good for <a href="qh-impre.htm#approximate">approximating</a> +a convex hull.</p> + +<p>Options '<a href="qh-optq.htm#Qc">Qc</a>' and '<a +href="qh-optq.htm#Qi">Qi</a>' use the minimum vertex to +distinguish coplanar points from interior points.</p> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optf.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optf.htm new file mode 100644 index 0000000000000000000000000000000000000000..887ef4f67613cfcee7164ad668e8c185c7ee68d7 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optf.htm @@ -0,0 +1,719 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull format options (F)</title> +</head> + +<body><!-- Navigation links --> +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <A href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <A href="qh-quick.htm#programs">Programs</a> +• <A href="qh-quick.htm#options">Options</a> +• <A href="qh-opto.htm#output">Output</a> +• <A href="qh-optf.htm#format">Formats</a> +• <A href="qh-optg.htm#geomview">Geomview</a> +• <A href="qh-optp.htm#print">Print</a> +• <A href="qh-optq.htm#qhull">Qhull</a> +• <A href="qh-optc.htm#prec">Precision</a> +• <A href="qh-optt.htm#trace">Trace</a></p> +<hr> +<!-- Main text of document --> +<h1><a + href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><IMG + align=middle alt=[delaunay] height=100 + src="qh--dt.gif" width=100 ></a> Qhull format options (F)</h1> + +<p>This section lists the format options for Qhull. These options +are indicated by 'F' followed by a letter. See <A + href="qh-opto.htm#output" >Output</a>, <A href="qh-optp.htm#print">Print</a>, +and <A href="qh-optg.htm#geomview">Geomview</a> for other output +options. </p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<p><A href="index.htm#TOC">»</a> <A href="qh-quick.htm#programs">Programs</a> +<a name="format">•</a> <A href="qh-quick.htm#options">Options</a> +• <A href="qh-opto.htm#output">Output</a> +• <A href="qh-optf.htm#format">Formats</a> +• <A href="qh-optg.htm#geomview">Geomview</a> +• <A href="qh-optp.htm#print">Print</a> +• <A href="qh-optq.htm#qhull">Qhull</a> +• <A href="qh-optc.htm#prec">Precision</a> +• <A href="qh-optt.htm#trace">Trace</a></p> + +<h2>Additional input & output formats</h2> + +<p>These options allow for automatic processing of Qhull output. +Options '<A href="qh-opto.htm#i">i</a>', '<A href="qh-opto.htm#o">o</a>', +'<A href="qh-opto.htm#n">n</a>', and '<A href="qh-opto.htm#p">p</a>' +may also be used.</p> + +<dl compact> + <dt> + <dd><b>Summary and control</b> + <dt><A href="#FA">FA</a> + <dd>compute total area and volume for option '<A + href="qh-opto.htm#s">s</a>' + + <dt><A href="#FV">FV</a> + <dd>print average vertex (interior point for '<A + href="qhalf.htm">qhalf</a>') + <dt><A href="#FQ">FQ</a> + <dd>print command for qhull and input + <dt><A href="#FO">FO</a> + <dd>print options to stderr or stdout + <dt><A href="#FS">FS</a> + <dd>print sizes: total area and volume + <dt><A href="#Fs">Fs</a> + <dd>print summary: dim, #points, total vertices and + facets, #vertices, #facets, max outer and inner plane + <dt><A href="#Fd">Fd</a> + <dd>use format for input (offset first) + <dt><A href="#FD">FD</a> + <dd>use cdd format for normals (offset first) + <dt><a href="#FM">FM</a> + <dd>print Maple output (2-d and 3-d) + <dt> + <dt> + <dd><b>Facets, points, and vertices</b> + <dt><A href="#Fa">Fa</a> + <dd>print area for each facet + <dt><A href="#FC">FC</a> + <dd>print centrum for each facet + <dt><A href="#Fc">Fc</a> + <dd>print coplanar points for each facet + <dt><A href="#Fx">Fx</a> + <dd>print extreme points (i.e., vertices) of convex hull. + + <dt><A href="#FF">FF</a> + <dd>print facets w/o ridges + <dt><A href="#FI">FI</a> + <dd>print ID for each facet + <dt><A href="#Fi">Fi</a> + <dd>print inner planes for each facet + <dt><A href="#Fm">Fm</a> + <dd>print merge count for each facet (511 max) + <dt><A href="#FP">FP</a> + <dd>print nearest vertex for coplanar points + <dt><A href="#Fn">Fn</a> + <dd>print neighboring facets for each facet + <dt><A href="#FN">FN</a> + <dd>print neighboring facets for each point + <dt><A href="#Fo">Fo</a> + <dd>print outer planes for each facet + <dt><A href="#Ft">Ft</a> + <dd>print triangulation with added points + <dt><A href="#Fv">Fv</a> + <dd>print vertices for each facet + <dt> + <dt> + <dd><b>Delaunay, Voronoi, and halfspace</b> + <dt><A href="#Fx">Fx</a> + <dd>print extreme input sites of Delaunay triangulation + or Voronoi diagram. + <dt><A href="#Fp">Fp</a> + <dd>print points at halfspace intersections + <dt><A href="#Fi2">Fi</a> + <dd>print separating hyperplanes for inner, bounded + Voronoi regions + <dt><A href="#Fo2">Fo</a> + <dd>print separating hyperplanes for outer, unbounded + Voronoi regions + <dt><A href="#Fv2">Fv</a> + <dd>print Voronoi diagram as ridges for each input pair + <dt><A href="#FC">FC</a> + <dd>print Voronoi vertex ("center") for each facet</dd> +</dl> + +<hr> + +<h3><A href="#format">»</a><a name="Fa">Fa - print area for each +facet </a></h3> + +<p>The first line is the number of facets. The remaining lines +are the area for each facet, one facet per line. See '<A + href="#FA" >FA</a>' for computing the total area and volume.</p> + +<p>Use '<A href="qh-optp.htm#PAn">PAn</a>' for printing the n +largest facets. Use option '<A href="qh-optp.htm#PFn">PFn</a>' +for printing facets larger than <i>n</i>.</p> + +<p>For Delaunay triangulations, the area is the area of each +Delaunay triangle. For Voronoi vertices, the area is the area of +the dual facet to each vertex. </p> + +<p>Qhull uses the centrum and ridges to triangulate +non-simplicial facets. The area for non-simplicial facets is the +sum of the areas for each triangle. It is an approximation of the +actual area. The ridge's vertices are projected to the facet's +hyperplane. If a vertex is far below a facet (qh_WIDEcoplanar in <tt>user.h</tt>), +the corresponding triangles are ignored.</p> + +<p>For non-simplicial facets, vertices are often below the +facet's hyperplane. If so, the approximation is less than the +actual value and it may be significantly less. </p> + +<h3><A href="#format">»</a><a name="FA">FA - compute total area +and volume for option 's' </a></h3> + +<p>With option 'FA', Qhull includes the total area and volume in +the summary ('<A href="qh-opto.htm#s">s</a>'). If facets are +merged, the area and volume are approximations. Option 'FA' is +automatically set for options '<A href="#Fa">Fa </a>', '<A + href="qh-optp.htm#PAn" >PAn</a>', and '<A href="qh-optp.htm#PFn">PFn</a>'. +</p> + +<p>With '<A href="qdelaun.htm">qdelaunay</a> <A + href="qh-opto.htm#s" >s</a> FA', Qhull computes the total area of +the Delaunay triangulation. This equals the volume of the convex +hull of the data points. With options '<A href="qdelau_f.htm">qdelaunay Qu</a> +<A href="qh-opto.htm#s">s</a> FA', Qhull computes the +total area of the furthest-site Delaunay triangulation. This +equals of the total area of the Delaunay triangulation. </p> + +<p>See '<A href="#Fa">Fa</a>' for further details. </p> + +<h3><A href="#format">»</a><a name="Fc">Fc - print coplanar +points for each facet </a></h3> + +<p>The output starts with the number of facets. Then each facet +is printed one per line. Each line is the number of coplanar +points followed by the point ids. </p> + +<p>By default, option 'Fc' reports coplanar points +('<A href="qh-optq.htm#Qc">Qc</a>'). You may also use +option '<A href="qh-optq.htm#Qi">Qi</a>'. Options 'Qi Fc' prints +interior points while 'Qci Fc' prints both coplanar and interior +points. + +<p>Each coplanar point or interior point is assigned to the +facet it is furthest above (resp., least below). </p> + +<p>Use 'Qc <A href="qh-opto.htm#p">p</a>' to print vertex and +coplanar point coordinates. Use '<A href="qh-optf.htm#Fv">Fv</a>' +to print vertices. </p> + +<h3><A href="#format">»</a><a name="FC">FC - print centrum or +Voronoi vertex for each facet </a></h3> + +<p>The output starts with the dimension followed by the number of +facets. Then each facet centrum is printed, one per line. For +<A href="qvoronoi.htm">qvoronoi</a>, Voronoi vertices are +printed instead. </p> + +<h3><A href="#format">»</a><a name="Fd">Fd - use cdd format for +input </a></h3> + +<p>The input starts with comments. The first comment is reported +in the summary. Data starts after a "begin" line. The +next line is the number of points followed by the dimension plus +one and "real" or "integer". Then the points +are listed with a leading "1" or "1.0". The +data ends with an "end" line.</p> + +<p>For halfspaces ('<A href="qhalf.htm">qhalf</a> Fd'), +the input format is the same. Each halfspace starts with its +offset. The signs of the offset and coefficients are the +opposite of Qhull's +convention. The first two lines of the input may be an interior +point in '<A href="#FV">FV</a>' format.</p> + +<h3><A href="#format">»</a><a name="FD">FD - use cdd format for +normals </a></h3> + +<p>Option 'FD' prints normals ('<A href="qh-opto.htm#n">n</a>', '<A + href="#Fo" >Fo</a>', '<A href="#Fi">Fi</a>') or points ('<A + href="qh-opto.htm#p" >p</a>') in cdd format. The first line is the +command line that invoked Qhull. Data starts with a +"begin" line. The next line is the number of normals or +points followed by the dimension plus one and "real". +Then the normals or points are listed with the offset before the +coefficients. The offset for points is 1.0. For normals, +the offset and coefficients use the opposite sign from Qhull. +The data ends with an "end" line.</p> + +<h3><A href="#format">»</a><a name="FF">FF - print facets w/o +ridges </a></h3> + +<p>Option 'FF' prints all fields of all facets (as in '<A + href="qh-opto.htm#f" >f</a>') without printing the ridges. This is +useful in higher dimensions where a facet may have many ridges. +For simplicial facets, options 'FF' and '<A href="qh-opto.htm#f">f +</a>' are equivalent.</p> + +<h3><A href="#format">»</a><a name="Fi">Fi - print inner planes +for each facet </a></h3> + +<p>The first line is the dimension plus one. The second line is +the number of facets. The remainder is one inner plane per line. +The format is the same as option '<A href="qh-opto.htm#n">n</a>'.</p> + +<p>The inner plane is a plane that is below the facet's vertices. +It is an offset from the facet's hyperplane. It includes a +roundoff error for computing the vertex distance.</p> + +<p>Note that the inner planes for Geomview output ('<A + href="qh-optg.htm#Gi" >Gi</a>') include an additional offset for +vertex visualization and roundoff error. </p> + +<h3><A href="#format">»</a><a name="Fi2">Fi - print separating +hyperplanes for inner, bounded Voronoi regions</a></h3> + +<p>With <A href="qvoronoi.htm" >qvoronoi</a>, 'Fi' prints the +separating hyperplanes for inner, bounded regions of the Voronoi +diagram. The first line is the number of ridges. Then each +hyperplane is printed, one per line. A line starts with the +number of indices and floats. The first pair of indices indicates +an adjacent pair of input sites. The next <i>d</i> floats are the +normalized coefficients for the hyperplane, and the last float is +the offset. The hyperplane is oriented toward '<A + href="qh-optq.htm#QVn" >QVn</a>' (if defined), or the first input +site of the pair. </p> + +<p>Use '<A href="qh-optf.htm#Fo2">Fo</a>' for unbounded regions, +and '<A href="qh-optf.htm#Fv2">Fv</a>' for the corresponding +Voronoi vertices. </p> + +<p>Use '<A href="qh-optt.htm#Tv">Tv</a>' to verify that the +hyperplanes are perpendicular bisectors. It will list relevant +statistics to stderr. The hyperplane is a perpendicular bisector +if the midpoint of the input sites lies on the plane, all Voronoi +vertices in the ridge lie on the plane, and the angle between the +input sites and the plane is ninety degrees. This is true if all +statistics are zero. Roundoff and computation errors make these +non-zero. The deviations appear to be largest when the +corresponding Delaunay triangles are large and thin; for example, +the Voronoi diagram of nearly cospherical points. </p> + +<h3><A href="#format">»</a><a name="FI">FI - print ID for each +facet </a></h3> + +<p>Print facet identifiers. These are used internally and listed +with options '<A href="qh-opto.htm#f">f</a>' and '<A href="#FF">FF</a>'. +Options '<A href="#Fn">Fn </a>' and '<A href="#FN">FN</a>' use +facet identifiers for negative indices. </p> + +<h3><A href="#format">»</a><a name="Fm">Fm - print merge count +for each facet </a></h3> + +<p>The first line is the number of facets. The remainder is the +number of merges for each facet, one per line. At most 511 merges +are reported for a facet. See '<A href="qh-optp.htm#PMn">PMn</a>' +for printing the facets with the most merges. </p> + +<h3><a href="#format">»</a><a name="FM">FM - print Maple +output </a></h3> + +<p>Qhull writes a Maple file for 2-d and 3-d convex hulls, +2-d and 3-d halfspace intersections, +and 2-d Delaunay triangulations. Qhull produces a 2-d +or 3-d plot. + +<p><i>Warning</i>: This option has not been tested in Maple. + +<p>[From T. K. Abraham with help from M. R. Feinberg and N. Platinova.] +The following steps apply while working within the +Maple worksheet environment : +<ol> +<li>Generate the data and store it as an array . For example, in 3-d, data generated +in Maple is of the form : x[i],y[i],z[i] +<p> +<li>Create a single variable and assign the entire array of data points to this variable. +Use the "seq" command within square brackets as shown in the following example. +(The square brackets are essential for the rest of the steps to work.) +<p> +>data:=[seq([x[i],y[i],z[i]],i=1..n)]:# here n is the number of data points + +<li>Next we need to write the data to a file to be read by qhull. Before +writing the data to a file, make sure that the qhull executable files and +the data file lie in the same subdirectory. If the executable files are +stored in the "C:\qhull3.1\" subdirectory, then save the file in the same +subdirectory, say "C:\qhull3.1\datafile.txt". For the sake of integrity of +the data file , it is best to first ensure that the data file does not +exist before writing into the data file. This can be done by running a +delete command first . To write the data to the file, use the "writedata" +and the "writedata[APPEND]" commands as illustrated in the following example : +<p> +>system("del c:\\qhull3.1\\datafile.txt");#To erase any previous versions of the file +<br>>writedata("c:\\qhull3.1\\datafile.txt ",[3, nops(data)]);#writing in qhull format +<br>>writedata[APPEND]("c:\\ qhull3.1\\datafile.txt ", data);#writing the data points +<li> +Use the 'FM' option to produce Maple output. Store the output as a ".mpl" file. +For example, using the file we created above, we type the following (in DOS environment) +<p> +qconvex s FM <datafile.txt >dataplot.mpl + +<li> +To read 3-d output in Maple, we use the 'read' command followed by +a 'display3d' command. For example (in Maple environment): +<p> +>with(plots): +<br>>read `c:\\qhull3.1\\dataplot.mpl`:#IMPORTANT - Note that the punctuation mark used is ' and NOT '. The correct punctuation mark is the one next to the key for "1" (not the punctuation mark near the enter key) +<br>> qhullplot:=%: +<br>> display3d(qhullplot); +</ol> + +<p>For Delaunay triangulation orthogonal projection is better. + +<p>For halfspace intersections, Qhull produces the dual +convex hull. + +<p>See <a href="qh-faq.htm#math">Is Qhull available for Maple?</a> +for other URLs. + +<h3><A href="#format">»</a><a name="Fn">Fn - print neighboring +facets for each facet </a></h3> + +<p>The output starts with the number of facets. Then each facet +is printed one per line. Each line is the number of neighbors +followed by an index for each neighbor. The indices match the +other facet output formats.</p> + +<p>A negative index indicates an unprinted facet due to printing +only good facets ('<A href="qh-optp.htm#Pg">Pg</a>', <A href="qdelaun.htm" >qdelaunay</a>, +<A href="qvoronoi.htm" >qvoronoi</a>). It +is the negation of the facet's ID (option '<A href="#FI">FI</a>'). +For example, negative indices are used for facets "at +infinity" in the Delaunay triangulation.</p> + +<h3><A href="#format">»</a><a name="FN">FN - print neighboring +facets for each point </a></h3> + +<p>The first line is the number of points. Then each point is +printed, one per line. For unassigned points (either interior or +coplanar), the line is "0". For assigned coplanar +points ('<A href="qh-optq.htm#Qc">Qc</a>'), the line is +"1" followed by the index of the facet that is furthest +below the point. For assigned interior points ('<A + href="qh-optq.htm#Qi" >Qi</a>'), the line is "1" +followed by the index of the facet that is least above the point. +For vertices that do not belong to good facet, the line is +"0"</p> + +<p>For vertices of good facets, the line is the number of +neighboring facets followed by the facet indices. The indices +correspond to the other '<A href="#format">F</a>' formats. In 4-d +and higher, the facets are sorted by index. In 3-d, the facets +are in adjacency order (not oriented).</p> + +<p>A negative index indicates an unprinted facet due to printing +only good facets (<A href="qdelaun.htm" >qdelaunay</a>, +<A href="qvoronoi.htm" >qvoronoi</a>, '<A href="qh-optp.htm#Pdk">Pdk</a>', +'<A href="qh-optp.htm#Pg">Pg</a>'). It is the negation of the +facet's ID ('<A href="#FI"> FI</a>'). For example, negative +indices are used for facets "at infinity" in the +Delaunay triangulation.</p> + +<p>For Voronoi vertices, option 'FN' lists the vertices of the +Voronoi region for each input site. Option 'FN' lists the regions +in site ID order. Option 'FN' corresponds to the second half of +option '<A href="qh-opto.htm#o">o</a>'. To convert from 'FN' to '<A + href="qh-opto.htm#o" >o</a>', replace negative indices with zero +and increment non-negative indices by one. </p> + +<p>If you are using the <A href="qh-in.htm#library">Qhull +library</a>, option 'FN' has the side effect of reordering the +neighbors for a vertex </p> + +<h3><A href="#format">»</a><a name="Fo">Fo - print outer planes +for each facet </a></h3> + +<p>The first line is the dimension plus one. The second line is +the number of facets. The remainder is one outer plane per line. +The format is the same as option '<A href="qh-opto.htm#n">n</a>'.</p> + +<p>The outer plane is a plane that is above all points. It is an +offset from the facet's hyperplane. It includes a roundoff error +for computing the point distance. When testing the outer plane +(e.g., '<A href="qh-optt.htm#Tv">Tv</a>'), another roundoff error +should be added for the tested point.</p> + +<p>If outer planes are not checked ('<A href="qh-optq.htm#Q5">Q5</a>') +or not computed (!qh_MAXoutside), the maximum, computed outside +distance is used instead. This can be much larger than the actual +outer planes.</p> + +<p>Note that the outer planes for Geomview output ('<A + href="qh-optg.htm#G" >G</a>') include an additional offset for +vertex/point visualization, 'lines closer,' and roundoff error.</p> + +<h3><A href="#format">»</a><a name="Fo2">Fo - print separating +hyperplanes for outer, unbounded Voronoi regions</a></h3> + +<p>With <A href="qvoronoi.htm" >qvoronoi</a>, 'Fo' prints the +separating hyperplanes for outer, unbounded regions of the +Voronoi diagram. The first line is the number of ridges. Then +each hyperplane is printed, one per line. A line starts with the +number of indices and floats. The first pair of indices indicates +an adjacent pair of input sites. The next <i>d</i> floats are the +normalized coefficients for the hyperplane, and the last float is +the offset. The hyperplane is oriented toward '<A + href="qh-optq.htm#QVn" >QVn</a>' (if defined), or the first input +site of the pair. </p> + +<p>Option 'Fo' gives the hyperplanes for the unbounded rays of +the unbounded regions of the Voronoi diagram. Each hyperplane +goes through the midpoint of the corresponding input sites. The +rays are directed away from the input sites. </p> + +<p>Use '<A href="qh-optf.htm#Fi2">Fi</a>' for bounded regions, +and '<A href="qh-optf.htm#Fv2">Fv</a>' for the corresponding +Voronoi vertices. Use '<A href="qh-optt.htm#Tv">Tv</a>' to verify +that the corresponding Voronoi vertices lie on the hyperplane. </p> + +<h3><A href="#format">»</a><a name="FO">FO - print list of +selected options </a></h3> + +<p>Lists selected options and default values to stderr. +Additional 'FO's are printed to stdout. </p> + +<h3><A href="#format">»</a><a name="Fp">Fp - print points at +halfspace intersections</a></h3> + +<p>The first line is the number of intersection points. The +remainder is one intersection point per line. A intersection +point is the intersection of <i>d</i> or more halfspaces from +'<A href="qhalf.htm">qhalf</a>'. It corresponds to a +facet of the dual polytope. The "infinity" point +[-10.101,-10.101,...] indicates an unbounded intersection.</p> + +<p>If [x,y,z] are the dual facet's normal coefficients and <i>b<0</i> +is its offset, the halfspace intersection occurs at +[x/-b,y/-b,z/-b] plus the interior point. If <i>b>=0</i>, the +halfspace intersection is unbounded. </p> + +<h3><A href="#format">»</a><a name="FP">FP - print nearest +vertex for coplanar points </a></h3> + +<p>The output starts with the number of coplanar points. Then +each coplanar point is printed one per line. Each line is the +point ID of the closest vertex, the point ID of the coplanar +point, the corresponding facet ID, and the distance. Sort the +lines to list the coplanar points nearest to each vertex. </p> + +<p>Use options '<A href="qh-optq.htm#Qc">Qc</a>' and/or '<A + href="qh-optq.htm#Qi" >Qi</a>' with 'FP'. Options 'Qc FP' prints +coplanar points while 'Qci FP' prints coplanar and interior +points. Option 'Qc' is automatically selected if 'Qi' is not +selected. + +<p>For Delaunay triangulations (<A href="qdelaun.htm" >qdelaunay</a> +or <A href="qvoronoi.htm" >qvoronoi</a>), a coplanar point is nearly +incident to a vertex. The distance is the distance in the +original point set.</p> + +<p>If imprecision problems are severe, Qhull will delete input +sites when constructing the Delaunay triangulation. Option 'FP' will +list these points along with coincident points.</p> + +<p>If there are many coplanar or coincident points and non-simplicial +facets are triangulated ('<A href="qh-optq.htm#Qt">Qt</a>'), option +'FP' may be inefficient. It redetermines the original vertex set +for each coplanar point.</p> + +<h3><A href="#format">»</a><a name="FQ">FQ - print command for +qhull and input </a></h3> + +<p>Prints qhull and input command, e.g., "rbox 10 s | qhull +FQ". Option 'FQ' may be repeated multiple times.</p> + +<h3><A href="#format">»</a><a name="Fs">Fs - print summary</a></h3> + +<p>The first line consists of number of integers ("10") +followed by the: +<ul> +<li>dimension +<li>number of points +<li>number of vertices +<li>number of facets +<li>number of vertices selected for output +<li>number of facets selected for output +<li>number of coplanar points for selected facets +<li>number of nonsimplicial or merged facets selected for + output +<LI>number of deleted vertices</LI> +<LI>number of triangulated facets ('<A href="qh-optq.htm#Qt">Qt</a>')</LI> +</ul> + +<p>The second line consists of the number of reals +("2") followed by the: +<ul> +<li>maximum offset to an outer plane +<li>minimum offset to an inner plane.</li> +</ul> +Roundoff and joggle are included. +<P></P> + +<p>For Delaunay triangulations and Voronoi diagrams, the +number of deleted vertices should be zero. If greater than zero, then the +input is highly degenerate and coplanar points are not necessarily coincident +points. For example, <tt>'RBOX 1000 s W1e-13 t995138628 | QHULL d Qbb'</tt> reports +deleted vertices; the input is nearly cospherical.</p> + +<P>Later versions of Qhull may produce additional integers or reals.</P> + +<h3><A href="#format">»</a><a name="FS">FS - print sizes</a></h3> + +<p>The first line consists of the number of integers +("0"). The second line consists of the number of reals +("2"), followed by the total facet area, and the total +volume. Later versions of Qhull may produce additional integers +or reals.</p> + +<p>The total volume measures the volume of the intersection of +the halfspaces defined by each facet. It is computed from the +facet area. Both area and volume are approximations for +non-simplicial facets. See option '<A href="#Fa">Fa </a>' for +further notes. </p> + +<h3><A href="#format">»</a><a name="Ft">Ft - print triangulation</a></h3> + +<p>Prints a triangulation with added points for non-simplicial +facets. The output is </p> + +<ul> + <li>The first line is the dimension + <li>The second line is the number of points, the number + of facets, and the number of ridges. + <li>All of the input points follow, one per line. + <li>The centrums follow, one per non-simplicial facet + <li>Then the facets follow as a list of point indices + preceded by the number of points. The simplices are + oriented. </li> +</ul> + +<p>For convex hulls with simplicial facets, the output is the +same as option '<A href="qh-opto.htm#o">o</a>'.</p> + +<p>The added points are the centrums of the non-simplicial +facets. Except for large facets, the centrum is the average +vertex coordinate projected to the facet's hyperplane. Large +facets may use an old centrum to avoid recomputing the centrum +after each merge. In either case, the centrum is clearly below +neighboring facets. See <A href="qh-impre.htm">Precision issues</a>. +</p> + +<p>The new simplices will not be clearly convex with their +neighbors and they will not satisfy the Delaunay property. They +may even have a flipped orientation. Use triangulated input ('<A + href="qh-optq.htm#Qt">Qt</a>') for Delaunay triangulations. + +<p>For Delaunay triangulations with simplicial facets, the output is the +same as option '<A href="qh-opto.htm#o">o</a>' without the lifted +coordinate. Since 'Ft' is invalid for merged Delaunay facets, option +'Ft' is not available for qdelaunay or qvoronoi. It may be used with +joggled input ('<A href="qh-optq.htm#QJn" >QJ</a>') or triangulated output ('<A + href="qh-optq.htm#Qt" >Qt</a>'), for example, rbox 10 c G 0.01 | qhull d QJ Ft</p> + +<p>If you add a point-at-infinity with '<A href="qh-optq.htm#Qz">Qz</a>', +it is printed after the input sites and before any centrums. It +will not be used in a Delaunay facet.</p> + +<h3><A href="#format">»</a><a name="Fv">Fv - print vertices for +each facet</a></h3> + +<p>The first line is the number of facets. Then each facet is +printed, one per line. Each line is the number of vertices +followed by the corresponding point ids. Vertices are listed in +the order they were added to the hull (the last one added is the +first listed). Similar to option '<A href="qh-opto.htm#i">i</a>'.</p> + +<h3><A href="#format">»</a><a name="Fv2">Fv - print Voronoi +diagram</a></h3> + +<p>With <A href="qvoronoi.htm" >qvoronoi</a>, 'Fv' prints the +Voronoi diagram or furthest-site Voronoi diagram. The first line +is the number of ridges. Then each ridge is printed, one per +line. The first number is the count of indices. The second pair +of indices indicates a pair of input sites. The remaining indices +list the corresponding ridge of Voronoi vertices. Vertex 0 is the +vertex-at-infinity. It indicates an unbounded ray. </p> + +<p>All vertices of a ridge are coplanar. If the ridge is +unbounded, add the midpoint of the pair of input sites. The +unbounded ray is directed from the Voronoi vertices to infinity. </p> + +<p>Use '<A href="qh-optf.htm#Fo2">Fo</a>' for separating +hyperplanes of outer, unbounded regions. Use '<A + href="qh-optf.htm#Fi2" >Fi</a>' for separating hyperplanes of +inner, bounded regions. </p> + +<p>Option 'Fv' does not list ridges that require more than one +midpoint. For example, the Voronoi diagram of cospherical points +lists zero ridges (e.g., 'rbox 10 s | qvoronoi Fv Qz'). +Other examples are the Voronoi diagrams of a rectangular mesh +(e.g., 'rbox 27 M1,0 | qvoronoi Fv') or a point set with +a rectangular corner (e.g., +'rbox P4,4,4 P4,2,4 P2,4,4 P4,4,2 10 | qvoronoi Fv'). +Both cases miss unbounded rays at the corners. +To determine these ridges, surround the points with a +large cube (e.g., 'rbox 10 s c G2.0 | qvoronoi Fv Qz'). +Please report any other cases that are missed. If you +can formally describe these cases or +write code to handle them, please send email to <A + href="mailto:qhull@qhull.org" >qhull@qhull.org</a>. </p> + +<h3><A href="#format">»</a><a name="FV">FV - print average +vertex </a></h3> + +<p>The average vertex is the average of all vertex coordinates. +It is an interior point for halfspace intersection. The first +line is the dimension and "1"; the second line is the +coordinates. For example,</p> + +<blockquote> + <p>qconvex FV <A + href="qh-opto.htm#n">n</a> | qhalf <A href="#Fp">Fp</a></p> +</blockquote> + +<p>prints the extreme points of the original point set (roundoff +included). </p> + +<h3><A href="#format">»</a><a name="Fx">Fx - print extreme +points (vertices) of convex hulls and Delaunay triangulations</a></h3> + +<p>The first line is the number of points. The following lines +give the index of the corresponding points. The first point is +'0'. </p> + +<p>In 2-d, the extreme points (vertices) are listed in +counterclockwise order (by qh_ORIENTclock in user.h). </p> + +<p>In 3-d and higher convex hulls, the extreme points (vertices) +are sorted by index. This is the same order as option '<A + href="qh-opto.htm#p" >p</a>' when it doesn't include coplanar or +interior points. </p> + +<p>For Delaunay triangulations, 'Fx' lists the extreme +points of the input sites (i.e., the vertices of their convex hull). The points +are unordered. <!-- Navigation links --> </p> + +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <A href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <A href="qh-quick.htm#programs">Programs</a> +• <A href="qh-quick.htm#options">Options</a> +• <A href="qh-opto.htm#output">Output</a> +• <A href="qh-optf.htm#format">Formats</a> +• <A href="qh-optg.htm#geomview">Geomview</a> +• <A href="qh-optp.htm#print">Print</a> +• <A href="qh-optq.htm#qhull">Qhull</a> +• <A href="qh-optc.htm#prec">Precision</a> +• <A href="qh-optt.htm#trace">Trace</a></p><!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><IMG align=middle + height=40 src="qh--geom.gif" width=40 ></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: +Sept. 25, 1995 --- <!-- hhmts start -->Last modified: see top +<!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optg.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optg.htm new file mode 100644 index 0000000000000000000000000000000000000000..c4fc5a77e67009c6dbdb5d4eb3f4f12d0e81b0b5 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optg.htm @@ -0,0 +1,271 @@ +<html> + +<head> +<title>Qhull Geomview options (G)</title> +</head> + +<body> +<!-- Navigation links --> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<hr> +<!-- Main text of document --> + +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a> Qhull Geomview options (G)</h1> + +This section lists the Geomview options for Qhull. These options are +indicated by 'G' followed by a letter. See +<a href="qh-opto.htm#output">Output</a>, <a href="qh-optp.htm#print">Print</a>, +and <a href="qh-optf.htm#format">Format</a> for other output options. + + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<p><a href="index.htm#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +<a name="geomview">•</a> <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<h2>Geomview output options</h2> + +<p><a href="http://www.geomview.org">Geomview</a> is the graphical +viewer for visualizing Qhull output in 2-d, 3-d and 4-d.</p> + +<p>Geomview displays each facet of the convex hull. The color of +a facet is determined by the coefficients of the facet's normal +equation. For imprecise hulls, Geomview displays the inner and +outer hull. Geomview can also display points, ridges, vertices, +coplanar points, and facet intersections. </p> + +<p>For 2-d Delaunay triangulations, Geomview displays the +corresponding paraboloid. Geomview displays the 2-d Voronoi +diagram. For halfspace intersections, it displays the +dual convex hull. </p> + +<dl compact> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="#G">G</a></dt> + <dd>display Geomview output</dd> + <dt><a href="#Gt">Gt</a></dt> + <dd>display transparent 3-d Delaunay triangulation</dd> + <dt><a href="#GDn">GDn</a></dt> + <dd>drop dimension n in 3-d and 4-d output </dd> + + <dt> </dt> + <dt> </dt> + <dd><b>Specific</b></dd> + <dt><a href="#Ga">Ga</a></dt> + <dd>display all points as dots</dd> + <dt><a href="#Gc">Gc</a></dt> + <dd>display centrums (2-d, 3-d)</dd> + <dt><a href="#Gp">Gp</a></dt> + <dd>display coplanar points and vertices as radii</dd> + <dt><a href="#Gh">Gh</a></dt> + <dd>display hyperplane intersections</dd> + <dt><a href="#Gi">Gi</a></dt> + <dd>display inner planes only (2-d, 3-d)</dd> + <dt><a href="#Go">Go</a></dt> + <dd>display outer planes only (2-d, 3-d)</dd> + <dt><a href="#Gr">Gr</a></dt> + <dd>display ridges (3-d)</dd> + <dt><a href="#Gv">Gv</a></dt> + <dd>display vertices as spheres</dd> + <dt><a href="#Gn">Gn</a></dt> + <dd>do not display planes</dd> + +</dl> + +<hr> + +<h3><a href="#geomview">»</a><a name="G">G - produce output for +viewing with Geomview</a></h3> + +<p>By default, option 'G' displays edges in 2-d, outer planes in +3-d, and ridges in 4-d.</p> + +<p>A ridge can be explicit or implicit. An explicit ridge is a <i>(d-1)</i>-dimensional +simplex between two facets. In 4-d, the explicit ridges are +triangles. An implicit ridge is the topological intersection of +two neighboring facets. It is the union of explicit ridges.</p> + +<p>For non-simplicial 4-d facets, the explicit ridges can be +quite complex. When displaying a ridge in 4-d, Qhull projects the +ridge's vertices to one of its facets' hyperplanes. Use '<a +href="#Gh">Gh</a>' to project ridges to the intersection of both +hyperplanes. This usually results in a cleaner display. </p> + +<p>For 2-d Delaunay triangulations, Geomview displays the +corresponding paraboloid. Geomview displays the 2-d Voronoi +diagram. For halfspace intersections, it displays the +dual convex hull. + +<h3><a href="#geomview">»</a><a name="Ga">Ga - display all +points as dots </a></h3> + +<p>Each input point is displayed as a green dot.</p> + +<h3><a href="#geomview">»</a><a name="Gc">Gc - display centrums +(3-d) </a></h3> + +<p>The centrum is defined by a green radius sitting on a blue +plane. The plane corresponds to the facet's hyperplane. If you +sight along a facet's hyperplane, you will see that all +neighboring centrums are below the facet. The radius is defined +by '<a href="qh-optc.htm#Cn">C-n</a>' or '<a +href="qh-optc.htm#Cn2">Cn</a>'.</p> + +<h3><a href="#geomview">»</a><a name="GDn">GDn - drop dimension +n in 3-d and 4-d output </a></h3> + +<p>The result is a 2-d or 3-d object. In 4-d, this corresponds to +viewing the 4-d object from the nth axis without perspective. +It's best to view 4-d objects in pieces. Use the '<a +href="qh-optp.htm#Pdk">Pdk</a>' '<a href="qh-optp.htm#Pg">Pg</a>' +'<a href="qh-optp.htm#PG">PG</a>' '<a href="qh-optq.htm#QGn">QGn</a>' +and '<a href="qh-optq.htm#QVn">QVn</a>' options to select a few +facets. If one of the facets is perpendicular to an axis, then +projecting along that axis will show the facet exactly as it is +in 4-d. If you generate many facets, use Geomview's <tt>ginsu</tt> +module to view the interior</p> + +<p>To view multiple 4-d dimensions at once, output the object +without 'GDn' and read it with Geomview's <tt>ndview</tt>. As you +rotate the object in one set of dimensions, you can see how it +changes in other sets of dimensions.</p> + +<p>For additional control over 4-d objects, output the object +without 'GDn' and read it with Geomview's <tt>4dview</tt>. You +can slice the object along any 4-d plane. You can also flip the +halfspace that's deleted when slicing. By combining these +features, you can get some interesting cross sections.</p> + +<h3><a href="#geomview">»</a><a name="Gh">Gh - display +hyperplane intersections (3-d, 4-d)</a></h3> + +<p>In 3-d, the intersection is a black line. It lies on two +neighboring hyperplanes, c.f., the blue squares associated with +centrums ('<a href="#Gc">Gc </a>'). In 4-d, the ridges are +projected to the intersection of both hyperplanes. If you turn on +edges (Geomview's 'appearances' menu), each triangle corresponds +to one ridge. The ridges may overlap each other.</p> + +<h3><a href="#geomview">»</a><a name="Gi">Gi - display inner +planes only (2-d, 3-d)</a></h3> + +<p>The inner plane of a facet is below all of its vertices. It is +parallel to the facet's hyperplane. The inner plane's color is +the opposite of the outer plane's color, i.e., <i>[1-r,1-g,1-b] </i>. +Its edges are determined by the vertices.</p> + +<h3><a href="#geomview">»</a><a name="Gn">Gn - do not display +planes </a></h3> + +<p>By default, Geomview displays the precise plane (no merging) +or both inner and output planes (if merging). If merging, +Geomview does not display the inner plane if the the difference +between inner and outer is too small.</p> + +<h3><a href="#geomview">»</a><a name="Go">Go - display outer +planes only (2-d, 3-d)</a></h3> + +<p>The outer plane of a facet is above all input points. It is +parallel to the facet's hyperplane. Its color is determined by +the facet's normal, and its edges are determined by the vertices.</p> + +<h3><a href="#geomview">»</a><a name="Gp">Gp - display coplanar +points and vertices as radii </a></h3> + +<p>Coplanar points ('<a href="qh-optq.htm#Qc">Qc</a>'), interior +points ('<a href="qh-optq.htm#Qi">Qi</a>'), outside points ('<a +href="qh-optt.htm#TCn">TCn</a>' or '<a href="qh-optt.htm#TVn">TVn</a>'), +and vertices are displayed as red and yellow radii. The radii are +perpendicular to the corresponding facet. Vertices are aligned +with an interior point. The radii define a ball which corresponds +to the imprecision of the point. The imprecision is the maximum +of the roundoff error, the centrum radius, and <i>maxcoord * (1 - +</i><a href="qh-optc.htm#An"><i>A-n</i></a><i>)</i>. It is at +least 1/20'th of the maximum coordinate, and ignores post merging +if pre-merging is done.</p> + +<p>If '<a href="qh-optg.htm#Gv">Gv</a>' (print vertices as +spheres) is also selected, option 'Gp' displays coplanar +points as radii. Select options <a href="qh-optq.htm#Qc">Qc</a>' +and/or '<a href="qh-optq.htm#Qi">Qi</a>'. Options 'Qc Gpv' displays +coplanar points while 'Qci Gpv' displays coplanar and interior +points. Option 'Qc' is automatically selected if 'Qi' is not +selected with options 'Gpv'. + +<h3><a href="#geomview">»</a><a name="Gr">Gr - display ridges +(3-d) </a></h3> + +<p>A ridge connects the two vertices that are shared by +neighboring facets. It is displayed in green. A ridge is the +topological edge between two facets while the hyperplane +intersection is the geometric edge between two facets. Ridges are +always displayed in 4-d.</p> + +<h3><a href="#geomview">»</a><a name="Gt">Gt - transparent 3-d +Delaunay </a></h3> + +<p>A 3-d Delaunay triangulation looks like a convex hull with +interior facets. Option 'Gt' removes the outside ridges to reveal +the outermost facets. It automatically sets options '<a +href="#Gr">Gr</a>' and '<a href="#GDn">GDn</a>'. See example <a +href="qh-eg.htm#17f">eg.17f.delaunay.3</a>.</p> + +<h3><a href="#geomview">»</a><a name="Gv">Gv - display vertices +as spheres (2-d, 3-d)</a></h3> + +<p>The radius of the sphere corresponds to the imprecision of the +data. See '<a href="#Gp">Gp</a>' for determining the radius.</p> + +<!-- Navigation links --> + +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> +<!-- GC common information --> + +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-opto.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-opto.htm new file mode 100644 index 0000000000000000000000000000000000000000..128bbcdb5adc5d5391c5f768e82c5089ebfec965 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-opto.htm @@ -0,0 +1,343 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull output options</title> +</head> + +<body> +<!-- Navigation links --> +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a> Qhull output options</h1> + +<p>This section lists the output options for Qhull. These options +are indicated by lower case characters. See <a +href="qh-optf.htm#format">Formats</a>, <a +href="qh-optp.htm#print">Print</a>, and <a +href="qh-optg.htm#geomview">Geomview</a> for other output +options. </p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<p><a href="index.htm#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +<a name="output">•</a> <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<h2>Output options</h2> + +<p>Qhull prints its output to standard out. All output is printed +text. The default output is a summary (option '<a href="#s">s</a>'). +Other outputs may be specified as follows. </p> + +<dl compact> + <dt><a href="#f">f</a></dt> + <dd>print all fields of all facets</dd> + <dt><a href="#n">n</a></dt> + <dd>print hyperplane normals with offsets</dd> + <dt><a href="#m">m</a></dt> + <dd>print Mathematica output (2-d and 3-d)</dd> + <dt><a href="#o">o</a></dt> + <dd>print OFF file format (dim, points and facets)</dd> + <dt><a href="#s">s</a></dt> + <dd>print summary to stderr</dd> + <dt><a href="#p">p</a></dt> + <dd>print vertex and point coordinates</dd> + <dt><a href="#i">i</a></dt> + <dd>print vertices incident to each facet </dd> + <dt> </dt> + <dt> </dt> + <dd><b>Related options</b></dd> + <dt><a href="qh-optf.htm#format">F</a></dt> + <dd>additional input/output formats</dd> + <dt><a href="qh-optg.htm#geomview">G</a></dt> + <dd>Geomview output</dd> + <dt><a href="qh-optp.htm#print">P</a></dt> + <dd>Print options</dd> + <dt><a href="qh-optf.htm#Ft">Ft</a></dt> + <dd>print triangulation with added points</dd> + <dt> </dt> +</dl> + +<hr> + +<h3><a href="#output">»</a><a name="f">f - print all fields of +all facets </a></h3> + +<p>Print <a href=../src/qhull.h#facetT>all fields</a> of all facets. +The facet is the primary <a href=index.htm#structure>data structure</a> for +Qhull. + +<p>Option 'f' is for +debugging. Most of the fields are available via the '<a +href="qh-optf.htm#format">F</a>' options. If you need specialized +information from Qhull, you can use the <a +href="qh-in.htm#library">Qhull library</a>.</p> + +<p>Use the '<a href="qh-optf.htm#FF">FF</a>' option to print the +facets but not the ridges. </p> + +<h3><a href="#output">»</a><a name="i">i - print vertices +incident to each facet </a></h3> + +<p>The first line is the number of facets. The remaining lines +list the vertices for each facet, one facet per line. The indices +are 0-relative indices of the corresponding input points. The +facets are oriented. Option '<a href="qh-optf.htm#Fv">Fv</a>' +displays an unoriented list of vertices with a vertex count per +line. Options '<a href="qh-opto.htm#o">o</a>' and '<a +href="qh-optf.htm#Ft">Ft</a>' displays coordinates for each +vertex prior to the vertices for each facet. </p> + +<p>Simplicial facets (e.g., triangles in 3-d) consist of <i>d</i> +vertices. Non-simplicial facets in 3-d consist of 4 or more +vertices. For example, a facet of a cube consists of 4 vertices.</p> + +<p>For 4-d and higher convex hulls and 3-d and higher Delaunay +triangulations, <i>d</i> vertices are listed for all facets. A +non-simplicial facet is triangulated with its centrum and each +ridge. The index of the centrum is higher than any input point. +Use option '<a href="qh-optf.htm#Fv">Fv</a>' to list the vertices +of non-simplicial facets as is. Use option '<a +href="qh-optf.htm#Ft">Ft</a>' to print the coordinates of the +centrums as well as those of the input points. </p> + +<h3><a href="#output">»</a><a name="m">m - print Mathematica +output </a></h3> + +<p>Qhull writes a Mathematica file for 2-d and 3-d convex hulls, +2-d and 3-d halfspace intersections, +and 2-d Delaunay triangulations. Qhull produces a list of +objects that you can assign to a variable in Mathematica, for +example: "<tt>list= << <outputfilename> </tt>". +If the object is 2-d, it can be visualized by "<tt>Show[Graphics[list]] +</tt>". For 3-d objects the command is "<tt>Show[Graphics3D[list]] +</tt>". Now the object can be manipulated by commands of the +form <tt>"Show[%, <parametername> -> +<newvalue>]</tt>". </p> + +<p>For Delaunay triangulation orthogonal projection is better. +This can be specified, for example, by "<tt>BoxRatios: +Show[%, BoxRatios -> {1, 1, 1e-8}]</tt>". To see the +meaningful side of the 3-d object used to visualize 2-d Delaunay, +you need to change the viewpoint: "<tt>Show[%, ViewPoint +-> {0, 0, -1}]</tt>". By specifying different viewpoints +you can slowly rotate objects. </p> + +<p>For halfspace intersections, Qhull produces the dual +convex hull. + +<p>See <a href="qh-faq.htm#math">Is Qhull available for Mathematica?</a> +for URLs. + +<h3><a href="#output">»</a><a name="n">n - print hyperplane +normals with offsets </a></h3> + +<p>The first line is the dimension plus one. The second line is +the number of facets. The remaining lines are the normals for +each facet, one normal per line. The facet's offset follows its +normal coefficients.</p> + +<p>The normals point outward, i.e., the convex hull satisfies <i>Ax +<= -b </i>where <i>A</i> is the matrix of coefficients and <i>b</i> +is the vector of offsets.</p> + +<p>If cdd output is specified ('<a href="qh-optf.htm#FD">FD</a>'), +Qhull prints the command line, the keyword "begin", the +number of facets, the dimension (plus one), the keyword +"real", and the normals for each facet. The facet's +negative offset precedes its normal coefficients (i.e., if the +origin is an interior point, the offset is positive). Qhull ends +the output with the keyword "end". </p> + +<h3><a href="#output">»</a><a name="o">o - print OFF file format +</a></h3> + +<p>The output is: </p> + +<ul> + <li>The first line is the dimension </li> + <li>The second line is the number of points, the number of + facets, and the number of ridges. </li> + <li>All of the input points follow, one per line. </li> + <li>Then Qhull prints the vertices for each facet. Each facet + is on a separate line. The first number is the number of + vertices. The remainder is the indices of the + corresponding points. The vertices are oriented in 2-d, + 3-d, and in simplicial facets. </li> +</ul> + +<p>Option '<a href="qh-optf.htm#Ft">Ft</a>' prints the same +information with added points for non-simplicial facets.</p> + +<p>Option '<a href="qh-opto.htm#i">i</a>' displays vertices +without the point coordinates. Option '<a href="qh-opto.htm#p">p</a>' +displays the point coordinates without vertex and facet information.</p> + +<p>In 3-d, Geomview can load the file directly if you delete the +first line (e.g., by piping through '<tt>tail +2</tt>').</p> + +<p>For Voronoi diagrams (<a href=qvoronoi.htm>qvoronoi</a>), option +'o' prints Voronoi vertices and Voronoi regions instead of input +points and facets. The first vertex is the infinity vertex +[-10.101, -10.101, ...]. Then, option 'o' lists the vertices in +the Voronoi region for each input site. The regions appear in +site ID order. In 2-d, the vertices of a Voronoi region are +sorted by adjacency (non-oriented). In 3-d and higher, the +Voronoi vertices are sorted by index. See the '<a +href="qh-optf.htm#FN">FN</a>' option for listing Voronoi regions +without listing Voronoi vertices.</p> + +<p>If you are using the Qhull library, options 'v o' have the +side effect of reordering the neighbors for a vertex.</p> + +<h3><a href="#output">»</a><a name="p">p - print vertex and +point coordinates </a></h3> + +<p>The first line is the dimension. The second line is the number +of vertices. The remaining lines are the vertices, one vertex per +line. A vertex consists of its point coordinates</p> + +<p>With the '<a href="qh-optg.htm#Gc">Gc</a>' and '<a +href="qh-optg.htm#Gi">Gi</a>' options, option 'p' also prints +coplanar and interior points respectively.</p> + +<p>For <a href=qvoronoi.htm>qvoronoi</a>, it prints the +coordinates of each Voronoi vertex.</p> + +<p>For <a href=qdelaun.htm>qdelaunay</a>, it prints the +input sites as lifted to a paraboloid. For <a href=qhalf.htm>qhalf</a> +it prints the dual points. For both, option 'p' is the same as the first +section of option '<a href="qh-opto.htm#o">o</a>'.</p> + +<p>Use '<a href="qh-optf.htm#Fx">Fx</a>' to list the point ids of +the extreme points (i.e., vertices). </p> + +<p>If a subset of the facets is selected ('<a +href="qh-optp.htm#Pdk">Pdk</a>', '<a href="qh-optp.htm#PDk">PDk</a>', +'<a href="qh-optp.htm#Pg">Pg</a>' options), option 'p' only +prints vertices and points associated with those facets.</p> + +<p>If cdd-output format is selected ('<a href="qh-optf.htm#FD">FD</a>'), +the first line is "begin". The second line is the +number of vertices, the dimension plus one, and "real". +The vertices follow with a leading "1". Output ends +with "end". </p> + +<h3><a href="#output">»</a><a name="s">s - print summary to +stderr </a></h3> + +<p>The default output of Qhull is a summary to stderr. Options '<a +href="qh-optf.htm#FS">FS</a>' and '<a href="qh-optf.htm#Fs">Fs</a>' +produce the same information for programs. <b>Note</b>: Windows 95 and 98 +treats stderr the same as stdout. Use option '<a href="qh-optt.htm#TO">TO file</a>' to separate +stderr and stdout.</p> + +<p>The summary lists the number of input points, the dimension, +the number of vertices in the convex hull, and the number of +facets in the convex hull. It lists the number of selected +("good") facets for options '<a href="qh-optp.htm#Pg">Pg</a>', +'<a href="qh-optp.htm#Pdk">Pdk</a>', <a href=qdelaun.htm>qdelaunay</a>, +or <a href=qvoronoi.htm>qvoronoi</a> (Delaunay triangulations only +use the lower half of a convex hull). It lists the number of +coplanar points. For Delaunay triangulations without '<a +href="qh-optq.htm#Qc">Qc</a>', it lists the total number of +coplanar points. It lists the number of simplicial facets in +the output.</p> + +<p>The terminology depends on the output structure. </p> + +<p>The summary lists these statistics:</p> + +<ul> + <li>number of points processed by Qhull </li> + <li>number of hyperplanes created</li> + <li>number of distance tests (not counting statistics, + summary, and checking) </li> + <li>number of merged facets (if any)</li> + <li>number of distance tests for merging (if any)</li> + <li>CPU seconds to compute the hull</li> + <li>the maximum joggle for '<a href="qh-optq.htm#QJn">QJ</a>'<br> + or, the probability of precision errors for '<a + href="qh-optq.htm#QJn">QJ</a> <a href="qh-optt.htm#TRn">TRn</a>' + </li> + <li>total area and volume (if computed, see '<a + href="qh-optf.htm#FS">FS</a>' '<a href="qh-optf.htm#FA">FA</a>' + '<a href="qh-optf.htm#Fa">Fa</a>' '<a + href="qh-optp.htm#PAn">PAn</a>')</li> + <li>max. distance of a point above a facet (if non-zero)</li> + <li>max. distance of a vertex below a facet (if non-zero)</li> +</ul> + +<p>The statistics include intermediate hulls. For example 'rbox d +D4 | qhull' reports merged facets even though the final hull is +simplicial. </p> + +<p>Qhull starts counting CPU seconds after it has read and +projected the input points. It stops counting before producing +output. In the code, CPU seconds measures the execution time of +function qhull() in <tt>qhull.c</tt>. If the number of CPU +seconds is clearly wrong, check qh_SECticks in <tt>user.h</tt>. </p> + +<p>The last two figures measure the maximum distance from a point +or vertex to a facet. They are not printed if less than roundoff +or if not merging. They account for roundoff error in computing +the distance (c.f., option '<a href="qh-optc.htm#Rn">Rn</a>'). +Use '<a href="qh-optf.htm#Fs">Fs</a>' to report the maximum outer +and inner plane. </p> + +<p>A number may appear in parentheses after the maximum distance +(e.g., 2.1x). It is the ratio between the maximum distance and +the worst-case distance due to merging two simplicial facets. It +should be small for 2-d, 3-d, and 4-d, and for higher dimensions +with '<a href="qh-optq.htm#Qx">Qx</a>'. It is not printed if less +than 0.05. </p> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optp.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optp.htm new file mode 100644 index 0000000000000000000000000000000000000000..fccb87bccb1f2f621048d8e6c111ab41c88fd35b --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optp.htm @@ -0,0 +1,249 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull print options (P)</title> +</head> + +<body> +<!-- Navigation links --> +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a> Qhull print options (P)</h1> + +This section lists the print options for Qhull. These options are +indicated by 'P' followed by a letter. See +<a href="qh-opto.htm#output">Output</a>, <a href="qh-optg.htm#geomview">Geomview</a>, +and <a href="qh-optf.htm#format">Format</a> for other output options. + + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<p><a href="index.htm#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +<a name="print">•</a> <a href="qh-quick.htm#options">Options</a> • <a +href="qh-opto.htm#output">Output</a> • <a +href="qh-optf.htm#format">Formats</a> • <a +href="qh-optg.htm#geomview">Geomview</a> • <a href="#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> • <a +href="qh-optc.htm#prec">Precision</a> • <a +href="qh-optt.htm#trace">Trace</a></p> + +<h2>Print options</h2> +<blockquote> +<dl compact> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="#Pp">Pp</a></dt> + <dd>do not report precision problems </dd> + <dt><a href="#Po">Po</a></dt> + <dd>force output despite precision problems</dd> + <dt><a href="#Po2">Po</a></dt> + <dd>if error, output neighborhood of facet</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Select</b></dd> + <dt><a href="#Pdk">Pdk:n</a></dt> + <dd>print facets with normal[k] >= n (default 0.0)</dd> + <dt><a href="#PDk">PDk:n</a></dt> + <dd>print facets with normal[k] <= n </dd> + <dt><a href="#PFn">PFn</a></dt> + <dd>print facets whose area is at least n</dd> + <dt><a href="#Pg">Pg</a></dt> + <dd>print good facets only (needs '<a href="qh-optq.htm#QGn">QGn</a>' + or '<a href="qh-optq.htm#QVn">QVn </a>')</dd> + <dt><a href="#PMn">PMn</a></dt> + <dd>print n facets with most merges</dd> + <dt><a href="#PAn">PAn</a></dt> + <dd>print n largest facets by area</dd> + <dt><a href="#PG">PG</a></dt> + <dd>print neighbors of good facets</dd> +</dl> +</blockquote> +<hr> + +<h3><a href="#print">»</a><a name="PAn">PAn - keep n largest +facets by area</a></h3> + +<p>The <i>n</i> largest facets are marked good for printing. This +may be useful for <a href="qh-impre.htm#approximate">approximating +a hull</a>. Unless '<a href="#PG">PG</a>' is set, '<a href="#Pg">Pg</a>' +is automatically set. </p> + +<h3><a href="#print">»</a><a name="Pdk">Pdk:n - print facet if +normal[k] >= n </a></h3> + +<p>For a given output, print only those facets with <i>normal[k] >= n</i> +and <i>drop</i> the others. For example, 'Pd0:0.5' prints facets with <i>normal[0] +>= 0.5 </i>. The default value of <i>n</i> is zero. For +example in 3-d, 'Pd0d1d2' prints facets in the positive octant. +<p> +If no facets match, the closest facet is returned.</p> +<p> +On Windows 95, do not combine multiple options. A 'd' is considered +part of a number. For example, use 'Pd0:0.5 Pd1:0.5' instead of +'Pd0:0.5d1:0.5'. + +<h3><a href="#print">»</a><a name="PDk">PDk:n - print facet if +normal[k] <= n</a></h3> + +<p>For a given output, print only those facets with <i>normal[k] <= n</i> +and <i>drop</i> the others. +For example, 'PD0:0.5' prints facets with <i>normal[0] +<= 0.5 </i>. The default value of <i>n</i> is zero. For +example in 3-d, 'PD0D1D2' displays facets in the negative octant. +<p> +If no facets match, the closest facet is returned.</p> + +<p>In 2-d, 'd G PD2' displays the Delaunay triangulation instead +of the corresponding paraboloid. </p> + +<p>Be careful of placing 'Dk' or 'dk' immediately after a real +number. Some compilers treat the 'D' as a double precision +exponent. </p> + +<h3><a href="#print">»</a><a name="PFn">PFn - keep facets whose +area is at least n</a></h3> + +<p>The facets with area at least <i>n</i> are marked good for +printing. This may be useful for <a +href="qh-impre.htm#approximate">approximating a hull</a>. Unless +'<a href="#PG">PG</a>' is set, '<a href="#Pg">Pg</a>' is +automatically set. </p> + +<h3><a href="#print">»</a><a name="Pg">Pg - print good facets </a></h3> + +<p>Qhull can mark facets as "good". This is used to</p> + +<ul> + <li>mark the lower convex hull for Delaunay triangulations + and Voronoi diagrams</li> + <li>mark the facets that are visible from a point (the '<a + href="qh-optq.htm#QGn">QGn </a>' option)</li> + <li>mark the facets that contain a point (the '<a + href="qh-optq.htm#QVn">QVn</a>' option).</li> + <li>indicate facets with a large enough area (options '<a + href="#PAn">PAn</a>' and '<a href="#PFn">PFn</a>')</li> +</ul> + +<p>Option '<a href="#Pg">Pg</a>' only prints good facets that +also meet '<a href="#Pdk">Pdk</a>' and '<a href="#PDk">PDk</a>' +options. It is automatically set for options '<a href="#PAn">PAn</a>', +'<a href="#PFn">PFn </a>', '<a href="qh-optq.htm#QGn">QGn</a>', +and '<a href="qh-optq.htm#QVn">QVn</a>'.</p> + +<h3><a href="#print">»</a><a name="PG">PG - print neighbors of +good facets</a></h3> + +<p>Option 'PG' can be used with or without option '<a href="#Pg">Pg</a>' +to print the neighbors of good facets. For example, options '<a +href="qh-optq.htm#QGn">QGn</a>' and '<a href="qh-optq.htm#QVn">QVn</a>' +print the horizon facets for point <i>n. </i></p> + +<h3><a href="#print">»</a><a name="PMn">PMn - keep n facets with +most merges</a></h3> + +<p>The n facets with the most merges are marked good for +printing. This may be useful for <a +href="qh-impre.htm#approximate">approximating a hull</a>. Unless +'<a href="#PG">PG</a>' is set, '<a href="#Pg">Pg</a>' is +automatically set. </p> + +<p>Use option '<a href="qh-optf.htm#Fm">Fm</a>' to print merges +per facet. + +<h3><a href="#print">»</a><a name="Po">Po - force output despite +precision problems</a></h3> + +<p>Use options 'Po' and '<a href="qh-optq.htm#Q0">Q0</a>' if you +can not merge facets, triangulate the output ('<a href="qh-optq.htm#Qt">Qt</a>'), +or joggle the input (<a href="qh-optq.htm#QJn">QJ</a>). + +<p>Option 'Po' can not force output when +duplicate ridges or duplicate facets occur. It may produce +erroneous results. For these reasons, merged facets, joggled input, or <a +href="qh-impre.htm#exact">exact arithmetic</a> are better.</p> + +<p>If you need a simplicial Delaunay triangulation, use +joggled input '<a href="qh-optq.htm#QJn">QJ</a>' or triangulated +output '<a +href="qh-optf.htm#Ft">Ft</a>'. + +<p>Option 'Po' may be used without '<a href="qh-optq.htm#Q0">Q0</a>' +to remove some steps from Qhull or to output the neighborhood of +an error.</p> + +<p>Option 'Po' may be used with option '<a href="qh-optq.htm#Q5">Q5</a>') +to skip qh_check_maxout (i.e., do not determine the maximum outside distance). +This can save a significant amount of time. + +<p>If option 'Po' is used,</p> + +<ul> + <li>most precision errors allow Qhull to continue. </li> + <li>verify ('<a href="qh-optt.htm#Tv">Tv</a>') does not check + coplanar points.</li> + <li>points are not partitioned into flipped facets and a + flipped facet is always visible to a point. This may + delete flipped facets from the output. </li> +</ul> + +<h3><a href="#print">»</a><a name="Po2">Po - if error, output +neighborhood of facet</a></h3> + +<p>If an error occurs before the completion of Qhull and tracing +is not active, 'Po' outputs a neighborhood of the erroneous +facets (if any). It uses the current output options.</p> + +<p>See '<a href="qh-optp.htm#Po">Po</a>' - force output despite +precision problems. + +<h3><a href="#print">»</a><a name="Pp">Pp - do not report +precision problems </a></h3> + +<p>With option 'Pp', Qhull does not print statistics about +precision problems, and it removes some of the warnings. It +removes the narrow hull warning.</p> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optq.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optq.htm new file mode 100644 index 0000000000000000000000000000000000000000..69dbc0a4a941b76b890d85bc2c97974e24c8fea4 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optq.htm @@ -0,0 +1,696 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull control options (Q)</title> +</head> + +<body> +<!-- Navigation links --> +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a> Qhull control options (Q)</h1> + +<p>This section lists the control options for Qhull. These +options are indicated by 'Q' followed by a letter. </p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<p><a href="index.htm#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +<a name="qhull">•</a> <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<h2>Qhull control options</h2> + +<dl compact> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="#Qu">Qu</a></dt> + <dd>compute upper hull for furthest-site Delaunay + triangulation </dd> + <dt><a href="#Qc">Qc</a></dt> + <dd>keep coplanar points with nearest facet</dd> + <dt><a href="#Qi">Qi</a></dt> + <dd>keep interior points with nearest facet</dd> + <dt><a href="#QJn">QJ</a></dt> + <dd>joggled input to avoid precision problems</dd> + <dt><a href="#Qt">Qt</a></dt> + <dd>triangulated output</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Precision handling</b></dd> + <dt><a href="#Qz">Qz</a></dt> + <dd>add a point-at-infinity for Delaunay triangulations</dd> + <dt><a href="#Qx">Qx</a></dt> + <dd>exact pre-merges (allows coplanar facets)</dd> + <dt><a href="#Qs">Qs</a></dt> + <dd>search all points for the initial simplex</dd> + <dt><a href="#Qbb">Qbb</a></dt> + <dd>scale last coordinate to [0,m] for Delaunay</dd> + <dt><a href="#Qv">Qv</a></dt> + <dd>test vertex neighbors for convexity</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Transform input</b></dd> + <dt><a href="#Qb0">Qbk:0Bk:0</a></dt> + <dd>drop dimension k from input</dd> + <dt><a href="#QRn">QRn</a></dt> + <dd>random rotation (n=seed, n=0 time, n=-1 time/no rotate)</dd> + <dt><a href="#Qbk">Qbk:n</a></dt> + <dd>scale coord[k] to low bound of n (default -0.5)</dd> + <dt><a href="#QBk">QBk:n</a></dt> + <dd>scale coord[k] to upper bound of n (default 0.5)</dd> + <dt><a href="#QbB">QbB</a></dt> + <dd>scale input to fit the unit cube</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Select facets</b></dd> + <dt><a href="#QVn">QVn</a></dt> + <dd>good facet if it includes point n, -n if not</dd> + <dt><a href="#QGn">QGn</a></dt> + <dd>good facet if visible from point n, -n for not visible</dd> + <dt><a href="#Qg">Qg</a></dt> + <dd>only build good facets (needs '<a href="#QGn">QGn</a>', '<a + href="#QVn">QVn </a>', or '<a href="qh-optp.htm#Pdk">Pdk</a>')</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Experimental</b></dd> + <dt><a href="#Q4">Q4</a></dt> + <dd>avoid merging old facets into new facets</dd> + <dt><a href="#Q5">Q5</a></dt> + <dd>do not correct outer planes at end of qhull</dd> + <dt><a href="#Q3">Q3</a></dt> + <dd>do not merge redundant vertices</dd> + <dt><a href="#Q6">Q6</a></dt> + <dd>do not pre-merge concave or coplanar facets</dd> + <dt><a href="#Q0">Q0</a></dt> + <dd>do not pre-merge facets with 'C-0' or 'Qx'</dd> + <dt><a href="#Q8">Q8</a></dt> + <dd>ignore near-interior points</dd> + <dt><a href="#Q2">Q2</a></dt> + <dd>merge all non-convex at once instead of independent sets</dd> + <dt><a href="#Qf">Qf</a></dt> + <dd>partition point to furthest outside facet</dd> + <dt><a href="#Q7">Q7</a></dt> + <dd>process facets depth-first instead of breadth-first</dd> + <dt><a href="#Q9">Q9</a></dt> + <dd>process furthest of furthest points</dd> + <dt><a href="#Q10">Q10</a></dt> + <dd>no special processing for narrow distributions</dd> + <dt><a href="#Q11">Q11</a></dt> + <dd>copy normals and recompute centrums for tricoplanar facets</dd> + <dt><a href="#Qm">Qm</a></dt> + <dd>process points only if they would increase the max. outer + plane </dd> + <dt><a href="#Qr">Qr</a></dt> + <dd>process random outside points instead of furthest one</dd> + <dt><a href="#Q1">Q1</a></dt> + <dd>sort merges by type instead of angle</dd> +</dl> + +<hr> + +<h3><a href="#qhull">»</a><a name="Qbb">Qbb - scale the last +coordinate to [0,m] for Delaunay</a></h3> + +<p>After scaling with option 'Qbb', the lower bound of the last +coordinate will be 0 and the upper bound will be the maximum +width of the other coordinates. Scaling happens after projecting +the points to a paraboloid and scaling other coordinates. </p> + +<p>Option 'Qbb' is automatically set for <a href=qdelaun.htm>qdelaunay</a> +and <a href=qvoronoi.htm>qvoronoi</a>. Option 'Qbb' is automatically set for joggled input '<a +href="qh-optq.htm#QJn">QJ</a>'. </p> + +<p>Option 'Qbb' should be used for Delaunay triangulations with +integer coordinates. Since the last coordinate is the sum of +squares, it may be much larger than the other coordinates. For +example, <tt>rbox 10000 D2 B1e8 | qhull d</tt> has precision +problems while <tt>rbox 10000 D2 B1e8 | qhull d Qbb</tt> is OK. </p> + +<h3><a href="#qhull">»</a><a name="QbB">QbB - scale the input to +fit the unit cube</a></h3> + +<p>After scaling with option 'QbB', the lower bound will be -0.5 +and the upper bound +0.5 in all dimensions. For different bounds +change qh_DEFAULTbox in <tt>user.h</tt> (0.5 is best for <a +href="index.htm#geomview">Geomview</a>).</p> + +<p>For Delaunay and Voronoi diagrams, scaling happens after +projection to the paraboloid. Under precise arithmetic, scaling +does not change the topology of the convex hull. Scaling may +reduce precision errors if coordinate values vary widely.</p> + +<h3><a href="#qhull">»</a><a name="Qbk">Qbk:n - scale coord[k] +to low bound</a></h3> + +<p>After scaling, the lower bound for dimension k of the input +points will be n. 'Qbk' scales coord[k] to -0.5. </p> + +<h3><a href="#qhull">»</a><a name="QBk">QBk:n - scale coord[k] +to upper bound </a></h3> + +<p>After scaling, the upper bound for dimension k of the input +points will be n. 'QBk' scales coord[k] to 0.5. </p> + +<h3><a href="#qhull">»</a><a name="Qb0">Qbk:0Bk:0 - drop +dimension k from the input points</a></h3> + +<p>Drop dimension<em> k </em>from the input points. For example, +'Qb1:0B1:0' deletes the y-coordinate from all input points. This +allows the user to take convex hulls of sub-dimensional objects. +It happens before the Delaunay and Voronoi transformation. +It happens after the halfspace transformation for both the data +and the feasible point.</p> + +<h3><a href="#qhull">»</a><a name="Qc">Qc - keep coplanar points +with nearest facet </a></h3> + +<p>During construction of the hull, a point is coplanar if it is +between '<a href="qh-optc.htm#Wn">Wn</a>' above and '<a +href="qh-optc.htm#Un">Un</a>' below a facet's hyperplane. A +different definition is used for output from Qhull. </p> + +<p>For output, a coplanar point is above the minimum vertex +(i.e., above the inner plane). With joggle ('<a +href="qh-optq.htm#QJn">QJ</a>'), a coplanar point includes points +within one joggle of the inner plane. </p> + +<p>With option 'Qc', output formats '<a href="qh-opto.htm#p">p </a>', +'<a href="qh-opto.htm#f">f</a>', '<a href="qh-optg.htm#Gp">Gp</a>', +'<a href="qh-optf.htm#Fc">Fc</a>', '<a href="qh-optf.htm#FN">FN</a>', +and '<a href="qh-optf.htm#FP">FP</a>' will print the coplanar +points. With options 'Qc <a href="#Qi">Qi</a>' these outputs +include the interior points.</p> + +<p>For Delaunay triangulations (<a href=qdelaun.htm>qdelaunay</a> +or <a href=qvoronoi.htm>qvoronoi</a>), a coplanar point is a point +that is nearly incident to a vertex. All input points are either +vertices of the triangulation or coplanar.</p> + +<p>Qhull stores coplanar points with a facet. While constructing +the hull, it retains all points within qh_RATIOnearInside +(user.h) of a facet. In qh_check_maxout(), it uses these points +to determine the outer plane for each facet. With option 'Qc', +qh_check_maxout() retains points above the minimum vertex for the +hull. Other points are removed. If qh_RATIOnearInside is wrong or +if options '<a href="#Q5">Q5</a> <a href="#Q8">Q8</a>' are set, a +coplanar point may be missed in the output (see <a +href="qh-impre.htm#limit">Qhull limitations</a>).</p> + +<h3><a href="#qhull">»</a><a name="Qf">Qf - partition point to +furthest outside facet </a></h3> + +<p>After adding a new point to the convex hull, Qhull partitions +the outside points and coplanar points of the old, visible +facets. Without the '<a href="qh-opto.htm#f">f </a>' option and +merging, it assigns a point to the first facet that it is outside +('<a href="qh-optc.htm#Wn">Wn</a>'). When merging, it assigns a +point to the first facet that is more than several times outside +(see qh_DISToutside in user.h).</p> + +<p>If option 'Qf' is selected, Qhull performs a directed search +(no merging) or an exhaustive search (merging) of new facets. +Option 'Qf' may reduce precision errors if pre-merging does not +occur.</p> + +<p>Option '<a href="#Q9">Q9</a>' processes the furthest of all +furthest points.</p> + +<h3><a href="#qhull">»</a><a name="Qg">Qg - only build good +facets (needs 'QGn' 'QVn' or 'Pdk') </a></h3> + +<p>Qhull has several options for defining and printing good +facets. With the '<a href="#Qg">Qg</a>' option, Qhull will only +build those facets that it needs to determine the good facets in +the output. This may speed up Qhull in 2-d and 3-d. It is +useful for furthest-site Delaunay +triangulations (<a href=qdelau_f.htm>qdelaunay Qu</a>, +invoke with 'qhull d Qbb <a href="#Qu">Qu</a> Qg'). +It is not effective in higher +dimensions because many facets see a given point and contain a +given vertex. It is not guaranteed to work for all combinations.</p> + +<p>See '<a href="#QGn">QGn</a>', '<a href="#QVn">QVn</a>', and '<a +href="qh-optp.htm#Pdk">Pdk</a>' for defining good facets, and '<a +href="qh-optp.htm#Pg">Pg</a>' and '<a href="qh-optp.htm#PG">PG</a>' +for printing good facets and their neighbors. If pre-merging ('<a +href="qh-optc.htm#Cn">C-n</a>') is not used and there are +coplanar facets, then 'Qg Pg' may produce a different result than +'<a href="qh-optp.htm#Pg">Pg</a>'. </p> + +<h3><a href="#qhull">»</a><a name="QGn">QGn - good facet if +visible from point n, -n for not visible </a></h3> + +<p>With option 'QGn', a facet is good (see '<a href="#Qg">Qg</a>' +and '<a href="qh-optp.htm#Pg">Pg</a>') if it is visible from +point n. If <i>n < 0</i>, a facet is good if it is not visible +from point n. Point n is not added to the hull (unless '<a +href="qh-optt.htm#TCn">TCn</a>' or '<a href="qh-optt.htm#TPn">TPn</a>').</p> + +<p>With <a href="rbox.htm">rbox</a>, use the 'Pn,m,r' option +to define your point; it will be point 0 ('QG0'). </p> + +<h3><a href="#qhull">»</a><a name="Qi">Qi - keep interior points +with nearest facet </a></h3> + +<p>Normally Qhull ignores points that are clearly interior to the +convex hull. With option 'Qi', Qhull treats interior points the +same as coplanar points. Option 'Qi' does not retain coplanar +points. You will probably want '<a href="#Qc">Qc </a>' as well. </p> + +<p>Option 'Qi' is automatically set for '<a href=qdelaun.htm>qdelaunay</a> +<a href="#Qc">Qc</a>' and '<a href=qvoronoi.htm>qvoronoi</a> +<a href="#Qc">Qc</a>'. If you use +'<a href=qdelaun.htm>qdelaunay</a> Qi' or '<a href=qvoronoi.htm>qvoronoi</a> +Qi', option '<a href="qh-opto.htm#s">s</a>' reports all nearly +incident points while option '<a href="qh-optf.htm#Fs">Fs</a>' +reports the number of interior points (should always be zero).</p> + +<p>With option 'Qi', output formats '<a href="qh-opto.htm#p">p</a>', +'<a href="qh-opto.htm#f">f</a>','<a href="qh-optg.htm#Gp">Gp</a>', +'<a href="qh-optf.htm#Fc">Fc</a>', '<a href="qh-optf.htm#FN">FN</a>', +and '<a href="qh-optf.htm#FP">FP</a>' include interior points. </p> + +<h3><a href="#qhull">»</a><a name="QJ">QJ</a> or <a name="QJn">QJn</a> - joggled +input to avoid precision errors</a></h3> + +<p>Option 'QJ' or 'QJn' joggles each input coordinate by adding a +random number in the range [-n,n]. If a precision error occurs, +It tries again. If precision errors still occur, Qhull increases <i>n</i> +ten-fold and tries again. The maximum value for increasing <i>n</i> +is 0.01 times the maximum width of the input. Option 'QJ' selects +a default value for <i>n</i>. <a href="../src/user.h#JOGGLEdefault">User.h</a> +defines these parameters and a maximum number of retries. See <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>Option 'QJ' also sets '<a href="qh-optq.htm#Qbb">Qbb</a>' for +Delaunay triangulations and Voronoi diagrams. It does not set +'Qbb' if '<a href="qh-optq.htm#Qbk">Qbk:n</a>' or '<a +href="qh-optq.htm#QBk">QBk:n</a>' are set. </p> + +<p>If 'QJn' is set, Qhull does not merge facets unless requested +to. All facets are simplicial (triangular in 2-d). This may be +important for your application. You may also use triangulated output +('<a href="qh-optq.htm#Qt">Qt</a>') or Option '<a href="qh-optf.htm#Ft">Ft</a>'. + +<p>Qhull adjusts the outer and inner planes for 'QJn' ('<a +href="qh-optf.htm#Fs">Fs</a>'). They are increased by <i>sqrt(d)*n</i> +to account for the maximum distance between a joggled point and +the corresponding input point. Coplanar points ('<a +href="qh-optq.htm#Qc">Qc</a>') require an additional <i>sqrt(d)*n</i> +since vertices and coplanar points may be joggled in opposite +directions. </p> + +<p>For Delaunay triangulations (<a href=qdelaun.htm>qdelaunay</a>) +and Voronoi diagrams (<a href=qvoronoi.htm>qvoronoi</a>), joggle +happens before lifting the input sites to a paraboloid. Instead of +'QJ', you may use triangulated output ('<a +href="qh-optq.htm#Qt">Qt</a>')</p> + +<p>By default, 'QJn' uses a fixed random number seed. To use time +as the random number seed, select '<a href="qh-optq.htm#QRn">QR-1</a>'. +The summary ('<a href="qh-opto.htm#s">s</a>') will show the +selected seed as 'QR-n'. + +<p>With 'QJn', Qhull does not error on degenerate hyperplane +computations. Except for Delaunay and Voronoi computations, Qhull +does not error on coplanar points. </p> + +<p>Use option '<a href="qh-optf.htm#FO">FO</a>' to display the +selected options. Option 'FO' displays the joggle and the joggle +seed. If Qhull restarts, it will redisplay the options. </p> + +<p>Use option '<a href="qh-optt.htm#TRn">TRn</a>' to estimate the +probability that Qhull will fail for a given 'QJn'. + +<h3><a href="#qhull">»</a><a name="Qm">Qm - only process points +that increase the maximum outer plane </a></h3> + +<p>Qhull reports the maximum outer plane in its summary ('<a +href="qh-opto.htm#s">s</a>'). With option 'Qm', Qhull does not +process points that are below the current, maximum outer plane. +This is equivalent to always adjusting '<a href="qh-optc.htm#Wn">Wn +</a>' to the maximum distance of a coplanar point to a facet. It +is ignored for points above the upper convex hull of a Delaunay +triangulation. Option 'Qm' is no longer important for merging.</p> + +<h3><a href="#qhull">»</a><a name="Qr">Qr - process random +outside points instead of furthest ones </a></h3> + +<p>Normally, Qhull processes the furthest point of a facet's +outside points. Option 'Qr' instead selects a random outside +point for processing. This makes Qhull equivalent to the +randomized incremental algorithms.</p> + +<p>The original randomization algorithm by Clarkson and Shor [<a +href="index.htm#cla-sho89">'89</a>] used a conflict list which +is equivalent to Qhull's outside set. Later randomized algorithms +retained the previously constructed facets. </p> + +<p>To compare Qhull to the randomized algorithms with option +'Qr', compare "hyperplanes constructed" and +"distance tests". Qhull does not report CPU time +because the randomization is inefficient. </p> + +<h3><a href="#qhull">»</a><a name="QRn">QRn - random rotation </a></h3> + +<p>Option 'QRn' randomly rotates the input. For Delaunay +triangulations (<a href=qdelaun.htm>qdelaunay</a> or <a href=qvoronoi.htm>qvoronoi</a>), +it rotates the lifted points about +the last axis. </p> + +<p>If <em>n=0</em>, use time as the random number seed. If <em>n>0</em>, +use n as the random number seed. If <em>n=-1</em>, don't rotate +but use time as the random number seed. If <em>n<-1</em>, +don't rotate but use <em>n</em> as the random number seed. </p> + +<h3><a href="#qhull">»</a><a name="Qs">Qs - search all points +for the initial simplex </a></h3> + +<p>Qhull constructs an initial simplex from <i>d+1</i> points. It +selects points with the maximum and minimum coordinates and +non-zero determinants. If this fails, it searches all other +points. In 8-d and higher, Qhull selects points with the minimum +x or maximum coordinate (see qh_initialvertices in <tt>poly2.c </tt>). +It rejects points with nearly zero determinants. This should work +for almost all input sets.</p> + +<p>If Qhull can not construct an initial simplex, it reports a +descriptive message. Usually, the point set is degenerate and one +or more dimensions should be removed ('<a href="#Qb0">Qbk:0Bk:0</a>'). +If not, use option 'Qs'. It performs an exhaustive search for the +best initial simplex. This is expensive is high dimensions. </p> + +<h3><a href="#qhull">»</a><a name="Qt">Qt - triangulated output</a></h3> + +<p>By default, qhull merges facets to handle precision errors. This +produces non-simplicial facets (e.g., the convex hull of a cube has 6 square +facets). Each facet is non-simplicial because it has four vertices. + +<p>Use option 'Qt' to triangulate all non-simplicial facets before generating +results. Alternatively, use joggled input ('<a href="#QJn">QJ</a>') to +prevent non-simplical facets. Unless '<a href="qh-optp.htm#Pp">Pp</a>' is set, +qhull produces a warning if 'QJ' and 'Qt' are used together. + +<p>Option 'Qt' may produce degenerate facets with zero area.</p> + +<p>Facet area and hull volumes may differ with and without +'Qt'. The triangulations are different and different triangles +may be ignored due to precision errors. + +<p>With sufficient merging, the ridges of a non-simplicial facet may share more than two neighboring facets. If so, their triangulation ('<a href="#Qt">Qt</a>') will fail since two facets have the same vertex set. </p> + +<h3><a href="#qhull">»</a><a name="Qu">Qu - compute upper hull +for furthest-site Delaunay triangulation </a></h3> + +<p>When computing a Delaunay triangulation (<a href=qdelaun.htm>qdelaunay</a> +or <a href=qvoronoi.htm>qvoronoi</a>), +Qhull computes both the the convex hull of points on a +paraboloid. It normally prints facets of the lower hull. These +correspond to the Delaunay triangulation. With option 'Qu', Qhull +prints facets of the upper hull. These correspond to the <a +href="qdelau_f.htm">furthest-site Delaunay triangulation</a> +and the <a href="qvoron_f.htm">furthest-site Voronoi diagram</a>.</p> + +<p>Option 'qhull d Qbb Qu <a href="#Qg">Qg</a>' may improve the speed of option +'Qu'. If you use the Qhull library, a faster method is 1) use +Qhull to compute the convex hull of the input sites; 2) take the +extreme points (vertices) of the convex hull; 3) add one interior +point (e.g., +'<a href="qh-optf.htm#FV">FV</a>', the average of <em>d</em> extreme points); 4) run +'qhull d Qbb Qu' or 'qhull v Qbb Qu' on these points.</p> + +<h3><a href="#qhull">»</a><a name="Qv">Qv - test vertex +neighbors for convexity </a></h3> + +<p>Normally, Qhull tests all facet neighbors for convexity. +Non-neighboring facets which share a vertex may not satisfy the +convexity constraint. This occurs when a facet undercuts the +centrum of another facet. They should still be convex. Option +'Qv' extends Qhull's convexity testing to all neighboring facets +of each vertex. The extra testing occurs after the hull is +constructed.. </p> + +<h3><a href="#qhull">»</a><a name="QVn">QVn - good facet if it +includes point n, -n if not </a></h3> + +<p>With option 'QVn', a facet is good ('<a href="#Qg">Qg</a>', +'<a href="qh-optp.htm#Pg">Pg</a>') if one of its vertices is +point n. If <i>n<0</i>, a good facet does not include point n. + +<p>If options '<a href="qh-optp.htm#PG">PG</a>' +and '<a href="#Qg">Qg</a>' are not set, option '<a href="qh-optp.htm#Pg">Pg</a>' +(print only good) +is automatically set. +</p> + +<p>Option 'QVn' behaves oddly with options '<a href="qh-optf.htm#Fx">Fx</a>' +and '<a href=qvoronoi.htm>qvoronoi</a> <a href="qh-optf.htm#Fv2">Fv</a>'. + +<p>If used with option '<a href="#Qg">Qg</a>' (only process good facets), point n is +either in the initial simplex or it is the first +point added to the hull. Options 'QVn Qg' require either '<a href="#QJn">QJ</a>' or +'<a href="#Q0">Q0</a>' (no merging).</p> + +<h3><a href="#qhull">»</a><a name="Qx">Qx - exact pre-merges +(allows coplanar facets) </a></h3> + +<p>Option 'Qx' performs exact merges while building the hull. +Option 'Qx' is set by default in 5-d and higher. Use option '<a +href="#Q0">Q0</a>' to not use 'Qx' by default. Unless otherwise +specified, option 'Qx' sets option '<a href="qh-optc.htm#C0">C-0</a>'. +</p> + +<p>The "exact" merges are merging a point into a +coplanar facet (defined by '<a href="qh-optc.htm#Vn">Vn </a>', '<a +href="qh-optc.htm#Un">Un</a>', and '<a href="qh-optc.htm#Cn">C-n</a>'), +merging concave facets, merging duplicate ridges, and merging +flipped facets. Coplanar merges and angle coplanar merges ('<a +href="qh-optc.htm#An">A-n</a>') are not performed. Concavity +testing is delayed until a merge occurs.</p> + +<p>After the hull is built, all coplanar merges are performed +(defined by '<a href="qh-optc.htm#Cn">C-n</a>' and '<a +href="qh-optc.htm#An">A-n</a>'), then post-merges are performed +(defined by '<a href="qh-optc.htm#Cn2">Cn</a>' and '<a +href="qh-optc.htm#An2">An</a>'). If facet progress is logged ('<a +href="qh-optt.htm#TFn">TFn</a>'), Qhull reports each phase and +prints intermediate summaries and statistics ('<a +href="qh-optt.htm#Ts">Ts</a>'). </p> + +<p>Without 'Qx' in 5-d and higher, options '<a +href="qh-optc.htm#Cn">C-n</a>' and '<a href="qh-optc.htm#An">A-n</a>' +may merge too many facets. Since redundant vertices are not +removed effectively, facets become increasingly wide. </p> + +<p>Option 'Qx' may report a wide facet. With 'Qx', coplanar +facets are not merged. This can produce a "dent" in an +intermediate hull. If a point is partitioned into a dent and it +is below the surrounding facets but above other facets, one or +more wide facets will occur. In practice, this is unlikely. To +observe this effect, run Qhull with option '<a href="#Q6">Q6</a>' +which doesn't pre-merge concave facets. A concave facet makes a +large dent in the intermediate hull.</p> + +<p>Option 'Qx' may set an outer plane below one of the input +points. A coplanar point may be assigned to the wrong facet +because of a "dent" in an intermediate hull. After +constructing the hull, Qhull double checks all outer planes with +qh_check_maxout in <tt>poly2.c </tt>. If a coplanar point is +assigned to the wrong facet, qh_check_maxout may reach a local +maximum instead of locating all coplanar facets. This appears to +be unlikely.</p> + +<h3><a href="#qhull">»</a><a name="Qz">Qz - add a +point-at-infinity for Delaunay triangulations</a></h3> + +<p>Option 'Qz' adds a point above the paraboloid of lifted sites +for a Delaunay triangulation. It allows the Delaunay +triangulation of cospherical sites. It reduces precision errors +for nearly cospherical sites.</p> + +<h3><a href="#qhull">»</a><a name="Q0">Q0 - no merging with C-0 +and Qx</a></h3> + +<p>Turn off default merge options '<a href="qh-optc.htm#C0">C-0</a>' +and '<a href="#Qx">Qx</a>'. </p> + +<p>With 'Q0' and without other pre-merge options, Qhull ignores +precision issues while constructing the convex hull. This may +lead to precision errors. If so, a descriptive warning is +generated. See <a href="qh-impre.htm">Precision issues</a>.</p> + +<h3><a href="#qhull">»</a><a name="Q1">Q1 - sort merges by type +instead of angle </a></h3> + +<p>Qhull sorts the coplanar facets before picking a subset of the +facets to merge. It merges concave and flipped facets first. Then +it merges facets that meet at a steep angle. With 'Q1', Qhull +sorts merges by type (coplanar, angle coplanar, concave) instead +of by angle. This may make the facets wider. </p> + +<h3><a href="#qhull">»</a><a name="Q2">Q2 - merge all non-convex +at once instead of independent sets </a></h3> + +<p>With 'Q2', Qhull merges all facets at once instead of +performing merges in independent sets. This may make the facets +wider. </p> + +<h3><a href="#qhull">»</a><a name="Q3">Q3 - do not merge +redundant vertices </a></h3> + +<p>With 'Q3', Qhull does not remove redundant vertices. In 6-d +and higher, Qhull never removes redundant vertices (since +vertices are highly interconnected). Option 'Q3' may be faster, +but it may result in wider facets. Its effect is easiest to see +in 3-d and 4-d.</p> + +<h3><a href="#qhull">»</a><a name="Q4">Q4 - avoid merging </a>old +facets into new facets</h3> + +<p>With 'Q4', Qhull avoids merges of an old facet into a new +facet. This sometimes improves facet width and sometimes makes it +worse. </p> + +<h3><a href="#qhull">»</a><a name="Q5">Q5 - do not correct outer +planes at end of qhull </a></h3> + +<p>When merging facets or approximating a hull, Qhull tests +coplanar points and outer planes after constructing the hull. It +does this by performing a directed search (qh_findbest in <tt>geom.c</tt>). +It includes points that are just inside the hull. </p> + +<p>With options 'Q5' or '<a href="qh-optp.htm#Po">Po</a>', Qhull +does not test outer planes. The maximum outer plane is used +instead. Coplanar points ('<a href="#Qc">Qc</a>') are defined by +'<a href="qh-optc.htm#Un">Un</a>'. An input point may be outside +of the maximum outer plane (this appears to be unlikely). An +interior point may be above '<a href="qh-optc.htm#Un">Un</a>' +from a hyperplane.</p> + +<p>Option 'Q5' may be used if outer planes are not needed. Outer +planes are needed for options '<a href="qh-opto.htm#s">s</a>', '<a +href="qh-optg.htm#G">G</a>', '<a href="qh-optg.htm#Go">Go </a>', +'<a href="qh-optf.htm#Fs">Fs</a>', '<a href="qh-optf.htm#Fo">Fo</a>', +'<a href="qh-optf.htm#FF">FF</a>', and '<a href="qh-opto.htm#f">f</a>'.</p> + +<h3><a href="#qhull">»</a><a name="Q6">Q6 - do not pre-merge +concave or coplanar facets </a></h3> + +<p>With 'Q6', Qhull does not pre-merge concave or coplanar +facets. This demonstrates the effect of "dents" when +using '<a href="#Qx">Qx</a>'. </p> + +<h3><a href="#qhull">»</a><a name="Q7">Q7 - depth-first +processing instead of breadth-first </a></h3> + +<p>With 'Q7', Qhull processes facets in depth-first order instead +of breadth-first order. This may increase the locality of +reference in low dimensions. If so, Qhull may be able to use +virtual memory effectively. </p> + +<p>In 5-d and higher, many facets are visible from each +unprocessed point. So each iteration may access a large +proportion of allocated memory. This makes virtual memory +ineffectual. Once real memory is used up, Qhull will spend most +of its time waiting for I/O.</p> + +<p>Under 'Q7', Qhull runs slower and the facets may be wider. </p> + +<h3><a href="#qhull">»</a><a name="Q8">Q8 - ignore near-interior +points </a></h3> + +<p>With 'Q8' and merging, Qhull does not process interior points +that are near to a facet (as defined by qh_RATIOnearInside in +user.h). This avoids partitioning steps. It may miss a coplanar +point when adjusting outer hulls in qh_check_maxout(). The best +value for qh_RATIOnearInside is not known. Options 'Q8 <a +href="#Qc">Qc</a>' may be sufficient. </p> + +<h3><a href="#qhull">»</a><a name="Q9">Q9 - process furthest of +furthest points </a></h3> + +<p>With 'Q9', Qhull processes the furthest point of all outside +sets. This may reduce precision problems. The furthest point of +all outside sets is not necessarily the furthest point from the +convex hull.</p> + +<h3><a href="#qhull">»</a><a name="Q10">Q10 - no special processing +for narrow distributions</a></h3> + +<p>With 'Q10', Qhull does not special-case narrow distributions. +See <a href=qh-impre.htm#limit>Limitations of merged facets</a> for +more information. + +<h3><a href="#qhull">»</a><a name="Q11">Q11 - copy normals and recompute +centrums for +tricoplanar facets</a></h3> + +Option '<a href="#Qt">Qt</a>' triangulates non-simplicial facets +into "tricoplanar" facets. +Normally tricoplanar facets share the same normal, centrum, and +Voronoi vertex. They can not be merged or replaced. With +option 'Q11', Qhull duplicates the normal and Voronoi vertex. +It recomputes the centrum. + +<p>Use 'Q11' if you use the Qhull library to add points +incrementally and call qh_triangulate() after each point. +Otherwise, Qhull will report an error when it tries to +merge and replace a tricoplanar facet. + +<p>With sufficient merging and new points, option 'Q11' may +lead to precision problems such +as duplicate ridges and concave facets. For example, if qh_triangulate() +is added to qh_addpoint(), RBOX 1000 s W1e-12 t1001813667 P0 | QHULL d Q11 Tv, +reports an error due to a duplicate ridge. + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optt.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optt.htm new file mode 100644 index 0000000000000000000000000000000000000000..8deed94527aa45253bb235dddb1ccbe4915e6d05 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-optt.htm @@ -0,0 +1,275 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull trace options (T)</title> +</head> + +<body> +<!-- Navigation links --> +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a> Qhull trace options (T)</h1> + +This section lists the trace options for Qhull. These options are +indicated by 'T' followed by a letter. + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<p><a href="index.htm#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +<a name="trace">•</a> <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<h2>Trace options</h2> + +<dl compact> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="#Tz">Tz</a></dt> + <dd>output error information to stdout instead of stderr</dd> + <dt><a href="#TI">TI file</a></dt> + <dd>input data from a file</dd> + <dt><a href="#TO">TO file</a></dt> + <dd>output results to a file</dd> + <dt><a href="#Ts">Ts</a></dt> + <dd>print statistics</dd> + <dt><a href="#TFn">TFn</a></dt> + <dd>report progress whenever n or more facets created</dd> + <dt><a href="#TRn">TRn</a></dt> + <dd>rerun qhull n times</dd> + <dt><a href="#Tv">Tv</a></dt> + <dd>verify result: structure, convexity, and point inclusion</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>Debugging</b></dd> + <dt><a href="#Tc">Tc</a></dt> + <dd>check frequently during execution</dd> + <dt><a href="#TVn2">TVn</a></dt> + <dd>stop qhull after adding point n </dd> + <dt><a href="#TCn">TCn</a></dt> + <dd>stop qhull after building cone for point n</dd> + <dt><a href="#TVn">TV-n</a></dt> + <dd>stop qhull before adding point n</dd> + <dt><a href="#Tn">T4</a></dt> + <dd>trace at level n, 4=all, 5=mem/gauss, -1= events</dd> + <dt><a href="#TWn">TWn</a></dt> + <dd>trace merge facets when width > n</dd> + <dt><a href="#TMn">TMn</a></dt> + <dd>turn on tracing at merge n</dd> + <dt><a href="#TPn">TPn</a></dt> + <dd>turn on tracing when point n added to hull</dd> +</dl> + +<hr> + +<h3><a href="#trace">»</a><a name="Tc">Tc - check frequently +during execution </a></h3> + +<p>Qhull includes frequent checks of its data structures. Option +'Tc' will catch most inconsistency errors. It is slow and should +not be used for production runs. Option '<a href="#Tv">Tv</a>' +performs the same checks after the hull is constructed.</p> + +<h3><a href="#trace">»</a><a name="TCn">TCn - stop qhull after +building cone for point n</a></h3> + +<p>Qhull builds a cone from the point to its horizon facets. +Option 'TCn' stops Qhull just after building the cone. The output +for '<a href="qh-opto.htm#f">f</a>' includes the cone and the old +hull.'. </p> + +<h3><a href="#trace">»</a><a name="TFn">TFn - report summary +whenever n or more facets created </a></h3> + +<p>Option 'TFn' reports progress whenever more than n facets are +created. The test occurs just before adding a new point to the +hull. During post-merging, 'TFn' reports progress after more than +<i>n/2</i> merges. </p> + +<h3><a href="#trace">»</a><a name="TI">TI file - input data from file</a></h3> + +<p>Input data from 'file' instead of stdin. The filename may not +contain spaces or use single quotes. +You may use I/O redirection +instead (e.g., 'rbox 10 | qdelaunay >results').</P> + +<h3><a href="#trace">»</a><a name="TMn">TMn - turn on tracing at +merge n </a></h3> + +<p>Turn on tracing at n'th merge. </p> + +<h3><a href="#trace">»</a><a name="Tn">Tn - trace at level n</a></h3> + +<p>Qhull includes full execution tracing. 'T-1' traces events. +'T1' traces the overall execution of the program. 'T2' and 'T3' +trace overall execution and geometric and topological events. +'T4' traces the algorithm. 'T5' includes information about memory +allocation and Gaussian elimination. 'T1' is useful for logging +progress of Qhull in high dimensions.</p> + +<p>Option 'Tn' can produce large amounts of output. Use options '<a +href="#TPn">TPn</a>', '<a href="#TWn">TWn</a>', and '<a href="#TMn">TMn</a>' to selectively +turn on tracing. Since all errors report the last processed +point, option '<a href="#TPn">TPn</a>' is particularly useful.</p> + +<p>Different executions of the same program may produce different +traces and different results. The reason is that Qhull uses hashing +to match ridges of non-simplicial facets. For performance reasons, +the hash computation uses +memory addresses which may change across executions. + +<h3><a href="#trace">»</a><a name="TO">TO file - output results to file</a></h3> + +<p>Redirect stdout to 'file'. The filename may be enclosed in +single quotes. Unix and Windows NT users may use I/O redirection +instead (e.g., 'rbox 10 | qdelaunay >results').</P> +<p> +Windows95 users should always use 'TO file'. If they use I/O redirection, +error output is not sent to the console. Qhull uses single quotes instead +of double quotes because a missing double quote can +freeze Windows95 (e.g., do not run, rbox 10 | qhull TO "x)</p> +<p> + +<h3><a href="#trace">»</a><a name="TPn">TPn - turn on tracing +when point n added to hull </a></h3> + +<p>Option 'TPn' turns on tracing when point n is added to +the hull. It also traces partitions of point n. This option +reduces the output size when tracing. It is the normal +method to determine the cause of a Qhull error. All Qhull errors +report the last point added. + +<p>Use options 'TPn <a href="qh-optt.htm#TVn">TVn</a>' to +trace the addition of point n to the convex hull and stop when done.</p> + +<p>If used with option '<a href="qh-optt.htm#TWn">TWn</a>', +'TPn' turns off tracing after adding point n to the hull. +Use options 'TPn TWn' to +trace the addition of point n to the convex hull, partitions +of point n, and wide merges.</p> + +<h3><a href="#trace">»</a><a name="TRn">TRn - rerun qhull n times</a></h3> + +<p>Option 'TRn' reruns Qhull n times. It is usually used +with '<a href="qh-optq.htm#QJn">QJn</a>' to determine the probability +that a given joggle will fail. The summary +('<a href="qh-opto.htm#s">s</a>') lists the failure +rate and the precision errors that occurred. +Option '<a href="#Ts">Ts</a>' will report statistics for +all of the runs. Trace and output options only apply to the last +run. An event trace, '<a href="#Tn">T-1</a>' reports events for all runs. + +<p>Tracing applies to the last run of Qhull. If an error +is reported, the options list the run number as "_run". +To trace this run, set 'TRn' to the same value.</p> + +<h3><a href="#trace">»</a><a name="Ts">Ts - print statistics </a></h3> + +<p>Option 'Ts' collects statistics and prints them to stderr. For +Delaunay triangulations, the angle statistics are restricted to +the lower or upper envelope.</p> + +<h3><a href="#trace">»</a><a name="Tv">Tv - verify result: +structure, convexity, and point inclusion </a></h3> + +<p>Option 'Tv' checks the topological structure, convexity, and +point inclusion. If precision problems occurred, facet convexity +is tested whether or not 'Tv' is selected. Option 'Tv' does not +check point inclusion if forcing output with '<a +href="qh-optp.htm#Po">Po</a>', or if '<a href="qh-optq.htm#Q5">Q5</a>' +is set. </p> + +<p>The convex hull of a set of points is the smallest polytope +that includes the points. Option 'Tv' tests point inclusion. +Qhull verifies that all points are below all outer planes +(facet->maxoutside). Point inclusion is exhaustive if merging +or if the facet-point product is small enough; otherwise Qhull +verifies each point with a directed search (qh_findbest). To +force an exhaustive test when using option '<a +href="qh-optc.htm#C0">C-0</a>' (default), use 'C-1e-30' instead. </p> + +<p>Point inclusion testing occurs after producing output. It +prints a message to stderr unless option '<a +href="qh-optp.htm#Pp">Pp</a>' is used. This allows the user to +interrupt Qhull without changing the output. </p> + +<p>With '<a href=qvoronoi.htm>qvoronoi</a> <a href="qh-optf.htm#Fi2">Fi</a>' +and '<a href=qvoronoi.htm>qvoronoi</a> <a href="qh-optf.htm#Fo2">Fo</a>', +option 'Tv' collects statistics that verify all Voronoi vertices lie +on the separating hyperplane, and for bounded regions, all +separating hyperplanes are perpendicular bisectors. + +<h3><a href="#trace">»</a><a name="TVn">TV-n - stop qhull before +adding point n</a></h3> + +<p>Qhull adds one point at a time to the convex hull. See <a +href="qh-eg.htm#how">how Qhull adds a point</a>. Option 'TV-n' +stops Qhull just before adding a new point. Output shows the hull +at this time.</p> + +<h3><a href="#trace">»</a><a name="TVn2">TVn - stop qhull after +adding point n</a></h3> + +<p>Option 'TVn' stops Qhull after it has added point n. Output +shows the hull at this time.</p> + +<h3><a href="#trace">»</a><a name="TWn">TWn - trace merge facets +when width > n </a></h3> + +<p>Along with TMn, this option allows the user to determine the +cause of a wide merge.</p> +<h3><a href="#trace">»</a><a name="Tz">Tz - send all output to +stdout </a></h3> + +<p>Redirect stderr to stdout. </p> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-quick.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-quick.htm new file mode 100644 index 0000000000000000000000000000000000000000..6d8d1104cbeacf78fb905d34157330b4b9dd3060 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qh-quick.htm @@ -0,0 +1,491 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull quick reference</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOC"><b>Up:</b></a> <a href="http://www.qhull.org">Home +page for Qhull</a> <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a> <br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a> <br> +<b>To:</b> <a href="qh-in.htm#TOC">Qhull internals</a><br> +<b>To:</b> <a href="../src/index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="../src/index.htm#TOC">Qhull files</a><br> +<b>To:</b> <a href="../src/qh-geom.htm">Geom</a> • <a href="../src/qh-globa.htm">Global</a> +• <a href="../src/qh-io.htm">Io</a> • <a href="../src/qh-mem.htm">Mem</a> +• <a href="../src/qh-merge.htm">Merge</a> • <a href="../src/qh-poly.htm">Poly</a> +• <a href="../src/qh-qhull.htm">Qhull</a> • <a href="../src/qh-set.htm">Set</a> +• <a href="../src/qh-stat.htm">Stat</a> • <a href="../src/qh-user.htm">User</a> +</p> + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/cone.html"><img +src="qh--cone.gif" alt="[cone]" align="middle" width="100" +height="100"></a> Qhull quick reference</h1> + +This section lists all programs and options in Qhull. + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<p> +<a name="programs"> </a> +<hr> +<b>Qhull programs</b> +<p><a href="#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<dl> + <dt><a href="qconvex.htm">qconvex</a> -- convex hull</dt> + <dd><a href="qconvex.htm#synopsis">sy</a>nopsis • <a + href="qconvex.htm#input">in</a>put • <a + href="qconvex.htm#outputs">ou</a>tputs • <a + href="qconvex.htm#controls">co</a>ntrols • <a + href="qconvex.htm#graphics">gr</a>aphics • <a + href="qconvex.htm#notes">no</a>tes • <a + href="qconvex.htm#conventions">co</a>nventions • <a + href="qconvex.htm#options">op</a>tions</dd> + <dt> </dt> + <dt><a href="qdelaun.htm">qdelaunay</a> -- Delaunay triangulation</dt> + <dd><a href="qdelaun.htm#synopsis">sy</a>nopsis • <a + href="qdelaun.htm#input">in</a>put • <a + href="qdelaun.htm#outputs">ou</a>tputs • <a + href="qdelaun.htm#controls">co</a>ntrols • <a + href="qdelaun.htm#graphics">gr</a>aphics • <a + href="qdelaun.htm#notes">no</a>tes • <a + href="qdelaun.htm#conventions">co</a>nventions • <a + href="qdelaun.htm#options">op</a>tions</dd> + <dt> </dt> + <dt><a href="qdelau_f.htm">qdelaunay Qu</a> -- furthest-site Delaunay triangulation</dt> + <dd><a href="qdelau_f.htm#synopsis">sy</a>nopsis • <a + href="qdelau_f.htm#input">in</a>put • <a + href="qdelau_f.htm#outputs">ou</a>tputs • <a + href="qdelau_f.htm#controls">co</a>ntrols • <a + href="qdelau_f.htm#graphics">gr</a>aphics • <a + href="qdelau_f.htm#notes">no</a>tes • <a + href="qdelau_f.htm#conventions">co</a>nventions • <a + href="qdelau_f.htm#options">op</a>tions</dd> + <dt> </dt> + <dt><a href="qhalf.htm">qhalf</a> -- halfspace intersection about a point</dt> + <dd><a href="qhalf.htm#synopsis">sy</a>nopsis • <a + href="qhalf.htm#input">in</a>put • <a + href="qhalf.htm#outputs">ou</a>tputs • <a + href="qhalf.htm#controls">co</a>ntrols • <a + href="qhalf.htm#graphics">gr</a>aphics • <a + href="qhalf.htm#notes">no</a>tes • <a + href="qhalf.htm#conventions">co</a>nventions • <a + href="qhalf.htm#options">op</a>tions</dd> + <dt> </dt> + <dt><a href="qvoronoi.htm">qvoronoi</a> -- Voronoi diagram</dt> + <dd><a href="qvoronoi.htm#synopsis">sy</a>nopsis • <a + href="qvoronoi.htm#input">in</a>put • <a + href="qvoronoi.htm#outputs">ou</a>tputs • + <a href="qvoronoi.htm#controls">co</a>ntrols • <a + href="qvoronoi.htm#graphics">gr</a>aphics • <a + href="qvoronoi.htm#notes">no</a>tes • <a + href="qvoronoi.htm#conventions">co</a>nventions • <a + href="qvoronoi.htm#options">op</a>tions</dd> + <dt> </dt> + <dt><a href="qvoron_f.htm">qvoronoi Qu</a> -- furthest-site Voronoi diagram</dt> + <dd><a href="qvoron_f.htm#synopsis">sy</a>nopsis • <a + href="qvoron_f.htm#input">in</a>put • <a + href="qvoron_f.htm#outputs">ou</a>tputs • <a + href="qvoron_f.htm#controls">co</a>ntrols • <a + href="qvoron_f.htm#graphics">gr</a>aphics • <a + href="qvoron_f.htm#notes">no</a>tes • <a + href="qvoron_f.htm#conventions">co</a>nventions • <a + href="qvoron_f.htm#options">op</a>tions</dd> + <dt> </dt> + <dt><a href="rbox.htm">rbox</a> -- generate point distributions for qhull</dt> + <dd><a href="rbox.htm#synopsis">sy</a>nopsis • <a + href="rbox.htm#outputs">ou</a>tputs • <a + href="rbox.htm#examples">ex</a>amples • <a + href="rbox.htm#notes">no</a>tes • <a + href="rbox.htm#options">op</a>tions</dd> + <dt> </dt> + <dt><a href="qhull.htm">qhull</a> -- convex hull and related structures</dt> + <dd><a href="qhull.htm#synopsis">sy</a>nopsis • <a + href="qhull.htm#input">in</a>put • <a + href="qhull.htm#outputs">ou</a>tputs • <a + href="qhull.htm#controls">co</a>ntrols • <a + href="qhull.htm#options">op</a>tions</dd> + </dl> +<a name="options"> </a> +<hr> +<b>Qhull options</b> + +<p><a href="#TOC">»</a> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a></p> + +<p><table> +<!-- Note: keep all lines the same for easy reorganization --> +<tr> +<td><nobr>'<a href=qh-optf.htm#Fa>Fa</a>' +Farea +</nobr></td><td><nobr>'<a href=qh-optf.htm#FA>FA</a>' +FArea-total +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fc>Fc</a>' +Fcoplanars +</nobr></td><td><nobr>'<a href=qh-optf.htm#FC>FC</a>' +FCentrums + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optf.htm#Fd>Fd</a>' +Fd-cdd-in +</nobr></td><td><nobr>'<a href=qh-optf.htm#FD>FD</a>' +FD-cdd-out +</nobr></td><td><nobr>'<a href=qh-optf.htm#FF>FF</a>' +FF-dump-xridge +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fi>Fi</a>' +Finner + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optf.htm#Fi2>Fi</a>' +Finner_bounded +</nobr></td><td><nobr>'<a href=qh-optf.htm#FI>FI</a>' +FIDs +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fm>Fm</a>' +Fmerges +</nobr></td><td><nobr>'<a href=qh-optf.htm#FM>FM</a>' +FMaple + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optf.htm#Fn>Fn</a>' +Fneighbors +</nobr></td><td><nobr>'<a href=qh-optf.htm#FN>FN</a>' +FNeigh-vertex +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fo>Fo</a>' +Fouter +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fo2>Fo</a>' +Fouter_unbounded + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optf.htm#FO>FO</a>' +FOptions +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fp>Fp</a>' +Fpoint-intersect +</nobr></td><td><nobr>'<a href=qh-optf.htm#FP>FP</a>' +FPoint_near +</nobr></td><td><nobr>'<a href=qh-optf.htm#FQ>FQ</a>' +FQhull + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optf.htm#Fs>Fs</a>' +Fsummary +</nobr></td><td><nobr>'<a href=qh-optf.htm#FS>FS</a>' +FSize +</nobr></td><td><nobr>'<a href=qh-optf.htm#Ft>Ft</a>' +Ftriangles +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fv>Fv</a>' +Fvertices + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optf.htm#Fv2>Fv</a>' +Fvoronoi +</nobr></td><td><nobr>'<a href=qh-optf.htm#FV>FV</a>' +FVertex-ave +</nobr></td><td><nobr>'<a href=qh-optf.htm#Fx>Fx</a>' +Fxtremes +</nobr></td><td colspan=2><nobr> +<a href=qh-impre.htm#joggle>Merged facets or joggled input</a> + +</nobr></td></tr> +<tr><td> </td></tr><tr> +<td><nobr>'<a href=qh-optp.htm#PAn>PAn</a>' +PArea-keep +</nobr></td><td><nobr>'<a href=qh-optp.htm#Pdk>Pdk:n</a>' +Pdrop_low +</nobr></td><td><nobr>'<a href=qh-optp.htm#PDk>PDk:n</a>' +Pdrop_high +</nobr></td><td><nobr>'<a href=qh-optp.htm#Pg>Pg</a>' +Pgood + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optp.htm#PFn>PFn</a>' +PFacet_area_keep +</nobr></td><td><nobr>'<a href=qh-optp.htm#PG>PG</a>' +PGood_neighbors +</nobr></td><td><nobr>'<a href=qh-optp.htm#PMn>PMn</a>' +PMerge-keep +</nobr></td><td><nobr>'<a href=qh-optp.htm#Po>Po</a>' +Poutput_forced + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optp.htm#Po2>Po</a>' +Poutput_error +</nobr></td><td><nobr>'<a href=qh-optp.htm#Pp>Pp</a>' +Pprecision_not + +</nobr></td></tr> +<tr><td> </td></tr><tr> +<td><nobr>'<a href=qhull.htm#outputs>d</a>' +delaunay +</nobr></td><td><nobr>'<a href=qhull.htm#outputs>v</a>' +voronoi +</nobr></td><td><nobr>'<a href=qh-optg.htm>G</a>' +Geomview +</nobr></td><td><nobr>'<a href=qhull.htm#outputs>H</a>' +Halfspace + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-opto.htm#f>f</a>' +facet_dump +</nobr></td><td><nobr>'<a href=qh-opto.htm#i>i</a>' +incidences +</nobr></td><td><nobr>'<a href=qh-opto.htm#m>m</a>' +mathematica +</nobr></td><td><nobr>'<a href=qh-opto.htm#n>n</a>' +normals + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-opto.htm#o>o</a>' +OFF_format +</nobr></td><td><nobr>'<a href=qh-opto.htm#p>p</a>' +points +</nobr></td><td><nobr>'<a href=qh-opto.htm#s>s</a>' +summary + +</nobr></td></tr> +<tr><td> </td></tr><tr> +<td><nobr>'<a href=qh-optg.htm#Gv>Gv</a>' +Gvertices +</nobr></td><td><nobr>'<a href=qh-optg.htm#Gp>Gp</a>' +Gpoints +</nobr></td><td><nobr>'<a href=qh-optg.htm#Ga>Ga</a>' +Gall_points +</nobr></td><td><nobr>'<a href=qh-optg.htm#Gn>Gn</a>' +Gno_planes + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optg.htm#Gi>Gi</a>' +Ginner +</nobr></td><td><nobr>'<a href=qh-optg.htm#Gc>Gc</a>' +Gcentrums +</nobr></td><td><nobr>'<a href=qh-optg.htm#Gh>Gh</a>' +Ghyperplanes +</nobr></td><td><nobr>'<a href=qh-optg.htm#Gr>Gr</a>' +Gridges + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optg.htm#Go>Go</a>' +Gouter +</nobr></td><td><nobr>'<a href=qh-optg.htm#GDn>GDn</a>' +GDrop_dim +</nobr></td><td><nobr>'<a href=qh-optg.htm#Gt>Gt</a>' +Gtransparent + +</nobr></td></tr> +<tr><td> </td></tr><tr> +<td><nobr>'<a href=qh-optt.htm#Tn>T4</a>' +T4_trace +</nobr></td><td><nobr>'<a href=qh-optt.htm#Tc>Tc</a>' +Tcheck_often +</nobr></td><td><nobr>'<a href=qh-optt.htm#Ts>Ts</a>' +Tstatistics +</nobr></td><td><nobr>'<a href=qh-optt.htm#Tv>Tv</a>' +Tverify + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optt.htm#Tz>Tz</a>' +Tz_stdout +</nobr></td><td><nobr>'<a href=qh-optt.htm#TFn>TFn</a>' +TFacet_log +<td><nobr>'<a href=qh-optt.htm#TI>TI file</a>' +TInput_file +</nobr></td><td><nobr>'<a href=qh-optt.htm#TPn>TPn</a>' +TPoint_trace + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optt.htm#TMn>TMn</a>' +TMerge_trace +</nobr></td><td><nobr>'<a href=qh-optt.htm#TO>TO file</a>' +TOutput_file +</nobr></td><td><nobr>'<a href=qh-optt.htm#TRn>TRn</a>' +TRerun +</nobr></td><td><nobr>'<a href=qh-optt.htm#TWn>TWn</a>' +TWide_trace + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optt.htm#TVn>TV-n</a>' +TVertex_stop_before +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optt.htm#TVn2>TVn</a>' +TVertex_stop_after +</nobr></td><td><nobr>'<a href=qh-optt.htm#TCn>TCn</a>' +TCone_stop_after + +</nobr></td></tr> +<tr><td> </td></tr><tr> +<td><nobr>'<a href=qh-optc.htm#An>A-n</a>' +Angle_max_pre +</nobr></td><td><nobr>'<a href=qh-optc.htm#An2>An</a>' +Angle_max_post +</nobr></td><td><nobr>'<a href=qh-optc.htm#C0>C-0</a>' +Centrum_roundoff +</nobr></td><td><nobr>'<a href=qh-optc.htm#Cn>C-n</a>' +Centrum_size_pre + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optc.htm#Cn2>Cn</a>' +Centrum_size_post +</nobr></td><td><nobr>'<a href=qh-optc.htm#En>En</a>' +Error_round +</nobr></td><td><nobr>'<a href=qh-optc.htm#Rn>Rn</a>' +Random_dist +</nobr></td><td><nobr>'<a href=qh-optc.htm#Vn>Vn</a>' +Visible_min + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optc.htm#Un>Un</a>' +Ucoplanar_max +</nobr></td><td><nobr>'<a href=qh-optc.htm#Wn>Wn</a>' +Wide_outside + +</nobr></td></tr> +<tr><td> </td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Qbk>Qbk:n</a>' +Qbound_low +</nobr></td><td><nobr>'<a href=qh-optq.htm#QBk>QBk:n</a>' +QBound_high +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qb0>Qbk:0Bk:0</a>' +Qbound_drop +</nobr></td><td><nobr>'<a href=qh-optq.htm#QbB>QbB</a>' +QbB-scale-box + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Qbb>Qbb</a>' +Qbb-scale-last +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qc>Qc</a>' +Qcoplanar +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qf>Qf</a>' +Qfurthest +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qg>Qg</a>' +Qgood_only + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#QGn>QGn</a>' +QGood_point +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qi>Qi</a>' +Qinterior +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qm>Qm</a>' +Qmax_out +</nobr></td><td><nobr>'<a href=qh-optq.htm#QJn>QJn</a>' +QJoggle + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Qr>Qr</a>' +Qrandom +</nobr></td><td><nobr>'<a href=qh-optq.htm#QRn>QRn</a>' +QRotate +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qs>Qs</a>' +Qsearch_1st +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qt>Qt</a>' +Qtriangulate + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Qu>Qu</a>' +QupperDelaunay +</nobr><td><nobr>'<a href=qh-optq.htm#QVn>QVn</a>' +QVertex_good +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qv>Qv</a>' +Qvneighbors +</nobr></td><td><nobr>'<a href=qh-optq.htm#Qx>Qx</a>' +Qxact_merge + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Qz>Qz</a>' +Qzinfinite + +</nobr></td></tr> +<tr><td> </td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Q0>Q0</a>' +Q0_no_premerge +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q1>Q1</a>' +Q1_no_angle +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q2>Q2</a>' +Q2_no_independ +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q3>Q3</a>' +Q3_no_redundant + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Q4>Q4</a>' +Q4_no_old +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q5>Q5</a>' +Q5_no_check_out +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q6>Q6</a>' +Q6_no_concave +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q7>Q7</a>' +Q7_depth_first + +</nobr></td></tr><tr> +<td><nobr>'<a href=qh-optq.htm#Q8>Q8</a>' +Q8_no_near_in +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q9>Q9</a>' +Q9_pick_furthest +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q10>Q10</a>' +Q10_no_narrow +</nobr></td><td><nobr>'<a href=qh-optq.htm#Q11>Q11</a>' +Q11_trinormals +</td></tr></table> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home +page for Qhull</a> <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a> <br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="qh-in.htm#TOC">Qhull internals</a><br> +<b>To:</b> <a href="../src/index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="../src/index.htm#TOC">Qhull files</a><br> +<b>To:</b> <a href="../src/qh-geom.htm">Geom</a> • <a href="../src/qh-globa.htm">Global</a> +• <a href="../src/qh-io.htm">Io</a> • <a href="../src/qh-mem.htm">Mem</a> +• <a href="../src/qh-merge.htm">Merge</a> • <a href="../src/qh-poly.htm">Poly</a> +• <a href="../src/qh-qhull.htm">Qhull</a> • <a href="../src/qh-set.htm">Set</a> +• <a href="../src/qh-stat.htm">Stat</a> • <a href="../src/qh-user.htm">User</a><br> +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhalf.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhalf.htm new file mode 100644 index 0000000000000000000000000000000000000000..4d02879d9a3d1e0a1ecb60b35704aeb99fb61fd8 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhalf.htm @@ -0,0 +1,594 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>qhalf -- halfspace intersection about a point</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up</b></a><b>:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/half.html"><img +src="qh--half.gif" alt="[halfspace]" align="middle" width="100" +height="100"></a>qhalf -- halfspace intersection about a point</h1> + +<p>The intersection of a set of halfspaces is a polytope. The +polytope may be unbounded. See Preparata & Shamos [<a +href="index.htm#pre-sha85">'85</a>] for a discussion. In low +dimensions, halfspace intersection may be used for linear +programming. </p> + +<blockquote> +<dl compact> + <dt><b>Example:</b> rbox c | qconvex <a href="qh-optf.htm#FQ">FQ</a> <a href="qh-optf.htm#FV">FV</a> + <a href="qh-opto.htm#n">n</a> | qhalf <a + href="qh-optf.htm#Fp">Fp</a></dt> + <dd>Print the intersection of the facets of a cube. <tt>rbox c</tt> + generates the vertices of a cube. <tt>qconvex FV n</tt> returns of average + of the cube's vertices (in this case, the origin) and the halfspaces + that define the cube. <tt>qhalf Fp</tt> computes the intersection of + the halfspaces about the origin. The intersection is the vertices + of the original cube.</dd> + + <dt><p><b>Example:</b> rbox c d G0.55 | qconvex <a href="qh-optf.htm#FQ">FQ</a> <a href="qh-optf.htm#FV">FV</a> + <a href="qh-opto.htm#n">n</a> | qhalf <a + href="qh-optf.htm#Fp">Fp</a></dt> + <dd>Print the intersection of the facets of a cube and a diamond. There + are 24 facets and 14 intersection points. Four facets define each diamond + vertext. Six facets define each cube vertex. + </dd> + + <dt><p><b>Example:</b> rbox c d G0.55 | qconvex <a href="qh-optf.htm#FQ">FQ</a> <a href="qh-optf.htm#FV">FV</a> + <a href="qh-opto.htm#n">n</a> | qhalf <a + href="qh-optf.htm#Fp">Fp</a> + <a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>Same as above except triangulate before computing + the intersection points. Three facets define each intersection + point. There are two duplicates of the diamond and four duplicates of the cube. + </dd> +</dl> +</blockquote> + +<p>Qhull computes a halfspace intersection by the geometric +duality between points and halfspaces. +See <a href="qh-eg.htm#half">halfspace examples</a>, +<a href="#notes">qhalf notes</a>, and +option 'p' of <a href="#outputs">qhalf outputs</a>. </p> + +<p>By default, halfspace intersections may be defined by more than +<i>d</i> halfspaces. See the previous cube and diamond example. +This is the expected output for halfspace intersection. + +<p>You can try triangulated output and joggled input. It demonstrates +that triangulated output is more accurate than joggled input. + +<p>If you use '<a href="qh-optq.htm#Qt">Qt</a>' (triangulated output), all +halfspace intersections are simplicial (e.g., three halfspaces per +intersection in 3-d). In 3-d, if more than three halfspaces intersect +at the same point, triangulated output will produce +duplicate intersections, one for each additional halfspace. See the previous +cube and diamond example.</p> + +<p>If you use '<a href="qh-optq.htm#QJn">QJ</a>' (joggled input), all halfspace +intersections are simplicial. This may lead to nearly identical +intersections. For example, replace 'Qt' with 'QJ' above and +compare the duplicated intersections. +See <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>The 'qhalf' program is equivalent to +'<a href=qhull.htm#outputs>qhull H</a>' in 2-d to 4-d, and +'<a href=qhull.htm#outputs>qhull H</a> <a href=qh-optq.htm#Qx>Qx</a>' +in 5-d and higher. It disables the following Qhull +<a href=qh-quick.htm#options>options</a>: <i>d n v Qbb QbB Qf Qg Qm +Qr QR Qv Qx Qz TR E V Fa FA FC FD FS Ft FV Gt Q0,etc</i>. + + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> + +<h3><a href="#TOP">»</a><a name="synopsis">qhalf synopsis</a></h3> +<pre> +qhalf- halfspace intersection about a point. + input (stdin): [dim, 1, interior point] + dim+1, n + halfspace coefficients + offset + comments start with a non-numeric character + +options (qhalf.htm): + Hn,n - specify coordinates of interior point + Qt - triangulated output + QJ - joggle input instead of merging facets + Tv - verify result: structure, convexity, and redundancy + . - concise list of all options + - - one-line description of all options + +output options (subset): + s - summary of results (default) + Fp - intersection coordinates + Fv - non-redundant halfspaces incident to each intersection + Fx - non-redundant halfspaces + o - OFF file format (dual convex hull) + G - Geomview output (dual convex hull) + m - Mathematica output (dual convex hull) + QVn - print intersections for halfspace n, -n if not + TO file - output results to file, may be enclosed in single quotes + +examples: + rbox d | qconvex n | qhalf s H0,0,0 Fp + rbox c | qconvex FV n | qhalf s i + rbox c | qconvex FV n | qhalf s o +</pre> + +<h3><a href="#TOP">»</a><a name="input">qhalf input</a></h3> + +<blockquote> +<p>The input data on <tt>stdin</tt> consists of:</p> +<ul> + <li>[optional] interior point + <ul> + <li>dimension + <li>1 + <li>coordinates of interior point + </ul> + <li>dimension + 1 + <li>number of halfspaces</li> + <li>halfspace coefficients followed by offset</li> +</ul> + +<p>Use I/O redirection (e.g., qhalf < data.txt), a pipe (e.g., rbox c | qconvex FV n | qhalf), +or the '<a href=qh-optt.htm#TI>TI</a>' option (e.g., qhalf TI data.txt). + +<p>Qhull needs an interior point to compute the halfspace +intersection. An interior point is inside all of the halfspaces <i>Hx+b +<= 0</i>. The interior point may be in the input. If not, option +'Hn,n' defines the interior point as +[n,n,0,...] where <em>0</em> is the default coordinate (e.g., +'H0' is the origin). Use linear programming if you do not know +the interior point (see <a href="#notes">halfspace notes</a>),</p> + +<p>The input to qhalf is a set of halfspaces. Each halfspace is defined by +<em>d</em> coefficients followed by a signed offset. This defines +a linear inequality. The coefficients define a vector that is +normal to the halfspace. The vector may have any length. If it +has length one, the offset is the distance from the origin to the +halfspace's boundary. </p> + +<p>This is the same format used for output options '<a +href="qh-opto.htm#n">n</a>', '<a href="qh-optf.htm#Fo">Fo</a>', +and '<a href="qh-optf.htm#Fi">Fi</a>'. Use option '<a +href="qh-optf.htm#Fd">Fd</a>' to use cdd format for the +halfspaces.</p> + +<p>For example, here is the input for computing the intersection +of halfplanes that form a cube.</p> + +<blockquote> + <p>rbox c | qconvex FQ FV n TO data </p> + <pre> +RBOX c | QCONVEX FQ FV n +3 1 + 0 0 0 +4 +6 + 0 0 -1 -0.5 + 0 -1 0 -0.5 + 1 0 0 -0.5 + -1 0 0 -0.5 + 0 1 0 -0.5 + 0 0 1 -0.5 +</pre> + <p>qhalf s Fp < data </p> + <pre> + +Halfspace intersection by the convex hull of 6 points in 3-d: + + Number of halfspaces: 6 + Number of non-redundant halfspaces: 6 + Number of intersection points: 8 + +Statistics for: RBOX c | QCONVEX FQ FV n | QHALF s Fp + + Number of points processed: 6 + Number of hyperplanes created: 11 + Number of distance tests for qhull: 11 + Number of merged facets: 1 + Number of distance tests for merging: 45 + CPU seconds to compute hull (after input): 0 + +3 +3 +8 + -0.5 0.5 0.5 + 0.5 0.5 0.5 + -0.5 0.5 -0.5 + 0.5 0.5 -0.5 + 0.5 -0.5 0.5 + -0.5 -0.5 0.5 + -0.5 -0.5 -0.5 + 0.5 -0.5 -0.5 +</pre> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="outputs">qhalf outputs</a></h3> +<blockquote> + +<p>The following options control the output for halfspace +intersection.</p> +<blockquote> +<dl compact> + <dt> </dt> + <dd><b>Intersections</b></dd> + <dt><a href="qh-optf.htm#FN">FN</a></dt> + <dd>list intersection points for each non-redundant + halfspace. The first line + is the number of non-redundant halfspaces. Each remaining + lines starts with the number of intersection points. For the cube + example, each halfspace has four intersection points.</dd> + <dt><a href="qh-optf.htm#Fn">Fn</a></dt> + <dd>list neighboring intersections for each intersection point. The first line + is the number of intersection points. Each remaining line + starts with the number of neighboring intersections. For the cube + example, each intersection point has three neighboring intersections. + In 3-d, a non-simplicial intersection has more than three neighboring + intersections. Use option '<a href="qh-optq.htm#QJn">QJ</a>' to + avoid non-simplicial intersections. + </dd> + <dt><a href="qh-optf.htm#Fp">Fp</a></dt> + <dd>print intersection coordinates. The first line is the dimension and the + second line is the number of intersection points. The following lines are the + coordinates of each intersection.</dd> + <dt><a href="qh-optf.htm#FI">FI</a></dt> + <dd>list intersection IDs. The first line is the number of + intersections. The IDs follow, one per line.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Halfspaces</b></dd> + <dt><a href="qh-optf.htm#Fx">Fx</a></dt> + <dd>list non-redundant halfspaces. The first line is the number of + non-redundant halfspaces. The other lines list one halfspace per line. + A halfspace is <i>non-redundant</i> if it + defines a facet of the intersection. Redundant halfspaces are ignored. For + the cube example, all of the halfspaces are non-redundant. + </dd> + <dt><a href="qh-optf.htm#Fv">Fv</a></dt> + <dd>list non-redundant halfspaces incident to each intersection point. + The first line is the number of + non-redundant halfspaces. Each remaining line starts with the number + of non-redundant halfspaces. For the + cube example, each intersection is incident to three halfspaces.</dd> + <dt><a href="qh-opto.htm#i">i</a></dt> + <dd>list non-redundant halfspaces incident to each intersection point. The first + line is the number of intersection points. Each remaining line + lists the incident, non-redundant halfspaces. For the + cube example, each intersection is incident to three halfspaces. + </dd> + <dt><a href="qh-optf.htm#Fc">Fc</a></dt> + <dd>list coplanar halfspaces for each intersection point. The first line is + the number of intersection points. Each remaining line starts with + the number of coplanar halfspaces. A coplanar halfspace is listed for + one intersection point even though it is coplanar to multiple intersection + points.</dd> + <dt><a href="qh-optq.htm#Qc">Qi</a> <a href="qh-optf.htm#Fc">Fc</a></dt> + <dd>list redundant halfspaces for each intersection point. The first line is + the number of intersection points. Each remaining line starts with + the number of redundant halfspaces. Use options '<a href="qh-optq.htm#Qc">Qc</a> Qi Fc' to list + coplanar and redundant halfspaces.</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="qh-opto.htm#s">s</a></dt> + <dd>print summary for the halfspace intersection. Use '<a + href="qh-optf.htm#Fs">Fs</a>' if you need numeric data.</dd> + <dt><a href="qh-opto.htm#o">o</a></dt> + <dd>print vertices and facets of the dual convex hull. The + first line is the dimension. The second line is the number of + vertices, facets, and ridges. The vertex + coordinates are next, followed by the facets, one per line.</dd> + <dt><a href="qh-opto.htm#p">p</a></dt> + <dd>print vertex coordinates of the dual convex hull. Each vertex corresponds + to a non-redundant halfspace. Its coordinates are the negative of the hyperplane's coefficients + divided by the offset plus the inner product of the coefficients and + the interior point (-c/(b+a.p). + Options 'p <a href="qh-optq.htm#Qc">Qc</a>' includes coplanar halfspaces. + Options 'p <a href="qh-optq.htm#Qi">Qi</a>' includes redundant halfspaces.</dd> + <dt><a href="qh-opto.htm#m">m</a></dt> + <dd>Mathematica output for the dual convex hull in 2-d or 3-d.</dd> + <dt><a href="qh-optf.htm#FM">FM</a></dt> + <dd>Maple output for the dual convex hull in 2-d or 3-d.</dd> + <dt><a href="qh-optg.htm#G">G</a></dt> + <dd>Geomview output for the dual convex hull in 2-d, 3-d, or 4-d.</dd> + </dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="controls">qhalf controls</a></h3> +<blockquote> + +<p>These options provide additional control:</p> + +<blockquote> +<dl compact> + <dt><a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>triangulated output. If a 3-d intersection is defined by more than + three hyperplanes, Qhull produces duplicate intersections -- one for + each extra hyperplane.</dd> + <dt><a href="qh-optq.htm#QJn">QJ</a></dt> + <dd>joggle the input instead of merging facets. In 3-d, this guarantees that + each intersection is defined by three hyperplanes.</dd> + <dt><a href="qh-opto.htm#f">f </a></dt> + <dd>facet dump. Print the data structure for each intersection (i.e., + facet)</dd> + <dt><a href="qh-optt.htm#TFn">TFn</a></dt> + <dd>report summary after constructing <em>n</em> + intersections</dd> + <dt><a href="qh-optq.htm#QVn">QVn</a></dt> + <dd>select intersection points for halfspace <em>n</em> + (marked 'good')</dd> + <dt><a href="qh-optq.htm#QGn">QGn</a></dt> + <dd>select intersection points that are visible to halfspace <em>n</em> + (marked 'good'). Use <em>-n</em> for the remainder.</dd> + <dt><a href="qh-optq.htm#Qb0">Qbk:0Bk:0</a></dt> + <dd>remove the k-th coordinate from the input. This computes the + halfspace intersection in one lower dimension.</dd> + <dt><a href="qh-optt.htm#Tv">Tv</a></dt> + <dd>verify result</dd> + <dt><a href="qh-optt.htm#TO">TI file</a></dt> + <dd>input data from file. The filename may not use spaces or quotes.</dd> + <dt><a href="qh-optt.htm#TO">TO file</a></dt> + <dd>output results to file. Use single quotes if the filename + contains spaces (e.g., <tt>TO 'file with spaces.txt'</tt></dd> + <dt><a href="qh-optq.htm#Qs">Qs</a></dt> + <dd>search all points for the initial simplex. If Qhull can + not construct an initial simplex, it reports a +descriptive message. Usually, the point set is degenerate and one +or more dimensions should be removed ('<a href="qh-optq.htm#Qb0">Qbk:0Bk:0</a>'). +If not, use option 'Qs'. It performs an exhaustive search for the +best initial simplex. This is expensive is high dimensions.</dd> +</dl> +</blockquote> + + +</blockquote> +<h3><a href="#TOP">»</a><a name="graphics">qhalf graphics</a></h3> +<blockquote> + +<p>To view the results with Geomview, compute the convex hull of +the intersection points ('qhull FQ H0 Fp | qhull G'). See <a +href="qh-eg.htm#half">Halfspace examples</a>.</p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="notes">qhalf notes</a></h3> +<blockquote> + +<p>If you do not know an interior point for the halfspaces, use +linear programming to find one. Assume, <em>n</em> halfspaces +defined by: <em>aj*x1+bj*x2+cj*x3+dj>=0, j=1..n</em>. Perform +the following linear program: </p> + +<blockquote> + <p>max(x5) aj*x1+bj*x2+cj*x3+dj*x4-x5>=0, j=1..n</p> +</blockquote> + +<p>Then, if <em>[x1,x2,x3,x4,x5]</em> is an optimal solution with +<em>x4,x5>0</em> we get: </p> + +<blockquote> + <p>aj*(x1/x4)+bj*(x2/x4)+cj*(x3/x4)+dj>=(x5/x4)>0, + j=1..n </p> +</blockquote> + +<p>and conclude that the point <em>[x1/x4,x2/x4,x3/x4]</em> is in +the interior of all the halfspaces. Note that <em>x5</em> is +optimal, so this point is "way in" the interior (good +for precision errors).</p> + +<p>After finding an interior point, the rest of the intersection +algorithm is from Preparata & Shamos [<a +href="index.htm#pre-sha85">'85</a>, p. 316, "A simple case +..."]. Translate the halfspaces so that the interior point +is the origin. Calculate the dual polytope. The dual polytope is +the convex hull of the vertices dual to the original faces in +regard to the unit sphere (i.e., halfspaces at distance <em>d</em> +from the origin are dual to vertices at distance <em>1/d</em>). +Then calculate the resulting polytope, which is the dual of the +dual polytope, and translate the origin back to the interior +point [S. Spitz and S. Teller].</p> + + +</blockquote> +<h3><a href="#TOP">»</a><a name="conventions">qhalf +conventions</a></h3> +<blockquote> + +<p>The following terminology is used for halfspace intersection +in Qhull. This is the hardest structure to understand. The +underlying structure is a convex hull with one vertex per +non-redundant halfspace. See <a href="#conventions">convex hull +conventions</a> and <a href="index.htm#structure">Qhull's data structures</a>.</p> + +<ul> + <li><em>interior point</em> - a point in the intersection of + the halfspaces. Qhull needs an interior point to compute + the intersection. See <a href="#input">halfspace input</a>.</li> + <li><em>halfspace</em> - <em>d</em> coordinates for the + normal and a signed offset. The distance to an interior + point is negative.</li> + <li><em>non-redundant halfspace</em> - a halfspace that + defines an output facet</li> + <li><em>vertex</em> - a dual vertex in the convex hull + corresponding to a non-redundant halfspace</li> + <li><em>coplanar point</em> - the dual point corresponding to + a similar halfspace</li> + <li><em>interior point</em> - the dual point corresponding to + a redundant halfspace</li> + <li><em>intersection point</em>- the intersection of <em>d</em> + or more non-redundant halfspaces</li> + <li><em>facet</em> - a dual facet in the convex hull + corresponding to an intersection point</li> + <li><em>non-simplicial facet</em> - more than <em>d</em> + halfspaces intersect at a point</li> + <li><em>good facet</em> - an intersection point that + satisfies restriction '<a href="qh-optq.htm#QVn">QVn</a>', + etc.</li> +</ul> + +</blockquote> +<h3><a href="#TOP">»</a><a name="options">qhalf options</a></h3> + +<pre> +qhalf- compute the intersection of halfspaces about a point + http://www.qhull.org + +input (stdin): + optional interior point: dimension, 1, coordinates + first lines: dimension+1 and number of halfspaces + other lines: halfspace coefficients followed by offset + comments: start with a non-numeric character + +options: + Hn,n - specify coordinates of interior point + Qt - triangulated ouput + QJ - joggle input instead of merging facets + Qc - keep coplanar halfspaces + Qi - keep other redundant halfspaces + +Qhull control options: + QJn - randomly joggle input in range [-n,n] + Qbk:0Bk:0 - remove k-th coordinate from input + Qs - search all halfspaces for the initial simplex + QGn - print intersection if redundant to halfspace n, -n for not + QVn - print intersections for halfspace n, -n if not + +Trace options: + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events + Tc - check frequently during execution + Ts - print statistics + Tv - verify result: structure, convexity, and redundancy + Tz - send all output to stdout + TFn - report summary when n or more facets created + TI file - input data from file, no spaces or single quotes + TO file - output results to file, may be enclosed in single quotes + TPn - turn on tracing when halfspace n added to intersection + TMn - turn on tracing at merge n + TWn - trace merge facets when width > n + TVn - stop qhull after adding halfspace n, -n for before (see TCn) + TCn - stop qhull after building cone for halfspace n (see TVn) + +Precision options: + Cn - radius of centrum (roundoff added). Merge facets if non-convex + An - cosine of maximum angle. Merge facets if cosine > n or non-convex + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge + Rn - randomly perturb computations by a factor of [1-n,1+n] + Un - max distance below plane for a new, coplanar halfspace + Wn - min facet width for outside halfspace (before roundoff) + +Output formats (may be combined; if none, produces a summary to stdout): + f - facet dump + G - Geomview output (dual convex hull) + i - non-redundant halfspaces incident to each intersection + m - Mathematica output (dual convex hull) + o - OFF format (dual convex hull: dimension, points, and facets) + p - vertex coordinates of dual convex hull (coplanars if 'Qc' or 'Qi') + s - summary (stderr) + +More formats: + Fc - count plus redundant halfspaces for each intersection + - Qc (default) for coplanar and Qi for other redundant + Fd - use cdd format for input (homogeneous with offset first) + FF - facet dump without ridges + FI - ID of each intersection + Fm - merge count for each intersection (511 max) + FM - Maple output (dual convex hull) + Fn - count plus neighboring intersections for each intersection + FN - count plus intersections for each non-redundant halfspace + FO - options and precision constants + Fp - dim, count, and intersection coordinates + FP - nearest halfspace and distance for each redundant halfspace + FQ - command used for qhalf + Fs - summary: #int (8), dim, #halfspaces, #non-redundant, #intersections + for output: #non-redundant, #intersections, #coplanar + halfspaces, #non-simplicial intersections + #real (2), max outer plane, min vertex + Fv - count plus non-redundant halfspaces for each intersection + Fx - non-redundant halfspaces + +Geomview output (2-d, 3-d and 4-d; dual convex hull) + Ga - all points (i.e., transformed halfspaces) as dots + Gp - coplanar points and vertices as radii + Gv - vertices (i.e., non-redundant halfspaces) as spheres + Gi - inner planes (i.e., halfspace intersections) only + Gn - no planes + Go - outer planes only + Gc - centrums + Gh - hyperplane intersections + Gr - ridges + GDn - drop dimension n in 3-d and 4-d output + +Print options: + PAn - keep n largest facets (i.e., intersections) by area + Pdk:n- drop facet if normal[k] <= n (default 0.0) + PDk:n- drop facet if normal[k] >= n + Pg - print good facets (needs 'QGn' or 'QVn') + PFn - keep facets whose area is at least n + PG - print neighbors of good facets + PMn - keep n facets with most merges + Po - force output. If error, output neighborhood of facet + Pp - do not report precision problems + + . - list of all options + - - one line descriptions of all options +</pre> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.htm new file mode 100644 index 0000000000000000000000000000000000000000..dd8afb1b6aecadbc45e1ba33c589994f1d33c328 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.htm @@ -0,0 +1,469 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>qhull -- convex hull and related structures</title> +</head> + +<body> +<!-- Navigation links --> +<p><b><a name="TOP">Up</a></b><b>:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis • <a href="#input">in</a>put +• <a href="#outputs">ou</a>tputs • <a href="#controls">co</a>ntrols +• <a href="#options">op</a>tions +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/cone.html"><img +src="qh--cone.gif" alt="[cone]" align="middle" width="100" +height="100"></a>qhull -- convex hull and related structures</h1> + +<p>The convex hull of a set of points is the smallest convex set +containing the points. The Delaunay triangulation and furthest-site +Delaunay triangulation are equivalent to a convex hull in one +higher dimension. Halfspace intersection about a point is +equivalent to a convex hull by polar duality. + +<p>The <tt>qhull</tt> program provides options to build these +structures and to experiment with the process. Use the +<a href=qconvex.htm>qconvex</a>, +<a href=qdelaun.htm>qdelaunay</a>, <a href=qhalf.htm>qhalf</a>, +and <a href=qvoronoi.htm>qvoronoi</a> programs +to build specific structures. You may use <tt>qhull</tt> instead. +It takes the same options and uses the same code. +<blockquote> +<dl> + <dt><b>Example:</b> rbox 1000 D3 | qhull + <a href="qh-optc.htm#Cn">C-1e-4</a> + <a href="qh-optf.htm#FO">FO</a> + <a href="qh-optt.htm#Ts">Ts</a> + </dt> + <dd>Compute the 3-d convex hull of 1000 random + points. + Centrums must be 10^-4 below neighboring + hyperplanes. Print the options and precision constants. + When done, print statistics. These options may be + used with any of the Qhull programs.</dd> + <dt> </dt> + <dt><b>Example:</b> rbox 1000 D3 | qhull <a href=qhull.htm#outputs>d</a> + <a href="qh-optq.htm#Qbb">Qbb</a> + <a href="qh-optc.htm#Rn">R1e-4</a> + <a href="qh-optq.htm#Q0">Q0</a></dt> + <dd>Compute the 3-d Delaunay triangulation of 1000 random + points. Randomly perturb all calculations by + [0.9999,1.0001]. Do not correct precision problems. + This leads to serious precision errors.</dd> +</dl> +</blockquote> +<p>Use the following equivalences when calling <tt>qhull</tt> in 2-d to 4-d (a 3-d +Delaunay triangulation is a 4-d convex hull): +<blockquote> +<ul> +<li> +<a href="qconvex.htm">qconvex</a> == qhull +<li> +<a href=qdelaun.htm>qdelaunay</a> == qhull d <a href="qh-optq.htm#Qbb">Qbb</a> +<li> +<a href=qhalf.htm>qhalf</a> == qhull H +<li> +<a href=qvoronoi.htm>qvoronoi</a> == qhull v <a href="qh-optq.htm#Qbb">Qbb</a> +</ul> +</blockquote> + +<p>Use the following equivalences when calling <tt>qhull</tt> in 5-d and higher (a 4-d +Delaunay triangulation is a 5-d convex hull): +<blockquote> +<ul> +<li> +<a href="qconvex.htm">qconvex</a> == qhull <a href="qh-optq.htm#Qx">Qx</a> +<li> +<a href=qdelaun.htm>qdelaunay</a> == qhull d <a href="qh-optq.htm#Qbb">Qbb</a> <a href="qh-optq.htm#Qx">Qx</a> +<li> +<a href=qhalf.htm>qhalf</a> == qhull H <a href="qh-optq.htm#Qx">Qx</a> +<li> +<a href=qvoronoi.htm>qvoronoi</a> == qhull v <a href="qh-optq.htm#Qbb">Qbb</a> <a href="qh-optq.htm#Qx">Qx</a> +</ul> +</blockquote> + + +<p>By default, Qhull merges coplanar facets. For example, the convex +hull of a cube's vertices has six facets. + +<p>If you use '<a href="qh-optq.htm#Qt">Qt</a>' (triangulated output), +all facets will be simplicial (e.g., triangles in 2-d). For the cube +example, it will have 12 facets. Some facets may be +degenerate and have zero area. + +<p>If you use '<a href="qh-optq.htm#QJn">QJ</a>' (joggled input), +all facets will be simplicial. The corresponding vertices will be +slightly perturbed. Joggled input is less accurate that triangulated +output.See <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>The output for 4-d convex hulls may be confusing if the convex +hull contains non-simplicial facets (e.g., a hypercube). See +<a href=qh-faq.htm#extra>Why +are there extra points in a 4-d or higher convex hull?</a><br> +</p> + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h3><a href="#TOP">»</a><a name="synopsis">qhull synopsis</a></h3> +<pre> +qhull- compute convex hulls and related structures. + input (stdin): dimension, n, point coordinates + comments start with a non-numeric character + halfspace: use dim+1 and put offsets after coefficients + +options (qh-quick.htm): + d - Delaunay triangulation by lifting points to a paraboloid + d Qu - furthest-site Delaunay triangulation (upper convex hull) + v - Voronoi diagram as the dual of the Delaunay triangulation + v Qu - furthest-site Voronoi diagram + H1,1 - Halfspace intersection about [1,1,0,...] via polar duality + Qt - triangulated output + QJ - joggle input instead of merging facets + Tv - verify result: structure, convexity, and point inclusion + . - concise list of all options + - - one-line description of all options + +Output options (subset): + s - summary of results (default) + i - vertices incident to each facet + n - normals with offsets + p - vertex coordinates (if 'Qc', includes coplanar points) + if 'v', Voronoi vertices + Fp - halfspace intersections + Fx - extreme points (convex hull vertices) + FA - compute total area and volume + o - OFF format (if 'v', outputs Voronoi regions) + G - Geomview output (2-d, 3-d and 4-d) + m - Mathematica output (2-d and 3-d) + QVn - print facets that include point n, -n if not + TO file- output results to file, may be enclosed in single quotes + +examples: + rbox c d D2 | qhull Qc s f Fx | more rbox 1000 s | qhull Tv s FA + rbox 10 D2 | qhull d QJ s i TO result rbox 10 D2 | qhull v Qbb Qt p + rbox 10 D2 | qhull d Qu QJ m rbox 10 D2 | qhull v Qu QJ o + rbox c | qhull n rbox c | qhull FV n | qhull H Fp + rbox d D12 | qhull QR0 FA rbox c D7 | qhull FA TF1000 + rbox y 1000 W0 | qhull rbox 10 | qhull v QJ o Fv +</pre> + +<h3><a href="#TOP">»</a><a name="input">qhull input</a></h3> +<blockquote> + +<p>The input data on <tt>stdin</tt> consists of:</p> +<ul> + <li>dimension + <li>number of points</li> + <li>point coordinates</li> +</ul> + +<p>Use I/O redirection (e.g., qhull < data.txt), a pipe (e.g., rbox 10 | qhull), +or the '<a href=qh-optt.htm#TI>TI</a>' option (e.g., qhull TI data.txt). + +<p>Comments start with a non-numeric character. Error reporting is +simpler if there is one point per line. Dimension +and number of points may be reversed. For halfspace intersection, +an interior point may be prepended (see <a href=qhalf.htm#input>qhalf input</a>). + +<p>Here is the input for computing the convex +hull of the unit cube. The output is the normals, one +per facet.</p> + +<blockquote> + <p>rbox c > data </p> + <pre> +3 RBOX c +8 + -0.5 -0.5 -0.5 + -0.5 -0.5 0.5 + -0.5 0.5 -0.5 + -0.5 0.5 0.5 + 0.5 -0.5 -0.5 + 0.5 -0.5 0.5 + 0.5 0.5 -0.5 + 0.5 0.5 0.5 +</pre> + <p>qhull s n < data</p> + <pre> + +Convex hull of 8 points in 3-d: + + Number of vertices: 8 + Number of facets: 6 + Number of non-simplicial facets: 6 + +Statistics for: RBOX c | QHULL s n + + Number of points processed: 8 + Number of hyperplanes created: 11 + Number of distance tests for qhull: 35 + Number of merged facets: 6 + Number of distance tests for merging: 84 + CPU seconds to compute hull (after input): 0.081 + +4 +6 + 0 0 -1 -0.5 + 0 -1 0 -0.5 + 1 0 0 -0.5 + -1 0 0 -0.5 + 0 1 0 -0.5 + 0 0 1 -0.5 +</pre> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="outputs">qhull outputs</a></h3> +<blockquote> + +<p>These options control the output of qhull. They may be used +individually or together.</p> +<blockquote> +<dl compact> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a name="d">qhull d</a></dt> + <dd>compute the convex hull of the input points. + See <a href=qconvex.htm>qconvex</a>.</dd> + <dt><a name="d">qhull d Qbb</a></dt> + <dd>compute the Delaunay triangulation by lifting the points + to a paraboloid. Use option '<a href="qh-optq.htm#Qbb">Qbb</a>' + to scale the paraboloid and improve numeric precision. + See <a href=qdelaun.htm>qdelaunay</a>.</dd> + <dt><a name="v">qhull v Qbb</a></dt> + <dd>compute the Voronoi diagram by computing the Delaunay + triangulation. Use option '<a href="qh-optq.htm#Qbb">Qbb</a>' + to scale the paraboloid and improve numeric precision. + See <a href=qvoronoi.htm>qvoronoi</a>.</dd> + <dt><a name="H">qhull H</a></dt> + <dd>compute the halfspace intersection about a point via polar + duality. See <a href=qhalf.htm>qhalf</a>.</dd> +</dl> +</blockquote> + +<p>For a full list of output options see +<blockquote> +<ul> + <li><a href="qh-opto.htm#output">Output</a> formats</li> + <li><a href="qh-optf.htm#format">Additional</a> I/O + formats</li> + <li><a href="qh-optg.htm#geomview">Geomview</a> + output options</li> + <li><a href="qh-optp.htm#print">Print</a> options</li> +</ul> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="controls">qhull controls</a></h3> +<blockquote> + +<p>For a full list of control options see +<blockquote> +<ul> + <li><a href="qh-optq.htm#qhull">Qhull</a> control + options</li> + <li><a href="qh-optc.htm#prec">Precision</a> options</li> + <li><a href="qh-optt.htm#trace">Trace</a> options</li> +</ul> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="options">qhull options</a></h3> + +<pre> +qhull- compute convex hulls and related structures. + http://www.qhull.org + +input (stdin): + first lines: dimension and number of points (or vice-versa). + other lines: point coordinates, best if one point per line + comments: start with a non-numeric character + halfspaces: use dim plus one and put offset after coefficients. + May be preceded by a single interior point ('H'). + +options: + d - Delaunay triangulation by lifting points to a paraboloid + d Qu - furthest-site Delaunay triangulation (upper convex hull) + v - Voronoi diagram (dual of the Delaunay triangulation) + v Qu - furthest-site Voronoi diagram + Hn,n,... - halfspace intersection about point [n,n,0,...] + Qt - triangulated output + QJ - joggle input instead of merging facets + Qc - keep coplanar points with nearest facet + Qi - keep interior points with nearest facet + +Qhull control options: + Qbk:n - scale coord k so that low bound is n + QBk:n - scale coord k so that upper bound is n (QBk is 0.5) + QbB - scale input to unit cube centered at the origin + Qbb - scale last coordinate to [0,m] for Delaunay triangulations + Qbk:0Bk:0 - remove k-th coordinate from input + QJn - randomly joggle input in range [-n,n] + QRn - random rotation (n=seed, n=0 time, n=-1 time/no rotate) + Qf - partition point to furthest outside facet + Qg - only build good facets (needs 'QGn', 'QVn', or 'PdD') + Qm - only process points that would increase max_outside + Qr - process random outside points instead of furthest ones + Qs - search all points for the initial simplex + Qu - for 'd' or 'v', compute upper hull without point at-infinity + returns furthest-site Delaunay triangulation + Qv - test vertex neighbors for convexity + Qx - exact pre-merges (skips coplanar and anglomaniacs facets) + Qz - add point-at-infinity to Delaunay triangulation + QGn - good facet if visible from point n, -n for not visible + QVn - good facet if it includes point n, -n if not + Q0 - turn off default p remerge with 'C-0'/'Qx' + Q1 - sort merges by type instead of angle + Q2 - merge all non-convex at once instead of independent sets + Q3 - do not merge redundant vertices + Q4 - avoid old>new merges + Q5 - do not correct outer planes at end of qhull + Q6 - do not pre-merge concave or coplanar facets + Q7 - depth-first processing instead of breadth-first + Q8 - do not process near-inside points + Q9 - process furthest of furthest points + Q10 - no special processing for narrow distributions + Q11 - copy normals and recompute centrums for tricoplanar facets + +Towpaths Trace options: + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events + Tc - check frequently during execution + Ts - print statistics + Tv - verify result: structure, convexity, and point inclusion + Tz - send all output to stdout + TFn - report summary when n or more facets created + TI file - input data from file, no spaces or single quotes + TO file - output results to file, may be enclosed in single quotes + TPn - turn on tracing when point n added to hull + TMn - turn on tracing at merge n + TWn - trace merge facets when width > n + TRn - rerun qhull n times. Use with 'QJn' + TVn - stop qhull after adding point n, -n for before (see TCn) + TCn - stop qhull after building cone for point n (see TVn) + +Precision options: + Cn - radius of centrum (roundoff added). Merge facets if non-convex + An - cosine of maximum angle. Merge facets if cosine > n or non-convex + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge + En - max roundoff error for distance computation + Rn - randomly perturb computations by a factor of [1-n,1+n] + Vn - min distance above plane for a visible facet (default 3C-n or En) + Un - max distance below plane for a new, coplanar point (default Vn) + Wn - min facet width for outside point (before roundoff, default 2Vn) + +Output formats (may be combined; if none, produces a summary to stdout): + f - facet dump + G - Geomview output (see below) + i - vertices incident to each facet + m - Mathematica output (2-d and 3-d) + o - OFF format (dim, points and facets; Voronoi regions) + n - normals with offsets + p - vertex coordinates or Voronoi vertices (coplanar points if 'Qc') + s - summary (stderr) + +More formats: + Fa - area for each facet + FA - compute total area and volume for option 's' + Fc - count plus coplanar points for each facet + use 'Qc' (default) for coplanar and 'Qi' for interior + FC - centrum or Voronoi center for each facet + Fd - use cdd format for input (homogeneous with offset first) + FD - use cdd format for numeric output (offset first) + FF - facet dump without ridges + Fi - inner plane for each facet + for 'v', separating hyperplanes for bounded Voronoi regions + FI - ID of each facet + Fm - merge count for each facet (511 max) + FM - Maple output (2-d and 3-d) + Fn - count plus neighboring facets for each facet + FN - count plus neighboring facets for each point + Fo - outer plane (or max_outside) for each facet + for 'v', separating hyperplanes for unbounded Voronoi regions + FO - options and precision constants + Fp - dim, count, and intersection coordinates (halfspace only) + FP - nearest vertex and distance for each coplanar point + FQ - command used for qhull + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets, + output: #vertices, #facets, #coplanars, #nonsimplicial + #real (2), max outer plane, min vertex + FS - sizes: #int (0) + #real(2) tot area, tot volume + Ft - triangulation with centrums for non-simplicial facets (OFF format) + Fv - count plus vertices for each facet + for 'v', Voronoi diagram as Voronoi vertices for pairs of sites + FV - average of vertices (a feasible point for 'H') + Fx - extreme points (in order for 2-d) + +Geomview options (2-d, 3-d, and 4-d; 2-d Voronoi) + Ga - all points as dots + Gp - coplanar points and vertices as radii + Gv - vertices as spheres + Gi - inner planes only + Gn - no planes + Go - outer planes only + Gc - centrums + Gh - hyperplane intersections + Gr - ridges + GDn - drop dimension n in 3-d and 4-d output + Gt - for 3-d 'd', transparent outer ridges + +Print options: + PAn - keep n largest facets by area + Pdk:n - drop facet if normal[k] <= n (default 0.0) + PDk:n - drop facet if normal[k] >= n + Pg - print good facets (needs 'QGn' or 'QVn') + PFn - keep facets whose area is at least n + PG - print neighbors of good facets + PMn - keep n facets with most merges + Po - force output. If error, output neighborhood of facet + Pp - do not report precision problems + + . - list of all options + - - one line descriptions of all options +</pre> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis • <a href="#input">in</a>put +• <a href="#outputs">ou</a>tputs • <a href="#controls">co</a>ntrols +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.man b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.man new file mode 100644 index 0000000000000000000000000000000000000000..dc29c179544d2e008c13022bfa8d37c9ff99fb14 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.man @@ -0,0 +1,1006 @@ +./" This is the Unix manual page for qhull, written in nroff, the standard +./" manual formatter for Unix systems. To format it, type +./" +./" nroff -man qhull.man +./" +./" This will print a formatted copy to standard output. If you want +./" to ensure that the output is plain ASCII, free of any control +./" characters that nroff uses for underlining etc, pipe the output +./" through "col -b": +./" +./" nroff -man qhull.man | col -b +./" +./" Warning: a leading quote "'" or dot "." will not format correctly +./" +.TH qhull 1 "2003/12/30" "Geometry Center" +.SH NAME +qhull \- convex hull, Delaunay triangulation, Voronoi diagram, +halfspace intersection about a point, hull volume, facet area +.SH SYNOPSIS +.nf +qhull- compute convex hulls and related structures + input (stdin): dimension, #points, point coordinates + first comment (non-numeric) is listed in the summary + halfspace: use dim plus one with offsets after coefficients + +options (qh-quick.htm): + d - Delaunay triangulation by lifting points to a paraboloid + v - Voronoi diagram via the Delaunay triangulation + H1,1 - Halfspace intersection about [1,1,0,...] + d Qu - Furthest-site Delaunay triangulation (upper convex hull) + v Qu - Furthest-site Voronoi diagram + Qt - triangulated output + QJ - Joggle the input to avoid precision problems + . - concise list of all options + - - one-line description of all options + +Output options (subset): + FA - compute total area and volume + Fx - extreme points (convex hull vertices) + G - Geomview output (2-d, 3-d and 4-d) + Fp - halfspace intersection coordinates + m - Mathematica output (2-d and 3-d) + n - normals with offsets + o - OFF file format (if Voronoi, outputs regions) + TO file- output results to file, may be enclosed in single quotes + f - print all fields of all facets + s - summary of results (default) + Tv - verify result: structure, convexity, and point inclusion + p - vertex coordinates (centers for Voronoi) + i - vertices incident to each facet + +example: + rbox 1000 s | qhull Tv s FA +.fi + + - html manual: index.htm + - installation: README.txt + - see also: COPYING.txt, REGISTER.txt, Changes.txt + - WWW: <http://www.qhull.org> + - CVS: <http://savannah.gnu.org/projects/qhull/> + - mirror: <http://www6.uniovi.es/ftp/pub/mirrors/geom.umn.edu/software/ghindex.html> + - news: <http://www.qhull.org/news> + - Geomview: <http://www.geomview.org> + - news group: <news:comp.graphics.algorithms> + - FAQ: <http://exaflop.org/docs/cgafaq/cga6.html> + - email: qhull@qhull.org + - bug reports: qhull_bug@qhull.org + +The sections are: + - INTRODUCTION + - DESCRIPTION, a description of Qhull + - IMPRECISION, how Qhull handles imprecision + - OPTIONS + - Input and output options + - Additional input/output formats + - Precision options + - Geomview options + - Print options + - Qhull options + - Trace options + - BUGS + - E-MAIL + - SEE ALSO + - AUTHORS + - ACKNOWLEGEMENTS + +This man page briefly describes all Qhull options. Please report +any mismatches with Qhull's html manual (index.htm). + +.PP +.SH INTRODUCTION +Qhull is a general dimension code for computing convex hulls, Delaunay +triangulations, Voronoi diagram, furthest-site Voronoi diagram, +furthest-site Delaunay triangulations, and +halfspace intersections about a point. It implements the Quickhull algorithm for +computing the convex hull. Qhull handles round-off errors from floating +point arithmetic. It can approximate a convex hull. + +The program includes options for hull volume, facet area, partial hulls, +input transformations, randomization, tracing, multiple output formats, and +execution statistics. The program can be called from within your application. +You can view the results in 2-d, 3-d and 4-d with Geomview. +.PP +.SH DESCRIPTION +.PP +The format of input is the following: first line contains the dimension, +second line contains the number of input points, and point coordinates follow. +The dimension and number of points can be reversed. +Comments and line breaks are ignored. A comment starts with a +non-numeric character and continues to the end of line. The first comment +is reported in summaries and statistics. +Error reporting is +better if there is one point per line. +.PP +The default printout option is a short summary. There are many +other output formats. +.PP +Qhull implements the Quickhull algorithm for convex hull. This algorithm combines +the 2-d Quickhull algorithm with the n-d beneath-beyond algorithm +[c.f., Preparata & Shamos '85]. +It is similar to the randomized algorithms of Clarkson and +others [Clarkson et al. '93]. The main +advantages of Quickhull are output sensitive performance, reduced +space requirements, and automatic handling of precision problems. +.PP +The data structure produced by Qhull consists of vertices, ridges, and facets. +A vertex is a point of the input set. A ridge is a set of d vertices +and two neighboring facets. For example in 3-d, a ridge is an edge of the +polyhedron. A facet is a set of ridges, a set of neighboring facets, a set +of incident vertices, and a hyperplane equation. For simplicial facets, the +ridges are defined by the vertices and neighboring facets. When Qhull +merges two facets, it produces a non-simplicial +facet. A non-simplicial facet has more than d neighbors and may share more than +one ridge with a neighbor. +.PP +.SH IMPRECISION +.PP +Since Qhull uses floating point arithmetic, roundoff error may occur for each +calculation. This causes problems +for most geometric algorithms. +.PP +Qhull automatically sets option 'C-0' in 2-d, 3-d, and 4-d, or +option 'Qx' in 5-d and higher. These options handle precision problems +by merging facets. Alternatively, use option 'QJ' to joggle the +input. +.PP +With 'C-0', Qhull merges non-convex +facets while constructing the hull. The remaining facets are +clearly convex. With 'Qx', Qhull merges +coplanar horizon facets, flipped facets, concave facets and +duplicated ridges. It merges coplanar facets after constructing +the hull. +With 'Qx', coplanar points may be missed, but it +appears to be unlikely. +.PP +To guarantee triangular output, joggle the input with option 'QJ'. Facet +merging will not occur. +.SH OPTIONS +.PP +To get a list of the most important options, execute 'qhull' by itself. +To get a complete list of options, +execute 'qhull -'. +To get a complete, concise list of options, execute 'qhull .'. + +Options can be in any order. +Capitalized options take an argument (except 'PG' and 'F' options). +Single letters are used for output formats and precision constants. The +other options are grouped into menus for other output formats ('F'), +Geomview output ('G'), +printing ('P'), Qhull control ('Q'), and tracing ('T'). +.TP +Main options: +.TP +default +Compute the convex hull of the input points. Report a summary of +the result. +.TP +d +Compute the Delaunay triangulation by lifting the input points to a +paraboloid. The 'o' option prints the input points and facets. +The 'QJ' option guarantees triangular output. The 'Ft' +option prints a triangulation. It adds points (the centrums) to non-simplicial +facets. +.TP +v +Compute the Voronoi diagram from the Delaunay triangulation. +The 'p' option prints the Voronoi vertices. +The 'o' option prints the Voronoi vertices and the +vertices in each Voronoi region. It lists regions in +site ID order. +The 'Fv' option prints each ridge of the Voronoi diagram. +The first or zero'th vertex +indicates the infinity vertex. Its coordinates are +qh_INFINITE (-10.101). It indicates unbounded Voronoi +regions or degenerate Delaunay triangles. +.TP +Hn,n,... +Compute halfspace intersection about [n,n,0,...]. +The input is a set of halfspaces +defined in the same format as 'n', 'Fo', and 'Fi'. +Use 'Fp' to print the intersection points. Use 'Fv' +to list the intersection points for each halfspace. The +other output formats display the dual convex hull. + +The point [n,n,n,...] is a feasible point for the halfspaces, i.e., +a point that is inside all +of the halfspaces (Hx+b <= 0). The default coordinate value is 0. + +The input may start with a feasible point. If so, use 'H' by itself. +The input starts with a feasible point when the first number is the dimension, +the second number is "1", and the coordinates complete a line. The 'FV' +option produces a feasible point for a convex hull. +.TP +d Qu +Compute the furthest-site Delaunay triangulation from the upper +convex hull. The 'o' option prints the input points and facets. +The 'QJ' option guarantees triangular otuput. You can also use +'Ft' to triangulate via the centrums of non-simplicial +facets. +.TP +v Qu +Compute the furthest-site Voronoi diagram. +The 'p' option prints the Voronoi vertices. +The 'o' option prints the Voronoi vertices and the +vertices in each Voronoi region. +The 'Fv' option prints each ridge of the Voronoi diagram. +The first or zero'th vertex +indicates the infinity vertex at infinity. Its coordinates are +qh_INFINITE (-10.101). It indicates unbounded Voronoi regions +and degenerate Delaunay triangles. +.PP +.TP +Input/Output options: +.TP +f +Print out all facets and all fields of each facet. +.TP +G +Output the hull in Geomview format. For imprecise hulls, +Geomview displays the inner and outer hull. Geomview can also +display points, ridges, vertices, coplanar points, and +facet intersections. See below for a list of options. + +For Delaunay triangulations, 'G' displays the +corresponding paraboloid. For halfspace intersection, 'G' displays the +dual polytope. +.TP +i +Output the incident vertices for each facet. +Qhull prints the number of facets followed by the +vertices of each facet. One facet is printed per line. The numbers +are the 0-relative indices of the corresponding input points. +The facets +are oriented. + +In 4-d and higher, +Qhull triangulates non-simplicial facets. Each apex (the first vertex) is +a created point that corresponds to the facet's centrum. Its index is greater +than the indices of the input points. Each base +corresponds to a simplicial ridge between two facets. +To print the vertices without triangulation, use option 'Fv'. +.TP +m +Output the hull in Mathematica format. Qhull writes a Mathematica file for 2-d and 3-d +convex hulls and for 2-d Delaunay triangulations. Qhull produces a list of objects +that you can assign to a variable in Mathematica, for example: +"list= << <outputfilename> ". If the object is 2-d, it can be +visualized by "Show[Graphics[list]] ". For 3-d objects the command is +"Show[Graphics3D[list]]". +.TP +n +Output the normal equation for each facet. +Qhull prints the dimension (plus one), the number of facets, +and the normals for each facet. The facet's offset follows its +normal coefficients. +.TP +o +Output the facets in OFF file format. +Qhull prints the dimension, number of points, number +of facets, and number of ridges. Then it prints the coordinates of +the input points and the vertices for each facet. Each facet is on +a separate line. The first number is the number of vertices. The +remainder are the indices of the corresponding points. The vertices are +oriented in 2-d, 3-d, and in simplicial facets. + +For 2-d Voronoi diagrams, +the vertices are sorted by adjacency, but not oriented. In 3-d and higher, +the Voronoi vertices are sorted by index. +See the 'v' option for more information. +.TP +p +Output the coordinates of each vertex point. +Qhull prints the dimension, the number of points, +and the coordinates for each vertex. +With the 'Gc' and 'Gi' options, it also prints coplanar +and interior points. For Voronoi diagrams, it prints the coordinates +of each Voronoi vertex. +.TP +s +Print a summary to stderr. If no output options +are specified at all, a summary goes to stdout. The summary lists +the number of input points, the dimension, the number of vertices +in the convex hull, the number of facets in the convex hull, the +number of good facets (if 'Pg'), and statistics. + +The last two statistics (if needed) measure the maximum distance +from a point or vertex to a +facet. The number in parenthesis (e.g., 2.1x) is the ratio between the +maximum distance and the worst-case distance due to merging +two simplicial facets. +.PP +.TP +Precision options +.TP +An +Maximum angle given as a cosine. If the angle between a pair of facet +normals +is greater than n, Qhull merges one of the facets into a neighbor. +If 'n' is negative, Qhull tests angles after adding +each point to the hull (pre-merging). +If 'n' is positive, Qhull tests angles after +constructing the hull (post-merging). +Both pre- and post-merging can be defined. + +Option 'C0' or 'C-0' is set if the corresponding 'Cn' or 'C-n' +is not set. If 'Qx' +is set, then 'A-n' and 'C-n' are checked after the hull is constructed +and before 'An' and 'Cn' are checked. +.TP +Cn +Centrum radius. +If a centrum is less than n below a neighboring facet, Qhull merges one +of the facets. +If 'n' is negative or '-0', Qhull tests and merges facets after adding +each point to the hull. This is called "pre-merging". If 'n' is positive, +Qhull tests for convexity after constructing the hull ("post-merging"). +Both pre- and post-merging can be defined. + +For 5-d and higher, 'Qx' should be used +instead of 'C-n'. Otherwise, most or all facets may be merged +together. +.TP +En +Maximum roundoff error for distance computations. +.TP +Rn +Randomly perturb distance computations up to +/- n * max_coord. +This option perturbs every distance, hyperplane, and angle computation. +To use time as the random number seed, use option 'QR-1'. +.TP +Vn +Minimum distance for a facet to be visible. +A facet is visible if the distance from the point to the +facet is greater than 'Vn'. + +Without merging, the default value for 'Vn' is the round-off error ('En'). +With merging, the default value is the pre-merge centrum ('C-n') in 2-d or +3--d, or three times that in other dimensions. If the outside width +is specified ('Wn'), the maximum, default value for 'Vn' is 'Wn'. +.TP +Un +Maximum distance below a facet for a point to be coplanar to the facet. The +default value is 'Vn'. +.TP +Wn +Minimum outside width of the hull. Points are added to the convex hull +only if they are clearly outside of a facet. A point is outside of a +facet if its distance to the facet is greater than 'Wn'. The normal +value for 'Wn' is 'En'. If the user specifies pre-merging and +does not set 'Wn', than 'Wn' is set +to the premerge 'Cn' and maxcoord*(1-An). +.PP +.TP +Additional input/output formats +.TP +Fa +Print area for each facet. +For Delaunay triangulations, the area is the area of the triangle. +For Voronoi diagrams, the area is the area of the dual facet. +Use 'PAn' for printing the n largest facets, and option 'PFn' for +printing facets larger than 'n'. + +The area for non-simplicial facets is the sum of the +areas for each ridge to the centrum. Vertices far below +the facet's hyperplane are ignored. +The reported area may be significantly less than the actual area. +.TP +FA +Compute the total area and volume for option 's'. It is an approximation +for non-simplicial facets (see 'Fa'). +.TP +Fc +Print coplanar points for each facet. The output starts with the +number of facets. Then each facet is printed one per line. Each line +is the number of coplanar points followed by the point ids. +Option 'Qi' includes the interior points. Each coplanar point (interior point) is +assigned to the facet it is furthest above (resp., least below). +.TP +FC +Print centrums for each facet. The output starts with the +dimension followed by the number of facets. +Then each facet centrum is printed, one per line. +.TP +Fd +Read input in cdd format with homogeneous points. +The input starts with comments. The first comment is reported in +the summary. +Data starts after a "begin" line. The next line is the number of points +followed by the dimension+1 and "real" or "integer". Then the points +are listed with a leading "1" or "1.0". The data ends with an "end" line. + +For halfspaces ('Fd Hn,n,...'), the input format is the same. Each halfspace +starts with its offset. The sign of the offset is the opposite of Qhull's +convention. +.TP +FD +Print normals ('n', 'Fo', 'Fi') or points ('p') in cdd format. +The first line is the command line that invoked Qhull. +Data starts with a "begin" line. The next line is the number of normals or points +followed by the dimension+1 and "real". Then the normals or points +are listed with the offset before the coefficients. The offset for points is +1.0. The offset for normals has the opposite sign. +The data ends with an "end" line. +.TP +FF +Print facets (as in 'f') without printing the ridges. +.TP +Fi +Print inner planes for each facet. The inner plane is below all vertices. +.TP +Fi +Print separating hyperplanes for bounded, inner regions of the Voronoi +diagram. The first line is the number +of ridges. Then each hyperplane is printed, one per line. A line starts +with the number of indices and floats. The first pair lists +adjacent input +sites, the next d floats are the normalized coefficients for the hyperplane, +and the last float is the offset. The hyperplane is oriented toward +'QVn' (if defined), or the first input site of the pair. Use 'Tv' to +verify that the hyperplanes are perpendicular bisectors. Use 'Fo' for +unbounded regions, and 'Fv' for the corresponding Voronoi vertices. +.TP +FI +Print facet identifiers. +.TP +Fm +Print number of merges for each facet. At most 511 merges are reported for +a facet. See 'PMn' for printing the facets with the most merges. +.TP +FM +Output the hull in Maple format. Qhull writes a Maple +file for 2-d and 3-d +convex hulls and for 2-d Delaunay triangulations. Qhull produces a '.mpl' +file for displaying with display3d(). +.TP +Fn +Print neighbors for each facet. The output starts with the number of facets. +Then each facet is printed one per line. Each line +is the number of neighbors followed by an index for each neighbor. The indices +match the other facet output formats. + +A negative index indicates an unprinted +facet due to printing only good facets ('Pg'). It is the negation of the facet's +ID (option 'FI'). +For example, negative indices are used for facets +"at infinity" in the Delaunay triangulation. +.TP +FN +Print vertex neighbors or coplanar facet for each point. +The first line is the number +of points. Then each point is printed, one per line. If the +point is coplanar, the line is "1" followed by the facet's ID. +If the point is +not a selected vertex, the line is "0". +Otherwise, each line is the number of +neighbors followed by the corresponding facet indices (see 'Fn'). +.TP +Fo +Print outer planes for each facet in the same format as 'n'. +The outer plane is above all points. +.TP +Fo +Print separating hyperplanes for unbounded, outer regions of the Voronoi +diagram. The first line is the number +of ridges. Then each hyperplane is printed, one per line. A line starts +with the number of indices and floats. The first pair lists +adjacent input +sites, the next d floats are the normalized coefficients for the hyperplane, +and the last float is the offset. The hyperplane is oriented toward +'QVn' (if defined), or the first input site of the pair. Use 'Tv' to +verify that the hyperplanes are perpendicular bisectors. Use 'Fi' for +bounded regions, and 'Fv' for the corresponding Voronoi vertices. +.TP +FO +List all options to stderr, including the default values. Additional 'FO's +are printed to stdout. +.TP +Fp +Print points for halfspace intersections (option 'Hn,n,...'). Each +intersection corresponds to a facet of the dual polytope. +The "infinity" point [-10.101,-10.101,...] +indicates an unbounded intersection. +.TP +FP +For each coplanar point ('Qc') print the point ID of the nearest vertex, +the point ID, the facet ID, and the distance. +.TP +FQ +Print command used for qhull and input. +.TP +Fs +Print a summary. The first line consists of the number of integers ("8"), +followed by the dimension, the number of points, the number of vertices, +the number of facets, the number of vertices selected for output, the +number of facets selected for output, the number of coplanar points selected +for output, number of simplicial, unmerged facets in output + +The second line consists of the number of reals ("2"), +followed by the maxmimum offset to an outer plane and and minimum offset to +an inner plane. Roundoff is included. Later +versions of Qhull may produce additional integers or reals. +.TP +FS +Print the size of the hull. The first line consists of the number of integers ("0"). +The second line consists of the number of reals ("2"), +followed by the total facet area, and the total volume. +Later +versions of Qhull may produce additional integers or reals. + +The total volume measures the volume +of the intersection of the halfspaces defined by each facet. +Both area and volume are +approximations for non-simplicial facets. See option 'Fa'. +.TP +Ft +Print a triangulation with added points for non-simplicial +facets. The first line is the dimension and the second line is the +number of points and the number of facets. The points follow, one +per line, then the facets follow as a list of point indices. With option +'Qz', the +points include the point-at-infinity. +.TP +Fv +Print vertices for each facet. The first line is the number +of facets. Then each facet is printed, one per line. Each line is +the number of vertices followed by the corresponding point ids. Vertices +are listed in the order they were added to the hull (the last one is first). +.TP +Fv +Print all ridges of a Voronoi diagram. The first line is the number +of ridges. Then each ridge is printed, one per line. A line starts +with the number of indices. The first pair lists adjacent input +sites, the remaining indices list Voronoi vertices. Vertex '0' indicates +the vertex-at-infinity (i.e., an unbounded ray). In 3-d, the vertices +are listed in order. See 'Fi' and 'Fo' for separating hyperplanes. +.TP +FV +Print average vertex. The average vertex is a feasible point +for halfspace intersection. +.TP +Fx +List extreme points (vertices) of the convex hull. The first line +is the number of points. The other lines give the indices of the +corresponding points. The first point is '0'. In 2-d, the points +occur in counter-clockwise order; otherwise they occur in input order. +For Delaunay triangulations, 'Fx' lists the extreme points of the +input sites. The points are unordered. +.PP +.TP +Geomview options +.TP +G +Produce a file for viewing with Geomview. Without other options, +Qhull displays edges in 2-d, outer planes in 3-d, and ridges in 4-d. +A ridge can be +explicit or implicit. An explicit ridge is a dim-1 dimensional simplex +between two facets. +In 4-d, the explicit ridges are triangles. +When displaying a ridge in 4-d, Qhull projects the ridge's vertices to +one of its facets' hyperplanes. +Use 'Gh' to +project ridges to the intersection of both hyperplanes. +.TP +Ga +Display all input points as dots. +.TP +Gc +Display the centrum for each facet in 3-d. The centrum is defined by a +green radius sitting on a blue plane. The plane corresponds to the +facet's hyperplane. +The radius is defined by 'C-n' or 'Cn'. +.TP +GDn +Drop dimension n in 3-d or 4-d. The result is a 2-d or 3-d object. +.TP +Gh +Display hyperplane intersections in 3-d and 4-d. In 3-d, the +intersection is a black line. It lies on two neighboring hyperplanes +(c.f., the blue squares associated with centrums ('Gc')). In 4-d, +the ridges are projected to the intersection of both hyperplanes. +.TP +Gi +Display inner planes in 2-d and 3-d. The inner plane of a facet +is below all of its vertices. It is parallel to the facet's hyperplane. +The inner plane's color is the opposite (1-r,1-g,1-b) of the outer +plane. Its edges are determined by the vertices. +.TP +Gn +Do not display inner or outer planes. By default, +Geomview displays the precise plane (no merging) or both +inner and output planes (merging). Under merging, Geomview does +not display the inner plane if the +the difference between inner and outer is too small. +.TP +Go +Display outer planes in 2-d and 3-d. The outer plane of a facet +is above all input points. It is parallel to the facet's hyperplane. +Its color is determined by the facet's normal, and its +edges are determined by the vertices. +.TP +Gp +Display coplanar points and vertices as radii. A radius defines a ball +which corresponds to the imprecision of the point. The imprecision is +the maximum of the roundoff error, the centrum radius, and maxcoord * +(1-An). It is at least 1/20'th of the maximum coordinate, +and ignores post-merging if pre-merging is done. +.TP +Gr +Display ridges in 3-d. A ridge connects the two vertices that are shared +by neighboring facets. Ridges are always displayed in 4-d. +.TP +Gt +A 3-d Delaunay triangulation looks like a convex hull with interior +facets. Option 'Gt' removes the outside ridges to reveal the outermost +facets. It automatically sets options 'Gr' and 'GDn'. +.TP +Gv +Display vertices as spheres. The radius of the sphere corresponds to +the imprecision of the data. See 'Gp' for determining the radius. +.PP +.TP +Print options +.TP +PAn +Only the n largest facets are marked good for printing. +Unless 'PG' is set, 'Pg' is automatically set. +.TP +Pdk:n +Drop facet from output if normal[k] <= n. The option 'Pdk' uses the +default value of 0 for n. +.TP +PDk:n +Drop facet from output if normal[k] >= n. The option 'PDk' uses the +default value of 0 for n. +.TP +PFn +Only facets with area at least 'n' are marked good for printing. +Unless 'PG' is set, 'Pg' is automatically set. +.TP +Pg +Print only good facets. A good facet is either visible from a point +(the 'QGn' option) or includes a point (the 'QVn' option). It also meets the +requirements of 'Pdk' and 'PDk' options. Option 'Pg' is automatically +set for options 'PAn' and 'PFn'. +.TP +PG +Print neighbors of good facets. +.TP +PMn +Only the n facets with the most merges are marked good for printing. +Unless 'PG' is set, 'Pg' is automatically set. +.TP +Po +Force output despite precision problems. Verify ('Tv') does not check +coplanar points. +Flipped facets are reported and concave facets are counted. +If 'Po' is used, points are not +partitioned into flipped facets and a flipped facet is always visible +to a point. +Also, if an error occurs before the completion of Qhull and tracing is +not active, 'Po' outputs a neighborhood of the erroneous facets +(if any). +.TP +Pp +Do not report precision problems. +.PP +.TP +Qhull control options +.TP +Qbk:0Bk:0 +Drop dimension k from the input points. This allows the user to +take convex hulls of sub-dimensional objects. It happens before +the Delaunay and Voronoi transformation. +.TP +QbB +Scale the input points to fit the unit cube. After scaling, the lower +bound will be -0.5 and the upper bound +0.5 in all dimensions. +For Delaunay and +Voronoi diagrams, scaling happens after projection to the paraboloid. +Under precise +arithmetic, scaling does not change the topology of the convex hull. +.TP +Qbb +Scale the last coordinate to [0, m] where m is the maximum absolute +value of the other coordinates. For Delaunay and +Voronoi diagrams, scaling happens after projection to the paraboloid. +It reduces roundoff error for inputs with integer coordinates. +Under precise +arithmetic, scaling does not change the topology of the convex hull. +.TP +Qbk:n +Scale the k'th coordinate of the input points. After scaling, the lower +bound of the input points will be n. 'Qbk' scales to -0.5. +.TP +QBk:n +Scale the k'th coordinate of the input points. After scaling, the upper +bound will be n. 'QBk' scales to +0.5. +.TP +Qc +Keep coplanar points with the nearest facet. Output +formats 'p', 'f', 'Gp', 'Fc', 'FN', and 'FP' will print the points. +.TP +Qf +Partition points to the furthest outside facet. +.TP +Qg +Only build good facets. With the 'Qg' option, Qhull will only build +those facets that it needs to determine the good facets in the output. +See 'QGn', 'QVn', and 'PdD' for defining good facets, and 'Pg' and 'PG' +for printing good facets and their neighbors. +.TP +QGn +A facet is good (see 'Qg' and 'Pg') if it is visible from point n. If n < 0, a facet is +good if it is not visible from point n. Point n is not added to the +hull (unless 'TCn' or 'TPn'). +With rbox, use the 'Pn,m,r' option to define your point; it +will be point 0 (QG0). +.TP +Qi +Keep interior points with the nearest facet. +Output formats 'p', 'f', 'Gp', 'FN', 'FP', and 'Fc' will print the points. +.TP +QJn +Joggle each input coordinate by adding a random number in [-n,n]. If a +precision error occurs, then qhull increases n and tries again. It does +not increase n beyond a certain value, and it stops after a certain number +of attempts [see user.h]. Option 'QJ' +selects a default value for n. The output will be simplicial. For +Delaunay triangulations, 'QJn' sets 'Qbb' to scale the last coordinate +(not if 'Qbk:n' or 'QBk:n' is set). See also 'Qt'. +.TP +Qm +Only process points that would otherwise increase max_outside. Other +points are treated as coplanar or interior points. +.TP +Qr +Process random outside points instead of furthest ones. This makes +Qhull equivalent to the randomized incremental algorithms. CPU time +is not reported since the randomization is inefficient. +.TP +QRn +Randomly rotate the input points. If n=0, use time as the random number seed. +If n>0, use n as the random number seed. If n=-1, don't rotate but use +time as the random number seed. For Delaunay triangulations ('d' and 'v'), +rotate about the last axis. +.TP +Qs +Search all points for the initial simplex. +.TP +Qt +Triangulated output. Triangulate all non-simplicial facets. See also 'QJ'. +.TP +Qv +Test vertex neighbors for convexity after post-merging. +To use the 'Qv' option, you also need to set a merge option +(e.g., 'Qx' or 'C-0'). +.TP +QVn +A good facet (see 'Qg' and 'Pg') includes point n. If n<0, then a good facet does not +include point n. The point is either in the initial simplex or it +is the first point added to the hull. Option 'QVn' may not be used with merging. +.TP +Qx +Perform exact merges while building the hull. The "exact" merges +are merging a point into a coplanar facet (defined by 'Vn', 'Un', +and 'C-n'), merging concave facets, merging duplicate ridges, and +merging flipped facets. Coplanar merges and angle coplanar merges ('A-n') +are not performed. Concavity testing is delayed until a merge occurs. + +After +the hull is built, all coplanar merges are performed (defined by 'C-n' +and 'A-n'), then post-merges are performed +(defined by 'Cn' and 'An'). +.TP +Qz +Add a point "at infinity" that is above the paraboloid for Delaunay triangulations +and Voronoi diagrams. This reduces precision problems and allows the triangulation +of cospherical points. +.PP +.TP +Qhull experiments and speedups +.TP +Q0 +Turn off pre-merging as a default option. +With 'Q0'/'Qx' and without explicit pre-merge options, Qhull +ignores precision issues while constructing the convex hull. This +may lead to precision errors. If so, a descriptive warning is +generated. +.TP +Q1 +With 'Q1', Qhull sorts merges by type (coplanar, angle coplanar, concave) +instead of by angle. +.TP +Q2 +With 'Q2', Qhull merges all facets at once instead of using +independent sets of merges and then retesting. +.TP +Q3 +With 'Q3', Qhull does not remove redundant vertices. +.TP +Q4 +With 'Q4', Qhull avoids merges of an old facet into a new facet. +.TP +Q5 +With 'Q5', Qhull does not correct outer planes at the end. The +maximum outer plane is used instead. +.TP +Q6 +With 'Q6', Qhull does not pre-merge concave or coplanar facets. +.TP +Q7 +With 'Q7', Qhull processes facets in depth-first order instead of +breadth-first order. +.TP +Q8 +With 'Q8' and merging, Qhull does not retain near-interior points for adjusting +outer planes. 'Qc' will probably retain +all points that adjust outer planes. +.TP +Q9 +With 'Q9', Qhull processes the furthest of all outside sets at each iteration. +.TP +Q10 +With 'Q10', Qhull does not use special processing for narrow distributions. +.TP +Q11 +With 'Q11', Qhull copies normals and recompute centrums for tricoplanar facets. +.PP +.TP +Trace options +.TP +Tn +Trace at level n. Qhull includes full execution tracing. 'T-1' +traces events. 'T1' traces +the overall execution of the program. 'T2' and 'T3' trace overall +execution and geometric and topological events. 'T4' traces the +algorithm. 'T5' includes information about memory allocation and +Gaussian elimination. +.TP +Tc +Check frequently during execution. This will catch most inconsistency +errors. +.TP +TCn +Stop Qhull after building the cone of new facets for point n. The +output for 'f' includes the cone and the old hull. +See also 'TVn'. +.TP +TFn +Report progress whenever more than n facets are created +During post-merging, 'TFn' +reports progress after more than n/2 merges. +.TP +TI file +Input data from 'file'. The filename may not include spaces or +quotes. +.TP +TO file +Output results to 'file'. The name may be enclosed in single +quotes. +.TP +TPn +Turn on tracing when point n is added to the hull. Trace +partitions of point n. If used with TWn, turn off +tracing after adding point n to the hull. +.TP +TRn +Rerun qhull n times. Usually used with 'QJn' to determine the +probability that a given joggle will fail. +.TP +Ts +Collect statistics and print to stderr at the end of execution. +.TP +Tv +Verify the convex hull. This checks the topological structure, facet +convexity, and point inclusion. +If precision problems occurred, facet convexity is tested whether or +not 'Tv' is selected. +Option 'Tv' does not check point inclusion if forcing output with 'Po', +or if 'Q5' is set. + +For point inclusion testing, Qhull verifies that all points are below +all outer planes (facet->maxoutside). Point inclusion is exhaustive +if merging or if the facet-point product is small enough; +otherwise Qhull verifies each point with a directed +search (qh_findbest). + +Point inclusion testing occurs after producing output. It prints +a message to stderr unless option 'Pp' is used. This +allows the user to interrupt Qhull without changing the output. +.TP +TVn +Stop Qhull after adding point n. If n < 0, stop Qhull before adding +point n. Output shows the hull at this time. See also 'TCn' +.TP +TMn +Turn on tracing at n'th merge. +.TP +TWn +Trace merge facets when the width is greater than n. +.TP +Tz +Redirect stderr to stdout. +.PP +.SH BUGS +Please report bugs to Brad Barber at qhull_bug@qhull.org. + +If Qhull does not compile, it is due to an incompatibility between your +system and ours. The first thing to check is that your compiler is +ANSI standard. If it is, check the man page for the best options, or +find someone to help you. If you locate the cause of your problem, +please send email since it might help others. + +If Qhull compiles but crashes on the test case (rbox D4), there's +still incompatibility between your system and ours. Typically it's +been due to mem.c and memory alignment. You can use qh_NOmem in mem.h +to turn off memory management. Please let us know if you figure out +how to fix these problems. + +If you do find a problem, try to simplify it before reporting the +error. Try different size inputs to locate the smallest one that +causes an error. You're welcome to hunt through the code using the +execution trace as a guide. This is especially true if you're +incorporating Qhull into your own program. + +When you do report an error, please attach a data set to the +end of your message. This allows us to see the error for ourselves. +Qhull is maintained part-time. +.PP +.SH E-MAIL +Please send correspondence to qhull@qhull.org and report bugs to +qhull_bug@qhull.org. Let us know how you use Qhull. If you +mention it in a paper, please send the reference and an abstract. + +If you would like to get Qhull announcements (e.g., a new version) +and news (any bugs that get fixed, etc.), let us know and we will add you to +our mailing list. If you would like to communicate with other +Qhull users, we will add you to the qhull_users alias. +For Internet news about geometric algorithms and convex hulls, look at +comp.graphics.algorithms and sci.math.num-analysis + +.SH SEE ALSO +rbox(1) + +Barber, C. B., D.P. Dobkin, and H.T. Huhdanpaa, +"The Quickhull Algorithm for Convex Hulls," ACM +Trans. on Mathematical Software, Dec. 1996. +http://www.acm.org/pubs/citations/journals/toms/1996-22-4/p469-barber/ +http://citeseer.nj.nec.com/83502.html + +Clarkson, K.L., K. Mehlhorn, and R. Seidel, "Four results on randomized +incremental construction," Computational Geometry: Theory and Applications, +vol. 3, p. 185-211, 1993. + +Preparata, F. and M. Shamos, Computational +Geometry, Springer-Verlag, New York, 1985. + +.PP +.SH AUTHORS +.nf + C. Bradford Barber Hannu Huhdanpaa + bradb@qhull.org hannu@qhull.org + + c/o The Geometry Center + University of Minnesota + 400 Lind Hall + 207 Church Street S.E. + Minneapolis, MN 55455 + +.fi + +.SH ACKNOWLEDGEMENTS + +A special thanks to Albert Marden, Victor Milenkovic, the Geometry Center, +Harvard University, and Endocardial Solutions, Inc. for supporting this work. + +The software was developed under National Science Foundation grants +NSF/DMS-8920161 and NSF-CCR-91-15793 750-7504. David Dobkin guided +the original work at Princeton University. +If you find it useful, please let us know. + +The Geometry Center is supported by grant DMS-8920161 from the National +Science Foundation, by grant DOE/DE-FG02-92ER25137 from the Department +of Energy, by the University of Minnesota, and by Minnesota Technology, Inc. + +Qhull is available from http://www.qhull.org diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.txt new file mode 100644 index 0000000000000000000000000000000000000000..f3b2f88f7db800a691935555888372bc4bb8e059 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qhull.txt @@ -0,0 +1,1263 @@ + + + +qhull(1) qhull(1) + + +NAME + qhull - convex hull, Delaunay triangulation, Voronoi dia- + gram, halfspace intersection about a point, hull volume, facet area + +SYNOPSIS + qhull- compute convex hulls and related structures + input (stdin): dimension, #points, point coordinates + first comment (non-numeric) is listed in the summary + halfspace: use dim plus one with offsets after coefficients + + options (qh-quick.htm): + d - Delaunay triangulation by lifting points to a paraboloid + v - Voronoi diagram via the Delaunay triangulation + H1,1 - Halfspace intersection about [1,1,0,...] + d Qu - Furthest-site Delaunay triangulation (upper convex hull) + v Qu - Furthest-site Voronoi diagram + QJ - Joggle the input to avoid precision problems + . - concise list of all options + - - one-line description of all options + + Output options (subset): + FA - compute total area and volume + Fx - extreme points (convex hull vertices) + G - Geomview output (2-d, 3-d and 4-d) + Fp - halfspace intersection coordinates + m - Mathematica output (2-d and 3-d) + n - normals with offsets + o - OFF file format (if Voronoi, outputs regions) + TO file- output results to file, may be enclosed in single quotes + f - print all fields of all facets + s - summary of results (default) + Tv - verify result: structure, convexity, and point inclusion + p - vertex coordinates + i - vertices incident to each facet + + example: + rbox 1000 s | qhull Tv s FA + + - html manual: index.htm + - installation: README.txt + - see also: COPYING.txt, REGISTER.txt, Changes.txt + - WWW: <http://www.qhull.org> + - CVS: <http://savannah.gnu.org/projects/qhull/> + - mirror: <http://www6.uniovi.es/ftp/pub/mirrors/geom.umn.edu/software/ghindex.html> + - news: <http://www.qhull.org/news> + - Geomview: <http://www.geomview.org> + - news group: <news:comp.graphics.algorithms> + - FAQ: <http://exaflop.org/docs/cgafaq/cga6.html> + - email: qhull@qhull.org + - bug reports: qhull_bug@qhull.org + + + + +Geometry Center 2003/12/30 1 + + + + + +qhull(1) qhull(1) + + + The sections are: + - INTRODUCTION + - DESCRIPTION, a description of Qhull + - IMPRECISION, how Qhull handles imprecision + - OPTIONS + - Input and output options + - Additional input/output formats + - Precision options + - Geomview options + - Print options + - Qhull options + - Trace options + - BUGS + - E-MAIL + - SEE ALSO + - AUTHORS + - ACKNOWLEGEMENTS + + This man page briefly describes all Qhull options. Please + report any mismatches with Qhull's html manual (qh- + man.htm). + + + +INTRODUCTION + Qhull is a general dimension code for computing convex + hulls, Delaunay triangulations, Voronoi diagram, furthest- + site Voronoi diagram, furthest-site Delaunay triangula- + tions, and halfspace intersections about a point. It + implements the Quickhull algorithm for computing the con- + vex hull. Qhull handles round-off errors from floating + point arithmetic. It can approximate a convex hull. + + The program includes options for hull volume, facet area, + partial hulls, input transformations, randomization, trac- + ing, multiple output formats, and execution statistics. + The program can be called from within your application. + You can view the results in 2-d, 3-d and 4-d with + Geomview. + + +DESCRIPTION + The format of input is the following: first line contains + the dimension, second line contains the number of input + points, and point coordinates follow. The dimension and + number of points can be reversed. Comments and line + breaks are ignored. A comment starts with a non-numeric + character and continues to the end of line. The first + comment is reported in summaries and statistics. Error + reporting is better if there is one point per line. + + The default printout option is a short summary. There are + many other output formats. + + + + +Geometry Center 2003/12/30 2 + + + + + +qhull(1) qhull(1) + + + Qhull implements the Quickhull algorithm for convex hull. + This algorithm combines the 2-d Quickhull algorithm with + the n-d beneath-beyond algorithm [c.f., Preparata & Shamos + '85]. It is similar to the randomized algorithms of + Clarkson and others [Clarkson et al. '93]. The main + advantages of Quickhull are output sensitive performance, + reduced space requirements, and automatic handling of pre- + cision problems. + + The data structure produced by Qhull consists of vertices, + ridges, and facets. A vertex is a point of the input set. + A ridge is a set of d vertices and two neighboring facets. + For example in 3-d, a ridge is an edge of the polyhedron. + A facet is a set of ridges, a set of neighboring facets, a + set of incident vertices, and a hyperplane equation. For + simplicial facets, the ridges are defined by the vertices + and neighboring facets. When Qhull merges two facets, it + produces a non-simplicial facet. A non-simplicial facet + has more than d neighbors and may share more than one + ridge with a neighbor. + + +IMPRECISION + Since Qhull uses floating point arithmetic, roundoff error + may occur for each calculation. This causes problems for + most geometric algorithms. + + Qhull automatically sets option 'C-0' in 2-d, 3-d, and + 4-d, or option 'Qx' in 5-d and higher. These options han- + dle precision problems by merging facets. Alternatively, + use option 'QJ' to joggle the input. + + With 'C-0', Qhull merges non-convex facets while con- + structing the hull. The remaining facets are clearly con- + vex. With 'Qx', Qhull merges coplanar horizon facets, + flipped facets, concave facets and duplicated ridges. It + merges coplanar facets after constructing the hull. With + 'Qx', coplanar points may be missed, but it appears to be + unlikely. + + To guarantee triangular output, joggle the input with + option 'QJ'. Facet merging will not occur. + +OPTIONS + To get a list of the most important options, execute + 'qhull' by itself. To get a complete list of options, + execute 'qhull -'. To get a complete, concise list of + options, execute 'qhull .'. + + Options can be in any order. Capitalized options take an + argument (except 'PG' and 'F' options). Single letters + are used for output formats and precision constants. The + other options are grouped into menus for other output for- + mats ('F'), Geomview output ('G'), printing ('P'), Qhull + + + +Geometry Center 2003/12/30 3 + + + + + +qhull(1) qhull(1) + + + control ('Q'), and tracing ('T'). + + Main options: + + default + Compute the convex hull of the input points. + Report a summary of the result. + + d Compute the Delaunay triangulation by lifting the + input points to a paraboloid. The 'o' option + prints the input points and facets. The 'QJ' + option guarantees triangular output. The 'Ft' + option prints a triangulation. It adds points (the + centrums) to non-simplicial facets. + + v Compute the Voronoi diagram from the Delaunay tri- + angulation. The 'p' option prints the Voronoi ver- + tices. The 'o' option prints the Voronoi vertices + and the vertices in each Voronoi region. It lists + regions in site id order. The 'Fv' option prints + each ridge of the Voronoi diagram. The first or + zero'th vertex indicates the infinity vertex. Its + coordinates are qh_INFINITE (-10.101). It indi- + cates unbounded Voronoi regions or degenerate + Delaunay triangles. + + Hn,n,... + Compute halfspace intersection about [n,n,0,...]. + The input is a set of halfspaces defined in the + same format as 'n', 'Fo', and 'Fi'. Use 'Fp' to + print the intersection points. Use 'Fv' to list + the intersection points for each halfspace. The + other output formats display the dual convex hull. + + The point [n,n,n,...] is a feasible point for the + halfspaces, i.e., a point that is inside all of the + halfspaces (Hx+b <= 0). The default coordinate + value is 0. + + The input may start with a feasible point. If so, + use 'H' by itself. The input starts with a feasi- + ble point when the first number is the dimension, + the second number is "1", and the coordinates com- + plete a line. The 'FV' option produces a feasible + point for a convex hull. + + d Qu Compute the furthest-site Delaunay triangulation + from the upper convex hull. The 'o' option prints + the input points and facets. The 'QJ' option guar- + antees triangular otuput. You can also use facets. + + v Qu Compute the furthest-site Voronoi diagram. The 'p' + option prints the Voronoi vertices. The 'o' option + prints the Voronoi vertices and the vertices in + + + +Geometry Center 2003/12/30 4 + + + + + +qhull(1) qhull(1) + + + each Voronoi region. The 'Fv' option prints each + ridge of the Voronoi diagram. The first or zero'th + vertex indicates the infinity vertex at infinity. + Its coordinates are qh_INFINITE (-10.101). It + indicates unbounded Voronoi regions and degenerate + Delaunay triangles. + + Qt Triangulated output. + + + Input/Output options: + + f Print out all facets and all fields of each facet. + + G Output the hull in Geomview format. For imprecise + hulls, Geomview displays the inner and outer hull. + Geomview can also display points, ridges, vertices, + coplanar points, and facet intersections. See + below for a list of options. + + For Delaunay triangulations, 'G' displays the cor- + responding paraboloid. For halfspace intersection, + 'G' displays the dual polytope. + + i Output the incident vertices for each facet. Qhull + prints the number of facets followed by the ver- + tices of each facet. One facet is printed per + line. The numbers are the 0-relative indices of + the corresponding input points. The facets are + oriented. + + In 4-d and higher, Qhull triangulates non-simpli- + cial facets. Each apex (the first vertex) is a + created point that corresponds to the facet's cen- + trum. Its index is greater than the indices of the + input points. Each base corresponds to a simpli- + cial ridge between two facets. To print the ver- + tices without triangulation, use option 'Fv'. + + m Output the hull in Mathematica format. Qhull + writes a Mathematica file for 2-d and 3-d convex + hulls and for 2-d Delaunay triangulations. Qhull + produces a list of objects that you can assign to a + variable in Mathematica, for example: "list= << + <outputfilename> ". If the object is 2-d, it can be + visualized by "Show[Graphics[list]] ". For 3-d + objects the command is "Show[Graphics3D[list]]". + + n Output the normal equation for each facet. Qhull + prints the dimension (plus one), the number of + facets, and the normals for each facet. The + facet's offset follows its normal coefficients. + + o Output the facets in OFF file format. Qhull prints + the dimension, number of points, number of facets, + and number of ridges. Then it prints the + + + +Geometry Center 2003/12/30 5 + + + + + +qhull(1) qhull(1) + + + coordinates of the input points and the vertices + for each facet. Each facet is on a separate line. + The first number is the number of vertices. The + remainder are the indices of the corresponding + points. The vertices are oriented in 2-d, 3-d, and + in simplicial facets. + + For 2-d Voronoi diagrams, the vertices are sorted + by adjacency, but not oriented. In 3-d and higher, + the Voronoi vertices are sorted by index. See the + 'v' option for more information. + + p Output the coordinates of each vertex point. Qhull + prints the dimension, the number of points, and the + coordinates for each vertex. With the 'Gc' and + 'Gi' options, it also prints coplanar and interior + points. For Voronoi diagrams, it prints the coor- + dinates of each Voronoi vertex. + + s Print a summary to stderr. If no output options + are specified at all, a summary goes to stdout. + The summary lists the number of input points, the + dimension, the number of vertices in the convex + hull, the number of facets in the convex hull, the + number of good facets (if 'Pg'), and statistics. + + The last two statistics (if needed) measure the + maximum distance from a point or vertex to a facet. + The number in parenthesis (e.g., 2.1x) is the ratio + between the maximum distance and the worst-case + distance due to merging two simplicial facets. + + + Precision options + + An Maximum angle given as a cosine. If the angle + between a pair of facet normals is greater than n, Qhull + merges one of the facets into a neighbor. If 'n' + is negative, Qhull tests angles after adding each + point to the hull (pre-merging). If 'n' is posi- + tive, Qhull tests angles after constructing the + hull (post-merging). Both pre- and post-merging + can be defined. + + Option 'C0' or 'C-0' is set if the corresponding + 'Cn' or 'C-n' is not set. If 'Qx' is set, then 'A- + n' and 'C-n' are checked after the hull is con- + structed and before 'An' and 'Cn' are checked. + + Cn Centrum radius. If a centrum is less than n below + a neighboring facet, Qhull merges one of the + facets. If 'n' is negative or '-0', Qhull tests + and merges facets after adding each point to the + hull. This is called "pre-merging". If 'n' is + + + +Geometry Center 2003/12/30 6 + + + + + +qhull(1) qhull(1) + + + positive, Qhull tests for convexity after con- + structing the hull ("post-merging"). Both pre- and + post-merging can be defined. + + For 5-d and higher, 'Qx' should be used instead of + 'C-n'. Otherwise, most or all facets may be merged + together. + + En Maximum roundoff error for distance computations. + + Rn Randomly perturb distance computations up to +/- n + * max_coord. This option perturbs every distance, + hyperplane, and angle computation. To use time as + the random number seed, use option 'QR-1'. + + Vn Minimum distance for a facet to be visible. A + facet is visible if the distance from the point to + the facet is greater than 'Vn'. + + Without merging, the default value for 'Vn' is the + round-off error ('En'). With merging, the default + value is the pre-merge centrum ('C-n') in 2-d or + 3--d, or three times that in other dimensions. If + the outside width is specified ('Wn'), the maximum, + default value for 'Vn' is 'Wn'. + + Un Maximum distance below a facet for a point to be + coplanar to the facet. The default value is 'Vn'. + + Wn Minimum outside width of the hull. Points are + added to the convex hull only if they are clearly + outside of a facet. A point is outside of a facet + if its distance to the facet is greater than 'Wn'. + The normal value for 'Wn' is 'En'. If the user + specifies pre-merging and does not set 'Wn', than + 'Wn' is set to the premerge 'Cn' and maxco- + ord*(1-An). + + + Additional input/output formats + + Fa Print area for each facet. For Delaunay triangula- + tions, the area is the area of the triangle. For + Voronoi diagrams, the area is the area of the dual + facet. Use 'PAn' for printing the n largest + facets, and option 'PFn' for printing facets larger + than 'n'. + + The area for non-simplicial facets is the sum of + the areas for each ridge to the centrum. Vertices + far below the facet's hyperplane are ignored. The + reported area may be significantly less than the + actual area. + + + + +Geometry Center 2003/12/30 7 + + + + + +qhull(1) qhull(1) + + + FA Compute the total area and volume for option 's'. + It is an approximation for non-simplicial facets + (see 'Fa'). + + Fc Print coplanar points for each facet. The output + starts with the number of facets. Then each facet + is printed one per line. Each line is the number + of coplanar points followed by the point ids. + Option 'Qi' includes the interior points. Each + coplanar point (interior point) is assigned to the + facet it is furthest above (resp., least below). + + FC Print centrums for each facet. The output starts + with the dimension followed by the number of + facets. Then each facet centrum is printed, one + per line. + + Fd Read input in cdd format with homogeneous points. + The input starts with comments. The first comment + is reported in the summary. Data starts after a + "begin" line. The next line is the number of + points followed by the dimension+1 and "real" or + "integer". Then the points are listed with a + leading "1" or "1.0". The data ends with an "end" + line. + + For halfspaces ('Fd Hn,n,...'), the input format is + the same. Each halfspace starts with its offset. + The sign of the offset is the opposite of Qhull's + convention. + + FD Print normals ('n', 'Fo', 'Fi') or points ('p') in + cdd format. The first line is the command line + that invoked Qhull. Data starts with a "begin" + line. The next line is the number of normals or + points followed by the dimension+1 and "real". + Then the normals or points are listed with the + offset before the coefficients. The offset for + points is 1.0. The offset for normals has the + opposite sign. The data ends with an "end" line. + + FF Print facets (as in 'f') without printing the + ridges. + + Fi Print inner planes for each facet. The inner plane + is below all vertices. + + Fi Print separating hyperplanes for bounded, inner + regions of the Voronoi diagram. The first line is + the number of ridges. Then each hyperplane is + printed, one per line. A line starts with the num- + ber of indices and floats. The first pair lists + adjacent input sites, the next d floats are the + normalized coefficients for the hyperplane, and the + + + +Geometry Center 2003/12/30 8 + + + + + +qhull(1) qhull(1) + + + last float is the offset. The hyperplane is ori- + ented toward verify that the hyperplanes are per- + pendicular bisectors. Use 'Fo' for unbounded + regions, and 'Fv' for the corresponding Voronoi + vertices. + + FI Print facet identifiers. + + Fm Print number of merges for each facet. At most 511 + merges are reported for a facet. See 'PMn' for + printing the facets with the most merges. + + FM Output the hull in Maple format. See 'm' + + Fn Print neighbors for each facet. The output starts + with the number of facets. Then each facet is + printed one per line. Each line is the number of + neighbors followed by an index for each neighbor. + The indices match the other facet output formats. + + A negative index indicates an unprinted facet due + to printing only good facets ('Pg'). It is the + negation of the facet's id (option 'FI'). For + example, negative indices are used for facets "at + infinity" in the Delaunay triangulation. + + FN Print vertex neighbors or coplanar facet for each + point. The first line is the number of points. + Then each point is printed, one per line. If the + point is coplanar, the line is "1" followed by the + facet's id. If the point is not a selected vertex, + the line is "0". Otherwise, each line is the num- + ber of neighbors followed by the corresponding + facet indices (see 'Fn'). + + Fo Print outer planes for each facet in the same for- + mat as 'n'. The outer plane is above all points. + + Fo Print separating hyperplanes for unbounded, outer + regions of the Voronoi diagram. The first line is + the number of ridges. Then each hyperplane is + printed, one per line. A line starts with the num- + ber of indices and floats. The first pair lists + adjacent input sites, the next d floats are the + normalized coefficients for the hyperplane, and the + last float is the offset. The hyperplane is ori- + ented toward verify that the hyperplanes are per- + pendicular bisectors. Use 'Fi' for bounded + regions, and 'Fv' for the corresponding Voronoi + vertices. + + FO List all options to stderr, including the default + values. Additional 'FO's are printed to stdout. + + Fp Print points for halfspace intersections (option + 'Hn,n,...'). Each intersection corresponds to a + + + +Geometry Center 2003/12/30 9 + + + +qhull(1) qhull(1) + + + facet of the dual polytope. The "infinity" point + [-10.101,-10.101,...] indicates an unbounded + intersection. + + FP For each coplanar point ('Qc') print the point id + of the nearest vertex, the point id, the facet id, + and the distance. + + FQ Print command used for qhull and input. + + Fs Print a summary. The first line consists of the + number of integers ("7"), followed by the dimen- + sion, the number of points, the number of vertices, + the number of facets, the number of vertices + selected for output, the number of facets selected + for output, the number of coplanar points selected + for output. + + The second line consists of the number of reals + ("2"), followed by the maxmimum offset to an outer + plane and and minimum offset to an inner plane. + Roundoff is included. Later versions of Qhull may + produce additional integers or reals. + + FS Print the size of the hull. The first line con- + sists of the number of integers ("0"). The second + line consists of the number of reals ("2"), fol- + lowed by the total facet area, and the total vol- + ume. Later versions of Qhull may produce addi- + tional integers or reals. + + The total volume measures the volume of the inter- + section of the halfspaces defined by each facet. + Both area and volume are approximations for non- + simplicial facets. See option 'Fa'. + + Ft Print a triangulation with added points for non- + simplicial facets. The first line is the dimension + and the second line is the number of points and the + number of facets. The points follow, one per line, + then the facets follow as a list of point indices. + With option points include the point-at-infinity. + + Fv Print vertices for each facet. The first line is + the number of facets. Then each facet is printed, + one per line. Each line is the number of vertices + followed by the corresponding point ids. Vertices + are listed in the order they were added to the hull + (the last one is first). + + Fv Print all ridges of a Voronoi diagram. The first + line is the number of ridges. Then each ridge is + printed, one per line. A line starts with the num- + ber of indices. The first pair lists adjacent + + + +Geometry Center 2003/12/30 10 + + + + + +qhull(1) qhull(1) + + + input sites, the remaining indices list Voronoi + vertices. Vertex '0' indicates the vertex-at- + infinity (i.e., an unbounded ray). In 3-d, the + vertices are listed in order. See 'Fi' and 'Fo' + for separating hyperplanes. + + FV Print average vertex. The average vertex is a fea- + sible point for halfspace intersection. + + Fx List extreme points (vertices) of the convex hull. + The first line is the number of points. The other + lines give the indices of the corresponding points. + The first point is '0'. In 2-d, the points occur + in counter-clockwise order; otherwise they occur in + input order. For Delaunay triangulations, 'Fx' + lists the extreme points of the input sites. The + points are unordered. + + + Geomview options + + G Produce a file for viewing with Geomview. Without + other options, Qhull displays edges in 2-d, outer + planes in 3-d, and ridges in 4-d. A ridge can be + explicit or implicit. An explicit ridge is a dim-1 + dimensional simplex between two facets. In 4-d, + the explicit ridges are triangles. When displaying + a ridge in 4-d, Qhull projects the ridge's vertices + to one of its facets' hyperplanes. Use 'Gh' to + project ridges to the intersection of both hyper- + planes. + + Ga Display all input points as dots. + + Gc Display the centrum for each facet in 3-d. The + centrum is defined by a green radius sitting on a + blue plane. The plane corresponds to the facet's + hyperplane. The radius is defined by 'C-n' or + 'Cn'. + + GDn Drop dimension n in 3-d or 4-d. The result is a + 2-d or 3-d object. + + Gh Display hyperplane intersections in 3-d and 4-d. + In 3-d, the intersection is a black line. It lies + on two neighboring hyperplanes (c.f., the blue + squares associated with centrums ('Gc')). In 4-d, + the ridges are projected to the intersection of + both hyperplanes. + + Gi Display inner planes in 2-d and 3-d. The inner + plane of a facet is below all of its vertices. It + is parallel to the facet's hyperplane. The inner + plane's color is the opposite (1-r,1-g,1-b) of the + + + +Geometry Center 2003/12/30 11 + + + + + +qhull(1) qhull(1) + + + outer plane. Its edges are determined by the ver- + tices. + + Gn Do not display inner or outer planes. By default, + Geomview displays the precise plane (no merging) or + both inner and output planes (merging). Under + merging, Geomview does not display the inner plane + if the the difference between inner and outer is + too small. + + Go Display outer planes in 2-d and 3-d. The outer + plane of a facet is above all input points. It is + parallel to the facet's hyperplane. Its color is + determined by the facet's normal, and its edges are + determined by the vertices. + + Gp Display coplanar points and vertices as radii. A + radius defines a ball which corresponds to the + imprecision of the point. The imprecision is the + maximum of the roundoff error, the centrum radius, + and maxcoord * (1-An). It is at least 1/20'th of + the maximum coordinate, and ignores post-merging if + pre-merging is done. + + Gr Display ridges in 3-d. A ridge connects the two + vertices that are shared by neighboring facets. + Ridges are always displayed in 4-d. + + Gt A 3-d Delaunay triangulation looks like a convex + hull with interior facets. Option 'Gt' removes the + outside ridges to reveal the outermost facets. It + automatically sets options 'Gr' and 'GDn'. + + Gv Display vertices as spheres. The radius of the + sphere corresponds to the imprecision of the data. + See 'Gp' for determining the radius. + + + Print options + + PAn Only the n largest facets are marked good for + printing. Unless 'PG' is set, 'Pg' is automati- + cally set. + + Pdk:n Drop facet from output if normal[k] <= n. The + option 'Pdk' uses the default value of 0 for n. + + PDk:n Drop facet from output if normal[k] >= n. The + option 'PDk' uses the default value of 0 for n. + + PFn Only facets with area at least 'n' are marked good + for printing. Unless 'PG' is set, 'Pg' is automat- + ically set. + + + + +Geometry Center 2003/12/30 12 + + + + + +qhull(1) qhull(1) + + + Pg Print only good facets. A good facet is either + visible from a point (the 'QGn' option) or includes + a point (the 'QVn' option). It also meets the + requirements of 'Pdk' and 'PDk' options. Option + 'Pg' is automatically set for options 'PAn' and + 'PFn'. + + PG Print neighbors of good facets. + + PMn Only the n facets with the most merges are marked + good for printing. Unless 'PG' is set, 'Pg' is + automatically set. + + Po Force output despite precision problems. Verify ('Tv') does not check + coplanar points. Flipped facets are reported and + concave facets are counted. If 'Po' is used, + points are not partitioned into flipped facets and + a flipped facet is always visible to a point. + Also, if an error occurs before the completion of + Qhull and tracing is not active, 'Po' outputs a + neighborhood of the erroneous facets (if any). + + Pp Do not report precision problems. + + + Qhull control options + + Qbk:0Bk:0 + Drop dimension k from the input points. This + allows the user to take convex hulls of sub-dimen- + sional objects. It happens before the Delaunay and + Voronoi transformation. + + QbB Scale the input points to fit the unit cube. After + scaling, the lower bound will be -0.5 and the upper + bound +0.5 in all dimensions. For Delaunay and + Voronoi diagrams, scaling happens after projection + to the paraboloid. Under precise arithmetic, scal- + ing does not change the topology of the convex + hull. + + Qbb Scale the last coordinate to [0, m] where m is the + maximum absolute value of the other coordinates. + For Delaunay and Voronoi diagrams, scaling happens + after projection to the paraboloid. It reduces + roundoff error for inputs with integer coordinates. + Under precise arithmetic, scaling does not change + the topology of the convex hull. + + Qbk:n Scale the k'th coordinate of the input points. + After scaling, the lower bound of the input points + will be n. 'Qbk' scales to -0.5. + + + +Geometry Center 2003/12/30 13 + + + + + +qhull(1) qhull(1) + + + QBk:n Scale the k'th coordinate of the input points. + After scaling, the upper bound will be n. 'QBk' + scales to +0.5. + + Qc Keep coplanar points with the nearest facet. Out- + put formats 'p', 'f', 'Gp', 'Fc', 'FN', and 'FP' + will print the points. + + Qf Partition points to the furthest outside facet. + + Qg Only build good facets. With the 'Qg' option, + Qhull will only build those facets that it needs to + determine the good facets in the output. See + 'QGn', 'QVn', and 'PdD' for defining good facets, + and 'Pg' and 'PG' for printing good facets and + their neighbors. + + QGn A facet is good (see 'Qg' and 'Pg') if it is visi- + ble from point n. If n < 0, a facet is good if it + is not visible from point n. Point n is not added + to the hull (unless 'TCn' or 'TPn'). With rbox, + use the 'Pn,m,r' option to define your point; it + will be point 0 (QG0). + + Qi Keep interior points with the nearest facet. Out- + put formats 'p', 'f', 'Gp', 'FN', 'FP', and 'Fc' + will print the points. + + QJn Joggle each input coordinate by adding a random + number in [-n,n]. If a precision error occurs, + then qhull increases n and tries again. It does + not increase n beyond a certain value, and it stops + after a certain number of attempts [see user.h]. + Option 'QJ' selects a default value for n. The + output will be simplicial. For Delaunay triangula- + tions, 'QJn' sets 'Qbb' to scale the last coordi- + nate (not if 'Qbk:n' or 'QBk:n' is set). See also + 'Qt'. + + Qm Only process points that would otherwise increase + max_outside. Other points are treated as coplanar + or interior points. + + Qr Process random outside points instead of furthest + ones. This makes Qhull equivalent to the random- + ized incremental algorithms. CPU time is not + reported since the randomization is inefficient. + + QRn Randomly rotate the input points. If n=0, use time + as the random number seed. If n>0, use n as the + random number seed. If n=-1, don't rotate but use + time as the random number seed. For Delaunay tri- + angulations ('d' and 'v'), rotate about the last + axis. + + + + +Geometry Center 2003/12/30 14 + + + + + +qhull(1) qhull(1) + + + Qs Search all points for the initial simplex. + + Qt Triangulated output. Triangulate non-simplicial + facets. + + Qv Test vertex neighbors for convexity after post- + merging. To use the 'Qv' option, you also need to + set a merge option (e.g., 'Qx' or 'C-0'). + + QVn A good facet (see 'Qg' and 'Pg') includes point n. + If n<0, then a good facet does not include point n. + The point is either in the initial simplex or it is + the first point added to the hull. Option 'QVn' + may not be used with merging. + + Qx Perform exact merges while building the hull. The + "exact" merges are merging a point into a coplanar + facet (defined by 'Vn', 'Un', and 'C-n'), merging + concave facets, merging duplicate ridges, and merg- + ing flipped facets. Coplanar merges and angle + coplanar merges ('A-n') are not performed. Concav- + ity testing is delayed until a merge occurs. + + After the hull is built, all coplanar merges are + performed (defined by 'C-n' and 'A-n'), then post- + merges are performed (defined by 'Cn' and 'An'). + + Qz Add a point "at infinity" that is above the + paraboloid for Delaunay triangulations and Voronoi + diagrams. This reduces precision problems and + allows the triangulation of cospherical points. + + + Qhull experiments and speedups + + Q0 Turn off pre-merging as a default option. With + 'Q0'/'Qx' and without explicit pre-merge options, + Qhull ignores precision issues while constructing + the convex hull. This may lead to precision + errors. If so, a descriptive warning is generated. + + Q1 With 'Q1', Qhull sorts merges by type (coplanar, + angle coplanar, concave) instead of by angle. + + Q2 With 'Q2', Qhull merges all facets at once instead + of using independent sets of merges and then + retesting. + + Q3 With 'Q3', Qhull does not remove redundant ver- + tices. + + Q4 With 'Q4', Qhull avoids merges of an old facet into + a new facet. + + Q5 With 'Q5', Qhull does not correct outer planes at + the end. The maximum outer plane is used instead. + + + + +Geometry Center 2003/12/30 15 + + + + + +qhull(1) qhull(1) + + + Q6 With 'Q6', Qhull does not pre-merge concave or + coplanar facets. + + Q7 With 'Q7', Qhull processes facets in depth-first + order instead of breadth-first order. + + Q8 With 'Q8' and merging, Qhull does not retain near- + interior points for adjusting outer planes. 'Qc' + will probably retain all points that adjust outer + planes. + + Q9 With 'Q9', Qhull processes the furthest of all out- + side sets at each iteration. + + Q10 With 'Q10', Qhull does not use special processing + for narrow distributions. + + Q11 With 'Q11', Qhull copies normals and recomputes + centrums for tricoplanar facets. + + Trace options + + Tn Trace at level n. Qhull includes full execution + tracing. 'T-1' traces events. 'T1' traces the + overall execution of the program. 'T2' and 'T3' + trace overall execution and geometric and topologi- + cal events. 'T4' traces the algorithm. 'T5' + includes information about memory allocation and + Gaussian elimination. + + Tc Check frequently during execution. This will catch + most inconsistency errors. + + TCn Stop Qhull after building the cone of new facets + for point n. The output for 'f' includes the cone + and the old hull. See also 'TVn'. + + TFn Report progress whenever more than n facets are + created During post-merging, 'TFn' reports progress + after more than n/2 merges. + + TI file + Input data from 'file'. The filename may not include + spaces or quotes. + + TO file + Output results to 'file'. The name may be enclosed + in single quotes. + + TPn Turn on tracing when point n is added to the hull. + Trace partitions of point n. If used with TWn, turn off + tracing after adding point n to the hull. + + TRn Rerun qhull n times. Usually used with 'QJn' to + determine the probability that a given joggle will + fail. + + Ts Collect statistics and print to stderr at the end + of execution. + + Tv Verify the convex hull. This checks the topologi- + cal structure, facet convexity, and point inclu- + sion. If precision problems occurred, facet con- + vexity is tested whether or not 'Tv' is selected. + Option 'Tv' does not check point inclusion if + + + +Geometry Center 2003/12/30 16 + + + + + +qhull(1) qhull(1) + + + forcing output with 'Po', or if 'Q5' is set. + + For point inclusion testing, Qhull verifies that + all points are below all outer planes (facet->max- + outside). Point inclusion is exhaustive if merging + or if the facet-point product is small enough; oth- + erwise Qhull verifies each point with a directed + search (qh_findbest). + + Point inclusion testing occurs after producing out- + put. It prints a message to stderr unless option + 'Pp' is used. This allows the user to interrupt + Qhull without changing the output. + + TVn Stop Qhull after adding point n. If n < 0, stop + Qhull before adding point n. Output shows the hull + at this time. See also 'TCn' + + TMn Turn on tracing at n'th merge. + + TWn Trace merge facets when the width is greater than + n. + + Tz Redirect stderr to stdout. + + +BUGS + Please report bugs to Brad Barber at + qhull_bug@qhull.org. + + If Qhull does not compile, it is due to an incompatibility + between your system and ours. The first thing to check is + that your compiler is ANSI standard. If it is, check the + man page for the best options, or find someone to help + you. If you locate the cause of your problem, please send + email since it might help others. + + If Qhull compiles but crashes on the test case (rbox D4), + there's still incompatibility between your system and + ours. Typically it's been due to mem.c and memory align- + ment. You can use qh_NOmem in mem.h to turn off memory + management. Please let us know if you figure out how to + fix these problems. + + If you do find a problem, try to simplify it before + reporting the error. Try different size inputs to locate + the smallest one that causes an error. You're welcome to + hunt through the code using the execution trace as a + guide. This is especially true if you're incorporating + Qhull into your own program. + + When you do report an error, please attach a data set to + the end of your message. This allows us to see the error + for ourselves. Qhull is maintained part-time. + + + +Geometry Center 2003/12/30 17 + + + + + +qhull(1) qhull(1) + + +E-MAIL + Please send correspondence to qhull@qhull.org and + report bugs to qhull_bug@qhull.org. Let us know how + you use Qhull. If you mention it in a paper, please send + the reference and an abstract. + + If you would like to get Qhull announcements (e.g., a new + version) and news (any bugs that get fixed, etc.), let us + know and we will add you to our mailing list. If you + would like to communicate with other Qhull users, we will + add you to the qhull_users alias. For Internet news about + geometric algorithms and convex hulls, look at comp.graph- + ics.algorithms and sci.math.num-analysis + + +SEE ALSO + rbox(1) + + Barber, C. B., D.P. Dobkin, and H.T. Huhdanpaa, "The + Quickhull Algorithm for Convex Hulls," ACM Trans. on Math- + ematical Software, Dec. 1996. + http://www.acm.org/pubs/citations/journals/toms/1996-22-4/p469-barber/ + http://citeseer.nj.nec.com/83502.html + + + Clarkson, K.L., K. Mehlhorn, and R. Seidel, "Four results + on randomized incremental construction," Computational + Geometry: Theory and Applications, vol. 3, p. 185-211, + 1993. + + Preparata, F. and M. Shamos, Computational Geometry, + Springer-Verlag, New York, 1985. + + + +AUTHORS + C. Bradford Barber Hannu Huhdanpaa + bradb@qhull.org hannu@qhull.org + + c/o The Geometry Center + University of Minnesota + 400 Lind Hall + 207 Church Street S.E. + Minneapolis, MN 55455 + + + +ACKNOWLEDGEMENTS + A special thanks to Albert Marden, Victor Milenkovic, the + Geometry Center, Harvard University, and Endocardial Solu- + tions, Inc. for supporting this work. + + The software was developed under National Science Founda- + tion grants NSF/DMS-8920161 and NSF-CCR-91-15793 750-7504. + + + +Geometry Center 2003/12/30 18 + + + + + +qhull(1) qhull(1) + + + David Dobkin guided the original work at Princeton Univer- + sity. If you find it useful, please let us know. + + The Geometry Center is supported by grant DMS-8920161 from + the National Science Foundation, by grant DOE/DE- + FG02-92ER25137 from the Department of Energy, by the Uni- + versity of Minnesota, and by Minnesota Technology, Inc. + + Qhull is available at http://www.qhull.org + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +Geometry Center 2003/12/30 19 + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qvoron_f.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qvoron_f.htm new file mode 100644 index 0000000000000000000000000000000000000000..de64310f8c0da0639bfe18e3939d653ed00b0eb4 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qvoron_f.htm @@ -0,0 +1,408 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>qvoronoi Qu -- furthest-site Voronoi diagram</title> +</head> + +<body> +<!-- Navigation links --> +<a name="TOP"><b>Up</b></a><b>:</b> +<a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a>qvoronoi Qu -- furthest-site Voronoi diagram</h1> + +<p>The furthest-site Voronoi diagram is the furthest-neighbor map for a set of +points. Each region contains those points that are further +from one input site than any other input site. See the +survey article by Aurenhammer [<a href="index.htm#aure91">'91</a>] +and the brief introduction by O'Rourke [<a +href="index.htm#orou94">'94</a>]. The furthest-site Voronoi diagram is the dual of the <a +href="qdelau_f.htm">furthest-site Delaunay triangulation</a>. +</p> + +<blockquote> +<dl> + <dt><b>Example:</b> rbox 10 D2 | qvoronoi <a + href="qh-optq.htm#Qu">Qu</a> <a href="qh-opto.htm#s">s</a> + <a href="qh-opto.htm#o">o</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d, furthest-site Voronoi diagram of 10 + random points. Write a summary to the console and the Voronoi + regions and vertices to 'result'. The first vertex of the + result indicates unbounded regions. Almost all regions + are unbounded.</dd> +</dl> + +<dl> + <dt><b>Example:</b> rbox r y c G1 D2 | qvoronoi <a + href="qh-optq.htm#Qu">Qu</a> + <a href="qh-opto.htm#s">s</a> + <a href="qh-optf.htm#Fn">Fn</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d furthest-site Voronoi diagram of a square + and a small triangle. Write a summary to the console and the Voronoi + vertices for each input site to 'result'. + The origin is the only furthest-site Voronoi vertex. The + negative indices indicate vertices-at-infinity.</dd> +</dl> +</blockquote> + +<p> +Qhull computes the furthest-site Voronoi diagram via the <a href="qdelau_f.htm"> +furthest-site Delaunay triangulation</a>. +Each furthest-site Voronoi vertex is the circumcenter of an upper +facet of the Delaunay triangulation. Each furthest-site Voronoi +region corresponds to a vertex of the Delaunay triangulation +(i.e., an input site).</p> + +<p>See <a href="http://www.qhull.org/html/qh-faq.htm#TOC">Qhull FAQ</a> - Delaunay and +Voronoi diagram questions.</p> + +<p>Options '<a href="qh-optq.htm#Qt">Qt</a>' (triangulated output) +and '<a href="qh-optq.htm#QJn">QJ</a>' (joggled input), may produce +unexpected results. Cocircular and cospherical input sites will +produce duplicate or nearly duplicate furthest-site Voronoi vertices. See also <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>The 'qvonoroi' program is equivalent to +'<a href=qhull.htm#outputs>qhull v</a> <a href=qh-optq.htm#Qbb>Qbb</a>' in 2-d to 3-d, and +'<a href=qhull.htm#outputs>qhull v</a> <a href=qh-optq.htm#Qbb>Qbb</a> <a href=qh-optq.htm#Qx>Qx</a>' +in 4-d and higher. It disables the following Qhull +<a href=qh-quick.htm#options>options</a>: <i>d n m v H U Qb +QB Qc Qf Qg Qi Qm Qr QR Qv Qx TR E V Fa FA FC Fp FS Ft FV Gt Q0,etc</i>. + + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> +<h3><a href="#TOP">»</a><a name="synopsis">furthest-site qvoronoi synopsis</a></h3> +<blockquote> + +See <a href="qvoronoi.htm#synopsis">qvoronoi synopsis</a>. The same +program is used for both constructions. Use option '<a href="qh-optq.htm#Qu">Qu</a>' +for furthest-site Voronoi diagrams. + + +</blockquote> +<h3><a href="#TOP">»</a><a name="input">furthest-site qvoronoi +input</a></h3> +<blockquote> +<p>The input data on <tt>stdin</tt> consists of:</p> +<ul> + <li>dimension + <li>number of points</li> + <li>point coordinates</li> +</ul> + +<p>Use I/O redirection (e.g., qvoronoi Qu < data.txt), a pipe (e.g., rbox 10 | qvoronoi Qu), +or the '<a href=qh-optt.htm#TI>TI</a>' option (e.g., qvoronoi TI data.txt Qu). + +<p>For example, this is a square containing four random points. +Its furthest-site Voronoi diagram has on vertex and four unbounded, +separating hyperplanes (i.e., the coordinate axes) +<p> +<blockquote> +<tt>rbox c 4 D2 > data</tt> +<blockquote><pre> +2 RBOX c 4 D2 +8 +-0.4999921736307369 -0.3684622117955817 +0.2556053225468894 -0.0413498678629751 +0.0327672376602583 -0.2810408135699488 +-0.452955383763607 0.17886471718444 + -0.5 -0.5 + -0.5 0.5 + 0.5 -0.5 + 0.5 0.5 +</pre></blockquote> + +<p><tt>qvoronoi Qu s Fo < data</tt> +<blockquote><pre> + +Furthest-site Voronoi vertices by the convex hull of 8 points in 3-d: + + Number of Voronoi regions: 8 + Number of Voronoi vertices: 1 + Number of non-simplicial Voronoi vertices: 1 + +Statistics for: RBOX c 4 D2 | QVORONOI Qu s Fo + + Number of points processed: 8 + Number of hyperplanes created: 20 + Number of facets in hull: 11 + Number of distance tests for qhull: 34 + Number of merged facets: 1 + Number of distance tests for merging: 107 + CPU seconds to compute hull (after input): 0 + +4 +5 4 5 0 1 0 +5 4 6 1 0 0 +5 5 7 1 0 0 +5 6 7 0 1 0 +</pre></blockquote> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a> <a name="outputs">furthest-site qvoronoi +outputs</a></h3> +<blockquote> + +<p>These options control the output of furthest-site Voronoi diagrams.</p> +<blockquote> + +<dl compact> + <dt> </dt> + <dd><b>furthest-site Voronoi vertices</b></dd> + <dt><a href="qh-opto.htm#p">p</a></dt> + <dd>print the coordinates of the furthest-site Voronoi vertices. The first line + is the dimension. The second line is the number of vertices. Each + remaining line is a furthest-site Voronoi vertex. The points-in-square example + has one furthest-site Voronoi vertex at the origin.</dd> + <dt><a href="qh-optf.htm#Fn">Fn</a></dt> + <dd>list the neighboring furthest-site Voronoi vertices for each furthest-site Voronoi + vertex. The first line is the number of Voronoi vertices. Each + remaining line starts with the number of neighboring vertices. Negative indices (e.g., <em>-1</em>) indicate vertices + outside of the Voronoi diagram. In the points-in-square example, the + Voronoi vertex at the origin has four neighbors-at-infinity.</dd> + <dt><a href="qh-optf.htm#FN">FN</a></dt> + <dd>list the furthest-site Voronoi vertices for each furthest-site Voronoi region. The first line is + the number of Voronoi regions. Each remaining line starts with the + number of Voronoi vertices. Negative indices (e.g., <em>-1</em>) indicate vertices + outside of the Voronoi diagram. + In the points-in-square example, all regions share the Voronoi vertex + at the origin.</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>furthest-site Voronoi regions</b></dd> + <dt><a href="qh-opto.htm#o">o</a></dt> + <dd>print the furthest-site Voronoi regions in OFF format. The first line is the + dimension. The second line is the number of vertices, the number + of input sites, and "1". The third line represents the vertex-at-infinity. + Its coordinates are "-10.101". The next lines are the coordinates + of the furthest-site Voronoi vertices. Each remaining line starts with the number + of Voronoi vertices in a Voronoi region. In 2-d, the vertices are +listed in adjacency order (unoriented). In 3-d and higher, the +vertices are listed in numeric order. In the points-in-square + example, each unbounded region includes the Voronoi vertex at + the origin. Lines consisting of <em>0</em> indicate + interior input sites. </dd> + <dt><a href="qh-optf.htm#Fi2">Fi</a></dt> + <dd>print separating hyperplanes for inner, bounded furthest-site Voronoi + regions. The first number is the number of separating + hyperplanes. Each remaining line starts with <i>3+dim</i>. The + next two numbers are adjacent input sites. The next <i>dim</i> + numbers are the coefficients of the separating hyperplane. The + last number is its offset. The are no bounded, separating hyperplanes + for the points-in-square example.</dd> + <dt><a href="qh-optf.htm#Fo2">Fo</a></dt> + <dd>print separating hyperplanes for outer, unbounded furthest-site Voronoi + regions. The first number is the number of separating + hyperplanes. Each remaining line starts with <i>3+dim</i>. The + next two numbers are adjacent input sites on the convex hull. The + next <i>dim</i> + numbers are the coefficients of the separating hyperplane. The + last number is its offset. The points-in-square example has four + unbounded, separating hyperplanes.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Input sites</b></dd> + <dt><a href="qh-optf.htm#Fv2">Fv</a></dt> + <dd>list ridges of furthest-site Voronoi vertices for pairs of input sites. The + first line is the number of ridges. Each remaining line starts with + two plus the number of Voronoi vertices in the ridge. The next + two numbers are two adjacent input sites. The remaining numbers list + the Voronoi vertices. As with option 'o', a <em>0</em> indicates + the vertex-at-infinity + and an unbounded, separating hyperplane. + The perpendicular bisector (separating hyperplane) + of the input sites is a flat through these vertices. + In the points-in-square example, the ridge for each edge of the square + is unbounded.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="qh-opto.htm#s">s</a></dt> + <dd>print summary of the furthest-site Voronoi diagram. Use '<a + href="qh-optf.htm#Fs">Fs</a>' for numeric data.</dd> + <dt><a href="qh-opto.htm#i">i</a></dt> + <dd>list input sites for each <a href=qdelau_f.htm>furthest-site Delaunay region</a>. Use option '<a href="qh-optp.htm#Pp">Pp</a>' + to avoid the warning. The first line is the number of regions. The + remaining lines list the input sites for each region. The regions are + oriented. In the points-in-square example, the square region has four + input sites. In 3-d and higher, report cospherical sites by adding extra points. + </dd> + <dt><a href="qh-optg.htm#G">G</a></dt> + <dd>Geomview output for 2-d furthest-site Voronoi diagrams.</dd> + </dl> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a> <a name="controls">furthest-site qvoronoi +controls</a></h3> +<blockquote> + +<p>These options provide additional control:</p> +<blockquote> + +<dl compact> + <dt><a href="qh-optq.htm#Qu">Qu</a></dt> + <dd>must be used.</dd> + <dt><a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>triangulated output. If a furthest-site Voronoi vertex is defined by cospherical data, Qhull + duplicates the vertex. For example, if 2-d data is contained in a square, the output + will contain two, identical furthest-site Voronoi vertices.</dd> + <dt><a href="qh-optq.htm#QJn">QJ</a></dt> + <dd>joggle the input to avoid furthest-site Voronoi vertices defined by more + than <i>dim+1</i> points. It is less accurate than triangulated output ('Qt'). + </dd> + <dt><a href="qh-optq.htm#QVn">QVn</a></dt> + <dd>select furthest-site Voronoi vertices for input site <em>n</em> </dd> + <dt><a href="qh-optt.htm#Tv">Tv</a></dt> + <dd>verify result</dd> + <dt><a href="qh-optt.htm#TO">TI file</a></dt> + <dd>input data from file. The filename may not use spaces or quotes.</dd> + <dt><a href="qh-optt.htm#TO">TO file</a></dt> + <dd>output results to file. Use single quotes if the filename + contains spaces (e.g., <tt>TO 'file with spaces.txt'</tt></dd> + <dt><a href="qh-optt.htm#TFn">TFn</a></dt> + <dd>report progress after constructing <em>n</em> facets</dd> + <dt><a href="qh-optp.htm#PDk">PDk:1</a></dt> + <dd>include upper and lower facets in the output. Set <em>k</em> + to the last dimension (e.g., 'PD2:1' for 2-d inputs). </dd> + <dt><a href="qh-opto.htm#f">f </a></dt> + <dd>facet dump. Print the data structure for each facet (i.e., + furthest-site Voronoi vertex).</dd> +</dl> + +</blockquote> +</blockquote> +<h3><a href="#TOP">»</a> <a name="graphics">furthest-site qvoronoi +graphics</a></h3> +<blockquote> +<p>In 2-d, Geomview output ('<a href="qh-optg.htm#G">G</a>') +displays a furthest-site Voronoi diagram with extra edges to +close the unbounded furthest-site Voronoi regions. All regions +will be unbounded. Since the points-in-box example has only +one furthest-site Voronoi vertex, the Geomview output is one +point.</p> + +<p>See the <a href="qh-eg.htm#delaunay">Delaunay and Voronoi +examples</a> for a 2-d example. Turn off normalization (on +Geomview's 'obscure' menu) when comparing the furthest-site +Voronoi diagram with the corresponding Voronoi diagram. </p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="notes">furthest-site qvoronoi +notes</a></h3> +<blockquote> + +<p>See <a href="qvoronoi.htm#notes">Voronoi notes</a>.</p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="conventions">furthest-site qvoronoi conventions</a></h3> +<blockquote> + +<p>The following terminology is used for furthest-site Voronoi +diagrams in Qhull. The underlying structure is a furthest-site +Delaunay triangulation from a convex hull in one higher +dimension. Upper facets of the Delaunay triangulation correspond +to vertices of the furthest-site Voronoi diagram. Vertices of the +furthest-site Delaunay triangulation correspond to input sites. +They also define regions of the furthest-site Voronoi diagram. +All vertices are extreme points of the input sites. See <a +href="qconvex.htm#conventions">qconvex conventions</a>, <a +href="qdelau_f.htm#conventions">furthest-site delaunay +conventions</a>, and <a href="index.htm#structure">Qhull's data structures</a>.</p> + +<ul> + <li><em>input site</em> - a point in the input (one dimension + lower than a point on the convex hull)</li> + <li><em>point</em> - a point has <i>d+1</i> coordinates. The + last coordinate is the sum of the squares of the input + site's coordinates</li> + <li><em>vertex</em> - a point on the upper facets of the + paraboloid. It corresponds to a unique input site. </li> + <li><em>furthest-site Delaunay facet</em> - an upper facet of the + paraboloid. The last coefficient of its normal is + clearly positive.</li> + <li><em>furthest-site Voronoi vertex</em> - the circumcenter + of a furthest-site Delaunay facet</li> + <li><em>furthest-site Voronoi region</em> - the region of + Euclidean space further from an input site than any other + input site. Qhull lists the furthest-site Voronoi + vertices that define each furthest-site Voronoi region.</li> + <li><em>furthest-site Voronoi diagram</em> - the graph of the + furthest-site Voronoi regions with the ridges (edges) + between the regions.</li> + <li><em>infinity vertex</em> - the Voronoi vertex for + unbounded furthest-site Voronoi regions in '<a + href="qh-opto.htm#o">o</a>' output format. Its + coordinates are <em>-10.101</em>.</li> + <li><em>good facet</em> - an furthest-site Voronoi vertex with + optional restrictions by '<a href="qh-optq.htm#QVn">QVn</a>', + etc.</li> +</ul> + +</blockquote> +<h3><a href="#TOP">»</a><a name="options">furthest-site qvoronoi options</a></h3> +<blockquote> + +See <a href="qvoronoi.htm#options">qvoronoi options</a>. The same +program is used for both constructions. Use option '<a href="qh-optq.htm#Qu">Qu</a>' +for furthest-site Voronoi diagrams. +</blockquote> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qvoronoi.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qvoronoi.htm new file mode 100644 index 0000000000000000000000000000000000000000..322a54e15a18c96db604dcabc8533f8136c7a5a0 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/qvoronoi.htm @@ -0,0 +1,673 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>qvoronoi -- Voronoi diagram</title> +</head> + +<body> +<!-- Navigation links --> +<a name="TOP"><b>Up</b></a><b>:</b> +<a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions + +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/delaunay.html"><img +src="qh--dt.gif" alt="[delaunay]" align="middle" width="100" +height="100"></a>qvoronoi -- Voronoi diagram</h1> + +<p>The Voronoi diagram is the nearest-neighbor map for a set of +points. Each region contains those points that are nearer +one input site than any other input site. It has many useful properties and applications. See the +survey article by Aurenhammer [<a href="index.htm#aure91">'91</a>] +and the detailed introduction by O'Rourke [<a +href="index.htm#orou94">'94</a>]. The Voronoi diagram is the +dual of the <a href=qdelaun.htm>Delaunay triangulation</a>. </p> + +<blockquote> +<dl> + <dt><b>Example:</b> rbox 10 D3 | qvoronoi <a href="qh-opto.htm#s">s</a> + <a href="qh-opto.htm#o">o</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 3-d Voronoi diagram of 10 random points. Write a + summary to the console and the Voronoi vertices and + regions to 'result'. The first vertex of the result + indicates unbounded regions.</dd> + + <dt> </dt> + <dt><b>Example:</b> rbox r y c G0.1 D2 | qvoronoi + <a href="qh-opto.htm#s">s</a> + <a href="qh-opto.htm#o">o</a> <a href="qh-optt.htm#TO">TO + result</a></dt> + <dd>Compute the 2-d Voronoi diagram of a triangle and a small + square. Write a + summary to the console and Voronoi vertices and regions + to 'result'. Report a single Voronoi vertex for + cocircular input sites. The first vertex of the result + indicates unbounded regions. The origin is the Voronoi + vertex for the square.</dd> + + <dt> </dt> + <dt><b>Example:</b> rbox r y c G0.1 D2 | qvoronoi <a href="qh-optf.htm#Fv2">Fv</a> + <a href="qh-optt.htm#TO">TO result</a></dt> + <dd>Compute the 2-d Voronoi diagram of a triangle and a small + square. Write a + summary to the console and the Voronoi ridges to + 'result'. Each ridge is the perpendicular bisector of a + pair of input sites. Vertex "0" indicates + unbounded ridges. Vertex "8" is the Voronoi + vertex for the square.</dd> + + <dt> </dt> + <dt><b>Example:</b> rbox r y c G0.1 D2 | qvoronoi <a href="qh-optf.htm#Fi2">Fi</a></dt> + <dd>Print the bounded, separating hyperplanes for the 2-d Voronoi diagram of a + triangle and a small + square. Note the four hyperplanes (i.e., lines) for Voronoi vertex + "8". It is at the origin. + </dd> +</dl> +</blockquote> + +<p>Qhull computes the Voronoi diagram via the <a href="qdelaun.htm">Delaunay +triangulation</a>. Each Voronoi +vertex is the circumcenter of a facet of the Delaunay +triangulation. Each Voronoi region corresponds to a vertex (i.e., input site) of the +Delaunay triangulation. </p> + +<p>Qhull outputs the Voronoi vertices for each Voronoi region. With +option '<a href="qh-optf.htm#Fv2">Fv</a>', +it lists all ridges of the Voronoi diagram with the corresponding +pairs of input sites. With +options '<a href="qh-optf.htm#Fi2">Fi</a>' and '<a href="qh-optf.htm#Fo2">Fo</a>', +it lists the bounded and unbounded separating hyperplanes. +You can also output a single Voronoi region +for further processing [see <a href="#graphics">graphics</a>].</p> + +<p>See <a href="http://www.qhull.org/html/qh-faq.htm#TOC">Qhull FAQ</a> - Delaunay and +Voronoi diagram questions.</p> + +<p>Options '<a href="qh-optq.htm#Qt">Qt</a>' (triangulated output) +and '<a href="qh-optq.htm#QJn">QJ</a>' (joggled input) may produce +unexpected results. Cocircular and cospherical input sites will +produce duplicate or nearly duplicate Voronoi vertices. See also <a +href="qh-impre.htm#joggle">Merged facets or joggled input</a>. </p> + +<p>The 'qvonoroi' program is equivalent to +'<a href=qhull.htm#outputs>qhull v</a> <a href=qh-optq.htm#Qbb>Qbb</a>' in 2-d to 3-d, and +'<a href=qhull.htm#outputs>qhull v</a> <a href=qh-optq.htm#Qbb>Qbb</a> <a href=qh-optq.htm#Qx>Qx</a>' +in 4-d and higher. It disables the following Qhull +<a href=qh-quick.htm#options>options</a>: <i>d n v Qbb QbB Qf Qg Qm +Qr QR Qv Qx Qz TR E V Fa FA FC FD FS Ft FV Gt Q0,etc</i>. + +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> +<h3><a href="#TOP">»</a><a name="synopsis">qvoronoi synopsis</a></h3> + +<pre> +qvoronoi- compute the Voronoi diagram. + input (stdin): dimension, number of points, point coordinates + comments start with a non-numeric character + +options (qh-voron.htm): + Qu - compute furthest-site Voronoi diagram + Qt - triangulated output + QJ - joggle input instead of merging facets + Tv - verify result: structure, convexity, and in-circle test + . - concise list of all options + - - one-line description of all options + +output options (subset): + s - summary of results (default) + p - Voronoi vertices + o - OFF file format (dim, Voronoi vertices, and Voronoi regions) + FN - count and Voronoi vertices for each Voronoi region + Fv - Voronoi diagram as Voronoi vertices between adjacent input sites + Fi - separating hyperplanes for bounded regions, 'Fo' for unbounded + G - Geomview output (2-d only) + QVn - Voronoi vertices for input point n, -n if not + TO file- output results to file, may be enclosed in single quotes + +examples: +rbox c P0 D2 | qvoronoi s o rbox c P0 D2 | qvoronoi Fi +rbox c P0 D2 | qvoronoi Fo rbox c P0 D2 | qvoronoi Fv +rbox c P0 D2 | qvoronoi s Qu Fv rbox c P0 D2 | qvoronoi Qu Fo +rbox c G1 d D2 | qvoronoi s p rbox c G1 d D2 | qvoronoi Qt s p +rbox c P0 D2 | qvoronoi s Fv QV0 +</pre> + +<h3><a href="#TOP">»</a><a name="input">qvoronoi input</a></h3> +<blockquote> +The input data on <tt>stdin</tt> consists of: +<ul> + <li>dimension + <li>number of points</li> + <li>point coordinates</li> +</ul> + +<p>Use I/O redirection (e.g., qvoronoi < data.txt), a pipe (e.g., rbox 10 | qvoronoi), +or the '<a href=qh-optt.htm#TI>TI</a>' option (e.g., qvoronoi TI data.txt). + +<p>For example, this is four cocircular points inside a square. Their Voronoi +diagram has nine vertices and eight regions. Notice the Voronoi vertex +at the origin, and the Voronoi vertices (on each axis) for the four +sides of the square. +<p> +<blockquote> +<tt>rbox s 4 W0 c G1 D2 > data</tt> +<blockquote><pre> +2 RBOX s 4 W0 c D2 +8 +-0.4941988586954018 -0.07594397977563715 +-0.06448037284989526 0.4958248496365813 +0.4911154367094632 0.09383830681375946 +-0.348353580869097 -0.3586778257652367 + -1 -1 + -1 1 + 1 -1 + 1 1 +</pre></blockquote> + +<p><tt>qvoronoi s p < data</tt> +<blockquote><pre> + +Voronoi diagram by the convex hull of 8 points in 3-d: + + Number of Voronoi regions: 8 + Number of Voronoi vertices: 9 + Number of non-simplicial Voronoi vertices: 1 + +Statistics for: RBOX s 4 W0 c D2 | QVORONOI s p + + Number of points processed: 8 + Number of hyperplanes created: 18 + Number of facets in hull: 10 + Number of distance tests for qhull: 33 + Number of merged facets: 2 + Number of distance tests for merging: 102 + CPU seconds to compute hull (after input): 0.094 + +2 +9 +4.217546450968612e-17 1.735507986399734 +-8.402566836762659e-17 -1.364368854147395 +0.3447488772716865 -0.6395484723719818 +1.719446929853986 2.136555906154247e-17 +0.4967882915039657 0.68662371396699 +-1.729928876283549 1.343733067524222e-17 +-0.8906163241424728 -0.4594150543829102 +-0.6656840313875723 0.5003013793414868 +-7.318364664277155e-19 -1.188217818408333e-16 +</pre></blockquote> +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a> <a name="outputs">qvoronoi +outputs</a></h3> +<blockquote> + +<p>These options control the output of Voronoi diagrams.</p> +<blockquote> + +<dl compact> + <dt> </dt> + <dd><b>Voronoi vertices</b></dd> + <dt><a href="qh-opto.htm#p">p</a></dt> + <dd>print the coordinates of the Voronoi vertices. The first line + is the dimension. The second line is the number of vertices. Each + remaining line is a Voronoi vertex.</dd> + <dt><a href="qh-optf.htm#Fn">Fn</a></dt> + <dd>list the neighboring Voronoi vertices for each Voronoi + vertex. The first line is the number of Voronoi vertices. Each + remaining line starts with the number of neighboring vertices. + Negative vertices (e.g., <em>-1</em>) indicate vertices + outside of the Voronoi diagram. + In the circle-in-box example, the + Voronoi vertex at the origin has four neighbors.</dd> + <dt><a href="qh-optf.htm#FN">FN</a></dt> + <dd>list the Voronoi vertices for each Voronoi region. The first line is + the number of Voronoi regions. Each remaining line starts with the + number of Voronoi vertices. Negative indices (e.g., <em>-1</em>) indicate vertices + outside of the Voronoi diagram. + In the circle-in-box example, the four bounded regions are defined by four + Voronoi vertices.</dd> + + <dt> </dt> + <dt> </dt> + <dd><b>Voronoi regions</b></dd> + <dt><a href="qh-opto.htm#o">o</a></dt> + <dd>print the Voronoi regions in OFF format. The first line is the + dimension. The second line is the number of vertices, the number + of input sites, and "1". The third line represents the vertex-at-infinity. + Its coordinates are "-10.101". The next lines are the coordinates + of the Voronoi vertices. Each remaining line starts with the number + of Voronoi vertices in a Voronoi region. In 2-d, the vertices are +listed in adjacency order (unoriented). In 3-d and higher, the +vertices are listed in numeric order. In the circle-in-square + example, each bounded region includes the Voronoi vertex at + the origin. Lines consisting of <em>0</em> indicate + coplanar input sites or '<a href="qh-optq.htm#Qz">Qz</a>'. </dd> + <dt><a href="qh-optf.htm#Fi2">Fi</a></dt> + <dd>print separating hyperplanes for inner, bounded Voronoi + regions. The first number is the number of separating + hyperplanes. Each remaining line starts with <i>3+dim</i>. The + next two numbers are adjacent input sites. The next <i>dim</i> + numbers are the coefficients of the separating hyperplane. The + last number is its offset. Use '<a href="qh-optt.htm#Tv">Tv</a>' to verify that the +hyperplanes are perpendicular bisectors. It will list relevant +statistics to stderr. </dd> + <dt><a href="qh-optf.htm#Fo2">Fo</a></dt> + <dd>print separating hyperplanes for outer, unbounded Voronoi + regions. The first number is the number of separating + hyperplanes. Each remaining line starts with <i>3+dim</i>. The + next two numbers are adjacent input sites on the convex hull. The + next <i>dim</i> + numbers are the coefficients of the separating hyperplane. The + last number is its offset. Use '<a href="qh-optt.htm#Tv">Tv</a>' to verify that the +hyperplanes are perpendicular bisectors. It will list relevant +statistics to stderr,</dd> + <dt> </dt> + <dt> </dt> + <dd><b>Input sites</b></dd> + <dt><a href="qh-optf.htm#Fv2">Fv</a></dt> + <dd>list ridges of Voronoi vertices for pairs of input sites. The + first line is the number of ridges. Each remaining line starts with + two plus the number of Voronoi vertices in the ridge. The next + two numbers are two adjacent input sites. The remaining numbers list + the Voronoi vertices. As with option 'o', a <em>0</em> indicates + the vertex-at-infinity + and an unbounded, separating hyperplane. + The perpendicular bisector (separating hyperplane) + of the input sites is a flat through these vertices. + In the circle-in-square example, the ridge for each edge of the square + is unbounded.</dd> + <dt><a href="qh-optf.htm#Fc">Fc</a></dt> + <dd>list coincident input sites for each Voronoi vertex. + The first line is the number of vertices. The remaining lines start with + the number of coincident sites and deleted vertices. Deleted vertices + indicate highly degenerate input (see'<A href="qh-optf.htm#Fs">Fs</a>'). + A coincident site is assigned to one Voronoi + vertex. Do not use '<a href="qh-optq.htm#QJn">QJ</a>' with 'Fc'; the joggle will separate + coincident sites.</dd> + <dt><a href="qh-optf.htm#FP">FP</a></dt> + <dd>print coincident input sites with distance to + nearest site (i.e., vertex). The first line is the + number of coincident sites. Each remaining line starts with the point ID of + an input site, followed by the point ID of a coincident point, its vertex, and distance. + Includes deleted vertices which + indicate highly degenerate input (see'<A href="qh-optf.htm#Fs">Fs</a>'). + Do not use '<a href="qh-optq.htm#QJn">QJ</a>' with 'FP'; the joggle will separate + coincident sites.</dd> + <dt> </dt> + <dt> </dt> + <dd><b>General</b></dd> + <dt><a href="qh-opto.htm#s">s</a></dt> + <dd>print summary of the Voronoi diagram. Use '<a + href="qh-optf.htm#Fs">Fs</a>' for numeric data.</dd> + <dt><a href="qh-opto.htm#i">i</a></dt> + <dd>list input sites for each <a href=qdelaun.htm>Delaunay region</a>. Use option '<a href="qh-optp.htm#Pp">Pp</a>' + to avoid the warning. The first line is the number of regions. The + remaining lines list the input sites for each region. The regions are + oriented. In the circle-in-square example, the cocircular region has four + edges. In 3-d and higher, report cospherical sites by adding extra points. + </dd> + <dt><a href="qh-optg.htm#G">G</a></dt> + <dd>Geomview output for 2-d Voronoi diagrams.</dd> + </dl> +</blockquote> +</blockquote> +<h3><a href="#TOP">»</a> <a name="controls">qvoronoi +controls</a></h3> +<blockquote> + +<p>These options provide additional control:</p> +<blockquote> + +<dl compact> + <dt><a href="qh-optq.htm#Qt">Qt</a></dt> + <dd>triangulated output. If a Voronoi vertex is defined by cospherical data, Qhull + duplicates the vertex. For example, if the data contains a square, the output + will contain two copies of the Voronoi vertex.</dd> + <dt><a href="qh-optq.htm#QJn">QJ</a></dt> + <dd>joggle the input to avoid Voronoi vertices defined by more + than <i>dim+1</i> points. It is less accurate than triangulated output ('Qt'). + </dd> + <dt><a href="qh-optq.htm#Qu">Qu</a></dt> + <dd>compute the <a href="qvoron_f.htm">furthest-site Voronoi diagram</a>.</dd> + <dt><a href="qh-optq.htm#QVn">QVn</a></dt> + <dd>select Voronoi vertices for input site <em>n</em> </dd> + <dt><a href="qh-optq.htm#Qz">Qz</a></dt> + <dd>add a point above the paraboloid to reduce precision + errors. Use it for nearly cocircular/cospherical input + (e.g., 'rbox c | qvoronoi Qz').</dd> + <dt><a href="qh-optt.htm#Tv">Tv</a></dt> + <dd>verify result</dd> + <dt><a href="qh-optt.htm#TO">TI file</a></dt> + <dd>input data from file. The filename may not use spaces or quotes.</dd> + <dt><a href="qh-optt.htm#TO">TO file</a></dt> + <dd>output results to file. Use single quotes if the filename + contains spaces (e.g., <tt>TO 'file with spaces.txt'</tt></dd> + <dt><a href="qh-optt.htm#TFn">TFn</a></dt> + <dd>report progress after constructing <em>n</em> facets</dd> + <dt><a href="qh-optp.htm#PDk">PDk:1</a></dt> + <dd>include upper and lower facets in the output. Set <em>k</em> + to the last dimension (e.g., 'PD2:1' for 2-d inputs). </dd> + <dt><a href="qh-opto.htm#f">f </a></dt> + <dd>facet dump. Print the data structure for each facet (i.e., + Voronoi vertex).</dd> +</dl> + +</blockquote> +</blockquote> +<h3><a href="#TOP">»</a> <a name="graphics">qvoronoi +graphics</a></h3> +<blockquote> + +<p>In 2-d, Geomview output ('<a href="qh-optg.htm#G">G</a>') +displays a Voronoi diagram with extra edges to close the +unbounded Voronoi regions. To view the unbounded rays, enclose +the input points in a square.</p> + +<p>You can also view <i>individual</i> Voronoi regions in 3-d. To +view the Voronoi region for site 3 in Geomview, execute</p> + +<blockquote> + <p>qvoronoi <data '<a href="qh-optq.htm#QVn">QV3</a>' '<a + href="qh-opto.htm#p">p</a>' | qconvex s G >output</p> +</blockquote> + +<p>The <tt>qvoronoi</tt> command returns the Voronoi vertices +for input site 3. The <tt>qconvex</tt> command computes their convex hull. +This is the Voronoi region for input site 3. Its +hyperplane normals (qconvex 'n') are the same as the separating hyperplanes +from options '<a href="qh-optf.htm#Fi">Fi</a>' +and '<a href="qh-optf.htm#Fo">Fo</a>' (up to roundoff error). + +<p>See the <a href="qh-eg.htm#delaunay">Delaunay and Voronoi +examples</a> for 2-d and 3-d examples. Turn off normalization (on +Geomview's 'obscure' menu) when comparing the Voronoi diagram +with the corresponding Delaunay triangulation. </p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="notes">qvoronoi +notes</a></h3> +<blockquote> + +<p>You can simplify the Voronoi diagram by enclosing the input +sites in a large square or cube. This is particularly recommended +for cocircular or cospherical input data.</p> + +<p>See <a href="#graphics">Voronoi graphics</a> for computing +the convex hull of a Voronoi region. </p> + +<p>Voronoi diagrams do not include facets that are +coplanar with the convex hull of the input sites. A facet is +coplanar if the last coefficient of its normal is +nearly zero (see <a href="../src/user.h#ZEROdelaunay">qh_ZEROdelaunay</a>). + +<p>Unbounded regions can be confusing. For example, '<tt>rbox c | +qvoronoi Qz o</tt>' produces the Voronoi regions for the vertices +of a cube centered at the origin. All regions are unbounded. The +output is </p> + +<blockquote> + <pre>3 +2 9 1 +-10.101 -10.101 -10.101 + 0 0 0 +2 0 1 +2 0 1 +2 0 1 +2 0 1 +2 0 1 +2 0 1 +2 0 1 +2 0 1 +0 +</pre> +</blockquote> + +<p>The first line is the dimension. The second line is the number +of vertices and the number of regions. There is one region per +input point plus a region for the point-at-infinity added by +option '<a href="qh-optq.htm#Qz">Qz</a>'. The next two lines +lists the Voronoi vertices. The first vertex is the infinity +vertex. It is indicate by the coordinates <em>-10.101</em>. The +second vertex is the origin. The next nine lines list the +regions. Each region lists two vertices -- the infinity vertex +and the origin. The last line is "0" because no region +is associated with the point-at-infinity. A "0" would +also be listed for nearly incident input sites. </p> + +<p>To use option '<a href="qh-optf.htm#Fv">Fv</a>', add an +interior point. For example, </p> + +<blockquote> + <pre> +rbox c P0 | qvoronoi Fv +20 +5 0 7 1 3 5 +5 0 3 1 4 5 +5 0 5 1 2 3 +5 0 1 1 2 4 +5 0 6 2 3 6 +5 0 2 2 4 6 +5 0 4 4 5 6 +5 0 8 5 3 6 +5 1 2 0 2 4 +5 1 3 0 1 4 +5 1 5 0 1 2 +5 2 4 0 4 6 +5 2 6 0 2 6 +5 3 4 0 4 5 +5 3 7 0 1 5 +5 4 8 0 6 5 +5 5 6 0 2 3 +5 5 7 0 1 3 +5 6 8 0 6 3 +5 7 8 0 3 5 +</pre> +</blockquote> + +<p>The output consists of 20 ridges and each ridge lists a pair +of input sites and a triplet of Voronoi vertices. The first eight +ridges connect the origin ('P0'). The remainder list the edges of +the cube. Each edge generates an unbounded ray through the +midpoint. The corresponding separating planes ('Fo') follow each +pair of coordinate axes. </p> + +</blockquote> +<h3><a href="#TOP">»</a><a name="conventions">qvoronoi conventions</a></h3> +<blockquote> + +<p>The following terminology is used for Voronoi diagrams in +Qhull. The underlying structure is a Delaunay triangulation from +a convex hull in one higher dimension. Facets of the Delaunay +triangulation correspond to vertices of the Voronoi diagram. +Vertices of the Delaunay triangulation correspond to input sites. +They also correspond to regions of the Voronoi diagram. See <a +href="qconvex.htm#conventions">convex hull conventions</a>, <a +href="qdelaun.htm#conventions">Delaunay conventions</a>, and +<a href="index.htm#structure">Qhull's data structures</a>.</p> +<blockquote> + +<ul> + <li><em>input site</em> - a point in the input (one dimension + lower than a point on the convex hull)</li> + <li><em>point</em> - a point has <i>d+1</i> coordinates. The + last coordinate is the sum of the squares of the input + site's coordinates</li> + <li><em>coplanar point</em> - a <em>nearly incident</em> + input site</li> + <li><em>vertex</em> - a point on the paraboloid. It + corresponds to a unique input site. </li> + <li><em>point-at-infinity</em> - a point added above the + paraboloid by option '<a href="qh-optq.htm#Qz">Qz</a>'</li> + <li><em>Delaunay facet</em> - a lower facet of the + paraboloid. The last coefficient of its normal is + clearly negative.</li> + <li><em>Voronoi vertex</em> - the circumcenter of a Delaunay + facet</li> + <li><em>Voronoi region</em> - the Voronoi vertices for an + input site. The region of Euclidean space nearest to an + input site.</li> + <li><em>Voronoi diagram</em> - the graph of the Voronoi + regions. It includes the ridges (i.e., edges) between the + regions.</li> + <li><em>vertex-at-infinity</em> - the Voronoi vertex that + indicates unbounded Voronoi regions in '<a + href="qh-opto.htm#o">o</a>' output format. Its + coordinates are <em>-10.101</em>.</li> + <li><em>good facet</em> - a Voronoi vertex with optional + restrictions by '<a href="qh-optq.htm#QVn">QVn</a>', etc.</li> +</ul> + +</blockquote> +</blockquote> +<h3><a href="#TOP">»</a><a name="options">qvoronoi options</a></h3> + +<pre> +qvoronoi- compute the Voronoi diagram + http://www.qhull.org + +input (stdin): + first lines: dimension and number of points (or vice-versa). + other lines: point coordinates, best if one point per line + comments: start with a non-numeric character + +options: + Qu - compute furthest-site Voronoi diagram + Qt - triangulated output + QJ - joggle input instead of merging facets + +Qhull control options: + QJn - randomly joggle input in range [-n,n] + Qs - search all points for the initial simplex + Qz - add point-at-infinity to Voronoi diagram + QGn - Voronoi vertices if visible from point n, -n if not + QVn - Voronoi vertices for input point n, -n if not + +Trace options: + T4 - trace at level n, 4=all, 5=mem/gauss, -1= events + Tc - check frequently during execution + Ts - statistics + Tv - verify result: structure, convexity, and in-circle test + Tz - send all output to stdout + TFn - report summary when n or more facets created + TI file - input data from file, no spaces or single quotes + TO file - output results to file, may be enclosed in single quotes + TPn - turn on tracing when point n added to hull + TMn - turn on tracing at merge n + TWn - trace merge facets when width > n + TVn - stop qhull after adding point n, -n for before (see TCn) + TCn - stop qhull after building cone for point n (see TVn) + +Precision options: + Cn - radius of centrum (roundoff added). Merge facets if non-convex + An - cosine of maximum angle. Merge facets if cosine > n or non-convex + C-0 roundoff, A-0.99/C-0.01 pre-merge, A0.99/C0.01 post-merge + Rn - randomly perturb computations by a factor of [1-n,1+n] + Wn - min facet width for non-coincident point (before roundoff) + +Output formats (may be combined; if none, produces a summary to stdout): + s - summary to stderr + p - Voronoi vertices + o - OFF format (dim, Voronoi vertices, and Voronoi regions) + i - Delaunay regions (use 'Pp' to avoid warning) + f - facet dump + +More formats: + Fc - count plus coincident points (by Voronoi vertex) + Fd - use cdd format for input (homogeneous with offset first) + FD - use cdd format for output (offset first) + FF - facet dump without ridges + Fi - separating hyperplanes for bounded Voronoi regions + FI - ID for each Voronoi vertex + Fm - merge count for each Voronoi vertex (511 max) + Fn - count plus neighboring Voronoi vertices for each Voronoi vertex + FN - count and Voronoi vertices for each Voronoi region + Fo - separating hyperplanes for unbounded Voronoi regions + FO - options and precision constants + FP - nearest point and distance for each coincident point + FQ - command used for qvoronoi + Fs - summary: #int (8), dimension, #points, tot vertices, tot facets, + for output: #Voronoi regions, #Voronoi vertices, + #coincident points, #non-simplicial regions + #real (2), max outer plane and min vertex + Fv - Voronoi diagram as Voronoi vertices between adjacent input sites + Fx - extreme points of Delaunay triangulation (on convex hull) + +Geomview options (2-d only) + Ga - all points as dots + Gp - coplanar points and vertices as radii + Gv - vertices as spheres + Gi - inner planes only + Gn - no planes + Go - outer planes only + Gc - centrums + Gh - hyperplane intersections + Gr - ridges + GDn - drop dimension n in 3-d and 4-d output + +Print options: + PAn - keep n largest Voronoi vertices by 'area' + Pdk:n - drop facet if normal[k] <= n (default 0.0) + PDk:n - drop facet if normal[k] >= n + Pg - print good Voronoi vertices (needs 'QGn' or 'QVn') + PFn - keep Voronoi vertices whose 'area' is at least n + PG - print neighbors of good Voronoi vertices + PMn - keep n Voronoi vertices with most merges + Po - force output. If error, output neighborhood of facet + Pp - do not report precision problems + + . - list of all options + - - one line descriptions of all options +</pre> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis +• <a href="#input">in</a>put • <a href="#outputs">ou</a>tputs +• <a href="#controls">co</a>ntrols • <a href="#graphics">gr</a>aphics +• <a href="#notes">no</a>tes • <a href="#conventions">co</a>nventions +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.htm new file mode 100644 index 0000000000000000000000000000000000000000..f039ead1d139ae4a889c733d5fe523b24810360e --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.htm @@ -0,0 +1,272 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>rbox -- generate point distributions</title> +</head> + +<body> +<!-- Navigation links --> +<p><b><a name="TOP">Up:</a></b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis • <a href="#outputs">ou</a>tputs +• <a href="#examples">ex</a>amples • <a href="#notes">no</a>tes +• <a href="#options">op</a>tions +<hr> +<!-- Main text of document --> +<h1><a +href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/cone.html"><img +src="qh--cone.gif" alt="[CONE]" align="middle" width="100" +height="100"></a>rbox -- generate point distributions</h1> + +<blockquote> + rbox generates random or regular points according to the + options given, and outputs the points to stdout. The + points are generated in a cube, unless 's', 'x', or 'y' + are given. + +</blockquote> +<h3><a href="#TOP">»</a><a name="synopsis">rbox synopsis</a></h3> +<pre> +rbox- generate various point distributions. Default is random in cube. + +args (any order, space separated): + 3000 number of random points in cube, lens, spiral, sphere or grid + D3 dimension 3-d + c add a unit cube to the output ('c G2.0' sets size) + d add a unit diamond to the output ('d G2.0' sets size) + l generate a regular 3-d spiral + r generate a regular polygon, ('r s Z1 G0.1' makes a cone) + s generate cospherical points + x generate random points in simplex, may use 'r' or 'Wn' + y same as 'x', plus simplex + Pn,m,r add point [n,m,r] first, pads with 0 + + Ln lens distribution of radius n. Also 's', 'r', 'G', 'W'. + Mn,m,r lattice (Mesh) rotated by [n,-m,0], [m,n,0], [0,0,r], ... + '27 M1,0,1' is {0,1,2} x {0,1,2} x {0,1,2}. Try 'M3,4 z'. + W0.1 random distribution within 0.1 of the cube's or sphere's surface + Z0.5 s random points in a 0.5 disk projected to a sphere + Z0.5 s G0.6 same as Z0.5 within a 0.6 gap + + Bn bounding box coordinates, default 0.5 + h output as homogeneous coordinates for cdd + n remove command line from the first line of output + On offset coordinates by n + t use time as the random number seed (default is command line) + tn use n as the random number seed + z print integer coordinates, default 'Bn' is 1e+06 +</pre> + +<h3><a href="#TOP">»</a><a name="outputs">rbox outputs</a></h3> +<blockquote> + +The format of the output is the following: first line contains + the dimension and a comment, second line contains the + number of points, and the following lines contain the points, + one point per line. Points are represented by their coordinate values. + +<p>For example, <tt>rbox c 10 D2</tt> generates +<blockquote> +<pre> +2 RBOX c 10 D2 +14 +-0.4999921736307369 -0.3684622117955817 +0.2556053225468894 -0.0413498678629751 +0.0327672376602583 -0.2810408135699488 +-0.452955383763607 0.17886471718444 +0.1792964061529342 0.4346928963760779 +-0.1164979223315585 0.01941637230982666 +0.3309653464993139 -0.4654278894564396 +-0.4465383649305798 0.02970019358182344 +0.1711493843897706 -0.4923018137852678 +-0.1165843490665633 -0.433157762450313 + -0.5 -0.5 + -0.5 0.5 + 0.5 -0.5 + 0.5 0.5 +</pre> + +</blockquote> + +</blockquote> +<h3><a href="#TOP">»</a><a name="examples">rbox examples</a></h3> + +<pre> + rbox 10 + 10 random points in the unit cube centered at the + origin. + + rbox 10 s D2 + 10 random points on a 2-d circle. + + rbox 100 W0 + 100 random points on the surface of a cube. + + rbox 1000 s D4 + 1000 random points on a 4-d sphere. + + rbox c D5 O0.5 + a 5-d hypercube with one corner at the origin. + + rbox d D10 + a 10-d diamond. + + rbox x 1000 r W0 + 100 random points on the surface of a fixed simplex + + rbox y D12 + a 12-d simplex. + + rbox l 10 + 10 random points along a spiral + + rbox l 10 r + 10 regular points along a spiral plus two end + points + + rbox 1000 L10000 D4 s + 1000 random points on the surface of a narrow lens. + + rbox 1000 L100000 s G1e-6 + 1000 random points near the edge of a narrow lens + + rbox c G2 d G3 + a cube with coordinates +2/-2 and a diamond with + coordinates +3/-3. + + rbox 64 M3,4 z + a rotated, {0,1,2,3} x {0,1,2,3} x {0,1,2,3} lat- + tice (Mesh) of integer points. + + rbox P0 P0 P0 P0 P0 + 5 copies of the origin in 3-d. Try 'rbox P0 P0 P0 + P0 P0 | qhull QJ'. + + r 100 s Z1 G0.1 + two cospherical 100-gons plus another cospherical + point. + + 100 s Z1 + a cone of points. + + 100 s Z1e-7 + a narrow cone of points with many precision errors. +</pre> + +<h3><a href="#TOP">»</a><a name="notes">rbox notes</a></h3> +<blockquote> +Some combinations of arguments generate odd results. + +</blockquote> +<h3><a href="#TOP">»</a><a name="options">rbox options</a></h3> + +<pre> + n number of points + + Dn dimension n-d (default 3-d) + + Bn bounding box coordinates (default 0.5) + + l spiral distribution, available only in 3-d + + Ln lens distribution of radius n. May be used with + 's', 'r', 'G', and 'W'. + + Mn,m,r lattice (Mesh) rotated by {[n,-m,0], [m,n,0], + [0,0,r], ...}. Use 'Mm,n' for a rigid rotation + with r = sqrt(n^2+m^2). 'M1,0' is an orthogonal + lattice. For example, '27 M1,0' is {0,1,2} x + {0,1,2} x {0,1,2}. + + s cospherical points randomly generated in a cube and + projected to the unit sphere + + x simplicial distribution. It is fixed for option + 'r'. May be used with 'W'. + + y simplicial distribution plus a simplex. Both 'x' + and 'y' generate the same points. + + Wn restrict points to distance n of the surface of a + sphere or a cube + + c add a unit cube to the output + + c Gm add a cube with all combinations of +m and -m to + the output + + d add a unit diamond to the output. + + d Gm add a diamond made of 0, +m and -m to the output + + Pn,m,r add point [n,m,r] to the output first. Pad coordi- + nates with 0.0. + + n Remove the command line from the first line of out- + put. + + On offset the data by adding n to each coordinate. + + t use time in seconds as the random number seed + (default is command line). + + tn set the random number seed to n. + + z generate integer coordinates. Use 'Bn' to change + the range. The default is 'B1e6' for six-digit + coordinates. In R^4, seven-digit coordinates will + overflow hyperplane normalization. + + Zn s restrict points to a disk about the z+ axis and the + sphere (default Z1.0). Includes the opposite pole. + 'Z1e-6' generates degenerate points under single + precision. + + Zn Gm s + same as Zn with an empty center (default G0.5). + + r s D2 generate a regular polygon + + r s Z1 G0.1 + generate a regular cone +</pre> + +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual</a>: Table of Contents<br> +<b>To:</b> <a href="qh-quick.htm#programs">Programs</a> +• <a href="qh-quick.htm#options">Options</a> +• <a href="qh-opto.htm#output">Output</a> +• <a href="qh-optf.htm#format">Formats</a> +• <a href="qh-optg.htm#geomview">Geomview</a> +• <a href="qh-optp.htm#print">Print</a> +• <a href="qh-optq.htm#qhull">Qhull</a> +• <a href="qh-optc.htm#prec">Precision</a> +• <a href="qh-optt.htm#trace">Trace</a><br> +<b>To:</b> <a href="#synopsis">sy</a>nopsis • <a href="#outputs">ou</a>tputs +• <a href="#examples">ex</a>amples • <a href="#notes">no</a>tes +• <a href="#options">op</a>tions +<!-- GC common information --> +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="qh--geom.gif" +align="middle" width="40" height="40"></a><i>The Geometry Center +Home Page </i></p> + +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +<br> +Created: Sept. 25, 1995 --- <!-- hhmts start --> Last modified: August 12, 1998 <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.man b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.man new file mode 100644 index 0000000000000000000000000000000000000000..84ef6e306823f040f35490304d9154faef9c2b71 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.man @@ -0,0 +1,176 @@ +./" This is the Unix manual page for rbox, written in nroff, the standard +./" manual formatter for Unix systems. To format it, type +./" +./" nroff -man rbox.man +./" +./" This will print a formatted copy to standard output. If you want +./" to ensure that the output is plain ascii, free of any control +./" characters that nroff uses for underlining etc, pipe the output +./" through "col -b": +./" +./" nroff -man rbox.man | col -b +./" +.TH rbox 1 "August 10, 1998" "Geometry Center" +.SH NAME +rbox \- generate point distributions for qhull +.SH SYNOPSIS +Command "rbox" (w/o arguments) lists the options. +.SH DESCRIPTION +.PP +rbox generates random or regular points according to the options given, and +outputs +the points to stdout. The points are generated in a cube, unless 's' or +'k' option is +given. The format of the output is the following: first line +contains the dimension and a comment, +second line contains the number of points, and the +following lines contain the points, one point per line. Points are represented +by their coordinate values. +.SH EXAMPLES +.TP +rbox 10 +10 random points in the unit cube centered at the origin. +.TP +rbox 10 s D2 +10 random points on a 2-d circle. +.TP +rbox 100 W0 +100 random points on the surface of a cube. +.TP +rbox 1000 s D4 +1000 random points on a 4-d sphere. +.TP +rbox c D5 O0.5 +a 5-d hypercube with one corner at the origin. +.TP +rbox d D10 +a 10-d diamond. +.TP +rbox x 1000 r W0 +100 random points on the surface of a fixed simplex +.TP +rbox y D12 +a 12-d simplex. +.TP +rbox l 10 +10 random points along a spiral +.TP +rbox l 10 r +10 regular points along a spiral plus two end points +.TP +rbox 1000 L10000 D4 s +1000 random points on the surface of a narrow lens. +.TP +rbox c G2 d G3 +a cube with coordinates +2/-2 and a diamond with coordinates +3/-3. +.TP +rbox 64 M3,4 z +a rotated, {0,1,2,3} x {0,1,2,3} x {0,1,2,3} lattice (Mesh) of integer points. +'rbox 64 M1,0' is orthogonal. +.TP +rbox P0 P0 P0 P0 P0 +5 copies of the origin in 3-d. Try 'rbox P0 P0 P0 P0 P0 | qhull QJ'. +.TP +r 100 s Z1 G0.1 +two cospherical 100-gons plus another cospherical point. +.TP +100 s Z1 +a cone of points. +.TP +100 s Z1e-7 +a narrow cone of points with many precision errors. +.SH OPTIONS +.TP +n +number of points +.TP +Dn +dimension n-d (default 3-d) +.TP +Bn +bounding box coordinates (default 0.5) +.TP +l +spiral distribution, available only in 3-d +.TP +Ln +lens distribution of radius n. May be used with 's', 'r', 'G', and 'W'. +.TP +Mn,m,r +lattice (Mesh) rotated by {[n,-m,0], [m,n,0], [0,0,r], ...}. +Use 'Mm,n' for a rigid rotation with r = sqrt(n^2+m^2). 'M1,0' is an +orthogonal lattice. For example, '27 M1,0' is {0,1,2} x {0,1,2} x {0,1,2}. +'27 M3,4 z' is a rotated integer lattice. +.TP +s +cospherical points randomly generated in a cube and projected to the unit sphere +.TP +x +simplicial distribution. It is fixed for option 'r'. May be used with 'W'. +.TP +y +simplicial distribution plus a simplex. Both 'x' and 'y' generate the same points. +.TP +Wn +restrict points to distance n of the surface of a sphere or a cube +.TP +c +add a unit cube to the output +.TP +c Gm +add a cube with all combinations of +m and -m to the output +.TP +d +add a unit diamond to the output. +.TP +d Gm +add a diamond made of 0, +m and -m to the output +.TP +Pn,m,r +add point [n,m,r] to the output first. Pad coordinates with 0.0. +.TP +n +Remove the command line from the first line of output. +.TP +On +offset the data by adding n to each coordinate. +.TP +t +use time in seconds as the random number seed (default is command line). +.TP +tn +set the random number seed to n. +.TP +z +generate integer coordinates. Use 'Bn' to change the range. +The default is 'B1e6' for six-digit coordinates. In R^4, seven-digit +coordinates will overflow hyperplane normalization. +.TP +Zn s +restrict points to a disk about the z+ axis and the sphere (default Z1.0). +Includes the opposite pole. 'Z1e-6' generates degenerate points under +single precision. +.TP +Zn Gm s +same as Zn with an empty center (default G0.5). +.TP +r s D2 +generate a regular polygon +.TP +r s Z1 G0.1 +generate a regular cone +.SH BUGS +Some combinations of arguments generate odd results. + +Report bugs to qhull_bug@qhull.org, other correspondence to qhull@qhull.org +.SH SEE ALSO +qhull(1) +.SH AUTHOR +.nf +C. Bradford Barber +c/o The Geometry Center +400 Lind Hall +207 Church Street S.E. +Minneapolis, MN 55455 +.fi + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.txt new file mode 100644 index 0000000000000000000000000000000000000000..bc459525fe97fb3ea9ac062e1eba7f7279f0e733 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/html/rbox.txt @@ -0,0 +1,198 @@ + + + +rbox(1) rbox(1) + + +NAME + rbox - generate point distributions for qhull + +SYNOPSIS + Command "rbox" (w/o arguments) lists the options. + +DESCRIPTION + rbox generates random or regular points according to the + options given, and outputs the points to stdout. The + points are generated in a cube, unless 's' or given. The + format of the output is the following: first line contains + the dimension and a comment, second line contains the num- + ber of points, and the following lines contain the points, + one point per line. Points are represented by their coor- + dinate values. + +EXAMPLES + rbox 10 + 10 random points in the unit cube centered at the + origin. + + rbox 10 s D2 + 10 random points on a 2-d circle. + + rbox 100 W0 + 100 random points on the surface of a cube. + + rbox 1000 s D4 + 1000 random points on a 4-d sphere. + + rbox c D5 O0.5 + a 5-d hypercube with one corner at the origin. + + rbox d D10 + a 10-d diamond. + + rbox x 1000 r W0 + 100 random points on the surface of a fixed simplex + + rbox y D12 + a 12-d simplex. + + rbox l 10 + 10 random points along a spiral + + rbox l 10 r + 10 regular points along a spiral plus two end + points + + rbox 1000 L10000 D4 s + 1000 random points on the surface of a narrow lens. + + rbox c G2 d G3 + a cube with coordinates +2/-2 and a diamond with + + + +Geometry Center August 10, 1998 1 + + + + + +rbox(1) rbox(1) + + + coordinates +3/-3. + + rbox 64 M3,4 z + a rotated, {0,1,2,3} x {0,1,2,3} x {0,1,2,3} lat- + tice (Mesh) of integer points. + + rbox P0 P0 P0 P0 P0 + 5 copies of the origin in 3-d. Try 'rbox P0 P0 P0 + P0 P0 | qhull QJ'. + + r 100 s Z1 G0.1 + two cospherical 100-gons plus another cospherical + point. + + 100 s Z1 + a cone of points. + + 100 s Z1e-7 + a narrow cone of points with many precision errors. + +OPTIONS + n number of points + + Dn dimension n-d (default 3-d) + + Bn bounding box coordinates (default 0.5) + + l spiral distribution, available only in 3-d + + Ln lens distribution of radius n. May be used with + 's', 'r', 'G', and 'W'. + + Mn,m,r lattice (Mesh) rotated by {[n,-m,0], [m,n,0], + [0,0,r], ...}. Use 'Mm,n' for a rigid rotation + with r = sqrt(n^2+m^2). 'M1,0' is an orthogonal + lattice. For example, '27 M1,0' is {0,1,2} x + {0,1,2} x {0,1,2}. + + s cospherical points randomly generated in a cube and + projected to the unit sphere + + x simplicial distribution. It is fixed for option + 'r'. May be used with 'W'. + + y simplicial distribution plus a simplex. Both 'x' + and 'y' generate the same points. + + Wn restrict points to distance n of the surface of a + sphere or a cube + + c add a unit cube to the output + + c Gm add a cube with all combinations of +m and -m to + the output + + + +Geometry Center August 10, 1998 2 + + + + + +rbox(1) rbox(1) + + + d add a unit diamond to the output. + + d Gm add a diamond made of 0, +m and -m to the output + + Pn,m,r add point [n,m,r] to the output first. Pad coordi- + nates with 0.0. + + n Remove the command line from the first line of out- + put. + + On offset the data by adding n to each coordinate. + + t use time in seconds as the random number seed + (default is command line). + + tn set the random number seed to n. + + z generate integer coordinates. Use 'Bn' to change + the range. The default is 'B1e6' for six-digit + coordinates. In R^4, seven-digit coordinates will + overflow hyperplane normalization. + + Zn s restrict points to a disk about the z+ axis and the + sphere (default Z1.0). Includes the opposite pole. + 'Z1e-6' generates degenerate points under single + precision. + + Zn Gm s + same as Zn with an empty center (default G0.5). + + r s D2 generate a regular polygon + + r s Z1 G0.1 + generate a regular cone + +BUGS + Some combinations of arguments generate odd results. + + Report bugs to qhull_bug@qhull.org, other correspon- + dence to qhull@qhull.org + +SEE ALSO + qhull(1) + +AUTHOR + C. Bradford Barber + c/o The Geometry Center + 400 Lind Hall + 207 Church Street S.E. + Minneapolis, MN 55455 + + + + + + + +Geometry Center August 10, 1998 3 + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/geom.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/geom.h new file mode 100644 index 0000000000000000000000000000000000000000..6b8ee627884e9ef5e0d23f1fe6fc7bb3c1246113 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/geom.h @@ -0,0 +1,177 @@ +/*<html><pre> -<a href="qh-geom.htm" + >-------------------------------</a><a name="TOP">-</a> + + geom.h + header file for geometric routines + + see qh-geom.htm and geom.c + + copyright (c) 1993-2003 The Geometry Center +*/ + +#ifndef qhDEFgeom +#define qhDEFgeom 1 + +/* ============ -macros- ======================== */ + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="fabs_">-</a> + + fabs_(a) + returns the absolute value of a +*/ +#define fabs_( a ) ((( a ) < 0 ) ? -( a ):( a )) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="fmax_">-</a> + + fmax_(a,b) + returns the maximum value of a and b +*/ +#define fmax_( a,b ) ( ( a ) < ( b ) ? ( b ) : ( a ) ) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="fmin_">-</a> + + fmin_(a,b) + returns the minimum value of a and b +*/ +#define fmin_( a,b ) ( ( a ) > ( b ) ? ( b ) : ( a ) ) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="maximize_">-</a> + + maximize_(maxval, val) + set maxval to val if val is greater than maxval +*/ +#define maximize_( maxval, val ) {if (( maxval ) < ( val )) ( maxval )= ( val );} + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="minimize_">-</a> + + minimize_(minval, val) + set minval to val if val is less than minval +*/ +#define minimize_( minval, val ) {if (( minval ) > ( val )) ( minval )= ( val );} + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="det2_">-</a> + + det2_(a1, a2, + b1, b2) + + compute a 2-d determinate +*/ +#define det2_( a1,a2,b1,b2 ) (( a1 )*( b2 ) - ( a2 )*( b1 )) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="det3_">-</a> + + det3_(a1, a2, a3, + b1, b2, b3, + c1, c2, c3) + + compute a 3-d determinate +*/ +#define det3_( a1,a2,a3,b1,b2,b3,c1,c2,c3 ) ( ( a1 )*det2_( b2,b3,c2,c3 ) \ + - ( b1 )*det2_( a2,a3,c2,c3 ) + ( c1 )*det2_( a2,a3,b2,b3 ) ) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="dX">-</a> + + dX( p1, p2 ) + dY( p1, p2 ) + dZ( p1, p2 ) + + given two indices into rows[], + + compute the difference between X, Y, or Z coordinates +*/ +#define dX( p1,p2 ) ( *( rows[p1] ) - *( rows[p2] )) +#define dY( p1,p2 ) ( *( rows[p1]+1 ) - *( rows[p2]+1 )) +#define dZ( p1,p2 ) ( *( rows[p1]+2 ) - *( rows[p2]+2 )) +#define dW( p1,p2 ) ( *( rows[p1]+3 ) - *( rows[p2]+3 )) + +/*============= prototypes in alphabetical order, infrequent at end ======= */ + +void qh_backnormal (realT **rows, int numrow, int numcol, boolT sign, coordT *normal, boolT *nearzero); +void qh_distplane (pointT *point, facetT *facet, realT *dist); +facetT *qh_findbest (pointT *point, facetT *startfacet, + boolT bestoutside, boolT isnewfacets, boolT noupper, + realT *dist, boolT *isoutside, int *numpart); +facetT *qh_findbesthorizon (boolT ischeckmax, pointT *point, + facetT *startfacet, boolT noupper, realT *bestdist, int *numpart); +facetT *qh_findbestnew (pointT *point, facetT *startfacet, realT *dist, + boolT bestoutside, boolT *isoutside, int *numpart); +void qh_gausselim(realT **rows, int numrow, int numcol, boolT *sign, boolT *nearzero); +realT qh_getangle(pointT *vect1, pointT *vect2); +pointT *qh_getcenter(setT *vertices); +pointT *qh_getcentrum(facetT *facet); +realT qh_getdistance(facetT *facet, facetT *neighbor, realT *mindist, realT *maxdist); +void qh_normalize (coordT *normal, int dim, boolT toporient); +void qh_normalize2 (coordT *normal, int dim, boolT toporient, + realT *minnorm, boolT *ismin); +pointT *qh_projectpoint(pointT *point, facetT *facet, realT dist); + +void qh_setfacetplane(facetT *newfacets); +void qh_sethyperplane_det (int dim, coordT **rows, coordT *point0, + boolT toporient, coordT *normal, realT *offset, boolT *nearzero); +void qh_sethyperplane_gauss (int dim, coordT **rows, pointT *point0, + boolT toporient, coordT *normal, coordT *offset, boolT *nearzero); +boolT qh_sharpnewfacets (void); + +/*========= infrequently used code in geom2.c =============*/ + + +coordT *qh_copypoints (coordT *points, int numpoints, int dimension); +void qh_crossproduct (int dim, realT vecA[3], realT vecB[3], realT vecC[3]); +realT qh_determinant (realT **rows, int dim, boolT *nearzero); +realT qh_detjoggle (pointT *points, int numpoints, int dimension); +void qh_detroundoff (void); +realT qh_detsimplex(pointT *apex, setT *points, int dim, boolT *nearzero); +realT qh_distnorm (int dim, pointT *point, pointT *normal, realT *offsetp); +realT qh_distround (int dimension, realT maxabs, realT maxsumabs); +realT qh_divzero(realT numer, realT denom, realT mindenom1, boolT *zerodiv); +realT qh_facetarea (facetT *facet); +realT qh_facetarea_simplex (int dim, coordT *apex, setT *vertices, + vertexT *notvertex, boolT toporient, coordT *normal, realT *offset); +pointT *qh_facetcenter (setT *vertices); +facetT *qh_findgooddist (pointT *point, facetT *facetA, realT *distp, facetT **facetlist); +void qh_getarea (facetT *facetlist); +boolT qh_gram_schmidt(int dim, realT **rows); +boolT qh_inthresholds (coordT *normal, realT *angle); +void qh_joggleinput (void); +realT *qh_maxabsval (realT *normal, int dim); +setT *qh_maxmin(pointT *points, int numpoints, int dimension); +realT qh_maxouter (void); +void qh_maxsimplex (int dim, setT *maxpoints, pointT *points, int numpoints, setT **simplex); +realT qh_minabsval (realT *normal, int dim); +int qh_mindiff (realT *vecA, realT *vecB, int dim); +boolT qh_orientoutside (facetT *facet); +void qh_outerinner (facetT *facet, realT *outerplane, realT *innerplane); +coordT qh_pointdist(pointT *point1, pointT *point2, int dim); +void qh_printmatrix (FILE *fp, char *string, realT **rows, int numrow, int numcol); +void qh_printpoints (FILE *fp, char *string, setT *points); +void qh_projectinput (void); +void qh_projectpoints (signed char *project, int n, realT *points, + int numpoints, int dim, realT *newpoints, int newdim); +int qh_rand( void); +void qh_srand( int seed); +realT qh_randomfactor (void); +void qh_randommatrix (realT *buffer, int dim, realT **row); +void qh_rotateinput (realT **rows); +void qh_rotatepoints (realT *points, int numpoints, int dim, realT **rows); +void qh_scaleinput (void); +void qh_scalelast (coordT *points, int numpoints, int dim, coordT low, + coordT high, coordT newhigh); +void qh_scalepoints (pointT *points, int numpoints, int dim, + realT *newlows, realT *newhighs); +boolT qh_sethalfspace (int dim, coordT *coords, coordT **nextp, + coordT *normal, coordT *offset, coordT *feasible); +coordT *qh_sethalfspace_all (int dim, int count, coordT *halfspaces, pointT *feasible); +pointT *qh_voronoi_center (int dim, setT *points); + +#endif /* qhDEFgeom */ + + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/io.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/io.h new file mode 100644 index 0000000000000000000000000000000000000000..77bb61d7de764d5c9b097809bd5d3fcecb74b0cb --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/io.h @@ -0,0 +1,154 @@ +/*<html><pre> -<a href="qh-io.htm" + >-------------------------------</a><a name="TOP">-</a> + + io.h + declarations of Input/Output functions + + see README, qhull.h and io.c + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFio +#define qhDEFio 1 + +/*============ constants and flags ==================*/ + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_MAXfirst">-</a> + + qh_MAXfirst + maximum length of first two lines of stdin +*/ +#define qh_MAXfirst 200 + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_MINradius">-</a> + + qh_MINradius + min radius for Gp and Gv, fraction of maxcoord +*/ +#define qh_MINradius 0.02 + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_GEOMepsilon">-</a> + + qh_GEOMepsilon + adjust outer planes for 'lines closer' and geomview roundoff. + This prevents bleed through. +*/ +#define qh_GEOMepsilon 2e-3 + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_WHITESPACE">-</a> + + qh_WHITESPACE + possible values of white space +*/ +#define qh_WHITESPACE " \n\t\v\r\f" + + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="RIDGE">-</a> + + qh_RIDGE + to select which ridges to print in qh_eachvoronoi +*/ +typedef enum +{ + qh_RIDGEall = 0, qh_RIDGEinner, qh_RIDGEouter +} +qh_RIDGE; + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="printvridgeT">-</a> + + printvridgeT + prints results of qh_printvdiagram + + see: + <a href="io.c#printvridge">qh_printvridge</a> for an example +*/ +typedef void (*printvridgeT)(FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded); + +/*============== -prototypes in alphabetical order =========*/ + +void dfacet( unsigned id); +void dvertex( unsigned id); +int qh_compare_facetarea(const void *p1, const void *p2); +int qh_compare_facetmerge(const void *p1, const void *p2); +int qh_compare_facetvisit(const void *p1, const void *p2); +int qh_compare_vertexpoint(const void *p1, const void *p2); /* not used */ + +void qh_countfacets (facetT *facetlist, setT *facets, boolT printall, + int *numfacetsp, int *numsimplicialp, int *totneighborsp, + int *numridgesp, int *numcoplanarsp, int *numnumtricoplanarsp); +pointT *qh_detvnorm (vertexT *vertex, vertexT *vertexA, setT *centers, realT *offsetp); +setT *qh_detvridge (vertexT *vertex); +setT *qh_detvridge3 (vertexT *atvertex, vertexT *vertex); +int qh_eachvoronoi (FILE *fp, printvridgeT printvridge, vertexT *atvertex, boolT visitall, qh_RIDGE innerouter, boolT inorder); +int qh_eachvoronoi_all (FILE *fp, printvridgeT printvridge, boolT isupper, qh_RIDGE innerouter, boolT inorder); +void qh_facet2point(facetT *facet, pointT **point0, pointT **point1, realT *mindist); +setT *qh_facetvertices (facetT *facetlist, setT *facets, boolT allfacets); +void qh_geomplanes (facetT *facet, realT *outerplane, realT *innerplane); +void qh_markkeep (facetT *facetlist); +setT *qh_markvoronoi (facetT *facetlist, setT *facets, boolT printall, boolT *islowerp, int *numcentersp); +void qh_order_vertexneighbors(vertexT *vertex); +void qh_printafacet(FILE *fp, int format, facetT *facet, boolT printall); +void qh_printbegin (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printcenter (FILE *fp, int format, char *string, facetT *facet); +void qh_printcentrum (FILE *fp, facetT *facet, realT radius); +void qh_printend (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printend4geom (FILE *fp, facetT *facet, int *num, boolT printall); +void qh_printextremes (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printextremes_2d (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printextremes_d (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printfacet(FILE *fp, facetT *facet); +void qh_printfacet2math(FILE *fp, facetT *facet, int format, int notfirst); +void qh_printfacet2geom(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet2geom_points(FILE *fp, pointT *point1, pointT *point2, + facetT *facet, realT offset, realT color[3]); +void qh_printfacet3math (FILE *fp, facetT *facet, int format, int notfirst); +void qh_printfacet3geom_nonsimplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet3geom_points(FILE *fp, setT *points, facetT *facet, realT offset, realT color[3]); +void qh_printfacet3geom_simplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet3vertex(FILE *fp, facetT *facet, int format); +void qh_printfacet4geom_nonsimplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet4geom_simplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacetNvertex_nonsimplicial(FILE *fp, facetT *facet, int id, int format); +void qh_printfacetNvertex_simplicial(FILE *fp, facetT *facet, int format); +void qh_printfacetheader(FILE *fp, facetT *facet); +void qh_printfacetridges(FILE *fp, facetT *facet); +void qh_printfacets(FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printhelp_degenerate(FILE *fp); +void qh_printhelp_singular(FILE *fp); +void qh_printhyperplaneintersection(FILE *fp, facetT *facet1, facetT *facet2, + setT *vertices, realT color[3]); +void qh_printneighborhood (FILE *fp, int format, facetT *facetA, facetT *facetB, boolT printall); +void qh_printline3geom (FILE *fp, pointT *pointA, pointT *pointB, realT color[3]); +void qh_printpoint(FILE *fp, char *string, pointT *point); +void qh_printpointid(FILE *fp, char *string, int dim, pointT *point, int id); +void qh_printpoint3 (FILE *fp, pointT *point); +void qh_printpoints_out (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printpointvect (FILE *fp, pointT *point, coordT *normal, pointT *center, realT radius, realT color[3]); +void qh_printpointvect2 (FILE *fp, pointT *point, coordT *normal, pointT *center, realT radius); +void qh_printridge(FILE *fp, ridgeT *ridge); +void qh_printspheres(FILE *fp, setT *vertices, realT radius); +void qh_printvdiagram (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +int qh_printvdiagram2 (FILE *fp, printvridgeT printvridge, setT *vertices, qh_RIDGE innerouter, boolT inorder); +void qh_printvertex(FILE *fp, vertexT *vertex); +void qh_printvertexlist (FILE *fp, char* string, facetT *facetlist, + setT *facets, boolT printall); +void qh_printvertices (FILE *fp, char* string, setT *vertices); +void qh_printvneighbors (FILE *fp, facetT* facetlist, setT *facets, boolT printall); +void qh_printvoronoi (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printvnorm (FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded); +void qh_printvridge (FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded); +void qh_produce_output(void); +void qh_projectdim3 (pointT *source, pointT *destination); +int qh_readfeasible (int dim, char *remainder); +coordT *qh_readpoints(int *numpoints, int *dimension, boolT *ismalloc); +void qh_setfeasible (int dim); +boolT qh_skipfacet(facetT *facet); + +#endif /* qhDEFio */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/mem.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/mem.h new file mode 100644 index 0000000000000000000000000000000000000000..fb141f0c4d717b358c1cecb63345f29f92919c03 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/mem.h @@ -0,0 +1,174 @@ +/*<html><pre> -<a href="qh-mem.htm" + >-------------------------------</a><a name="TOP">-</a> + + mem.h + prototypes for memory management functions + + see qh-mem.htm, mem.c and qset.h + + for error handling, writes message and calls + qh_errexit (qhmem_ERRmem, NULL, NULL) if insufficient memory + and + qh_errexit (qhmem_ERRqhull, NULL, NULL) otherwise + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFmem +#define qhDEFmem + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="NOmem">-</a> + + qh_NOmem + turn off quick-fit memory allocation + + notes: + mem.c implements Quickfit memory allocation for about 20% time + savings. If it fails on your machine, try to locate the + problem, and send the answer to qhull@qhull.org. If this can + not be done, define qh_NOmem to use malloc/free instead. + + #define qh_NOmem +*/ + +/*------------------------------------------- + to avoid bus errors, memory allocation must consider alignment requirements. + malloc() automatically takes care of alignment. Since mem.c manages + its own memory, we need to explicitly specify alignment in + qh_meminitbuffers(). + + A safe choice is sizeof(double). sizeof(float) may be used if doubles + do not occur in data structures and pointers are the same size. Be careful + of machines (e.g., DEC Alpha) with large pointers. If gcc is available, + use __alignof__(double) or fmax_(__alignof__(float), __alignof__(void *)). + + see <a href="user.h#MEMalign">qh_MEMalign</a> in user.h for qhull's alignment +*/ + +#define qhmem_ERRmem 4 /* matches qh_ERRmem in qhull.h */ +#define qhmem_ERRqhull 5 /* matches qh_ERRqhull in qhull.h */ + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="ptr_intT">-</a> + + ptr_intT + for casting a void* to an integer-type + + notes: + On 64-bit machines, a pointer may be larger than an 'int'. + qh_meminit() checks that 'long' holds a 'void*' +*/ +typedef unsigned long ptr_intT; + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="qhmemT">-</a> + + qhmemT + global memory structure for mem.c + + notes: + users should ignore qhmem except for writing extensions + qhmem is allocated in mem.c + + qhmem could be swapable like qh and qhstat, but then + multiple qh's and qhmem's would need to keep in synch. + A swapable qhmem would also waste memory buffers. As long + as memory operations are atomic, there is no problem with + multiple qh structures being active at the same time. + If you need separate address spaces, you can swap the + contents of qhmem. +*/ +typedef struct qhmemT qhmemT; +extern qhmemT qhmem; + +struct qhmemT { /* global memory management variables */ + int BUFsize; /* size of memory allocation buffer */ + int BUFinit; /* initial size of memory allocation buffer */ + int TABLEsize; /* actual number of sizes in free list table */ + int NUMsizes; /* maximum number of sizes in free list table */ + int LASTsize; /* last size in free list table */ + int ALIGNmask; /* worst-case alignment, must be 2^n-1 */ + void **freelists; /* free list table, linked by offset 0 */ + int *sizetable; /* size of each freelist */ + int *indextable; /* size->index table */ + void *curbuffer; /* current buffer, linked by offset 0 */ + void *freemem; /* free memory in curbuffer */ + int freesize; /* size of free memory in bytes */ + void *tempstack; /* stack of temporary memory, managed by users */ + FILE *ferr; /* file for reporting errors */ + int IStracing; /* =5 if tracing memory allocations */ + int cntquick; /* count of quick allocations */ + /* remove statistics doesn't effect speed */ + int cntshort; /* count of short allocations */ + int cntlong; /* count of long allocations */ + int curlong; /* current count of inuse, long allocations */ + int freeshort; /* count of short memfrees */ + int freelong; /* count of long memfrees */ + int totshort; /* total size of short allocations */ + int totlong; /* total size of long allocations */ + int maxlong; /* maximum totlong */ + int cntlarger; /* count of setlarger's */ + int totlarger; /* total copied by setlarger */ +}; + + +/*==================== -macros ====================*/ + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="memalloc_">-</a> + + qh_memalloc_(size, object, type) + returns object of size bytes + assumes size<=qhmem.LASTsize and void **freelistp is a temp +*/ + +#ifdef qh_NOmem +#define qh_memalloc_(size, freelistp, object, type) {\ + object= (type*)qh_memalloc (size); } +#else /* !qh_NOmem */ + +#define qh_memalloc_(size, freelistp, object, type) {\ + freelistp= qhmem.freelists + qhmem.indextable[size];\ + if ((object= (type*)*freelistp)) {\ + qhmem.cntquick++; \ + *freelistp= *((void **)*freelistp);\ + }else object= (type*)qh_memalloc (size);} +#endif + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="memfree_">-</a> + + qh_memfree_(object, size) + free up an object + + notes: + object may be NULL + assumes size<=qhmem.LASTsize and void **freelistp is a temp +*/ +#ifdef qh_NOmem +#define qh_memfree_(object, size, freelistp) {\ + qh_memfree (object, size); } +#else /* !qh_NOmem */ + +#define qh_memfree_(object, size, freelistp) {\ + if (object) { \ + qhmem .freeshort++;\ + freelistp= qhmem.freelists + qhmem.indextable[size];\ + *((void **)object)= *freelistp;\ + *freelistp= object;}} +#endif + +/*=============== prototypes in alphabetical order ============*/ + +void *qh_memalloc(int insize); +void qh_memfree (void *object, int size); +void qh_memfreeshort (int *curlong, int *totlong); +void qh_meminit (FILE *ferr); +void qh_meminitbuffers (int tracelevel, int alignment, int numsizes, + int bufsize, int bufinit); +void qh_memsetup (void); +void qh_memsize(int size); +void qh_memstatistics (FILE *fp); + +#endif /* qhDEFmem */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/merge.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/merge.h new file mode 100644 index 0000000000000000000000000000000000000000..5dc8ac1f17e6a7d5226dfc0241f56e0d2bbdb70d --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/merge.h @@ -0,0 +1,174 @@ +/*<html><pre> -<a href="qh-merge.htm" + >-------------------------------</a><a name="TOP">-</a> + + merge.h + header file for merge.c + + see qh-merge.htm and merge.c + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFmerge +#define qhDEFmerge 1 + + +/*============ -constants- ==============*/ + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_ANGLEredundant">-</a> + + qh_ANGLEredundant + indicates redundant merge in mergeT->angle +*/ +#define qh_ANGLEredundant 6.0 + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_ANGLEdegen">-</a> + + qh_ANGLEdegen + indicates degenerate facet in mergeT->angle +*/ +#define qh_ANGLEdegen 5.0 + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_ANGLEconcave">-</a> + + qh_ANGLEconcave + offset to indicate concave facets in mergeT->angle + + notes: + concave facets are assigned the range of [2,4] in mergeT->angle + roundoff error may make the angle less than 2 +*/ +#define qh_ANGLEconcave 1.5 + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="MRG">-</a> + + MRG... (mergeType) + indicates the type of a merge (mergeT->type) +*/ +typedef enum { /* in sort order for facet_mergeset */ + MRGnone= 0, + MRGcoplanar, /* centrum coplanar */ + MRGanglecoplanar, /* angle coplanar */ + /* could detect half concave ridges */ + MRGconcave, /* concave ridge */ + MRGflip, /* flipped facet. facet1 == facet2 */ + MRGridge, /* duplicate ridge (qh_MERGEridge) */ + /* degen and redundant go onto degen_mergeset */ + MRGdegen, /* degenerate facet (not enough neighbors) facet1 == facet2 */ + MRGredundant, /* redundant facet (vertex subset) */ + /* merge_degenredundant assumes degen < redundant */ + MRGmirror, /* mirror facet from qh_triangulate */ + ENDmrg +} mergeType; + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_MERGEapex">-</a> + + qh_MERGEapex + flag for qh_mergefacet() to indicate an apex merge +*/ +#define qh_MERGEapex True + +/*============ -structures- ====================*/ + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="mergeT">-</a> + + mergeT + structure used to merge facets +*/ + +typedef struct mergeT mergeT; +struct mergeT { /* initialize in qh_appendmergeset */ + realT angle; /* angle between normals of facet1 and facet2 */ + facetT *facet1; /* will merge facet1 into facet2 */ + facetT *facet2; + mergeType type; +}; + + +/*=========== -macros- =========================*/ + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="FOREACHmerge_">-</a> + + FOREACHmerge_( merges ) {...} + assign 'merge' to each merge in merges + + notes: + uses 'mergeT *merge, **mergep;' + if qh_mergefacet(), + restart since qh.facet_mergeset may change + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHmerge_( merges ) FOREACHsetelement_(mergeT, merges, merge) + +/*============ prototypes in alphabetical order after pre/postmerge =======*/ + +void qh_premerge (vertexT *apex, realT maxcentrum, realT maxangle); +void qh_postmerge (char *reason, realT maxcentrum, realT maxangle, + boolT vneighbors); +void qh_all_merges (boolT othermerge, boolT vneighbors); +void qh_appendmergeset(facetT *facet, facetT *neighbor, mergeType mergetype, realT *angle); +setT *qh_basevertices( facetT *samecycle); +void qh_checkconnect (void /* qh new_facets */); +boolT qh_checkzero (boolT testall); +int qh_compareangle(const void *p1, const void *p2); +int qh_comparemerge(const void *p1, const void *p2); +int qh_comparevisit (const void *p1, const void *p2); +void qh_copynonconvex (ridgeT *atridge); +void qh_degen_redundant_facet (facetT *facet); +void qh_degen_redundant_neighbors (facetT *facet, facetT *delfacet); +vertexT *qh_find_newvertex (vertexT *oldvertex, setT *vertices, setT *ridges); +void qh_findbest_test (boolT testcentrum, facetT *facet, facetT *neighbor, + facetT **bestfacet, realT *distp, realT *mindistp, realT *maxdistp); +facetT *qh_findbestneighbor(facetT *facet, realT *distp, realT *mindistp, realT *maxdistp); +void qh_flippedmerges(facetT *facetlist, boolT *wasmerge); +void qh_forcedmerges( boolT *wasmerge); +void qh_getmergeset(facetT *facetlist); +void qh_getmergeset_initial (facetT *facetlist); +void qh_hashridge (setT *hashtable, int hashsize, ridgeT *ridge, vertexT *oldvertex); +ridgeT *qh_hashridge_find (setT *hashtable, int hashsize, ridgeT *ridge, + vertexT *vertex, vertexT *oldvertex, int *hashslot); +void qh_makeridges(facetT *facet); +void qh_mark_dupridges(facetT *facetlist); +void qh_maydropneighbor (facetT *facet); +int qh_merge_degenredundant (void); +void qh_merge_nonconvex( facetT *facet1, facetT *facet2, mergeType mergetype); +void qh_mergecycle (facetT *samecycle, facetT *newfacet); +void qh_mergecycle_all (facetT *facetlist, boolT *wasmerge); +void qh_mergecycle_facets( facetT *samecycle, facetT *newfacet); +void qh_mergecycle_neighbors(facetT *samecycle, facetT *newfacet); +void qh_mergecycle_ridges(facetT *samecycle, facetT *newfacet); +void qh_mergecycle_vneighbors( facetT *samecycle, facetT *newfacet); +void qh_mergefacet(facetT *facet1, facetT *facet2, realT *mindist, realT *maxdist, boolT mergeapex); +void qh_mergefacet2d (facetT *facet1, facetT *facet2); +void qh_mergeneighbors(facetT *facet1, facetT *facet2); +void qh_mergeridges(facetT *facet1, facetT *facet2); +void qh_mergesimplex(facetT *facet1, facetT *facet2, boolT mergeapex); +void qh_mergevertex_del (vertexT *vertex, facetT *facet1, facetT *facet2); +void qh_mergevertex_neighbors(facetT *facet1, facetT *facet2); +void qh_mergevertices(setT *vertices1, setT **vertices); +setT *qh_neighbor_intersections (vertexT *vertex); +void qh_newvertices (setT *vertices); +boolT qh_reducevertices (void); +vertexT *qh_redundant_vertex (vertexT *vertex); +boolT qh_remove_extravertices (facetT *facet); +vertexT *qh_rename_sharedvertex (vertexT *vertex, facetT *facet); +void qh_renameridgevertex(ridgeT *ridge, vertexT *oldvertex, vertexT *newvertex); +void qh_renamevertex(vertexT *oldvertex, vertexT *newvertex, setT *ridges, + facetT *oldfacet, facetT *neighborA); +boolT qh_test_appendmerge (facetT *facet, facetT *neighbor); +boolT qh_test_vneighbors (void /* qh newfacet_list */); +void qh_tracemerge (facetT *facet1, facetT *facet2); +void qh_tracemerging (void); +void qh_updatetested( facetT *facet1, facetT *facet2); +setT *qh_vertexridges (vertexT *vertex); +void qh_vertexridges_facet (vertexT *vertex, facetT *facet, setT **ridges); +void qh_willdelete (facetT *facet, facetT *replace); + +#endif /* qhDEFmerge */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/poly.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/poly.h new file mode 100644 index 0000000000000000000000000000000000000000..2d49c284ce2de41c09a85ac49f552dba5fce1777 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/poly.h @@ -0,0 +1,291 @@ +/*<html><pre> -<a href="qh-poly.htm" + >-------------------------------</a><a name="TOP">-</a> + + poly.h + header file for poly.c and poly2.c + + see qh-poly.htm, qhull.h and poly.c + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFpoly +#define qhDEFpoly 1 + +/*=============== constants ========================== */ + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="ALGORITHMfault">-</a> + + ALGORITHMfault + use as argument to checkconvex() to report errors during buildhull +*/ +#define qh_ALGORITHMfault 0 + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="DATAfault">-</a> + + DATAfault + use as argument to checkconvex() to report errors during initialhull +*/ +#define qh_DATAfault 1 + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="DUPLICATEridge">-</a> + + DUPLICATEridge + special value for facet->neighbor to indicate a duplicate ridge + + notes: + set by matchneighbor, used by matchmatch and mark_dupridge +*/ +#define qh_DUPLICATEridge ( facetT * ) 1L + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="MERGEridge">-</a> + + MERGEridge flag in facet + special value for facet->neighbor to indicate a merged ridge + + notes: + set by matchneighbor, used by matchmatch and mark_dupridge +*/ +#define qh_MERGEridge ( facetT * ) 2L + + +/*============ -structures- ====================*/ + +/*=========== -macros- =========================*/ + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLfacet_">-</a> + + FORALLfacet_( facetlist ) { ... } + assign 'facet' to each facet in facetlist + + notes: + uses 'facetT *facet;' + assumes last facet is a sentinel + + see: + FORALLfacets +*/ +#define FORALLfacet_( facetlist ) if ( facetlist ) for( facet=( facetlist );facet && facet->next;facet=facet->next ) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLnew_facets">-</a> + + FORALLnew_facets { ... } + assign 'newfacet' to each facet in qh.newfacet_list + + notes: + uses 'facetT *newfacet;' + at exit, newfacet==NULL +*/ +#define FORALLnew_facets for( newfacet=qh newfacet_list;newfacet && newfacet->next;newfacet=newfacet->next ) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLvertex_">-</a> + + FORALLvertex_( vertexlist ) { ... } + assign 'vertex' to each vertex in vertexlist + + notes: + uses 'vertexT *vertex;' + at exit, vertex==NULL +*/ +#define FORALLvertex_( vertexlist ) for ( vertex=( vertexlist );vertex && vertex->next;vertex= vertex->next ) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLvisible_facets">-</a> + + FORALLvisible_facets { ... } + assign 'visible' to each visible facet in qh.visible_list + + notes: + uses 'vacetT *visible;' + at exit, visible==NULL +*/ +#define FORALLvisible_facets for (visible=qh visible_list; visible && visible->visible; visible= visible->next) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLsame_">-</a> + + FORALLsame_( newfacet ) { ... } + assign 'same' to each facet in newfacet->f.samecycle + + notes: + uses 'facetT *same;' + stops when it returns to newfacet +*/ +#define FORALLsame_(newfacet) for (same= newfacet->f.samecycle; same != newfacet; same= same->f.samecycle) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLsame_cycle_">-</a> + + FORALLsame_cycle_( newfacet ) { ... } + assign 'same' to each facet in newfacet->f.samecycle + + notes: + uses 'facetT *same;' + at exit, same == NULL +*/ +#define FORALLsame_cycle_(newfacet) \ + for (same= newfacet->f.samecycle; \ + same; same= (same == newfacet ? NULL : same->f.samecycle)) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHneighborA_">-</a> + + FOREACHneighborA_( facet ) { ... } + assign 'neighborA' to each neighbor in facet->neighbors + + FOREACHneighborA_( vertex ) { ... } + assign 'neighborA' to each neighbor in vertex->neighbors + + declare: + facetT *neighborA, **neighborAp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHneighborA_(facet) FOREACHsetelement_(facetT, facet->neighbors, neighborA) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvisible_">-</a> + + FOREACHvisible_( facets ) { ... } + assign 'visible' to each facet in facets + + notes: + uses 'facetT *facet, *facetp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvisible_(facets) FOREACHsetelement_(facetT, facets, visible) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHnewfacet_">-</a> + + FOREACHnewfacet_( facets ) { ... } + assign 'newfacet' to each facet in facets + + notes: + uses 'facetT *newfacet, *newfacetp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHnewfacet_(facets) FOREACHsetelement_(facetT, facets, newfacet) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertexA_">-</a> + + FOREACHvertexA_( vertices ) { ... } + assign 'vertexA' to each vertex in vertices + + notes: + uses 'vertexT *vertexA, *vertexAp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvertexA_(vertices) FOREACHsetelement_(vertexT, vertices, vertexA) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertexreverse12_">-</a> + + FOREACHvertexreverse12_( vertices ) { ... } + assign 'vertex' to each vertex in vertices + reverse order of first two vertices + + notes: + uses 'vertexT *vertex, *vertexp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvertexreverse12_(vertices) FOREACHsetelementreverse12_(vertexT, vertices, vertex) + + +/*=============== prototypes poly.c in alphabetical order ================*/ + +void qh_appendfacet(facetT *facet); +void qh_appendvertex(vertexT *vertex); +void qh_attachnewfacets (void); +boolT qh_checkflipped (facetT *facet, realT *dist, boolT allerror); +void qh_delfacet(facetT *facet); +void qh_deletevisible(void /*qh visible_list, qh horizon_list*/); +setT *qh_facetintersect (facetT *facetA, facetT *facetB, int *skipAp,int *skipBp, int extra); +unsigned qh_gethash (int hashsize, setT *set, int size, int firstindex, void *skipelem); +facetT *qh_makenewfacet(setT *vertices, boolT toporient, facetT *facet); +void qh_makenewplanes ( void /* newfacet_list */); +facetT *qh_makenew_nonsimplicial (facetT *visible, vertexT *apex, int *numnew); +facetT *qh_makenew_simplicial (facetT *visible, vertexT *apex, int *numnew); +void qh_matchneighbor (facetT *newfacet, int newskip, int hashsize, + int *hashcount); +void qh_matchnewfacets (void); +boolT qh_matchvertices (int firstindex, setT *verticesA, int skipA, + setT *verticesB, int *skipB, boolT *same); +facetT *qh_newfacet(void); +ridgeT *qh_newridge(void); +int qh_pointid (pointT *point); +void qh_removefacet(facetT *facet); +void qh_removevertex(vertexT *vertex); +void qh_updatevertices (void); + + +/*========== -prototypes poly2.c in alphabetical order ===========*/ + +void qh_addhash (void* newelem, setT *hashtable, int hashsize, unsigned hash); +void qh_check_bestdist (void); +void qh_check_maxout (void); +void qh_check_output (void); +void qh_check_point (pointT *point, facetT *facet, realT *maxoutside, realT *maxdist, facetT **errfacet1, facetT **errfacet2); +void qh_check_points(void); +void qh_checkconvex(facetT *facetlist, int fault); +void qh_checkfacet(facetT *facet, boolT newmerge, boolT *waserrorp); +void qh_checkflipped_all (facetT *facetlist); +void qh_checkpolygon(facetT *facetlist); +void qh_checkvertex (vertexT *vertex); +void qh_clearcenters (qh_CENTER type); +void qh_createsimplex(setT *vertices); +void qh_delridge(ridgeT *ridge); +void qh_delvertex (vertexT *vertex); +setT *qh_facet3vertex (facetT *facet); +facetT *qh_findbestfacet (pointT *point, boolT bestoutside, + realT *bestdist, boolT *isoutside); +facetT *qh_findbestlower (facetT *upperfacet, pointT *point, realT *bestdistp, int *numpart); +facetT *qh_findfacet_all (pointT *point, realT *bestdist, boolT *isoutside, + int *numpart); +int qh_findgood (facetT *facetlist, int goodhorizon); +void qh_findgood_all (facetT *facetlist); +void qh_furthestnext (void /* qh facet_list */); +void qh_furthestout (facetT *facet); +void qh_infiniteloop (facetT *facet); +void qh_initbuild(void); +void qh_initialhull(setT *vertices); +setT *qh_initialvertices(int dim, setT *maxpoints, pointT *points, int numpoints); +vertexT *qh_isvertex (pointT *point, setT *vertices); +vertexT *qh_makenewfacets (pointT *point /*horizon_list, visible_list*/); +void qh_matchduplicates (facetT *atfacet, int atskip, int hashsize, int *hashcount); +void qh_nearcoplanar ( void /* qh.facet_list */); +vertexT *qh_nearvertex (facetT *facet, pointT *point, realT *bestdistp); +int qh_newhashtable(int newsize); +vertexT *qh_newvertex(pointT *point); +ridgeT *qh_nextridge3d (ridgeT *atridge, facetT *facet, vertexT **vertexp); +void qh_outcoplanar (void /* facet_list */); +pointT *qh_point (int id); +void qh_point_add (setT *set, pointT *point, void *elem); +setT *qh_pointfacet (void /*qh facet_list*/); +setT *qh_pointvertex (void /*qh facet_list*/); +void qh_prependfacet(facetT *facet, facetT **facetlist); +void qh_printhashtable(FILE *fp); +void qh_printlists (void); +void qh_resetlists (boolT stats, boolT resetVisible /*qh newvertex_list newfacet_list visible_list*/); +void qh_setvoronoi_all (void); +void qh_triangulate (void /*qh facet_list*/); +void qh_triangulate_facet (facetT *facetA, vertexT **first_vertex); +void qh_triangulate_link (facetT *oldfacetA, facetT *facetA, facetT *oldfacetB, facetT *facetB); +void qh_triangulate_mirror (facetT *facetA, facetT *facetB); +void qh_triangulate_null (facetT *facetA); +void qh_vertexintersect(setT **vertexsetA,setT *vertexsetB); +setT *qh_vertexintersect_new(setT *vertexsetA,setT *vertexsetB); +void qh_vertexneighbors (void /*qh facet_list*/); +boolT qh_vertexsubset(setT *vertexsetA, setT *vertexsetB); + + +#endif /* qhDEFpoly */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qhull.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qhull.h new file mode 100644 index 0000000000000000000000000000000000000000..657f5e44d1657c294ebd86db6e134cbbd8a06c1e --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qhull.h @@ -0,0 +1,1030 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qhull.h + user-level header file for using qhull.a library + + see qh-qhull.htm, qhull_a.h + + copyright (c) 1993-2003, The Geometry Center + + NOTE: access to qh_qh is via the 'qh' macro. This allows + qh_qh to be either a pointer or a structure. An example + of using qh is "qh DROPdim" which accesses the DROPdim + field of qh_qh. Similarly, access to qh_qhstat is via + the 'qhstat' macro. + + includes function prototypes for qhull.c, geom.c, global.c, io.c, user.c + + use mem.h for mem.c + use qset.h for qset.c + + see unix.c for an example of using qhull.h + + recompile qhull if you change this file +*/ + +#ifndef qhDEFqhull +#define qhDEFqhull 1 + +/*=========================== -included files ==============*/ + +#include <setjmp.h> +#include <float.h> +#include <time.h> + +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <Desk.h> +#endif + +#ifndef __STDC__ +#ifndef __cplusplus +#if !_MSC_VER +#error Neither __STDC__ nor __cplusplus is defined. Please use strict ANSI C or C++ to compile +#error Qhull. You may need to turn off compiler extensions in your project configuration. If +#error your compiler is a standard C compiler, you can delete this warning from qhull.h +#endif +#endif +#endif + +#include "user.h" /* user defineable constants */ + +/*============ constants and basic types ====================*/ + +extern char *qh_version; /* defined in global.c */ + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="coordT">-</a> + + coordT + coordinates and coefficients are stored as realT (i.e., double) + + notes: + could use 'float' for data and 'double' for calculations (realT vs. coordT) + This requires many type casts, and adjusted error bounds. + Also C compilers may do expressions in double anyway. +*/ +#define coordT realT + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="pointT">-</a> + + pointT + a point is an array of DIM3 coordinates +*/ +#define pointT coordT + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="flagT">-</a> + + flagT + Boolean flag as a bit +*/ +#define flagT unsigned int + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="boolT">-</a> + + boolT + boolean value, either True or False + + notes: + needed for portability +*/ +#define boolT unsigned int +#ifdef False +#undef False +#endif +#ifdef True +#undef True +#endif +#define False 0 +#define True 1 + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="CENTERtype">-</a> + + qh_CENTER + to distinguish facet->center +*/ +typedef enum +{ + qh_ASnone = 0, qh_ASvoronoi, qh_AScentrum +} +qh_CENTER; + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="qh_PRINT">-</a> + + qh_PRINT + output formats for printing (qh.PRINTout). + 'Fa' 'FV' 'Fc' 'FC' + + + notes: + some of these names are similar to qh names. The similar names are only + used in switch statements in qh_printbegin() etc. +*/ +typedef enum {qh_PRINTnone= 0, + qh_PRINTarea, qh_PRINTaverage, /* 'Fa' 'FV' 'Fc' 'FC' */ + qh_PRINTcoplanars, qh_PRINTcentrums, + qh_PRINTfacets, qh_PRINTfacets_xridge, /* 'f' 'FF' 'G' 'FI' 'Fi' 'Fn' */ + qh_PRINTgeom, qh_PRINTids, qh_PRINTinner, qh_PRINTneighbors, + qh_PRINTnormals, qh_PRINTouter, qh_PRINTmaple, /* 'n' 'Fo' 'i' 'm' 'Fm' 'FM', 'o' */ + qh_PRINTincidences, qh_PRINTmathematica, qh_PRINTmerges, qh_PRINToff, + qh_PRINToptions, qh_PRINTpointintersect, /* 'FO' 'Fp' 'FP' 'p' 'FQ' 'FS' */ + qh_PRINTpointnearest, qh_PRINTpoints, qh_PRINTqhull, qh_PRINTsize, + qh_PRINTsummary, qh_PRINTtriangles, /* 'Fs' 'Ft' 'Fv' 'FN' 'Fx' */ + qh_PRINTvertices, qh_PRINTvneighbors, qh_PRINTextremes, + qh_PRINTEND} qh_PRINT; + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="qh_ALL">-</a> + + qh_ALL + argument flag for selecting everything +*/ +#define qh_ALL True +#define qh_NOupper True /* argument for qh_findbest */ +#define qh_IScheckmax True /* argument for qh_findbesthorizon */ +#define qh_ISnewfacets True /* argument for qh_findbest */ +#define qh_RESETvisible True /* argument for qh_resetlists */ + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="qh_ERR">-</a> + + qh_ERR + Qhull exit codes, for indicating errors +*/ +#define qh_ERRnone 0 /* no error occurred during qhull */ +#define qh_ERRinput 1 /* input inconsistency */ +#define qh_ERRsingular 2 /* singular input data */ +#define qh_ERRprec 3 /* precision error */ +#define qh_ERRmem 4 /* insufficient memory, matches mem.h */ +#define qh_ERRqhull 5 /* internal error detected, matches mem.h */ + +/* ============ -structures- ==================== + each of the following structures is defined by a typedef + all realT and coordT fields occur at the beginning of a structure + (otherwise space may be wasted due to alignment) + define all flags together and pack into 32-bit number +*/ + +typedef struct vertexT vertexT; +typedef struct ridgeT ridgeT; +typedef struct facetT facetT; +#ifndef DEFsetT +#define DEFsetT 1 +typedef struct setT setT; /* defined in qset.h */ +#endif + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="facetT">-</a> + + facetT + defines a facet + + notes: + qhull() generates the hull as a list of facets. + + topological information: + f.previous,next doubly-linked list of facets + f.vertices set of vertices + f.ridges set of ridges + f.neighbors set of neighbors + f.toporient True if facet has top-orientation (else bottom) + + geometric information: + f.offset,normal hyperplane equation + f.maxoutside offset to outer plane -- all points inside + f.center centrum for testing convexity + f.simplicial True if facet is simplicial + f.flipped True if facet does not include qh.interior_point + + for constructing hull: + f.visible True if facet on list of visible facets (will be deleted) + f.newfacet True if facet on list of newly created facets + f.coplanarset set of points coplanar with this facet + (includes near-inside points for later testing) + f.outsideset set of points outside of this facet + f.furthestdist distance to furthest point of outside set + f.visitid marks visited facets during a loop + f.replace replacement facet for to-be-deleted, visible facets + f.samecycle,newcycle cycle of facets for merging into horizon facet + + see below for other flags and fields +*/ +struct facetT { +#if !qh_COMPUTEfurthest + coordT furthestdist;/* distance to furthest point of outsideset */ +#endif +#if qh_MAXoutside + coordT maxoutside; /* max computed distance of point to facet + Before QHULLfinished this is an approximation + since maxdist not always set for mergefacet + Actual outer plane is +DISTround and + computed outer plane is +2*DISTround */ +#endif + coordT offset; /* exact offset of hyperplane from origin */ + coordT *normal; /* normal of hyperplane, hull_dim coefficients */ + /* if tricoplanar, shared with a neighbor */ + union { /* in order of testing */ + realT area; /* area of facet, only in io.c if ->isarea */ + facetT *replace; /* replacement facet if ->visible and NEWfacets + is NULL only if qh_mergedegen_redundant or interior */ + facetT *samecycle; /* cycle of facets from the same visible/horizon intersection, + if ->newfacet */ + facetT *newcycle; /* in horizon facet, current samecycle of new facets */ + facetT *trivisible; /* visible facet for ->tricoplanar facets during qh_triangulate() */ + facetT *triowner; /* owner facet for ->tricoplanar, !isarea facets w/ ->keepcentrum */ + }f; + coordT *center; /* centrum for convexity, qh CENTERtype == qh_AScentrum */ + /* Voronoi center, qh CENTERtype == qh_ASvoronoi */ + /* if tricoplanar, shared with a neighbor */ + facetT *previous; /* previous facet in the facet_list */ + facetT *next; /* next facet in the facet_list */ + setT *vertices; /* vertices for this facet, inverse sorted by ID + if simplicial, 1st vertex was apex/furthest */ + setT *ridges; /* explicit ridges for nonsimplicial facets. + for simplicial facets, neighbors defines ridge */ + setT *neighbors; /* neighbors of the facet. If simplicial, the kth + neighbor is opposite the kth vertex, and the first + neighbor is the horizon facet for the first vertex*/ + setT *outsideset; /* set of points outside this facet + if non-empty, last point is furthest + if NARROWhull, includes coplanars for partitioning*/ + setT *coplanarset; /* set of points coplanar with this facet + > qh.min_vertex and <= facet->max_outside + a point is assigned to the furthest facet + if non-empty, last point is furthest away */ + unsigned visitid; /* visit_id, for visiting all neighbors, + all uses are independent */ + unsigned id; /* unique identifier from qh facet_id */ + unsigned nummerge:9; /* number of merges */ +#define qh_MAXnummerge 511 /* 2^9-1, 32 flags total, see "flags:" in io.c */ + flagT tricoplanar:1; /* True if TRIangulate and simplicial and coplanar with a neighbor */ + /* all tricoplanars share the same ->center, ->normal, ->offset, ->maxoutside */ + /* all tricoplanars share the same apex */ + /* if ->degenerate, does not span facet (one logical ridge) */ + /* one tricoplanar has ->keepcentrum and ->coplanarset */ + /* during qh_triangulate, f.trivisible points to original facet */ + flagT newfacet:1; /* True if facet on qh newfacet_list (new or merged) */ + flagT visible:1; /* True if visible facet (will be deleted) */ + flagT toporient:1; /* True if created with top orientation + after merging, use ridge orientation */ + flagT simplicial:1;/* True if simplicial facet, ->ridges may be implicit */ + flagT seen:1; /* used to perform operations only once, like visitid */ + flagT seen2:1; /* used to perform operations only once, like visitid */ + flagT flipped:1; /* True if facet is flipped */ + flagT upperdelaunay:1; /* True if facet is upper envelope of Delaunay triangulation */ + flagT notfurthest:1; /* True if last point of outsideset is not furthest*/ + +/*-------- flags primarily for output ---------*/ + flagT good:1; /* True if a facet marked good for output */ + flagT isarea:1; /* True if facet->f.area is defined */ + +/*-------- flags for merging ------------------*/ + flagT dupridge:1; /* True if duplicate ridge in facet */ + flagT mergeridge:1; /* True if facet or neighbor contains a qh_MERGEridge + ->normal defined (also defined for mergeridge2) */ + flagT mergeridge2:1; /* True if neighbor contains a qh_MERGEridge (mark_dupridges */ + flagT coplanar:1; /* True if horizon facet is coplanar at last use */ + flagT mergehorizon:1; /* True if will merge into horizon (->coplanar) */ + flagT cycledone:1;/* True if mergecycle_all already done */ + flagT tested:1; /* True if facet convexity has been tested (false after merge */ + flagT keepcentrum:1; /* True if keep old centrum after a merge, or marks owner for ->tricoplanar */ + flagT newmerge:1; /* True if facet is newly merged for reducevertices */ + flagT degenerate:1; /* True if facet is degenerate (degen_mergeset or ->tricoplanar) */ + flagT redundant:1; /* True if facet is redundant (degen_mergeset) */ +}; + + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="ridgeT">-</a> + + ridgeT + defines a ridge + + notes: + a ridge is DIM3-1 simplex between two neighboring facets. If the + facets are non-simplicial, there may be more than one ridge between + two facets. E.G. a 4-d hypercube has two triangles between each pair + of neighboring facets. + + topological information: + vertices a set of vertices + top,bottom neighboring facets with orientation + + geometric information: + tested True if ridge is clearly convex + nonconvex True if ridge is non-convex +*/ +struct ridgeT { + setT *vertices; /* vertices belonging to this ridge, inverse sorted by ID + NULL if a degen ridge (matchsame) */ + facetT *top; /* top facet this ridge is part of */ + facetT *bottom; /* bottom facet this ridge is part of */ + unsigned id:24; /* unique identifier, =>room for 8 flags */ + flagT seen:1; /* used to perform operations only once */ + flagT tested:1; /* True when ridge is tested for convexity */ + flagT nonconvex:1; /* True if getmergeset detected a non-convex neighbor + only one ridge between neighbors may have nonconvex */ +}; + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="vertexT">-</a> + + vertexT + defines a vertex + + topological information: + next,previous doubly-linked list of all vertices + neighbors set of adjacent facets (only if qh.VERTEXneighbors) + + geometric information: + point array of DIM3 coordinates +*/ +struct vertexT { + vertexT *next; /* next vertex in vertex_list */ + vertexT *previous; /* previous vertex in vertex_list */ + pointT *point; /* hull_dim coordinates (coordT) */ + setT *neighbors; /* neighboring facets of vertex, qh_vertexneighbors() + inits in io.c or after first merge */ + unsigned visitid; /* for use with qh vertex_visit */ + unsigned id:24; /* unique identifier, =>room for 8 flags */ + flagT seen:1; /* used to perform operations only once */ + flagT seen2:1; /* another seen flag */ + flagT delridge:1; /* vertex was part of a deleted ridge */ + flagT deleted:1; /* true if vertex on qh del_vertices */ + flagT newlist:1; /* true if vertex on qh newvertex_list */ +}; + +/*======= -global variables -qh ============================*/ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh">-</a> + + qh + all global variables for qhull are in qh, qhmem, and qhstat + + notes: + qhmem is defined in mem.h and qhstat is defined in stat.h + Access to qh_qh is via the "qh" macro. See qh_QHpointer in user.h +*/ +typedef struct qhT qhT; +#if qh_QHpointer +#define qh qh_qh-> +extern qhT *qh_qh; /* allocated in global.c */ +#else +#define qh qh_qh. +extern qhT qh_qh; +#endif + +struct qhT { + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-const">-</a> + + qh constants + configuration flags and constants for Qhull + + notes: + The user configures Qhull by defining flags. They are + copied into qh by qh_setflags(). qh-quick.htm#options defines the flags. +*/ + boolT ALLpoints; /* true 'Qs' if search all points for initial simplex */ + boolT ANGLEmerge; /* true 'Qa' if sort potential merges by angle */ + boolT APPROXhull; /* true 'Wn' if MINoutside set */ + realT MINoutside; /* 'Wn' min. distance for an outside point */ + boolT ATinfinity; /* true 'Qz' if point num_points-1 is "at-infinity" + for improving precision in Delaunay triangulations */ + boolT AVOIDold; /* true 'Q4' if avoid old->new merges */ + boolT BESToutside; /* true 'Qf' if partition points into best outsideset */ + boolT CDDinput; /* true 'Pc' if input uses CDD format (1.0/offset first) */ + boolT CDDoutput; /* true 'PC' if print normals in CDD format (offset first) */ + boolT CHECKfrequently; /* true 'Tc' if checking frequently */ + realT premerge_cos; /* 'A-n' cos_max when pre merging */ + realT postmerge_cos; /* 'An' cos_max when post merging */ + boolT DELAUNAY; /* true 'd' if computing DELAUNAY triangulation */ + boolT DOintersections; /* true 'Gh' if print hyperplane intersections */ + int DROPdim; /* drops dim 'GDn' for 4-d -> 3-d output */ + boolT FORCEoutput; /* true 'Po' if forcing output despite degeneracies */ + int GOODpoint; /* 1+n for 'QGn', good facet if visible/not(-) from point n*/ + pointT *GOODpointp; /* the actual point */ + boolT GOODthreshold; /* true if qh lower_threshold/upper_threshold defined + false if qh SPLITthreshold */ + int GOODvertex; /* 1+n, good facet if vertex for point n */ + pointT *GOODvertexp; /* the actual point */ + boolT HALFspace; /* true 'Hn,n,n' if halfspace intersection */ + int IStracing; /* trace execution, 0=none, 1=least, 4=most, -1=events */ + int KEEParea; /* 'PAn' number of largest facets to keep */ + boolT KEEPcoplanar; /* true 'Qc' if keeping nearest facet for coplanar points */ + boolT KEEPinside; /* true 'Qi' if keeping nearest facet for inside points + set automatically if 'd Qc' */ + int KEEPmerge; /* 'PMn' number of facets to keep with most merges */ + realT KEEPminArea; /* 'PFn' minimum facet area to keep */ + realT MAXcoplanar; /* 'Un' max distance below a facet to be coplanar*/ + boolT MERGEexact; /* true 'Qx' if exact merges (coplanar, degen, dupridge, flipped) */ + boolT MERGEindependent; /* true 'Q2' if merging independent sets */ + boolT MERGING; /* true if exact-, pre- or post-merging, with angle and centrum tests */ + realT premerge_centrum; /* 'C-n' centrum_radius when pre merging. Default is round-off */ + realT postmerge_centrum; /* 'Cn' centrum_radius when post merging. Default is round-off */ + boolT MERGEvertices; /* true 'Q3' if merging redundant vertices */ + realT MINvisible; /* 'Vn' min. distance for a facet to be visible */ + boolT NOnarrow; /* true 'Q10' if no special processing for narrow distributions */ + boolT NOnearinside; /* true 'Q8' if ignore near-inside points when partitioning */ + boolT NOpremerge; /* true 'Q0' if no defaults for C-0 or Qx */ + boolT ONLYgood; /* true 'Qg' if process points with good visible or horizon facets */ + boolT ONLYmax; /* true 'Qm' if only process points that increase max_outside */ + boolT PICKfurthest; /* true 'Q9' if process furthest of furthest points*/ + boolT POSTmerge; /* true if merging after buildhull (Cn or An) */ + boolT PREmerge; /* true if merging during buildhull (C-n or A-n) */ + /* NOTE: some of these names are similar to qh_PRINT names */ + boolT PRINTcentrums; /* true 'Gc' if printing centrums */ + boolT PRINTcoplanar; /* true 'Gp' if printing coplanar points */ + int PRINTdim; /* print dimension for Geomview output */ + boolT PRINTdots; /* true 'Ga' if printing all points as dots */ + boolT PRINTgood; /* true 'Pg' if printing good facets */ + boolT PRINTinner; /* true 'Gi' if printing inner planes */ + boolT PRINTneighbors; /* true 'PG' if printing neighbors of good facets */ + boolT PRINTnoplanes; /* true 'Gn' if printing no planes */ + boolT PRINToptions1st; /* true 'FO' if printing options to stderr */ + boolT PRINTouter; /* true 'Go' if printing outer planes */ + boolT PRINTprecision; /* false 'Pp' if not reporting precision problems */ + qh_PRINT PRINTout[qh_PRINTEND]; /* list of output formats to print */ + boolT PRINTridges; /* true 'Gr' if print ridges */ + boolT PRINTspheres; /* true 'Gv' if print vertices as spheres */ + boolT PRINTstatistics; /* true 'Ts' if printing statistics to stderr */ + boolT PRINTsummary; /* true 's' if printing summary to stderr */ + boolT PRINTtransparent; /* true 'Gt' if print transparent outer ridges */ + boolT PROJECTdelaunay; /* true if DELAUNAY, no readpoints() and + need projectinput() for Delaunay in qh_init_B */ + int PROJECTinput; /* number of projected dimensions 'bn:0Bn:0' */ + boolT QUICKhelp; /* true if quick help message for degen input */ + boolT RANDOMdist; /* true if randomly change distplane and setfacetplane */ + realT RANDOMfactor; /* maximum random perturbation */ + realT RANDOMa; /* qh_randomfactor is randr * RANDOMa + RANDOMb */ + realT RANDOMb; + boolT RANDOMoutside; /* true if select a random outside point */ + int REPORTfreq; /* buildtracing reports every n facets */ + int REPORTfreq2; /* tracemerging reports every REPORTfreq/2 facets */ + int RERUN; /* 'TRn' rerun qhull n times (qh.build_cnt) */ + int ROTATErandom; /* 'QRn' seed, 0 time, >= rotate input */ + boolT SCALEinput; /* true 'Qbk' if scaling input */ + boolT SCALElast; /* true 'Qbb' if scale last coord to max prev coord */ + boolT SETroundoff; /* true 'E' if qh DISTround is predefined */ + boolT SKIPcheckmax; /* true 'Q5' if skip qh_check_maxout */ + boolT SKIPconvex; /* true 'Q6' if skip convexity testing during pre-merge */ + boolT SPLITthresholds; /* true if upper_/lower_threshold defines a region + used only for printing (not for qh ONLYgood) */ + int STOPcone; /* 'TCn' 1+n for stopping after cone for point n*/ + /* also used by qh_build_withresart for err exit*/ + int STOPpoint; /* 'TVn' 'TV-n' 1+n for stopping after/before(-) + adding point n */ + int TESTpoints; /* 'QTn' num of test points after qh.num_points. Test points always coplanar. */ + boolT TESTvneighbors; /* true 'Qv' if test vertex neighbors at end */ + int TRACElevel; /* 'Tn' conditional IStracing level */ + int TRACElastrun; /* qh.TRACElevel applies to last qh.RERUN */ + int TRACEpoint; /* 'TPn' start tracing when point n is a vertex */ + realT TRACEdist; /* 'TWn' start tracing when merge distance too big */ + int TRACEmerge; /* 'TMn' start tracing before this merge */ + boolT TRIangulate; /* true 'Qt' if triangulate non-simplicial facets */ + boolT TRInormals; /* true 'Q11' if triangulate duplicates normals (sets Qt) */ + boolT UPPERdelaunay; /* true 'Qu' if computing furthest-site Delaunay */ + boolT VERIFYoutput; /* true 'Tv' if verify output at end of qhull */ + boolT VIRTUALmemory; /* true 'Q7' if depth-first processing in buildhull */ + boolT VORONOI; /* true 'v' if computing Voronoi diagram */ + + /*--------input constants ---------*/ + realT AREAfactor; /* 1/(hull_dim-1)! for converting det's to area */ + boolT DOcheckmax; /* true if calling qh_check_maxout (qh_initqhull_globals) */ + char *feasible_string; /* feasible point 'Hn,n,n' for halfspace intersection */ + coordT *feasible_point; /* as coordinates, both malloc'd */ + boolT GETarea; /* true 'Fa', 'FA', 'FS', 'PAn', 'PFn' if compute facet area/Voronoi volume in io.c */ + boolT KEEPnearinside; /* true if near-inside points in coplanarset */ + int hull_dim; /* dimension of hull, set by initbuffers */ + int input_dim; /* dimension of input, set by initbuffers */ + int num_points; /* number of input points */ + pointT *first_point; /* array of input points, see POINTSmalloc */ + boolT POINTSmalloc; /* true if qh first_point/num_points allocated */ + pointT *input_points; /* copy of original qh.first_point for input points for qh_joggleinput */ + boolT input_malloc; /* true if qh input_points malloc'd */ + char qhull_command[256];/* command line that invoked this program */ + char rbox_command[256]; /* command line that produced the input points */ + char qhull_options[512];/* descriptive list of options */ + int qhull_optionlen; /* length of last line */ + int qhull_optionsiz; /* size of qhull_options before qh_initbuild */ + boolT VERTEXneighbors; /* true if maintaining vertex neighbors */ + boolT ZEROcentrum; /* true if 'C-0' or 'C-0 Qx'. sets ZEROall_ok */ + realT *upper_threshold; /* don't print if facet->normal[k]>=upper_threshold[k] + must set either GOODthreshold or SPLITthreshold + if Delaunay, default is 0.0 for upper envelope */ + realT *lower_threshold; /* don't print if facet->normal[k] <=lower_threshold[k] */ + realT *upper_bound; /* scale point[k] to new upper bound */ + realT *lower_bound; /* scale point[k] to new lower bound + project if both upper_ and lower_bound == 0 */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-prec">-</a> + + qh precision constants + precision constants for Qhull + + notes: + qh_detroundoff() computes the maximum roundoff error for distance + and other computations. It also sets default values for the + qh constants above. +*/ + realT ANGLEround; /* max round off error for angles */ + realT centrum_radius; /* max centrum radius for convexity (roundoff added) */ + realT cos_max; /* max cosine for convexity (roundoff added) */ + realT DISTround; /* max round off error for distances, 'E' overrides */ + realT MAXabs_coord; /* max absolute coordinate */ + realT MAXlastcoord; /* max last coordinate for qh_scalelast */ + realT MAXsumcoord; /* max sum of coordinates */ + realT MAXwidth; /* max rectilinear width of point coordinates */ + realT MINdenom_1; /* min. abs. value for 1/x */ + realT MINdenom; /* use divzero if denominator < MINdenom */ + realT MINdenom_1_2; /* min. abs. val for 1/x that allows normalization */ + realT MINdenom_2; /* use divzero if denominator < MINdenom_2 */ + realT MINlastcoord; /* min. last coordinate for qh_scalelast */ + boolT NARROWhull; /* set in qh_initialhull if angle < qh_MAXnarrow */ + realT *NEARzero; /* hull_dim array for near zero in gausselim */ + realT NEARinside; /* keep points for qh_check_maxout if close to facet */ + realT ONEmerge; /* max distance for merging simplicial facets */ + realT outside_err; /* application's epsilon for coplanar points + qh_check_bestdist() qh_check_points() reports error if point outside */ + realT WIDEfacet; /* size of wide facet for skipping ridge in + area computation and locking centrum */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-intern">-</a> + + qh internal constants + internal constants for Qhull +*/ + char qhull[sizeof("qhull")]; /* for checking ownership */ + void *old_stat; /* pointer to saved qh_qhstat, qh_save_qhull */ + jmp_buf errexit; /* exit label for qh_errexit, defined by setjmp() */ + char jmpXtra[40]; /* extra bytes in case jmp_buf is defined wrong by compiler */ + jmp_buf restartexit; /* restart label for qh_errexit, defined by setjmp() */ + char jmpXtra2[40]; /* extra bytes in case jmp_buf is defined wrong by compiler*/ + FILE *fin; /* pointer to input file, init by qh_meminit */ + FILE *fout; /* pointer to output file */ + FILE *ferr; /* pointer to error file */ + pointT *interior_point; /* center point of the initial simplex*/ + int normal_size; /* size in bytes for facet normals and point coords*/ + int center_size; /* size in bytes for Voronoi centers */ + int TEMPsize; /* size for small, temporary sets (in quick mem) */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-lists">-</a> + + qh facet and vertex lists + defines lists of facets, new facets, visible facets, vertices, and + new vertices. Includes counts, next ids, and trace ids. + see: + qh_resetlists() +*/ + facetT *facet_list; /* first facet */ + facetT *facet_tail; /* end of facet_list (dummy facet) */ + facetT *facet_next; /* next facet for buildhull() + previous facets do not have outside sets + NARROWhull: previous facets may have coplanar outside sets for qh_outcoplanar */ + facetT *newfacet_list; /* list of new facets to end of facet_list */ + facetT *visible_list; /* list of visible facets preceeding newfacet_list, + facet->visible set */ + int num_visible; /* current number of visible facets */ + unsigned tracefacet_id; /* set at init, then can print whenever */ + facetT *tracefacet; /* set in newfacet/mergefacet, undone in delfacet*/ + unsigned tracevertex_id; /* set at buildtracing, can print whenever */ + vertexT *tracevertex; /* set in newvertex, undone in delvertex*/ + vertexT *vertex_list; /* list of all vertices, to vertex_tail */ + vertexT *vertex_tail; /* end of vertex_list (dummy vertex) */ + vertexT *newvertex_list; /* list of vertices in newfacet_list, to vertex_tail + all vertices have 'newlist' set */ + int num_facets; /* number of facets in facet_list + includes visble faces (num_visible) */ + int num_vertices; /* number of vertices in facet_list */ + int num_outside; /* number of points in outsidesets (for tracing and RANDOMoutside) + includes coplanar outsideset points for NARROWhull/qh_outcoplanar() */ + int num_good; /* number of good facets (after findgood_all) */ + unsigned facet_id; /* ID of next, new facet from newfacet() */ + unsigned ridge_id; /* ID of next, new ridge from newridge() */ + unsigned vertex_id; /* ID of next, new vertex from newvertex() */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-var">-</a> + + qh global variables + defines minimum and maximum distances, next visit ids, several flags, + and other global variables. + initialize in qh_initbuild or qh_maxmin if used in qh_buildhull +*/ + unsigned long hulltime; /* ignore time to set up input and randomize */ + /* use unsigned to avoid wrap-around errors */ + boolT ALLOWrestart; /* true if qh_precision can use qh.restartexit */ + int build_cnt; /* number of calls to qh_initbuild */ + qh_CENTER CENTERtype; /* current type of facet->center, qh_CENTER */ + int furthest_id; /* pointid of furthest point, for tracing */ + facetT *GOODclosest; /* closest facet to GOODthreshold in qh_findgood */ + realT JOGGLEmax; /* set 'QJn' if randomly joggle input */ + boolT maxoutdone; /* set qh_check_maxout(), cleared by qh_addpoint() */ + realT max_outside; /* maximum distance from a point to a facet, + before roundoff, not simplicial vertices + actual outer plane is +DISTround and + computed outer plane is +2*DISTround */ + realT max_vertex; /* maximum distance (>0) from vertex to a facet, + before roundoff, due to a merge */ + realT min_vertex; /* minimum distance (<0) from vertex to a facet, + before roundoff, due to a merge + if qh.JOGGLEmax, qh_makenewplanes sets it + recomputed if qh.DOcheckmax, default -qh.DISTround */ + boolT NEWfacets; /* true while visible facets invalid due to new or merge + from makecone/attachnewfacets to deletevisible */ + boolT findbestnew; /* true if partitioning calls qh_findbestnew */ + boolT findbest_notsharp; /* true if new facets are at least 90 degrees */ + boolT NOerrexit; /* true if qh.errexit is not available */ + realT PRINTcradius; /* radius for printing centrums */ + realT PRINTradius; /* radius for printing vertex spheres and points */ + boolT POSTmerging; /* true when post merging */ + int printoutvar; /* temporary variable for qh_printbegin, etc. */ + int printoutnum; /* number of facets printed */ + boolT QHULLfinished; /* True after qhull() is finished */ + realT totarea; /* 'FA': total facet area computed by qh_getarea */ + realT totvol; /* 'FA': total volume computed by qh_getarea */ + unsigned int visit_id; /* unique ID for searching neighborhoods, */ + unsigned int vertex_visit; /* unique ID for searching vertices */ + boolT ZEROall_ok; /* True if qh_checkzero always succeeds */ + boolT WAScoplanar; /* True if qh_partitioncoplanar (qh_check_maxout) */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-set">-</a> + + qh global sets + defines sets for merging, initial simplex, hashing, extra input points, + and deleted vertices +*/ + setT *facet_mergeset; /* temporary set of merges to be done */ + setT *degen_mergeset; /* temporary set of degenerate and redundant merges */ + setT *hash_table; /* hash table for matching ridges in qh_matchfacets + size is setsize() */ + setT *other_points; /* additional points (first is qh interior_point) */ + setT *del_vertices; /* vertices to partition and delete with visible + facets. Have deleted set for checkfacet */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-buf">-</a> + + qh global buffers + defines buffers for maxtrix operations, input, and error messages +*/ + coordT *gm_matrix; /* (dim+1)Xdim matrix for geom.c */ + coordT **gm_row; /* array of gm_matrix rows */ + char* line; /* malloc'd input line of maxline+1 chars */ + int maxline; + coordT *half_space; /* malloc'd input array for halfspace (qh normal_size+coordT) */ + coordT *temp_malloc; /* malloc'd input array for points */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-static">-</a> + + qh static variables + defines static variables for individual functions + + notes: + do not use 'static' within a function. Multiple instances of qhull + may exist. + + do not assume zero initialization, 'QPn' may cause a restart +*/ + boolT ERREXITcalled; /* true during errexit (prevents duplicate calls */ + boolT firstcentrum; /* for qh_printcentrum */ + realT last_low; /* qh_scalelast parameters for qh_setdelaunay */ + realT last_high; + realT last_newhigh; + unsigned lastreport; /* for qh_buildtracing */ + int mergereport; /* for qh_tracemerging */ + boolT old_randomdist; /* save RANDOMdist when io, tracing, or statistics */ + int ridgeoutnum; /* number of ridges in 4OFF output */ + void *old_qhstat; /* for saving qh_qhstat in save_qhull() */ + setT *old_tempstack; /* for saving qhmem.tempstack in save_qhull */ + setT *coplanarset; /* set of coplanar facets for searching qh_findbesthorizon() */ +}; + +/*=========== -macros- =========================*/ + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="otherfacet_">-</a> + + otherfacet_(ridge, facet) + return neighboring facet for a ridge in facet +*/ +#define otherfacet_(ridge, facet) \ + (((ridge)->top == (facet)) ? (ridge)->bottom : (ridge)->top) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="getid_">-</a> + + getid_(p) + return ID for facet, ridge, or vertex + return MAXINT if NULL (-1 causes type conversion error ) +*/ +#define getid_(p) ((p) ? (p)->id : -1) + +/*============== FORALL macros ===================*/ + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLfacets">-</a> + + FORALLfacets { ... } + assign 'facet' to each facet in qh.facet_list + + notes: + uses 'facetT *facet;' + assumes last facet is a sentinel + + see: + FORALLfacet_( facetlist ) +*/ +#define FORALLfacets for (facet=qh facet_list;facet && facet->next;facet=facet->next) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLpoints">-</a> + + FORALLpoints { ... } + assign 'point' to each point in qh.first_point, qh.num_points + + declare: + coordT *point, *pointtemp; +*/ +#define FORALLpoints FORALLpoint_(qh first_point, qh num_points) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLpoint_">-</a> + + FORALLpoint_( points, num) { ... } + assign 'point' to each point in points array of num points + + declare: + coordT *point, *pointtemp; +*/ +#define FORALLpoint_(points, num) for(point= (points), \ + pointtemp= (points)+qh hull_dim*(num); point < pointtemp; point += qh hull_dim) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLvertices">-</a> + + FORALLvertices { ... } + assign 'vertex' to each vertex in qh.vertex_list + + declare: + vertexT *vertex; + + notes: + assumes qh.vertex_list terminated with a sentinel +*/ +#define FORALLvertices for (vertex=qh vertex_list;vertex && vertex->next;vertex= vertex->next) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHfacet_">-</a> + + FOREACHfacet_( facets ) { ... } + assign 'facet' to each facet in facets + + declare: + facetT *facet, **facetp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHfacet_(facets) FOREACHsetelement_(facetT, facets, facet) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHneighbor_">-</a> + + FOREACHneighbor_( facet ) { ... } + assign 'neighbor' to each neighbor in facet->neighbors + + FOREACHneighbor_( vertex ) { ... } + assign 'neighbor' to each neighbor in vertex->neighbors + + declare: + facetT *neighbor, **neighborp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHneighbor_(facet) FOREACHsetelement_(facetT, facet->neighbors, neighbor) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHpoint_">-</a> + + FOREACHpoint_( points ) { ... } + assign 'point' to each point in points set + + declare: + pointT *point, **pointp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHpoint_(points) FOREACHsetelement_(pointT, points, point) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHridge_">-</a> + + FOREACHridge_( ridges ) { ... } + assign 'ridge' to each ridge in ridges set + + declare: + ridgeT *ridge, **ridgep; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHridge_(ridges) FOREACHsetelement_(ridgeT, ridges, ridge) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertex_">-</a> + + FOREACHvertex_( vertices ) { ... } + assign 'vertex' to each vertex in vertices set + + declare: + vertexT *vertex, **vertexp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvertex_(vertices) FOREACHsetelement_(vertexT, vertices,vertex) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHfacet_i_">-</a> + + FOREACHfacet_i_( facets ) { ... } + assign 'facet' and 'facet_i' for each facet in facets set + + declare: + facetT *facet; + int facet_n, facet_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHfacet_i_(facets) FOREACHsetelement_i_(facetT, facets, facet) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHneighbor_i_">-</a> + + FOREACHneighbor_i_( facet ) { ... } + assign 'neighbor' and 'neighbor_i' for each neighbor in facet->neighbors + + FOREACHneighbor_i_( vertex ) { ... } + assign 'neighbor' and 'neighbor_i' for each neighbor in vertex->neighbors + + declare: + facetT *neighbor; + int neighbor_n, neighbor_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHneighbor_i_(facet) FOREACHsetelement_i_(facetT, facet->neighbors, neighbor) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHpoint_i_">-</a> + + FOREACHpoint_i_( points ) { ... } + assign 'point' and 'point_i' for each point in points set + + declare: + pointT *point; + int point_n, point_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHpoint_i_(points) FOREACHsetelement_i_(pointT, points, point) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHridge_i_">-</a> + + FOREACHridge_i_( ridges ) { ... } + assign 'ridge' and 'ridge_i' for each ridge in ridges set + + declare: + ridgeT *ridge; + int ridge_n, ridge_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHridge_i_(ridges) FOREACHsetelement_i_(ridgeT, ridges, ridge) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertex_i_">-</a> + + FOREACHvertex_i_( vertices ) { ... } + assign 'vertex' and 'vertex_i' for each vertex in vertices set + + declare: + vertexT *vertex; + int vertex_n, vertex_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> + */ +#define FOREACHvertex_i_(vertices) FOREACHsetelement_i_(vertexT, vertices,vertex) + +/********* -qhull.c prototypes (duplicated from qhull_a.h) **********************/ + +void qh_qhull (void); +boolT qh_addpoint (pointT *furthest, facetT *facet, boolT checkdist); +void qh_printsummary(FILE *fp); + +/********* -user.c prototypes (alphabetical) **********************/ + +void qh_errexit(int exitcode, facetT *facet, ridgeT *ridge); +void qh_errprint(char* string, facetT *atfacet, facetT *otherfacet, ridgeT *atridge, vertexT *atvertex); +int qh_new_qhull (int dim, int numpoints, coordT *points, boolT ismalloc, + char *qhull_cmd, FILE *outfile, FILE *errfile); +void qh_printfacetlist(facetT *facetlist, setT *facets, boolT printall); +void qh_user_memsizes (void); + +/***** -geom.c/geom2.c prototypes (duplicated from geom.h) ****************/ + +facetT *qh_findbest (pointT *point, facetT *startfacet, + boolT bestoutside, boolT newfacets, boolT noupper, + realT *dist, boolT *isoutside, int *numpart); +facetT *qh_findbestnew (pointT *point, facetT *startfacet, + realT *dist, boolT bestoutside, boolT *isoutside, int *numpart); +boolT qh_gram_schmidt(int dim, realT **rows); +void qh_outerinner (facetT *facet, realT *outerplane, realT *innerplane); +void qh_printsummary(FILE *fp); +void qh_projectinput (void); +void qh_randommatrix (realT *buffer, int dim, realT **row); +void qh_rotateinput (realT **rows); +void qh_scaleinput (void); +void qh_setdelaunay (int dim, int count, pointT *points); +coordT *qh_sethalfspace_all (int dim, int count, coordT *halfspaces, pointT *feasible); + +/***** -global.c prototypes (alphabetical) ***********************/ + +unsigned long qh_clock (void); +void qh_checkflags (char *command, char *hiddenflags); +void qh_freebuffers (void); +void qh_freeqhull (boolT allmem); +void qh_init_A (FILE *infile, FILE *outfile, FILE *errfile, int argc, char *argv[]); +void qh_init_B (coordT *points, int numpoints, int dim, boolT ismalloc); +void qh_init_qhull_command (int argc, char *argv[]); +void qh_initbuffers (coordT *points, int numpoints, int dim, boolT ismalloc); +void qh_initflags (char *command); +void qh_initqhull_buffers (void); +void qh_initqhull_globals (coordT *points, int numpoints, int dim, boolT ismalloc); +void qh_initqhull_mem (void); +void qh_initqhull_start (FILE *infile, FILE *outfile, FILE *errfile); +void qh_initthresholds (char *command); +void qh_option (char *option, int *i, realT *r); +#if qh_QHpointer +void qh_restore_qhull (qhT **oldqh); +qhT *qh_save_qhull (void); +#endif + +/***** -io.c prototypes (duplicated from io.h) ***********************/ + +void dfacet( unsigned id); +void dvertex( unsigned id); +void qh_printneighborhood (FILE *fp, int format, facetT *facetA, facetT *facetB, boolT printall); +void qh_produce_output(void); +coordT *qh_readpoints(int *numpoints, int *dimension, boolT *ismalloc); + + +/********* -mem.c prototypes (duplicated from mem.h) **********************/ + +void qh_meminit (FILE *ferr); +void qh_memfreeshort (int *curlong, int *totlong); + +/********* -poly.c/poly2.c prototypes (duplicated from poly.h) **********************/ + +void qh_check_output (void); +void qh_check_points (void); +setT *qh_facetvertices (facetT *facetlist, setT *facets, boolT allfacets); +facetT *qh_findbestfacet (pointT *point, boolT bestoutside, + realT *bestdist, boolT *isoutside); +vertexT *qh_nearvertex (facetT *facet, pointT *point, realT *bestdistp); +pointT *qh_point (int id); +setT *qh_pointfacet (void /*qh.facet_list*/); +int qh_pointid (pointT *point); +setT *qh_pointvertex (void /*qh.facet_list*/); +void qh_setvoronoi_all (void); +void qh_triangulate (void /*qh facet_list*/); + +/********* -stat.c prototypes (duplicated from stat.h) **********************/ + +void qh_collectstatistics (void); +void qh_printallstatistics (FILE *fp, char *string); + +#endif /* qhDEFqhull */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qhull_a.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qhull_a.h new file mode 100644 index 0000000000000000000000000000000000000000..027cafa78380c60f8d170dec25628df6c88b484c --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qhull_a.h @@ -0,0 +1,127 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qhull_a.h + all header files for compiling qhull + + see qh-qhull.htm + + see qhull.h for user-level definitions + + see user.h for user-defineable constants + + defines internal functions for qhull.c global.c + + copyright (c) 1993-2003, The Geometry Center + + Notes: grep for ((" and (" to catch fprintf("lkasdjf"); + full parens around (x?y:z) + use '#include qhull/qhull_a.h' to avoid name clashes +*/ + +#ifndef qhDEFqhulla +#define qhDEFqhulla + +#include <stdio.h> +#include <stdlib.h> +#include <setjmp.h> +#include <string.h> +#include <math.h> +#include <float.h> /* some compilers will not need float.h */ +#include <limits.h> +#include <time.h> +#include <ctype.h> +/*** uncomment here and qset.c + if string.h does not define memcpy() +#include <memory.h> +*/ +#include "qhull.h" +#include "mem.h" +#include "qset.h" +#include "geom.h" +#include "merge.h" +#include "poly.h" +#include "io.h" +#include "stat.h" + +#if qh_CLOCKtype == 2 /* defined in user.h from qhull.h */ +#include <sys/types.h> +#include <sys/times.h> +#include <unistd.h> +#endif + +#ifdef _MSC_VER /* Microsoft Visual C++ */ +#pragma warning( disable : 4056) /* float constant expression. Looks like a compiler bug */ +#pragma warning( disable : 4146) /* unary minus applied to unsigned type */ +#pragma warning( disable : 4244) /* conversion from 'unsigned long' to 'real' */ +#pragma warning( disable : 4305) /* conversion from 'const double' to 'float' */ +#endif + +/* ======= -macros- =========== */ + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="traceN">-</a> + + traceN((fp.ferr, "format\n", vars)); + calls fprintf if qh.IStracing >= N + + notes: + removing tracing reduces code size but doesn't change execution speed +*/ +#ifndef qh_NOtrace +#define trace0(args) {if (qh IStracing) fprintf args;} +#define trace1(args) {if (qh IStracing >= 1) fprintf args;} +#define trace2(args) {if (qh IStracing >= 2) fprintf args;} +#define trace3(args) {if (qh IStracing >= 3) fprintf args;} +#define trace4(args) {if (qh IStracing >= 4) fprintf args;} +#define trace5(args) {if (qh IStracing >= 5) fprintf args;} +#else /* qh_NOtrace */ +#define trace0(args) {} +#define trace1(args) {} +#define trace2(args) {} +#define trace3(args) {} +#define trace4(args) {} +#define trace5(args) {} +#endif /* qh_NOtrace */ + +/***** -qhull.c prototypes (alphabetical after qhull) ********************/ + +void qh_qhull (void); +boolT qh_addpoint (pointT *furthest, facetT *facet, boolT checkdist); +void qh_buildhull(void); +void qh_buildtracing (pointT *furthest, facetT *facet); +void qh_build_withrestart (void); +void qh_errexit2(int exitcode, facetT *facet, facetT *otherfacet); +void qh_findhorizon(pointT *point, facetT *facet, int *goodvisible,int *goodhorizon); +pointT *qh_nextfurthest (facetT **visible); +void qh_partitionall(setT *vertices, pointT *points,int npoints); +void qh_partitioncoplanar (pointT *point, facetT *facet, realT *dist); +void qh_partitionpoint (pointT *point, facetT *facet); +void qh_partitionvisible(boolT allpoints, int *numpoints); +void qh_precision (char *reason); +void qh_printsummary(FILE *fp); + +/***** -global.c internal prototypes (alphabetical) ***********************/ + +void qh_appendprint (qh_PRINT format); +void qh_freebuild (boolT allmem); +void qh_freebuffers (void); +void qh_initbuffers (coordT *points, int numpoints, int dim, boolT ismalloc); +int qh_strtol (const char *s, char **endp); +double qh_strtod (const char *s, char **endp); + +/***** -stat.c internal prototypes (alphabetical) ***********************/ + +void qh_allstatA (void); +void qh_allstatB (void); +void qh_allstatC (void); +void qh_allstatD (void); +void qh_allstatE (void); +void qh_allstatE2 (void); +void qh_allstatF (void); +void qh_allstatG (void); +void qh_allstatH (void); +void qh_freebuffers (void); +void qh_initbuffers (coordT *points, int numpoints, int dim, boolT ismalloc); + +#endif /* qhDEFqhulla */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qset.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qset.h new file mode 100644 index 0000000000000000000000000000000000000000..57524984e2b81840920475f2567ec4c0e0f6456b --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/qset.h @@ -0,0 +1,468 @@ +/*<html><pre> -<a href="qh-set.htm" + >-------------------------------</a><a name="TOP">-</a> + + qset.h + header file for qset.c that implements set + + see qh-set.htm and qset.c + + only uses mem.c, malloc/free + + for error handling, writes message and calls + qh_errexit (qhmem_ERRqhull, NULL, NULL); + + set operations satisfy the following properties: + - sets have a max size, the actual size (if different) is stored at the end + - every set is NULL terminated + - sets may be sorted or unsorted, the caller must distinguish this + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFset +#define qhDEFset 1 + +/*================= -structures- ===============*/ + +#ifndef DEFsetT +#define DEFsetT 1 +typedef struct setT setT; /* a set is a sorted or unsorted array of pointers */ +#endif + +/*-<a href="qh-set.htm#TOC" +>----------------------------------------</a><a name="setT">-</a> + +setT + a set or list of pointers with maximum size and actual size. + +variations: + unsorted, unique -- a list of unique pointers with NULL terminator + user guarantees uniqueness + sorted -- a sorted list of unique pointers with NULL terminator + qset.c guarantees uniqueness + unsorted -- a list of pointers terminated with NULL + indexed -- an array of pointers with NULL elements + +structure for set of n elements: + + -------------- + | maxsize + -------------- + | e[0] - a pointer, may be NULL for indexed sets + -------------- + | e[1] + + -------------- + | ... + -------------- + | e[n-1] + -------------- + | e[n] = NULL + -------------- + | ... + -------------- + | e[maxsize] - n+1 or NULL (determines actual size of set) + -------------- + +*/ + +/*-- setelemT -- internal type to allow both pointers and indices +*/ +typedef union setelemT setelemT; +union setelemT { + void *p; + int i; /* integer used for e[maxSize] */ +}; + +struct setT { + int maxsize; /* maximum number of elements (except NULL) */ + setelemT e[1]; /* array of pointers, tail is NULL */ + /* last slot (unless NULL) is actual size+1 + e[maxsize]==NULL or e[e[maxsize]-1]==NULL */ + /* this may generate a warning since e[] contains + maxsize elements */ +}; + +/*=========== -constants- =========================*/ + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="SETelemsize">-</a> + + SETelemsize + size of a set element in bytes +*/ +#define SETelemsize sizeof(setelemT) + + +/*=========== -macros- =========================*/ + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHsetelement_">-</a> + + FOREACHsetelement_(type, set, variable) + define FOREACH iterator + + declare: + assumes *variable and **variablep are declared + no space in "variable)" [DEC Alpha cc compiler] + + each iteration: + variable is set element + variablep is one beyond variable. + + to repeat an element: + variablep--; / *repeat* / + + at exit: + variable is NULL at end of loop + + example: + #define FOREACHfacet_( facets ) FOREACHsetelement_( facetT, facets, facet ) + + notes: + use FOREACHsetelement_i_() if need index or include NULLs + + WARNING: + nested loops can't use the same variable (define another FOREACH) + + needs braces if nested inside another FOREACH + this includes intervening blocks, e.g. FOREACH...{ if () FOREACH...} ) +*/ +#define FOREACHsetelement_(type, set, variable) \ + if (((variable= NULL), set)) for(\ + variable##p= (type **)&((set)->e[0].p); \ + (variable= *variable##p++);) + +/*-<a href="qh-set.htm#TOC" + >----------------------------------------</a><a name="FOREACHsetelement_i_">-</a> + + FOREACHsetelement_i_(type, set, variable) + define indexed FOREACH iterator + + declare: + type *variable, variable_n, variable_i; + + each iteration: + variable is set element, may be NULL + variable_i is index, variable_n is qh_setsize() + + to repeat an element: + variable_i--; variable_n-- repeats for deleted element + + at exit: + variable==NULL and variable_i==variable_n + + example: + #define FOREACHfacet_i_( facets ) FOREACHsetelement_i_( facetT, facets, facet ) + + WARNING: + nested loops can't use the same variable (define another FOREACH) + + needs braces if nested inside another FOREACH + this includes intervening blocks, e.g. FOREACH...{ if () FOREACH...} ) +*/ +#define FOREACHsetelement_i_(type, set, variable) \ + if (((variable= NULL), set)) for (\ + variable##_i= 0, variable= (type *)((set)->e[0].p), \ + variable##_n= qh_setsize(set);\ + variable##_i < variable##_n;\ + variable= (type *)((set)->e[++variable##_i].p) ) + +/*-<a href="qh-set.htm#TOC" + >--------------------------------------</a><a name="FOREACHsetelementreverse_">-</a> + + FOREACHsetelementreverse_(type, set, variable)- + define FOREACH iterator in reverse order + + declare: + assumes *variable and **variablep are declared + also declare 'int variabletemp' + + each iteration: + variable is set element + + to repeat an element: + variabletemp++; / *repeat* / + + at exit: + variable is NULL + + example: + #define FOREACHvertexreverse_( vertices ) FOREACHsetelementreverse_( vertexT, vertices, vertex ) + + notes: + use FOREACHsetelementreverse12_() to reverse first two elements + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHsetelementreverse_(type, set, variable) \ + if (((variable= NULL), set)) for(\ + variable##temp= qh_setsize(set)-1, variable= qh_setlast(set);\ + variable; variable= \ + ((--variable##temp >= 0) ? SETelemt_(set, variable##temp, type) : NULL)) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHsetelementreverse12_">-</a> + + FOREACHsetelementreverse12_(type, set, variable)- + define FOREACH iterator with e[1] and e[0] reversed + + declare: + assumes *variable and **variablep are declared + + each iteration: + variable is set element + variablep is one after variable. + + to repeat an element: + variablep--; / *repeat* / + + at exit: + variable is NULL at end of loop + + example + #define FOREACHvertexreverse12_( vertices ) FOREACHsetelementreverse12_( vertexT, vertices, vertex ) + + notes: + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHsetelementreverse12_(type, set, variable) \ + if (((variable= NULL), set)) for(\ + variable##p= (type **)&((set)->e[1].p); \ + (variable= *variable##p); \ + variable##p == ((type **)&((set)->e[0].p))?variable##p += 2: \ + (variable##p == ((type **)&((set)->e[1].p))?variable##p--:variable##p++)) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHelem_">-</a> + + FOREACHelem_( set )- + iterate elements in a set + + declare: + void *elem, *elemp; + + each iteration: + elem is set element + elemp is one beyond + + to repeat an element: + elemp--; / *repeat* / + + at exit: + elem == NULL at end of loop + + example: + FOREACHelem_(set) { + + notes: + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHelem_(set) FOREACHsetelement_(void, set, elem) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHset_">-</a> + + FOREACHset_( set )- + iterate a set of sets + + declare: + setT *set, **setp; + + each iteration: + set is set element + setp is one beyond + + to repeat an element: + setp--; / *repeat* / + + at exit: + set == NULL at end of loop + + example + FOREACHset_(sets) { + + notes: + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHset_(sets) FOREACHsetelement_(setT, sets, set) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------------</a><a name="SETindex_">-</a> + + SETindex_( set, elem ) + return index of elem in set + + notes: + for use with FOREACH iteration + + example: + i= SETindex_(ridges, ridge) +*/ +#define SETindex_(set, elem) ((void **)elem##p - (void **)&(set)->e[1].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETref_">-</a> + + SETref_( elem ) + l.h.s. for modifying the current element in a FOREACH iteration + + example: + SETref_(ridge)= anotherridge; +*/ +#define SETref_(elem) (elem##p[-1]) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETelem_">-</a> + + SETelem_(set, n) + return the n'th element of set + + notes: + assumes that n is valid [0..size] and that set is defined + use SETelemt_() for type cast +*/ +#define SETelem_(set, n) ((set)->e[n].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETelemt_">-</a> + + SETelemt_(set, n, type) + return the n'th element of set as a type + + notes: + assumes that n is valid [0..size] and that set is defined +*/ +#define SETelemt_(set, n, type) ((type*)((set)->e[n].p)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETelemaddr_">-</a> + + SETelemaddr_(set, n, type) + return address of the n'th element of a set + + notes: + assumes that n is valid [0..size] and set is defined +*/ +#define SETelemaddr_(set, n, type) ((type **)(&((set)->e[n].p))) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETfirst_">-</a> + + SETfirst_(set) + return first element of set + +*/ +#define SETfirst_(set) ((set)->e[0].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETfirstt_">-</a> + + SETfirstt_(set, type) + return first element of set as a type + +*/ +#define SETfirstt_(set, type) ((type*)((set)->e[0].p)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETsecond_">-</a> + + SETsecond_(set) + return second element of set + +*/ +#define SETsecond_(set) ((set)->e[1].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETsecondt_">-</a> + + SETsecondt_(set, type) + return second element of set as a type +*/ +#define SETsecondt_(set, type) ((type*)((set)->e[1].p)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETaddr_">-</a> + + SETaddr_(set, type) + return address of set's elements +*/ +#define SETaddr_(set,type) ((type **)(&((set)->e[0].p))) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETreturnsize_">-</a> + + SETreturnsize_(set, size) + return size of a set + + notes: + set must be defined + use qh_setsize(set) unless speed is critical +*/ +#define SETreturnsize_(set, size) (((size)= ((set)->e[(set)->maxsize].i))?(--(size)):((size)= (set)->maxsize)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETempty_">-</a> + + SETempty_(set) + return true (1) if set is empty + + notes: + set may be NULL +*/ +#define SETempty_(set) (!set || (SETfirst_(set) ? 0:1)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETtruncate_">-</a> + + SETtruncate_(set) + return first element of set + + see: + qh_settruncate() + +*/ +#define SETtruncate_(set, size) {set->e[set->maxsize].i= size+1; /* maybe overwritten */ \ + set->e[size].p= NULL;} + +/*======= prototypes in alphabetical order ============*/ + +void qh_setaddsorted(setT **setp, void *elem); +void qh_setaddnth(setT **setp, int nth, void *newelem); +void qh_setappend(setT **setp, void *elem); +void qh_setappend_set(setT **setp, setT *setA); +void qh_setappend2ndlast(setT **setp, void *elem); +void qh_setcheck(setT *set, char *tname, int id); +void qh_setcompact(setT *set); +setT *qh_setcopy(setT *set, int extra); +void *qh_setdel(setT *set, void *elem); +void *qh_setdellast(setT *set); +void *qh_setdelnth(setT *set, int nth); +void *qh_setdelnthsorted(setT *set, int nth); +void *qh_setdelsorted(setT *set, void *newelem); +setT *qh_setduplicate( setT *set, int elemsize); +int qh_setequal(setT *setA, setT *setB); +int qh_setequal_except (setT *setA, void *skipelemA, setT *setB, void *skipelemB); +int qh_setequal_skip (setT *setA, int skipA, setT *setB, int skipB); +void qh_setfree(setT **set); +void qh_setfree2( setT **setp, int elemsize); +void qh_setfreelong(setT **set); +int qh_setin(setT *set, void *setelem); +int qh_setindex(setT *set, void *setelem); +void qh_setlarger(setT **setp); +void *qh_setlast(setT *set); +setT *qh_setnew(int size); +setT *qh_setnew_delnthsorted(setT *set, int size, int nth, int prepend); +void qh_setprint(FILE *fp, char* string, setT *set); +void qh_setreplace(setT *set, void *oldelem, void *newelem); +int qh_setsize(setT *set); +setT *qh_settemp(int setsize); +void qh_settempfree(setT **set); +void qh_settempfree_all(void); +setT *qh_settemppop(void); +void qh_settemppush(setT *set); +void qh_settruncate (setT *set, int size); +int qh_setunique (setT **set, void *elem); +void qh_setzero (setT *set, int index, int size); + + +#endif /* qhDEFset */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/stat.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/stat.h new file mode 100644 index 0000000000000000000000000000000000000000..96c4e6035992bfe6205862b16269a4402f6ffe87 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/stat.h @@ -0,0 +1,522 @@ + /*<html><pre> -<a href="qh-stat.htm" + >-------------------------------</a><a name="TOP">-</a> + + stat.h + contains all statistics that are collected for qhull + + see qh-stat.htm and stat.c + + copyright (c) 1993-2003, The Geometry Center + + recompile qhull if you change this file + + Integer statistics are Z* while real statistics are W*. + + define maydebugx to call a routine at every statistic event + +*/ + +#ifndef qhDEFstat +#define qhDEFstat 1 + + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="KEEPstatistics">-</a> + + qh_KEEPstatistics + 0 turns off statistic gathering (except zzdef/zzinc/zzadd/zzval/wwval) +*/ +#ifndef qh_KEEPstatistics +#define qh_KEEPstatistics 1 +#endif + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="statistics">-</a> + + Zxxx for integers, Wxxx for reals + + notes: + be sure that all statistics are defined in stat.c + otherwise initialization may core dump + can pick up all statistics by: + grep '[zw].*_[(][ZW]' *.c >z.x + remove trailers with query">-</a> + remove leaders with query-replace-regexp [ ^I]+ ( +*/ +#if qh_KEEPstatistics +enum statistics { /* alphabetical after Z/W */ + Zacoplanar, + Wacoplanarmax, + Wacoplanartot, + Zangle, + Wangle, + Wanglemax, + Wanglemin, + Zangletests, + Wareatot, + Wareamax, + Wareamin, + Zavoidold, + Wavoidoldmax, + Wavoidoldtot, + Zback0, + Zbestcentrum, + Zbestdist, + Zbestlower, + Zbestlowerv, + Zcentrumtests, + Zcheckpart, + Zcomputefurthest, + Zconcave, + Wconcavemax, + Wconcavetot, + Zconcaveridges, + Zconcaveridge, + Zcoplanar, + Wcoplanarmax, + Wcoplanartot, + Zcoplanarangle, + Zcoplanarcentrum, + Zcoplanarhorizon, + Zcoplanarinside, + Zcoplanarpart, + Zcoplanarridges, + Wcpu, + Zcyclefacetmax, + Zcyclefacettot, + Zcyclehorizon, + Zcyclevertex, + Zdegen, + Wdegenmax, + Wdegentot, + Zdegenvertex, + Zdelfacetdup, + Zdelridge, + Zdelvertextot, + Zdelvertexmax, + Zdetsimplex, + Zdistcheck, + Zdistconvex, + Zdistgood, + Zdistio, + Zdistplane, + Zdiststat, + Zdistvertex, + Zdistzero, + Zdoc1, + Zdoc2, + Zdoc3, + Zdoc4, + Zdoc5, + Zdoc6, + Zdoc7, + Zdoc8, + Zdoc9, + Zdoc10, + Zdoc11, + Zdoc12, + Zdropdegen, + Zdropneighbor, + Zdupflip, + Zduplicate, + Wduplicatemax, + Wduplicatetot, + Zdupridge, + Zdupsame, + Zflipped, + Wflippedmax, + Wflippedtot, + Zflippedfacets, + Zfindbest, + Zfindbestmax, + Zfindbesttot, + Zfindcoplanar, + Zfindfail, + Zfindhorizon, + Zfindhorizonmax, + Zfindhorizontot, + Zfindjump, + Zfindnew, + Zfindnewmax, + Zfindnewtot, + Zfindnewjump, + Zfindnewsharp, + Zgauss0, + Zgoodfacet, + Zhashlookup, + Zhashridge, + Zhashridgetest, + Zhashtests, + Zinsidevisible, + Zintersect, + Zintersectfail, + Zintersectmax, + Zintersectnum, + Zintersecttot, + Zmaxneighbors, + Wmaxout, + Wmaxoutside, + Zmaxridges, + Zmaxvertex, + Zmaxvertices, + Zmaxvneighbors, + Zmemfacets, + Zmempoints, + Zmemridges, + Zmemvertices, + Zmergeflipdup, + Zmergehorizon, + Zmergeinittot, + Zmergeinitmax, + Zmergeinittot2, + Zmergeintohorizon, + Zmergenew, + Zmergesettot, + Zmergesetmax, + Zmergesettot2, + Zmergesimplex, + Zmergevertex, + Wmindenom, + Wminvertex, + Zminnorm, + Zmultiridge, + Znearlysingular, + Zneighbor, + Wnewbalance, + Wnewbalance2, + Znewfacettot, + Znewfacetmax, + Znewvertex, + Wnewvertex, + Wnewvertexmax, + Znoarea, + Znonsimplicial, + Znowsimplicial, + Znotgood, + Znotgoodnew, + Znotmax, + Znumfacets, + Znummergemax, + Znummergetot, + Znumneighbors, + Znumridges, + Znumvertices, + Znumvisibility, + Znumvneighbors, + Zonehorizon, + Zpartangle, + Zpartcoplanar, + Zpartflip, + Zparthorizon, + Zpartinside, + Zpartition, + Zpartitionall, + Zpartnear, + Zpbalance, + Wpbalance, + Wpbalance2, + Zpostfacets, + Zpremergetot, + Zprocessed, + Zremvertex, + Zremvertexdel, + Zrenameall, + Zrenamepinch, + Zrenameshare, + Zretry, + Wretrymax, + Zridge, + Wridge, + Wridgemax, + Zridge0, + Wridge0, + Wridge0max, + Zridgemid, + Wridgemid, + Wridgemidmax, + Zridgeok, + Wridgeok, + Wridgeokmax, + Zsearchpoints, + Zsetplane, + Ztestvneighbor, + Ztotcheck, + Ztothorizon, + Ztotmerge, + Ztotpartcoplanar, + Ztotpartition, + Ztotridges, + Ztotvertices, + Ztotvisible, + Ztricoplanar, + Ztricoplanarmax, + Ztricoplanartot, + Ztridegen, + Ztrimirror, + Ztrinull, + Wvertexmax, + Wvertexmin, + Zvertexridge, + Zvertexridgetot, + Zvertexridgemax, + Zvertices, + Zvisfacettot, + Zvisfacetmax, + Zvisvertextot, + Zvisvertexmax, + Zwidefacet, + Zwidevertices, + ZEND}; + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="ZZstat">-</a> + + Zxxx/Wxxx statistics that remain defined if qh_KEEPstatistics=0 + + notes: + be sure to use zzdef, zzinc, etc. with these statistics (no double checking!) +*/ +#else +enum statistics { /* for zzdef etc. macros */ + Zback0, + Zbestdist, + Zcentrumtests, + Zcheckpart, + Zconcaveridges, + Zcoplanarhorizon, + Zcoplanarpart, + Zcoplanarridges, + Zcyclefacettot, + Zcyclehorizon, + Zdelvertextot, + Zdistcheck, + Zdistconvex, + Zdistzero, + Zdoc1, + Zdoc2, + Zdoc3, + Zdoc11, + Zflippedfacets, + Zgauss0, + Zminnorm, + Zmultiridge, + Znearlysingular, + Wnewvertexmax, + Znumvisibility, + Zpartcoplanar, + Zpartition, + Zpartitionall, + Zprocessed, + Zretry, + Zridge, + Wridge, + Wridgemax, + Zridge0, + Wridge0, + Wridge0max, + Zridgemid, + Wridgemid, + Wridgemidmax, + Zridgeok, + Wridgeok, + Wridgeokmax, + Zsetplane, + Ztotmerge, + ZEND}; +#endif + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="ztype">-</a> + + ztype + the type of a statistic sets its initial value. + + notes: + The type should be the same as the macro for collecting the statistic +*/ +enum ztypes {zdoc,zinc,zadd,zmax,zmin,ZTYPEreal,wadd,wmax,wmin,ZTYPEend}; + +/*========== macros and constants =============*/ + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="MAYdebugx">-</a> + + MAYdebugx + define as maydebug() to be called frequently for error trapping +*/ +#define MAYdebugx + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zdef_">-</a> + + zzdef_, zdef_( type, name, doc, -1) + define a statistic (assumes 'qhstat.next= 0;') + + zdef_( type, name, doc, count) + define an averaged statistic + printed as name/count +*/ +#define zzdef_(stype,name,string,cnt) qhstat id[qhstat next++]=name; \ + qhstat doc[name]= string; qhstat count[name]= cnt; qhstat type[name]= stype +#if qh_KEEPstatistics +#define zdef_(stype,name,string,cnt) qhstat id[qhstat next++]=name; \ + qhstat doc[name]= string; qhstat count[name]= cnt; qhstat type[name]= stype +#else +#define zdef_(type,name,doc,count) +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zinc_">-</a> + + zzinc_( name ), zinc_( name) + increment an integer statistic +*/ +#define zzinc_(id) {MAYdebugx; qhstat stats[id].i++;} +#if qh_KEEPstatistics +#define zinc_(id) {MAYdebugx; qhstat stats[id].i++;} +#else +#define zinc_(id) {} +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zadd_">-</a> + + zzadd_( name, value ), zadd_( name, value ), wadd_( name, value ) + add value to an integer or real statistic +*/ +#define zzadd_(id, val) {MAYdebugx; qhstat stats[id].i += (val);} +#define wwadd_(id, val) {MAYdebugx; qhstat stats[id].r += (val);} +#if qh_KEEPstatistics +#define zadd_(id, val) {MAYdebugx; qhstat stats[id].i += (val);} +#define wadd_(id, val) {MAYdebugx; qhstat stats[id].r += (val);} +#else +#define zadd_(id, val) {} +#define wadd_(id, val) {} +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zval_">-</a> + + zzval_( name ), zval_( name ), wwval_( name ) + set or return value of a statistic +*/ +#define zzval_(id) ((qhstat stats[id]).i) +#define wwval_(id) ((qhstat stats[id]).r) +#if qh_KEEPstatistics +#define zval_(id) ((qhstat stats[id]).i) +#define wval_(id) ((qhstat stats[id]).r) +#else +#define zval_(id) qhstat tempi +#define wval_(id) qhstat tempr +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zmax_">-</a> + + zmax_( id, val ), wmax_( id, value ) + maximize id with val +*/ +#define wwmax_(id, val) {MAYdebugx; maximize_(qhstat stats[id].r,(val));} +#if qh_KEEPstatistics +#define zmax_(id, val) {MAYdebugx; maximize_(qhstat stats[id].i,(val));} +#define wmax_(id, val) {MAYdebugx; maximize_(qhstat stats[id].r,(val));} +#else +#define zmax_(id, val) {} +#define wmax_(id, val) {} +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zmin_">-</a> + + zmin_( id, val ), wmin_( id, value ) + minimize id with val +*/ +#if qh_KEEPstatistics +#define zmin_(id, val) {MAYdebugx; minimize_(qhstat stats[id].i,(val));} +#define wmin_(id, val) {MAYdebugx; minimize_(qhstat stats[id].r,(val));} +#else +#define zmin_(id, val) {} +#define wmin_(id, val) {} +#endif + +/*================== stat.h types ==============*/ + + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="intrealT">-</a> + + intrealT + union of integer and real, used for statistics +*/ +typedef union intrealT intrealT; /* union of int and realT */ +union intrealT { + int i; + realT r; +}; + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="qhstat">-</a> + + qhstat + global data structure for statistics + + notes: + access to qh_qhstat is via the "qhstat" macro. There are two choices + qh_QHpointer = 1 access globals via a pointer + enables qh_saveqhull() and qh_restoreqhull() + = 0 qh_qhstat is a static data structure + only one instance of qhull() can be active at a time + default value + qh_QHpointer is defined in qhull.h + + allocated in stat.c +*/ +typedef struct qhstatT qhstatT; +#if qh_QHpointer +#define qhstat qh_qhstat-> +extern qhstatT *qh_qhstat; +#else +#define qhstat qh_qhstat. +extern qhstatT qh_qhstat; +#endif +struct qhstatT { + intrealT stats[ZEND]; /* integer and real statistics */ + unsigned char id[ZEND+10]; /* id's in print order */ + char *doc[ZEND]; /* array of documentation strings */ + short int count[ZEND]; /* -1 if none, else index of count to use */ + char type[ZEND]; /* type, see ztypes above */ + char printed[ZEND]; /* true, if statistic has been printed */ + intrealT init[ZTYPEend]; /* initial values by types, set initstatistics */ + + int next; /* next index for zdef_ */ + int precision; /* index for precision problems */ + int vridges; /* index for Voronoi ridges */ + int tempi; + realT tempr; +}; + +/*========== function prototypes ===========*/ + +void qh_allstatA(void); +void qh_allstatB(void); +void qh_allstatC(void); +void qh_allstatD(void); +void qh_allstatE(void); +void qh_allstatE2(void); +void qh_allstatF(void); +void qh_allstatG(void); +void qh_allstatH(void); +void qh_allstatI(void); +void qh_allstatistics (void); +void qh_collectstatistics (void); +void qh_freestatistics (void); +void qh_initstatistics (void); +boolT qh_newstats (int index, int *nextindex); +boolT qh_nostatistic (int i); +void qh_printallstatistics (FILE *fp, char *string); +void qh_printstatistics (FILE *fp, char *string); +void qh_printstatlevel (FILE *fp, int id, int start); +void qh_printstats (FILE *fp, int index, int *nextindex); +realT qh_stddev (int num, realT tot, realT tot2, realT *ave); + +#endif /* qhDEFstat */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/user.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/user.h new file mode 100644 index 0000000000000000000000000000000000000000..79558967a52a5555028d9a16e2cf856c8d3f45e7 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/include/qhull/user.h @@ -0,0 +1,762 @@ +/*<html><pre> -<a href="qh-user.htm" + >-------------------------------</a><a name="TOP">-</a> + + user.h + user redefinable constants + + see qh-user.htm. see COPYING for copyright information. + + before reading any code, review qhull.h for data structure definitions and + the "qh" macro. +*/ + +#ifndef qhDEFuser +#define qhDEFuser 1 + +/*============= data types and configuration macros ==========*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="realT">-</a> + + realT + set the size of floating point numbers + + qh_REALdigits + maximimum number of significant digits + + qh_REAL_1, qh_REAL_2n, qh_REAL_3n + format strings for printf + + qh_REALmax, qh_REALmin + maximum and minimum (near zero) values + + qh_REALepsilon + machine roundoff. Maximum roundoff error for addition and multiplication. + + notes: + Select whether to store floating point numbers in single precision (float) + or double precision (double). + + Use 'float' to save about 8% in time and 25% in space. This is particularly + help if high-d where convex hulls are space limited. Using 'float' also + reduces the printed size of Qhull's output since numbers have 8 digits of + precision. + + Use 'double' when greater arithmetic precision is needed. This is needed + for Delaunay triangulations and Voronoi diagrams when you are not merging + facets. + + If 'double' gives insufficient precision, your data probably includes + degeneracies. If so you should use facet merging (done by default) + or exact arithmetic (see imprecision section of manual, qh-impre.htm). + You may also use option 'Po' to force output despite precision errors. + + You may use 'long double', but many format statements need to be changed + and you may need a 'long double' square root routine. S. Grundmann + (sg@eeiwzb.et.tu-dresden.de) has done this. He reports that the code runs + much slower with little gain in precision. + + WARNING: on some machines, int f(){realT a= REALmax;return (a == REALmax);} + returns False. Use (a > REALmax/2) instead of (a == REALmax). + + REALfloat = 1 all numbers are 'float' type + = 0 all numbers are 'double' type +*/ +#define REALfloat 0 + +#if (REALfloat == 1) +#define realT float +#define REALmax FLT_MAX +#define REALmin FLT_MIN +#define REALepsilon FLT_EPSILON +#define qh_REALdigits 8 /* maximum number of significant digits */ +#define qh_REAL_1 "%6.8g " +#define qh_REAL_2n "%6.8g %6.8g\n" +#define qh_REAL_3n "%6.8g %6.8g %6.8g\n" + +#elif (REALfloat == 0) +#define realT double +#define REALmax DBL_MAX +#define REALmin DBL_MIN +#define REALepsilon DBL_EPSILON +#define qh_REALdigits 16 /* maximum number of significant digits */ +#define qh_REAL_1 "%6.16g " +#define qh_REAL_2n "%6.16g %6.16g\n" +#define qh_REAL_3n "%6.16g %6.16g %6.16g\n" + +#else +#error unknown float option +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="CPUclock">-</a> + + qh_CPUclock + define the clock() function for reporting the total time spent by Qhull + returns CPU ticks as a 'long int' + qh_CPUclock is only used for reporting the total time spent by Qhull + + qh_SECticks + the number of clock ticks per second + + notes: + looks for CLOCKS_PER_SEC, CLOCKS_PER_SECOND, or assumes microseconds + to define a custom clock, set qh_CLOCKtype to 0 + + if your system does not use clock() to return CPU ticks, replace + qh_CPUclock with the corresponding function. It is converted + to unsigned long to prevent wrap-around during long runs. + + + Set qh_CLOCKtype to + + 1 for CLOCKS_PER_SEC, CLOCKS_PER_SECOND, or microsecond + Note: may fail if more than 1 hour elapsed time + + 2 use qh_clock() with POSIX times() (see global.c) +*/ +#define qh_CLOCKtype 1 /* change to the desired number */ + +#if (qh_CLOCKtype == 1) + +#if defined (CLOCKS_PER_SECOND) +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks CLOCKS_PER_SECOND + +#elif defined (CLOCKS_PER_SEC) +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks CLOCKS_PER_SEC + +#elif defined (CLK_TCK) +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks CLK_TCK + +#else +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks 1E6 +#endif + +#elif (qh_CLOCKtype == 2) +#define qh_CPUclock qh_clock() /* return CPU clock */ +#define qh_SECticks 100 + +#else /* qh_CLOCKtype == ? */ +#error unknown clock option +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="RANDOM">-</a> + + qh_RANDOMtype, qh_RANDOMmax, qh_RANDOMseed + define random number generator + + qh_RANDOMint generates a random integer between 0 and qh_RANDOMmax. + qh_RANDOMseed sets the random number seed for qh_RANDOMint + + Set qh_RANDOMtype (default 5) to: + 1 for random() with 31 bits (UCB) + 2 for rand() with RAND_MAX or 15 bits (system 5) + 3 for rand() with 31 bits (Sun) + 4 for lrand48() with 31 bits (Solaris) + 5 for qh_rand() with 31 bits (included with Qhull) + + notes: + Random numbers are used by rbox to generate point sets. Random + numbers are used by Qhull to rotate the input ('QRn' option), + simulate a randomized algorithm ('Qr' option), and to simulate + roundoff errors ('Rn' option). + + Random number generators differ between systems. Most systems provide + rand() but the period varies. The period of rand() is not critical + since qhull does not normally use random numbers. + + The default generator is Park & Miller's minimal standard random + number generator [CACM 31:1195 '88]. It is included with Qhull. + + If qh_RANDOMmax is wrong, qhull will report a warning and Geomview + output will likely be invisible. +*/ +#define qh_RANDOMtype 5 /* *** change to the desired number *** */ + +#if (qh_RANDOMtype == 1) +#define qh_RANDOMmax ((realT)0x7fffffffUL) /* 31 bits, random()/MAX */ +#define qh_RANDOMint random() +#define qh_RANDOMseed_(seed) srandom(seed); + +#elif (qh_RANDOMtype == 2) +#ifdef RAND_MAX +#define qh_RANDOMmax ((realT)RAND_MAX) +#else +#define qh_RANDOMmax ((realT)32767) /* 15 bits (System 5) */ +#endif +#define qh_RANDOMint rand() +#define qh_RANDOMseed_(seed) srand((unsigned)seed); + +#elif (qh_RANDOMtype == 3) +#define qh_RANDOMmax ((realT)0x7fffffffUL) /* 31 bits, Sun */ +#define qh_RANDOMint rand() +#define qh_RANDOMseed_(seed) srand((unsigned)seed); + +#elif (qh_RANDOMtype == 4) +#define qh_RANDOMmax ((realT)0x7fffffffUL) /* 31 bits, lrand38()/MAX */ +#define qh_RANDOMint lrand48() +#define qh_RANDOMseed_(seed) srand48(seed); + +#elif (qh_RANDOMtype == 5) +#define qh_RANDOMmax ((realT)2147483646UL) /* 31 bits, qh_rand/MAX */ +#define qh_RANDOMint qh_rand() +#define qh_RANDOMseed_(seed) qh_srand(seed); +/* unlike rand(), never returns 0 */ + +#else +#error: unknown random option +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="ORIENTclock">-</a> + + qh_ORIENTclock + 0 for inward pointing normals by Geomview convention +*/ +#define qh_ORIENTclock 0 + + +/*========= performance related constants =========*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="HASHfactor">-</a> + + qh_HASHfactor + total hash slots / used hash slots. Must be at least 1.1. + + notes: + =2 for at worst 50% occupancy for qh hash_table and normally 25% occupancy +*/ +#define qh_HASHfactor 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="VERIFYdirect">-</a> + + qh_VERIFYdirect + with 'Tv' verify all points against all facets if op count is smaller + + notes: + if greater, calls qh_check_bestdist() instead +*/ +#define qh_VERIFYdirect 1000000 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="INITIALsearch">-</a> + + qh_INITIALsearch + if qh_INITIALmax, search points up to this dimension +*/ +#define qh_INITIALsearch 6 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="INITIALmax">-</a> + + qh_INITIALmax + if dim >= qh_INITIALmax, use min/max coordinate points for initial simplex + + notes: + from points with non-zero determinants + use option 'Qs' to override (much slower) +*/ +#define qh_INITIALmax 8 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEdefault">-</a> + + qh_JOGGLEdefault + default qh.JOGGLEmax is qh.DISTround * qh_JOGGLEdefault + + notes: + rbox s r 100 | qhull QJ1e-15 QR0 generates 90% faults at distround 7e-16 + rbox s r 100 | qhull QJ1e-14 QR0 generates 70% faults + rbox s r 100 | qhull QJ1e-13 QR0 generates 35% faults + rbox s r 100 | qhull QJ1e-12 QR0 generates 8% faults + rbox s r 100 | qhull QJ1e-11 QR0 generates 1% faults + rbox s r 100 | qhull QJ1e-10 QR0 generates 0% faults + rbox 1000 W0 | qhull QJ1e-12 QR0 generates 86% faults + rbox 1000 W0 | qhull QJ1e-11 QR0 generates 20% faults + rbox 1000 W0 | qhull QJ1e-10 QR0 generates 2% faults + the later have about 20 points per facet, each of which may interfere + + pick a value large enough to avoid retries on most inputs +*/ +#define qh_JOGGLEdefault 30000.0 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEincrease">-</a> + + qh_JOGGLEincrease + factor to increase qh.JOGGLEmax on qh_JOGGLEretry or qh_JOGGLEagain +*/ +#define qh_JOGGLEincrease 10.0 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEretry">-</a> + + qh_JOGGLEretry + if ZZretry = qh_JOGGLEretry, increase qh.JOGGLEmax + + notes: + try twice at the original value in case of bad luck the first time +*/ +#define qh_JOGGLEretry 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEagain">-</a> + + qh_JOGGLEagain + every following qh_JOGGLEagain, increase qh.JOGGLEmax + + notes: + 1 is OK since it's already failed qh_JOGGLEretry times +*/ +#define qh_JOGGLEagain 1 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEmaxincrease">-</a> + + qh_JOGGLEmaxincrease + maximum qh.JOGGLEmax due to qh_JOGGLEincrease + relative to qh.MAXwidth + + notes: + qh.joggleinput will retry at this value until qh_JOGGLEmaxretry +*/ +#define qh_JOGGLEmaxincrease 1e-2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEmaxretry">-</a> + + qh_JOGGLEmaxretry + stop after qh_JOGGLEmaxretry attempts +*/ +#define qh_JOGGLEmaxretry 100 + +/*========= memory constants =========*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MEMalign">-</a> + + qh_MEMalign + memory alignment for qh_meminitbuffers() in global.c + + notes: + to avoid bus errors, memory allocation must consider alignment requirements. + malloc() automatically takes care of alignment. Since mem.c manages + its own memory, we need to explicitly specify alignment in + qh_meminitbuffers(). + + A safe choice is sizeof(double). sizeof(float) may be used if doubles + do not occur in data structures and pointers are the same size. Be careful + of machines (e.g., DEC Alpha) with large pointers. + + If using gcc, best alignment is + #define qh_MEMalign fmax_(__alignof__(realT),__alignof__(void *)) +*/ +#define qh_MEMalign fmax_(sizeof(realT), sizeof(void *)) + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MEMbufsize">-</a> + + qh_MEMbufsize + size of additional memory buffers + + notes: + used for qh_meminitbuffers() in global.c +*/ +#define qh_MEMbufsize 0x10000 /* allocate 64K memory buffers */ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MEMinitbuf">-</a> + + qh_MEMinitbuf + size of initial memory buffer + + notes: + use for qh_meminitbuffers() in global.c +*/ +#define qh_MEMinitbuf 0x20000 /* initially allocate 128K buffer */ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="INFINITE">-</a> + + qh_INFINITE + on output, indicates Voronoi center at infinity +*/ +#define qh_INFINITE -10.101 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DEFAULTbox">-</a> + + qh_DEFAULTbox + default box size (Geomview expects 0.5) +*/ +#define qh_DEFAULTbox 0.5 + +/*======= conditional compilation ============================*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="compiler">-</a> + + __cplusplus + defined by C++ compilers + + __MSC_VER + defined by Microsoft Visual C++ + + __MWERKS__ && __POWERPC__ + defined by Metrowerks when compiling for the Power Macintosh + + __STDC__ + defined for strict ANSI C +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="COMPUTEfurthest">-</a> + + qh_COMPUTEfurthest + compute furthest distance to an outside point instead of storing it with the facet + =1 to compute furthest + + notes: + computing furthest saves memory but costs time + about 40% more distance tests for partitioning + removes facet->furthestdist +*/ +#define qh_COMPUTEfurthest 0 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="KEEPstatistics">-</a> + + qh_KEEPstatistics + =0 removes most of statistic gathering and reporting + + notes: + if 0, code size is reduced by about 4%. +*/ +#define qh_KEEPstatistics 1 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXoutside">-</a> + + qh_MAXoutside + record outer plane for each facet + =1 to record facet->maxoutside + + notes: + this takes a realT per facet and slightly slows down qhull + it produces better outer planes for geomview output +*/ +#define qh_MAXoutside 1 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="NOmerge">-</a> + + qh_NOmerge + disables facet merging if defined + + notes: + This saves about 10% space. + + Unless 'Q0' + qh_NOmerge sets 'QJ' to avoid precision errors + + #define qh_NOmerge + + see: + <a href="mem.h#NOmem">qh_NOmem</a> in mem.c + + see user.c/user_eg.c for removing io.o +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="NOtrace">-</a> + + qh_NOtrace + no tracing if defined + + notes: + This saves about 5% space. + + #define qh_NOtrace +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="QHpointer">-</a> + + qh_QHpointer + access global data with pointer or static structure + + qh_QHpointer = 1 access globals via a pointer to allocated memory + enables qh_saveqhull() and qh_restoreqhull() + costs about 8% in time and 2% in space + + = 0 qh_qh and qh_qhstat are static data structures + only one instance of qhull() can be active at a time + default value + + notes: + all global variables for qhull are in qh, qhmem, and qhstat + qh is defined in qhull.h + qhmem is defined in mem.h + qhstat is defined in stat.h + + see: + user_eg.c for an example +*/ +#define qh_QHpointer 0 +#if 0 /* sample code */ + qhT *oldqhA, *oldqhB; + + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, + flags, outfile, errfile); + /* use results from first call to qh_new_qhull */ + oldqhA= qh_save_qhull(); + exitcode= qh_new_qhull (dimB, numpointsB, pointsB, ismalloc, + flags, outfile, errfile); + /* use results from second call to qh_new_qhull */ + oldqhB= qh_save_qhull(); + qh_restore_qhull (&oldqhA); + /* use results from first call to qh_new_qhull */ + qh_freeqhull (qh_ALL); /* frees all memory used by first call */ + qh_restore_qhull (&oldqhB); + /* use results from second call to qh_new_qhull */ + qh_freeqhull (!qh_ALL); /* frees long memory used by second call */ + qh_memfreeshort (&curlong, &totlong); /* frees short memory and memory allocator */ +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="QUICKhelp">-</a> + + qh_QUICKhelp + =1 to use abbreviated help messages, e.g., for degenerate inputs +*/ +#define qh_QUICKhelp 0 + +/* ============ -merge constants- ==================== + + These constants effect facet merging. You probably will not need + to modify these. They effect the performance of facet merging. +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DIMmergeVertex">-</a> + + qh_DIMmergeVertex + max dimension for vertex merging (it is not effective in high-d) +*/ +#define qh_DIMmergeVertex 6 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DIMreduceBuild">-</a> + + qh_DIMreduceBuild + max dimension for vertex reduction during build (slow in high-d) +*/ +#define qh_DIMreduceBuild 5 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="BESTcentrum">-</a> + + qh_BESTcentrum + if > 2*dim+n vertices, qh_findbestneighbor() tests centrums (faster) + else, qh_findbestneighbor() tests all vertices (much better merges) + + qh_BESTcentrum2 + if qh_BESTcentrum2 * DIM3 + BESTcentrum < #vertices tests centrums +*/ +#define qh_BESTcentrum 20 +#define qh_BESTcentrum2 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="BESTnonconvex">-</a> + + qh_BESTnonconvex + if > dim+n neighbors, qh_findbestneighbor() tests nonconvex ridges. + + notes: + It is needed because qh_findbestneighbor is slow for large facets +*/ +#define qh_BESTnonconvex 15 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXnewmerges">-</a> + + qh_MAXnewmerges + if >n newmerges, qh_merge_nonconvex() calls qh_reducevertices_centrums. + + notes: + It is needed because postmerge can merge many facets at once +*/ +#define qh_MAXnewmerges 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXnewcentrum">-</a> + + qh_MAXnewcentrum + if <= dim+n vertices (n approximates the number of merges), + reset the centrum in qh_updatetested() and qh_mergecycle_facets() + + notes: + needed to reduce cost and because centrums may move too much if + many vertices in high-d +*/ +#define qh_MAXnewcentrum 5 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="COPLANARratio">-</a> + + qh_COPLANARratio + for 3-d+ merging, qh.MINvisible is n*premerge_centrum + + notes: + for non-merging, it's DISTround +*/ +#define qh_COPLANARratio 3 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DISToutside">-</a> + + qh_DISToutside + When is a point clearly outside of a facet? + Stops search in qh_findbestnew or qh_partitionall + qh_findbest uses qh.MINoutside since since it is only called if no merges. + + notes: + 'Qf' always searches for best facet + if !qh.MERGING, same as qh.MINoutside. + if qh_USEfindbestnew, increase value since neighboring facets may be ill-behaved + [Note: Zdelvertextot occurs normally with interior points] + RBOX 1000 s Z1 G1e-13 t1001188774 | QHULL Tv + When there is a sharp edge, need to move points to a + clearly good facet; otherwise may be lost in another partitioning. + if too big then O(n^2) behavior for partitioning in cone + if very small then important points not processed + Needed in qh_partitionall for + RBOX 1000 s Z1 G1e-13 t1001032651 | QHULL Tv + Needed in qh_findbestnew for many instances of + RBOX 1000 s Z1 G1e-13 t | QHULL Tv + + See: + qh_DISToutside -- when is a point clearly outside of a facet + qh_SEARCHdist -- when is facet coplanar with the best facet? + qh_USEfindbestnew -- when to use qh_findbestnew for qh_partitionpoint() +*/ +#define qh_DISToutside ((qh_USEfindbestnew ? 2 : 1) * \ + fmax_((qh MERGING ? 2 : 1)*qh MINoutside, qh max_outside)) + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="RATIOnearinside">-</a> + + qh_RATIOnearinside + ratio of qh.NEARinside to qh.ONEmerge for retaining inside points for + qh_check_maxout(). + + notes: + This is overkill since do not know the correct value. + It effects whether 'Qc' reports all coplanar points + Not used for 'd' since non-extreme points are coplanar +*/ +#define qh_RATIOnearinside 5 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="SEARCHdist">-</a> + + qh_SEARCHdist + When is a facet coplanar with the best facet? + qh_findbesthorizon: all coplanar facets of the best facet need to be searched. + + See: + qh_DISToutside -- when is a point clearly outside of a facet + qh_SEARCHdist -- when is facet coplanar with the best facet? + qh_USEfindbestnew -- when to use qh_findbestnew for qh_partitionpoint() +*/ +#define qh_SEARCHdist ((qh_USEfindbestnew ? 2 : 1) * \ + (qh max_outside + 2 * qh DISTround + fmax_( qh MINvisible, qh MAXcoplanar))); + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="USEfindbestnew">-</a> + + qh_USEfindbestnew + Always use qh_findbestnew for qh_partitionpoint, otherwise use + qh_findbestnew if merged new facet or sharpnewfacets. + + See: + qh_DISToutside -- when is a point clearly outside of a facet + qh_SEARCHdist -- when is facet coplanar with the best facet? + qh_USEfindbestnew -- when to use qh_findbestnew for qh_partitionpoint() +*/ +#define qh_USEfindbestnew (zzval_(Ztotmerge) > 50) + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="WIDEcoplanar">-</a> + + qh_WIDEcoplanar + n*MAXcoplanar or n*MINvisible for a WIDEfacet + + if vertex is further than qh.WIDEfacet from the hyperplane + then its ridges are not counted in computing the area, and + the facet's centrum is frozen. + + notes: + qh.WIDEfacet= max(qh.MAXoutside,qh_WIDEcoplanar*qh.MAXcoplanar, + qh_WIDEcoplanar * qh.MINvisible); +*/ +#define qh_WIDEcoplanar 6 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXnarrow">-</a> + + qh_MAXnarrow + max. cosine in initial hull that sets qh.NARROWhull + + notes: + If qh.NARROWhull, the initial partition does not make + coplanar points. If narrow, a coplanar point can be + coplanar to two facets of opposite orientations and + distant from the exact convex hull. + + Conservative estimate. Don't actually see problems until it is -1.0 +*/ +#define qh_MAXnarrow -0.99999999 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="WARNnarrow">-</a> + + qh_WARNnarrow + max. cosine in initial hull to warn about qh.NARROWhull + + notes: + this is a conservative estimate. + Don't actually see problems until it is -1.0. See qh-impre.htm +*/ +#define qh_WARNnarrow -0.999999999999999 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="ZEROdelaunay">-</a> + + qh_ZEROdelaunay + a zero Delaunay facet occurs for input sites coplanar with their convex hull + the last normal coefficient of a zero Delaunay facet is within + qh_ZEROdelaunay * qh.ANGLEround of 0 + + notes: + qh_ZEROdelaunay does not allow for joggled input ('QJ'). + + You can avoid zero Delaunay facets by surrounding the input with a box. + + Use option 'PDk:-n' to explicitly define zero Delaunay facets + k= dimension of input sites (e.g., 3 for 3-d Delaunay triangulation) + n= the cutoff for zero Delaunay facets (e.g., 'PD3:-1e-12') +*/ +#define qh_ZEROdelaunay 2 + +#endif /* qh_DEFuser */ + + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/index.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/index.htm new file mode 100644 index 0000000000000000000000000000000000000000..b00b715c793df145828d49a037b79cb2fd0283b0 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/index.htm @@ -0,0 +1,246 @@ +<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN"> +<html> + +<head> +<title>Qhull for Convex Hull, Delaunay Triangulation, Voronoi Diagram, and Halfspace Intersection about a Point</title> +</head> + +<body> +<!-- Navigation links --> +<b>URL:</b> <a href="http://www.qhull.org">http://www.qhull.org</a> +<br><b>To:</b> +<a href="http://www.qhull.org/news">News</a> +• <a href="http://www.qhull.org/download">Download</a> +• <a href="http://citeseer.nj.nec.com/83502.html">CiteSeer</a> +• <a href=http://images.google.com/images?q=qhull&num=100>Images</a> +• <a href="html/index.htm#TOC">Manual</a> +• <a href="http://www.qhull.org/html/qh-faq.htm">FAQ</a> +• <a href="html/qh-quick.htm#programs">Programs</a> +• <a href="html/qh-quick.htm#options">Options</a> +</p> + +<hr> +<!-- Main text of document --> +<table> +<tr><td valign=top> + <h1>Qhull</h1> + <a + href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/cone.html"><img + src="html/qh--cone.gif" alt="[CONE]" align="middle" width="100" + height="100"></a> +</td><td> +Qhull computes convex hulls, Delaunay triangulations, +halfspace intersections about a point, Voronoi diagrams, furthest-site Delaunay +triangulations, and furthest-site Voronoi diagrams. It runs in +2-d, 3-d, 4-d, and higher dimensions. It implements the Quickhull +algorithm for computing the convex hull. Qhull handles roundoff +errors from floating point arithmetic. It computes volumes, +surface areas, and approximations to the convex hull.</p> + +<!-- duplicated in index.htm and html/index.htm --> +<p>Qhull does <i>not</i> support constrained Delaunay +triangulations, triangulation of non-convex surfaces, mesh +generation of non-convex objects, or medium-sized inputs in 9-D +and higher. </p> +</td></tr></table> + +<hr> +<ul> + <li><a href="http://www.qhull.org/news">News</a> about Qhull </li> + <li><a href="http://www.qhull.org/download">Download</a> + Qhull</li> + <li><a + href="http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/welcome.html">Examples + </a>of Qhull output </li> + <li><a href=http://savannah.gnu.org/projects/qhull/>Qhull development</a> at Savannah + <p> + <li><a href="http://www.qhull.org/news/qhull-news.html#use">How</a> is Qhull used?</li> + <li><a href="http://citeseer.nj.nec.com/context/86585/83502">CiteSeer</a> references to Qhull +</p> + <li><a href=http://www.google.com/search?as_q=qhull&num=100>Google</a> Qhull, + Qhull <a href=http://images.google.com/images?q=qhull&num=100>Images</a>, + Qhull in <a href=http://groups.google.com/groups?as_q=qhull&num=100&as_scoring=d>Newsgroups</a>, + and <a href=http://www.googlism.com/who_is/q/qhull/>Who is</a> Qhull? + + <p> + <li><a href=http://www.mathworks.com/>MATLAB</a> uses Qhull for their computational geometry functions: <a href=http://www.mathworks.com/access/helpdesk/help/techdoc/ref/convhulln.shtml>convhulln</a> + <a href=http://www.mathworks.com/access/helpdesk/help/techdoc/ref/delaunayn.shtml>delaunayn</a> + <a href=http://www.mathworks.com/access/helpdesk/help/techdoc/ref/griddata3.shtml>griddata3</a> + <a href=http://www.mathworks.com/access/helpdesk/help/techdoc/ref/griddatan.shtml>griddatan</a> + <a href=http://www.mathworks.com/access/helpdesk/help/techdoc/ref/tsearch.shtml>tsearch</a> + <a href=http://www.mathworks.com/access/helpdesk/help/techdoc/ref/tsearchn.shtml>tsearchn</a> + <a href=http://www.mathworks.com/access/helpdesk/help/techdoc/ref/voronoin.shtml>voronoin</a>. MATLAB R14 will upgrade to + Qhull 2003.1 and triangulated output ('Qt'). + </li> + <li><a href=http://www.octave.org/>GNU Octave</a> uses Qhull for their + <a href=http://octave.sourceforge.net/index/analysis.html#Geometry>computational geometry functions</a>. Octave has partially upgraded + to Qhull 2002.1 and triangulated output ('Qt'). + <li><a href=http://www.wolfram.com/products/mathematica/>Mathematica</a>'s Delaunay interface: <a href=http://www.mathsource.com/Content/Enhancements/Geometry/0211-251>qh-math</a> + <li><a href=http://www.geomview.org>Geomview</a> for 3-D and 4-D visualization of Qhull output +</ul> + +<p><b>Introduction</b> +<ul> + <li><a + href="http://www.ifor.math.ethz.ch/staff/fukuda/polyfaq/polyfaq.html">Fukuda's + introduction</a> to convex hulls, Delaunay + triangulations, Voronoi diagrams, and linear programming</li> + <li><a + href="http://www.cse.unsw.edu.au/~lambert/java/3d/hull.html">Lambert's + Java</a> visualization of convex hull algorithms </li> + <li><a + href="http://www.cs.sunysb.edu/~algorith/major_section/1.6.shtml">Stony + Brook</a> Algorithm Repository, computational geometry</li> +</ul> + +<p><b>Qhull Documentation and Support</b> +<ul> + <li><a href="html/index.htm">Manual</a> for Qhull and rbox + <ul> + <li><a href="html/qconvex.htm">qconvex</a> -- convex hull + <li><a href="html/qdelaun.htm">qdelaunay</a> -- Delaunay triangulation + <li><a href="html/qvoronoi.htm">qvoronoi</a> -- Voronoi diagram + <li><a href="html/qhalf.htm">qhalf</a> -- halfspace intersection about a point + <li><a href="html/rbox.htm">rbox</a> -- generate point distributions + <li><a href=src/index.htm>Qhull functions</a>, macros, and data structures with source + </ul> + <li><a href="http://www.qhull.org/html/qh-faq.htm">Frequently</a> asked + questions about Qhull</li> + <li>Send e-mail to <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> </li> + <li>Report bugs to <a + href="mailto:qhull_bug@qhull.org">qhull_bug@qhull.org</a> +</ul> + +<p><b>Related URLs</b> +<ul> + <li><a href="http://www.geom.uiuc.edu/software/cglist">Amenta's directory</a> of + computational geometry software </li> + + <li><a href=http://www.boost.org/libs/graph/doc/table_of_contents.html>BGL</a> + Boost Graph Library provides C++ classes for graph data structures +and algorithms, + <li><a + href="http://netlib.bell-labs.com/netlib/voronoi/hull.html">Clarkson's + hull </a>program with exact arithmetic for convex hulls, Delaunay triangulations, + Voronoi volumes, and alpha shapes. </li> + <li><a href="http://compgeom.cs.uiuc.edu/~jeffe/compgeom/compgeom.html">Erickson's + Computational</a> Geometry Pages and + <a href="http://compgeom.cs.uiuc.edu/~jeffe/compgeom/code.html">Software</a> + <li><a + href="http://www.cs.mcgill.ca/~fukuda/soft/cdd_home/cdd.html">Fukuda's + cdd</a> program for halfspace intersection and convex hulls</li> + <li><a href="http://www.inf.ethz.ch/personal/gaertner/miniball.html">Gartner's + Miniball</a> for fast and robust smallest enclosing balls (up to 20-d) + + <li><a href=http://directory.google.com/Top/Science/Math/Geometry/Computational_Geometry/Software/>Google's directory</a> for + Science > Math > Geometry > Computational Geometry > Software + <li><a href=http://www.algorithmic-solutions.com/enleda.htm>Leda</a> +and <a href=http://www.cgal.org/>CGAL</a> libraries for writing computational +geometry programs and other combinatorial algorithms + <li><a href=http://www.magic-software.com>Magic Software</a> source code for computer + graphics, image analysis, and numerical methods + <li><a href=http://www.mathtools.net/>Mathtools.net</a> of scientific and engineering + software + <li><a href="http://www.andrew.cmu.edu/user/sowen/mesh.html">Owen's Meshing</a> Research Corner + <li><a + href="http://www-users.informatik.rwth-aachen.de/~roberts/meshgeneration.html">Schneiders' + Finite Element</a> Mesh Generation page</li> + <li><a href="http://www.cs.cmu.edu/~quake/triangle.html">Shewchuk's + triangle </a>program for 2-d Delaunay</li> + <li><a href=ftp://ftp.zib.de/pub/Packages/mathprog/index.html>Skorobohatyj's Mathprog@ZIB</a> for + mathematical software + <li><a href=http://www.voronoi.com>Voronoi Web Site</a> for all things Voronoi +</ul> + +<p><b>FAQs and Newsgroups</b> +<ul> + <li><a + href="http://exaflop.org/docs/cgafaq/">FAQ</a> + for computer graphics algorithms + (<a href="http://exaflop.org/docs/cgafaq/cga6.html">geometric</a> structures) + </li> + <li><a + href="http://www-unix.mcs.anl.gov/otc/Guide/faq/linear-programming-faq.html">FAQ + </a>for linear programming </li> + <li><a href="news:comp.graphics.algorithms">Newsgroup</a>: + comp.graphics.algorithms </li> + <li><a href="news:comp.soft-sys.matlab">Newsgroup</a>: + comp.soft-sys.matlab</li> + <li><a href="news:sci.math.num-analysis">Newsgroup</a>: + sci.math.num-analysis </li> + <li><a href="news:sci.op-research">Newsgroup</a>: + sci.op-research </li> +</ul> +</blockquote> +<hr> + +<p>The program includes options for input transformations, +randomization, tracing, multiple output formats, and execution +statistics. The program can be called from within your +application. </p> + +<p>You can view the results in 2-d, 3-d and 4-d with <a +href="http://www.geomview.org">Geomview</a>. An alternative +is <a href=http://www.kitware.com/vtk.html>VTK</a>.</p> + +<p>For an article about Qhull, download from <a +href="http://citeseer.nj.nec.com/83502.html">CiteSeer</a>: +</p> + +<blockquote> + <p>Barber, C.B., Dobkin, D.P., and Huhdanpaa, H.T., "The + Quickhull algorithm for convex hulls," <i>ACM Trans. on + Mathematical Software</i>, Dec 1996. <a + href="http://www.acm.org/pubs/citations/journals/toms/1996-22-4/p469-barber/">http://www.acm.org</a>; + </p> +</blockquote> + +<p>Abstract: </p> + +<blockquote> + <p>The convex hull of a set of points is the smallest convex + set that contains the points. This article presents a + practical convex hull algorithm that combines the + two-dimensional Quickhull Algorithm with the general + dimension Beneath-Beyond Algorithm. It is similar to the + randomized, incremental algorithms for convex hull and + Delaunay triangulation. We provide empirical evidence that + the algorithm runs faster when the input contains non-extreme + points, and that it uses less memory. </p> + <p>Computational geometry algorithms have traditionally + assumed that input sets are well behaved. When an algorithm + is implemented with floating point arithmetic, this + assumption can lead to serious errors. We briefly describe a + solution to this problem when computing the convex hull in + two, three, or four dimensions. The output is a set of + "thick" facets that contain all possible exact convex hulls + of the input. A variation is effective in five or more + dimensions. </p> +</blockquote> +<!-- Navigation links --> +<hr> + +<p><b>Up:</b> <a href="http://www.geom.uiuc.edu/software/past-projects.html"><i>Past Software +Projects of the Geometry Center</i></a> <br> +<b>URL:</b> <a href="http://www.qhull.org">http://www.qhull.org</a> +<br><b>To:</b> +<a href="http://www.qhull.org/news">News</a> +• <a href="http://www.qhull.org/download">Download</a> +• <a href="http://citeseer.nj.nec.com/83502.html">CiteSeer</a> +• <a href=http://images.google.com/images?q=qhull&num=100>Images</a> +• <a href="html/index.htm#TOC">Manual</a> +• <a href="http://www.qhull.org/html/qh-faq.htm">FAQ</a> +• <a href="html/qh-quick.htm#programs">Programs</a> +• <a href="html/qh-quick.htm#options">Options</a> +<!-- GC common information --></p> + +<hr> + +<p><a href="http://www.geom.uiuc.edu/"><img src="html/qh--geom.gif" alt="[HOME]" +align="middle"></a> <i>The Geometry Center Home Page</i> </p> + +<p>Comments to: <a href="mailto:qhull@qhull.org">qhull@qhull.org</a> +<br> +Created: May 17 1995 --- <!-- hhmts start --> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Changes.txt b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Changes.txt new file mode 100644 index 0000000000000000000000000000000000000000..626008a6068791b2cd340deb97977bcaf2741094 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Changes.txt @@ -0,0 +1,1063 @@ + +.............This file lists all changes to qhull and rbox..................... + +qhull 2003.1 2003/12/30 + +New Features: + - Add Maple output ('FM') for 2-d and 3-d convex hulls [T. Abraham] + +Bug Fixes and Code Changes: + - Fixed qh_findbest() for upperdelaunay facets w/o better, lower neighbors + - For library users and some qhull users [A. Cutti, E. Milloti, K. Sun] + - Preserved qhmem.ferr in qh_memfreeshort() for library users + - Removed 'static' from qh_compare... for io.h and merge.h [V. Brumberg] + +Documentation: + - Add warning to findDelaunay() and qh_in.htm about tricoplanar facets + - Noted Edelsbrunner's Geometry & Topology for Mesh Generation [qh-impre.htm] + - Noted Gartner's Miniball algorithm [qh_impre.htm] + - Noted Veron and Leon's shape preserving simplification [qh_impre.htm] + +qhull 2003.1 2003/12/19 + +Bug Fixes: + - Reversed coordinate order for qh.ATinfinity in qh_projectinput [V. Brumberg] + - This effects: + - Qhull library 'd' or 'v' users with 'Qz' and unequal coordinate ranges. + - qdelaunay/qvoronoi users with 'Qbk:0Bk:0', 'Qz', and unequal coordinate ranges + +Changes to code: + - Replaced qh_VERSION with qh_version in global.c [B. Pearlmutter] + The previous techniques were either clumsy or caused compiler errors + - Removed unused variables from qh_findbest and qh_findbestnew [B. Pearlmutter] + - Note that qh.TESTpoints was added in 2002.1 for tsearch implementation + +Changes to distribution: + - Added Unix distribution including Debian files [R. Laboissiere] + The previous Unix distribution is now the source distribution + - Added rpm distribution [L. Mazet] + - Investigated generation of Win32 dll. Need to define a C++ interface. + +Changes to documentation: + - Moved Qhull to www.qhull.org (geom.umn.edu is not available) + - The Geometry Center is archived at http://www.geom.uiuc.edu + - Reviewed introduction to each program + Triangulated output ('Qt') is more accurate than joggled input ('QJ') + qdelaunay is 'qhull d Qbb' [C. Ulbrich] + qvoronoi is 'qhull v Qbb' + Added example of non-simplicial intersection to halfspace intersections + - Added warning about using the Qhull library. + - Added qhull timings to When to use Qhull [C. Ulbrich] + - Reorganized the home page index and the manual index + - Moved qh-home.htm to index.htm + +Changes to examples + - Fixed options for eg/eg.t23.voronoi.imprecise [B. Pearlmutter] + + +qhull 2002.1 2002/8/20 + +Changes to distribution: + - Set up savannah.gnu.org/projects/qhull/ [R. Laboissiere] + - Set up www.thesa.com as a backup + - Added qh-get.htm, a local copy of the download page + - Added Visual C++ interface to Qhull, qhull_interface.cpp [K. Erleben] + - Use HTTP instead of FTP for downloading Qhull + - Renamed qhull-1.0.sit.hqx + +Bug fixes: + - Fixed sign of coefficients for cdd halfspaces ('FD','Fd') [T. Abraham] + +Changes to code: + - Replace qh_version with qh_VERSION in qhull.h. + Allows shared libraries and single point of definition + - Added qh.TESTpoints for future implementation of tsearch + +Changes to build + - Makefile.txt works under cygwin + - Added Make-config.sh to create a Debian build [R. Laboissiere] + - Added .exe to Makefile.txt#clean. + - In README, use -fno-strict-aliasing with gcc-2.95.1 [Karas, Krishnaswami] + - Fixed chmod in Makefile.txt [B. Karas] + +Documentation updates + - Documented input options for each program [A. Montesinos] + - FAQ: "How to get the radii of the empty spheres for Voronoi vertices" + +URL updates: + - Changed official URL from locate/qhull to software/qhull + - Changed URLs from relative to absolute in qh-home.htm and qh-get.htm + - Added URL for Newsgroup: comp.soft-sys.matlab + - Added URL for GNU Octave + - Added URLs for Google and Google Groups + - Replaced qhull_mail.html and qhull_bug.html with mailto links. + - Removed URL for Computational Geometry Tribune + - Changed URL for locate/cglist to software/cglist + - Used site relative links for qh-home.htm + +qhull 3.1 2001/10/04 + +New features + - Added option 'Qt' to triangulate non-simplicial facets + - Added option 'TI file' to input data from file + - Added option 'Q10' to prevent special processing for narrow distributions + e.g., RBOX 1000 L100000 s G1e-6 t1001803691 | QHULL Tv Q10 + - Tried to compute Voronoi volumes ('Pv'). Requires dual face graph--not easy + See Clarkson's hull program for code. + +Changes to options + - Added numtricoplanars to 'Fs'. Number of good, triangulated facets for 'Qt' + - Added Zdelvertextot to 'Fs'. If non-zero and Delaunay, input is degenerate + - Qhull command ('FQ') may be repeated. + - If 'TPn' and 'TWn' defined, trace the addition of point 'n' + otherwise continue tracing (previously it stopped in 4-d) + - Removed 'Ft' from qdelaunay. Use 'Qt o' or 'qhull d QJ Qt' instead. + For non-simplicial regions, 'Ft' does not satisify the Delaunay property. + - If 'Po' or 'TVn', qhull checks outer planes. Use 'Q5' to turn off. + - If 'T4', print facet lists and check polygon after adding each point + +Corrections to code + - rbox: allow 'c' and 'd' with 's r', meshes, etc. + - qh_findbest: redesigned as directed search. qh_findbesthorizon for coplanar + qh_findbest is faster for many distributions + - qh_findbestnew: redesigned to search horizon of coplanar best newfacets + needed for distributions with a sharp edge, + e.g., rbox 1000 s Z1 G1e-13 | qhull Tv + - qh_findbest/qh_findbestnew: search neighbors of better horizon facets + was needed for RBOX 1000 s Z1 G1e-13 t996564279 | qhull Tv + and RBOX 1000 s W1e-13 P0 t996547055 | QHULL d Qbb Qc Tv + - qh_findbest with noupper: could return an upperdelaunay facet if dist>qh.MINoutside. + - qh_findbestnew: allow facet->upperdelaunay if dist > qh.MINoutside + - qh_partitioncoplanar: call qh_partitionpoint if outside and perpendicular + for distributions with a sharp edge + - qh_partitionvisible: report precision error if all newfacets degenerate. + was needed for RBOX 1000 s W1e-13 t995138628 | QHULL d + - qh_createsimplex: clears qh.num_visible, may be non-zero with 'TRn QJ' + +Changes to prompts, warnings, and statistics + - For Delaunay & Voronoi, 's' reports deleted vertices due to facet merging. + They were incorrectly reported as nearly incident points. + - Warn if recompute centrum after constructing hull + - Simplified narrow hull warning and print all digits of cosine. + A narrow hull may lead to a point outside of the hull. + - Print total vertices deleted instead of ave. per iteration (Zdelvertextot) + - Improved tracing for qh_partitionpoint and qh_partitioncoplanar + - Added number of distance tests for checking outer planes (qh_check_maxout) + - Simplified "qhull precision error: Only n facets remain." + - Included 'TRn' in the causes of a premature exit + +Changes to documentation + - README.txt: Added quickstart instructions for Visual C++ + - rbox: Added example of edge of narrow lens, rbox 1000 L100000 s G1e-6 + - Added cross references between options 'o' and 'p'. + - qh-eg.html: added examples comparing 'Qt', 'QJ', and neither 'Qt' nor 'QJ' + eg.15a.surface, eg.15b.triangle, eg.17a.delaunay.2, etc. + - Reorganized and enhanced discussion of precision problems in qh_impre.htm + - Fixed spelling errors [K. Briggs] + - Fixed link errors, validated HTML, and spell checked [HomeSite] + - Removed unnecessary #TOP links + - Added source links to the qh-quick.htm's header and footer + - qh-geom.htm, qh-poly.htm: add links to Voronoi functions in io.c + - src/index.htm: Added how to search qhull.h for qhull options + - qvoronoi.htm/qdelaun.htm: 'Fc' and 'FN' includes deleted vertices + +Changes to URLs + - Added http://www.voronoi.com and http://www.magic-software.com + +Changes to code + - qh_qhull: if 'TVn' or 'TCn' do not call qh_check_maxout and qh_nearcoplanar + - reviewed time profile. Qhull is slower. Optimized qh_findbestnew() + - qh_addpoint: Added warning note about avoiding a local minimum + - qh_checkpolygon: report qh.facet_next error if NARROWhull & dist>MINoutside + - qh_findbest: renamed "newfacets" parameter to "isnewfacets" since it is boolT + - qh_findbest/qh_findbestnew: testhorizon even if !MERGING + Otherwise qhull c D6 | qhull Q0 Tv assigns coplanar points + - qh_resetlists: add qh_RESETvisible for qh_triangulate + - qh_findbest: search better facets first. Rewritten. + - qh_findbest: increased minminsearch, always check coplanar facets. + See: RBOX 1000 s Z1 G1e-13 t996564279 | QHULL Tv + - qh_findbestnew: report precision error for deleted cones [rare event] + e.g.: RBOX 1000 s W1e-13 P0 t1001034076 | QHULL d Qbb Qc Tv + - qh_findbesthorizon: search horizon of qh.coplanarset. New. + - qh_findbestsharp: replaced with qh_sharpnewfacets followed by qh_findbestnew + - qh_partitionpoint, Delaunay sites can not be inside. Otherwise points may + be outside upperDelaunay facets yet not near-inside Delaunay facets + See: RBOX s 1000 t993602376 | QHULL C-1e-3 d Qbb FA Tv + - qh_partitioncoplanar: call qh_findbest/qh_findbestnew with qh DELAUNAY + - qh_printlists: format long lines + - qh_printvertex: format long lines + - user.h: tightened qh_WARNnarrow and qh_MAXnarrow. Do not see problems + until they are -1.0. + - user.h: defined qh_DISToutside, qh_SEARCHdist, and qh_USEfindbestnew + - qh_checkfacet: in 3-d, allow #ridges > #vertices. Can get a vertex twice + in a ridge list, e.g, RBOX 1000 s W1e-13 t995849315 D2 | QHULL d Tc Tv + +Changes to FAQ + - Recommended use of triangulated output ('Qt') + +Changes to distribution + - Recompiled in Visual C++ 5.0 with optimization (as was version 2.6) + - q_test: Added bad cases for Qhull and tests for new features + +Changes to Qhull library + - Added qh_triangulate() to poly2.c. It triangulates the output. + - Added option 'Q11' to copy normals and recompute centrums for tricoplanar facets + 'FP' may not print the nearest vertex for coplanar points + Use option 'Q11' when adding points after qh_triangulate() + +qhull 3.0 2001/02/11 + +Changes to distribution + - added qconvex, qdelaunay, qhalf, and qvoronoi + - added qhull-interface.cpp on calling Qhull from C++ [K. Erleben] + - renamed to qhull3.0/. + - added eg/, html/, and src/ directories + +Changes to URLs + - MathLab6 qhull: convhulln, delaunayn, griddatan, tsearchn, vororoin [Z. You] + - Wolfram Research wrote a Mathematica interface for qdelaunay [Hinton] + - Geomview moved from www.geom.umn.edu to www.geomview.org [M. Phillips} + - added URLs for tcsh and cygwin to README.txt + +Changes to documentation + - reorganized table of contents and renamed qh-man.htm to index.htm + - wrote program documentation, dropped qh-opt.htm and qh-optv.htm + - added quick reference, qh-quick.htm + - reorganized qh-rbox.htm and renamed it to rbox.htm + - split up qh-c.htm for quick navigation + +Corrections to code + - fixed type of arg for error message in qh_initqhull_globals [N. Max] + - fixed incorrect initialization of qh MINdenom_1 for scalepoints + - fixed drop dim for 'H Qbk:0Bk:0'. Added qh.feasible_point to qh_projectinput + - qh_WARNnarrow is angle between facet normals. Negate for warning message. + - statistics for Wangle/etc. concerns facet normals. Reworded [E. Voth] + - fixed error message for 'FC v' + - report cospherical points if Delaunay and error in qh_scalelast() + +Changes to code + - turn on Pg if (QVn or QGn) and not (Qg or PG) + - turn on Qc if format option 'Fc', 'FP', or 'Gp' (removes warning) + - removed last good facet unless ONLYgood ('Qg'). + - added count of non-simplicial or merged facets to 'FS' + - added count of non-simplicial facets to 's' (OK if #vertices==dim) + - added Znonsimplicial and Znowsimplicial to 'Ts' + - allow Mathematica output of dual polytope for halfspace intersection + - added qh_checkflags to globals.c for multiple front ends + - qh_initqhull_globals sets qh.MERGING if qh.MERGEexact + - removed hashentryT. It is no longer used. + +Changes to prompts and warnings + - reorganized prompts + - reorganized synopsis for rbox.c + - print warning if 'Fc' or 'FP' with 'd QJ'. Coincident points are unlikely. + - ignore warning if options 'v i Pp'. qvoronoi users may need Delaunay tri. + - reworded warning if 'Pg' and 'QVn' is not a vertex. + - reworded warning for 'Qx Tv', qh_check_points() in poly2.c + - if 'Pp', turn off warning for 'Qbb' without 'd' or 'v' + - in qh_printsummary() of Voronoi and Delaunay, distinguish QVn, QGn, Pdn, PDn + +Changes to FAQ + - added FAQ item for nearly flat Delaunay triangles [Z. You] + - added FAQ item for area and volume [R. Konatham] + - added FAQ item for Voronoi diagram of a square [J. Yong] + - edited FAQ item on point location in Delaunay triangles [Z. You] + - added FAQ item on nearly flat Delaunay triangles [Dehghani] + - added FAQ item about halfspace intersection [V. Tyberghein] + - added FAQ item for missing ridges [M. Schmidt] + - added FAQ item about qh_call_qhull [R. Snyder] + - added FAQ item about memory statistics [Masi] + - added FAQ item on meshing non-convex objects + - added FAQ item on MATLAB and Mathematica interface + +qhull 2.6 1999/04/19 + - fixed memory overwrite (no effect) in qh_initstatistics() [K. Ford] + - added zdoc11 to qh-stat.h#ZZdef for !qh_KEEPstatistics [K. Ford] + - enhanced qh_initqhull_globals() test of qh_RANDOMint and qh_RANDOMmax + - added debugging option to always return qh_RANDOMmax from qh_rand() + - fixed option 'Qr', qh_initialvertices(), to allow a broken qh_rand() + fixed option 'Qr', qh_nextfurthest(), to allow narrow hulls + Option 'Qr' simulates the random incremental algorithm for convex hulls + - added note that qh.num_outside includes coplanar points for narrow hulls + - added FAQ items for triangles/ridges of 3-d Delaunay triangulation[P. Kumar] + - added FAQ item about on-line processing with the Qhull library [O. Skare] + - changed name of option 'Error-roundoff' to 'Distance-roundoff' + +qhull 2.6 1998/12/30 + - for the unbounded rays of the Voronoi diagram, use a large box [Schmidt] + e.g., 'rbox P4,4,4 P4,2,4 P2,4,4 P4,4,2 10 | qhull v Fv' fails for point 0 + while 'rbox P4,4,4 P4,2,4 P2,4,4 P4,4,2 10 c G5 | qhull v Fv' is OK. + - fixed qh_new_qhull() to use outfile/errfile instead of stdout/stderr [Ford] + - clarified COPYING.txt for use in other programs [Archer] + - improved qh_readpoints() error message for incorrect point count. + - added FAQ item for closest triangle to a point [Sminchisescu & Heijting] + - added FAQ item for is a point outside of a facet [Beardsley] + - added FAQ item for visiting facets of the Delaunay triangulation [Heijting] + - added link to Erickson's Computational Geometry Software + - added link to Sharman's HTML version of the comp.graphics.algorithms FAQ + - added link to Owen's Meshing Research Corner + - added note to 'd' about quadratic size of 'rbox 100 l | qhull d' [Kumar] + - added 'about a point' to mentions of halfspace intersection + - added request to qh-in.htm to compute largest empty circle [Hase] + - the DOS window in Windows NT is better than the DOS window in Windows 95 + - removed obsolete qh_findfacet() from qh-c.htm [Sminchisescu] + +qhull 2.6 1998/8/12 + new and modified features + - rbox: added option Mn,m,r to generate a rotated lattice or mesh + - rbox: report error if more than one of 'l', 'x', 'L', 's', or 'M' + + Qhull library changes + - added qh_new_qhull() to user.c for calling qhull() from a program [D. Zwick] + rewrote user_eg.c to use qh_new_qhull(). Added qh_QHpointer example. + - added qh_CLOCKtype 2 in user.h to call times() for CPU time [B. Hemkemeier] + - renamed set.c/set.h to avoid conflict with STL's set.h [G. van den Bergen] + can also use '#include <qhull/qhull_a.h>' for Qhull library + + fixed errors + - fixed qh_init_B() to call qh_initqhull_mem() once only [D. Zwick] + This only effects Qhull library users of qh_QHpointer. + - fixed qh_mergecycle_all() to check for redundant vertices [J. Nagle] + e.g., 'rbox M3,4 64 | qhull Qc' should report 8 vertices & 48 coplanars + - fixed gmcoord initialization in qh_setfacetplane() for qh.RANDOMdist + - turn off qh.RANDOMdist during qh_printfacetheader() + - fixed error messages for unknown option flags + + documentation changes + - added note to FAQ on the Voronoi diagram of cospherical points [H. Hase] + - added note to 'Qu' on furthest-site Delaunay triangulation via convex hull + - added note to 'QJ' about coplanar points on the convex hull of the input + - added note that 'o' and 'FN' list Voronoi regions in site id order [Arvind] + - added links to Fukuda's introduction to convex hulls, etc. [H. Hase] + - added .c and .h source files to web distribution and qh-c.htm [J. Sands] + - documented qh_ZEROdelaunay. Delaunay and Voronoi diagrams do not include + facets that are coplanar with the convex hull of the input sites. + + modified code + - replaced computed minnorm in qh_sethyperplane_det with distance test + - removed 'qh rand_seed' since random number generator is independent of qhull + - dropt 'qhull-PPC.sit.bin' from the distribution (out-of-date) [M. Harris] + +qhull 2.5 1998/5/4 + + fixed errors + - removed zero-area Delaunay triangles and furthest-site triangles [I. Beichl] + Zero-area triangles occur for points coplanar with the input's convex hull. + - replaced qh.MINnorm with computed minnorm in qh_sethyperplane_det [J. Nagle] + qh.MINnorm was incorrect for the convex hull of the "teapot" example. + Qhull runs 0-20% slower in 3-d and 4-d. + - allow 'Qg' with furthest-site Delaunay triangulation (may be faster) + - removed extra space in definition of FOREACHmerge_() for Alpha cc [R. LeRoy] + - fixed innerouter type in qh_printvdiagram2 [does not effect code, R. Adams] + + documentation changes + - to browse qh-c.htm, set MIME type of .c and .h files to text/html + - added example of 3-d Delaunay triangulation to q-faq.htm + - added Delaunay and Voronoi examples to qh-optv.htm + +qhull 2.5 1998/2/4 + - added option 'v Fi' for separating hyperplanes of bounded Voronoi regions + - added option 'v Fo' for separating hyperplanes of unbounded Voronoi regions + - option 'Pp' turns off the warning, "initial hull is narrow" + - fixed partial, 3-d Voronoi diagrams (i.e., 'v Fv QVn Tc') + - fixed missing statistics in qh_allstat* [T. Johnson] + - rearranged qh_printvdiagram. Use qh_eachvoronoi to iterate Voronoi ridges. + - added qh_check_points to qh-math.c + +qhull 2.5 1998/1/28 + - added option 'Fv' to print the Voronoi diagram + - added rbox option 'x' to generate random points in a simplex + - added rbox option 'y' to generate a simplex and random points + - added rbox option 'On' to offset the output + - add unpacking instructions to README.txt + - updated discussion of forced output, 'Po' + - sorted the options alphabetically + - removed __STDC__ check from qhull.h for VisualC++ + - moved qh_markvoronoi from qh_printvoronoi and cleared facet->seen flags + - added facet->seen2 flag for 'Fv' + +qhull 2.5 1998/1/16 + - fixed initialization of qh.last_low on restart of 'd QJ' + - renamed qh_JOGGLEmax to qh_JOGGLEmaxincrease + - updated URL for Fukuda's cdd program + +qhull 2.5 1998/1/12 + +New or modified features + - added option 'Fx' to print vertices by point id [G. Harris, T. McClendon] + in 2-d, the vertices are printed in counter-clockwise order + for Delaunay triangl., 'Fx' lists the extreme points of the input sites + - added option 'QJ' to randomly joggle input instead of merging facets + - added option 'TO file' to output results to a file. Needed for Windows95. + - added option 'TRn' to rerun Qhull n times. Use to collect joggle statistics + +Corrections + - fixed 2-d facet orientation for 'i' and 'o' outputs + - for Mathematica 2.2 ('m') changed %10.8g to %16.8f [A. Zhaxybayev] + - fixed incorrect warning for 'QV0 Qg' in qh_initbuild [B. Wythoff] + - fixed unaccessible statistic if !qh_KEEPstatistics for Wnewvertexmax + - fixed overestimate of qh ONEmerge for point sets outside of + first quadrant and far from the origin + - fixed overestimate of 'Qbb' for point sets far from the origin + - fixed potential overestimate of qh DISTround under 'Qbb' + - fixed 'p' printing coplanar points of unselected facets + - fixed 'Ft' printing centrums unnecessarily in 2-d + +Changes to documentation + - wrote internal design documentation (qh-c.htm) + - started frequently asked questions (qh-faq.htm) + - added a section on joggled input to qh-impre.htm + - added joggle example to qh-eg.htm (eg.15.joggle) + - changed q_eg to use 'QJ' instead of 'Q0' were appropriate + - added an example to each of the main options + - added examples to rbox.htm + - added examples to the synopsis + - added a reference to Mucke, et al ['96], Fast randomized point location ... + - added code for printing Delaunay triangles to qh-in.htm [A. Tsui] + - options 'Pdk' and 'PDk' do not drop on equality + +Improvements to the code + - reviewed warning messages for Qhull options in qh_initflags + - added warning to 's' if premature exit from 'TCn' or 'TVn' + - 's' prints max distance above/below only if qh.MERGING + - reduced maxoutside in qh_check_bestdist/qh_check_points for 'Rn' + - in post-merging, rebuild centrums of large facets that become small + - lowered cutoff for coplanar facets for ischeckmax/qh_findbest + - removed qh_check_maxout for 'Wn Q0' + - reset tracing after 'TPn' adds point in 4-d and higher + +Changes for the Qhull library + - changed qh_setdelaunay to call qh_scalelast if used with 'Qbb' or 'QJ' + Delaunay callers to qh_findbestfacet, qh_addpoint, or qh_findfacet_all + should always use qh_setdelaunay as in user_eg.c + - defined qh.outside_err to check points against a given epsilon [E. Voth] + - added header to user_eg.c to avoid its incorporation into qhull [C. Begnis] + - added qh_nearcoplanar() calls to user_eg.c + only needed if use 'QJ' + - expanded __STDC__ warning message in qhull.h [C. Begnis] + - renamed qh maxmaxcoord to qh MAXabs_coord + - replaced qh MAXlowcoord with qh MAXabs_coord + - random seed ('QR-n') is reset in qh_initqhull_globals after testing + - replaced 'FO' options "_max-coord/_min-coord" with "_max-width"/qh.MAXwidth + +Other changes to Qhull functions + - report error for !bestfacet in qh_findbest (it shouldn't happen) [H. Meuret] + - set newbest in qh_findbest only if bestfacet updated + - renamed facet parameter for qh_findbest + - added maxdist argument to qh_checkpoint + - moved 'FO' output after qh_partitionall + - separated qh_initbuild from qh_qhull + - reinitialize globals modified by qh_buildhull + - moved initialization of qh.GOODvertexp & qh.GOODpointp to qh_initbuild + - separated qh_nearcoplanar from qh_check_maxout + - separated qh_geomplanes from qh_printfacet2geom, etc. + - separated qh_freebuild from qh_freeqhull + - separated qh_outerinner from io.c to return outer and inner planes + - separated qh_distround and qh_detroundoff from qh_maxmin + + +qhull 2.4 97/4/2 + +New or modified features + - made 'C-0' and 'Qx' default options. Use 'Q0' to not handle roundoff errors + - removed point-at-infinity from Delaunay/Voronoi. + you no longer need to use 'Qu PDk' + - added 'Qz' to add a point-at-infinity to Delaunay and Voronoi constructions + - added published version of qhull article in ACM Trans Math Software + - ported qhull to Windows95 under Visual C++ and Borland C++ + - added 'Ft' for triangulation by adding the centrums of non-simplicial facets + - added 'Gt' to display 3-d Delaunay triangulations with a transparent hull + - changed definition of coplanar point in output to qh min_vertex (see 'Qc') + it was qh MAXcoplanar ('Un') [could make vertices non-coplanar] + - automatically set 'Qi' for options 'd Qc' and 'v Qc'. + - reworded summary ('s') for Delaunay/Voronoi/halfspace. + use 'Fs' and 'FS' for summary statistics. + - for summary 's' of Delaunay/Voronoi, display number of coplanars for facets + if none, display total number of coplanars (i.e., non-vertices) + - input comment is first non-numeric text (previously limited to header) + - added input warning if use 'Qbb' without 'd' or 'v' + - 'Q3' can not be followed with a numeric option + +Corrections + - fixed qh_partitioncoplanar() to not drop interior points if 'Qi' is used + - fixed 'FP d' to report distance in point set instead of paraboloid + - fixed qh_findbest() to search all coplanar facets for qh_check_maxout() + +Changes to documentation + - added example eg.17f.delaunay.3 to show a triangulation of cospherical sites + - split up qh-opt.htm into multiple pieces + - split off qh-in.htm for Qhull internals + - renamed .html files to .htm for Windows95 + - rewrote qh-optv.htm on Delaunay triangulation and Voronoi vertices + - added 'man' pages qhull.txt and rbox.txt. These list all the options + - removed 'txt' versions of html files + - added note to 'PDk' about avoiding a 'd' option immediately after a float + - under option 'd', display the triangulation with 'GrD3', not 'GnrD3' + +Changes to the Qhull library + - added 'format' argument to qh_printfacetNvertex_nonsimplicial() in io.c + - removed C++ type errors [J. Stern, S. Marino] + - created SETelemt, SETfirstt, etc. for specifying data types. + - use SETelem,etc. for assignments. + - changed setT.maxsize to 'int' to prevent type conversion warnings + - changed FOREACH.. to a comma expression for better code and less warning + - changed qh.vertex_visit and .visit_id to unsigned int to prevent warnings + - changed clock() to qh_CPUclock (user.h) + - qh_init_B() and qh_readpoints() do not set qh.ATinfinity for Delaunay tri. + - qh_setvoronoi_all() sets upper Delaunay facets if qh.UPPERdelaunay is set + - qh_nearvertex() returns distance in dim-1 for qh.DELAUNAY + - removed MSDOS path from qhull_command. Spaces in Win95 tripped qh_setflags + - removed memory.h from qhull_a.h. memset,etc. should be in strings.h + - split qh_prompt into pieces for Visual C++ + - added note to qh_addpoint that coordinates can not be deallocated + - added Baker '89 to constrained Delaunay diagrams under Enhancements + please let me know if you try this + - added request for unbounded Voronoi rays to Enhancements + please let me know if you try this + +qhull V2.3 96/6/5 + - fixed total area of Delaunay triangulation. [A. Enge] + It should ignore facets on the upper-envelope. + - if 'd Qu FA', the total area is summed over the upper-Delaunay triangles + - fixed sign of area for Delaunay triangulations. + - fixed cdd input format for Delaunay triangulation. [A. Enge] + - for cdd input, allow feasible point for halfspaces. + - warning if cdd output format for centrums, halfspace intersections, or OFF. + - print '0' for area ('Fa') if area is not computed for a facet + - option 'QR-n' sets random number seed to n without rotating input + - fixed qh_findbest() to retest coplanar and flipped facets after a restart + - for 'd Qu Ts' collects angle statistics for upper Delaunay facets + + Changes to the Qhull library + - expanded user_eg.c for incremental constructions and Delaunay triangulation + - added discussion of incremental construction to qh_man.html#library + - WARNING: The default value of qh ATinfinity was changed from True to False. + A new flag, qh UPPERdelaunay, was defined for 'Qu'. + Please set qh ATinfinity if you explicitly add the point "at-infinity" + Please set qh ATinfinity if you explicitly call qh_projectinput. + Please set qh UPPERdelaunay if you explicitly cleared qh ATinfinity. + Other users do not need to change their code. + Now you can build a Delaunay triangulation without creating a point + "at-infinity". This removes a potential, hard-to-understand error. + qh_readpoints sets qh ATinfinity for options 'd' or 'v' without 'Qu'. + qh_initB sets qh ATinfinity for qh PROJECTdelaunay w/o qh UPPERdelaunay. + qh_projectinput adds a point "at-infinity" only if qh ATinfinity is set. + - added qh_setdelaunay to geom2.c to project points to paraboloid + - added qh_findbestfacet() to poly2.c to replace qh_findfacet() + - removed procedure qh_findfacet(). It does not always work. + - removed NULL option for facet in qh_addpoint(). It does not always work. + - added noupper parameter to qh_findbest. + - added search of upperdelaunay facets to qh_findbest() + - allowed qh_findbest() to start with a flipped or upperdelaunay facet + - removed qh_nonupper() -- it is no longer needed + - allow space at end of options + - fixed qh_projectinput for furthest-site Delaunay (qh PROJECTdelaunay 'd Qu') + - added voids to procedure declarations with empty parameter lists + +qhull V2.3 96/3/25 + - fixed missing qh_check_maxout when coplanar points and no merged facets. + - fixed qh_freeqhull/allmem (e.g., if qh_NOmem) [only custom code] [E. Voth] + - fixed qh_freeqhull to free qh interior_point [E. Voth] + - fixed main() to free all of memory if qh_NOmem. Include "mem.h" [E. Voth] + - reset f.newcycle link in qh_mergecycle_all [E. Voth] + - fixed false error if 'Tc', coplanar points, and a narrow hull + - turn off 'Rn' when computing statistics, checking code, or tracing code + - removed ={0} from global.c and stat.c to reduce compiler warnings + - changed Makefile dependences to $(HFILES) for all but unix.o, set.o, mem.o + - pulled out qh_printpointid and reordered qh_pointid [E. Voth] + - removed some compiler warnings + - moved 'FO' print of options to just before qh_buildhull + - changed 'FO' to list difference from -1 for _narrow-hull + +qhull V2.2 96/2/15 + - detect narrow initial hulls (cosine of min angle < qh_MAXnarrow in user.h). + Write warning if cosine < qh_WARNnarrow in user.h. If narrow (qh NARROWhull), + partition coplanar points as outside points and delay coplanar test to end. + Needed for RBOX 1000 L100000 s G1e-6 t995127886 | QHULL Tv + See 'limitations' in qh-impre.html for further discussion. + - corrected some confusions between 'FV' and 'Fv' in qh-opt.html + - fixed initialization error for small Voronoi diagrams (e.g., [0,0], [1,0], [0,1]) [J. Velez] + - fixed bad return from qh_mindiff in geom2.c + - for initial hull, first process facet with furthest outside point (qh_furthestnext) + - added facet->notfurthest flag for recomputing furthest point + - added check for __STDC__ (needs ANSI C) [E. Voth] + - reduced warning messages from [E. Voth]. e[1] in setT still reports a warning. + - added a cube to the discussion of option 'v' (Voronoi) in qh-opt.html [J. Velez] + - added notes about adjacent vertices to "Calling Qhull" in qh-man.html [R. Lewis & J. Velez] + - added note about 'lines closer' when viewing 3-d Delaunay triangulations [P. Kallberg] + - added option 'Q9' to always process furthest of furthest outside points. + - removed option 'Pp' from q_eg and qh-eg.html. + +qhull V2.2 95/12/28 + - added option 'Qbb' to scale the last coordinate to [0,m] (max prev coord). + This reduces roundoff errors for Delaunay triangulations with integer coordinates. + - changed option 'Qu d' to print out the furthest-site Delaunay triangulation + Use options 'Qu d PD2' to compute the normal 2-d Delaunay triangulation without + the point at infinity. + - added notes to the documentation of option 'd' + - added notes to limitations of how Qhull handles imprecision + - added warning if options 'FP', 'Fc', or 'Gp' without option 'Qc' or 'Qi' + - added note to options 'PDk:n' and 'Pdk:n' that closest facet is returned if none match + - added check that 'Qbk' and 'QBk' does not invert paraboloid for 'd' + - added notes that qh_findfacet and qh_addpoint require lifted points for Delaunay triangulations + - fixed 'rbox s 5000 W1e-13 D2 | qhull d Qcu C-0 Qbb' + - fixed option 'QbB' (qh SCALEpoints was not set) + - fixed minor confusion between 'Gc' (print centrums) and 'Gp' (print points) + - rewrote qh_findbestnew for upper convex hull, Delaunay facets + - changed option name for 'Gp' from 'Gcoplanar' to 'Gpoints' + - changed "nearest" facet for 'Pdk' to threshold - normal + - reworked qh GOODclosest for 'Qg' + - added note that 'Qg' does not always work + - recorded option 'C-0' as "_zero-merge" in option 'FO' + - refined qh DISTround in qh_maxmin/geom2.c for Delaunay triangulations + +qhull V2.2 95/12/4 + - Version 2.2 fixes an important bug in computing Delaunay triangulations + and convex hulls with edges sharper than ninety degrees. The problem + occurs when processing a point at a sharp edge. A directed search can + not be used for partitioning because one side may hide facets from the + other side. The new lens-shaped distribution for rbox demonstrates the + problem. For example, 'rbox 100 L3 G0.5 s | qhull Tv' fails for Version 2.1. + O. Schramm found the bug when computing the Delaunay triangulation of points + near an outside edge. + + I rewrote qh_findbest and related functions. Qh_findbest + uses an exhaustive test for sharp edges (qh_findbest_sharp). + Qh_findbest avoids the upper convex hull of Delaunay triangulations. + + Options 'd' and 'v' no longer assign coplanar points to the upper convex hull. + + Qh_check_maxout tests near-inside points. It ignores fully inside points. + When done, it removes near-inside points from the coplanar sets. + + If you use qh_addpoint or qh_findbest, please review the function headers. + They do not work for lens-shaped hulls for arbitrary facets. They do work for + Delaunay triangulations. + + Changes to options for V2.2 + - added 'Qu' for computing the furthest-site Delaunay triangulation (upper hull) + and furthest-site Voronoi vertices. + - added 'FP' to print nearest vertex for coplanar points + - added coplanar count to 'Fs' and 's' + - added number of similar points to summary for Delaunay/Voronoi + - Option 'Qc' is no longer necessary when merging. + - 'o' format for Voronoi vertices ('v') generates "0" lines for similar points + - 'QRn' for Delaunay ('d' or 'v') now rotates about the Z axis (see qh_init_B). + Otherwise Qhull does not identify the upper hull + - removed option 'Qa' for "all points outside". In V2.1 it was automatically + set for 'd'. Even though it was a bad idea, it revealed the above bug. + - for option list ('FO'), added version, values for one-merge, maxpos, maxneg, + and near-inside, and flags for zero-centrum + - optimized 'C-0' and 'Qx'. These options ("zero-centrum") test vertices + instead of centrums for adjacent simplicial facets. + - if 'Tv', report number of points that are not verified due to qh_findbest + - Option 'Q8' ignores near-inside points. + + rbox 95/12/3 + - added lens distribution ('Ln') It may be used with 's', 'r', 'Wn', and 'Gn' + - removed default point count except for the test case, 'Dn' + - divided main() of rbox.c into sections + + Documentation changes for V2.2 + - added examples for lens distribution and furthest-site Delaunay triangulation + and renumbered the examples for q_eg + - described facet orientation in 'data structure' section [P. Soikkonen] + - added notes to qh-man.html/"What to do if something goes wrong" + - added note about using 'Tv' to verify the results [O. Schramm] + - expanded definition of f_r in Performance section [S. Grundmann] + - noted that Geomview display of a Voronoi diagram adds extra edges + for unbounded Voronoi cells + - rewrote error "convexity constraints may be too strong" [D. Newland] + - added limitations section to "Imprecision in Qhull" + - added note about zero-area facets to 'Imprecise convex hulls' in qh-impre.html + - added note to 'Qi' that it does not retain coplanar points + - added note that option 'Q5' may be used if outer planes are not needed + - added note to README.txt about Irix 5.1 [C. Goudeseune] + - added code fragment for visiting Voronoi vertices to "Calling Qhull" [M. Downes] + - added note about qh_addpoint() to "Calling Qhull" [M. Downes] + + Errors fixed for V2.2 + - use qh_sethyperplane_gauss if 3-d or 4-d norm is less than qh MINnorm + - count Zcentrumtests if qh_NOstat + - reversed sign convention for qh_sethyperplane_gauss + it was opposite to qh_sethyperplane_det + this effects qh_determinant and qh_detsimplex + - fixed error in qh_findgood_all with multiple restrictions, e.g., 'QVn Pdk' + - fixed call to qh_clearcenters for qh_produce_output + - in qh_errexit, report p0 if last furthest point + + Changes for users of the Qhull library + - user code needs to define qh_version (see user_eg.c) + - merged initialization code into qh_init_A and qh_init_B [M. Mazzario] + old code works as before. + qh_initflags also sets qh qhull_command for qh_initthresholds + redid initialization for user_eg.c + - simplified user_eg.c. It computes the convex hull of a cube. + - added qh_setvoronoi_all in poly2.c to compute Voronoi centers + added related note to call_qhull + - added qh_findfacet to use in place of qh_findbest + - added qh_nearvertex to return nearest vertex in facet to point + - redid notes on multiple, concurrent calls in call_qhull/user.c + - changed BoolT to unsigned int (prevent implicit enum conversion warnings) + - added upperdelaunay to facetT. It indicates a facet of the upper convex hull. + - converted qhull-PPC.sit for CodeWarrior 7 + + Code changes for V2.2 + - simplified calls to setjmp() for Cray J916 [Salazar & Velez] + - replaced fprintf(fp,string) with fputs in io.c + - 'qh num_coplanar' removed (too expensive and not used). + - added numcoplanars to qh_countfacets() + - added min norm to qh_normalize2(). qh_normalize() wasn't changed + - removed type casts from qh_point and qh_pointid [Salazar & Velez] + - account for roundoff error in detecting upper convex hull (qh ANGLEround). + - post merging uses qh_findbestnew for partitioning + - qh_findbestnew for qh_partitioncoplanar goes to best facet + - 'Qm' always processes points above the upper hull of a Delaunay triangulation + - GOODvertex initialized with qh_findbestnew instead of qh_findbest + - for 'v', qh_facetcenter returns furthest-neighbor vertex if 'Qu' + - added test for qh feasible_point if use 'Fp' and qh_sethalfspace_all + - reviewed Sugihara's algorithm for topologically robust beneath-beyond + - on error, report if qhull in post-merging or has completed + - if tracing, option 'FO' and qhull command always printed + - added current furthest point ("during p%d") to 'T-1' events + - added 'TWn' tracing for vertices of new facets (qh_setfacetplane) + - added 'TWn' tracing for vertices in an output facet (qh_check_maxout) + - reorganized split between poly/poly2.c and geom/geom2.c + - reordered object files in Makefile + +qhull V2.1 95/9/25 + - converted qhull.man to html format, many edits + - referenced Shewchuk's triangle program and Schneiders' Mesh Generation page + - added option 'Qa' to force all points outside + automatically set for "precise" Delaunay or Voronoi [Salazar & Velez] + it is turned off by merging, 'Wn', 'Qc' or 'Qi' + - added coplanar points to option 'FN' + - moved option 'FO' to include default precision options + - changed default random number generator to qh_rand in geom2.c (user.h) + + other code changes + - fixed option comment Pdrop-facets-dim-less, Qbound-dim-low, QbBound-unit-box + - defined ptr_intT for converting 64-bit ptrs to 'unsigned long' [D. Bremner] + - defined setelemT to distinguish actual size from pointers [D. Bremner] + use either e[...].p or e[...].i (user-level code should use set.h macros) + - changed %x to %p in format statements for pointers [D. Bremner] + - added test of REALmax,etc. to qh_maxmin [H. Poulard] + - added type parameter to qh_memalloc_() macro for type conversion + - added type conversion to qh_memalloc() calls where missing + - added type conversion to calls into set.c that return void* + + other documentation changes: + - new URLs for graphics images + - fixed comment for facetT.neighbors in qhull.h [P. Soikkonen] + - changed recommendations for precision errors in qh_printhelp_degenerate() + - added recommendation for 'V0' (facet is visible if distance >= 0) + - added note about 'long double' to user.h [S. Grundmann] + - added note about zero area Delaunay triangles for the 'v' option + - added note about locating Delaunay triangles to option 'd' [A. Curtis] + - added note that coplanar vertices correspond to duplicate points for 'd' + - added note about option 'd' automatically setting 'PDk' (lower convex hull) + - added note about precision errors to option 'd' [many users] + - added note about qh_findbest() to the Qhull library section [L. Lai] + - 'make install' renames qhull.man to qhull.1 for Unix [M. Phillips] + - renamed README, etc. to *.txt to match WWW conventions [D. Cervone] + +qhull V2.1 7/10/95 + - in 2-d, 'v o' lists the vertex at infinity in order [R. Critchlow] + - it used to always list the vertex at infinity ("0") first + - rewrote description of 'v' option (Voronoi vertices and 2-d diagrams) + - added 'PFn' for printing facets whose area is at least 'n' [D. Holland] + - prefixed 'Q',etc. to the 'Q',etc. options in the long help prompt + - fixed references to 'Fv' and 'FV' options under option 'Hn,n,n' + - updated reference to cdd, <ftp://ifor13.ethz.ch/pub/fukuda/cdd/> + - in set.c, added some missing type coercions for qhmem.tempstack + +qhull V2.1 6/12/95 + - replaced qhull.ps with qhull-2.ps (paper submitted to ACM TOMS) + - use BUFSIZ for setvbuf for Power Macintosh + - number of good facets printed if QVn, QGn, Pd, or PD + - added Makefile for Borland C++ 4.02 with Win32 [D. Zwick] + - added note to Enhancements section of qhull.1 about constrained + Delaunay triangulations [T. Rasanen] + +qhull V2.1 6/7/95 + - fixed qh_facetarea_simplex() for non-simplicial facets [L. Schramm] + - fixed cast in qh_point and qh_pointid for 64-bit architectures + - fixed URL for Amenta's list of computational geometry software + - fixed cast in qh_meminitbuffers for qhmem.freelists + - added test for !qh half_space in qh readpoints + - clarified options for qh_printhelp_singular() + - discussed non-simplicial facet area under option 'Fa' in qhull.1 + +qhull V2.1 6/3/95 + - home page for Qhull and new descriptions for the Qhull examples + http://www.qhull.org + - changed SIOUX buffering for Power Macintosh. It runs fast now. + added a project file for Metrowerk's C + - note in README about compiling qhull on the PC + +qhull V2.1 beta 5/15/95 + + ======= main changes ======== + - added halfspace intersection ('Hn,n,n') + - facet merging is better, especially for high dimensions + - added 'Qx' for exact merges of coplanar points and precision faults + - facet merging is faster, especially for high dimensions. + e.g., convex hull of the 8-d hypercube is seven times faster + - added 'F' output formats for printing each aspect of the convex hull + - format 'Fa' computes facet areas, total area, and total volume + - format 'FO' writes a descriptive list of selected options to stderr + - moved all customization options to user.h + - changed the default precision to 'double' since it's needed for Delaunay. + using 'float' is faster and takes less space (REALfloat in user.h) + - option 'Qm' is no longer important for facet merging + - removed need for 'Qs' by selecting initial simplex with pos. determinants + - renamed 'Qv' to 'Q7' since virtual memory does not work well for qhull + - Qhull is available for the Power Mac (no graphical output) + + ====== other new and modified options =========== + - changed default MINvisible ('Vn') to a multiple of premerge_centrum ('C-n') + - added 'Un' option to set width of facet for coplanar points. + This replaces the previous rules for determining coplanar points. + - changed default MINoutside ('Wn') to twice MINvisible ('Vn') + - Geomview output adjusts point radii for MINvisible 'Vn' + - the input format allows the number of points to precede the dimension + - options 'v' 'd' 'FAn' and 'FMn' record good facets ('Pg') + - added 'Fd' and 'FD' options for homogeneous coordinates in cdd format + - in rbox, added 'h' flag to generate homogeneous coordinates in cdd format + - option 'PAn' prints out the n facets with the largest area + - option 'PMn' prints out the n facets with the most merges + - option 'Po' under tracing ('Tn') no longer tries to write erroneous facets + - option 'TCn' only prints the old 'visible' facets for 'f' + - 'TFn' reports intermediate results when post-merging + - option 'Ts' with option 'TFn' prints intermediate statistics + - the message for 'Tv' reports if it is checking outer planes + - 'Tz' sends stderr output to stdout + - added 'Q1' to ignore angle when sorting merges (merges are worse) + - added 'Q2' to not perform merges in independent sets (merges are worse) + - added 'Q3' to not remove redundant vertices (faster) + - added 'Q4' to avoid merges of old facets into new facets (does worse) + - added 'Q5' to skip qh_check_maxout (faster, but less accurate) + - added 'Q6' to skip pre-merge of concave and coplanar facets + - added 'Qv' for testing vertex neighbors for convexity (needs merge option) + - added warning if mix Geomview output with other outputs ('Po' turns off) + - options 'o v' for 3-d and higher sort the Voronoi vertices by index + + ======= documentation ======= + - rewrote the introduction and precision sections + - added a section on performance + - added an example on halfspace intersection + - installed examples of Qhull in + <http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/> + + ======= Makefile, user.h, and messages ======= + - Makefile calls ./qhull, ./rbox, and prints short prompt for qhull + - added new statistics, e.g., for buildhull + - changed default qh_RANDOMtype to RAND_MAX with rand() + - added comment about numeric overflow to printhelp_singular + - reorganized the code to improve locality of reference + - option in mem.h (qh_NOmem) to turn off memory management in qhull + - option in user.h (qh_NOtrace) to turn off tracing in qhull + - option in user.h (qh_NOmerge) to turn off merging in qhull. + - use this instead of redefining qh_merge_nonconvex in user.c + - simplified user_eg.c. See qh_call_qhull() in user.c for the full version + + ======== bug fixes ============ + - fixed error in number of points for 'rbox 100 r' (odd distribution) + - fixed performance error in qh_degen_redundant_neighbors + - qh_partitionpoint now sets facet->maxoutside for first outside point + - fixed performance error in partitioning when merging a large, regular cone + - removed memory leak in qh_appendmergeset + - removed double free of qh line under errors in qh_readinput() + - forcing output on error ('Po') fixed for options 'n' 'o' 'i' 's' + - fixed optimization error on HP machines [fprintf(... *p++)] + + ======== changes to qhull.h for user code ======= + - qh_collectstatistics and qh_printstatistics removed from qhull.h. + should use qh_printallstatistics instead + - qh_findbest uses boolT for newfacets + - added qh_findbestnew for non-simplicial facets. qh_findbest is + too slow in this case since it needs to look at many nearly coplanar + facets. + - renamed qh_voronoi/qh_centrum to qh_ASvoronoi, qh_AScentrum + - changed facet->id to 32-bits, added new flags for merging + - added facet->f for facet pointers while merging and for facet area + - added dfacet/dvertex for printing facets/vertices while debugging + - added qh_produce_output and qh_printsummary + + ======== changes to code ========== + - moved qh_setfacetplane from qh_makenewfacets to qh_makenewplanes + - added qh_setfree2, qh_setcompact, and qh_setduplicate to set.c + - qh_findgooddist returns list of visible facets instead of setting global + - in qh_check_maxout, inside points may be added to coplanar list. + - qh_findbestnew used for qh_partitionall. It is faster. + - in qh_findbest, changed searchdist to MINvisible+max_outside+DISTround. + MINvisible is the default MAXcoplanar. + - cleaned up list management via qh_resetlists + - uses facet->dupridge to indicate duplicated ridges instead of ->seen + - qh_buildtracing records CPU time relative to qh hulltime instead of 0 + + ========== changes to merging ======= + - many performance improvements, especially in high-d. + - when merging, qh_findbest and qh_findbestnew stops search at qh_DISToutside + - vertex neighbors delayed until first merge + - post merges reported every TFn/2 merges + - vertex merging turned off in 7-d and higher (lots of work, no benefit). + vertex merging moved to qh_qhull_postmerging in 6-d. + - replaced qh min_vertex with MAXcoplanar for determining coplanarity + - pick closest facets to merge in duplicate ridge instead of flip/flip + (see qh_matchduplicates in poly2.c) + - optimize merge of simplex into a facet + - optimize merge of a "samecycle" of facets into a coplanar horizon facet + - cleaned up qh_forcedmerges/qh_flippedmerges and removed facet->newmerge + - rewrote qh_merge_degenredundant with separate queue + - qh_facetdegen replaced by facet->degenredun + - flipped neighbors no longer merged in preference to flip/non-flip pairs + - removed angle argument from qh_merge_degenredundant and qh_mergefacet + only used for tracing + - getmergeset_initial had extra test of neighbor->simplicial + - ridge->nonconvex is now set on only one ridge between non-convex facets + - moved centrum deletion to qh_updatetested + - qh_isnewmerge(facet) changed to facet->newmerge (removed NEWmerges) + - qh_findbestneighbor reports correct distance even if testcentrum + - added hull_dim factor to qh_BESTcentrum + - removed horizon preference in qh_merge_nonconvex (qh AVOIDold) + - facet->keepcentrum if qh WIDEfacet or qh_MAXnewcentrum extra vertices + +qhull V2.02 1/25/95 + - rbox 'z' prints integer coordinates, use 'Bn' to change range + - fixed rare bug in qh_check_maxout when qh_bestfacet returns NULL + - fixed performance bug in findbestneighbor, should be + BESTnonconvex + - renamed 'notgood' flag in 'f' option to 'notG' flag (caused confusion) + - changed qh.hulltime to (unsigned) to prevent negative CPU times + - added random perturbations to qh_getangle under the 'Rn' option + - reviewed the documentation and enhancement list + - added discussion of how to intersect halfspaces using qhull + - replaced expression that caused incorrect code under an old version of gcc + - added buffer after qh.errexit in case 'jmp_buf' has the wrong size + - rewrote qh_printhelp_singular for lower-dimensional inputs + - rewrote qh_printhelp_degenerate + - added options for qh_RANDOMint in qhull_a.h + - changed time format for 'TFn' to %02d + +qhull V2.01 6/20/94 + - fixed bug in qh_matchnewfacets that occured when memory alignment makes + facet->neighbors larger than necessary. + - fixed bug in computing worst-case simplicial merge of angle coplanar + facets (ONEmerge). This decreases (...x) in printsummary. + +qhull V2.01 6/17/94 + - added recommendation for 'Qcm' to documentation and help prompts + - added an input warning to qh_check_points ('Tv') if coplanars and no 'Qc' + - qh_partitionpoint: always counts coplanar partitions (Zcoplanarpart) + - rewrote qh_printhelp_degenerate to emphasize option 'C-0' + - For Geomview output, roundoff is not needed when printing the inner and + outer planes. This improves Geomview output for the 'Rn' option. + - For Geomview output without coplanar points or vertices, qh_GEOMepislon + is not needed. This removes the edge gap when displaying a Voronoi cell. + - For Geomview output 'Gp', direct vertices to the interior point + instead of the arithmetic center of the displayed vertices. + +qhull V2.01 6/11/94 + - if pre-merge, 'Qf' is automatically set. Otherwise an outside point may + be dropt by qh_findbest(). This slows down partitioning. + - always use 'Qc' if merging and all facet->maxoutside's must be right. + Otherwise distributions with many coplanar points may occassionally + miss a coplanar point for a facet. This is because qh_findbest, when + called by qh_check_maxout, can become stuck at a local maximum if + the search is started at an arbitrary facet. With 'Qc', the search + is started from a coplanar facet. For example, + rbox 1000 W8e-6 t | qhull C-0 Tv + will (rarely) report that a facet->minoutside is incorrect + - option 'Pp' turns off "Verifying" message for 'Tv' + - added qh_copynonconvex to qh_renameridgevertex (fixes rare error) + - 'rbox tn' sets random seed to n + - 'rbox t' reports random seed in comment line + - qh_errexit reports rbox_command | qhull_command and 'QR' random seed + - added additional tracing to bestdist and setfacetplane + - in qh_checkconvex, need to test coplanar against 0 instead of -DISTround + - in qh_checkconvex, always test centrums if merging. The opposite + vertex of a simplicial facet may be coplanar since a vertex of + a simplicial facet may be above the facet's hyperplane. + - fixed error handling in qh_checkconvex when merging + - in qh_printsummary, one merge ratio not printed if less than 'Wn' + - documented that 'Tv' verifies all facet->maxoutside + +qhull V2.01 6/2/94 + - 's' prints summary to stderr + - multiple output formats printed in order to stdout + - added statistic for worst-case distance for merging simplicial facets + can not hope for a better "max distance above/below facet" + print factor for "max distance.."/"merge simplicial" in printsummary + - fixed error in scaling input with min/max reversed ('Qb0:1B0:-1') + - fixed error in scaling if project & Delaunay & scale ('d Qb0:0B1:0b2:0') + - user_eg.c: qh_delpoint removed since it does not always work + - user_eg.c now works for either convex hull or Delaunay triangulation + - added PROJECTdelaunay for Delaunay triangulations and Voronoi diagrams + with libqhull.a and user_eg.c + - user_eg.c: if project or scale input, need to copy points + - user_eg.c: default just defines main, added fprintf's for qh_errprint etc. + - qh_gausselim: a 0 pivot no longer zeros the rest of the array, + need the remaining elements for area computation + - qh_qhull: restore cos_max, centrum_radius at end of POSTmerging + - qh_checkflipped with !allerror is >=0.0 instead of >0.0 + - removed -Wall from gcc due to unnecesssary "warning: implicit declaration" + - renamed 'new' variables and fields to allow compilation by g++ + - added README notes on C++, and "size isn't known" + - updated manual on 'Qg' with coplanar facets and no merging ('rbox c D7') + 'Qg Pg' and 'Pg' produce different results because of precision problems + +Converting from qhull 1.01 to qhull 2.00 + - 'qhull An' is now 'qhull Wn' + option 'Wn Po' is faster but it doesn't check coplanars + - 'qhull g' is now 'qhull G', and the output formats are different + - 'qhull c' is now 'qhull Tc' + - 'qhull f' is now 'qhull Qf' + - 'qhull o' is now 'qhull Po' + - 'qhull b' is now always done + - qhull and rbox now use floats, change REALfloat in qhull.h for doubles + - qhull 2.00 fixes several initialization errors and performanace errors + e.g., "singular input" on data with lots of 0 coordinates + - 'rbox b' is now 'rbox c G0.48' + - all rbox distributions are now scaled to a 0.5 box (use 'Bn' to change) + - rbox now adds a comment line. This may be removed by 'rbox n' + - 'rbox r s Z G' no longer includes the positive pole + - no changes to the Macintosh version + +qhull V2.00 5/23/94 + - if force output ('Po'), facet->maxoutside= 'Wn' since coplanars not updated + convexity checked if precision problems or verify ('Tv') + - if merging, new facets always checked for flipped orientation + - a facet is visible (findhorizon) under 'Qm' if distance > max_vertex + - if using 'Qm' then min. outside is max_vertex instead of max_outside + - default is random()/srandom() in qhull_a.h, checked in initqhull_globals + - created internal strtod since strtod may skip spacing after number + - removed lower bound (1.0) for qh maxmaxcoord + - divzero needs to handle 0/0 and large/small + - decreased size of precise vertices + - need to initialize qh MINdenom_1 for scalepoints + - added factor of qh maxmaxcoord into DISTround (needed for offset) + - 'Rn' perturbs distance computations by random +/-n + - check_points needs an extra DISTround to get from precise point to computed + - rewrote some of the IMPRECISION section in qhull.man + - added the input format to the qhull prompt + - moved include files to qhull_a.h since some compilers do not use float.h + - added qhull.1 and rbox.1 since SGI does not ship nroff + - reduced cutoff for printpointvect + - use time for qhull random seed only if QR0 or QR-1 + - radius of vertices and coplanar points determined by pre-merge values + +qhull V2.00 5/12/94 + - facet2point (io.c) used vertex0 instead of vertex1 + - can't print visible facets after cone is attached + - shouldn't check output after STOPcone (TCn) + - statistic 'Wminvertex' and 'Wmaxoutside' only if MERGING or APPROXhull + - 'make doc' uses lineprinter format for paging + - warning if Gpv in 4-d + +qhull V2.b05 5/9/94 + - decreased size of precise vertices + - precise facets in 2-d print hyperplanes + - accounted for GOODpoint in printsummary + - added IMPRECISION section to qhull.man + - 'Qg' does work in 7-d unless there's many coplanar facets + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Make-config.sh b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Make-config.sh new file mode 100755 index 0000000000000000000000000000000000000000..cc247865257de3b26f6c1665b9f85d243812c62a --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Make-config.sh @@ -0,0 +1,285 @@ +#!/bin/sh -e +# +# Make-config.sh +# +# Setup for Debian build +# +# Writes configure.in and Makefile.am files +# and runs automake and autoconfig +# +# Use 'make dist' to build Unix distribution. +# Use 'configure; make' to build Qhull +# +#note: +# 'configure; make' does not work under cygwin. +# src/unix.c:354: variable 'qh_qh' can't be auto-imported. +# Please read the documentation for ld's --enable-auto-import for details. + +################################################### +########### ../configure.in ###################### +################################################### + +echo Create ../configure.in +cat >../configure.in <<\HERE-CONFIGURE +dnl configure.in for the qhull package +dnl Author: Rafael Laboissiere <rafael@debian.org> +dnl Created: Mon Dec 3 21:36:21 CET 2001 + +AC_INIT(src/qhull.c) +AM_INIT_AUTOMAKE(qhull, 2002.1) + +AC_PROG_CC +AC_PROG_LIBTOOL + +AC_OUTPUT([Makefile src/Makefile html/Makefile eg/Makefile]) + +HERE-CONFIGURE + +################################################### +########### ../Makefile.am ####################### +################################################### + +echo Create ../Makefile.am +cat >../Makefile.am <<\HERE-TOP +### Makefile.am for the qhull package (main) +### Author: Rafael Laboissiere <rafael@debian.org> +### Created: Mon Dec 3 21:36:21 CET 2001 + +### Documentation files + +# to: +docdir = $(prefix)/share/doc/$(PACKAGE) + +# which: +doc_DATA = \ + index.htm \ + Announce.txt \ + COPYING.txt \ + README.txt \ + REGISTER.txt + +### Extra files to be included in the tarball + +EXTRA_DIST = \ + $(doc_DATA) \ + File_id.diz \ + QHULL-GO.pif + +### Subdirectories for Automaking + +SUBDIRS = src html eg + +HERE-TOP + +################################################### +########### ../eg/Makefile.am #################### +################################################### + +echo Create ../eg/Makefile.am +cat >../eg/Makefile.am <<\HERE-AM +### Makefile.am for the qhull package (eg) +### Author: Rafael Laboissiere <rafael@debian.org> +### Created: Mon Dec 3 21:36:21 CET 2001 + +### Documentation files + +# to: +docdir = $(prefix)/share/doc/$(PACKAGE) +examplesdir = $(docdir)/examples + +# which: +examples_DATA = \ + q_eg \ + q_egtest \ + q_test \ + Qhull-go.bat \ + q_test.bat + +### Extra files to be included in the tarball + +EXTRA_DIST = $(examples_DATA) + +HERE-AM + +################################################### +########### ../html/Makefile.am ################## +################################################### + +echo Create ../html/Makefile.am +cat >../html/Makefile.am <<\HERE-HTML +### Makefile.am for the qhull package (html) +### Author: Rafael Laboissiere <rafael@debian.org> +### Created: Mon Dec 3 21:36:21 CET 2001 + +### Man pages (trick to get around .man extension) + +%.1: %.man + cp $< $@ +CLEANFILES = *.1 +man_MANS = rbox.1 qhull.1 + +### Documentation files + +# to: +docdir = $(prefix)/share/doc/$(PACKAGE) +htmldir = $(docdir)/html + +# which: +html_DATA = \ + index.htm \ + qconvex.htm \ + qdelau_f.htm \ + qdelaun.htm \ + qh--4d.gif \ + qh--cone.gif \ + qh--dt.gif \ + qh--geom.gif \ + qh--half.gif \ + qh--rand.gif \ + qh-eg.htm \ + qh-faq.htm \ + qh-get.htm \ + qh-impre.htm \ + qh-in.htm \ + qh-optc.htm \ + qh-optf.htm \ + qh-optg.htm \ + qh-opto.htm \ + qh-optp.htm \ + qh-optq.htm \ + qh-optt.htm \ + qh-quick.htm \ + qhalf.htm \ + qhull.htm \ + qvoron_f.htm \ + qvoronoi.htm \ + rbox.htm + +### Extra files to be included in the tarball + +EXTRA_DIST = \ + $(html_DATA) \ + qhull.man \ + qhull.txt \ + rbox.man \ + rbox.txt + +HERE-HTML + +################################################### +########### ../src/Makefile.am ################### +################################################### + +echo Create ../src/Makefile.am +cat >../src/Makefile.am <<\HERE-SRC +### Makefile.am for the qhull package (src) +### Author: Rafael Laboissiere <rafael@debian.org> +### Created: Mon Dec 3 21:36:21 CET 2001 + +### Shared Library + +# to: +lib_LTLIBRARIES = libqhull.la + +# from: +libqhull_la_SOURCES = \ + user.c \ + global.c \ + stat.c \ + io.c \ + geom2.c \ + poly2.c \ + merge.c \ + qhull.c \ + geom.c \ + poly.c \ + qset.c \ + mem.c + +# how: +libqhull_la_LDFLAGS = -version-info 0:0:0 -lm + +### Utility programs + +# to: +bin_PROGRAMS = qhull rbox qconvex qdelaunay qvoronoi qhalf + +# from: +qhull_SOURCES = unix.c +rbox_SOURCES = rbox.c +qconvex_SOURCES = qconvex.c +qdelaunay_SOURCES = qdelaun.c +qvoronoi_SOURCES = qvoronoi.c +qhalf_SOURCES = qhalf.c + +# how: +qhull_LDADD = libqhull.la +rbox_LDADD = libqhull.la +qconvex_LDADD = libqhull.la +qdelaunay_LDADD = libqhull.la +qvoronoi_LDADD = libqhull.la +qhalf_LDADD = libqhull.la + +### Include files + +pkginclude_HEADERS = \ + geom.h \ + mem.h \ + poly.h \ + qhull_a.h \ + stat.h \ + io.h \ + merge.h \ + qhull.h \ + qset.h \ + user.h + + +### Example programs + +# to: +docdir = $(prefix)/share/doc/$(PACKAGE) +examplesdir = $(docdir)/examples + +# which: +examples_DATA = \ + user_eg.c \ + user_eg2.c \ + qhull_interface.cpp \ + Makefile.txt \ + Make-config.sh \ + MBorland + +doc_DATA = Changes.txt \ + index.htm \ + qh-geom.htm \ + qh-globa.htm \ + qh-io.htm \ + qh-mem.htm \ + qh-merge.htm \ + qh-poly.htm \ + qh-qhull.htm \ + qh-set.htm \ + qh-stat.htm \ + qh-user.htm + + +### Extra files to be included in the tarball + +EXTRA_DIST = \ + $(doc_DATA) \ + $(examples_DATA) + +HERE-SRC + +################################################### +########### run automake autoconf ################ +################################################### + + +echo Run automake, libtoolize, and autoconf +cd ..; aclocal &&\ + automake --foreign --add-missing --force-missing && \ + libtoolize --force && \ + autoconf + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Makefile b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..498ffbade83e2a1bccff32d82cc9521d10ce3c3c --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Makefile @@ -0,0 +1,193 @@ +# Unix Makefile for qhull and rbox +# +# see README.txt +# the Unix distribution contains a configure Makefile +# +# make to produce qhull qconvex qdelaunay qhalf qvoronoi rbox +# make qvoronoi to produce qvoronoi (etc.) +# make qhullx to produce qhull qconvex etc. w/o using libqhull.so +# make doc to print documentation +# make install to copy qhull, rbox, qhull.1, rbox.1 to BINDIR, MANDIR +# make new to rebuild qhull and rbox from source +# +# make printall to print all files +# make user_eg to produce user_eg +# make user_eg2 to produce user_eg2 +# make clean to remove object files and core +# make cleanall to remove all generated files +# +# PRINTMAN -- command for printing manual pages +# PRINTC -- command for printing C files +# BINDIR -- directory where to copy executables +# MANDIR -- directory where to copy manual pages +# CC -- ANSI C or C++ compiler +# CCOPTS1 - options used to compile .c files +# CCOPTS2 -- options used to link .o files +# +# CFILES -- .c files for printing +# HFILES -- .h files for printing +# DFILES -- documentation files +# MFILES -- man pages and html files +# TFILES -- .txt versions of html html files +# FILES -- all other files +# OBJS -- specifies the object files of libqhull.so +# +BINDIR = /usr/local/bin +MANDIR = /usr/local/man/man1 + +# if you do not have enscript, try a2ps or just use lpr. The files are text. +PRINTMAN = enscript -2rl +PRINTC = enscript -2r +# PRINTMAN = lpr +# PRINTC = lpr + +#for Gnu's gcc compiler -O2 for optimization, -g for debugging, -Wall for check +# +CC = gcc +CCOPTS1 = -O1 -ansi + +# for Sun's cc compiler, -fast or O2 for optimization, -g for debugging, -Xc for ANSI +#CC = cc +#CCOPTS1 = -Xc -v -fast + +# for Silicon Graphics cc compiler, -O2 for optimization, -g for debugging +#CC = cc +#CCOPTS1 = -ansi -O2 + +# for Next cc compiler with fat executable +#CC = cc +#CCOPTS1 = -ansi -O2 -arch m68k -arch i386 -arch hppa + +# for loader, ld +CCOPTS2 = $(CCOPTS1) + +# OBJS in execution frequency order. CFILES after qhull.c are alphabetical +OBJS = user.o global.o stat.o io.o geom2.o poly2.o \ + merge.o qhull.o geom.o poly.o qset.o mem.o + +CFILES= unix.c qhull.c geom.c geom2.c global.c io.c mem.c merge.c poly.c \ + poly2.c qset.c stat.c user.c qconvex.c qdelaun.c qhalf.c qvoronoi.c +HFILES= user.h qhull.h qhull_a.h geom.h io.h mem.h merge.h poly.h qset.h stat.h +TXTFILES= ../Announce.txt ../REGISTER.txt ../COPYING.txt ../README.txt Changes.txt +DOCFILES= ../html/rbox.txt ../html/qhull.txt +FILES= Makefile rbox.c user_eg.c ../eg/q_test ../eg/q_egtest ../eg/q_eg +HTMFILES= qhull.man rbox.man qh-in.htm qh-optg.htm qh-optt.htm qh-optp.htm \ + index.htm qh-quick.htm qh-impre.htm qh-eg.htm \ + qh-optc.htm qh-opto.htm qh-optf.htm qh-optq.htm \ + qh-c.htm qh-faq.htm qhull.htm qconvex.htm qdelaun.htm \ + qh-geom.htm qh-globa.htm qh-io.htm qh-mem.htm qh-merge.htm \ + qh-poly.htm qh-qhull.htm qh-set.htm qh-stat.htm qh-user.htm \ + qdelau_f.htm qhalf.htm qvoronoi.htm qvoron_f.htm rbox.htm + +all: rbox qconvex qdelaunay qhalf qvoronoi qhull + +unix.o: qhull.h user.h mem.h +qconvex.o: qhull.h user.h mem.h +qdelaun.o: qhull.h user.h mem.h +qhalf.o: qhull.h user.h mem.h +qvoronoi.o: qhull.h user.h mem.h +qhull.o: $(HFILES) +geom.o: $(HFILES) +geom2.o: $(HFILES) +global.o: $(HFILES) +io.o: $(HFILES) +mem.o: mem.h +merge.o: $(HFILES) +poly.o: $(HFILES) +poly2.o: $(HFILES) +qset.o: qset.h mem.h +stat.o: $(HFILES) +user.o: $(HFILES) + +.c.o: + $(CC) -c $(CCOPTS1) $< + +clean: + rm -f *.o ../core qconvex qdelaunay qhalf qvoronoi qhull libqhull.so ../lib/linux/libqhull.so ../../../lib/linux/libqhull.so + +cleanall: clean + rm -f *~ ../rbox ../qhull ../qhalf ../qconvex ../qdelaunay ../qhalf\ + ../qvoronoi ../user_eg ../user_eg2 ../*.exe >/dev/null + +doc: + $(PRINTMAN) $(TXTFILES) $(DOCFILES) + +install: all + cp ../qconvex $(BINDIR)/qconvex + cp ../qdelaunay $(BINDIR)/qdelaunay + cp ../qhalf $(BINDIR)/qhalf + cp ../qhull $(BINDIR)/qhull + cp ../qvoronoi $(BINDIR)/qvoronoi + cp ../rbox $(BINDIR)/rbox + cp ../html/qhull.man $(MANDIR)/qhull.1 + cp ../html/rbox.man $(MANDIR)/rbox.1 + +new: cleanall all + +printall: doc printh printc printf + +printh: + $(PRINTC) $(HFILES) + +printc: + $(PRINTC) $(CFILES) + +printf: + $(PRINTC) $(FILES) + +libqhull.so: $(OBJS) + ${CC} ${CCOPTS1} -shared -Wl,-soname,libqhull.so -o $@ $(OBJS) + cp libqhull.so ../lib/linux + cp libqhull.so ../../../lib/linux + #@echo if 'ar' or 'ranlib' fails, try 'make qhullx' + #ar rv libqhull.so $(OBJS) + #@echo the next line may need to be removed. + # -test -x /bin/ranlib -o -x /usr/bin/ranlib && ranlib libqhull.so + +# don't use ../qconvex. Does not work on Red Hat Linux +qconvex: qconvex.o libqhull.so + $(CC) -o qconvex $(CCOPTS2) qconvex.o -L. -lqhull -lm + cp qconvex .. + +qdelaunay: qdelaun.o libqhull.so + $(CC) -o qdelaunay $(CCOPTS2) qdelaun.o -L. -lqhull -lm + cp qdelaunay .. + +qhalf: qhalf.o libqhull.so + $(CC) -o qhalf $(CCOPTS2) qhalf.o -L. -lqhull -lm + cp qhalf .. + +qvoronoi: qvoronoi.o libqhull.so + $(CC) -o qvoronoi $(CCOPTS2) qvoronoi.o -L. -lqhull -lm + cp qvoronoi .. + +qhull: unix.o libqhull.so + $(CC) -o qhull $(CCOPTS2) unix.o -L. -lqhull -lm + cp qhull .. + -chmod +x ../eg/q_test ../eg/q_eg ../eg/q_egtest +#-cd ..; ./rbox D4 | ./qhull + +# compile qhull without using libqhull.so +qhullx: qconvex.o qdelaun.o qhalf.o qvoronoi.o unix.o $(OBJS) + $(CC) -o qconvex $(CCOPTS2) qconvex.o $(OBJS) -lm + $(CC) -o qdelaunay $(CCOPTS2) qdelaun.o $(OBJS) -lm + $(CC) -o qhalf $(CCOPTS2) qhalf.o $(OBJS) -lm + $(CC) -o qvoronoi $(CCOPTS2) qvoronoi.o $(OBJS) -lm + $(CC) -o qhull $(CCOPTS2) unix.o $(OBJS) -lm + cp qconvex qdelaunay qhalf qvoronoi qhull .. + -chmod +x ../eg/q_test ../eg/q_eg ../eg/q_egtest +#-cd ..; ./rbox D4 | ./qhull + +rbox: rbox.o + $(CC) -o rbox rbox.o $(CCOPTS2) -lm + cp rbox .. + +user_eg: user_eg.o libqhull.so + $(CC) -o user_eg $(CCOPTS2) user_eg.o -L. -lqhull -lm + cp user_eg .. + +user_eg2: user_eg2.o libqhull.so + $(CC) -o user_eg2 $(CCOPTS2) user_eg2.o -L. -lqhull -lm + cp user_eg2 .. + +# end of Makefile diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Mborland b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Mborland new file mode 100644 index 0000000000000000000000000000000000000000..103c49e7a916d707485d03b8e81710953e09ba71 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/Mborland @@ -0,0 +1,186 @@ +######################################################################### +# Borland C++ 4.02 for Win32 and DOS Power Pack # +# Makefile for qhull and rbox # +# # +# make -fMborland all to produce qconvex, qhull, and rbox # +# make -fMborland user_eg to produce user_eg # +# make -fMborland user_eg2 to produce user_eg2 # +# make -fMborland new to rebuild qhull and rbox from source # +# make -fMborland clean to remove object files # +# make -fMborland cleanall to remove all generated files # +# make -fMborland test to test rbox and qhull # +# # +# Author: D. Zwick of Germany, C.B. Barber # +######################################################################### + +CC = bcc32 # 32 bit compiler for DOS + # bcc32i - Intel's compiler +LINKER = $(CC) # bcc calls tlink32 with needed options +CFLAGS = -w- -A -O2 + # -w- no warnings, bcc doesn't handle assigns in conditions + # -A Ansi standard + # -X no auto-dependency outputs + # -v debugging, use CCOPTS for both + # -O2 optimization +!if $d(_DPMI) +LFLAGS = -WX -w- # -WX loads DPMI library +!else +LFLAGS = -lap -lx -lc + # -lap 32-bit console application + # -lx no map file + # -lc case is significant +!endif + +EXERB = rbox +EXEQH = qhull +EXEQC = qconvex +EXEQD = qdelaunay +EXEQV = qvoronoi +EXEQF = qhalf +EXEEG = user_eg +EXEEG2 = user_eg2 + +TMPFILE = BCC32tmp.cfg + +OBJS1 = global.obj stat.obj geom2.obj poly2.obj io.obj +OBJS2 = merge.obj qhull.obj mem.obj qset.obj poly.obj geom.obj + +HFILES1 = qhull.h stat.h qhull_a.h user.h + + +# General rules + +.c.obj: + $(CC) -c $(CFLAGS) $< + +# Default + +all: $(EXERB) $(EXEQH) $(EXEQC) $(EXEQD) $(EXEQV) $(EXEQF) test + +help: + @echo USAGE: + @echo "make all to produce qhull, rbox, qconvex, qdelaun, qvoronoi, qhalf" + @echo "make user_eg to produce user_eg" + @echo "make user_eg2 to produce user_eg2" + @echo "make new to rebuild qhull and rbox from source" + @echo "make clean to remove object files" + @echo "make cleanall to remove all generated file" + @echo "make test to test rbox and qhull" + @echo OPTIONS (default is 32-bit console app): + @echo "-D_DPMI for C++ 4.01 and DOS Power Pack" + +# Executables + +$(EXEQH): ..\$(EXEQH).exe + @echo Made ..\$(EXEQH).exe + +..\$(EXEQH).exe: unix.obj user.obj $(OBJS1) $(OBJS2) + @echo unix.obj user.obj > $(TMPFILE) + @echo $(OBJS1) >> $(TMPFILE) + @echo $(OBJS2) >> $(TMPFILE) + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) @$(TMPFILE) + +$(EXEQC): ..\$(EXEQC).exe + @echo Made ..\$(EXEQC).exe + +..\$(EXEQC).exe: qconvex.obj user.obj $(OBJS1) $(OBJS2) + @echo qconvex.obj user.obj > $(TMPFILE) + @echo $(OBJS1) >> $(TMPFILE) + @echo $(OBJS2) >> $(TMPFILE) + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) @$(TMPFILE) + +$(EXEQD): ..\$(EXEQD).exe + @echo Made ..\$(EXEQD).exe + +..\$(EXEQD).exe: qdelaun.obj user.obj $(OBJS1) $(OBJS2) + @echo qdelaun.obj user.obj > $(TMPFILE) + @echo $(OBJS1) >> $(TMPFILE) + @echo $(OBJS2) >> $(TMPFILE) + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) @$(TMPFILE) + +$(EXEQV): ..\$(EXEQV).exe + @echo Made ..\$(EXEQV).exe + +..\$(EXEQV).exe: qvoronoi.obj user.obj $(OBJS1) $(OBJS2) + @echo qvoronoi.obj user.obj > $(TMPFILE) + @echo $(OBJS1) >> $(TMPFILE) + @echo $(OBJS2) >> $(TMPFILE) + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) @$(TMPFILE) + +$(EXEQF): ..\$(EXEQF).exe + @echo Made ..\$(EXEQF).exe + +..\$(EXEQF).exe: qhalf.obj user.obj $(OBJS1) $(OBJS2) + @echo qhalf.obj user.obj > $(TMPFILE) + @echo $(OBJS1) >> $(TMPFILE) + @echo $(OBJS2) >> $(TMPFILE) + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) @$(TMPFILE) + +$(EXEEG): ..\$(EXEEG).exe + @echo Made ..\$(EXEEG).exe + +..\$(EXEEG).exe: user_eg.obj $(OBJS1) $(OBJS2) + @echo user_eg.obj user.obj > $(TMPFILE) + @echo $(OBJS1) >> $(TMPFILE) + @echo $(OBJS2) >> $(TMPFILE) + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) @$(TMPFILE) + +$(EXEEG2): ..\$(EXEEG2).exe + @echo Made ..\$(EXEEG2).exe + +..\$(EXEEG2).exe: user_eg2.obj $(OBJS1) $(OBJS2) + @echo user_eg2.obj > $(TMPFILE) + @echo $(OBJS1) >> $(TMPFILE) + @echo $(OBJS2) >> $(TMPFILE) + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) @$(TMPFILE) + +$(EXERB): ..\$(EXERB).exe + @echo Made ..\$(EXERB).exe + +..\$(EXERB).exe: rbox.obj + $(LINKER) -e$@ $(CFLAGS) $(LFLAGS) rbox.obj + +# Test rbox and qhull + +test: + @..\rbox D4 > test.x + @..\qhull <test.x + @del test.x + +# Clean up + +clean: + @del *.obj + @del $(TMPFILE) + +cleanall: clean + @del ..\$(EXERB).exe + @del ..\$(EXEQC).exe + @del ..\$(EXEQD).exe + @del ..\$(EXEQF).exe + @del ..\$(EXEQH).exe + @del ..\$(EXEQV).exe + @del ..\$(EXEEG).exe + @del ..\$(EXEEG2).exe + @del ..\q_test.x + @del ..\q_test.log.1 + +# Clean up and rebuild all + +new: cleanall all + +# Header file dependencies + +qhull.obj stat.obj user.obj global.obj: $(HFILES1) +geom.obj geom2.obj: $(HFILES1) geom.h +poly.obj poly2.obj: $(HFILES1) poly.h +io.obj: $(HFILES1) io.h +merge.obj: $(HFILES1) merge.h +mem.obj: mem.h +qset.obj: qset.h mem.h +unix.obj: qhull.h user.h +qconvex.obj: qhull.h user.h +qdelaun.obj: qhull.h user.h +qhalf.obj: qhull.h user.h +qvoronoi.obj: qhull.h user.h +rbox.obj: user.h diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom.c new file mode 100644 index 0000000000000000000000000000000000000000..af633eeafc2710e7b354e7c76b36b5d131d8126f --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom.c @@ -0,0 +1,1229 @@ +/*<html><pre> -<a href="qh-geom.htm" + >-------------------------------</a><a name="TOP">-</a> + + geom.c + geometric routines of qhull + + see qh-geom.htm and geom.h + + copyright (c) 1993-2003 The Geometry Center + + infrequent code goes into geom2.c +*/ + +#include "qhull_a.h" + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="distplane">-</a> + + qh_distplane( point, facet, dist ) + return distance from point to facet + + returns: + dist + if qh.RANDOMdist, joggles result + + notes: + dist > 0 if point is above facet (i.e., outside) + does not error (for sortfacets) + + see: + qh_distnorm in geom2.c +*/ +void qh_distplane (pointT *point, facetT *facet, realT *dist) { + coordT *normal= facet->normal, *coordp, randr; + int k; + + switch(qh hull_dim){ + case 2: + *dist= facet->offset + point[0] * normal[0] + point[1] * normal[1]; + break; + case 3: + *dist= facet->offset + point[0] * normal[0] + point[1] * normal[1] + point[2] * normal[2]; + break; + case 4: + *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]; + break; + case 5: + *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4]; + break; + case 6: + *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4]+point[5]*normal[5]; + break; + case 7: + *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4]+point[5]*normal[5]+point[6]*normal[6]; + break; + case 8: + *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4]+point[5]*normal[5]+point[6]*normal[6]+point[7]*normal[7]; + break; + default: + *dist= facet->offset; + coordp= point; + for (k= qh hull_dim; k--; ) + *dist += *coordp++ * *normal++; + break; + } + zinc_(Zdistplane); + if (!qh RANDOMdist && qh IStracing < 4) + return; + if (qh RANDOMdist) { + randr= qh_RANDOMint; + *dist += (2.0 * randr / qh_RANDOMmax - 1.0) * + qh RANDOMfactor * qh MAXabs_coord; + } + if (qh IStracing >= 4) { + fprintf (qh ferr, "qh_distplane: "); + fprintf (qh ferr, qh_REAL_1, *dist); + fprintf (qh ferr, "from p%d to f%d\n", qh_pointid(point), facet->id); + } + return; +} /* distplane */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="findbest">-</a> + + qh_findbest( point, startfacet, bestoutside, qh_ISnewfacets, qh_NOupper, dist, isoutside, numpart ) + find facet that is furthest below a point + for upperDelaunay facets + returns facet only if !qh_NOupper and clearly above + + input: + starts search at 'startfacet' (can not be flipped) + if !bestoutside (qh_ALL), stops at qh.MINoutside + + returns: + best facet (reports error if NULL) + early out if isoutside defined and bestdist > qh.MINoutside + dist is distance to facet + isoutside is true if point is outside of facet + numpart counts the number of distance tests + + see also: + qh_findbestnew() + + notes: + If merging (testhorizon), searches horizon facets of coplanar best facets because + after qh_distplane, this and qh_partitionpoint are the most expensive in 3-d + avoid calls to distplane, function calls, and real number operations. + caller traces result + Optimized for outside points. Tried recording a search set for qh_findhorizon. + Made code more complicated. + + when called by qh_partitionvisible(): + indicated by qh_ISnewfacets + qh.newfacet_list is list of simplicial, new facets + qh_findbestnew set if qh_sharpnewfacets returns True (to use qh_findbestnew) + qh.bestfacet_notsharp set if qh_sharpnewfacets returns False + + when called by qh_findfacet(), qh_partitionpoint(), qh_partitioncoplanar(), + qh_check_bestdist(), qh_addpoint() + indicated by !qh_ISnewfacets + returns best facet in neighborhood of given facet + this is best facet overall if dist > - qh.MAXcoplanar + or hull has at least a "spherical" curvature + + design: + initialize and test for early exit + repeat while there are better facets + for each neighbor of facet + exit if outside facet found + test for better facet + if point is inside and partitioning + test for new facets with a "sharp" intersection + if so, future calls go to qh_findbestnew() + test horizon facets +*/ +facetT *qh_findbest (pointT *point, facetT *startfacet, + boolT bestoutside, boolT isnewfacets, boolT noupper, + realT *dist, boolT *isoutside, int *numpart) { + realT bestdist= -REALmax/2 /* avoid underflow */; + facetT *facet, *neighbor, **neighborp; + facetT *bestfacet= NULL, *lastfacet= NULL; + int oldtrace= qh IStracing; + unsigned int visitid= ++qh visit_id; + int numpartnew=0; + boolT testhorizon = True; /* needed if precise, e.g., rbox c D6 | qhull Q0 Tv */ + + zinc_(Zfindbest); + if (qh IStracing >= 3 || (qh TRACElevel && qh TRACEpoint >= 0 && qh TRACEpoint == qh_pointid (point))) { + if (qh TRACElevel > qh IStracing) + qh IStracing= qh TRACElevel; + fprintf (qh ferr, "qh_findbest: point p%d starting at f%d isnewfacets? %d, unless %d exit if > %2.2g\n", + qh_pointid(point), startfacet->id, isnewfacets, bestoutside, qh MINoutside); + fprintf(qh ferr, " testhorizon? %d noupper? %d", testhorizon, noupper); + fprintf (qh ferr, " Last point added was p%d.", qh furthest_id); + fprintf(qh ferr, " Last merge was #%d. max_outside %2.2g\n", zzval_(Ztotmerge), qh max_outside); + } + if (isoutside) + *isoutside= True; + if (!startfacet->flipped) { /* test startfacet */ + *numpart= 1; + qh_distplane (point, startfacet, dist); /* this code is duplicated below */ + if (!bestoutside && *dist >= qh MINoutside + && (!startfacet->upperdelaunay || !noupper)) { + bestfacet= startfacet; + goto LABELreturn_best; + } + bestdist= *dist; + if (!startfacet->upperdelaunay) { + bestfacet= startfacet; + } + }else + *numpart= 0; + startfacet->visitid= visitid; + facet= startfacet; + while (facet) { + trace4((qh ferr, "qh_findbest: neighbors of f%d, bestdist %2.2g f%d\n", + facet->id, bestdist, getid_(bestfacet))); + lastfacet= facet; + FOREACHneighbor_(facet) { + if (!neighbor->newfacet && isnewfacets) + continue; + if (neighbor->visitid == visitid) + continue; + neighbor->visitid= visitid; + if (!neighbor->flipped) { /* code duplicated above */ + (*numpart)++; + qh_distplane (point, neighbor, dist); + if (*dist > bestdist) { + if (!bestoutside && *dist >= qh MINoutside + && (!neighbor->upperdelaunay || !noupper)) { + bestfacet= neighbor; + goto LABELreturn_best; + } + if (!neighbor->upperdelaunay) { + bestfacet= neighbor; + bestdist= *dist; + break; /* switch to neighbor */ + }else if (!bestfacet) { + bestdist= *dist; + break; /* switch to neighbor */ + } + } /* end of *dist>bestdist */ + } /* end of !flipped */ + } /* end of FOREACHneighbor */ + facet= neighbor; /* non-NULL only if *dist>bestdist */ + } /* end of while facet (directed search) */ + if (isnewfacets) { + if (!bestfacet) { + bestdist= -REALmax/2; + bestfacet= qh_findbestnew (point, startfacet->next, &bestdist, bestoutside, isoutside, &numpartnew); + testhorizon= False; /* qh_findbestnew calls qh_findbesthorizon */ + }else if (!qh findbest_notsharp && bestdist < - qh DISTround) { + if (qh_sharpnewfacets()) { + /* seldom used, qh_findbestnew will retest all facets */ + zinc_(Zfindnewsharp); + bestfacet= qh_findbestnew (point, bestfacet, &bestdist, bestoutside, isoutside, &numpartnew); + testhorizon= False; /* qh_findbestnew calls qh_findbesthorizon */ + qh findbestnew= True; + }else + qh findbest_notsharp= True; + } + } + if (!bestfacet) + bestfacet= qh_findbestlower (lastfacet, point, &bestdist, numpart); + if (testhorizon) + bestfacet= qh_findbesthorizon (!qh_IScheckmax, point, bestfacet, noupper, &bestdist, &numpartnew); + *dist= bestdist; + if (isoutside && bestdist < qh MINoutside) + *isoutside= False; +LABELreturn_best: + zadd_(Zfindbesttot, *numpart); + zmax_(Zfindbestmax, *numpart); + (*numpart) += numpartnew; + qh IStracing= oldtrace; + return bestfacet; +} /* findbest */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="findbesthorizon">-</a> + + qh_findbesthorizon( qh_IScheckmax, point, startfacet, qh_NOupper, &bestdist, &numpart ) + search coplanar and better horizon facets from startfacet/bestdist + ischeckmax turns off statistics and minsearch update + all arguments must be initialized + returns (ischeckmax): + best facet + returns (!ischeckmax): + best facet that is not upperdelaunay + allows upperdelaunay that is clearly outside + returns: + bestdist is distance to bestfacet + numpart -- updates number of distance tests + + notes: + no early out -- use qh_findbest() or qh_findbestnew() + Searches coplanar or better horizon facets + + when called by qh_check_maxout() (qh_IScheckmax) + startfacet must be closest to the point + Otherwise, if point is beyond and below startfacet, startfacet may be a local minimum + even though other facets are below the point. + updates facet->maxoutside for good, visited facets + may return NULL + + searchdist is qh.max_outside + 2 * DISTround + + max( MINvisible('Vn'), MAXcoplanar('Un')); + This setting is a guess. It must be at least max_outside + 2*DISTround + because a facet may have a geometric neighbor across a vertex + + design: + for each horizon facet of coplanar best facets + continue if clearly inside + unless upperdelaunay or clearly outside + update best facet +*/ +facetT *qh_findbesthorizon (boolT ischeckmax, pointT* point, facetT *startfacet, boolT noupper, realT *bestdist, int *numpart) { + facetT *bestfacet= startfacet; + realT dist; + facetT *neighbor, **neighborp, *facet; + facetT *nextfacet= NULL; /* optimize last facet of coplanarset */ + int numpartinit= *numpart, coplanarset_size; + unsigned int visitid= ++qh visit_id; + boolT newbest= False; /* for tracing */ + realT minsearch, searchdist; /* skip facets that are too far from point */ + + if (!ischeckmax) { + zinc_(Zfindhorizon); + }else { +#if qh_MAXoutside + if ((!qh ONLYgood || startfacet->good) && *bestdist > startfacet->maxoutside) + startfacet->maxoutside= *bestdist; +#endif + } + searchdist= qh_SEARCHdist; /* multiple of qh.max_outside and precision constants */ + minsearch= *bestdist - searchdist; + if (ischeckmax) { + /* Always check coplanar facets. Needed for RBOX 1000 s Z1 G1e-13 t996564279 | QHULL Tv */ + minimize_(minsearch, -searchdist); + } + coplanarset_size= 0; + facet= startfacet; + while (True) { + trace4((qh ferr, "qh_findbesthorizon: neighbors of f%d bestdist %2.2g f%d ischeckmax? %d noupper? %d minsearch %2.2g searchdist %2.2g\n", + facet->id, *bestdist, getid_(bestfacet), ischeckmax, noupper, + minsearch, searchdist)); + FOREACHneighbor_(facet) { + if (neighbor->visitid == visitid) + continue; + neighbor->visitid= visitid; + if (!neighbor->flipped) { + qh_distplane (point, neighbor, &dist); + (*numpart)++; + if (dist > *bestdist) { + if (!neighbor->upperdelaunay || ischeckmax || (!noupper && dist >= qh MINoutside)) { + bestfacet= neighbor; + *bestdist= dist; + newbest= True; + if (!ischeckmax) { + minsearch= dist - searchdist; + if (dist > *bestdist + searchdist) { + zinc_(Zfindjump); /* everything in qh.coplanarset at least searchdist below */ + coplanarset_size= 0; + } + } + } + }else if (dist < minsearch) + continue; /* if ischeckmax, dist can't be positive */ +#if qh_MAXoutside + if (ischeckmax && dist > neighbor->maxoutside) + neighbor->maxoutside= dist; +#endif + } /* end of !flipped */ + if (nextfacet) { + if (!coplanarset_size++) { + SETfirst_(qh coplanarset)= nextfacet; + SETtruncate_(qh coplanarset, 1); + }else + qh_setappend (&qh coplanarset, nextfacet); /* Was needed for RBOX 1000 s W1e-13 P0 t996547055 | QHULL d Qbb Qc Tv + and RBOX 1000 s Z1 G1e-13 t996564279 | qhull Tv */ + } + nextfacet= neighbor; + } /* end of EACHneighbor */ + facet= nextfacet; + if (facet) + nextfacet= NULL; + else if (!coplanarset_size) + break; + else if (!--coplanarset_size) { + facet= SETfirst_(qh coplanarset); + SETtruncate_(qh coplanarset, 0); + }else + facet= (facetT*)qh_setdellast (qh coplanarset); + } /* while True, for each facet in qh.coplanarset */ + if (!ischeckmax) { + zadd_(Zfindhorizontot, *numpart - numpartinit); + zmax_(Zfindhorizonmax, *numpart - numpartinit); + if (newbest) + zinc_(Zparthorizon); + } + trace4((qh ferr, "qh_findbesthorizon: newbest? %d bestfacet f%d bestdist %2.2g\n", newbest, getid_(bestfacet), *bestdist)); + return bestfacet; +} /* findbesthorizon */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="findbestnew">-</a> + + qh_findbestnew( point, startfacet, dist, isoutside, numpart ) + find best newfacet for point + searches all of qh.newfacet_list starting at startfacet + searches horizon facets of coplanar best newfacets + searches all facets if startfacet == qh.facet_list + returns: + best new or horizon facet that is not upperdelaunay + early out if isoutside and not 'Qf' + dist is distance to facet + isoutside is true if point is outside of facet + numpart is number of distance tests + + notes: + Always used for merged new facets (see qh_USEfindbestnew) + Avoids upperdelaunay facet unless (isoutside and outside) + + Uses qh.visit_id, qh.coplanarset. + If share visit_id with qh_findbest, coplanarset is incorrect. + + If merging (testhorizon), searches horizon facets of coplanar best facets because + a point maybe coplanar to the bestfacet, below its horizon facet, + and above a horizon facet of a coplanar newfacet. For example, + rbox 1000 s Z1 G1e-13 | qhull + rbox 1000 s W1e-13 P0 t992110337 | QHULL d Qbb Qc + + qh_findbestnew() used if + qh_sharpnewfacets -- newfacets contains a sharp angle + if many merges, qh_premerge found a merge, or 'Qf' (qh.findbestnew) + + see also: + qh_partitionall() and qh_findbest() + + design: + for each new facet starting from startfacet + test distance from point to facet + return facet if clearly outside + unless upperdelaunay and a lowerdelaunay exists + update best facet + test horizon facets +*/ +facetT *qh_findbestnew (pointT *point, facetT *startfacet, + realT *dist, boolT bestoutside, boolT *isoutside, int *numpart) { + realT bestdist= -REALmax/2; + facetT *bestfacet= NULL, *facet; + int oldtrace= qh IStracing, i; + unsigned int visitid= ++qh visit_id; + realT distoutside= 0.0; + boolT isdistoutside; /* True if distoutside is defined */ + boolT testhorizon = True; /* needed if precise, e.g., rbox c D6 | qhull Q0 Tv */ + + if (!startfacet) { + if (qh MERGING) + fprintf(qh ferr, "qhull precision error (qh_findbestnew): merging has formed and deleted a cone of new facets. Can not continue.\n"); + else + fprintf(qh ferr, "qhull internal error (qh_findbestnew): no new facets for point p%d\n", + qh furthest_id); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + zinc_(Zfindnew); + if (qh BESToutside || bestoutside) + isdistoutside= False; + else { + isdistoutside= True; + distoutside= qh_DISToutside; /* multiple of qh.MINoutside & qh.max_outside, see user.h */ + } + if (isoutside) + *isoutside= True; + *numpart= 0; + if (qh IStracing >= 3 || (qh TRACElevel && qh TRACEpoint >= 0 && qh TRACEpoint == qh_pointid (point))) { + if (qh TRACElevel > qh IStracing) + qh IStracing= qh TRACElevel; + fprintf(qh ferr, "qh_findbestnew: point p%d facet f%d. Stop? %d if dist > %2.2g\n", + qh_pointid(point), startfacet->id, isdistoutside, distoutside); + fprintf(qh ferr, " Last point added p%d visitid %d.", qh furthest_id, visitid); + fprintf(qh ferr, " Last merge was #%d.\n", zzval_(Ztotmerge)); + } + /* visit all new facets starting with startfacet, maybe qh facet_list */ + for (i= 0, facet= startfacet; i < 2; i++, facet= qh newfacet_list) { + FORALLfacet_(facet) { + if (facet == startfacet && i) + break; + facet->visitid= visitid; + if (!facet->flipped) { + qh_distplane (point, facet, dist); + (*numpart)++; + if (*dist > bestdist) { + if (!facet->upperdelaunay || *dist >= qh MINoutside) { + bestfacet= facet; + if (isdistoutside && *dist >= distoutside) + goto LABELreturn_bestnew; + bestdist= *dist; + } + } + } /* end of !flipped */ + } /* FORALLfacet from startfacet or qh newfacet_list */ + } + if (testhorizon || !bestfacet) + bestfacet= qh_findbesthorizon (!qh_IScheckmax, point, bestfacet ? bestfacet : startfacet, + !qh_NOupper, &bestdist, numpart); + *dist= bestdist; + if (isoutside && *dist < qh MINoutside) + *isoutside= False; +LABELreturn_bestnew: + zadd_(Zfindnewtot, *numpart); + zmax_(Zfindnewmax, *numpart); + trace4((qh ferr, "qh_findbestnew: bestfacet f%d bestdist %2.2g\n", getid_(bestfacet), *dist)); + qh IStracing= oldtrace; + return bestfacet; +} /* findbestnew */ + +/* ============ hyperplane functions -- keep code together [?] ============ */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="backnormal">-</a> + + qh_backnormal( rows, numrow, numcol, sign, normal, nearzero ) + given an upper-triangular rows array and a sign, + solve for normal equation x using back substitution over rows U + + returns: + normal= x + + if will not be able to divzero() when normalized (qh.MINdenom_2 and qh.MINdenom_1_2), + if fails on last row + this means that the hyperplane intersects [0,..,1] + sets last coordinate of normal to sign + otherwise + sets tail of normal to [...,sign,0,...], i.e., solves for b= [0...0] + sets nearzero + + notes: + assumes numrow == numcol-1 + + see Golub & van Loan 4.4-9 for back substitution + + solves Ux=b where Ax=b and PA=LU + b= [0,...,0,sign or 0] (sign is either -1 or +1) + last row of A= [0,...,0,1] + + 1) Ly=Pb == y=b since P only permutes the 0's of b + + design: + for each row from end + perform back substitution + if near zero + use qh_divzero for division + if zero divide and not last row + set tail of normal to 0 +*/ +void qh_backnormal (realT **rows, int numrow, int numcol, boolT sign, + coordT *normal, boolT *nearzero) { + int i, j; + coordT *normalp, *normal_tail, *ai, *ak; + realT diagonal; + boolT waszero; + int zerocol= -1; + + normalp= normal + numcol - 1; + *normalp--= (sign ? -1.0 : 1.0); + for(i= numrow; i--; ) { + *normalp= 0.0; + ai= rows[i] + i + 1; + ak= normalp+1; + for(j= i+1; j < numcol; j++) + *normalp -= *ai++ * *ak++; + diagonal= (rows[i])[i]; + if (fabs_(diagonal) > qh MINdenom_2) + *(normalp--) /= diagonal; + else { + waszero= False; + *normalp= qh_divzero (*normalp, diagonal, qh MINdenom_1_2, &waszero); + if (waszero) { + zerocol= i; + *(normalp--)= (sign ? -1.0 : 1.0); + for (normal_tail= normalp+2; normal_tail < normal + numcol; normal_tail++) + *normal_tail= 0.0; + }else + normalp--; + } + } + if (zerocol != -1) { + zzinc_(Zback0); + *nearzero= True; + trace4((qh ferr, "qh_backnormal: zero diagonal at column %d.\n", i)); + qh_precision ("zero diagonal on back substitution"); + } +} /* backnormal */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="gausselim">-</a> + + qh_gausselim( rows, numrow, numcol, sign ) + Gaussian elimination with partial pivoting + + returns: + rows is upper triangular (includes row exchanges) + flips sign for each row exchange + sets nearzero if pivot[k] < qh.NEARzero[k], else clears it + + notes: + if nearzero, the determinant's sign may be incorrect. + assumes numrow <= numcol + + design: + for each row + determine pivot and exchange rows if necessary + test for near zero + perform gaussian elimination step +*/ +void qh_gausselim(realT **rows, int numrow, int numcol, boolT *sign, boolT *nearzero) { + realT *ai, *ak, *rowp, *pivotrow; + realT n, pivot, pivot_abs= 0.0, temp; + int i, j, k, pivoti, flip=0; + + *nearzero= False; + for(k= 0; k < numrow; k++) { + pivot_abs= fabs_((rows[k])[k]); + pivoti= k; + for(i= k+1; i < numrow; i++) { + if ((temp= fabs_((rows[i])[k])) > pivot_abs) { + pivot_abs= temp; + pivoti= i; + } + } + if (pivoti != k) { + rowp= rows[pivoti]; + rows[pivoti]= rows[k]; + rows[k]= rowp; + *sign ^= 1; + flip ^= 1; + } + if (pivot_abs <= qh NEARzero[k]) { + *nearzero= True; + if (pivot_abs == 0.0) { /* remainder of column == 0 */ + if (qh IStracing >= 4) { + fprintf (qh ferr, "qh_gausselim: 0 pivot at column %d. (%2.2g < %2.2g)\n", k, pivot_abs, qh DISTround); + qh_printmatrix (qh ferr, "Matrix:", rows, numrow, numcol); + } + zzinc_(Zgauss0); + qh_precision ("zero pivot for Gaussian elimination"); + goto LABELnextcol; + } + } + pivotrow= rows[k] + k; + pivot= *pivotrow++; /* signed value of pivot, and remainder of row */ + for(i= k+1; i < numrow; i++) { + ai= rows[i] + k; + ak= pivotrow; + n= (*ai++)/pivot; /* divzero() not needed since |pivot| >= |*ai| */ + for(j= numcol - (k+1); j--; ) + *ai++ -= n * *ak++; + } + LABELnextcol: + ; + } + wmin_(Wmindenom, pivot_abs); /* last pivot element */ + if (qh IStracing >= 5) + qh_printmatrix (qh ferr, "qh_gausselem: result", rows, numrow, numcol); +} /* gausselim */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="getangle">-</a> + + qh_getangle( vect1, vect2 ) + returns the dot product of two vectors + if qh.RANDOMdist, joggles result + + notes: + the angle may be > 1.0 or < -1.0 because of roundoff errors + +*/ +realT qh_getangle(pointT *vect1, pointT *vect2) { + realT angle= 0, randr; + int k; + + for(k= qh hull_dim; k--; ) + angle += *vect1++ * *vect2++; + if (qh RANDOMdist) { + randr= qh_RANDOMint; + angle += (2.0 * randr / qh_RANDOMmax - 1.0) * + qh RANDOMfactor; + } + trace4((qh ferr, "qh_getangle: %2.2g\n", angle)); + return(angle); +} /* getangle */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="getcenter">-</a> + + qh_getcenter( vertices ) + returns arithmetic center of a set of vertices as a new point + + notes: + allocates point array for center +*/ +pointT *qh_getcenter(setT *vertices) { + int k; + pointT *center, *coord; + vertexT *vertex, **vertexp; + int count= qh_setsize(vertices); + + if (count < 2) { + fprintf (qh ferr, "qhull internal error (qh_getcenter): not defined for %d points\n", count); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + center= (pointT *)qh_memalloc(qh normal_size); + for (k=0; k < qh hull_dim; k++) { + coord= center+k; + *coord= 0.0; + FOREACHvertex_(vertices) + *coord += vertex->point[k]; + *coord /= count; + } + return(center); +} /* getcenter */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="getcentrum">-</a> + + qh_getcentrum( facet ) + returns the centrum for a facet as a new point + + notes: + allocates the centrum +*/ +pointT *qh_getcentrum(facetT *facet) { + realT dist; + pointT *centrum, *point; + + point= qh_getcenter(facet->vertices); + zzinc_(Zcentrumtests); + qh_distplane (point, facet, &dist); + centrum= qh_projectpoint(point, facet, dist); + qh_memfree(point, qh normal_size); + trace4((qh ferr, "qh_getcentrum: for f%d, %d vertices dist= %2.2g\n", + facet->id, qh_setsize(facet->vertices), dist)); + return centrum; +} /* getcentrum */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="getdistance">-</a> + + qh_getdistance( facet, neighbor, mindist, maxdist ) + returns the maxdist and mindist distance of any vertex from neighbor + + returns: + the max absolute value + + design: + for each vertex of facet that is not in neighbor + test the distance from vertex to neighbor +*/ +realT qh_getdistance(facetT *facet, facetT *neighbor, realT *mindist, realT *maxdist) { + vertexT *vertex, **vertexp; + realT dist, maxd, mind; + + FOREACHvertex_(facet->vertices) + vertex->seen= False; + FOREACHvertex_(neighbor->vertices) + vertex->seen= True; + mind= 0.0; + maxd= 0.0; + FOREACHvertex_(facet->vertices) { + if (!vertex->seen) { + zzinc_(Zbestdist); + qh_distplane(vertex->point, neighbor, &dist); + if (dist < mind) + mind= dist; + else if (dist > maxd) + maxd= dist; + } + } + *mindist= mind; + *maxdist= maxd; + mind= -mind; + if (maxd > mind) + return maxd; + else + return mind; +} /* getdistance */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="normalize">-</a> + + qh_normalize( normal, dim, toporient ) + normalize a vector and report if too small + does not use min norm + + see: + qh_normalize2 +*/ +void qh_normalize (coordT *normal, int dim, boolT toporient) { + qh_normalize2( normal, dim, toporient, NULL, NULL); +} /* normalize */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="normalize2">-</a> + + qh_normalize2( normal, dim, toporient, minnorm, ismin ) + normalize a vector and report if too small + qh.MINdenom/MINdenom1 are the upper limits for divide overflow + + returns: + normalized vector + flips sign if !toporient + if minnorm non-NULL, + sets ismin if normal < minnorm + + notes: + if zero norm + sets all elements to sqrt(1.0/dim) + if divide by zero (divzero ()) + sets largest element to +/-1 + bumps Znearlysingular + + design: + computes norm + test for minnorm + if not near zero + normalizes normal + else if zero norm + sets normal to standard value + else + uses qh_divzero to normalize + if nearzero + sets norm to direction of maximum value +*/ +void qh_normalize2 (coordT *normal, int dim, boolT toporient, + realT *minnorm, boolT *ismin) { + int k; + realT *colp, *maxp, norm= 0, temp, *norm1, *norm2, *norm3; + boolT zerodiv; + + norm1= normal+1; + norm2= normal+2; + norm3= normal+3; + if (dim == 2) + norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1)); + else if (dim == 3) + norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2)); + else if (dim == 4) { + norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2) + + (*norm3)*(*norm3)); + }else if (dim > 4) { + norm= (*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2) + + (*norm3)*(*norm3); + for (k= dim-4, colp= normal+4; k--; colp++) + norm += (*colp) * (*colp); + norm= sqrt(norm); + } + if (minnorm) { + if (norm < *minnorm) + *ismin= True; + else + *ismin= False; + } + wmin_(Wmindenom, norm); + if (norm > qh MINdenom) { + if (!toporient) + norm= -norm; + *normal /= norm; + *norm1 /= norm; + if (dim == 2) + ; /* all done */ + else if (dim == 3) + *norm2 /= norm; + else if (dim == 4) { + *norm2 /= norm; + *norm3 /= norm; + }else if (dim >4) { + *norm2 /= norm; + *norm3 /= norm; + for (k= dim-4, colp= normal+4; k--; ) + *colp++ /= norm; + } + }else if (norm == 0.0) { + temp= sqrt (1.0/dim); + for (k= dim, colp= normal; k--; ) + *colp++ = temp; + }else { + if (!toporient) + norm= -norm; + for (k= dim, colp= normal; k--; colp++) { /* k used below */ + temp= qh_divzero (*colp, norm, qh MINdenom_1, &zerodiv); + if (!zerodiv) + *colp= temp; + else { + maxp= qh_maxabsval(normal, dim); + temp= ((*maxp * norm >= 0.0) ? 1.0 : -1.0); + for (k= dim, colp= normal; k--; colp++) + *colp= 0.0; + *maxp= temp; + zzinc_(Znearlysingular); + trace0((qh ferr, "qh_normalize: norm=%2.2g too small during p%d\n", + norm, qh furthest_id)); + return; + } + } + } +} /* normalize */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="projectpoint">-</a> + + qh_projectpoint( point, facet, dist ) + project point onto a facet by dist + + returns: + returns a new point + + notes: + if dist= distplane(point,facet) + this projects point to hyperplane + assumes qh_memfree_() is valid for normal_size +*/ +pointT *qh_projectpoint(pointT *point, facetT *facet, realT dist) { + pointT *newpoint, *np, *normal; + int normsize= qh normal_size,k; + void **freelistp; /* used !qh_NOmem */ + + qh_memalloc_(normsize, freelistp, newpoint, pointT); + np= newpoint; + normal= facet->normal; + for(k= qh hull_dim; k--; ) + *(np++)= *point++ - dist * *normal++; + return(newpoint); +} /* projectpoint */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="setfacetplane">-</a> + + qh_setfacetplane( facet ) + sets the hyperplane for a facet + if qh.RANDOMdist, joggles hyperplane + + notes: + uses global buffers qh.gm_matrix and qh.gm_row + overwrites facet->normal if already defined + updates Wnewvertex if PRINTstatistics + sets facet->upperdelaunay if upper envelope of Delaunay triangulation + + design: + copy vertex coordinates to qh.gm_matrix/gm_row + compute determinate + if nearzero + recompute determinate with gaussian elimination + if nearzero + force outside orientation by testing interior point +*/ +void qh_setfacetplane(facetT *facet) { + pointT *point; + vertexT *vertex, **vertexp; + int k,i, normsize= qh normal_size, oldtrace= 0; + realT dist; + void **freelistp; /* used !qh_NOmem */ + coordT *coord, *gmcoord; + pointT *point0= SETfirstt_(facet->vertices, vertexT)->point; + boolT nearzero= False; + + zzinc_(Zsetplane); + if (!facet->normal) + qh_memalloc_(normsize, freelistp, facet->normal, coordT); + if (facet == qh tracefacet) { + oldtrace= qh IStracing; + qh IStracing= 5; + fprintf (qh ferr, "qh_setfacetplane: facet f%d created.\n", facet->id); + fprintf (qh ferr, " Last point added to hull was p%d.", qh furthest_id); + if (zzval_(Ztotmerge)) + fprintf(qh ferr, " Last merge was #%d.", zzval_(Ztotmerge)); + fprintf (qh ferr, "\n\nCurrent summary is:\n"); + qh_printsummary (qh ferr); + } + if (qh hull_dim <= 4) { + i= 0; + if (qh RANDOMdist) { + gmcoord= qh gm_matrix; + FOREACHvertex_(facet->vertices) { + qh gm_row[i++]= gmcoord; + coord= vertex->point; + for (k= qh hull_dim; k--; ) + *(gmcoord++)= *coord++ * qh_randomfactor(); + } + }else { + FOREACHvertex_(facet->vertices) + qh gm_row[i++]= vertex->point; + } + qh_sethyperplane_det(qh hull_dim, qh gm_row, point0, facet->toporient, + facet->normal, &facet->offset, &nearzero); + } + if (qh hull_dim > 4 || nearzero) { + i= 0; + gmcoord= qh gm_matrix; + FOREACHvertex_(facet->vertices) { + if (vertex->point != point0) { + qh gm_row[i++]= gmcoord; + coord= vertex->point; + point= point0; + for(k= qh hull_dim; k--; ) + *(gmcoord++)= *coord++ - *point++; + } + } + qh gm_row[i]= gmcoord; /* for areasimplex */ + if (qh RANDOMdist) { + gmcoord= qh gm_matrix; + for (i= qh hull_dim-1; i--; ) { + for (k= qh hull_dim; k--; ) + *(gmcoord++) *= qh_randomfactor(); + } + } + qh_sethyperplane_gauss(qh hull_dim, qh gm_row, point0, facet->toporient, + facet->normal, &facet->offset, &nearzero); + if (nearzero) { + if (qh_orientoutside (facet)) { + trace0((qh ferr, "qh_setfacetplane: flipped orientation after testing interior_point during p%d\n", qh furthest_id)); + /* this is part of using Gaussian Elimination. For example in 5-d + 1 1 1 1 0 + 1 1 1 1 1 + 0 0 0 1 0 + 0 1 0 0 0 + 1 0 0 0 0 + norm= 0.38 0.38 -0.76 0.38 0 + has a determinate of 1, but g.e. after subtracting pt. 0 has + 0's in the diagonal, even with full pivoting. It does work + if you subtract pt. 4 instead. */ + } + } + } + facet->upperdelaunay= False; + if (qh DELAUNAY) { + if (qh UPPERdelaunay) { /* matches qh_triangulate_facet and qh.lower_threshold in qh_initbuild */ + if (facet->normal[qh hull_dim -1] >= qh ANGLEround * qh_ZEROdelaunay) + facet->upperdelaunay= True; + }else { + if (facet->normal[qh hull_dim -1] > -qh ANGLEround * qh_ZEROdelaunay) + facet->upperdelaunay= True; + } + } + if (qh PRINTstatistics || qh IStracing || qh TRACElevel || qh JOGGLEmax < REALmax) { + qh old_randomdist= qh RANDOMdist; + qh RANDOMdist= False; + FOREACHvertex_(facet->vertices) { + if (vertex->point != point0) { + boolT istrace= False; + zinc_(Zdiststat); + qh_distplane(vertex->point, facet, &dist); + dist= fabs_(dist); + zinc_(Znewvertex); + wadd_(Wnewvertex, dist); + if (dist > wwval_(Wnewvertexmax)) { + wwval_(Wnewvertexmax)= dist; + if (dist > qh max_outside) { + qh max_outside= dist; /* used by qh_maxouter() */ + if (dist > qh TRACEdist) + istrace= True; + } + }else if (-dist > qh TRACEdist) + istrace= True; + if (istrace) { + fprintf (qh ferr, "qh_setfacetplane: ====== vertex p%d (v%d) increases max_outside to %2.2g for new facet f%d last p%d\n", + qh_pointid(vertex->point), vertex->id, dist, facet->id, qh furthest_id); + qh_errprint ("DISTANT", facet, NULL, NULL, NULL); + } + } + } + qh RANDOMdist= qh old_randomdist; + } + if (qh IStracing >= 3) { + fprintf (qh ferr, "qh_setfacetplane: f%d offset %2.2g normal: ", + facet->id, facet->offset); + for (k=0; k < qh hull_dim; k++) + fprintf (qh ferr, "%2.2g ", facet->normal[k]); + fprintf (qh ferr, "\n"); + } + if (facet == qh tracefacet) + qh IStracing= oldtrace; +} /* setfacetplane */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="sethyperplane_det">-</a> + + qh_sethyperplane_det( dim, rows, point0, toporient, normal, offset, nearzero ) + given dim X dim array indexed by rows[], one row per point, + toporient (flips all signs), + and point0 (any row) + set normalized hyperplane equation from oriented simplex + + returns: + normal (normalized) + offset (places point0 on the hyperplane) + sets nearzero if hyperplane not through points + + notes: + only defined for dim == 2..4 + rows[] is not modified + solves det(P-V_0, V_n-V_0, ..., V_1-V_0)=0, i.e. every point is on hyperplane + see Bower & Woodworth, A programmer's geometry, Butterworths 1983. + + derivation of 3-d minnorm + Goal: all vertices V_i within qh.one_merge of hyperplane + Plan: exactly translate the facet so that V_0 is the origin + exactly rotate the facet so that V_1 is on the x-axis and y_2=0. + exactly rotate the effective perturbation to only effect n_0 + this introduces a factor of sqrt(3) + n_0 = ((y_2-y_0)*(z_1-z_0) - (z_2-z_0)*(y_1-y_0)) / norm + Let M_d be the max coordinate difference + Let M_a be the greater of M_d and the max abs. coordinate + Let u be machine roundoff and distround be max error for distance computation + The max error for n_0 is sqrt(3) u M_a M_d / norm. n_1 is approx. 1 and n_2 is approx. 0 + The max error for distance of V_1 is sqrt(3) u M_a M_d M_d / norm. Offset=0 at origin + Then minnorm = 1.8 u M_a M_d M_d / qh.ONEmerge + Note that qh.one_merge is approx. 45.5 u M_a and norm is usually about M_d M_d + + derivation of 4-d minnorm + same as above except rotate the facet so that V_1 on x-axis and w_2, y_3, w_3=0 + [if two vertices fixed on x-axis, can rotate the other two in yzw.] + n_0 = det3_(...) = y_2 det2_(z_1, w_1, z_3, w_3) = - y_2 w_1 z_3 + [all other terms contain at least two factors nearly zero.] + The max error for n_0 is sqrt(4) u M_a M_d M_d / norm + Then minnorm = 2 u M_a M_d M_d M_d / qh.ONEmerge + Note that qh.one_merge is approx. 82 u M_a and norm is usually about M_d M_d M_d +*/ +void qh_sethyperplane_det (int dim, coordT **rows, coordT *point0, + boolT toporient, coordT *normal, realT *offset, boolT *nearzero) { + realT maxround, dist; + int i; + pointT *point; + + + if (dim == 2) { + normal[0]= dY(1,0); + normal[1]= dX(0,1); + qh_normalize2 (normal, dim, toporient, NULL, NULL); + *offset= -(point0[0]*normal[0]+point0[1]*normal[1]); + *nearzero= False; /* since nearzero norm => incident points */ + }else if (dim == 3) { + normal[0]= det2_(dY(2,0), dZ(2,0), + dY(1,0), dZ(1,0)); + normal[1]= det2_(dX(1,0), dZ(1,0), + dX(2,0), dZ(2,0)); + normal[2]= det2_(dX(2,0), dY(2,0), + dX(1,0), dY(1,0)); + qh_normalize2 (normal, dim, toporient, NULL, NULL); + *offset= -(point0[0]*normal[0] + point0[1]*normal[1] + + point0[2]*normal[2]); + maxround= qh DISTround; + for (i=dim; i--; ) { + point= rows[i]; + if (point != point0) { + dist= *offset + (point[0]*normal[0] + point[1]*normal[1] + + point[2]*normal[2]); + if (dist > maxround || dist < -maxround) { + *nearzero= True; + break; + } + } + } + }else if (dim == 4) { + normal[0]= - det3_(dY(2,0), dZ(2,0), dW(2,0), + dY(1,0), dZ(1,0), dW(1,0), + dY(3,0), dZ(3,0), dW(3,0)); + normal[1]= det3_(dX(2,0), dZ(2,0), dW(2,0), + dX(1,0), dZ(1,0), dW(1,0), + dX(3,0), dZ(3,0), dW(3,0)); + normal[2]= - det3_(dX(2,0), dY(2,0), dW(2,0), + dX(1,0), dY(1,0), dW(1,0), + dX(3,0), dY(3,0), dW(3,0)); + normal[3]= det3_(dX(2,0), dY(2,0), dZ(2,0), + dX(1,0), dY(1,0), dZ(1,0), + dX(3,0), dY(3,0), dZ(3,0)); + qh_normalize2 (normal, dim, toporient, NULL, NULL); + *offset= -(point0[0]*normal[0] + point0[1]*normal[1] + + point0[2]*normal[2] + point0[3]*normal[3]); + maxround= qh DISTround; + for (i=dim; i--; ) { + point= rows[i]; + if (point != point0) { + dist= *offset + (point[0]*normal[0] + point[1]*normal[1] + + point[2]*normal[2] + point[3]*normal[3]); + if (dist > maxround || dist < -maxround) { + *nearzero= True; + break; + } + } + } + } + if (*nearzero) { + zzinc_(Zminnorm); + trace0((qh ferr, "qh_sethyperplane_det: degenerate norm during p%d.\n", qh furthest_id)); + zzinc_(Znearlysingular); + } +} /* sethyperplane_det */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="sethyperplane_gauss">-</a> + + qh_sethyperplane_gauss( dim, rows, point0, toporient, normal, offset, nearzero ) + given (dim-1) X dim array of rows[i]= V_{i+1} - V_0 (point0) + set normalized hyperplane equation from oriented simplex + + returns: + normal (normalized) + offset (places point0 on the hyperplane) + + notes: + if nearzero + orientation may be incorrect because of incorrect sign flips in gausselim + solves [V_n-V_0,...,V_1-V_0, 0 .. 0 1] * N == [0 .. 0 1] + or [V_n-V_0,...,V_1-V_0, 0 .. 0 1] * N == [0] + i.e., N is normal to the hyperplane, and the unnormalized + distance to [0 .. 1] is either 1 or 0 + + design: + perform gaussian elimination + flip sign for negative values + perform back substitution + normalize result + compute offset +*/ +void qh_sethyperplane_gauss (int dim, coordT **rows, pointT *point0, + boolT toporient, coordT *normal, coordT *offset, boolT *nearzero) { + coordT *pointcoord, *normalcoef; + int k; + boolT sign= toporient, nearzero2= False; + + qh_gausselim(rows, dim-1, dim, &sign, nearzero); + for(k= dim-1; k--; ) { + if ((rows[k])[k] < 0) + sign ^= 1; + } + if (*nearzero) { + zzinc_(Znearlysingular); + trace0((qh ferr, "qh_sethyperplane_gauss: nearly singular or axis parallel hyperplane during p%d.\n", qh furthest_id)); + qh_backnormal(rows, dim-1, dim, sign, normal, &nearzero2); + }else { + qh_backnormal(rows, dim-1, dim, sign, normal, &nearzero2); + if (nearzero2) { + zzinc_(Znearlysingular); + trace0((qh ferr, "qh_sethyperplane_gauss: singular or axis parallel hyperplane at normalization during p%d.\n", qh furthest_id)); + } + } + if (nearzero2) + *nearzero= True; + qh_normalize2(normal, dim, True, NULL, NULL); + pointcoord= point0; + normalcoef= normal; + *offset= -(*pointcoord++ * *normalcoef++); + for(k= dim-1; k--; ) + *offset -= *pointcoord++ * *normalcoef++; +} /* sethyperplane_gauss */ + + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom.h new file mode 100644 index 0000000000000000000000000000000000000000..6b8ee627884e9ef5e0d23f1fe6fc7bb3c1246113 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom.h @@ -0,0 +1,177 @@ +/*<html><pre> -<a href="qh-geom.htm" + >-------------------------------</a><a name="TOP">-</a> + + geom.h + header file for geometric routines + + see qh-geom.htm and geom.c + + copyright (c) 1993-2003 The Geometry Center +*/ + +#ifndef qhDEFgeom +#define qhDEFgeom 1 + +/* ============ -macros- ======================== */ + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="fabs_">-</a> + + fabs_(a) + returns the absolute value of a +*/ +#define fabs_( a ) ((( a ) < 0 ) ? -( a ):( a )) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="fmax_">-</a> + + fmax_(a,b) + returns the maximum value of a and b +*/ +#define fmax_( a,b ) ( ( a ) < ( b ) ? ( b ) : ( a ) ) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="fmin_">-</a> + + fmin_(a,b) + returns the minimum value of a and b +*/ +#define fmin_( a,b ) ( ( a ) > ( b ) ? ( b ) : ( a ) ) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="maximize_">-</a> + + maximize_(maxval, val) + set maxval to val if val is greater than maxval +*/ +#define maximize_( maxval, val ) {if (( maxval ) < ( val )) ( maxval )= ( val );} + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="minimize_">-</a> + + minimize_(minval, val) + set minval to val if val is less than minval +*/ +#define minimize_( minval, val ) {if (( minval ) > ( val )) ( minval )= ( val );} + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="det2_">-</a> + + det2_(a1, a2, + b1, b2) + + compute a 2-d determinate +*/ +#define det2_( a1,a2,b1,b2 ) (( a1 )*( b2 ) - ( a2 )*( b1 )) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="det3_">-</a> + + det3_(a1, a2, a3, + b1, b2, b3, + c1, c2, c3) + + compute a 3-d determinate +*/ +#define det3_( a1,a2,a3,b1,b2,b3,c1,c2,c3 ) ( ( a1 )*det2_( b2,b3,c2,c3 ) \ + - ( b1 )*det2_( a2,a3,c2,c3 ) + ( c1 )*det2_( a2,a3,b2,b3 ) ) + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="dX">-</a> + + dX( p1, p2 ) + dY( p1, p2 ) + dZ( p1, p2 ) + + given two indices into rows[], + + compute the difference between X, Y, or Z coordinates +*/ +#define dX( p1,p2 ) ( *( rows[p1] ) - *( rows[p2] )) +#define dY( p1,p2 ) ( *( rows[p1]+1 ) - *( rows[p2]+1 )) +#define dZ( p1,p2 ) ( *( rows[p1]+2 ) - *( rows[p2]+2 )) +#define dW( p1,p2 ) ( *( rows[p1]+3 ) - *( rows[p2]+3 )) + +/*============= prototypes in alphabetical order, infrequent at end ======= */ + +void qh_backnormal (realT **rows, int numrow, int numcol, boolT sign, coordT *normal, boolT *nearzero); +void qh_distplane (pointT *point, facetT *facet, realT *dist); +facetT *qh_findbest (pointT *point, facetT *startfacet, + boolT bestoutside, boolT isnewfacets, boolT noupper, + realT *dist, boolT *isoutside, int *numpart); +facetT *qh_findbesthorizon (boolT ischeckmax, pointT *point, + facetT *startfacet, boolT noupper, realT *bestdist, int *numpart); +facetT *qh_findbestnew (pointT *point, facetT *startfacet, realT *dist, + boolT bestoutside, boolT *isoutside, int *numpart); +void qh_gausselim(realT **rows, int numrow, int numcol, boolT *sign, boolT *nearzero); +realT qh_getangle(pointT *vect1, pointT *vect2); +pointT *qh_getcenter(setT *vertices); +pointT *qh_getcentrum(facetT *facet); +realT qh_getdistance(facetT *facet, facetT *neighbor, realT *mindist, realT *maxdist); +void qh_normalize (coordT *normal, int dim, boolT toporient); +void qh_normalize2 (coordT *normal, int dim, boolT toporient, + realT *minnorm, boolT *ismin); +pointT *qh_projectpoint(pointT *point, facetT *facet, realT dist); + +void qh_setfacetplane(facetT *newfacets); +void qh_sethyperplane_det (int dim, coordT **rows, coordT *point0, + boolT toporient, coordT *normal, realT *offset, boolT *nearzero); +void qh_sethyperplane_gauss (int dim, coordT **rows, pointT *point0, + boolT toporient, coordT *normal, coordT *offset, boolT *nearzero); +boolT qh_sharpnewfacets (void); + +/*========= infrequently used code in geom2.c =============*/ + + +coordT *qh_copypoints (coordT *points, int numpoints, int dimension); +void qh_crossproduct (int dim, realT vecA[3], realT vecB[3], realT vecC[3]); +realT qh_determinant (realT **rows, int dim, boolT *nearzero); +realT qh_detjoggle (pointT *points, int numpoints, int dimension); +void qh_detroundoff (void); +realT qh_detsimplex(pointT *apex, setT *points, int dim, boolT *nearzero); +realT qh_distnorm (int dim, pointT *point, pointT *normal, realT *offsetp); +realT qh_distround (int dimension, realT maxabs, realT maxsumabs); +realT qh_divzero(realT numer, realT denom, realT mindenom1, boolT *zerodiv); +realT qh_facetarea (facetT *facet); +realT qh_facetarea_simplex (int dim, coordT *apex, setT *vertices, + vertexT *notvertex, boolT toporient, coordT *normal, realT *offset); +pointT *qh_facetcenter (setT *vertices); +facetT *qh_findgooddist (pointT *point, facetT *facetA, realT *distp, facetT **facetlist); +void qh_getarea (facetT *facetlist); +boolT qh_gram_schmidt(int dim, realT **rows); +boolT qh_inthresholds (coordT *normal, realT *angle); +void qh_joggleinput (void); +realT *qh_maxabsval (realT *normal, int dim); +setT *qh_maxmin(pointT *points, int numpoints, int dimension); +realT qh_maxouter (void); +void qh_maxsimplex (int dim, setT *maxpoints, pointT *points, int numpoints, setT **simplex); +realT qh_minabsval (realT *normal, int dim); +int qh_mindiff (realT *vecA, realT *vecB, int dim); +boolT qh_orientoutside (facetT *facet); +void qh_outerinner (facetT *facet, realT *outerplane, realT *innerplane); +coordT qh_pointdist(pointT *point1, pointT *point2, int dim); +void qh_printmatrix (FILE *fp, char *string, realT **rows, int numrow, int numcol); +void qh_printpoints (FILE *fp, char *string, setT *points); +void qh_projectinput (void); +void qh_projectpoints (signed char *project, int n, realT *points, + int numpoints, int dim, realT *newpoints, int newdim); +int qh_rand( void); +void qh_srand( int seed); +realT qh_randomfactor (void); +void qh_randommatrix (realT *buffer, int dim, realT **row); +void qh_rotateinput (realT **rows); +void qh_rotatepoints (realT *points, int numpoints, int dim, realT **rows); +void qh_scaleinput (void); +void qh_scalelast (coordT *points, int numpoints, int dim, coordT low, + coordT high, coordT newhigh); +void qh_scalepoints (pointT *points, int numpoints, int dim, + realT *newlows, realT *newhighs); +boolT qh_sethalfspace (int dim, coordT *coords, coordT **nextp, + coordT *normal, coordT *offset, coordT *feasible); +coordT *qh_sethalfspace_all (int dim, int count, coordT *halfspaces, pointT *feasible); +pointT *qh_voronoi_center (int dim, setT *points); + +#endif /* qhDEFgeom */ + + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom2.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom2.c new file mode 100644 index 0000000000000000000000000000000000000000..55b6d80832e49d0fd6842e04a26bab0fd7bb268f --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/geom2.c @@ -0,0 +1,2160 @@ +/*<html><pre> -<a href="qh-geom.htm" + >-------------------------------</a><a name="TOP">-</a> + + + geom2.c + infrequently used geometric routines of qhull + + see qh-geom.htm and geom.h + + copyright (c) 1993-2003 The Geometry Center + + frequently used code goes into geom.c +*/ + +#include "qhull_a.h" + +/*================== functions in alphabetic order ============*/ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="copypoints">-</a> + + qh_copypoints( points, numpoints, dimension) + return malloc'd copy of points +*/ +coordT *qh_copypoints (coordT *points, int numpoints, int dimension) { + int size; + coordT *newpoints; + + size= numpoints * dimension * sizeof(coordT); + if (!(newpoints=(coordT*)malloc(size))) { + fprintf(qh ferr, "qhull error: insufficient memory to copy %d points\n", + numpoints); + qh_errexit(qh_ERRmem, NULL, NULL); + } + memcpy ((char *)newpoints, (char *)points, size); + return newpoints; +} /* copypoints */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="crossproduct">-</a> + + qh_crossproduct( dim, vecA, vecB, vecC ) + crossproduct of 2 dim vectors + C= A x B + + notes: + from Glasner, Graphics Gems I, p. 639 + only defined for dim==3 +*/ +void qh_crossproduct (int dim, realT vecA[3], realT vecB[3], realT vecC[3]){ + + if (dim == 3) { + vecC[0]= det2_(vecA[1], vecA[2], + vecB[1], vecB[2]); + vecC[1]= - det2_(vecA[0], vecA[2], + vecB[0], vecB[2]); + vecC[2]= det2_(vecA[0], vecA[1], + vecB[0], vecB[1]); + } +} /* vcross */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="determinant">-</a> + + qh_determinant( rows, dim, nearzero ) + compute signed determinant of a square matrix + uses qh.NEARzero to test for degenerate matrices + + returns: + determinant + overwrites rows and the matrix + if dim == 2 or 3 + nearzero iff determinant < qh NEARzero[dim-1] + (not quite correct, not critical) + if dim >= 4 + nearzero iff diagonal[k] < qh NEARzero[k] +*/ +realT qh_determinant (realT **rows, int dim, boolT *nearzero) { + realT det=0; + int i; + boolT sign= False; + + *nearzero= False; + if (dim < 2) { + fprintf (qh ferr, "qhull internal error (qh_determinate): only implemented for dimension >= 2\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + }else if (dim == 2) { + det= det2_(rows[0][0], rows[0][1], + rows[1][0], rows[1][1]); + if (fabs_(det) < qh NEARzero[1]) /* not really correct, what should this be? */ + *nearzero= True; + }else if (dim == 3) { + det= det3_(rows[0][0], rows[0][1], rows[0][2], + rows[1][0], rows[1][1], rows[1][2], + rows[2][0], rows[2][1], rows[2][2]); + if (fabs_(det) < qh NEARzero[2]) /* not really correct, what should this be? */ + *nearzero= True; + }else { + qh_gausselim(rows, dim, dim, &sign, nearzero); /* if nearzero, diagonal still ok*/ + det= 1.0; + for (i= dim; i--; ) + det *= (rows[i])[i]; + if (sign) + det= -det; + } + return det; +} /* determinant */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="detjoggle">-</a> + + qh_detjoggle( points, numpoints, dimension ) + determine default max joggle for point array + as qh_distround * qh_JOGGLEdefault + + returns: + initial value for JOGGLEmax from points and REALepsilon + + notes: + computes DISTround since qh_maxmin not called yet + if qh SCALElast, last dimension will be scaled later to MAXwidth + + loop duplicated from qh_maxmin +*/ +realT qh_detjoggle (pointT *points, int numpoints, int dimension) { + realT abscoord, distround, joggle, maxcoord, mincoord; + pointT *point, *pointtemp; + realT maxabs= -REALmax; + realT sumabs= 0; + realT maxwidth= 0; + int k; + + for (k= 0; k < dimension; k++) { + if (qh SCALElast && k == dimension-1) + abscoord= maxwidth; + else if (qh DELAUNAY && k == dimension-1) /* will qh_setdelaunay() */ + abscoord= 2 * maxabs * maxabs; /* may be low by qh hull_dim/2 */ + else { + maxcoord= -REALmax; + mincoord= REALmax; + FORALLpoint_(points, numpoints) { + maximize_(maxcoord, point[k]); + minimize_(mincoord, point[k]); + } + maximize_(maxwidth, maxcoord-mincoord); + abscoord= fmax_(maxcoord, -mincoord); + } + sumabs += abscoord; + maximize_(maxabs, abscoord); + } /* for k */ + distround= qh_distround (qh hull_dim, maxabs, sumabs); + joggle= distround * qh_JOGGLEdefault; + maximize_(joggle, REALepsilon * qh_JOGGLEdefault); + trace2((qh ferr, "qh_detjoggle: joggle=%2.2g maxwidth=%2.2g\n", joggle, maxwidth)); + return joggle; +} /* detjoggle */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="detroundoff">-</a> + + qh_detroundoff() + determine maximum roundoff errors from + REALepsilon, REALmax, REALmin, qh.hull_dim, qh.MAXabs_coord, + qh.MAXsumcoord, qh.MAXwidth, qh.MINdenom_1 + + accounts for qh.SETroundoff, qh.RANDOMdist, qh MERGEexact + qh.premerge_cos, qh.postmerge_cos, qh.premerge_centrum, + qh.postmerge_centrum, qh.MINoutside, + qh_RATIOnearinside, qh_COPLANARratio, qh_WIDEcoplanar + + returns: + sets qh.DISTround, etc. (see below) + appends precision constants to qh.qhull_options + + see: + qh_maxmin() for qh.NEARzero + + design: + determine qh.DISTround for distance computations + determine minimum denominators for qh_divzero + determine qh.ANGLEround for angle computations + adjust qh.premerge_cos,... for roundoff error + determine qh.ONEmerge for maximum error due to a single merge + determine qh.NEARinside, qh.MAXcoplanar, qh.MINvisible, + qh.MINoutside, qh.WIDEfacet + initialize qh.max_vertex and qh.minvertex +*/ +void qh_detroundoff (void) { + + qh_option ("_max-width", NULL, &qh MAXwidth); + if (!qh SETroundoff) { + qh DISTround= qh_distround (qh hull_dim, qh MAXabs_coord, qh MAXsumcoord); + if (qh RANDOMdist) + qh DISTround += qh RANDOMfactor * qh MAXabs_coord; + qh_option ("Error-roundoff", NULL, &qh DISTround); + } + qh MINdenom= qh MINdenom_1 * qh MAXabs_coord; + qh MINdenom_1_2= sqrt (qh MINdenom_1 * qh hull_dim) ; /* if will be normalized */ + qh MINdenom_2= qh MINdenom_1_2 * qh MAXabs_coord; + /* for inner product */ + qh ANGLEround= 1.01 * qh hull_dim * REALepsilon; + if (qh RANDOMdist) + qh ANGLEround += qh RANDOMfactor; + if (qh premerge_cos < REALmax/2) { + qh premerge_cos -= qh ANGLEround; + if (qh RANDOMdist) + qh_option ("Angle-premerge-with-random", NULL, &qh premerge_cos); + } + if (qh postmerge_cos < REALmax/2) { + qh postmerge_cos -= qh ANGLEround; + if (qh RANDOMdist) + qh_option ("Angle-postmerge-with-random", NULL, &qh postmerge_cos); + } + qh premerge_centrum += 2 * qh DISTround; /*2 for centrum and distplane()*/ + qh postmerge_centrum += 2 * qh DISTround; + if (qh RANDOMdist && (qh MERGEexact || qh PREmerge)) + qh_option ("Centrum-premerge-with-random", NULL, &qh premerge_centrum); + if (qh RANDOMdist && qh POSTmerge) + qh_option ("Centrum-postmerge-with-random", NULL, &qh postmerge_centrum); + { /* compute ONEmerge, max vertex offset for merging simplicial facets */ + realT maxangle= 1.0, maxrho; + + minimize_(maxangle, qh premerge_cos); + minimize_(maxangle, qh postmerge_cos); + /* max diameter * sin theta + DISTround for vertex to its hyperplane */ + qh ONEmerge= sqrt (qh hull_dim) * qh MAXwidth * + sqrt (1.0 - maxangle * maxangle) + qh DISTround; + maxrho= qh hull_dim * qh premerge_centrum + qh DISTround; + maximize_(qh ONEmerge, maxrho); + maxrho= qh hull_dim * qh postmerge_centrum + qh DISTround; + maximize_(qh ONEmerge, maxrho); + if (qh MERGING) + qh_option ("_one-merge", NULL, &qh ONEmerge); + } + qh NEARinside= qh ONEmerge * qh_RATIOnearinside; /* only used if qh KEEPnearinside */ + if (qh JOGGLEmax < REALmax/2 && (qh KEEPcoplanar || qh KEEPinside)) { + realT maxdist; /* adjust qh.NEARinside for joggle */ + qh KEEPnearinside= True; + maxdist= sqrt (qh hull_dim) * qh JOGGLEmax + qh DISTround; + maxdist= 2*maxdist; /* vertex and coplanar point can joggle in opposite directions */ + maximize_(qh NEARinside, maxdist); /* must agree with qh_nearcoplanar() */ + } + if (qh KEEPnearinside) + qh_option ("_near-inside", NULL, &qh NEARinside); + if (qh JOGGLEmax < qh DISTround) { + fprintf (qh ferr, "qhull error: the joggle for 'QJn', %.2g, is below roundoff for distance computations, %.2g\n", + qh JOGGLEmax, qh DISTround); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (qh MINvisible > REALmax/2) { + if (!qh MERGING) + qh MINvisible= qh DISTround; + else if (qh hull_dim <= 3) + qh MINvisible= qh premerge_centrum; + else + qh MINvisible= qh_COPLANARratio * qh premerge_centrum; + if (qh APPROXhull && qh MINvisible > qh MINoutside) + qh MINvisible= qh MINoutside; + qh_option ("Visible-distance", NULL, &qh MINvisible); + } + if (qh MAXcoplanar > REALmax/2) { + qh MAXcoplanar= qh MINvisible; + qh_option ("U-coplanar-distance", NULL, &qh MAXcoplanar); + } + if (!qh APPROXhull) { /* user may specify qh MINoutside */ + qh MINoutside= 2 * qh MINvisible; + if (qh premerge_cos < REALmax/2) + maximize_(qh MINoutside, (1- qh premerge_cos) * qh MAXabs_coord); + qh_option ("Width-outside", NULL, &qh MINoutside); + } + qh WIDEfacet= qh MINoutside; + maximize_(qh WIDEfacet, qh_WIDEcoplanar * qh MAXcoplanar); + maximize_(qh WIDEfacet, qh_WIDEcoplanar * qh MINvisible); + qh_option ("_wide-facet", NULL, &qh WIDEfacet); + if (qh MINvisible > qh MINoutside + 3 * REALepsilon + && !qh BESToutside && !qh FORCEoutput) + fprintf (qh ferr, "qhull input warning: minimum visibility V%.2g is greater than \nminimum outside W%.2g. Flipped facets are likely.\n", + qh MINvisible, qh MINoutside); + qh max_vertex= qh DISTround; + qh min_vertex= -qh DISTround; + /* numeric constants reported in printsummary */ +} /* detroundoff */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="detsimplex">-</a> + + qh_detsimplex( apex, points, dim, nearzero ) + compute determinant of a simplex with point apex and base points + + returns: + signed determinant and nearzero from qh_determinant + + notes: + uses qh.gm_matrix/qh.gm_row (assumes they're big enough) + + design: + construct qm_matrix by subtracting apex from points + compute determinate +*/ +realT qh_detsimplex(pointT *apex, setT *points, int dim, boolT *nearzero) { + pointT *coorda, *coordp, *gmcoord, *point, **pointp; + coordT **rows; + int k, i=0; + realT det; + + zinc_(Zdetsimplex); + gmcoord= qh gm_matrix; + rows= qh gm_row; + FOREACHpoint_(points) { + if (i == dim) + break; + rows[i++]= gmcoord; + coordp= point; + coorda= apex; + for (k= dim; k--; ) + *(gmcoord++)= *coordp++ - *coorda++; + } + if (i < dim) { + fprintf (qh ferr, "qhull internal error (qh_detsimplex): #points %d < dimension %d\n", + i, dim); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + det= qh_determinant (rows, dim, nearzero); + trace2((qh ferr, "qh_detsimplex: det=%2.2g for point p%d, dim %d, nearzero? %d\n", + det, qh_pointid(apex), dim, *nearzero)); + return det; +} /* detsimplex */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="distnorm">-</a> + + qh_distnorm( dim, point, normal, offset ) + return distance from point to hyperplane at normal/offset + + returns: + dist + + notes: + dist > 0 if point is outside of hyperplane + + see: + qh_distplane in geom.c +*/ +realT qh_distnorm (int dim, pointT *point, pointT *normal, realT *offsetp) { + coordT *normalp= normal, *coordp= point; + realT dist; + int k; + + dist= *offsetp; + for (k= dim; k--; ) + dist += *(coordp++) * *(normalp++); + return dist; +} /* distnorm */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="distround">-</a> + + qh_distround ( dimension, maxabs, maxsumabs ) + compute maximum round-off error for a distance computation + to a normalized hyperplane + maxabs is the maximum absolute value of a coordinate + maxsumabs is the maximum possible sum of absolute coordinate values + + returns: + max dist round for REALepsilon + + notes: + calculate roundoff error according to + Lemma 3.2-1 of Golub and van Loan "Matrix Computation" + use sqrt(dim) since one vector is normalized + or use maxsumabs since one vector is < 1 +*/ +realT qh_distround (int dimension, realT maxabs, realT maxsumabs) { + realT maxdistsum, maxround; + + maxdistsum= sqrt (dimension) * maxabs; + minimize_( maxdistsum, maxsumabs); + maxround= REALepsilon * (dimension * maxdistsum * 1.01 + maxabs); + /* adds maxabs for offset */ + trace4((qh ferr, "qh_distround: %2.2g maxabs %2.2g maxsumabs %2.2g maxdistsum %2.2g\n", + maxround, maxabs, maxsumabs, maxdistsum)); + return maxround; +} /* distround */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="divzero">-</a> + + qh_divzero( numer, denom, mindenom1, zerodiv ) + divide by a number that's nearly zero + mindenom1= minimum denominator for dividing into 1.0 + + returns: + quotient + sets zerodiv and returns 0.0 if it would overflow + + design: + if numer is nearly zero and abs(numer) < abs(denom) + return numer/denom + else if numer is nearly zero + return 0 and zerodiv + else if denom/numer non-zero + return numer/denom + else + return 0 and zerodiv +*/ +realT qh_divzero (realT numer, realT denom, realT mindenom1, boolT *zerodiv) { + realT temp, numerx, denomx; + + + if (numer < mindenom1 && numer > -mindenom1) { + numerx= fabs_(numer); + denomx= fabs_(denom); + if (numerx < denomx) { + *zerodiv= False; + return numer/denom; + }else { + *zerodiv= True; + return 0.0; + } + } + temp= denom/numer; + if (temp > mindenom1 || temp < -mindenom1) { + *zerodiv= False; + return numer/denom; + }else { + *zerodiv= True; + return 0.0; + } +} /* divzero */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="facetarea">-</a> + + qh_facetarea( facet ) + return area for a facet + + notes: + if non-simplicial, + uses centrum to triangulate facet and sums the projected areas. + if (qh DELAUNAY), + computes projected area instead for last coordinate + assumes facet->normal exists + projecting tricoplanar facets to the hyperplane does not appear to make a difference + + design: + if simplicial + compute area + else + for each ridge + compute area from centrum to ridge + negate area if upper Delaunay facet +*/ +realT qh_facetarea (facetT *facet) { + vertexT *apex; + pointT *centrum; + realT area= 0.0; + ridgeT *ridge, **ridgep; + + if (facet->simplicial) { + apex= SETfirstt_(facet->vertices, vertexT); + area= qh_facetarea_simplex (qh hull_dim, apex->point, facet->vertices, + apex, facet->toporient, facet->normal, &facet->offset); + }else { + if (qh CENTERtype == qh_AScentrum) + centrum= facet->center; + else + centrum= qh_getcentrum (facet); + FOREACHridge_(facet->ridges) + area += qh_facetarea_simplex (qh hull_dim, centrum, ridge->vertices, + NULL, (ridge->top == facet), facet->normal, &facet->offset); + if (qh CENTERtype != qh_AScentrum) + qh_memfree (centrum, qh normal_size); + } + if (facet->upperdelaunay && qh DELAUNAY) + area= -area; /* the normal should be [0,...,1] */ + trace4((qh ferr, "qh_facetarea: f%d area %2.2g\n", facet->id, area)); + return area; +} /* facetarea */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="facetarea_simplex">-</a> + + qh_facetarea_simplex( dim, apex, vertices, notvertex, toporient, normal, offset ) + return area for a simplex defined by + an apex, a base of vertices, an orientation, and a unit normal + if simplicial or tricoplanar facet, + notvertex is defined and it is skipped in vertices + + returns: + computes area of simplex projected to plane [normal,offset] + returns 0 if vertex too far below plane (qh WIDEfacet) + vertex can't be apex of tricoplanar facet + + notes: + if (qh DELAUNAY), + computes projected area instead for last coordinate + uses qh gm_matrix/gm_row and qh hull_dim + helper function for qh_facetarea + + design: + if Notvertex + translate simplex to apex + else + project simplex to normal/offset + translate simplex to apex + if Delaunay + set last row/column to 0 with -1 on diagonal + else + set last row to Normal + compute determinate + scale and flip sign for area +*/ +realT qh_facetarea_simplex (int dim, coordT *apex, setT *vertices, + vertexT *notvertex, boolT toporient, coordT *normal, realT *offset) { + pointT *coorda, *coordp, *gmcoord; + coordT **rows, *normalp; + int k, i=0; + realT area, dist; + vertexT *vertex, **vertexp; + boolT nearzero; + + gmcoord= qh gm_matrix; + rows= qh gm_row; + FOREACHvertex_(vertices) { + if (vertex == notvertex) + continue; + rows[i++]= gmcoord; + coorda= apex; + coordp= vertex->point; + normalp= normal; + if (notvertex) { + for (k= dim; k--; ) + *(gmcoord++)= *coordp++ - *coorda++; + }else { + dist= *offset; + for (k= dim; k--; ) + dist += *coordp++ * *normalp++; + if (dist < -qh WIDEfacet) { + zinc_(Znoarea); + return 0.0; + } + coordp= vertex->point; + normalp= normal; + for (k= dim; k--; ) + *(gmcoord++)= (*coordp++ - dist * *normalp++) - *coorda++; + } + } + if (i != dim-1) { + fprintf (qh ferr, "qhull internal error (qh_facetarea_simplex): #points %d != dim %d -1\n", + i, dim); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + rows[i]= gmcoord; + if (qh DELAUNAY) { + for (i= 0; i < dim-1; i++) + rows[i][dim-1]= 0.0; + for (k= dim; k--; ) + *(gmcoord++)= 0.0; + rows[dim-1][dim-1]= -1.0; + }else { + normalp= normal; + for (k= dim; k--; ) + *(gmcoord++)= *normalp++; + } + zinc_(Zdetsimplex); + area= qh_determinant (rows, dim, &nearzero); + if (toporient) + area= -area; + area *= qh AREAfactor; + trace4((qh ferr, "qh_facetarea_simplex: area=%2.2g for point p%d, toporient %d, nearzero? %d\n", + area, qh_pointid(apex), toporient, nearzero)); + return area; +} /* facetarea_simplex */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="facetcenter">-</a> + + qh_facetcenter( vertices ) + return Voronoi center (Voronoi vertex) for a facet's vertices + + returns: + return temporary point equal to the center + + see: + qh_voronoi_center() +*/ +pointT *qh_facetcenter (setT *vertices) { + setT *points= qh_settemp (qh_setsize (vertices)); + vertexT *vertex, **vertexp; + pointT *center; + + FOREACHvertex_(vertices) + qh_setappend (&points, vertex->point); + center= qh_voronoi_center (qh hull_dim-1, points); + qh_settempfree (&points); + return center; +} /* facetcenter */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="findgooddist">-</a> + + qh_findgooddist( point, facetA, dist, facetlist ) + find best good facet visible for point from facetA + assumes facetA is visible from point + + returns: + best facet, i.e., good facet that is furthest from point + distance to best facet + NULL if none + + moves good, visible facets (and some other visible facets) + to end of qh facet_list + + notes: + uses qh visit_id + + design: + initialize bestfacet if facetA is good + move facetA to end of facetlist + for each facet on facetlist + for each unvisited neighbor of facet + move visible neighbors to end of facetlist + update best good neighbor + if no good neighbors, update best facet +*/ +facetT *qh_findgooddist (pointT *point, facetT *facetA, realT *distp, + facetT **facetlist) { + realT bestdist= -REALmax, dist; + facetT *neighbor, **neighborp, *bestfacet=NULL, *facet; + boolT goodseen= False; + + if (facetA->good) { + zinc_(Zcheckpart); /* calls from check_bestdist occur after print stats */ + qh_distplane (point, facetA, &bestdist); + bestfacet= facetA; + goodseen= True; + } + qh_removefacet (facetA); + qh_appendfacet (facetA); + *facetlist= facetA; + facetA->visitid= ++qh visit_id; + FORALLfacet_(*facetlist) { + FOREACHneighbor_(facet) { + if (neighbor->visitid == qh visit_id) + continue; + neighbor->visitid= qh visit_id; + if (goodseen && !neighbor->good) + continue; + zinc_(Zcheckpart); + qh_distplane (point, neighbor, &dist); + if (dist > 0) { + qh_removefacet (neighbor); + qh_appendfacet (neighbor); + if (neighbor->good) { + goodseen= True; + if (dist > bestdist) { + bestdist= dist; + bestfacet= neighbor; + } + } + } + } + } + if (bestfacet) { + *distp= bestdist; + trace2((qh ferr, "qh_findgooddist: p%d is %2.2g above good facet f%d\n", + qh_pointid(point), bestdist, bestfacet->id)); + return bestfacet; + } + trace4((qh ferr, "qh_findgooddist: no good facet for p%d above f%d\n", + qh_pointid(point), facetA->id)); + return NULL; +} /* findgooddist */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="getarea">-</a> + + qh_getarea( facetlist ) + set area of all facets in facetlist + collect statistics + + returns: + sets qh totarea/totvol to total area and volume of convex hull + for Delaunay triangulation, computes projected area of the lower or upper hull + ignores upper hull if qh ATinfinity + + notes: + could compute outer volume by expanding facet area by rays from interior + the following attempt at perpendicular projection underestimated badly: + qh.totoutvol += (-dist + facet->maxoutside + qh DISTround) + * area/ qh hull_dim; + design: + for each facet on facetlist + compute facet->area + update qh.totarea and qh.totvol +*/ +void qh_getarea (facetT *facetlist) { + realT area; + realT dist; + facetT *facet; + + if (qh REPORTfreq) + fprintf (qh ferr, "computing area of each facet and volume of the convex hull\n"); + else + trace1((qh ferr, "qh_getarea: computing volume and area for each facet\n")); + qh totarea= qh totvol= 0.0; + FORALLfacet_(facetlist) { + if (!facet->normal) + continue; + if (facet->upperdelaunay && qh ATinfinity) + continue; + facet->f.area= area= qh_facetarea (facet); + facet->isarea= True; + if (qh DELAUNAY) { + if (facet->upperdelaunay == qh UPPERdelaunay) + qh totarea += area; + }else { + qh totarea += area; + qh_distplane (qh interior_point, facet, &dist); + qh totvol += -dist * area/ qh hull_dim; + } + if (qh PRINTstatistics) { + wadd_(Wareatot, area); + wmax_(Wareamax, area); + wmin_(Wareamin, area); + } + } +} /* getarea */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="gram_schmidt">-</a> + + qh_gram_schmidt( dim, row ) + implements Gram-Schmidt orthogonalization by rows + + returns: + false if zero norm + overwrites rows[dim][dim] + + notes: + see Golub & van Loan Algorithm 6.2-2 + overflow due to small divisors not handled + + design: + for each row + compute norm for row + if non-zero, normalize row + for each remaining rowA + compute inner product of row and rowA + reduce rowA by row * inner product +*/ +boolT qh_gram_schmidt(int dim, realT **row) { + realT *rowi, *rowj, norm; + int i, j, k; + + for(i=0; i < dim; i++) { + rowi= row[i]; + for (norm= 0.0, k= dim; k--; rowi++) + norm += *rowi * *rowi; + norm= sqrt(norm); + wmin_(Wmindenom, norm); + if (norm == 0.0) /* either 0 or overflow due to sqrt */ + return False; + for(k= dim; k--; ) + *(--rowi) /= norm; + for(j= i+1; j < dim; j++) { + rowj= row[j]; + for(norm= 0.0, k=dim; k--; ) + norm += *rowi++ * *rowj++; + for(k=dim; k--; ) + *(--rowj) -= *(--rowi) * norm; + } + } + return True; +} /* gram_schmidt */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="inthresholds">-</a> + + qh_inthresholds( normal, angle ) + return True if normal within qh.lower_/upper_threshold + + returns: + estimate of angle by summing of threshold diffs + angle may be NULL + smaller "angle" is better + + notes: + invalid if qh.SPLITthresholds + + see: + qh.lower_threshold in qh_initbuild() + qh_initthresholds() + + design: + for each dimension + test threshold +*/ +boolT qh_inthresholds (coordT *normal, realT *angle) { + boolT within= True; + int k; + realT threshold; + + if (angle) + *angle= 0.0; + for(k= 0; k < qh hull_dim; k++) { + threshold= qh lower_threshold[k]; + if (threshold > -REALmax/2) { + if (normal[k] < threshold) + within= False; + if (angle) { + threshold -= normal[k]; + *angle += fabs_(threshold); + } + } + if (qh upper_threshold[k] < REALmax/2) { + threshold= qh upper_threshold[k]; + if (normal[k] > threshold) + within= False; + if (angle) { + threshold -= normal[k]; + *angle += fabs_(threshold); + } + } + } + return within; +} /* inthresholds */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="joggleinput">-</a> + + qh_joggleinput() + randomly joggle input to Qhull by qh.JOGGLEmax + initial input is qh.first_point/qh.num_points of qh.hull_dim + repeated calls use qh.input_points/qh.num_points + + returns: + joggles points at qh.first_point/qh.num_points + copies data to qh.input_points/qh.input_malloc if first time + determines qh.JOGGLEmax if it was zero + if qh.DELAUNAY + computes the Delaunay projection of the joggled points + + notes: + if qh.DELAUNAY, unnecessarily joggles the last coordinate + the initial 'QJn' may be set larger than qh_JOGGLEmaxincrease + + design: + if qh.DELAUNAY + set qh.SCALElast for reduced precision errors + if first call + initialize qh.input_points to the original input points + if qh.JOGGLEmax == 0 + determine default qh.JOGGLEmax + else + increase qh.JOGGLEmax according to qh.build_cnt + joggle the input by adding a random number in [-qh.JOGGLEmax,qh.JOGGLEmax] + if qh.DELAUNAY + sets the Delaunay projection +*/ +void qh_joggleinput (void) { + int size, i, seed; + coordT *coordp, *inputp; + realT randr, randa, randb; + + if (!qh input_points) { /* first call */ + qh input_points= qh first_point; + qh input_malloc= qh POINTSmalloc; + size= qh num_points * qh hull_dim * sizeof(coordT); + if (!(qh first_point=(coordT*)malloc(size))) { + fprintf(qh ferr, "qhull error: insufficient memory to joggle %d points\n", + qh num_points); + qh_errexit(qh_ERRmem, NULL, NULL); + } + qh POINTSmalloc= True; + if (qh JOGGLEmax == 0.0) { + qh JOGGLEmax= qh_detjoggle (qh input_points, qh num_points, qh hull_dim); + qh_option ("QJoggle", NULL, &qh JOGGLEmax); + } + }else { /* repeated call */ + if (!qh RERUN && qh build_cnt > qh_JOGGLEretry) { + if (((qh build_cnt-qh_JOGGLEretry-1) % qh_JOGGLEagain) == 0) { + realT maxjoggle= qh MAXwidth * qh_JOGGLEmaxincrease; + if (qh JOGGLEmax < maxjoggle) { + qh JOGGLEmax *= qh_JOGGLEincrease; + minimize_(qh JOGGLEmax, maxjoggle); + } + } + } + qh_option ("QJoggle", NULL, &qh JOGGLEmax); + } + if (qh build_cnt > 1 && qh JOGGLEmax > fmax_(qh MAXwidth/4, 0.1)) { + fprintf (qh ferr, "qhull error: the current joggle for 'QJn', %.2g, is too large for the width\nof the input. If possible, recompile Qhull with higher-precision reals.\n", + qh JOGGLEmax); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + /* for some reason, using qh ROTATErandom and qh_RANDOMseed does not repeat the run. Use 'TRn' instead */ + seed= qh_RANDOMint; + qh_option ("_joggle-seed", &seed, NULL); + trace0((qh ferr, "qh_joggleinput: joggle input by %2.2g with seed %d\n", + qh JOGGLEmax, seed)); + inputp= qh input_points; + coordp= qh first_point; + randa= 2.0 * qh JOGGLEmax/qh_RANDOMmax; + randb= -qh JOGGLEmax; + size= qh num_points * qh hull_dim; + for (i= size; i--; ) { + randr= qh_RANDOMint; + *(coordp++)= *(inputp++) + (randr * randa + randb); + } + if (qh DELAUNAY) { + qh last_low= qh last_high= qh last_newhigh= REALmax; + qh_setdelaunay (qh hull_dim, qh num_points, qh first_point); + } +} /* joggleinput */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="maxabsval">-</a> + + qh_maxabsval( normal, dim ) + return pointer to maximum absolute value of a dim vector + returns NULL if dim=0 +*/ +realT *qh_maxabsval (realT *normal, int dim) { + realT maxval= -REALmax; + realT *maxp= NULL, *colp, absval; + int k; + + for (k= dim, colp= normal; k--; colp++) { + absval= fabs_(*colp); + if (absval > maxval) { + maxval= absval; + maxp= colp; + } + } + return maxp; +} /* maxabsval */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="maxmin">-</a> + + qh_maxmin( points, numpoints, dimension ) + return max/min points for each dimension + determine max and min coordinates + + returns: + returns a temporary set of max and min points + may include duplicate points. Does not include qh.GOODpoint + sets qh.NEARzero, qh.MAXabs_coord, qh.MAXsumcoord, qh.MAXwidth + qh.MAXlastcoord, qh.MINlastcoord + initializes qh.max_outside, qh.min_vertex, qh.WAScoplanar, qh.ZEROall_ok + + notes: + loop duplicated in qh_detjoggle() + + design: + initialize global precision variables + checks definition of REAL... + for each dimension + for each point + collect maximum and minimum point + collect maximum of maximums and minimum of minimums + determine qh.NEARzero for Gaussian Elimination +*/ +setT *qh_maxmin(pointT *points, int numpoints, int dimension) { + int k; + realT maxcoord, temp; + pointT *minimum, *maximum, *point, *pointtemp; + setT *set; + + qh max_outside= 0.0; + qh MAXabs_coord= 0.0; + qh MAXwidth= -REALmax; + qh MAXsumcoord= 0.0; + qh min_vertex= 0.0; + qh WAScoplanar= False; + if (qh ZEROcentrum) + qh ZEROall_ok= True; + if (REALmin < REALepsilon && REALmin < REALmax && REALmin > -REALmax + && REALmax > 0.0 && -REALmax < 0.0) + ; /* all ok */ + else { + fprintf (qh ferr, "qhull error: floating point constants in user.h are wrong\n\ +REALepsilon %g REALmin %g REALmax %g -REALmax %g\n", + REALepsilon, REALmin, REALmax, -REALmax); + qh_errexit (qh_ERRinput, NULL, NULL); + } + set= qh_settemp(2*dimension); + for(k= 0; k < dimension; k++) { + if (points == qh GOODpointp) + minimum= maximum= points + dimension; + else + minimum= maximum= points; + FORALLpoint_(points, numpoints) { + if (point == qh GOODpointp) + continue; + if (maximum[k] < point[k]) + maximum= point; + else if (minimum[k] > point[k]) + minimum= point; + } + if (k == dimension-1) { + qh MINlastcoord= minimum[k]; + qh MAXlastcoord= maximum[k]; + } + if (qh SCALElast && k == dimension-1) + maxcoord= qh MAXwidth; + else { + maxcoord= fmax_(maximum[k], -minimum[k]); + if (qh GOODpointp) { + temp= fmax_(qh GOODpointp[k], -qh GOODpointp[k]); + maximize_(maxcoord, temp); + } + temp= maximum[k] - minimum[k]; + maximize_(qh MAXwidth, temp); + } + maximize_(qh MAXabs_coord, maxcoord); + qh MAXsumcoord += maxcoord; + qh_setappend (&set, maximum); + qh_setappend (&set, minimum); + /* calculation of qh NEARzero is based on error formula 4.4-13 of + Golub & van Loan, authors say n^3 can be ignored and 10 be used in + place of rho */ + qh NEARzero[k]= 80 * qh MAXsumcoord * REALepsilon; + } + if (qh IStracing >=1) + qh_printpoints (qh ferr, "qh_maxmin: found the max and min points (by dim):", set); + return(set); +} /* maxmin */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="maxouter">-</a> + + qh_maxouter() + return maximum distance from facet to outer plane + normally this is qh.max_outside+qh.DISTround + does not include qh.JOGGLEmax + + see: + qh_outerinner() + + notes: + need to add another qh.DISTround if testing actual point with computation + + for joggle: + qh_setfacetplane() updated qh.max_outer for Wnewvertexmax (max distance to vertex) + need to use Wnewvertexmax since could have a coplanar point for a high + facet that is replaced by a low facet + need to add qh.JOGGLEmax if testing input points +*/ +realT qh_maxouter (void) { + realT dist; + + dist= fmax_(qh max_outside, qh DISTround); + dist += qh DISTround; + trace4((qh ferr, "qh_maxouter: max distance from facet to outer plane is %2.2g max_outside is %2.2g\n", dist, qh max_outside)); + return dist; +} /* maxouter */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="maxsimplex">-</a> + + qh_maxsimplex( dim, maxpoints, points, numpoints, simplex ) + determines maximum simplex for a set of points + starts from points already in simplex + skips qh.GOODpointp (assumes that it isn't in maxpoints) + + returns: + simplex with dim+1 points + + notes: + assumes at least pointsneeded points in points + maximizes determinate for x,y,z,w, etc. + uses maxpoints as long as determinate is clearly non-zero + + design: + initialize simplex with at least two points + (find points with max or min x coordinate) + for each remaining dimension + add point that maximizes the determinate + (use points from maxpoints first) +*/ +void qh_maxsimplex (int dim, setT *maxpoints, pointT *points, int numpoints, setT **simplex) { + pointT *point, **pointp, *pointtemp, *maxpoint, *minx=NULL, *maxx=NULL; + boolT nearzero, maxnearzero= False; + int k, sizinit; + realT maxdet= -REALmax, det, mincoord= REALmax, maxcoord= -REALmax; + + sizinit= qh_setsize (*simplex); + if (sizinit < 2) { + if (qh_setsize (maxpoints) >= 2) { + FOREACHpoint_(maxpoints) { + if (maxcoord < point[0]) { + maxcoord= point[0]; + maxx= point; + } + if (mincoord > point[0]) { + mincoord= point[0]; + minx= point; + } + } + }else { + FORALLpoint_(points, numpoints) { + if (point == qh GOODpointp) + continue; + if (maxcoord < point[0]) { + maxcoord= point[0]; + maxx= point; + } + if (mincoord > point[0]) { + mincoord= point[0]; + minx= point; + } + } + } + qh_setunique (simplex, minx); + if (qh_setsize (*simplex) < 2) + qh_setunique (simplex, maxx); + sizinit= qh_setsize (*simplex); + if (sizinit < 2) { + qh_precision ("input has same x coordinate"); + if (zzval_(Zsetplane) > qh hull_dim+1) { + fprintf (qh ferr, "qhull precision error (qh_maxsimplex for voronoi_center):\n%d points with the same x coordinate.\n", + qh_setsize(maxpoints)+numpoints); + qh_errexit (qh_ERRprec, NULL, NULL); + }else { + fprintf (qh ferr, "qhull input error: input is less than %d-dimensional since it has the same x coordinate\n", qh hull_dim); + qh_errexit (qh_ERRinput, NULL, NULL); + } + } + } + for(k= sizinit; k < dim+1; k++) { + maxpoint= NULL; + maxdet= -REALmax; + FOREACHpoint_(maxpoints) { + if (!qh_setin (*simplex, point)) { + det= qh_detsimplex(point, *simplex, k, &nearzero); + if ((det= fabs_(det)) > maxdet) { + maxdet= det; + maxpoint= point; + maxnearzero= nearzero; + } + } + } + if (!maxpoint || maxnearzero) { + zinc_(Zsearchpoints); + if (!maxpoint) { + trace0((qh ferr, "qh_maxsimplex: searching all points for %d-th initial vertex.\n", k+1)); + }else { + trace0((qh ferr, "qh_maxsimplex: searching all points for %d-th initial vertex, better than p%d det %2.2g\n", + k+1, qh_pointid(maxpoint), maxdet)); + } + FORALLpoint_(points, numpoints) { + if (point == qh GOODpointp) + continue; + if (!qh_setin (*simplex, point)) { + det= qh_detsimplex(point, *simplex, k, &nearzero); + if ((det= fabs_(det)) > maxdet) { + maxdet= det; + maxpoint= point; + maxnearzero= nearzero; + } + } + } + } /* !maxpoint */ + if (!maxpoint) { + fprintf (qh ferr, "qhull internal error (qh_maxsimplex): not enough points available\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + qh_setappend(simplex, maxpoint); + trace1((qh ferr, "qh_maxsimplex: selected point p%d for %d`th initial vertex, det=%2.2g\n", + qh_pointid(maxpoint), k+1, maxdet)); + } /* k */ +} /* maxsimplex */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="minabsval">-</a> + + qh_minabsval( normal, dim ) + return minimum absolute value of a dim vector +*/ +realT qh_minabsval (realT *normal, int dim) { + realT minval= 0; + realT maxval= 0; + realT *colp; + int k; + + for (k= dim, colp= normal; k--; colp++) { + maximize_(maxval, *colp); + minimize_(minval, *colp); + } + return fmax_(maxval, -minval); +} /* minabsval */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="mindiff">-</a> + + qh_mindif( vecA, vecB, dim ) + return index of min abs. difference of two vectors +*/ +int qh_mindiff (realT *vecA, realT *vecB, int dim) { + realT mindiff= REALmax, diff; + realT *vecAp= vecA, *vecBp= vecB; + int k, mink= 0; + + for (k= 0; k < dim; k++) { + diff= *vecAp++ - *vecBp++; + diff= fabs_(diff); + if (diff < mindiff) { + mindiff= diff; + mink= k; + } + } + return mink; +} /* mindiff */ + + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="orientoutside">-</a> + + qh_orientoutside( facet ) + make facet outside oriented via qh.interior_point + + returns: + True if facet reversed orientation. +*/ +boolT qh_orientoutside (facetT *facet) { + int k; + realT dist; + + qh_distplane (qh interior_point, facet, &dist); + if (dist > 0) { + for (k= qh hull_dim; k--; ) + facet->normal[k]= -facet->normal[k]; + facet->offset= -facet->offset; + return True; + } + return False; +} /* orientoutside */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="outerinner">-</a> + + qh_outerinner( facet, outerplane, innerplane ) + if facet and qh.maxoutdone (i.e., qh_check_maxout) + returns outer and inner plane for facet + else + returns maximum outer and inner plane + accounts for qh.JOGGLEmax + + see: + qh_maxouter(), qh_check_bestdist(), qh_check_points() + + notes: + outerplaner or innerplane may be NULL + + includes qh.DISTround for actual points + adds another qh.DISTround if testing with floating point arithmetic +*/ +void qh_outerinner (facetT *facet, realT *outerplane, realT *innerplane) { + realT dist, mindist; + vertexT *vertex, **vertexp; + + if (outerplane) { + if (!qh_MAXoutside || !facet || !qh maxoutdone) { + *outerplane= qh_maxouter(); /* includes qh.DISTround */ + }else { /* qh_MAXoutside ... */ +#if qh_MAXoutside + *outerplane= facet->maxoutside + qh DISTround; +#endif + + } + if (qh JOGGLEmax < REALmax/2) + *outerplane += qh JOGGLEmax * sqrt (qh hull_dim); + } + if (innerplane) { + if (facet) { + mindist= REALmax; + FOREACHvertex_(facet->vertices) { + zinc_(Zdistio); + qh_distplane (vertex->point, facet, &dist); + minimize_(mindist, dist); + } + *innerplane= mindist - qh DISTround; + }else + *innerplane= qh min_vertex - qh DISTround; + if (qh JOGGLEmax < REALmax/2) + *innerplane -= qh JOGGLEmax * sqrt (qh hull_dim); + } +} /* outerinner */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="pointdist">-</a> + + qh_pointdist( point1, point2, dim ) + return distance between two points + + notes: + returns distance squared if 'dim' is negative +*/ +coordT qh_pointdist(pointT *point1, pointT *point2, int dim) { + coordT dist, diff; + int k; + + dist= 0.0; + for (k= (dim > 0 ? dim : -dim); k--; ) { + diff= *point1++ - *point2++; + dist += diff * diff; + } + if (dim > 0) + return(sqrt(dist)); + return dist; +} /* pointdist */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="printmatrix">-</a> + + qh_printmatrix( fp, string, rows, numrow, numcol ) + print matrix to fp given by row vectors + print string as header + + notes: + print a vector by qh_printmatrix(fp, "", &vect, 1, len) +*/ +void qh_printmatrix (FILE *fp, char *string, realT **rows, int numrow, int numcol) { + realT *rowp; + realT r; /*bug fix*/ + int i,k; + + fprintf (fp, "%s\n", string); + for (i= 0; i < numrow; i++) { + rowp= rows[i]; + for (k= 0; k < numcol; k++) { + r= *rowp++; + fprintf (fp, "%6.3g ", r); + } + fprintf (fp, "\n"); + } +} /* printmatrix */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="printpoints">-</a> + + qh_printpoints( fp, string, points ) + print pointids to fp for a set of points + if string, prints string and 'p' point ids +*/ +void qh_printpoints (FILE *fp, char *string, setT *points) { + pointT *point, **pointp; + + if (string) { + fprintf (fp, "%s", string); + FOREACHpoint_(points) + fprintf (fp, " p%d", qh_pointid(point)); + fprintf (fp, "\n"); + }else { + FOREACHpoint_(points) + fprintf (fp, " %d", qh_pointid(point)); + fprintf (fp, "\n"); + } +} /* printpoints */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="projectinput">-</a> + + qh_projectinput() + project input points using qh.lower_bound/upper_bound and qh DELAUNAY + if qh.lower_bound[k]=qh.upper_bound[k]= 0, + removes dimension k + if halfspace intersection + removes dimension k from qh.feasible_point + input points in qh first_point, num_points, input_dim + + returns: + new point array in qh first_point of qh hull_dim coordinates + sets qh POINTSmalloc + if qh DELAUNAY + projects points to paraboloid + lowbound/highbound is also projected + if qh ATinfinity + adds point "at-infinity" + if qh POINTSmalloc + frees old point array + + notes: + checks that qh.hull_dim agrees with qh.input_dim, PROJECTinput, and DELAUNAY + + + design: + sets project[k] to -1 (delete), 0 (keep), 1 (add for Delaunay) + determines newdim and newnum for qh hull_dim and qh num_points + projects points to newpoints + projects qh.lower_bound to itself + projects qh.upper_bound to itself + if qh DELAUNAY + if qh ATINFINITY + projects points to paraboloid + computes "infinity" point as vertex average and 10% above all points + else + uses qh_setdelaunay to project points to paraboloid +*/ +void qh_projectinput (void) { + int k,i; + int newdim= qh input_dim, newnum= qh num_points; + signed char *project; + int size= (qh input_dim+1)*sizeof(*project); + pointT *newpoints, *coord, *infinity; + realT paraboloid, maxboloid= 0; + + project= (signed char*)qh_memalloc (size); + memset ((char*)project, 0, size); + for (k= 0; k < qh input_dim; k++) { /* skip Delaunay bound */ + if (qh lower_bound[k] == 0 && qh upper_bound[k] == 0) { + project[k]= -1; + newdim--; + } + } + if (qh DELAUNAY) { + project[k]= 1; + newdim++; + if (qh ATinfinity) + newnum++; + } + if (newdim != qh hull_dim) { + fprintf(qh ferr, "qhull internal error (qh_projectinput): dimension after projection %d != hull_dim %d\n", newdim, qh hull_dim); + qh_errexit(qh_ERRqhull, NULL, NULL); + } + if (!(newpoints=(coordT*)malloc(newnum*newdim*sizeof(coordT)))){ + fprintf(qh ferr, "qhull error: insufficient memory to project %d points\n", + qh num_points); + qh_errexit(qh_ERRmem, NULL, NULL); + } + qh_projectpoints (project, qh input_dim+1, qh first_point, + qh num_points, qh input_dim, newpoints, newdim); + trace1((qh ferr, "qh_projectinput: updating lower and upper_bound\n")); + qh_projectpoints (project, qh input_dim+1, qh lower_bound, + 1, qh input_dim+1, qh lower_bound, newdim+1); + qh_projectpoints (project, qh input_dim+1, qh upper_bound, + 1, qh input_dim+1, qh upper_bound, newdim+1); + if (qh HALFspace) { + if (!qh feasible_point) { + fprintf(qh ferr, "qhull internal error (qh_projectinput): HALFspace defined without qh.feasible_point\n"); + qh_errexit(qh_ERRqhull, NULL, NULL); + } + qh_projectpoints (project, qh input_dim, qh feasible_point, + 1, qh input_dim, qh feasible_point, newdim); + } + qh_memfree(project, ((qh input_dim+1)*sizeof(*project))); + if (qh POINTSmalloc) + free (qh first_point); + qh first_point= newpoints; + qh POINTSmalloc= True; + if (qh DELAUNAY && qh ATinfinity) { + coord= qh first_point; + infinity= qh first_point + qh hull_dim * qh num_points; + for (k=qh hull_dim-1; k--; ) + infinity[k]= 0.0; + for (i=qh num_points; i--; ) { + paraboloid= 0.0; + for (k=0; k < qh hull_dim-1; k++) { + paraboloid += *coord * *coord; + infinity[k] += *coord; + coord++; + } + *(coord++)= paraboloid; + maximize_(maxboloid, paraboloid); + } + /* coord == infinity */ + for (k=qh hull_dim-1; k--; ) + *(coord++) /= qh num_points; + *(coord++)= maxboloid * 1.1; + qh num_points++; + trace0((qh ferr, "qh_projectinput: projected points to paraboloid for Delaunay\n")); + }else if (qh DELAUNAY) /* !qh ATinfinity */ + qh_setdelaunay( qh hull_dim, qh num_points, qh first_point); +} /* projectinput */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="projectpoints">-</a> + + qh_projectpoints( project, n, points, numpoints, dim, newpoints, newdim ) + project points/numpoints/dim to newpoints/newdim + if project[k] == -1 + delete dimension k + if project[k] == 1 + add dimension k by duplicating previous column + n is size of project + + notes: + newpoints may be points if only adding dimension at end + + design: + check that 'project' and 'newdim' agree + for each dimension + if project == -1 + skip dimension + else + determine start of column in newpoints + determine start of column in points + if project == +1, duplicate previous column + copy dimension (column) from points to newpoints +*/ +void qh_projectpoints (signed char *project, int n, realT *points, + int numpoints, int dim, realT *newpoints, int newdim) { + int testdim= dim, oldk=0, newk=0, i,j=0,k; + realT *newp, *oldp; + + for (k= 0; k < n; k++) + testdim += project[k]; + if (testdim != newdim) { + fprintf (qh ferr, "qhull internal error (qh_projectpoints): newdim %d should be %d after projection\n", + newdim, testdim); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + for (j= 0; j<n; j++) { + if (project[j] == -1) + oldk++; + else { + newp= newpoints+newk++; + if (project[j] == +1) { + if (oldk >= dim) + continue; + oldp= points+oldk; + }else + oldp= points+oldk++; + for (i=numpoints; i--; ) { + *newp= *oldp; + newp += newdim; + oldp += dim; + } + } + if (oldk >= dim) + break; + } + trace1((qh ferr, "qh_projectpoints: projected %d points from dim %d to dim %d\n", + numpoints, dim, newdim)); +} /* projectpoints */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="rand">-</a> + + qh_rand() + qh_srand( seed ) + generate pseudo-random number between 1 and 2^31 -2 + + notes: + from Park & Miller's minimimal standard random number generator + Communications of the ACM, 31:1192-1201, 1988. + does not use 0 or 2^31 -1 + this is silently enforced by qh_srand() + can make 'Rn' much faster by moving qh_rand to qh_distplane +*/ +int qh_rand_seed= 1; /* define as global variable instead of using qh */ + +int qh_rand( void) { +#define qh_rand_a 16807 +#define qh_rand_m 2147483647 +#define qh_rand_q 127773 /* m div a */ +#define qh_rand_r 2836 /* m mod a */ + int lo, hi, test; + int seed = qh_rand_seed; + + hi = seed / qh_rand_q; /* seed div q */ + lo = seed % qh_rand_q; /* seed mod q */ + test = qh_rand_a * lo - qh_rand_r * hi; + if (test > 0) + seed= test; + else + seed= test + qh_rand_m; + qh_rand_seed= seed; + /* seed = seed < qh_RANDOMmax/2 ? 0 : qh_RANDOMmax; for testing */ + /* seed = qh_RANDOMmax; for testing */ + return seed; +} /* rand */ + +void qh_srand( int seed) { + if (seed < 1) + qh_rand_seed= 1; + else if (seed >= qh_rand_m) + qh_rand_seed= qh_rand_m - 1; + else + qh_rand_seed= seed; +} /* qh_srand */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="randomfactor">-</a> + + qh_randomfactor() + return a random factor within qh.RANDOMmax of 1.0 + + notes: + qh.RANDOMa/b are defined in global.c +*/ +realT qh_randomfactor (void) { + realT randr; + + randr= qh_RANDOMint; + return randr * qh RANDOMa + qh RANDOMb; +} /* randomfactor */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="randommatrix">-</a> + + qh_randommatrix( buffer, dim, rows ) + generate a random dim X dim matrix in range [-1,1] + assumes buffer is [dim+1, dim] + + returns: + sets buffer to random numbers + sets rows to rows of buffer + sets row[dim] as scratch row +*/ +void qh_randommatrix (realT *buffer, int dim, realT **rows) { + int i, k; + realT **rowi, *coord, realr; + + coord= buffer; + rowi= rows; + for (i=0; i < dim; i++) { + *(rowi++)= coord; + for (k=0; k < dim; k++) { + realr= qh_RANDOMint; + *(coord++)= 2.0 * realr/(qh_RANDOMmax+1) - 1.0; + } + } + *rowi= coord; +} /* randommatrix */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="rotateinput">-</a> + + qh_rotateinput( rows ) + rotate input using row matrix + input points given by qh first_point, num_points, hull_dim + assumes rows[dim] is a scratch buffer + if qh POINTSmalloc, overwrites input points, else mallocs a new array + + returns: + rotated input + sets qh POINTSmalloc + + design: + see qh_rotatepoints +*/ +void qh_rotateinput (realT **rows) { + + if (!qh POINTSmalloc) { + qh first_point= qh_copypoints (qh first_point, qh num_points, qh hull_dim); + qh POINTSmalloc= True; + } + qh_rotatepoints (qh first_point, qh num_points, qh hull_dim, rows); +} /* rotateinput */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="rotatepoints">-</a> + + qh_rotatepoints( points, numpoints, dim, row ) + rotate numpoints points by a d-dim row matrix + assumes rows[dim] is a scratch buffer + + returns: + rotated points in place + + design: + for each point + for each coordinate + use row[dim] to compute partial inner product + for each coordinate + rotate by partial inner product +*/ +void qh_rotatepoints (realT *points, int numpoints, int dim, realT **row) { + realT *point, *rowi, *coord= NULL, sum, *newval; + int i,j,k; + + if (qh IStracing >= 1) + qh_printmatrix (qh ferr, "qh_rotatepoints: rotate points by", row, dim, dim); + for (point= points, j= numpoints; j--; point += dim) { + newval= row[dim]; + for (i= 0; i < dim; i++) { + rowi= row[i]; + coord= point; + for (sum= 0.0, k= dim; k--; ) + sum += *rowi++ * *coord++; + *(newval++)= sum; + } + for (k= dim; k--; ) + *(--coord)= *(--newval); + } +} /* rotatepoints */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="scaleinput">-</a> + + qh_scaleinput() + scale input points using qh low_bound/high_bound + input points given by qh first_point, num_points, hull_dim + if qh POINTSmalloc, overwrites input points, else mallocs a new array + + returns: + scales coordinates of points to low_bound[k], high_bound[k] + sets qh POINTSmalloc + + design: + see qh_scalepoints +*/ +void qh_scaleinput (void) { + + if (!qh POINTSmalloc) { + qh first_point= qh_copypoints (qh first_point, qh num_points, qh hull_dim); + qh POINTSmalloc= True; + } + qh_scalepoints (qh first_point, qh num_points, qh hull_dim, + qh lower_bound, qh upper_bound); +} /* scaleinput */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="scalelast">-</a> + + qh_scalelast( points, numpoints, dim, low, high, newhigh ) + scale last coordinate to [0,m] for Delaunay triangulations + input points given by points, numpoints, dim + + returns: + changes scale of last coordinate from [low, high] to [0, newhigh] + overwrites last coordinate of each point + saves low/high/newhigh in qh.last_low, etc. for qh_setdelaunay() + + notes: + when called by qh_setdelaunay, low/high may not match actual data + + design: + compute scale and shift factors + apply to last coordinate of each point +*/ +void qh_scalelast (coordT *points, int numpoints, int dim, coordT low, + coordT high, coordT newhigh) { + realT scale, shift; + coordT *coord; + int i; + boolT nearzero= False; + + trace4((qh ferr, "qh_scalelast: scale last coordinate from [%2.2g, %2.2g] to [0,%2.2g]\n", + low, high, newhigh)); + qh last_low= low; + qh last_high= high; + qh last_newhigh= newhigh; + scale= qh_divzero (newhigh, high - low, + qh MINdenom_1, &nearzero); + if (nearzero) { + if (qh DELAUNAY) + fprintf (qh ferr, "qhull input error: can not scale last coordinate. Input is cocircular\n or cospherical. Use option 'Qz' to add a point at infinity.\n"); + else + fprintf (qh ferr, "qhull input error: can not scale last coordinate. New bounds [0, %2.2g] are too wide for\nexisting bounds [%2.2g, %2.2g] (width %2.2g)\n", + newhigh, low, high, high-low); + qh_errexit (qh_ERRinput, NULL, NULL); + } + shift= - low * newhigh / (high-low); + coord= points + dim - 1; + for (i= numpoints; i--; coord += dim) + *coord= *coord * scale + shift; +} /* scalelast */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="scalepoints">-</a> + + qh_scalepoints( points, numpoints, dim, newlows, newhighs ) + scale points to new lowbound and highbound + retains old bound when newlow= -REALmax or newhigh= +REALmax + + returns: + scaled points + overwrites old points + + design: + for each coordinate + compute current low and high bound + compute scale and shift factors + scale all points + enforce new low and high bound for all points +*/ +void qh_scalepoints (pointT *points, int numpoints, int dim, + realT *newlows, realT *newhighs) { + int i,k; + realT shift, scale, *coord, low, high, newlow, newhigh, mincoord, maxcoord; + boolT nearzero= False; + + for (k= 0; k < dim; k++) { + newhigh= newhighs[k]; + newlow= newlows[k]; + if (newhigh > REALmax/2 && newlow < -REALmax/2) + continue; + low= REALmax; + high= -REALmax; + for (i= numpoints, coord= points+k; i--; coord += dim) { + minimize_(low, *coord); + maximize_(high, *coord); + } + if (newhigh > REALmax/2) + newhigh= high; + if (newlow < -REALmax/2) + newlow= low; + if (qh DELAUNAY && k == dim-1 && newhigh < newlow) { + fprintf (qh ferr, "qhull input error: 'Qb%d' or 'QB%d' inverts paraboloid since high bound %.2g < low bound %.2g\n", + k, k, newhigh, newlow); + qh_errexit (qh_ERRinput, NULL, NULL); + } + scale= qh_divzero (newhigh - newlow, high - low, + qh MINdenom_1, &nearzero); + if (nearzero) { + fprintf (qh ferr, "qhull input error: %d'th dimension's new bounds [%2.2g, %2.2g] too wide for\nexisting bounds [%2.2g, %2.2g]\n", + k, newlow, newhigh, low, high); + qh_errexit (qh_ERRinput, NULL, NULL); + } + shift= (newlow * high - low * newhigh)/(high-low); + coord= points+k; + for (i= numpoints; i--; coord += dim) + *coord= *coord * scale + shift; + coord= points+k; + if (newlow < newhigh) { + mincoord= newlow; + maxcoord= newhigh; + }else { + mincoord= newhigh; + maxcoord= newlow; + } + for (i= numpoints; i--; coord += dim) { + minimize_(*coord, maxcoord); /* because of roundoff error */ + maximize_(*coord, mincoord); + } + trace0((qh ferr, "qh_scalepoints: scaled %d'th coordinate [%2.2g, %2.2g] to [%.2g, %.2g] for %d points by %2.2g and shifted %2.2g\n", + k, low, high, newlow, newhigh, numpoints, scale, shift)); + } +} /* scalepoints */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="setdelaunay">-</a> + + qh_setdelaunay( dim, count, points ) + project count points to dim-d paraboloid for Delaunay triangulation + + dim is one more than the dimension of the input set + assumes dim is at least 3 (i.e., at least a 2-d Delaunay triangulation) + + points is a dim*count realT array. The first dim-1 coordinates + are the coordinates of the first input point. array[dim] is + the first coordinate of the second input point. array[2*dim] is + the first coordinate of the third input point. + + if qh.last_low defined (i.e., 'Qbb' called qh_scalelast) + calls qh_scalelast to scale the last coordinate the same as the other points + + returns: + for each point + sets point[dim-1] to sum of squares of coordinates + scale points to 'Qbb' if needed + + notes: + to project one point, use + qh_setdelaunay (qh hull_dim, 1, point) + + Do not use options 'Qbk', 'QBk', or 'QbB' since they scale + the coordinates after the original projection. + +*/ +void qh_setdelaunay (int dim, int count, pointT *points) { + int i, k; + coordT *coordp, coord; + realT paraboloid; + + trace0((qh ferr, "qh_setdelaunay: project %d points to paraboloid for Delaunay triangulation\n", count)); + coordp= points; + for (i= 0; i < count; i++) { + coord= *coordp++; + paraboloid= coord*coord; + for (k= dim-2; k--; ) { + coord= *coordp++; + paraboloid += coord*coord; + } + *coordp++ = paraboloid; + } + if (qh last_low < REALmax/2) + qh_scalelast (points, count, dim, qh last_low, qh last_high, qh last_newhigh); +} /* setdelaunay */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="sethalfspace">-</a> + + qh_sethalfspace( dim, coords, nextp, normal, offset, feasible ) + set point to dual of halfspace relative to feasible point + halfspace is normal coefficients and offset. + + returns: + false if feasible point is outside of hull (error message already reported) + overwrites coordinates for point at dim coords + nextp= next point (coords) + + design: + compute distance from feasible point to halfspace + divide each normal coefficient by -dist +*/ +boolT qh_sethalfspace (int dim, coordT *coords, coordT **nextp, + coordT *normal, coordT *offset, coordT *feasible) { + coordT *normp= normal, *feasiblep= feasible, *coordp= coords; + realT dist; + realT r; /*bug fix*/ + int k; + boolT zerodiv; + + dist= *offset; + for (k= dim; k--; ) + dist += *(normp++) * *(feasiblep++); + if (dist > 0) + goto LABELerroroutside; + normp= normal; + if (dist < -qh MINdenom) { + for (k= dim; k--; ) + *(coordp++)= *(normp++) / -dist; + }else { + for (k= dim; k--; ) { + *(coordp++)= qh_divzero (*(normp++), -dist, qh MINdenom_1, &zerodiv); + if (zerodiv) + goto LABELerroroutside; + } + } + *nextp= coordp; + if (qh IStracing >= 4) { + fprintf (qh ferr, "qh_sethalfspace: halfspace at offset %6.2g to point: ", *offset); + for (k= dim, coordp= coords; k--; ) { + r= *coordp++; + fprintf (qh ferr, " %6.2g", r); + } + fprintf (qh ferr, "\n"); + } + return True; +LABELerroroutside: + feasiblep= feasible; + normp= normal; + fprintf(qh ferr, "qhull input error: feasible point is not clearly inside halfspace\nfeasible point: "); + for (k= dim; k--; ) + fprintf (qh ferr, qh_REAL_1, r=*(feasiblep++)); + fprintf (qh ferr, "\n halfspace: "); + for (k= dim; k--; ) + fprintf (qh ferr, qh_REAL_1, r=*(normp++)); + fprintf (qh ferr, "\n at offset: "); + fprintf (qh ferr, qh_REAL_1, *offset); + fprintf (qh ferr, " and distance: "); + fprintf (qh ferr, qh_REAL_1, dist); + fprintf (qh ferr, "\n"); + return False; +} /* sethalfspace */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="sethalfspace_all">-</a> + + qh_sethalfspace_all( dim, count, halfspaces, feasible ) + generate dual for halfspace intersection with feasible point + array of count halfspaces + each halfspace is normal coefficients followed by offset + the origin is inside the halfspace if the offset is negative + + returns: + malloc'd array of count X dim-1 points + + notes: + call before qh_init_B or qh_initqhull_globals + unused/untested code: please email bradb@shore.net if this works ok for you + If using option 'Fp', also set qh feasible_point. It is a malloc'd array + that is freed by qh_freebuffers. + + design: + see qh_sethalfspace +*/ +coordT *qh_sethalfspace_all (int dim, int count, coordT *halfspaces, pointT *feasible) { + int i, newdim; + pointT *newpoints; + coordT *coordp, *normalp, *offsetp; + + trace0((qh ferr, "qh_sethalfspace_all: compute dual for halfspace intersection\n")); + newdim= dim - 1; + if (!(newpoints=(coordT*)malloc(count*newdim*sizeof(coordT)))){ + fprintf(qh ferr, "qhull error: insufficient memory to compute dual of %d halfspaces\n", + count); + qh_errexit(qh_ERRmem, NULL, NULL); + } + coordp= newpoints; + normalp= halfspaces; + for (i= 0; i < count; i++) { + offsetp= normalp + newdim; + if (!qh_sethalfspace (newdim, coordp, &coordp, normalp, offsetp, feasible)) { + fprintf (qh ferr, "The halfspace was at index %d\n", i); + qh_errexit (qh_ERRinput, NULL, NULL); + } + normalp= offsetp + 1; + } + return newpoints; +} /* sethalfspace_all */ + + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="sharpnewfacets">-</a> + + qh_sharpnewfacets() + + returns: + true if could be an acute angle (facets in different quadrants) + + notes: + for qh_findbest + + design: + for all facets on qh.newfacet_list + if two facets are in different quadrants + set issharp +*/ +boolT qh_sharpnewfacets () { + facetT *facet; + boolT issharp = False; + int *quadrant, k; + + quadrant= (int*)qh_memalloc (qh hull_dim * sizeof(int)); + FORALLfacet_(qh newfacet_list) { + if (facet == qh newfacet_list) { + for (k= qh hull_dim; k--; ) + quadrant[ k]= (facet->normal[ k] > 0); + }else { + for (k= qh hull_dim; k--; ) { + if (quadrant[ k] != (facet->normal[ k] > 0)) { + issharp= True; + break; + } + } + } + if (issharp) + break; + } + qh_memfree( quadrant, qh hull_dim * sizeof(int)); + trace3((qh ferr, "qh_sharpnewfacets: %d\n", issharp)); + return issharp; +} /* sharpnewfacets */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="voronoi_center">-</a> + + qh_voronoi_center( dim, points ) + return Voronoi center for a set of points + dim is the orginal dimension of the points + gh.gm_matrix/qh.gm_row are scratch buffers + + returns: + center as a temporary point + if non-simplicial, + returns center for max simplex of points + + notes: + from Bowyer & Woodwark, A Programmer's Geometry, 1983, p. 65 + + design: + if non-simplicial + determine max simplex for points + translate point0 of simplex to origin + compute sum of squares of diagonal + compute determinate + compute Voronoi center (see Bowyer & Woodwark) +*/ +pointT *qh_voronoi_center (int dim, setT *points) { + pointT *point, **pointp, *point0; + pointT *center= (pointT*)qh_memalloc (qh center_size); + setT *simplex; + int i, j, k, size= qh_setsize(points); + coordT *gmcoord; + realT *diffp, sum2, *sum2row, *sum2p, det, factor; + boolT nearzero, infinite; + + if (size == dim+1) + simplex= points; + else if (size < dim+1) { + fprintf (qh ferr, "qhull internal error (qh_voronoi_center):\n need at least %d points to construct a Voronoi center\n", + dim+1); + qh_errexit (qh_ERRqhull, NULL, NULL); + }else { + simplex= qh_settemp (dim+1); + qh_maxsimplex (dim, points, NULL, 0, &simplex); + } + point0= SETfirstt_(simplex, pointT); + gmcoord= qh gm_matrix; + for (k=0; k < dim; k++) { + qh gm_row[k]= gmcoord; + FOREACHpoint_(simplex) { + if (point != point0) + *(gmcoord++)= point[k] - point0[k]; + } + } + sum2row= gmcoord; + for (i=0; i < dim; i++) { + sum2= 0.0; + for (k= 0; k < dim; k++) { + diffp= qh gm_row[k] + i; + sum2 += *diffp * *diffp; + } + *(gmcoord++)= sum2; + } + det= qh_determinant (qh gm_row, dim, &nearzero); + factor= qh_divzero (0.5, det, qh MINdenom, &infinite); + if (infinite) { + for (k=dim; k--; ) + center[k]= qh_INFINITE; + if (qh IStracing) + qh_printpoints (qh ferr, "qh_voronoi_center: at infinity for ", simplex); + }else { + for (i=0; i < dim; i++) { + gmcoord= qh gm_matrix; + sum2p= sum2row; + for (k=0; k < dim; k++) { + qh gm_row[k]= gmcoord; + if (k == i) { + for (j= dim; j--; ) + *(gmcoord++)= *sum2p++; + }else { + FOREACHpoint_(simplex) { + if (point != point0) + *(gmcoord++)= point[k] - point0[k]; + } + } + } + center[i]= qh_determinant (qh gm_row, dim, &nearzero)*factor + point0[i]; + } +#ifndef qh_NOtrace + if (qh IStracing >= 3) { + fprintf (qh ferr, "qh_voronoi_center: det %2.2g factor %2.2g ", det, factor); + qh_printmatrix (qh ferr, "center:", ¢er, 1, dim); + if (qh IStracing >= 5) { + qh_printpoints (qh ferr, "points", simplex); + FOREACHpoint_(simplex) + fprintf (qh ferr, "p%d dist %.2g, ", qh_pointid (point), + qh_pointdist (point, center, dim)); + fprintf (qh ferr, "\n"); + } + } +#endif + } + if (simplex != points) + qh_settempfree (&simplex); + return center; +} /* voronoi_center */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/global.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/global.c new file mode 100644 index 0000000000000000000000000000000000000000..ff0cafb5eb339f21fdda7f6b908e1f82b0044b62 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/global.c @@ -0,0 +1,2041 @@ +/*<html><pre> -<a href="qh-globa.htm" + >-------------------------------</a><a name="TOP">-</a> + + global.c + initializes all the globals of the qhull application + + see README + + see qhull.h for qh.globals and function prototypes + + see qhull_a.h for internal functions + + copyright (c) 1993-2003, The Geometry Center + */ + +#include "qhull_a.h" + +/*========= qh definition (see qhull.h) =======================*/ + +#if qh_QHpointer +qhT *qh_qh= NULL; /* pointer to all global variables */ +#else +qhT qh_qh; /* all global variables. + Add "= {0}" if this causes a compiler error. + Also qh_qhstat in stat.c and qhmem in mem.c. */ +#endif + +/*-<a href ="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh_version">-</a> + + qh_version + version string by year and date + + the revision increases on code changes only + + notes: + change date: Changes.txt, Announce.txt, README.txt, + qhull.man, qhull.txt, qhull-news.html, Eudora signatures, + change version: README.txt, qh-get.htm, File_id.diz, Makefile.txt + change year: Copying.txt + check download size + recompile user_eg.c, rbox.c, qhull.c, qconvex.c, qdelaun.c qvoronoi.c, qhalf.c +*/ + +char *qh_version = "2003.1 2003/12/30"; + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="appendprint">-</a> + + qh_appendprint( printFormat ) + append printFormat to qh.PRINTout unless already defined +*/ +void qh_appendprint (qh_PRINT format) { + int i; + + for (i=0; i < qh_PRINTEND; i++) { + if (qh PRINTout[i] == format && format != qh_PRINTqhull) + break; + if (!qh PRINTout[i]) { + qh PRINTout[i]= format; + break; + } + } +} /* appendprint */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="checkflags">-</a> + + qh_checkflags( commandStr, hiddenFlags ) + errors if commandStr contains hiddenFlags + hiddenFlags starts and ends with a space and is space deliminated (checked) + + notes: + ignores first word (e.g., "qconvex i") + use qh_strtol/strtod since strtol/strtod may or may not skip trailing spaces + + see: + qh_initflags() initializes Qhull according to commandStr +*/ +void qh_checkflags(char *command, char *hiddenflags) { + char *s= command, *t, *chkerr, key, opt, prevopt; + char chkkey[]= " "; + char chkopt[]= " "; + char chkopt2[]= " "; + + if (*hiddenflags != ' ' || hiddenflags[strlen(hiddenflags)-1] != ' ') { + fprintf(qh ferr, "qhull error (qh_checkflags): hiddenflags must start and end with a space: \"%s\"", hiddenflags); + qh_errexit(qh_ERRinput, NULL, NULL); + } + if (strpbrk(hiddenflags, ",\n\r\t")) { + fprintf(qh ferr, "qhull error (qh_checkflags): hiddenflags contains commas, newlines, or tabs: \"%s\"", hiddenflags); + qh_errexit(qh_ERRinput, NULL, NULL); + } + while (*s && !isspace(*s)) /* skip program name */ + s++; + while (*s) { + while (*s && isspace(*s)) + s++; + if (*s == '-') + s++; + if (!*s) + break; + key = *s++; + chkerr = NULL; + if (key == '\'') { /* TO 'file name' */ + t= strchr(s, '\''); + if (!t) { + fprintf(qh ferr, "qhull error (qh_checkflags): missing the 2nd single-quote for:\n%s\n", s-1); + qh_errexit(qh_ERRinput, NULL, NULL); + } + s= t+1; + continue; + } + chkkey[1]= key; + if (strstr(hiddenflags, chkkey)) { + chkerr= chkkey; + }else if (isupper(key)) { + opt= ' '; + prevopt= ' '; + chkopt[1]= key; + chkopt2[1]= key; + while (!chkerr && *s && !isspace(*s)) { + opt= *s++; + if (isalpha(opt)) { + chkopt[2]= opt; + if (strstr(hiddenflags, chkopt)) + chkerr= chkopt; + if (prevopt != ' ') { + chkopt2[2]= prevopt; + chkopt2[3]= opt; + if (strstr(hiddenflags, chkopt2)) + chkerr= chkopt2; + } + }else if (key == 'Q' && isdigit(opt) && prevopt != 'b' + && (prevopt == ' ' || islower(prevopt))) { + chkopt[2]= opt; + if (strstr(hiddenflags, chkopt)) + chkerr= chkopt; + }else { + qh_strtod (s-1, &t); + if (s < t) + s= t; + } + prevopt= opt; + } + } + if (chkerr) { + *chkerr= '\''; + chkerr[strlen(chkerr)-1]= '\''; + fprintf(qh ferr, "qhull error: option %s is not used with this program.\n It may be used with qhull.\n", chkerr); + qh_errexit(qh_ERRinput, NULL, NULL); + } + } +} /* checkflags */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="clock">-</a> + + qh_clock() + return user CPU time in 100ths (qh_SECtick) + only defined for qh_CLOCKtype == 2 + + notes: + use first value to determine time 0 + from Stevens '92 8.15 +*/ +unsigned long qh_clock (void) { + +#if (qh_CLOCKtype == 2) + struct tms time; + static long clktck; /* initialized first call */ + double ratio, cpu; + unsigned long ticks; + + if (!clktck) { + if ((clktck= sysconf (_SC_CLK_TCK)) < 0) { + fprintf (qh ferr, "qhull internal error (qh_clock): sysconf() failed. Use qh_CLOCKtype 1 in user.h\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + } + if (times (&time) == -1) { + fprintf (qh ferr, "qhull internal error (qh_clock): times() failed. Use qh_CLOCKtype 1 in user.h\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + ratio= qh_SECticks / (double)clktck; + ticks= time.tms_utime * ratio; + return ticks; +#else + fprintf (qh ferr, "qhull internal error (qh_clock): use qh_CLOCKtype 2 in user.h\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); /* never returns */ + return 0; +#endif +} /* clock */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="freebuffers">-</a> + + qh_freebuffers() + free up global memory buffers + + notes: + must match qh_initbuffers() +*/ +void qh_freebuffers (void) { + + trace5((qh ferr, "qh_freebuffers: freeing up global memory buffers\n")); + /* allocated by qh_initqhull_buffers */ + qh_memfree (qh NEARzero, qh hull_dim * sizeof(realT)); + qh_memfree (qh lower_threshold, (qh input_dim+1) * sizeof(realT)); + qh_memfree (qh upper_threshold, (qh input_dim+1) * sizeof(realT)); + qh_memfree (qh lower_bound, (qh input_dim+1) * sizeof(realT)); + qh_memfree (qh upper_bound, (qh input_dim+1) * sizeof(realT)); + qh_memfree (qh gm_matrix, (qh hull_dim+1) * qh hull_dim * sizeof(coordT)); + qh_memfree (qh gm_row, (qh hull_dim+1) * sizeof(coordT *)); + qh NEARzero= qh lower_threshold= qh upper_threshold= NULL; + qh lower_bound= qh upper_bound= NULL; + qh gm_matrix= NULL; + qh gm_row= NULL; + qh_setfree (&qh other_points); + qh_setfree (&qh del_vertices); + qh_setfree (&qh coplanarset); + if (qh line) /* allocated by qh_readinput, freed if no error */ + free (qh line); + if (qh half_space) + free (qh half_space); + if (qh temp_malloc) + free (qh temp_malloc); + if (qh feasible_point) /* allocated by qh_readfeasible */ + free (qh feasible_point); + if (qh feasible_string) /* allocated by qh_initflags */ + free (qh feasible_string); + qh line= qh feasible_string= NULL; + qh half_space= qh feasible_point= qh temp_malloc= NULL; + /* usually allocated by qh_readinput */ + if (qh first_point && qh POINTSmalloc) { + free(qh first_point); + qh first_point= NULL; + } + if (qh input_points && qh input_malloc) { /* set by qh_joggleinput */ + free (qh input_points); + qh input_points= NULL; + } + trace5((qh ferr, "qh_freebuffers: finished\n")); +} /* freebuffers */ + + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="freebuild">-</a> + + qh_freebuild( allmem ) + free global memory used by qh_initbuild and qh_buildhull + if !allmem, + does not free short memory (freed by qh_memfreeshort) + + design: + free centrums + free each vertex + mark unattached ridges + for each facet + free ridges + free outside set, coplanar set, neighbor set, ridge set, vertex set + free facet + free hash table + free interior point + free merge set + free temporary sets +*/ +void qh_freebuild (boolT allmem) { + facetT *facet; + vertexT *vertex; + ridgeT *ridge, **ridgep; + mergeT *merge, **mergep; + + trace1((qh ferr, "qh_freebuild: free memory from qh_inithull and qh_buildhull\n")); + if (qh del_vertices) + qh_settruncate (qh del_vertices, 0); + if (allmem) { + qh_clearcenters (qh_ASnone); + while ((vertex= qh vertex_list)) { + if (vertex->next) + qh_delvertex (vertex); + else { + qh_memfree (vertex, sizeof(vertexT)); + qh newvertex_list= qh vertex_list= NULL; + } + } + }else if (qh VERTEXneighbors) { + FORALLvertices + qh_setfreelong (&(vertex->neighbors)); + } + qh VERTEXneighbors= False; + qh GOODclosest= NULL; + if (allmem) { + FORALLfacets { + FOREACHridge_(facet->ridges) + ridge->seen= False; + } + FORALLfacets { + if (facet->visible) { + FOREACHridge_(facet->ridges) { + if (!otherfacet_(ridge, facet)->visible) + ridge->seen= True; /* an unattached ridge */ + } + } + } + while ((facet= qh facet_list)) { + FOREACHridge_(facet->ridges) { + if (ridge->seen) { + qh_setfree(&(ridge->vertices)); + qh_memfree(ridge, sizeof(ridgeT)); + }else + ridge->seen= True; + } + qh_setfree (&(facet->outsideset)); + qh_setfree (&(facet->coplanarset)); + qh_setfree (&(facet->neighbors)); + qh_setfree (&(facet->ridges)); + qh_setfree (&(facet->vertices)); + if (facet->next) + qh_delfacet (facet); + else { + qh_memfree (facet, sizeof(facetT)); + qh visible_list= qh newfacet_list= qh facet_list= NULL; + } + } + }else { + FORALLfacets { + qh_setfreelong (&(facet->outsideset)); + qh_setfreelong (&(facet->coplanarset)); + if (!facet->simplicial) { + qh_setfreelong (&(facet->neighbors)); + qh_setfreelong (&(facet->ridges)); + qh_setfreelong (&(facet->vertices)); + } + } + } + qh_setfree (&(qh hash_table)); + qh_memfree (qh interior_point, qh normal_size); + qh interior_point= NULL; + FOREACHmerge_(qh facet_mergeset) /* usually empty */ + qh_memfree (merge, sizeof(mergeT)); + qh facet_mergeset= NULL; /* temp set */ + qh degen_mergeset= NULL; /* temp set */ + qh_settempfree_all(); +} /* freebuild */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="freeqhull">-</a> + + qh_freeqhull( allmem ) + free global memory + if !allmem, + does not free short memory (freed by qh_memfreeshort) + + notes: + sets qh.NOerrexit in case caller forgets to + + design: + free global and temporary memory from qh_initbuild and qh_buildhull + free buffers + free statistics +*/ +void qh_freeqhull (boolT allmem) { + + trace1((qh ferr, "qh_freeqhull: free global memory\n")); + qh NOerrexit= True; /* no more setjmp since called at exit */ + qh_freebuild (allmem); + qh_freebuffers(); + qh_freestatistics(); +#if qh_QHpointer + free (qh_qh); + qh_qh= NULL; +#else + memset((char *)&qh_qh, 0, sizeof(qhT)); + qh NOerrexit= True; +#endif +} /* freeqhull */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="init_A">-</a> + + qh_init_A( infile, outfile, errfile, argc, argv ) + initialize memory and stdio files + convert input options to option string (qh.qhull_command) + + notes: + infile may be NULL if qh_readpoints() is not called + + errfile should always be defined. It is used for reporting + errors. outfile is used for output and format options. + + argc/argv may be 0/NULL + + called before error handling initialized + qh_errexit() may not be used +*/ +void qh_init_A (FILE *infile, FILE *outfile, FILE *errfile, int argc, char *argv[]) { + qh_meminit (errfile); + qh_initqhull_start (infile, outfile, errfile); + qh_init_qhull_command (argc, argv); +} /* init_A */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="init_B">-</a> + + qh_init_B( points, numpoints, dim, ismalloc ) + initialize globals for points array + + points has numpoints dim-dimensional points + points[0] is the first coordinate of the first point + points[1] is the second coordinate of the first point + points[dim] is the first coordinate of the second point + + ismalloc=True + Qhull will call free(points) on exit or input transformation + ismalloc=False + Qhull will allocate a new point array if needed for input transformation + + qh.qhull_command + is the option string. + It is defined by qh_init_B(), qh_qhull_command(), or qh_initflags + + returns: + if qh.PROJECTinput or (qh.DELAUNAY and qh.PROJECTdelaunay) + projects the input to a new point array + + if qh.DELAUNAY, + qh.hull_dim is increased by one + if qh.ATinfinity, + qh_projectinput adds point-at-infinity for Delaunay tri. + + if qh.SCALEinput + changes the upper and lower bounds of the input, see qh_scaleinput() + + if qh.ROTATEinput + rotates the input by a random rotation, see qh_rotateinput() + if qh.DELAUNAY + rotates about the last coordinate + + notes: + called after points are defined + qh_errexit() may be used +*/ +void qh_init_B (coordT *points, int numpoints, int dim, boolT ismalloc) { + qh_initqhull_globals (points, numpoints, dim, ismalloc); + if (qhmem.LASTsize == 0) + qh_initqhull_mem(); + /* mem.c and qset.c are initialized */ + qh_initqhull_buffers(); + qh_initthresholds (qh qhull_command); + if (qh PROJECTinput || (qh DELAUNAY && qh PROJECTdelaunay)) + qh_projectinput(); + if (qh SCALEinput) + qh_scaleinput(); + if (qh ROTATErandom >= 0) { + qh_randommatrix (qh gm_matrix, qh hull_dim, qh gm_row); + if (qh DELAUNAY) { + int k, lastk= qh hull_dim-1; + for (k= 0; k < lastk; k++) { + qh gm_row[k][lastk]= 0.0; + qh gm_row[lastk][k]= 0.0; + } + qh gm_row[lastk][lastk]= 1.0; + } + qh_gram_schmidt (qh hull_dim, qh gm_row); + qh_rotateinput (qh gm_row); + } +} /* init_B */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="init_qhull_command">-</a> + + qh_init_qhull_command( argc, argv ) + build qh.qhull_command from argc/argv + + returns: + a space-deliminated string of options (just as typed) + + notes: + makes option string easy to input and output + + argc/argv may be 0/NULL +*/ +void qh_init_qhull_command(int argc, char *argv[]) { + int i; + char *s; + + if (argc) { + if ((s= strrchr( argv[0], '\\'))) /* Borland gives full path */ + strcpy (qh qhull_command, s+1); + else + strcpy (qh qhull_command, argv[0]); + if ((s= strstr (qh qhull_command, ".EXE")) + || (s= strstr (qh qhull_command, ".exe"))) + *s= '\0'; + } + for (i=1; i < argc; i++) { + if (strlen (qh qhull_command) + strlen(argv[i]) + 1 < sizeof(qh qhull_command)) { + strcat (qh qhull_command, " "); + strcat (qh qhull_command, argv[i]); + }else { + fprintf (qh ferr, "qhull input error: more than %d characters in command line\n", + (int)sizeof(qh qhull_command)); + exit (1); /* can not use qh_errexit */ + } + } +} /* init_qhull_command */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="initflags">-</a> + + qh_initflags( commandStr ) + set flags and initialized constants from commandStr + + returns: + sets qh.qhull_command to command if needed + + notes: + ignores first word (e.g., "qhull d") + use qh_strtol/strtod since strtol/strtod may or may not skip trailing spaces + + see: + qh_initthresholds() continues processing of 'Pdn' and 'PDn' + 'prompt' in unix.c for documentation + + design: + for each space-deliminated option group + if top-level option + check syntax + append approriate option to option string + set appropriate global variable or append printFormat to print options + else + for each sub-option + check syntax + append approriate option to option string + set appropriate global variable or append printFormat to print options + + +*/ +void qh_initflags(char *command) { + int k, i, lastproject; + char *s= command, *t, *prev_s, *start, key; + boolT isgeom= False, wasproject; + realT r; + + if (command != &qh qhull_command[0]) { + *qh qhull_command= '\0'; + strncat( qh qhull_command, command, sizeof( qh qhull_command)); + } + while (*s && !isspace(*s)) /* skip program name */ + s++; + while (*s) { + while (*s && isspace(*s)) + s++; + if (*s == '-') + s++; + if (!*s) + break; + prev_s= s; + switch (*s++) { + case 'd': + qh_option ("delaunay", NULL, NULL); + qh DELAUNAY= True; + break; + case 'f': + qh_option ("facets", NULL, NULL); + qh_appendprint (qh_PRINTfacets); + break; + case 'i': + qh_option ("incidence", NULL, NULL); + qh_appendprint (qh_PRINTincidences); + break; + case 'm': + qh_option ("mathematica", NULL, NULL); + qh_appendprint (qh_PRINTmathematica); + break; + case 'n': + qh_option ("normals", NULL, NULL); + qh_appendprint (qh_PRINTnormals); + break; + case 'o': + qh_option ("offFile", NULL, NULL); + qh_appendprint (qh_PRINToff); + break; + case 'p': + qh_option ("points", NULL, NULL); + qh_appendprint (qh_PRINTpoints); + break; + case 's': + qh_option ("summary", NULL, NULL); + qh PRINTsummary= True; + break; + case 'v': + qh_option ("voronoi", NULL, NULL); + qh VORONOI= True; + qh DELAUNAY= True; + break; + case 'A': + if (!isdigit(*s) && *s != '.' && *s != '-') + fprintf(qh ferr, "qhull warning: no maximum cosine angle given for option 'An'. Ignored.\n"); + else { + if (*s == '-') { + qh premerge_cos= -qh_strtod (s, &s); + qh_option ("Angle-premerge-", NULL, &qh premerge_cos); + qh PREmerge= True; + }else { + qh postmerge_cos= qh_strtod (s, &s); + qh_option ("Angle-postmerge", NULL, &qh postmerge_cos); + qh POSTmerge= True; + } + qh MERGING= True; + } + break; + case 'C': + if (!isdigit(*s) && *s != '.' && *s != '-') + fprintf(qh ferr, "qhull warning: no centrum radius given for option 'Cn'. Ignored.\n"); + else { + if (*s == '-') { + qh premerge_centrum= -qh_strtod (s, &s); + qh_option ("Centrum-premerge-", NULL, &qh premerge_centrum); + qh PREmerge= True; + }else { + qh postmerge_centrum= qh_strtod (s, &s); + qh_option ("Centrum-postmerge", NULL, &qh postmerge_centrum); + qh POSTmerge= True; + } + qh MERGING= True; + } + break; + case 'E': + if (*s == '-') + fprintf(qh ferr, "qhull warning: negative maximum roundoff given for option 'An'. Ignored.\n"); + else if (!isdigit(*s)) + fprintf(qh ferr, "qhull warning: no maximum roundoff given for option 'En'. Ignored.\n"); + else { + qh DISTround= qh_strtod (s, &s); + qh_option ("Distance-roundoff", NULL, &qh DISTround); + qh SETroundoff= True; + } + break; + case 'H': + start= s; + qh HALFspace= True; + qh_strtod (s, &t); + while (t > s) { + if (*t && !isspace (*t)) { + if (*t == ',') + t++; + else + fprintf (qh ferr, "qhull warning: origin for Halfspace intersection should be 'Hn,n,n,...'\n"); + } + s= t; + qh_strtod (s, &t); + } + if (start < t) { + if (!(qh feasible_string= (char*)calloc (t-start+1, 1))) { + fprintf(qh ferr, "qhull error: insufficient memory for 'Hn,n,n'\n"); + qh_errexit(qh_ERRmem, NULL, NULL); + } + strncpy (qh feasible_string, start, t-start); + qh_option ("Halfspace-about", NULL, NULL); + qh_option (qh feasible_string, NULL, NULL); + }else + qh_option ("Halfspace", NULL, NULL); + break; + case 'R': + if (!isdigit(*s)) + fprintf(qh ferr, "qhull warning: missing random perturbation for option 'Rn'. Ignored\n"); + else { + qh RANDOMfactor= qh_strtod (s, &s); + qh_option ("Random_perturb", NULL, &qh RANDOMfactor); + qh RANDOMdist= True; + } + break; + case 'V': + if (!isdigit(*s) && *s != '-') + fprintf(qh ferr, "qhull warning: missing visible distance for option 'Vn'. Ignored\n"); + else { + qh MINvisible= qh_strtod (s, &s); + qh_option ("Visible", NULL, &qh MINvisible); + } + break; + case 'U': + if (!isdigit(*s) && *s != '-') + fprintf(qh ferr, "qhull warning: missing coplanar distance for option 'Un'. Ignored\n"); + else { + qh MAXcoplanar= qh_strtod (s, &s); + qh_option ("U-coplanar", NULL, &qh MAXcoplanar); + } + break; + case 'W': + if (*s == '-') + fprintf(qh ferr, "qhull warning: negative outside width for option 'Wn'. Ignored.\n"); + else if (!isdigit(*s)) + fprintf(qh ferr, "qhull warning: missing outside width for option 'Wn'. Ignored\n"); + else { + qh MINoutside= qh_strtod (s, &s); + qh_option ("W-outside", NULL, &qh MINoutside); + qh APPROXhull= True; + } + break; + /************ sub menus ***************/ + case 'F': + while (*s && !isspace(*s)) { + switch(*s++) { + case 'a': + qh_option ("Farea", NULL, NULL); + qh_appendprint (qh_PRINTarea); + qh GETarea= True; + break; + case 'A': + qh_option ("FArea-total", NULL, NULL); + qh GETarea= True; + break; + case 'c': + qh_option ("Fcoplanars", NULL, NULL); + qh_appendprint (qh_PRINTcoplanars); + break; + case 'C': + qh_option ("FCentrums", NULL, NULL); + qh_appendprint (qh_PRINTcentrums); + break; + case 'd': + qh_option ("Fd-cdd-in", NULL, NULL); + qh CDDinput= True; + break; + case 'D': + qh_option ("FD-cdd-out", NULL, NULL); + qh CDDoutput= True; + break; + case 'F': + qh_option ("FFacets-xridge", NULL, NULL); + qh_appendprint (qh_PRINTfacets_xridge); + break; + case 'i': + qh_option ("Finner", NULL, NULL); + qh_appendprint (qh_PRINTinner); + break; + case 'I': + qh_option ("FIDs", NULL, NULL); + qh_appendprint (qh_PRINTids); + break; + case 'm': + qh_option ("Fmerges", NULL, NULL); + qh_appendprint (qh_PRINTmerges); + break; + case 'M': + qh_option ("FMaple", NULL, NULL); + qh_appendprint (qh_PRINTmaple); + break; + case 'n': + qh_option ("Fneighbors", NULL, NULL); + qh_appendprint (qh_PRINTneighbors); + break; + case 'N': + qh_option ("FNeighbors-vertex", NULL, NULL); + qh_appendprint (qh_PRINTvneighbors); + break; + case 'o': + qh_option ("Fouter", NULL, NULL); + qh_appendprint (qh_PRINTouter); + break; + case 'O': + if (qh PRINToptions1st) { + qh_option ("FOptions", NULL, NULL); + qh_appendprint (qh_PRINToptions); + }else + qh PRINToptions1st= True; + break; + case 'p': + qh_option ("Fpoint-intersect", NULL, NULL); + qh_appendprint (qh_PRINTpointintersect); + break; + case 'P': + qh_option ("FPoint-nearest", NULL, NULL); + qh_appendprint (qh_PRINTpointnearest); + break; + case 'Q': + qh_option ("FQhull", NULL, NULL); + qh_appendprint (qh_PRINTqhull); + break; + case 's': + qh_option ("Fsummary", NULL, NULL); + qh_appendprint (qh_PRINTsummary); + break; + case 'S': + qh_option ("FSize", NULL, NULL); + qh_appendprint (qh_PRINTsize); + qh GETarea= True; + break; + case 't': + qh_option ("Ftriangles", NULL, NULL); + qh_appendprint (qh_PRINTtriangles); + break; + case 'v': + /* option set in qh_initqhull_globals */ + qh_appendprint (qh_PRINTvertices); + break; + case 'V': + qh_option ("FVertex-average", NULL, NULL); + qh_appendprint (qh_PRINTaverage); + break; + case 'x': + qh_option ("Fxtremes", NULL, NULL); + qh_appendprint (qh_PRINTextremes); + break; + default: + s--; + fprintf (qh ferr, "qhull warning: unknown 'F' output option %c, rest ignored\n", (int)s[0]); + while (*++s && !isspace(*s)); + break; + } + } + break; + case 'G': + isgeom= True; + qh_appendprint (qh_PRINTgeom); + while (*s && !isspace(*s)) { + switch(*s++) { + case 'a': + qh_option ("Gall-points", NULL, NULL); + qh PRINTdots= True; + break; + case 'c': + qh_option ("Gcentrums", NULL, NULL); + qh PRINTcentrums= True; + break; + case 'h': + qh_option ("Gintersections", NULL, NULL); + qh DOintersections= True; + break; + case 'i': + qh_option ("Ginner", NULL, NULL); + qh PRINTinner= True; + break; + case 'n': + qh_option ("Gno-planes", NULL, NULL); + qh PRINTnoplanes= True; + break; + case 'o': + qh_option ("Gouter", NULL, NULL); + qh PRINTouter= True; + break; + case 'p': + qh_option ("Gpoints", NULL, NULL); + qh PRINTcoplanar= True; + break; + case 'r': + qh_option ("Gridges", NULL, NULL); + qh PRINTridges= True; + break; + case 't': + qh_option ("Gtransparent", NULL, NULL); + qh PRINTtransparent= True; + break; + case 'v': + qh_option ("Gvertices", NULL, NULL); + qh PRINTspheres= True; + break; + case 'D': + if (!isdigit (*s)) + fprintf (qh ferr, "qhull input error: missing dimension for option 'GDn'\n"); + else { + if (qh DROPdim >= 0) + fprintf (qh ferr, "qhull warning: can only drop one dimension. Previous 'GD%d' ignored\n", + qh DROPdim); + qh DROPdim= qh_strtol (s, &s); + qh_option ("GDrop-dim", &qh DROPdim, NULL); + } + break; + default: + s--; + fprintf (qh ferr, "qhull warning: unknown 'G' print option %c, rest ignored\n", (int)s[0]); + while (*++s && !isspace(*s)); + break; + } + } + break; + case 'P': + while (*s && !isspace(*s)) { + switch(*s++) { + case 'd': case 'D': /* see qh_initthresholds() */ + key= s[-1]; + i= qh_strtol (s, &s); + r= 0; + if (*s == ':') { + s++; + r= qh_strtod (s, &s); + } + if (key == 'd') + qh_option ("Pdrop-facets-dim-less", &i, &r); + else + qh_option ("PDrop-facets-dim-more", &i, &r); + break; + case 'g': + qh_option ("Pgood-facets", NULL, NULL); + qh PRINTgood= True; + break; + case 'G': + qh_option ("PGood-facet-neighbors", NULL, NULL); + qh PRINTneighbors= True; + break; + case 'o': + qh_option ("Poutput-forced", NULL, NULL); + qh FORCEoutput= True; + break; + case 'p': + qh_option ("Pprecision-ignore", NULL, NULL); + qh PRINTprecision= False; + break; + case 'A': + if (!isdigit (*s)) + fprintf (qh ferr, "qhull input error: missing facet count for keep area option 'PAn'\n"); + else { + qh KEEParea= qh_strtol (s, &s); + qh_option ("PArea-keep", &qh KEEParea, NULL); + qh GETarea= True; + } + break; + case 'F': + if (!isdigit (*s)) + fprintf (qh ferr, "qhull input error: missing facet area for option 'PFn'\n"); + else { + qh KEEPminArea= qh_strtod (s, &s); + qh_option ("PFacet-area-keep", NULL, &qh KEEPminArea); + qh GETarea= True; + } + break; + case 'M': + if (!isdigit (*s)) + fprintf (qh ferr, "qhull input error: missing merge count for option 'PMn'\n"); + else { + qh KEEPmerge= qh_strtol (s, &s); + qh_option ("PMerge-keep", &qh KEEPmerge, NULL); + } + break; + default: + s--; + fprintf (qh ferr, "qhull warning: unknown 'P' print option %c, rest ignored\n", (int)s[0]); + while (*++s && !isspace(*s)); + break; + } + } + break; + case 'Q': + lastproject= -1; + while (*s && !isspace(*s)) { + switch(*s++) { + case 'b': case 'B': /* handled by qh_initthresholds */ + key= s[-1]; + if (key == 'b' && *s == 'B') { + s++; + r= qh_DEFAULTbox; + qh SCALEinput= True; + qh_option ("QbBound-unit-box", NULL, &r); + break; + } + if (key == 'b' && *s == 'b') { + s++; + qh SCALElast= True; + qh_option ("Qbbound-last", NULL, NULL); + break; + } + k= qh_strtol (s, &s); + r= 0.0; + wasproject= False; + if (*s == ':') { + s++; + if ((r= qh_strtod(s, &s)) == 0.0) { + t= s; /* need true dimension for memory allocation */ + while (*t && !isspace(*t)) { + if (toupper(*t++) == 'B' + && k == qh_strtol (t, &t) + && *t++ == ':' + && qh_strtod(t, &t) == 0.0) { + qh PROJECTinput++; + trace2((qh ferr, "qh_initflags: project dimension %d\n", k)); + qh_option ("Qb-project-dim", &k, NULL); + wasproject= True; + lastproject= k; + break; + } + } + } + } + if (!wasproject) { + if (lastproject == k && r == 0.0) + lastproject= -1; /* doesn't catch all possible sequences */ + else if (key == 'b') { + qh SCALEinput= True; + if (r == 0.0) + r= -qh_DEFAULTbox; + qh_option ("Qbound-dim-low", &k, &r); + }else { + qh SCALEinput= True; + if (r == 0.0) + r= qh_DEFAULTbox; + qh_option ("QBound-dim-high", &k, &r); + } + } + break; + case 'c': + qh_option ("Qcoplanar-keep", NULL, NULL); + qh KEEPcoplanar= True; + break; + case 'f': + qh_option ("Qfurthest-outside", NULL, NULL); + qh BESToutside= True; + break; + case 'g': + qh_option ("Qgood-facets-only", NULL, NULL); + qh ONLYgood= True; + break; + case 'i': + qh_option ("Qinterior-keep", NULL, NULL); + qh KEEPinside= True; + break; + case 'm': + qh_option ("Qmax-outside-only", NULL, NULL); + qh ONLYmax= True; + break; + case 'r': + qh_option ("Qrandom-outside", NULL, NULL); + qh RANDOMoutside= True; + break; + case 's': + qh_option ("Qsearch-initial-simplex", NULL, NULL); + qh ALLpoints= True; + break; + case 't': + qh_option ("Qtriangulate", NULL, NULL); + qh TRIangulate= True; + break; + case 'T': + qh_option ("QTestPoints", NULL, NULL); + if (!isdigit (*s)) + fprintf (qh ferr, "qhull input error: missing number of test points for option 'QTn'\n"); + else { + qh TESTpoints= qh_strtol (s, &s); + qh_option ("QTestPoints", &qh TESTpoints, NULL); + } + break; + case 'u': + qh_option ("QupperDelaunay", NULL, NULL); + qh UPPERdelaunay= True; + break; + case 'v': + qh_option ("Qvertex-neighbors-convex", NULL, NULL); + qh TESTvneighbors= True; + break; + case 'x': + qh_option ("Qxact-merge", NULL, NULL); + qh MERGEexact= True; + break; + case 'z': + qh_option ("Qz-infinity-point", NULL, NULL); + qh ATinfinity= True; + break; + case '0': + qh_option ("Q0-no-premerge", NULL, NULL); + qh NOpremerge= True; + break; + case '1': + if (!isdigit(*s)) { + qh_option ("Q1-no-angle-sort", NULL, NULL); + qh ANGLEmerge= False; + break; + } + switch(*s++) { + case '0': + qh_option ("Q10-no-narrow", NULL, NULL); + qh NOnarrow= True; + break; + case '1': + qh_option ("Q11-trinormals Qtriangulate", NULL, NULL); + qh TRInormals= True; + qh TRIangulate= True; + break; + default: + s--; + fprintf (qh ferr, "qhull warning: unknown 'Q' qhull option 1%c, rest ignored\n", (int)s[0]); + while (*++s && !isspace(*s)); + break; + } + break; + case '2': + qh_option ("Q2-no-merge-independent", NULL, NULL); + qh MERGEindependent= False; + goto LABELcheckdigit; + break; /* no warnings */ + case '3': + qh_option ("Q3-no-merge-vertices", NULL, NULL); + qh MERGEvertices= False; + LABELcheckdigit: + if (isdigit(*s)) + fprintf (qh ferr, "qhull warning: can not follow '1', '2', or '3' with a digit. '%c' skipped.\n", + *s++); + break; + case '4': + qh_option ("Q4-avoid-old-into-new", NULL, NULL); + qh AVOIDold= True; + break; + case '5': + qh_option ("Q5-no-check-outer", NULL, NULL); + qh SKIPcheckmax= True; + break; + case '6': + qh_option ("Q6-no-concave-merge", NULL, NULL); + qh SKIPconvex= True; + break; + case '7': + qh_option ("Q7-no-breadth-first", NULL, NULL); + qh VIRTUALmemory= True; + break; + case '8': + qh_option ("Q8-no-near-inside", NULL, NULL); + qh NOnearinside= True; + break; + case '9': + qh_option ("Q9-pick-furthest", NULL, NULL); + qh PICKfurthest= True; + break; + case 'G': + i= qh_strtol (s, &t); + if (qh GOODpoint) + fprintf (qh ferr, "qhull warning: good point already defined for option 'QGn'. Ignored\n"); + else if (s == t) + fprintf (qh ferr, "qhull warning: missing good point id for option 'QGn'. Ignored\n"); + else if (i < 0 || *s == '-') { + qh GOODpoint= i-1; + qh_option ("QGood-if-dont-see-point", &i, NULL); + }else { + qh GOODpoint= i+1; + qh_option ("QGood-if-see-point", &i, NULL); + } + s= t; + break; + case 'J': + if (!isdigit(*s) && *s != '-') + qh JOGGLEmax= 0.0; + else { + qh JOGGLEmax= (realT) qh_strtod (s, &s); + qh_option ("QJoggle", NULL, &qh JOGGLEmax); + } + break; + case 'R': + if (!isdigit(*s) && *s != '-') + fprintf (qh ferr, "qhull warning: missing random seed for option 'QRn'. Ignored\n"); + else { + qh ROTATErandom= i= qh_strtol(s, &s); + if (i > 0) + qh_option ("QRotate-id", &i, NULL ); + else if (i < -1) + qh_option ("QRandom-seed", &i, NULL ); + } + break; + case 'V': + i= qh_strtol (s, &t); + if (qh GOODvertex) + fprintf (qh ferr, "qhull warning: good vertex already defined for option 'QVn'. Ignored\n"); + else if (s == t) + fprintf (qh ferr, "qhull warning: no good point id given for option 'QVn'. Ignored\n"); + else if (i < 0) { + qh GOODvertex= i - 1; + qh_option ("QV-good-facets-not-point", &i, NULL); + }else { + qh_option ("QV-good-facets-point", &i, NULL); + qh GOODvertex= i + 1; + } + s= t; + break; + default: + s--; + fprintf (qh ferr, "qhull warning: unknown 'Q' qhull option %c, rest ignored\n", (int)s[0]); + while (*++s && !isspace(*s)); + break; + } + } + break; + case 'T': + while (*s && !isspace(*s)) { + if (isdigit(*s) || *s == '-') + qh IStracing= qh_strtol(s, &s); + else switch(*s++) { + case 'c': + qh_option ("Tcheck-frequently", NULL, NULL); + qh CHECKfrequently= True; + break; + case 's': + qh_option ("Tstatistics", NULL, NULL); + qh PRINTstatistics= True; + break; + case 'v': + qh_option ("Tverify", NULL, NULL); + qh VERIFYoutput= True; + break; + case 'z': + if (!qh fout) + fprintf (qh ferr, "qhull warning: output file undefined (stdout). Option 'Tz' ignored.\n"); + else { + qh_option ("Tz-stdout", NULL, NULL); + qh ferr= qh fout; + qhmem.ferr= qh fout; + } + break; + case 'C': + if (!isdigit(*s)) + fprintf (qh ferr, "qhull warning: missing point id for cone for trace option 'TCn'. Ignored\n"); + else { + i= qh_strtol (s, &s); + qh_option ("TCone-stop", &i, NULL); + qh STOPcone= i + 1; + } + break; + case 'F': + if (!isdigit(*s)) + fprintf (qh ferr, "qhull warning: missing frequency count for trace option 'TFn'. Ignored\n"); + else { + qh REPORTfreq= qh_strtol (s, &s); + qh_option ("TFacet-log", &qh REPORTfreq, NULL); + qh REPORTfreq2= qh REPORTfreq/2; /* for tracemerging() */ + } + break; + case 'I': + if (s[0] != ' ' || s[1] == '\"' || s[1] == '\'' ||isspace (s[1])) { + s++; + fprintf (qh ferr, "qhull warning: option 'TI' mistyped.\nUse 'TI', one space, file name, and space or end-of-line.\nDo not use quotes. Option 'FI' ignored.\n"); + }else { /* not a procedure because of qh_option (filename, NULL, NULL); */ + char filename[500], *t= filename; + + s++; + while (*s) { + if (t - filename >= sizeof (filename)-2) { + fprintf (qh ferr, "qhull error: filename for 'TI' too long.\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (isspace (*s)) + break; + *(t++)= *s++; + } + *t= '\0'; + if (!freopen (filename, "r", stdin)) { + fprintf (qh ferr, "qhull error: could not open file \"%s\".", filename); + qh_errexit (qh_ERRinput, NULL, NULL); + }else { + qh_option ("TInput-file", NULL, NULL); + qh_option (filename, NULL, NULL); + } + } + break; + case 'O': + if (s[0] != ' ' || s[1] == '\"' || isspace (s[1])) { + s++; + fprintf (qh ferr, "qhull warning: option 'TO' mistyped.\nUse 'TO', one space, file name, and space or end-of-line.\nThe file name may be enclosed in single quotes.\nDo not use double quotes. Option 'FO' ignored.\n"); + }else { /* not a procedure because of qh_option (filename, NULL, NULL); */ + char filename[500], *t= filename; + boolT isquote= False; + + s++; + if (*s == '\'') { + isquote= True; + s++; + } + while (*s) { + if (t - filename >= sizeof (filename)-2) { + fprintf (qh ferr, "qhull error: filename for 'TO' too long.\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (isquote) { + if (*s == '\'') { + s++; + isquote= False; + break; + } + }else if (isspace (*s)) + break; + *(t++)= *s++; + } + *t= '\0'; + if (isquote) + fprintf (qh ferr, "qhull error: missing end quote for option 'TO'. Rest of line ignored.\n"); + else if (!freopen (filename, "w", stdout)) { + fprintf (qh ferr, "qhull error: could not open file \"%s\".", filename); + qh_errexit (qh_ERRinput, NULL, NULL); + }else { + qh_option ("TOutput-file", NULL, NULL); + qh_option (filename, NULL, NULL); + } + } + break; + case 'P': + if (!isdigit(*s)) + fprintf (qh ferr, "qhull warning: missing point id for trace option 'TPn'. Ignored\n"); + else { + qh TRACEpoint= qh_strtol (s, &s); + qh_option ("Trace-point", &qh TRACEpoint, NULL); + } + break; + case 'M': + if (!isdigit(*s)) + fprintf (qh ferr, "qhull warning: missing merge id for trace option 'TMn'. Ignored\n"); + else { + qh TRACEmerge= qh_strtol (s, &s); + qh_option ("Trace-merge", &qh TRACEmerge, NULL); + } + break; + case 'R': + if (!isdigit(*s)) + fprintf (qh ferr, "qhull warning: missing rerun count for trace option 'TRn'. Ignored\n"); + else { + qh RERUN= qh_strtol (s, &s); + qh_option ("TRerun", &qh RERUN, NULL); + } + break; + case 'V': + i= qh_strtol (s, &t); + if (s == t) + fprintf (qh ferr, "qhull warning: missing furthest point id for trace option 'TVn'. Ignored\n"); + else if (i < 0) { + qh STOPpoint= i - 1; + qh_option ("TV-stop-before-point", &i, NULL); + }else { + qh STOPpoint= i + 1; + qh_option ("TV-stop-after-point", &i, NULL); + } + s= t; + break; + case 'W': + if (!isdigit(*s)) + fprintf (qh ferr, "qhull warning: missing max width for trace option 'TWn'. Ignored\n"); + else { + qh TRACEdist= (realT) qh_strtod (s, &s); + qh_option ("TWide-trace", NULL, &qh TRACEdist); + } + break; + default: + s--; + fprintf (qh ferr, "qhull warning: unknown 'T' trace option %c, rest ignored\n", (int)s[0]); + while (*++s && !isspace(*s)); + break; + } + } + break; + default: + fprintf (qh ferr, "qhull warning: unknown flag %c (%x)\n", (int)s[-1], + (int)s[-1]); + break; + } + if (s-1 == prev_s && *s && !isspace(*s)) { + fprintf (qh ferr, "qhull warning: missing space after flag %c (%x); reserved for menu. Skipped.\n", + (int)*prev_s, (int)*prev_s); + while (*s && !isspace(*s)) + s++; + } + } + if (isgeom && !qh FORCEoutput && qh PRINTout[1]) + fprintf (qh ferr, "qhull warning: additional output formats are not compatible with Geomview\n"); + /* set derived values in qh_initqhull_globals */ +} /* initflags */ + + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="initqhull_buffers">-</a> + + qh_initqhull_buffers() + initialize global memory buffers + + notes: + must match qh_freebuffers() +*/ +void qh_initqhull_buffers (void) { + int k; + + qh TEMPsize= (qhmem.LASTsize - sizeof (setT))/SETelemsize; + if (qh TEMPsize <= 0 || qh TEMPsize > qhmem.LASTsize) + qh TEMPsize= 8; /* e.g., if qh_NOmem */ + qh other_points= qh_setnew (qh TEMPsize); + qh del_vertices= qh_setnew (qh TEMPsize); + qh coplanarset= qh_setnew (qh TEMPsize); + qh NEARzero= (realT *)qh_memalloc(qh hull_dim * sizeof(realT)); + qh lower_threshold= (realT *)qh_memalloc((qh input_dim+1) * sizeof(realT)); + qh upper_threshold= (realT *)qh_memalloc((qh input_dim+1) * sizeof(realT)); + qh lower_bound= (realT *)qh_memalloc((qh input_dim+1) * sizeof(realT)); + qh upper_bound= (realT *)qh_memalloc((qh input_dim+1) * sizeof(realT)); + for(k= qh input_dim+1; k--; ) { + qh lower_threshold[k]= -REALmax; + qh upper_threshold[k]= REALmax; + qh lower_bound[k]= -REALmax; + qh upper_bound[k]= REALmax; + } + qh gm_matrix= (coordT *)qh_memalloc((qh hull_dim+1) * qh hull_dim * sizeof(coordT)); + qh gm_row= (coordT **)qh_memalloc((qh hull_dim+1) * sizeof(coordT *)); +} /* initqhull_buffers */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="initqhull_globals">-</a> + + qh_initqhull_globals( points, numpoints, dim, ismalloc ) + initialize globals + if ismalloc + points were malloc'd and qhull should free at end + + returns: + sets qh.first_point, num_points, input_dim, hull_dim and others + seeds random number generator (seed=1 if tracing) + modifies qh.hull_dim if ((qh.DELAUNAY and qh.PROJECTdelaunay) or qh.PROJECTinput) + adjust user flags as needed + also checks DIM3 dependencies and constants + + notes: + do not use qh_point() since an input transformation may move them elsewhere + + see: + qh_initqhull_start() sets default values for non-zero globals + + design: + initialize points array from input arguments + test for qh.ZEROcentrum + (i.e., use opposite vertex instead of cetrum for convexity testing) + test for qh.PRINTgood (i.e., only print 'good' facets) + initialize qh.CENTERtype, qh.normal_size, + qh.center_size, qh.TRACEpoint/level, + initialize and test random numbers + check for conflicting print output options +*/ +void qh_initqhull_globals (coordT *points, int numpoints, int dim, boolT ismalloc) { + int seed, pointsneeded, extra= 0, i, randi, k; + boolT printgeom= False, printmath= False, printcoplanar= False; + realT randr; + realT factorial; + + time_t timedata; + + trace0((qh ferr, "qh_initqhull_globals: for %s | %s\n", qh rbox_command, + qh qhull_command)); + qh POINTSmalloc= ismalloc; + qh first_point= points; + qh num_points= numpoints; + qh hull_dim= qh input_dim= dim; + if (!qh NOpremerge && !qh MERGEexact && !qh PREmerge && qh JOGGLEmax > REALmax/2) { + qh MERGING= True; + if (qh hull_dim <= 4) { + qh PREmerge= True; + qh_option ("_pre-merge", NULL, NULL); + }else { + qh MERGEexact= True; + qh_option ("Qxact_merge", NULL, NULL); + } + }else if (qh MERGEexact) + qh MERGING= True; + if (!qh NOpremerge && qh JOGGLEmax > REALmax/2) { +#ifdef qh_NOmerge + qh JOGGLEmax= 0.0; +#endif + } + if (qh TRIangulate && qh JOGGLEmax < REALmax/2 && qh PRINTprecision) + fprintf(qh ferr, "qhull warning: joggle ('QJ') always produces simplicial output. Triangulated output ('Qt') does nothing.\n"); + if (qh JOGGLEmax < REALmax/2 && qh DELAUNAY && !qh SCALEinput && !qh SCALElast) { + qh SCALElast= True; + qh_option ("Qbbound-last-qj", NULL, NULL); + } + if (qh MERGING && !qh POSTmerge && qh premerge_cos > REALmax/2 + && qh premerge_centrum == 0) { + qh ZEROcentrum= True; + qh ZEROall_ok= True; + qh_option ("_zero-centrum", NULL, NULL); + } + if (qh JOGGLEmax < REALmax/2 && REALepsilon > 2e-8 && qh PRINTprecision) + fprintf(qh ferr, "qhull warning: real epsilon, %2.2g, is probably too large for joggle ('QJn')\nRecompile with double precision reals (see user.h).\n", + REALepsilon); +#ifdef qh_NOmerge + if (qh MERGING) { + fprintf (qh ferr, "qhull input error: merging not installed (qh_NOmerge + 'Qx', 'Cn' or 'An')\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } +#endif + if (!(qh PRINTgood || qh PRINTneighbors)) { + if (qh KEEParea || qh KEEPminArea < REALmax/2 || qh KEEPmerge || qh DELAUNAY + || (!qh ONLYgood && (qh GOODvertex || qh GOODpoint))) { + qh PRINTgood= True; + qh_option ("Pgood", NULL, NULL); + } + } + if (qh DELAUNAY && qh KEEPcoplanar && !qh KEEPinside) { + qh KEEPinside= True; + qh_option ("Qinterior-keep", NULL, NULL); + } + if (qh DELAUNAY && qh HALFspace) { + fprintf (qh ferr, "qhull input error: can not use Delaunay ('d') or Voronoi ('v') with halfspace intersection ('H')\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (!qh DELAUNAY && (qh UPPERdelaunay || qh ATinfinity)) { + fprintf (qh ferr, "qhull input error: use upper-Delaunay ('Qu') or infinity-point ('Qz') with Delaunay ('d') or Voronoi ('v')\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (qh UPPERdelaunay && qh ATinfinity) { + fprintf (qh ferr, "qhull input error: can not use infinity-point ('Qz') with upper-Delaunay ('Qu')\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (qh SCALElast && !qh DELAUNAY && qh PRINTprecision) + fprintf (qh ferr, "qhull input warning: option 'Qbb' (scale-last-coordinate) is normally used with 'd' or 'v'\n"); + qh DOcheckmax= (!qh SKIPcheckmax && qh MERGING ); + qh KEEPnearinside= (qh DOcheckmax && !(qh KEEPinside && qh KEEPcoplanar) + && !qh NOnearinside); + if (qh MERGING) + qh CENTERtype= qh_AScentrum; + else if (qh VORONOI) + qh CENTERtype= qh_ASvoronoi; + if (qh TESTvneighbors && !qh MERGING) { + fprintf(qh ferr, "qhull input error: test vertex neighbors ('Qv') needs a merge option\n"); + qh_errexit (qh_ERRinput, NULL ,NULL); + } + if (qh PROJECTinput || (qh DELAUNAY && qh PROJECTdelaunay)) { + qh hull_dim -= qh PROJECTinput; + if (qh DELAUNAY) { + qh hull_dim++; + extra= 1; + } + } + if (qh hull_dim <= 1) { + fprintf(qh ferr, "qhull error: dimension %d must be > 1\n", qh hull_dim); + qh_errexit (qh_ERRinput, NULL, NULL); + } + for (k= 2, factorial=1.0; k < qh hull_dim; k++) + factorial *= k; + qh AREAfactor= 1.0 / factorial; + trace2((qh ferr, "qh_initqhull_globals: initialize globals. dim %d numpoints %d malloc? %d projected %d to hull_dim %d\n", + dim, numpoints, ismalloc, qh PROJECTinput, qh hull_dim)); + qh normal_size= qh hull_dim * sizeof(coordT); + qh center_size= qh normal_size - sizeof(coordT); + pointsneeded= qh hull_dim+1; + if (qh hull_dim > qh_DIMmergeVertex) { + qh MERGEvertices= False; + qh_option ("Q3-no-merge-vertices-dim-high", NULL, NULL); + } + if (qh GOODpoint) + pointsneeded++; +#ifdef qh_NOtrace + if (qh IStracing) { + fprintf (qh ferr, "qhull input error: tracing is not installed (qh_NOtrace in user.h)"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } +#endif + if (qh RERUN > 1) { + qh TRACElastrun= qh IStracing; /* qh_build_withrestart duplicates next conditional */ + if (qh IStracing != -1) + qh IStracing= 0; + }else if (qh TRACEpoint != -1 || qh TRACEdist < REALmax/2 || qh TRACEmerge) { + qh TRACElevel= (qh IStracing? qh IStracing : 3); + qh IStracing= 0; + } + if (qh ROTATErandom == 0 || qh ROTATErandom == -1) { + seed= time (&timedata); + if (qh ROTATErandom == -1) { + seed= -seed; + qh_option ("QRandom-seed", &seed, NULL ); + }else + qh_option ("QRotate-random", &seed, NULL); + qh ROTATErandom= seed; + } + seed= qh ROTATErandom; + if (seed == INT_MIN) /* default value */ + seed= 1; + else if (seed < 0) + seed= -seed; + qh_RANDOMseed_(seed); + randr= 0.0; + for (i= 1000; i--; ) { + randi= qh_RANDOMint; + randr += randi; + if (randi > qh_RANDOMmax) { + fprintf (qh ferr, "\ +qhull configuration error (qh_RANDOMmax in user.h):\n\ + random integer %d > qh_RANDOMmax (%.8g)\n", + randi, qh_RANDOMmax); + qh_errexit (qh_ERRinput, NULL, NULL); + } + } + qh_RANDOMseed_(seed); + randr = randr/1000; + if (randr < qh_RANDOMmax/10 + || randr > qh_RANDOMmax * 5) + fprintf (qh ferr, "\ +qhull configuration warning (qh_RANDOMmax in user.h):\n\ + average of 1000 random integers (%.2g) is much different than expected (%.2g).\n\ + Is qh_RANDOMmax (%.2g) wrong?\n", + randr, qh_RANDOMmax/2.0, qh_RANDOMmax); + qh RANDOMa= 2.0 * qh RANDOMfactor/qh_RANDOMmax; + qh RANDOMb= 1.0 - qh RANDOMfactor; + if (qh_HASHfactor < 1.1) { + fprintf(qh ferr, "qhull internal error (qh_initqhull_globals): qh_HASHfactor %d must be at least 1.1. Qhull uses linear hash probing\n", + qh_HASHfactor); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + if (numpoints+extra < pointsneeded) { + fprintf(qh ferr,"qhull input error: not enough points (%d) to construct initial simplex (need %d)\n", + numpoints, pointsneeded); + qh_errexit(qh_ERRinput, NULL, NULL); + } + if (qh PRINTtransparent) { + if (qh hull_dim != 4 || !qh DELAUNAY || qh VORONOI || qh DROPdim >= 0) { + fprintf(qh ferr,"qhull input error: transparent Delaunay ('Gt') needs 3-d Delaunay ('d') w/o 'GDn'\n"); + qh_errexit(qh_ERRinput, NULL, NULL); + } + qh DROPdim = 3; + qh PRINTridges = True; + } + for (i= qh_PRINTEND; i--; ) { + if (qh PRINTout[i] == qh_PRINTgeom) + printgeom= True; + else if (qh PRINTout[i] == qh_PRINTmathematica || qh PRINTout[i] == qh_PRINTmaple) + printmath= True; + else if (qh PRINTout[i] == qh_PRINTcoplanars) + printcoplanar= True; + else if (qh PRINTout[i] == qh_PRINTpointnearest) + printcoplanar= True; + else if (qh PRINTout[i] == qh_PRINTpointintersect && !qh HALFspace) { + fprintf (qh ferr, "qhull input error: option 'Fp' is only used for \nhalfspace intersection ('Hn,n,n').\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + }else if (qh PRINTout[i] == qh_PRINTtriangles && (qh HALFspace || qh VORONOI)) { + fprintf (qh ferr, "qhull input error: option 'Ft' is not available for Voronoi vertices or halfspace intersection\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + }else if (qh PRINTout[i] == qh_PRINTcentrums && qh VORONOI) { + fprintf (qh ferr, "qhull input error: option 'FC' is not available for Voronoi vertices ('v')\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + }else if (qh PRINTout[i] == qh_PRINTvertices) { + if (qh VORONOI) + qh_option ("Fvoronoi", NULL, NULL); + else + qh_option ("Fvertices", NULL, NULL); + } + } + if (printcoplanar && qh DELAUNAY && qh JOGGLEmax < REALmax/2) { + if (qh PRINTprecision) + fprintf (qh ferr, "qhull input warning: 'QJ' (joggle) will usually prevent coincident input sites for options 'Fc' and 'FP'\n"); + } + if (!qh KEEPcoplanar && !qh KEEPinside && !qh ONLYgood) { + if ((qh PRINTcoplanar && qh PRINTspheres) || printcoplanar) { + qh KEEPcoplanar = True; + qh_option ("Qcoplanar", NULL, NULL); + } + } + if (printmath && (qh hull_dim > 3 || qh VORONOI)) { + fprintf (qh ferr, "qhull input error: Mathematica and Maple output is only available for 2-d and 3-d convex hulls and 2-d Delaunay triangulations\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (printgeom) { + if (qh hull_dim > 4) { + fprintf (qh ferr, "qhull input error: Geomview output is only available for 2-d, 3-d and 4-d\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (qh PRINTnoplanes && !(qh PRINTcoplanar + qh PRINTcentrums + + qh PRINTdots + qh PRINTspheres + qh DOintersections + qh PRINTridges)) { + fprintf (qh ferr, "qhull input error: no output specified for Geomview\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (qh VORONOI && (qh hull_dim > 3 || qh DROPdim >= 0)) { + fprintf (qh ferr, "qhull input error: Geomview output for Voronoi diagrams only for 2-d\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + /* can not warn about furthest-site Geomview output: no lower_threshold */ + if (qh hull_dim == 4 && qh DROPdim == -1 && + (qh PRINTcoplanar || qh PRINTspheres || qh PRINTcentrums)) { + fprintf (qh ferr, "qhull input warning: coplanars, vertices, and centrums output not\n\ +available for 4-d output (ignored). Could use 'GDn' instead.\n"); + qh PRINTcoplanar= qh PRINTspheres= qh PRINTcentrums= False; + } + } + qh PRINTdim= qh hull_dim; + if (qh DROPdim >=0) { /* after Geomview checks */ + if (qh DROPdim < qh hull_dim) { + qh PRINTdim--; + if (!printgeom || qh hull_dim < 3) + fprintf (qh ferr, "qhull input warning: drop dimension 'GD%d' is only available for 3-d/4-d Geomview\n", qh DROPdim); + }else + qh DROPdim= -1; + }else if (qh VORONOI) { + qh DROPdim= qh hull_dim-1; + qh PRINTdim= qh hull_dim-1; + } +} /* initqhull_globals */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="initqhull_mem">-</a> + + qh_initqhull_mem( ) + initialize mem.c for qhull + qh.hull_dim and qh.normal_size determine some of the allocation sizes + if qh.MERGING, + includes ridgeT + calls qh_user_memsizes() to add up to 10 additional sizes for quick allocation + (see numsizes below) + + returns: + mem.c already for qh_memalloc/qh_memfree (errors if called beforehand) + + notes: + qh_produceoutput() prints memsizes + +*/ +void qh_initqhull_mem (void) { + int numsizes; + int i; + + numsizes= 8+10; + qh_meminitbuffers (qh IStracing, qh_MEMalign, numsizes, + qh_MEMbufsize,qh_MEMinitbuf); + qh_memsize(sizeof(vertexT)); + if (qh MERGING) { + qh_memsize(sizeof(ridgeT)); + qh_memsize(sizeof(mergeT)); + } + qh_memsize(sizeof(facetT)); + i= sizeof(setT) + (qh hull_dim - 1) * SETelemsize; /* ridge.vertices */ + qh_memsize(i); + qh_memsize(qh normal_size); /* normal */ + i += SETelemsize; /* facet.vertices, .ridges, .neighbors */ + qh_memsize(i); + qh_user_memsizes(); + qh_memsetup(); +} /* initqhull_mem */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="initqhull_start">-</a> + + qh_initqhull_start( infile, outfile, errfile ) + start initialization of qhull + initialize statistics, stdio, default values for global variables + + see: + qh_maxmin() determines the precision constants +*/ +void qh_initqhull_start (FILE *infile, FILE *outfile, FILE *errfile) { + + qh_CPUclock; /* start the clock */ +#if qh_QHpointer + if (!(qh_qh= (qhT *)malloc (sizeof(qhT)))) { + fprintf (errfile, "qhull error (qh_initqhull_globals): insufficient memory\n"); + exit (qh_ERRmem); /* no error handler */ + } + memset((char *)qh_qh, 0, sizeof(qhT)); /* every field is 0, FALSE, NULL */ +#else + memset((char *)&qh_qh, 0, sizeof(qhT)); +#endif + strcat (qh qhull, "qhull"); + qh_initstatistics(); + qh ANGLEmerge= True; + qh DROPdim= -1; + qh ferr= errfile; + qh fin= infile; + qh fout= outfile; + qh furthest_id= -1; + qh JOGGLEmax= REALmax; + qh KEEPminArea = REALmax; + qh last_low= REALmax; + qh last_high= REALmax; + qh last_newhigh= REALmax; + qh max_outside= 0.0; + qh max_vertex= 0.0; + qh MAXabs_coord= 0.0; + qh MAXsumcoord= 0.0; + qh MAXwidth= -REALmax; + qh MERGEindependent= True; + qh MINdenom_1= fmax_(1.0/REALmax, REALmin); /* used by qh_scalepoints */ + qh MINoutside= 0.0; + qh MINvisible= REALmax; + qh MAXcoplanar= REALmax; + qh outside_err= REALmax; + qh premerge_centrum= 0.0; + qh premerge_cos= REALmax; + qh PRINTprecision= True; + qh PRINTradius= 0.0; + qh postmerge_cos= REALmax; + qh postmerge_centrum= 0.0; + qh ROTATErandom= INT_MIN; + qh MERGEvertices= True; + qh totarea= 0.0; + qh totvol= 0.0; + qh TRACEdist= REALmax; + qh TRACEpoint= -1; /* recompile or use 'TPn' */ + qh tracefacet_id= UINT_MAX; /* recompile to trace a facet */ + qh tracevertex_id= UINT_MAX; /* recompile to trace a vertex */ + qh_RANDOMseed_(1); +} /* initqhull_start */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="initthresholds">-</a> + + qh_initthresholds( commandString ) + set thresholds for printing and scaling from commandString + + returns: + sets qh.GOODthreshold or qh.SPLITthreshold if 'Pd0D1' used + + see: + qh_initflags(), 'Qbk' 'QBk' 'Pdk' and 'PDk' + qh_inthresholds() + + design: + for each 'Pdn' or 'PDn' option + check syntax + set qh.lower_threshold or qh.upper_threshold + set qh.GOODthreshold if an unbounded threshold is used + set qh.SPLITthreshold if a bounded threshold is used +*/ +void qh_initthresholds(char *command) { + realT value; + int index, maxdim, k; + char *s= command; + char key; + + maxdim= qh input_dim; + if (qh DELAUNAY && (qh PROJECTdelaunay || qh PROJECTinput)) + maxdim++; + while (*s) { + if (*s == '-') + s++; + if (*s == 'P') { + s++; + while (*s && !isspace(key= *s++)) { + if (key == 'd' || key == 'D') { + if (!isdigit(*s)) { + fprintf(qh ferr, "qhull warning: no dimension given for Print option '%c' at: %s. Ignored\n", + key, s-1); + continue; + } + index= qh_strtol (s, &s); + if (index >= qh hull_dim) { + fprintf(qh ferr, "qhull warning: dimension %d for Print option '%c' is >= %d. Ignored\n", + index, key, qh hull_dim); + continue; + } + if (*s == ':') { + s++; + value= qh_strtod(s, &s); + if (fabs((double)value) > 1.0) { + fprintf(qh ferr, "qhull warning: value %2.4g for Print option %c is > +1 or < -1. Ignored\n", + value, key); + continue; + } + }else + value= 0.0; + if (key == 'd') + qh lower_threshold[index]= value; + else + qh upper_threshold[index]= value; + } + } + }else if (*s == 'Q') { + s++; + while (*s && !isspace(key= *s++)) { + if (key == 'b' && *s == 'B') { + s++; + for (k=maxdim; k--; ) { + qh lower_bound[k]= -qh_DEFAULTbox; + qh upper_bound[k]= qh_DEFAULTbox; + } + }else if (key == 'b' && *s == 'b') + s++; + else if (key == 'b' || key == 'B') { + if (!isdigit(*s)) { + fprintf(qh ferr, "qhull warning: no dimension given for Qhull option %c. Ignored\n", + key); + continue; + } + index= qh_strtol (s, &s); + if (index >= maxdim) { + fprintf(qh ferr, "qhull warning: dimension %d for Qhull option %c is >= %d. Ignored\n", + index, key, maxdim); + continue; + } + if (*s == ':') { + s++; + value= qh_strtod(s, &s); + }else if (key == 'b') + value= -qh_DEFAULTbox; + else + value= qh_DEFAULTbox; + if (key == 'b') + qh lower_bound[index]= value; + else + qh upper_bound[index]= value; + } + } + }else { + while (*s && !isspace (*s)) + s++; + } + while (isspace (*s)) + s++; + } + for (k= qh hull_dim; k--; ) { + if (qh lower_threshold[k] > -REALmax/2) { + qh GOODthreshold= True; + if (qh upper_threshold[k] < REALmax/2) { + qh SPLITthresholds= True; + qh GOODthreshold= False; + break; + } + }else if (qh upper_threshold[k] < REALmax/2) + qh GOODthreshold= True; + } +} /* initthresholds */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="option">-</a> + + qh_option( option, intVal, realVal ) + add an option description to qh.qhull_options + + notes: + will be printed with statistics ('Ts') and errors + strlen(option) < 40 +*/ +void qh_option (char *option, int *i, realT *r) { + char buf[200]; + int len, maxlen; + + sprintf (buf, " %s", option); + if (i) + sprintf (buf+strlen(buf), " %d", *i); + if (r) + sprintf (buf+strlen(buf), " %2.2g", *r); + len= strlen(buf); + qh qhull_optionlen += len; + maxlen= sizeof (qh qhull_options) - len -1; + maximize_(maxlen, 0); + if (qh qhull_optionlen >= 80 && maxlen > 0) { + qh qhull_optionlen= len; + strncat (qh qhull_options, "\n", maxlen--); + } + strncat (qh qhull_options, buf, maxlen); +} /* option */ + +#if qh_QHpointer +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="restore_qhull">-</a> + + qh_restore_qhull( oldqh ) + restores a previously saved qhull + also restores qh_qhstat and qhmem.tempstack + + notes: + errors if current qhull hasn't been saved or freed + uses qhmem for error reporting + + NOTE 1998/5/11: + Freeing memory after qh_save_qhull and qh_restore_qhull + is complicated. The procedures will be redesigned. + + see: + qh_save_qhull() +*/ +void qh_restore_qhull (qhT **oldqh) { + + if (*oldqh && strcmp ((*oldqh)->qhull, "qhull")) { + fprintf (qhmem.ferr, "qhull internal error (qh_restore_qhull): %p is not a qhull data structure\n", + *oldqh); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + if (qh_qh) { + fprintf (qhmem.ferr, "qhull internal error (qh_restore_qhull): did not save or free existing qhull\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + if (!*oldqh || !(*oldqh)->old_qhstat) { + fprintf (qhmem.ferr, "qhull internal error (qh_restore_qhull): did not previously save qhull %p\n", + *oldqh); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + qh_qh= *oldqh; + *oldqh= NULL; + qh_qhstat= qh old_qhstat; + qhmem.tempstack= qh old_tempstack; + trace1((qh ferr, "qh_restore_qhull: restored qhull from %p\n", *oldqh)); +} /* restore_qhull */ + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="save_qhull">-</a> + + qh_save_qhull( ) + saves qhull for a later qh_restore_qhull + also saves qh_qhstat and qhmem.tempstack + + returns: + qh_qh=NULL + + notes: + need to initialize qhull or call qh_restore_qhull before continuing + + NOTE 1998/5/11: + Freeing memory after qh_save_qhull and qh_restore_qhull + is complicated. The procedures will be redesigned. + + see: + qh_restore_qhull() +*/ +qhT *qh_save_qhull (void) { + qhT *oldqh; + + trace1((qhmem.ferr, "qh_save_qhull: save qhull %p\n", qh_qh)); + if (!qh_qh) { + fprintf (qhmem.ferr, "qhull internal error (qh_save_qhull): qhull not initialized\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + qh old_qhstat= qh_qhstat; + qh_qhstat= NULL; + qh old_tempstack= qhmem.tempstack; + qhmem.tempstack= NULL; + oldqh= qh_qh; + qh_qh= NULL; + return oldqh; +} /* save_qhull */ + +#endif + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="strtol">-</a> + + qh_strtol( s, endp) qh_strtod( s, endp) + internal versions of strtol() and strtod() + does not skip trailing spaces + notes: + some implementations of strtol()/strtod() skip trailing spaces +*/ +double qh_strtod (const char *s, char **endp) { + double result; + + result= strtod (s, endp); + if (s < (*endp) && (*endp)[-1] == ' ') + (*endp)--; + return result; +} /* strtod */ + +int qh_strtol (const char *s, char **endp) { + int result; + + result= (int) strtol (s, endp, 10); + if (s< (*endp) && (*endp)[-1] == ' ') + (*endp)--; + return result; +} /* strtol */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/index.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/index.htm new file mode 100644 index 0000000000000000000000000000000000000000..cdd0c63f60b044ae0041fbf0067ee92ffdfacdb3 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/index.htm @@ -0,0 +1,239 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>Qhull functions, macros, and data structures</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals</a><br> +<b>To:</b> <a href="#TOC">Qhull files</a><br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> + +<hr> +<!-- Main text of document. --> + +<h1>Qhull functions, macros, and data structures</h1> +<blockquote> +<p>The following sections provide an overview and index to +Qhull's functions, macros, and data structures. Each +section starts with an introduction. Also see <a href=../html/qh-in.htm#library>Calling +Qhull from your program</a>.</p> + +<p>Qhull uses the following conventions:</p> +<blockquote> + +<ul> +<li>in code, global variables start with "qh " +<li>in documentation, global variables start with 'qh.' +<li>constants start with an upper case word +<li>important globals include an '_' +<li>functions, macros, and constants start with "qh_"</li> +<li>data types end in "T"</li> +<li>macros with arguments end in "_"</li> +<li>iterators are macros that use local variables</li> +<li>iterators for sets start with "FOREACH"</li> +<li>iterators for lists start with "FORALL"</li> +<li>qhull options are in single quotes (e.g., 'Pdn')</li> +<li>lists are sorted alphabetically</li> +<li>preprocessor directives on left margin for older compilers</li> +</ul> +</blockquote> +<p> +When reading the code, please note that the +global data structure, 'qh', is a macro. It +either expands to "qh_qh." or to +"qh_qh->". The later is used for +applications which run concurrent calls to qh_qhull(). +<p> +When reading code with an editor, a search for +<i>"procedure</i> +will locate the header of <i>qh_procedure</i>. A search for <i>* procedure</i> +will locate the tail of <i>qh_procedure</i>. + +<p>A useful starting point is <a href="qhull.h">qhull.h</a>. It defines most +of Qhull data structures and top-level functions. Search for <i>'PFn'</i> to +determine the corresponding constant in Qhull. Search for <i>'Fp'</i> to +determine the corresponding <a href="qhull.h#qh_PRINT">qh_PRINT...</a> constant. +Search <a href="io.c">io.c</a> to learn how the print function is implemented.</p> + +<p>If your web browser loads .c and .h files with an external application, +change the MIME type of .c and .h files to "text/html". +Opera does not always work since it treats '<' characters as HTML tags. +<p> +Please report documentation and link errors +to <a href="mailto:qhull-bug@qhull.org">qhull-bug@qhull.org</a>. +<p> +<i>Brad Barber, Cambridge MA, June 1, 2002</i> +</p> +</blockquote> + +<p><b>Copyright © 1997-2003 The Geometry Center, Minneapolis MN</b></p> + +<hr> + +<h2><a href="#TOP">»</a><a name="TOC">Qhull files</a> </h2> +<blockquote> + +<p>This sections lists the .c and .h files for Qhull. Please +refer to these files for detailed information.</p> +<blockquote> + +<dl> +<dt><a href="qhull.h"><b>qhull.h</b></a> </dt> +<dd>Include file for the Qhull library, <tt>qhull.a</tt>. +Data structures are documented under <a href="qh-poly.htm">Poly</a>. +Global variables are documented under <a href="qh-globa.htm">Global</a>. +Other data structures and variables are documented under +<a href="qh-qhull.htm#TOC">Qhull</a> or <a href="qh-geom.htm"><b>Geom</b></a><b>.</b></dd> + +<dt> </dt> +<dt><a href="qh-geom.htm"><b>Geom</b></a><b>, </b> +<a href="geom.h"><b>geom.h</b></a><b>, </b> +<a href="geom.c"><b>geom.c</b></a><b>, </b> +<a href="geom2.c"><b>geom2.c</b></a> </dt> +<dd>Geometric routines. These routines implement mathematical +functions such as Gaussian elimination and geometric +routines needed for Qhull. Frequently used routines are +in <tt>geom.c</tt> while infrequent ones are in <tt>geom2.c</tt>. +</dd> + +<dt> </dt> +<dt><a href="qh-globa.htm"><b>Global</b></a><b>, </b> +<a href="global.c"><b>global.c</b></a><b>, </b> +<a href="qhull.h"><b>qhull.h</b></a> </dt> +<dd>Global routines. Qhull uses a global data structure, <tt>qh</tt>, +to store globally defined constants, lists, sets, and +variables. +<tt>global.c</tt> initializes and frees these +structures. </dd> + +<dt> </dt> +<dt><a href="qh-io.htm"><b>Io</b></a><b>, </b><a href="io.h"><b>io.h</b></a><b>, +</b><a href="io.c"><b>io.c</b></a> </dt> +<dd>Input and output routines. Qhull provides a wide range of +input and output options.</dd> + +<dt> </dt> +<dt><a href="qh-mem.htm"><b>Mem</b></a><b>, </b> +<a href="mem.h"><b>mem.h</b></a><b>, </b> +<a href="mem.c"><b>mem.c</b></a> </dt> +<dd>Memory routines. Qhull provides memory allocation and +deallocation. It uses quick-fit allocation.</dd> + +<dt> </dt> +<dt><a href="qh-merge.htm"><b>Merge</b></a><b>, </b> +<a href="merge.h"><b>merge.h</b></a><b>, </b> +<a href="merge.c"><b>merge.c</b></a> </dt> +<dd>Merge routines. Qhull handles precision problems by +merged facets or joggled input. These routines merge simplicial facets, +merge non-simplicial facets, merge cycles of facets, and +rename redundant vertices.</dd> + +<dt> </dt> +<dt><a href="qh-poly.htm"><b>Poly</b></a><b>, </b> +<a href="poly.h"><b>poly.h</b></a><b>, </b> +<a href="poly.c"><b>poly.c</b></a><b>, </b> +<a href="poly2.c"><b>poly2.c</b></a><b>, </b> +<a href="qhull.h"><b>qhull.h</b></a> </dt> +<dd>Polyhedral routines. Qhull produces a polyhedron as a +list of facets with vertices, neighbors, ridges, and +geometric information. <tt>Qhull.h</tt> defines the main +data structures. Frequently used routines are in <tt>poly.c</tt> +while infrequent ones are in <tt>poly2.c</tt>.</dd> + +<dt> </dt> +<dt><a href="qh-qhull.htm#TOC"><b>Qhull</b></a><b>, </b> +<a href="qhull.c"><b>qhull.c</b></a><b>, </b> +<a href="qhull.h"><b>qhull.h</b></a><b>, </b> +<a href="qhull_a.h"><b>qhull_a.h</b></a><b>, </b> +<a href="unix.c"><b>unix.c</b></a> <b>, </b> +<a href="qconvex.c"><b>qconvex.c</b></a> <b>, </b> +<a href="qdelaun.c"><b>qdelaun.c</b></a> <b>, </b> +<a href="qhalf.c"><b>qhalf.c</b></a> <b>, </b> +<a href="qvoronoi.c"><b>qvoronoi.c</b></a> </dt> +<dd>Top-level routines. The Quickhull algorithm is +implemented by <tt>qhull.c</tt>. <tt>qhull_a.h</tt> +includes all header files. </dd> + +<dt> </dt> +<dt><a href="qh-set.htm"><b>Set</b></a><b>, </b> +<a href="qset.h"><b>qset.h</b></a><b>, </b> +<a href="qset.c"><b>qset.c</b></a> </dt> +<dd>Set routines. Qhull implements its data structures as +sets. A set is an array of pointers that is expanded as +needed. This is a separate package that may be used in +other applications. </dd> + +<dt> </dt> +<dt><a href="qh-stat.htm"><b>Stat</b></a><b>, </b> +<a href="stat.h"><b>stat.h</b></a><b>, </b> +<a href="stat.c"><b>stat.c</b></a> </dt> +<dd>Statistical routines. Qhull maintains statistics about +its implementation. </dd> + +<dt> </dt> +<dt><a href="qh-user.htm"><b>User</b></a><b>, </b> +<a href="user.h"><b>user.h</b></a><b>, </b> +<a href="user.c"><b>user.c</b></a><b>, </b> +<a href="user_eg.c"><b>user_eg.c</b></a><b>, </b> +<a href="qhull_interface.cpp#TOP"><b>qhull_interface.cpp</b></a></dt> +<dd>User-defined routines. Qhull allows the user to configure +the code with defined constants and specialized routines. +</dd> +</dl> +</blockquote> + +</blockquote> +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="#TOC">Qhull files</a><br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> + +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/io.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/io.c new file mode 100644 index 0000000000000000000000000000000000000000..2ef5b946a50789e68d2ebd7fdb79ecefc1b9144f --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/io.c @@ -0,0 +1,4108 @@ +/*<html><pre> -<a href="qh-io.htm" + >-------------------------------</a><a name="TOP">-</a> + + io.c + Input/Output routines of qhull application + + see qh-io.htm and io.h + + see user.c for qh_errprint and qh_printfacetlist + + unix.c calls qh_readpoints and qh_produce_output + + unix.c and user.c are the only callers of io.c functions + This allows the user to avoid loading io.o from qhull.a + + copyright (c) 1993-2003 The Geometry Center +*/ + +#include "qhull_a.h" + +/*========= -functions in alphabetical order after qh_produce_output() =====*/ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="produce_output">-</a> + + qh_produce_output() + prints out the result of qhull in desired format + if qh.GETarea + computes and prints area and volume + qh.PRINTout[] is an array of output formats + + notes: + prints output in qh.PRINTout order +*/ +void qh_produce_output(void) { + int i, tempsize= qh_setsize ((setT*)qhmem.tempstack), d_1; + + if (qh VORONOI) { + qh_clearcenters (qh_ASvoronoi); + qh_vertexneighbors(); + } + if (qh TRIangulate) { + qh_triangulate(); + if (qh VERIFYoutput && !qh CHECKfrequently) + qh_checkpolygon (qh facet_list); + } + qh_findgood_all (qh facet_list); + if (qh GETarea) + qh_getarea(qh facet_list); + if (qh KEEParea || qh KEEPmerge || qh KEEPminArea < REALmax/2) + qh_markkeep (qh facet_list); + if (qh PRINTsummary) + qh_printsummary(qh ferr); + else if (qh PRINTout[0] == qh_PRINTnone) + qh_printsummary(qh fout); + for (i= 0; i < qh_PRINTEND; i++) + qh_printfacets (qh fout, qh PRINTout[i], qh facet_list, NULL, !qh_ALL); + qh_allstatistics(); + if (qh PRINTprecision && !qh MERGING && (qh JOGGLEmax > REALmax/2 || qh RERUN)) + qh_printstats (qh ferr, qhstat precision, NULL); + if (qh VERIFYoutput && (zzval_(Zridge) > 0 || zzval_(Zridgemid) > 0)) + qh_printstats (qh ferr, qhstat vridges, NULL); + if (qh PRINTstatistics) { + qh_collectstatistics(); + qh_printstatistics(qh ferr, ""); + qh_memstatistics (qh ferr); + d_1= sizeof(setT) + (qh hull_dim - 1) * SETelemsize; + fprintf(qh ferr, "\ + size in bytes: merge %d ridge %d vertex %d facet %d\n\ + normal %d ridge vertices %d facet vertices or neighbors %d\n", + sizeof(mergeT), sizeof(ridgeT), + sizeof(vertexT), sizeof(facetT), + qh normal_size, d_1, d_1 + SETelemsize); + } + if (qh_setsize ((setT*)qhmem.tempstack) != tempsize) { + fprintf (qh ferr, "qhull internal error (qh_produce_output): temporary sets not empty (%d)\n", + qh_setsize ((setT*)qhmem.tempstack)); + qh_errexit (qh_ERRqhull, NULL, NULL); + } +} /* produce_output */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="dfacet">-</a> + + dfacet( id ) + print facet by id, for debugging + +*/ +void dfacet (unsigned id) { + facetT *facet; + + FORALLfacets { + if (facet->id == id) { + qh_printfacet (qh fout, facet); + break; + } + } +} /* dfacet */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="dvertex">-</a> + + dvertex( id ) + print vertex by id, for debugging +*/ +void dvertex (unsigned id) { + vertexT *vertex; + + FORALLvertices { + if (vertex->id == id) { + qh_printvertex (qh fout, vertex); + break; + } + } +} /* dvertex */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="compare_vertexpoint">-</a> + + qh_compare_vertexpoint( p1, p2 ) + used by qsort() to order vertices by point id +*/ +int qh_compare_vertexpoint(const void *p1, const void *p2) { + vertexT *a= *((vertexT **)p1), *b= *((vertexT **)p2); + + return ((qh_pointid(a->point) > qh_pointid(b->point)?1:-1)); +} /* compare_vertexpoint */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="compare_facetarea">-</a> + + qh_compare_facetarea( p1, p2 ) + used by qsort() to order facets by area +*/ +int qh_compare_facetarea(const void *p1, const void *p2) { + facetT *a= *((facetT **)p1), *b= *((facetT **)p2); + + if (!a->isarea) + return -1; + if (!b->isarea) + return 1; + if (a->f.area > b->f.area) + return 1; + else if (a->f.area == b->f.area) + return 0; + return -1; +} /* compare_facetarea */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="compare_facetmerge">-</a> + + qh_compare_facetmerge( p1, p2 ) + used by qsort() to order facets by number of merges +*/ +int qh_compare_facetmerge(const void *p1, const void *p2) { + facetT *a= *((facetT **)p1), *b= *((facetT **)p2); + + return (a->nummerge - b->nummerge); +} /* compare_facetvisit */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="compare_facetvisit">-</a> + + qh_compare_facetvisit( p1, p2 ) + used by qsort() to order facets by visit id or id +*/ +int qh_compare_facetvisit(const void *p1, const void *p2) { + facetT *a= *((facetT **)p1), *b= *((facetT **)p2); + int i,j; + + if (!(i= a->visitid)) + i= - a->id; /* do not convert to int */ + if (!(j= b->visitid)) + j= - b->id; + return (i - j); +} /* compare_facetvisit */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="countfacets">-</a> + + qh_countfacets( facetlist, facets, printall, + numfacets, numsimplicial, totneighbors, numridges, numcoplanar, numtricoplanars ) + count good facets for printing and set visitid + if allfacets, ignores qh_skipfacet() + + notes: + qh_printsummary and qh_countfacets must match counts + + returns: + numfacets, numsimplicial, total neighbors, numridges, coplanars + each facet with ->visitid indicating 1-relative position + ->visitid==0 indicates not good + + notes + numfacets >= numsimplicial + if qh.NEWfacets, + does not count visible facets (matches qh_printafacet) + + design: + for all facets on facetlist and in facets set + unless facet is skipped or visible (i.e., will be deleted) + mark facet->visitid + update counts +*/ +void qh_countfacets (facetT *facetlist, setT *facets, boolT printall, + int *numfacetsp, int *numsimplicialp, int *totneighborsp, int *numridgesp, int *numcoplanarsp, int *numtricoplanarsp) { + facetT *facet, **facetp; + int numfacets= 0, numsimplicial= 0, numridges= 0, totneighbors= 0, numcoplanars= 0, numtricoplanars= 0; + + FORALLfacet_(facetlist) { + if ((facet->visible && qh NEWfacets) + || (!printall && qh_skipfacet(facet))) + facet->visitid= 0; + else { + facet->visitid= ++numfacets; + totneighbors += qh_setsize (facet->neighbors); + if (facet->simplicial) { + numsimplicial++; + if (facet->keepcentrum && facet->tricoplanar) + numtricoplanars++; + }else + numridges += qh_setsize (facet->ridges); + if (facet->coplanarset) + numcoplanars += qh_setsize (facet->coplanarset); + } + } + FOREACHfacet_(facets) { + if ((facet->visible && qh NEWfacets) + || (!printall && qh_skipfacet(facet))) + facet->visitid= 0; + else { + facet->visitid= ++numfacets; + totneighbors += qh_setsize (facet->neighbors); + if (facet->simplicial){ + numsimplicial++; + if (facet->keepcentrum && facet->tricoplanar) + numtricoplanars++; + }else + numridges += qh_setsize (facet->ridges); + if (facet->coplanarset) + numcoplanars += qh_setsize (facet->coplanarset); + } + } + qh visit_id += numfacets+1; + *numfacetsp= numfacets; + *numsimplicialp= numsimplicial; + *totneighborsp= totneighbors; + *numridgesp= numridges; + *numcoplanarsp= numcoplanars; + *numtricoplanarsp= numtricoplanars; +} /* countfacets */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="detvnorm">-</a> + + qh_detvnorm( vertex, vertexA, centers, offset ) + compute separating plane of the Voronoi diagram for a pair of input sites + centers= set of facets (i.e., Voronoi vertices) + facet->visitid= 0 iff vertex-at-infinity (i.e., unbounded) + + assumes: + qh_ASvoronoi and qh_vertexneighbors() already set + + returns: + norm + a pointer into qh.gm_matrix to qh.hull_dim-1 reals + copy the data before reusing qh.gm_matrix + offset + if 'QVn' + sign adjusted so that qh.GOODvertexp is inside + else + sign adjusted so that vertex is inside + + qh.gm_matrix= simplex of points from centers relative to first center + + notes: + in io.c so that code for 'v Tv' can be removed by removing io.c + returns pointer into qh.gm_matrix to avoid tracking of temporary memory + + design: + determine midpoint of input sites + build points as the set of Voronoi vertices + select a simplex from points (if necessary) + include midpoint if the Voronoi region is unbounded + relocate the first vertex of the simplex to the origin + compute the normalized hyperplane through the simplex + orient the hyperplane toward 'QVn' or 'vertex' + if 'Tv' or 'Ts' + if bounded + test that hyperplane is the perpendicular bisector of the input sites + test that Voronoi vertices not in the simplex are still on the hyperplane + free up temporary memory +*/ +pointT *qh_detvnorm (vertexT *vertex, vertexT *vertexA, setT *centers, realT *offsetp) { + facetT *facet, **facetp; + int i, k, pointid, pointidA, point_i, point_n; + setT *simplex= NULL; + pointT *point, **pointp, *point0, *midpoint, *normal, *inpoint; + coordT *coord, *gmcoord, *normalp; + setT *points= qh_settemp (qh TEMPsize); + boolT nearzero= False; + boolT unbounded= False; + int numcenters= 0; + int dim= qh hull_dim - 1; + realT dist, offset, angle, zero= 0.0; + + midpoint= qh gm_matrix + qh hull_dim * qh hull_dim; /* last row */ + for (k= 0; k < dim; k++) + midpoint[k]= (vertex->point[k] + vertexA->point[k])/2; + FOREACHfacet_(centers) { + numcenters++; + if (!facet->visitid) + unbounded= True; + else { + if (!facet->center) + facet->center= qh_facetcenter (facet->vertices); + qh_setappend (&points, facet->center); + } + } + if (numcenters > dim) { + simplex= qh_settemp (qh TEMPsize); + qh_setappend (&simplex, vertex->point); + if (unbounded) + qh_setappend (&simplex, midpoint); + qh_maxsimplex (dim, points, NULL, 0, &simplex); + qh_setdelnth (simplex, 0); + }else if (numcenters == dim) { + if (unbounded) + qh_setappend (&points, midpoint); + simplex= points; + }else { + fprintf(qh ferr, "qh_detvnorm: too few points (%d) to compute separating plane\n", numcenters); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + i= 0; + gmcoord= qh gm_matrix; + point0= SETfirstt_(simplex, pointT); + FOREACHpoint_(simplex) { + if (qh IStracing >= 4) + qh_printmatrix(qh ferr, "qh_detvnorm: Voronoi vertex or midpoint", + &point, 1, dim); + if (point != point0) { + qh gm_row[i++]= gmcoord; + coord= point0; + for (k= dim; k--; ) + *(gmcoord++)= *point++ - *coord++; + } + } + qh gm_row[i]= gmcoord; /* does not overlap midpoint, may be used later for qh_areasimplex */ + normal= gmcoord; + qh_sethyperplane_gauss (dim, qh gm_row, point0, True, + normal, &offset, &nearzero); + if (qh GOODvertexp == vertexA->point) + inpoint= vertexA->point; + else + inpoint= vertex->point; + zinc_(Zdistio); + dist= qh_distnorm (dim, inpoint, normal, &offset); + if (dist > 0) { + offset= -offset; + normalp= normal; + for (k= dim; k--; ) { + *normalp= -(*normalp); + normalp++; + } + } + if (qh VERIFYoutput || qh PRINTstatistics) { + pointid= qh_pointid (vertex->point); + pointidA= qh_pointid (vertexA->point); + if (!unbounded) { + zinc_(Zdiststat); + dist= qh_distnorm (dim, midpoint, normal, &offset); + if (dist < 0) + dist= -dist; + zzinc_(Zridgemid); + wwmax_(Wridgemidmax, dist); + wwadd_(Wridgemid, dist); + trace4((qh ferr, "qh_detvnorm: points %d %d midpoint dist %2.2g\n", + pointid, pointidA, dist)); + for (k= 0; k < dim; k++) + midpoint[k]= vertexA->point[k] - vertex->point[k]; /* overwrites midpoint! */ + qh_normalize (midpoint, dim, False); + angle= qh_distnorm (dim, midpoint, normal, &zero); /* qh_detangle uses dim+1 */ + if (angle < 0.0) + angle= angle + 1.0; + else + angle= angle - 1.0; + if (angle < 0.0) + angle -= angle; + trace4((qh ferr, "qh_detvnorm: points %d %d angle %2.2g nearzero %d\n", + pointid, pointidA, angle, nearzero)); + if (nearzero) { + zzinc_(Zridge0); + wwmax_(Wridge0max, angle); + wwadd_(Wridge0, angle); + }else { + zzinc_(Zridgeok) + wwmax_(Wridgeokmax, angle); + wwadd_(Wridgeok, angle); + } + } + if (simplex != points) { + FOREACHpoint_i_(points) { + if (!qh_setin (simplex, point)) { + facet= SETelemt_(centers, point_i, facetT); + zinc_(Zdiststat); + dist= qh_distnorm (dim, point, normal, &offset); + if (dist < 0) + dist= -dist; + zzinc_(Zridge); + wwmax_(Wridgemax, dist); + wwadd_(Wridge, dist); + trace4((qh ferr, "qh_detvnorm: points %d %d Voronoi vertex %d dist %2.2g\n", + pointid, pointidA, facet->visitid, dist)); + } + } + } + } + *offsetp= offset; + if (simplex != points) + qh_settempfree (&simplex); + qh_settempfree (&points); + return normal; +} /* detvnorm */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="detvridge">-</a> + + qh_detvridge( vertexA ) + determine Voronoi ridge from 'seen' neighbors of vertexA + include one vertex-at-infinite if an !neighbor->visitid + + returns: + temporary set of centers (facets, i.e., Voronoi vertices) + sorted by center id +*/ +setT *qh_detvridge (vertexT *vertex) { + setT *centers= qh_settemp (qh TEMPsize); + setT *tricenters= qh_settemp (qh TEMPsize); + facetT *neighbor, **neighborp; + boolT firstinf= True; + + FOREACHneighbor_(vertex) { + if (neighbor->seen) { + if (neighbor->visitid) { + if (!neighbor->tricoplanar || qh_setunique (&tricenters, neighbor->center)) + qh_setappend (¢ers, neighbor); + }else if (firstinf) { + firstinf= False; + qh_setappend (¢ers, neighbor); + } + } + } + qsort (SETaddr_(centers, facetT), qh_setsize (centers), + sizeof (facetT *), qh_compare_facetvisit); + qh_settempfree (&tricenters); + return centers; +} /* detvridge */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="detvridge3">-</a> + + qh_detvridge3( atvertex, vertex ) + determine 3-d Voronoi ridge from 'seen' neighbors of atvertex and vertex + include one vertex-at-infinite for !neighbor->visitid + assumes all facet->seen2= True + + returns: + temporary set of centers (facets, i.e., Voronoi vertices) + listed in adjacency order (not oriented) + all facet->seen2= True + + design: + mark all neighbors of atvertex + for each adjacent neighbor of both atvertex and vertex + if neighbor selected + add neighbor to set of Voronoi vertices +*/ +setT *qh_detvridge3 (vertexT *atvertex, vertexT *vertex) { + setT *centers= qh_settemp (qh TEMPsize); + setT *tricenters= qh_settemp (qh TEMPsize); + facetT *neighbor, **neighborp, *facet= NULL; + boolT firstinf= True; + + FOREACHneighbor_(atvertex) + neighbor->seen2= False; + FOREACHneighbor_(vertex) { + if (!neighbor->seen2) { + facet= neighbor; + break; + } + } + while (facet) { + facet->seen2= True; + if (neighbor->seen) { + if (facet->visitid) { + if (!facet->tricoplanar || qh_setunique (&tricenters, facet->center)) + qh_setappend (¢ers, facet); + }else if (firstinf) { + firstinf= False; + qh_setappend (¢ers, facet); + } + } + FOREACHneighbor_(facet) { + if (!neighbor->seen2) { + if (qh_setin (vertex->neighbors, neighbor)) + break; + else + neighbor->seen2= True; + } + } + facet= neighbor; + } + if (qh CHECKfrequently) { + FOREACHneighbor_(vertex) { + if (!neighbor->seen2) { + fprintf (stderr, "qh_detvridge3: neigbors of vertex p%d are not connected at facet %d\n", + qh_pointid (vertex->point), neighbor->id); + qh_errexit (qh_ERRqhull, neighbor, NULL); + } + } + } + FOREACHneighbor_(atvertex) + neighbor->seen2= True; + qh_settempfree (&tricenters); + return centers; +} /* detvridge3 */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="eachvoronoi">-</a> + + qh_eachvoronoi( fp, printvridge, vertex, visitall, innerouter, inorder ) + if visitall, + visit all Voronoi ridges for vertex (i.e., an input site) + else + visit all unvisited Voronoi ridges for vertex + all vertex->seen= False if unvisited + assumes + all facet->seen= False + all facet->seen2= True (for qh_detvridge3) + all facet->visitid == 0 if vertex_at_infinity + == index of Voronoi vertex + >= qh.num_facets if ignored + innerouter: + qh_RIDGEall-- both inner (bounded) and outer (unbounded) ridges + qh_RIDGEinner- only inner + qh_RIDGEouter- only outer + + if inorder + orders vertices for 3-d Voronoi diagrams + + returns: + number of visited ridges (does not include previously visited ridges) + + if printvridge, + calls printvridge( fp, vertex, vertexA, centers) + fp== any pointer (assumes FILE*) + vertex,vertexA= pair of input sites that define a Voronoi ridge + centers= set of facets (i.e., Voronoi vertices) + ->visitid == index or 0 if vertex_at_infinity + ordered for 3-d Voronoi diagram + notes: + uses qh.vertex_visit + + see: + qh_eachvoronoi_all() + + design: + mark selected neighbors of atvertex + for each selected neighbor (either Voronoi vertex or vertex-at-infinity) + for each unvisited vertex + if atvertex and vertex share more than d-1 neighbors + bump totalcount + if printvridge defined + build the set of shared neighbors (i.e., Voronoi vertices) + call printvridge +*/ +int qh_eachvoronoi (FILE *fp, printvridgeT printvridge, vertexT *atvertex, boolT visitall, qh_RIDGE innerouter, boolT inorder) { + boolT unbounded; + int count; + facetT *neighbor, **neighborp, *neighborA, **neighborAp; + setT *centers; + setT *tricenters= qh_settemp (qh TEMPsize); + + vertexT *vertex, **vertexp; + boolT firstinf; + unsigned int numfacets= (unsigned int)qh num_facets; + int totridges= 0; + + qh vertex_visit++; + atvertex->seen= True; + if (visitall) { + FORALLvertices + vertex->seen= False; + } + FOREACHneighbor_(atvertex) { + if (neighbor->visitid < numfacets) + neighbor->seen= True; + } + FOREACHneighbor_(atvertex) { + if (neighbor->seen) { + FOREACHvertex_(neighbor->vertices) { + if (vertex->visitid != qh vertex_visit && !vertex->seen) { + vertex->visitid= qh vertex_visit; + count= 0; + firstinf= True; + qh_settruncate (tricenters, 0); + FOREACHneighborA_(vertex) { + if (neighborA->seen) { + if (neighborA->visitid) { + if (!neighborA->tricoplanar || qh_setunique (&tricenters, neighborA->center)) + count++; + }else if (firstinf) { + count++; + firstinf= False; + } + } + } + if (count >= qh hull_dim - 1) { /* e.g., 3 for 3-d Voronoi */ + if (firstinf) { + if (innerouter == qh_RIDGEouter) + continue; + unbounded= False; + }else { + if (innerouter == qh_RIDGEinner) + continue; + unbounded= True; + } + totridges++; + trace4((qh ferr, "qh_eachvoronoi: Voronoi ridge of %d vertices between sites %d and %d\n", + count, qh_pointid (atvertex->point), qh_pointid (vertex->point))); + if (printvridge) { + if (inorder && qh hull_dim == 3+1) /* 3-d Voronoi diagram */ + centers= qh_detvridge3 (atvertex, vertex); + else + centers= qh_detvridge (vertex); + (*printvridge) (fp, atvertex, vertex, centers, unbounded); + qh_settempfree (¢ers); + } + } + } + } + } + } + FOREACHneighbor_(atvertex) + neighbor->seen= False; + qh_settempfree (&tricenters); + return totridges; +} /* eachvoronoi */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="eachvoronoi_all">-</a> + + qh_eachvoronoi_all( fp, printvridge, isupper, innerouter, inorder ) + visit all Voronoi ridges + + innerouter: + see qh_eachvoronoi() + + if inorder + orders vertices for 3-d Voronoi diagrams + + returns + total number of ridges + + if isupper == facet->upperdelaunay (i.e., a Vornoi vertex) + facet->visitid= Voronoi vertex index (same as 'o' format) + else + facet->visitid= 0 + + if printvridge, + calls printvridge( fp, vertex, vertexA, centers) + [see qh_eachvoronoi] + + notes: + Not used for qhull.exe + same effect as qh_printvdiagram but ridges not sorted by point id +*/ +int qh_eachvoronoi_all (FILE *fp, printvridgeT printvridge, boolT isupper, qh_RIDGE innerouter, boolT inorder) { + facetT *facet; + vertexT *vertex; + int numcenters= 1; /* vertex 0 is vertex-at-infinity */ + int totridges= 0; + + qh_clearcenters (qh_ASvoronoi); + qh_vertexneighbors(); + maximize_(qh visit_id, (unsigned) qh num_facets); + FORALLfacets { + facet->visitid= 0; + facet->seen= False; + facet->seen2= True; + } + FORALLfacets { + if (facet->upperdelaunay == isupper) + facet->visitid= numcenters++; + } + FORALLvertices + vertex->seen= False; + FORALLvertices { + if (qh GOODvertex > 0 && qh_pointid(vertex->point)+1 != qh GOODvertex) + continue; + totridges += qh_eachvoronoi (fp, printvridge, vertex, + !qh_ALL, innerouter, inorder); + } + return totridges; +} /* eachvoronoi_all */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="facet2point">-</a> + + qh_facet2point( facet, point0, point1, mindist ) + return two projected temporary vertices for a 2-d facet + may be non-simplicial + + returns: + point0 and point1 oriented and projected to the facet + returns mindist (maximum distance below plane) +*/ +void qh_facet2point(facetT *facet, pointT **point0, pointT **point1, realT *mindist) { + vertexT *vertex0, *vertex1; + realT dist; + + if (facet->toporient ^ qh_ORIENTclock) { + vertex0= SETfirstt_(facet->vertices, vertexT); + vertex1= SETsecondt_(facet->vertices, vertexT); + }else { + vertex1= SETfirstt_(facet->vertices, vertexT); + vertex0= SETsecondt_(facet->vertices, vertexT); + } + zadd_(Zdistio, 2); + qh_distplane(vertex0->point, facet, &dist); + *mindist= dist; + *point0= qh_projectpoint(vertex0->point, facet, dist); + qh_distplane(vertex1->point, facet, &dist); + minimize_(*mindist, dist); + *point1= qh_projectpoint(vertex1->point, facet, dist); +} /* facet2point */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="facetvertices">-</a> + + qh_facetvertices( facetlist, facets, allfacets ) + returns temporary set of vertices in a set and/or list of facets + if allfacets, ignores qh_skipfacet() + + returns: + vertices with qh.vertex_visit + + notes: + optimized for allfacets of facet_list + + design: + if allfacets of facet_list + create vertex set from vertex_list + else + for each selected facet in facets or facetlist + append unvisited vertices to vertex set +*/ +setT *qh_facetvertices (facetT *facetlist, setT *facets, boolT allfacets) { + setT *vertices; + facetT *facet, **facetp; + vertexT *vertex, **vertexp; + + qh vertex_visit++; + if (facetlist == qh facet_list && allfacets && !facets) { + vertices= qh_settemp (qh num_vertices); + FORALLvertices { + vertex->visitid= qh vertex_visit; + qh_setappend (&vertices, vertex); + } + }else { + vertices= qh_settemp (qh TEMPsize); + FORALLfacet_(facetlist) { + if (!allfacets && qh_skipfacet (facet)) + continue; + FOREACHvertex_(facet->vertices) { + if (vertex->visitid != qh vertex_visit) { + vertex->visitid= qh vertex_visit; + qh_setappend (&vertices, vertex); + } + } + } + } + FOREACHfacet_(facets) { + if (!allfacets && qh_skipfacet (facet)) + continue; + FOREACHvertex_(facet->vertices) { + if (vertex->visitid != qh vertex_visit) { + vertex->visitid= qh vertex_visit; + qh_setappend (&vertices, vertex); + } + } + } + return vertices; +} /* facetvertices */ + +/*-<a href="qh-geom.htm#TOC" + >-------------------------------</a><a name="geomplanes">-</a> + + qh_geomplanes( facet, outerplane, innerplane ) + return outer and inner planes for Geomview + qh.PRINTradius is size of vertices and points (includes qh.JOGGLEmax) + + notes: + assume precise calculations in io.c with roundoff covered by qh_GEOMepsilon +*/ +void qh_geomplanes (facetT *facet, realT *outerplane, realT *innerplane) { + realT radius; + + if (qh MERGING || qh JOGGLEmax < REALmax/2) { + qh_outerinner (facet, outerplane, innerplane); + radius= qh PRINTradius; + if (qh JOGGLEmax < REALmax/2) + radius -= qh JOGGLEmax * sqrt (qh hull_dim); /* already accounted for in qh_outerinner() */ + *outerplane += radius; + *innerplane -= radius; + if (qh PRINTcoplanar || qh PRINTspheres) { + *outerplane += qh MAXabs_coord * qh_GEOMepsilon; + *innerplane -= qh MAXabs_coord * qh_GEOMepsilon; + } + }else + *innerplane= *outerplane= 0; +} /* geomplanes */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="markkeep">-</a> + + qh_markkeep( facetlist ) + mark good facets that meet qh.KEEParea, qh.KEEPmerge, and qh.KEEPminArea + ignores visible facets (not part of convex hull) + + returns: + may clear facet->good + recomputes qh.num_good + + design: + get set of good facets + if qh.KEEParea + sort facets by area + clear facet->good for all but n largest facets + if qh.KEEPmerge + sort facets by merge count + clear facet->good for all but n most merged facets + if qh.KEEPminarea + clear facet->good if area too small + update qh.num_good +*/ +void qh_markkeep (facetT *facetlist) { + facetT *facet, **facetp; + setT *facets= qh_settemp (qh num_facets); + int size, count; + + trace2((qh ferr, "qh_markkeep: only keep %d largest and/or %d most merged facets and/or min area %.2g\n", + qh KEEParea, qh KEEPmerge, qh KEEPminArea)); + FORALLfacet_(facetlist) { + if (!facet->visible && facet->good) + qh_setappend (&facets, facet); + } + size= qh_setsize (facets); + if (qh KEEParea) { + qsort (SETaddr_(facets, facetT), size, + sizeof (facetT *), qh_compare_facetarea); + if ((count= size - qh KEEParea) > 0) { + FOREACHfacet_(facets) { + facet->good= False; + if (--count == 0) + break; + } + } + } + if (qh KEEPmerge) { + qsort (SETaddr_(facets, facetT), size, + sizeof (facetT *), qh_compare_facetmerge); + if ((count= size - qh KEEPmerge) > 0) { + FOREACHfacet_(facets) { + facet->good= False; + if (--count == 0) + break; + } + } + } + if (qh KEEPminArea < REALmax/2) { + FOREACHfacet_(facets) { + if (!facet->isarea || facet->f.area < qh KEEPminArea) + facet->good= False; + } + } + qh_settempfree (&facets); + count= 0; + FORALLfacet_(facetlist) { + if (facet->good) + count++; + } + qh num_good= count; +} /* markkeep */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="markvoronoi">-</a> + + qh_markvoronoi( facetlist, facets, printall, islower, numcenters ) + mark voronoi vertices for printing by site pairs + + returns: + temporary set of vertices indexed by pointid + islower set if printing lower hull (i.e., at least one facet is lower hull) + numcenters= total number of Voronoi vertices + bumps qh.printoutnum for vertex-at-infinity + clears all facet->seen and sets facet->seen2 + + if selected + facet->visitid= Voronoi vertex id + else if upper hull (or 'Qu' and lower hull) + facet->visitid= 0 + else + facet->visitid >= qh num_facets + + notes: + ignores qh.ATinfinity, if defined +*/ +setT *qh_markvoronoi (facetT *facetlist, setT *facets, boolT printall, boolT *islowerp, int *numcentersp) { + int numcenters=0; + facetT *facet, **facetp; + setT *vertices; + boolT islower= False; + + qh printoutnum++; + qh_clearcenters (qh_ASvoronoi); /* in case, qh_printvdiagram2 called by user */ + qh_vertexneighbors(); + vertices= qh_pointvertex(); + if (qh ATinfinity) + SETelem_(vertices, qh num_points-1)= NULL; + qh visit_id++; + maximize_(qh visit_id, (unsigned) qh num_facets); + FORALLfacet_(facetlist) { + if (printall || !qh_skipfacet (facet)) { + if (!facet->upperdelaunay) { + islower= True; + break; + } + } + } + FOREACHfacet_(facets) { + if (printall || !qh_skipfacet (facet)) { + if (!facet->upperdelaunay) { + islower= True; + break; + } + } + } + FORALLfacets { + if (facet->normal && (facet->upperdelaunay == islower)) + facet->visitid= 0; /* facetlist or facets may overwrite */ + else + facet->visitid= qh visit_id; + facet->seen= False; + facet->seen2= True; + } + numcenters++; /* qh_INFINITE */ + FORALLfacet_(facetlist) { + if (printall || !qh_skipfacet (facet)) + facet->visitid= numcenters++; + } + FOREACHfacet_(facets) { + if (printall || !qh_skipfacet (facet)) + facet->visitid= numcenters++; + } + *islowerp= islower; + *numcentersp= numcenters; + trace2((qh ferr, "qh_markvoronoi: islower %d numcenters %d\n", islower, numcenters)); + return vertices; +} /* markvoronoi */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="order_vertexneighbors">-</a> + + qh_order_vertexneighbors( vertex ) + order facet neighbors of a 2-d or 3-d vertex by adjacency + + notes: + does not orient the neighbors + + design: + initialize a new neighbor set with the first facet in vertex->neighbors + while vertex->neighbors non-empty + select next neighbor in the previous facet's neighbor set + set vertex->neighbors to the new neighbor set +*/ +void qh_order_vertexneighbors(vertexT *vertex) { + setT *newset; + facetT *facet, *neighbor, **neighborp; + + trace4((qh ferr, "qh_order_vertexneighbors: order neighbors of v%d for 3-d\n", vertex->id)); + newset= qh_settemp (qh_setsize (vertex->neighbors)); + facet= (facetT*)qh_setdellast (vertex->neighbors); + qh_setappend (&newset, facet); + while (qh_setsize (vertex->neighbors)) { + FOREACHneighbor_(vertex) { + if (qh_setin (facet->neighbors, neighbor)) { + qh_setdel(vertex->neighbors, neighbor); + qh_setappend (&newset, neighbor); + facet= neighbor; + break; + } + } + if (!neighbor) { + fprintf (qh ferr, "qhull internal error (qh_order_vertexneighbors): no neighbor of v%d for f%d\n", + vertex->id, facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + } + qh_setfree (&vertex->neighbors); + qh_settemppop (); + vertex->neighbors= newset; +} /* order_vertexneighbors */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printafacet">-</a> + + qh_printafacet( fp, format, facet, printall ) + print facet to fp in given output format (see qh.PRINTout) + + returns: + nop if !printall and qh_skipfacet() + nop if visible facet and NEWfacets and format != PRINTfacets + must match qh_countfacets + + notes + preserves qh.visit_id + facet->normal may be null if PREmerge/MERGEexact and STOPcone before merge + + see + qh_printbegin() and qh_printend() + + design: + test for printing facet + call appropriate routine for format + or output results directly +*/ +void qh_printafacet(FILE *fp, int format, facetT *facet, boolT printall) { + realT color[4], offset, dist, outerplane, innerplane; + boolT zerodiv; + coordT *point, *normp, *coordp, **pointp, *feasiblep; + int k; + vertexT *vertex, **vertexp; + facetT *neighbor, **neighborp; + + if (!printall && qh_skipfacet (facet)) + return; + if (facet->visible && qh NEWfacets && format != qh_PRINTfacets) + return; + qh printoutnum++; + switch (format) { + case qh_PRINTarea: + if (facet->isarea) { + fprintf (fp, qh_REAL_1, facet->f.area); + fprintf (fp, "\n"); + }else + fprintf (fp, "0\n"); + break; + case qh_PRINTcoplanars: + fprintf (fp, "%d", qh_setsize (facet->coplanarset)); + FOREACHpoint_(facet->coplanarset) + fprintf (fp, " %d", qh_pointid (point)); + fprintf (fp, "\n"); + break; + case qh_PRINTcentrums: + qh_printcenter (fp, format, NULL, facet); + break; + case qh_PRINTfacets: + qh_printfacet (fp, facet); + break; + case qh_PRINTfacets_xridge: + qh_printfacetheader (fp, facet); + break; + case qh_PRINTgeom: /* either 2 , 3, or 4-d by qh_printbegin */ + if (!facet->normal) + break; + for (k= qh hull_dim; k--; ) { + color[k]= (facet->normal[k]+1.0)/2.0; + maximize_(color[k], -1.0); + minimize_(color[k], +1.0); + } + qh_projectdim3 (color, color); + if (qh PRINTdim != qh hull_dim) + qh_normalize2 (color, 3, True, NULL, NULL); + if (qh hull_dim <= 2) + qh_printfacet2geom (fp, facet, color); + else if (qh hull_dim == 3) { + if (facet->simplicial) + qh_printfacet3geom_simplicial (fp, facet, color); + else + qh_printfacet3geom_nonsimplicial (fp, facet, color); + }else { + if (facet->simplicial) + qh_printfacet4geom_simplicial (fp, facet, color); + else + qh_printfacet4geom_nonsimplicial (fp, facet, color); + } + break; + case qh_PRINTids: + fprintf (fp, "%d\n", facet->id); + break; + case qh_PRINTincidences: + case qh_PRINToff: + case qh_PRINTtriangles: + if (qh hull_dim == 3 && format != qh_PRINTtriangles) + qh_printfacet3vertex (fp, facet, format); + else if (facet->simplicial || qh hull_dim == 2 || format == qh_PRINToff) + qh_printfacetNvertex_simplicial (fp, facet, format); + else + qh_printfacetNvertex_nonsimplicial (fp, facet, qh printoutvar++, format); + break; + case qh_PRINTinner: + qh_outerinner (facet, NULL, &innerplane); + offset= facet->offset - innerplane; + goto LABELprintnorm; + break; /* prevent warning */ + case qh_PRINTmerges: + fprintf (fp, "%d\n", facet->nummerge); + break; + case qh_PRINTnormals: + offset= facet->offset; + goto LABELprintnorm; + break; /* prevent warning */ + case qh_PRINTouter: + qh_outerinner (facet, &outerplane, NULL); + offset= facet->offset - outerplane; + LABELprintnorm: + if (!facet->normal) { + fprintf (fp, "no normal for facet f%d\n", facet->id); + break; + } + if (qh CDDoutput) { + fprintf (fp, qh_REAL_1, -offset); + for (k=0; k < qh hull_dim; k++) + fprintf (fp, qh_REAL_1, -facet->normal[k]); + }else { + for (k=0; k < qh hull_dim; k++) + fprintf (fp, qh_REAL_1, facet->normal[k]); + fprintf (fp, qh_REAL_1, offset); + } + fprintf (fp, "\n"); + break; + case qh_PRINTmathematica: /* either 2 or 3-d by qh_printbegin */ + case qh_PRINTmaple: + if (qh hull_dim == 2) + qh_printfacet2math (fp, facet, format, qh printoutvar++); + else + qh_printfacet3math (fp, facet, format, qh printoutvar++); + break; + case qh_PRINTneighbors: + fprintf (fp, "%d", qh_setsize (facet->neighbors)); + FOREACHneighbor_(facet) + fprintf (fp, " %d", + neighbor->visitid ? neighbor->visitid - 1: - neighbor->id); + fprintf (fp, "\n"); + break; + case qh_PRINTpointintersect: + if (!qh feasible_point) { + fprintf (fp, "qhull input error (qh_printafacet): option 'Fp' needs qh feasible_point\n"); + qh_errexit( qh_ERRinput, NULL, NULL); + } + if (facet->offset > 0) + goto LABELprintinfinite; + point= coordp= (coordT*)qh_memalloc (qh normal_size); + normp= facet->normal; + feasiblep= qh feasible_point; + if (facet->offset < -qh MINdenom) { + for (k= qh hull_dim; k--; ) + *(coordp++)= (*(normp++) / - facet->offset) + *(feasiblep++); + }else { + for (k= qh hull_dim; k--; ) { + *(coordp++)= qh_divzero (*(normp++), facet->offset, qh MINdenom_1, + &zerodiv) + *(feasiblep++); + if (zerodiv) { + qh_memfree (point, qh normal_size); + goto LABELprintinfinite; + } + } + } + qh_printpoint (fp, NULL, point); + qh_memfree (point, qh normal_size); + break; + LABELprintinfinite: + for (k= qh hull_dim; k--; ) + fprintf (fp, qh_REAL_1, qh_INFINITE); + fprintf (fp, "\n"); + break; + case qh_PRINTpointnearest: + FOREACHpoint_(facet->coplanarset) { + int id, id2; + vertex= qh_nearvertex (facet, point, &dist); + id= qh_pointid (vertex->point); + id2= qh_pointid (point); + fprintf (fp, "%d %d %d " qh_REAL_1 "\n", id, id2, facet->id, dist); + } + break; + case qh_PRINTpoints: /* VORONOI only by qh_printbegin */ + if (qh CDDoutput) + fprintf (fp, "1 "); + qh_printcenter (fp, format, NULL, facet); + break; + case qh_PRINTvertices: + fprintf (fp, "%d", qh_setsize (facet->vertices)); + FOREACHvertex_(facet->vertices) + fprintf (fp, " %d", qh_pointid (vertex->point)); + fprintf (fp, "\n"); + break; + } +} /* printafacet */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printbegin">-</a> + + qh_printbegin( ) + prints header for all output formats + + returns: + checks for valid format + + notes: + uses qh.visit_id for 3/4off + changes qh.interior_point if printing centrums + qh_countfacets clears facet->visitid for non-good facets + + see + qh_printend() and qh_printafacet() + + design: + count facets and related statistics + print header for format +*/ +void qh_printbegin (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall) { + int numfacets, numsimplicial, numridges, totneighbors, numcoplanars, numtricoplanars; + int i, num; + facetT *facet, **facetp; + vertexT *vertex, **vertexp; + setT *vertices; + pointT *point, **pointp, *pointtemp; + + qh printoutnum= 0; + qh_countfacets (facetlist, facets, printall, &numfacets, &numsimplicial, + &totneighbors, &numridges, &numcoplanars, &numtricoplanars); + switch (format) { + case qh_PRINTnone: + break; + case qh_PRINTarea: + fprintf (fp, "%d\n", numfacets); + break; + case qh_PRINTcoplanars: + fprintf (fp, "%d\n", numfacets); + break; + case qh_PRINTcentrums: + if (qh CENTERtype == qh_ASnone) + qh_clearcenters (qh_AScentrum); + fprintf (fp, "%d\n%d\n", qh hull_dim, numfacets); + break; + case qh_PRINTfacets: + case qh_PRINTfacets_xridge: + if (facetlist) + qh_printvertexlist (fp, "Vertices and facets:\n", facetlist, facets, printall); + break; + case qh_PRINTgeom: + if (qh hull_dim > 4) /* qh_initqhull_globals also checks */ + goto LABELnoformat; + if (qh VORONOI && qh hull_dim > 3) /* PRINTdim == DROPdim == hull_dim-1 */ + goto LABELnoformat; + if (qh hull_dim == 2 && (qh PRINTridges || qh DOintersections)) + fprintf (qh ferr, "qhull warning: output for ridges and intersections not implemented in 2-d\n"); + if (qh hull_dim == 4 && (qh PRINTinner || qh PRINTouter || + (qh PRINTdim == 4 && qh PRINTcentrums))) + fprintf (qh ferr, "qhull warning: output for outer/inner planes and centrums not implemented in 4-d\n"); + if (qh PRINTdim == 4 && (qh PRINTspheres)) + fprintf (qh ferr, "qhull warning: output for vertices not implemented in 4-d\n"); + if (qh PRINTdim == 4 && qh DOintersections && qh PRINTnoplanes) + fprintf (qh ferr, "qhull warning: 'Gnh' generates no output in 4-d\n"); + if (qh PRINTdim == 2) { + fprintf(fp, "{appearance {linewidth 3} LIST # %s | %s\n", + qh rbox_command, qh qhull_command); + }else if (qh PRINTdim == 3) { + fprintf(fp, "{appearance {+edge -evert linewidth 2} LIST # %s | %s\n", + qh rbox_command, qh qhull_command); + }else if (qh PRINTdim == 4) { + qh visit_id++; + num= 0; + FORALLfacet_(facetlist) /* get number of ridges to be printed */ + qh_printend4geom (NULL, facet, &num, printall); + FOREACHfacet_(facets) + qh_printend4geom (NULL, facet, &num, printall); + qh ridgeoutnum= num; + qh printoutvar= 0; /* counts number of ridges in output */ + fprintf (fp, "LIST # %s | %s\n", qh rbox_command, qh qhull_command); + } + if (qh PRINTdots) { + qh printoutnum++; + num= qh num_points + qh_setsize (qh other_points); + if (qh DELAUNAY && qh ATinfinity) + num--; + if (qh PRINTdim == 4) + fprintf (fp, "4VECT %d %d 1\n", num, num); + else + fprintf (fp, "VECT %d %d 1\n", num, num); + for (i= num; i--; ) { + if (i % 20 == 0) + fprintf (fp, "\n"); + fprintf (fp, "1 "); + } + fprintf (fp, "# 1 point per line\n1 "); + for (i= num-1; i--; ) { + if (i % 20 == 0) + fprintf (fp, "\n"); + fprintf (fp, "0 "); + } + fprintf (fp, "# 1 color for all\n"); + FORALLpoints { + if (!qh DELAUNAY || !qh ATinfinity || qh_pointid(point) != qh num_points-1) { + if (qh PRINTdim == 4) + qh_printpoint (fp, NULL, point); + else + qh_printpoint3 (fp, point); + } + } + FOREACHpoint_(qh other_points) { + if (qh PRINTdim == 4) + qh_printpoint (fp, NULL, point); + else + qh_printpoint3 (fp, point); + } + fprintf (fp, "0 1 1 1 # color of points\n"); + } + if (qh PRINTdim == 4 && !qh PRINTnoplanes) + /* 4dview loads up multiple 4OFF objects slowly */ + fprintf(fp, "4OFF %d %d 1\n", 3*qh ridgeoutnum, qh ridgeoutnum); + qh PRINTcradius= 2 * qh DISTround; /* include test DISTround */ + if (qh PREmerge) { + maximize_(qh PRINTcradius, qh premerge_centrum + qh DISTround); + }else if (qh POSTmerge) + maximize_(qh PRINTcradius, qh postmerge_centrum + qh DISTround); + qh PRINTradius= qh PRINTcradius; + if (qh PRINTspheres + qh PRINTcoplanar) + maximize_(qh PRINTradius, qh MAXabs_coord * qh_MINradius); + if (qh premerge_cos < REALmax/2) { + maximize_(qh PRINTradius, (1- qh premerge_cos) * qh MAXabs_coord); + }else if (!qh PREmerge && qh POSTmerge && qh postmerge_cos < REALmax/2) { + maximize_(qh PRINTradius, (1- qh postmerge_cos) * qh MAXabs_coord); + } + maximize_(qh PRINTradius, qh MINvisible); + if (qh JOGGLEmax < REALmax/2) + qh PRINTradius += qh JOGGLEmax * sqrt (qh hull_dim); + if (qh PRINTdim != 4 && + (qh PRINTcoplanar || qh PRINTspheres || qh PRINTcentrums)) { + vertices= qh_facetvertices (facetlist, facets, printall); + if (qh PRINTspheres && qh PRINTdim <= 3) + qh_printspheres (fp, vertices, qh PRINTradius); + if (qh PRINTcoplanar || qh PRINTcentrums) { + qh firstcentrum= True; + if (qh PRINTcoplanar&& !qh PRINTspheres) { + FOREACHvertex_(vertices) + qh_printpointvect2 (fp, vertex->point, NULL, + qh interior_point, qh PRINTradius); + } + FORALLfacet_(facetlist) { + if (!printall && qh_skipfacet(facet)) + continue; + if (!facet->normal) + continue; + if (qh PRINTcentrums && qh PRINTdim <= 3) + qh_printcentrum (fp, facet, qh PRINTcradius); + if (!qh PRINTcoplanar) + continue; + FOREACHpoint_(facet->coplanarset) + qh_printpointvect2 (fp, point, facet->normal, NULL, qh PRINTradius); + FOREACHpoint_(facet->outsideset) + qh_printpointvect2 (fp, point, facet->normal, NULL, qh PRINTradius); + } + FOREACHfacet_(facets) { + if (!printall && qh_skipfacet(facet)) + continue; + if (!facet->normal) + continue; + if (qh PRINTcentrums && qh PRINTdim <= 3) + qh_printcentrum (fp, facet, qh PRINTcradius); + if (!qh PRINTcoplanar) + continue; + FOREACHpoint_(facet->coplanarset) + qh_printpointvect2 (fp, point, facet->normal, NULL, qh PRINTradius); + FOREACHpoint_(facet->outsideset) + qh_printpointvect2 (fp, point, facet->normal, NULL, qh PRINTradius); + } + } + qh_settempfree (&vertices); + } + qh visit_id++; /* for printing hyperplane intersections */ + break; + case qh_PRINTids: + fprintf (fp, "%d\n", numfacets); + break; + case qh_PRINTincidences: + if (qh VORONOI && qh PRINTprecision) + fprintf (qh ferr, "qhull warning: writing Delaunay. Use 'p' or 'o' for Voronoi centers\n"); + qh printoutvar= qh vertex_id; /* centrum id for non-simplicial facets */ + if (qh hull_dim <= 3) + fprintf(fp, "%d\n", numfacets); + else + fprintf(fp, "%d\n", numsimplicial+numridges); + break; + case qh_PRINTinner: + case qh_PRINTnormals: + case qh_PRINTouter: + if (qh CDDoutput) + fprintf (fp, "%s | %s\nbegin\n %d %d real\n", qh rbox_command, + qh qhull_command, numfacets, qh hull_dim+1); + else + fprintf (fp, "%d\n%d\n", qh hull_dim+1, numfacets); + break; + case qh_PRINTmathematica: + case qh_PRINTmaple: + if (qh hull_dim > 3) /* qh_initbuffers also checks */ + goto LABELnoformat; + if (qh VORONOI) + fprintf (qh ferr, "qhull warning: output is the Delaunay triangulation\n"); + if (format == qh_PRINTmaple) { + if (qh hull_dim == 2) + fprintf(fp, "PLOT(CURVES(\n"); + else + fprintf(fp, "PLOT3D(POLYGONS(\n"); + }else + fprintf(fp, "{\n"); + qh printoutvar= 0; /* counts number of facets for notfirst */ + break; + case qh_PRINTmerges: + fprintf (fp, "%d\n", numfacets); + break; + case qh_PRINTpointintersect: + fprintf (fp, "%d\n%d\n", qh hull_dim, numfacets); + break; + case qh_PRINTneighbors: + fprintf (fp, "%d\n", numfacets); + break; + case qh_PRINToff: + case qh_PRINTtriangles: + if (qh VORONOI) + goto LABELnoformat; + num = qh hull_dim; + if (format == qh_PRINToff || qh hull_dim == 2) + fprintf (fp, "%d\n%d %d %d\n", num, + qh num_points+qh_setsize (qh other_points), numfacets, totneighbors/2); + else { /* qh_PRINTtriangles */ + qh printoutvar= qh num_points+qh_setsize (qh other_points); /* first centrum */ + if (qh DELAUNAY) + num--; /* drop last dimension */ + fprintf (fp, "%d\n%d %d %d\n", num, qh printoutvar + + numfacets - numsimplicial, numsimplicial + numridges, totneighbors/2); + } + FORALLpoints + qh_printpointid (qh fout, NULL, num, point, -1); + FOREACHpoint_(qh other_points) + qh_printpointid (qh fout, NULL, num, point, -1); + if (format == qh_PRINTtriangles && qh hull_dim > 2) { + FORALLfacets { + if (!facet->simplicial && facet->visitid) + qh_printcenter (qh fout, format, NULL, facet); + } + } + break; + case qh_PRINTpointnearest: + fprintf (fp, "%d\n", numcoplanars); + break; + case qh_PRINTpoints: + if (!qh VORONOI) + goto LABELnoformat; + if (qh CDDoutput) + fprintf (fp, "%s | %s\nbegin\n%d %d real\n", qh rbox_command, + qh qhull_command, numfacets, qh hull_dim); + else + fprintf (fp, "%d\n%d\n", qh hull_dim-1, numfacets); + break; + case qh_PRINTvertices: + fprintf (fp, "%d\n", numfacets); + break; + case qh_PRINTsummary: + default: + LABELnoformat: + fprintf (qh ferr, "qhull internal error (qh_printbegin): can not use this format for dimension %d\n", + qh hull_dim); + qh_errexit (qh_ERRqhull, NULL, NULL); + } +} /* printbegin */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printcenter">-</a> + + qh_printcenter( fp, string, facet ) + print facet->center as centrum or Voronoi center + string may be NULL. Don't include '%' codes. + nop if qh CENTERtype neither CENTERvoronoi nor CENTERcentrum + if upper envelope of Delaunay triangulation and point at-infinity + prints qh_INFINITE instead; + + notes: + defines facet->center if needed + if format=PRINTgeom, adds a 0 if would otherwise be 2-d +*/ +void qh_printcenter (FILE *fp, int format, char *string, facetT *facet) { + int k, num; + + if (qh CENTERtype != qh_ASvoronoi && qh CENTERtype != qh_AScentrum) + return; + if (string) + fprintf (fp, string, facet->id); + if (qh CENTERtype == qh_ASvoronoi) { + num= qh hull_dim-1; + if (!facet->normal || !facet->upperdelaunay || !qh ATinfinity) { + if (!facet->center) + facet->center= qh_facetcenter (facet->vertices); + for (k=0; k < num; k++) + fprintf (fp, qh_REAL_1, facet->center[k]); + }else { + for (k=0; k < num; k++) + fprintf (fp, qh_REAL_1, qh_INFINITE); + } + }else /* qh CENTERtype == qh_AScentrum */ { + num= qh hull_dim; + if (format == qh_PRINTtriangles && qh DELAUNAY) + num--; + if (!facet->center) + facet->center= qh_getcentrum (facet); + for (k=0; k < num; k++) + fprintf (fp, qh_REAL_1, facet->center[k]); + } + if (format == qh_PRINTgeom && num == 2) + fprintf (fp, " 0\n"); + else + fprintf (fp, "\n"); +} /* printcenter */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printcentrum">-</a> + + qh_printcentrum( fp, facet, radius ) + print centrum for a facet in OOGL format + radius defines size of centrum + 2-d or 3-d only + + returns: + defines facet->center if needed +*/ +void qh_printcentrum (FILE *fp, facetT *facet, realT radius) { + pointT *centrum, *projpt; + boolT tempcentrum= False; + realT xaxis[4], yaxis[4], normal[4], dist; + realT green[3]={0, 1, 0}; + vertexT *apex; + int k; + + if (qh CENTERtype == qh_AScentrum) { + if (!facet->center) + facet->center= qh_getcentrum (facet); + centrum= facet->center; + }else { + centrum= qh_getcentrum (facet); + tempcentrum= True; + } + fprintf (fp, "{appearance {-normal -edge normscale 0} "); + if (qh firstcentrum) { + qh firstcentrum= False; + fprintf (fp, "{INST geom { define centrum CQUAD # f%d\n\ +-0.3 -0.3 0.0001 0 0 1 1\n\ + 0.3 -0.3 0.0001 0 0 1 1\n\ + 0.3 0.3 0.0001 0 0 1 1\n\ +-0.3 0.3 0.0001 0 0 1 1 } transform { \n", facet->id); + }else + fprintf (fp, "{INST geom { : centrum } transform { # f%d\n", facet->id); + apex= SETfirstt_(facet->vertices, vertexT); + qh_distplane(apex->point, facet, &dist); + projpt= qh_projectpoint(apex->point, facet, dist); + for (k= qh hull_dim; k--; ) { + xaxis[k]= projpt[k] - centrum[k]; + normal[k]= facet->normal[k]; + } + if (qh hull_dim == 2) { + xaxis[2]= 0; + normal[2]= 0; + }else if (qh hull_dim == 4) { + qh_projectdim3 (xaxis, xaxis); + qh_projectdim3 (normal, normal); + qh_normalize2 (normal, qh PRINTdim, True, NULL, NULL); + } + qh_crossproduct (3, xaxis, normal, yaxis); + fprintf (fp, "%8.4g %8.4g %8.4g 0\n", xaxis[0], xaxis[1], xaxis[2]); + fprintf (fp, "%8.4g %8.4g %8.4g 0\n", yaxis[0], yaxis[1], yaxis[2]); + fprintf (fp, "%8.4g %8.4g %8.4g 0\n", normal[0], normal[1], normal[2]); + qh_printpoint3 (fp, centrum); + fprintf (fp, "1 }}}\n"); + qh_memfree (projpt, qh normal_size); + qh_printpointvect (fp, centrum, facet->normal, NULL, radius, green); + if (tempcentrum) + qh_memfree (centrum, qh normal_size); +} /* printcentrum */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printend">-</a> + + qh_printend( fp, format ) + prints trailer for all output formats + + see: + qh_printbegin() and qh_printafacet() + +*/ +void qh_printend (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall) { + int num; + facetT *facet, **facetp; + + if (!qh printoutnum) + fprintf (qh ferr, "qhull warning: no facets printed\n"); + switch (format) { + case qh_PRINTgeom: + if (qh hull_dim == 4 && qh DROPdim < 0 && !qh PRINTnoplanes) { + qh visit_id++; + num= 0; + FORALLfacet_(facetlist) + qh_printend4geom (fp, facet,&num, printall); + FOREACHfacet_(facets) + qh_printend4geom (fp, facet, &num, printall); + if (num != qh ridgeoutnum || qh printoutvar != qh ridgeoutnum) { + fprintf (qh ferr, "qhull internal error (qh_printend): number of ridges %d != number printed %d and at end %d\n", qh ridgeoutnum, qh printoutvar, num); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + }else + fprintf(fp, "}\n"); + break; + case qh_PRINTinner: + case qh_PRINTnormals: + case qh_PRINTouter: + if (qh CDDoutput) + fprintf (fp, "end\n"); + break; + case qh_PRINTmaple: + fprintf(fp, "));\n"); + break; + case qh_PRINTmathematica: + fprintf(fp, "}\n"); + break; + case qh_PRINTpoints: + if (qh CDDoutput) + fprintf (fp, "end\n"); + break; + } +} /* printend */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printend4geom">-</a> + + qh_printend4geom( fp, facet, numridges, printall ) + helper function for qh_printbegin/printend + + returns: + number of printed ridges + + notes: + just counts printed ridges if fp=NULL + uses facet->visitid + must agree with qh_printfacet4geom... + + design: + computes color for facet from its normal + prints each ridge of facet +*/ +void qh_printend4geom (FILE *fp, facetT *facet, int *nump, boolT printall) { + realT color[3]; + int i, num= *nump; + facetT *neighbor, **neighborp; + ridgeT *ridge, **ridgep; + + if (!printall && qh_skipfacet(facet)) + return; + if (qh PRINTnoplanes || (facet->visible && qh NEWfacets)) + return; + if (!facet->normal) + return; + if (fp) { + for (i=0; i < 3; i++) { + color[i]= (facet->normal[i]+1.0)/2.0; + maximize_(color[i], -1.0); + minimize_(color[i], +1.0); + } + } + facet->visitid= qh visit_id; + if (facet->simplicial) { + FOREACHneighbor_(facet) { + if (neighbor->visitid != qh visit_id) { + if (fp) + fprintf (fp, "3 %d %d %d %8.4g %8.4g %8.4g 1 # f%d f%d\n", + 3*num, 3*num+1, 3*num+2, color[0], color[1], color[2], + facet->id, neighbor->id); + num++; + } + } + }else { + FOREACHridge_(facet->ridges) { + neighbor= otherfacet_(ridge, facet); + if (neighbor->visitid != qh visit_id) { + if (fp) + fprintf (fp, "3 %d %d %d %8.4g %8.4g %8.4g 1 #r%d f%d f%d\n", + 3*num, 3*num+1, 3*num+2, color[0], color[1], color[2], + ridge->id, facet->id, neighbor->id); + num++; + } + } + } + *nump= num; +} /* printend4geom */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printextremes">-</a> + + qh_printextremes( fp, facetlist, facets, printall ) + print extreme points for convex hulls or halfspace intersections + + notes: + #points, followed by ids, one per line + + sorted by id + same order as qh_printpoints_out if no coplanar/interior points +*/ +void qh_printextremes (FILE *fp, facetT *facetlist, setT *facets, int printall) { + setT *vertices, *points; + pointT *point; + vertexT *vertex, **vertexp; + int id; + int numpoints=0, point_i, point_n; + int allpoints= qh num_points + qh_setsize (qh other_points); + + points= qh_settemp (allpoints); + qh_setzero (points, 0, allpoints); + vertices= qh_facetvertices (facetlist, facets, printall); + FOREACHvertex_(vertices) { + id= qh_pointid (vertex->point); + if (id >= 0) { + SETelem_(points, id)= vertex->point; + numpoints++; + } + } + qh_settempfree (&vertices); + fprintf (fp, "%d\n", numpoints); + FOREACHpoint_i_(points) { + if (point) + fprintf (fp, "%d\n", point_i); + } + qh_settempfree (&points); +} /* printextremes */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printextremes_2d">-</a> + + qh_printextremes_2d( fp, facetlist, facets, printall ) + prints point ids for facets in qh_ORIENTclock order + + notes: + #points, followed by ids, one per line + if facetlist/facets are disjoint than the output includes skips + errors if facets form a loop + does not print coplanar points +*/ +void qh_printextremes_2d (FILE *fp, facetT *facetlist, setT *facets, int printall) { + int numfacets, numridges, totneighbors, numcoplanars, numsimplicial, numtricoplanars; + setT *vertices; + facetT *facet, *startfacet, *nextfacet; + vertexT *vertexA, *vertexB; + + qh_countfacets (facetlist, facets, printall, &numfacets, &numsimplicial, + &totneighbors, &numridges, &numcoplanars, &numtricoplanars); /* marks qh visit_id */ + vertices= qh_facetvertices (facetlist, facets, printall); + fprintf(fp, "%d\n", qh_setsize (vertices)); + qh_settempfree (&vertices); + if (!numfacets) + return; + facet= startfacet= facetlist ? facetlist : SETfirstt_(facets, facetT); + qh vertex_visit++; + qh visit_id++; + do { + if (facet->toporient ^ qh_ORIENTclock) { + vertexA= SETfirstt_(facet->vertices, vertexT); + vertexB= SETsecondt_(facet->vertices, vertexT); + nextfacet= SETfirstt_(facet->neighbors, facetT); + }else { + vertexA= SETsecondt_(facet->vertices, vertexT); + vertexB= SETfirstt_(facet->vertices, vertexT); + nextfacet= SETsecondt_(facet->neighbors, facetT); + } + if (facet->visitid == qh visit_id) { + fprintf(qh ferr, "qh_printextremes_2d: loop in facet list. facet %d nextfacet %d\n", + facet->id, nextfacet->id); + qh_errexit2 (qh_ERRqhull, facet, nextfacet); + } + if (facet->visitid) { + if (vertexA->visitid != qh vertex_visit) { + vertexA->visitid= qh vertex_visit; + fprintf(fp, "%d\n", qh_pointid (vertexA->point)); + } + if (vertexB->visitid != qh vertex_visit) { + vertexB->visitid= qh vertex_visit; + fprintf(fp, "%d\n", qh_pointid (vertexB->point)); + } + } + facet->visitid= qh visit_id; + facet= nextfacet; + }while (facet && facet != startfacet); +} /* printextremes_2d */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printextremes_d">-</a> + + qh_printextremes_d( fp, facetlist, facets, printall ) + print extreme points of input sites for Delaunay triangulations + + notes: + #points, followed by ids, one per line + + unordered +*/ +void qh_printextremes_d (FILE *fp, facetT *facetlist, setT *facets, int printall) { + setT *vertices; + vertexT *vertex, **vertexp; + boolT upperseen, lowerseen; + facetT *neighbor, **neighborp; + int numpoints=0; + + vertices= qh_facetvertices (facetlist, facets, printall); + qh_vertexneighbors(); + FOREACHvertex_(vertices) { + upperseen= lowerseen= False; + FOREACHneighbor_(vertex) { + if (neighbor->upperdelaunay) + upperseen= True; + else + lowerseen= True; + } + if (upperseen && lowerseen) { + vertex->seen= True; + numpoints++; + }else + vertex->seen= False; + } + fprintf (fp, "%d\n", numpoints); + FOREACHvertex_(vertices) { + if (vertex->seen) + fprintf (fp, "%d\n", qh_pointid (vertex->point)); + } + qh_settempfree (&vertices); +} /* printextremes_d */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet">-</a> + + qh_printfacet( fp, facet ) + prints all fields of a facet to fp + + notes: + ridges printed in neighbor order +*/ +void qh_printfacet(FILE *fp, facetT *facet) { + + qh_printfacetheader (fp, facet); + if (facet->ridges) + qh_printfacetridges (fp, facet); +} /* printfacet */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet2geom">-</a> + + qh_printfacet2geom( fp, facet, color ) + print facet as part of a 2-d VECT for Geomview + + notes: + assume precise calculations in io.c with roundoff covered by qh_GEOMepsilon + mindist is calculated within io.c. maxoutside is calculated elsewhere + so a DISTround error may have occured. +*/ +void qh_printfacet2geom(FILE *fp, facetT *facet, realT color[3]) { + pointT *point0, *point1; + realT mindist, innerplane, outerplane; + int k; + + qh_facet2point (facet, &point0, &point1, &mindist); + qh_geomplanes (facet, &outerplane, &innerplane); + if (qh PRINTouter || (!qh PRINTnoplanes && !qh PRINTinner)) + qh_printfacet2geom_points(fp, point0, point1, facet, outerplane, color); + if (qh PRINTinner || (!qh PRINTnoplanes && !qh PRINTouter && + outerplane - innerplane > 2 * qh MAXabs_coord * qh_GEOMepsilon)) { + for(k= 3; k--; ) + color[k]= 1.0 - color[k]; + qh_printfacet2geom_points(fp, point0, point1, facet, innerplane, color); + } + qh_memfree (point1, qh normal_size); + qh_memfree (point0, qh normal_size); +} /* printfacet2geom */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet2geom_points">-</a> + + qh_printfacet2geom_points( fp, point1, point2, facet, offset, color ) + prints a 2-d facet as a VECT with 2 points at some offset. + The points are on the facet's plane. +*/ +void qh_printfacet2geom_points(FILE *fp, pointT *point1, pointT *point2, + facetT *facet, realT offset, realT color[3]) { + pointT *p1= point1, *p2= point2; + + fprintf(fp, "VECT 1 2 1 2 1 # f%d\n", facet->id); + if (offset != 0.0) { + p1= qh_projectpoint (p1, facet, -offset); + p2= qh_projectpoint (p2, facet, -offset); + } + fprintf(fp, "%8.4g %8.4g %8.4g\n%8.4g %8.4g %8.4g\n", + p1[0], p1[1], 0.0, p2[0], p2[1], 0.0); + if (offset != 0.0) { + qh_memfree (p1, qh normal_size); + qh_memfree (p2, qh normal_size); + } + fprintf(fp, "%8.4g %8.4g %8.4g 1.0\n", color[0], color[1], color[2]); +} /* printfacet2geom_points */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet2math">-</a> + + qh_printfacet2math( fp, facet, format, notfirst ) + print 2-d Maple or Mathematica output for a facet + may be non-simplicial + + notes: + use %16.8f since Mathematica 2.2 does not handle exponential format + see qh_printfacet3math +*/ +void qh_printfacet2math(FILE *fp, facetT *facet, int format, int notfirst) { + pointT *point0, *point1; + realT mindist; + char *pointfmt; + + qh_facet2point (facet, &point0, &point1, &mindist); + if (notfirst) + fprintf(fp, ","); + if (format == qh_PRINTmaple) + pointfmt= "[[%16.8f, %16.8f], [%16.8f, %16.8f]]\n"; + else + pointfmt= "Line[{{%16.8f, %16.8f}, {%16.8f, %16.8f}}]\n"; + fprintf(fp, pointfmt, point0[0], point0[1], point1[0], point1[1]); + qh_memfree (point1, qh normal_size); + qh_memfree (point0, qh normal_size); +} /* printfacet2math */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet3geom_nonsimplicial">-</a> + + qh_printfacet3geom_nonsimplicial( fp, facet, color ) + print Geomview OFF for a 3-d nonsimplicial facet. + if DOintersections, prints ridges to unvisited neighbors (qh visit_id) + + notes + uses facet->visitid for intersections and ridges +*/ +void qh_printfacet3geom_nonsimplicial(FILE *fp, facetT *facet, realT color[3]) { + ridgeT *ridge, **ridgep; + setT *projectedpoints, *vertices; + vertexT *vertex, **vertexp, *vertexA, *vertexB; + pointT *projpt, *point, **pointp; + facetT *neighbor; + realT dist, outerplane, innerplane; + int cntvertices, k; + realT black[3]={0, 0, 0}, green[3]={0, 1, 0}; + + qh_geomplanes (facet, &outerplane, &innerplane); + vertices= qh_facet3vertex (facet); /* oriented */ + cntvertices= qh_setsize(vertices); + projectedpoints= qh_settemp(cntvertices); + FOREACHvertex_(vertices) { + zinc_(Zdistio); + qh_distplane(vertex->point, facet, &dist); + projpt= qh_projectpoint(vertex->point, facet, dist); + qh_setappend (&projectedpoints, projpt); + } + if (qh PRINTouter || (!qh PRINTnoplanes && !qh PRINTinner)) + qh_printfacet3geom_points(fp, projectedpoints, facet, outerplane, color); + if (qh PRINTinner || (!qh PRINTnoplanes && !qh PRINTouter && + outerplane - innerplane > 2 * qh MAXabs_coord * qh_GEOMepsilon)) { + for (k=3; k--; ) + color[k]= 1.0 - color[k]; + qh_printfacet3geom_points(fp, projectedpoints, facet, innerplane, color); + } + FOREACHpoint_(projectedpoints) + qh_memfree (point, qh normal_size); + qh_settempfree(&projectedpoints); + qh_settempfree(&vertices); + if ((qh DOintersections || qh PRINTridges) + && (!facet->visible || !qh NEWfacets)) { + facet->visitid= qh visit_id; + FOREACHridge_(facet->ridges) { + neighbor= otherfacet_(ridge, facet); + if (neighbor->visitid != qh visit_id) { + if (qh DOintersections) + qh_printhyperplaneintersection(fp, facet, neighbor, ridge->vertices, black); + if (qh PRINTridges) { + vertexA= SETfirstt_(ridge->vertices, vertexT); + vertexB= SETsecondt_(ridge->vertices, vertexT); + qh_printline3geom (fp, vertexA->point, vertexB->point, green); + } + } + } + } +} /* printfacet3geom_nonsimplicial */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet3geom_points">-</a> + + qh_printfacet3geom_points( fp, points, facet, offset ) + prints a 3-d facet as OFF Geomview object. + offset is relative to the facet's hyperplane + Facet is determined as a list of points +*/ +void qh_printfacet3geom_points(FILE *fp, setT *points, facetT *facet, realT offset, realT color[3]) { + int k, n= qh_setsize(points), i; + pointT *point, **pointp; + setT *printpoints; + + fprintf(fp, "{ OFF %d 1 1 # f%d\n", n, facet->id); + if (offset != 0.0) { + printpoints= qh_settemp (n); + FOREACHpoint_(points) + qh_setappend (&printpoints, qh_projectpoint(point, facet, -offset)); + }else + printpoints= points; + FOREACHpoint_(printpoints) { + for (k=0; k < qh hull_dim; k++) { + if (k == qh DROPdim) + fprintf(fp, "0 "); + else + fprintf(fp, "%8.4g ", point[k]); + } + if (printpoints != points) + qh_memfree (point, qh normal_size); + fprintf (fp, "\n"); + } + if (printpoints != points) + qh_settempfree (&printpoints); + fprintf(fp, "%d ", n); + for(i= 0; i < n; i++) + fprintf(fp, "%d ", i); + fprintf(fp, "%8.4g %8.4g %8.4g 1.0 }\n", color[0], color[1], color[2]); +} /* printfacet3geom_points */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet3geom_simplicial">-</a> + + qh_printfacet3geom_simplicial( ) + print Geomview OFF for a 3-d simplicial facet. + + notes: + may flip color + uses facet->visitid for intersections and ridges + + assume precise calculations in io.c with roundoff covered by qh_GEOMepsilon + innerplane may be off by qh DISTround. Maxoutside is calculated elsewhere + so a DISTround error may have occured. +*/ +void qh_printfacet3geom_simplicial(FILE *fp, facetT *facet, realT color[3]) { + setT *points, *vertices; + vertexT *vertex, **vertexp, *vertexA, *vertexB; + facetT *neighbor, **neighborp; + realT outerplane, innerplane; + realT black[3]={0, 0, 0}, green[3]={0, 1, 0}; + int k; + + qh_geomplanes (facet, &outerplane, &innerplane); + vertices= qh_facet3vertex (facet); + points= qh_settemp (qh TEMPsize); + FOREACHvertex_(vertices) + qh_setappend(&points, vertex->point); + if (qh PRINTouter || (!qh PRINTnoplanes && !qh PRINTinner)) + qh_printfacet3geom_points(fp, points, facet, outerplane, color); + if (qh PRINTinner || (!qh PRINTnoplanes && !qh PRINTouter && + outerplane - innerplane > 2 * qh MAXabs_coord * qh_GEOMepsilon)) { + for (k= 3; k--; ) + color[k]= 1.0 - color[k]; + qh_printfacet3geom_points(fp, points, facet, innerplane, color); + } + qh_settempfree(&points); + qh_settempfree(&vertices); + if ((qh DOintersections || qh PRINTridges) + && (!facet->visible || !qh NEWfacets)) { + facet->visitid= qh visit_id; + FOREACHneighbor_(facet) { + if (neighbor->visitid != qh visit_id) { + vertices= qh_setnew_delnthsorted (facet->vertices, qh hull_dim, + SETindex_(facet->neighbors, neighbor), 0); + if (qh DOintersections) + qh_printhyperplaneintersection(fp, facet, neighbor, vertices, black); + if (qh PRINTridges) { + vertexA= SETfirstt_(vertices, vertexT); + vertexB= SETsecondt_(vertices, vertexT); + qh_printline3geom (fp, vertexA->point, vertexB->point, green); + } + qh_setfree(&vertices); + } + } + } +} /* printfacet3geom_simplicial */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet3math">-</a> + + qh_printfacet3math( fp, facet, notfirst ) + print 3-d Maple or Mathematica output for a facet + + notes: + may be non-simplicial + use %16.8f since Mathematica 2.2 does not handle exponential format + see qh_printfacet2math +*/ +void qh_printfacet3math (FILE *fp, facetT *facet, int format, int notfirst) { + vertexT *vertex, **vertexp; + setT *points, *vertices; + pointT *point, **pointp; + boolT firstpoint= True; + realT dist; + char *pointfmt, *endfmt; + + if (notfirst) + fprintf(fp, ",\n"); + vertices= qh_facet3vertex (facet); + points= qh_settemp (qh_setsize (vertices)); + FOREACHvertex_(vertices) { + zinc_(Zdistio); + qh_distplane(vertex->point, facet, &dist); + point= qh_projectpoint(vertex->point, facet, dist); + qh_setappend (&points, point); + } + if (format == qh_PRINTmaple) { + fprintf(fp, "["); + pointfmt= "[%16.8f, %16.8f, %16.8f]"; + endfmt= "]"; + }else { + fprintf(fp, "Polygon[{"); + pointfmt= "{%16.8f, %16.8f, %16.8f}"; + endfmt= "}]"; + } + FOREACHpoint_(points) { + if (firstpoint) + firstpoint= False; + else + fprintf(fp, ",\n"); + fprintf(fp, pointfmt, point[0], point[1], point[2]); + } + FOREACHpoint_(points) + qh_memfree (point, qh normal_size); + qh_settempfree(&points); + qh_settempfree(&vertices); + fprintf(fp, endfmt); +} /* printfacet3math */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet3vertex">-</a> + + qh_printfacet3vertex( fp, facet, format ) + print vertices in a 3-d facet as point ids + + notes: + prints number of vertices first if format == qh_PRINToff + the facet may be non-simplicial +*/ +void qh_printfacet3vertex(FILE *fp, facetT *facet, int format) { + vertexT *vertex, **vertexp; + setT *vertices; + + vertices= qh_facet3vertex (facet); + if (format == qh_PRINToff) + fprintf (fp, "%d ", qh_setsize (vertices)); + FOREACHvertex_(vertices) + fprintf (fp, "%d ", qh_pointid(vertex->point)); + fprintf (fp, "\n"); + qh_settempfree(&vertices); +} /* printfacet3vertex */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet4geom_nonsimplicial">-</a> + + qh_printfacet4geom_nonsimplicial( ) + print Geomview 4OFF file for a 4d nonsimplicial facet + prints all ridges to unvisited neighbors (qh.visit_id) + if qh.DROPdim + prints in OFF format + + notes: + must agree with printend4geom() +*/ +void qh_printfacet4geom_nonsimplicial(FILE *fp, facetT *facet, realT color[3]) { + facetT *neighbor; + ridgeT *ridge, **ridgep; + vertexT *vertex, **vertexp; + pointT *point; + int k; + realT dist; + + facet->visitid= qh visit_id; + if (qh PRINTnoplanes || (facet->visible && qh NEWfacets)) + return; + FOREACHridge_(facet->ridges) { + neighbor= otherfacet_(ridge, facet); + if (neighbor->visitid == qh visit_id) + continue; + if (qh PRINTtransparent && !neighbor->good) + continue; + if (qh DOintersections) + qh_printhyperplaneintersection(fp, facet, neighbor, ridge->vertices, color); + else { + if (qh DROPdim >= 0) + fprintf(fp, "OFF 3 1 1 # f%d\n", facet->id); + else { + qh printoutvar++; + fprintf (fp, "# r%d between f%d f%d\n", ridge->id, facet->id, neighbor->id); + } + FOREACHvertex_(ridge->vertices) { + zinc_(Zdistio); + qh_distplane(vertex->point,facet, &dist); + point=qh_projectpoint(vertex->point,facet, dist); + for(k= 0; k < qh hull_dim; k++) { + if (k != qh DROPdim) + fprintf(fp, "%8.4g ", point[k]); + } + fprintf (fp, "\n"); + qh_memfree (point, qh normal_size); + } + if (qh DROPdim >= 0) + fprintf(fp, "3 0 1 2 %8.4g %8.4g %8.4g\n", color[0], color[1], color[2]); + } + } +} /* printfacet4geom_nonsimplicial */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacet4geom_simplicial">-</a> + + qh_printfacet4geom_simplicial( fp, facet, color ) + print Geomview 4OFF file for a 4d simplicial facet + prints triangles for unvisited neighbors (qh.visit_id) + + notes: + must agree with printend4geom() +*/ +void qh_printfacet4geom_simplicial(FILE *fp, facetT *facet, realT color[3]) { + setT *vertices; + facetT *neighbor, **neighborp; + vertexT *vertex, **vertexp; + int k; + + facet->visitid= qh visit_id; + if (qh PRINTnoplanes || (facet->visible && qh NEWfacets)) + return; + FOREACHneighbor_(facet) { + if (neighbor->visitid == qh visit_id) + continue; + if (qh PRINTtransparent && !neighbor->good) + continue; + vertices= qh_setnew_delnthsorted (facet->vertices, qh hull_dim, + SETindex_(facet->neighbors, neighbor), 0); + if (qh DOintersections) + qh_printhyperplaneintersection(fp, facet, neighbor, vertices, color); + else { + if (qh DROPdim >= 0) + fprintf(fp, "OFF 3 1 1 # ridge between f%d f%d\n", + facet->id, neighbor->id); + else { + qh printoutvar++; + fprintf (fp, "# ridge between f%d f%d\n", facet->id, neighbor->id); + } + FOREACHvertex_(vertices) { + for(k= 0; k < qh hull_dim; k++) { + if (k != qh DROPdim) + fprintf(fp, "%8.4g ", vertex->point[k]); + } + fprintf (fp, "\n"); + } + if (qh DROPdim >= 0) + fprintf(fp, "3 0 1 2 %8.4g %8.4g %8.4g\n", color[0], color[1], color[2]); + } + qh_setfree(&vertices); + } +} /* printfacet4geom_simplicial */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacetNvertex_nonsimplicial">-</a> + + qh_printfacetNvertex_nonsimplicial( fp, facet, id, format ) + print vertices for an N-d non-simplicial facet + triangulates each ridge to the id +*/ +void qh_printfacetNvertex_nonsimplicial(FILE *fp, facetT *facet, int id, int format) { + vertexT *vertex, **vertexp; + ridgeT *ridge, **ridgep; + + if (facet->visible && qh NEWfacets) + return; + FOREACHridge_(facet->ridges) { + if (format == qh_PRINTtriangles) + fprintf(fp, "%d ", qh hull_dim); + fprintf(fp, "%d ", id); + if ((ridge->top == facet) ^ qh_ORIENTclock) { + FOREACHvertex_(ridge->vertices) + fprintf(fp, "%d ", qh_pointid(vertex->point)); + }else { + FOREACHvertexreverse12_(ridge->vertices) + fprintf(fp, "%d ", qh_pointid(vertex->point)); + } + fprintf(fp, "\n"); + } +} /* printfacetNvertex_nonsimplicial */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacetNvertex_simplicial">-</a> + + qh_printfacetNvertex_simplicial( fp, facet, format ) + print vertices for an N-d simplicial facet + prints vertices for non-simplicial facets + 2-d facets (orientation preserved by qh_mergefacet2d) + PRINToff ('o') for 4-d and higher +*/ +void qh_printfacetNvertex_simplicial(FILE *fp, facetT *facet, int format) { + vertexT *vertex, **vertexp; + + if (format == qh_PRINToff || format == qh_PRINTtriangles) + fprintf (fp, "%d ", qh_setsize (facet->vertices)); + if ((facet->toporient ^ qh_ORIENTclock) + || (qh hull_dim > 2 && !facet->simplicial)) { + FOREACHvertex_(facet->vertices) + fprintf(fp, "%d ", qh_pointid(vertex->point)); + }else { + FOREACHvertexreverse12_(facet->vertices) + fprintf(fp, "%d ", qh_pointid(vertex->point)); + } + fprintf(fp, "\n"); +} /* printfacetNvertex_simplicial */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacetheader">-</a> + + qh_printfacetheader( fp, facet ) + prints header fields of a facet to fp + + notes: + for 'f' output and debugging +*/ +void qh_printfacetheader(FILE *fp, facetT *facet) { + pointT *point, **pointp, *furthest; + facetT *neighbor, **neighborp; + realT dist; + + if (facet == qh_MERGEridge) { + fprintf (fp, " MERGEridge\n"); + return; + }else if (facet == qh_DUPLICATEridge) { + fprintf (fp, " DUPLICATEridge\n"); + return; + }else if (!facet) { + fprintf (fp, " NULLfacet\n"); + return; + } + qh old_randomdist= qh RANDOMdist; + qh RANDOMdist= False; + fprintf(fp, "- f%d\n", facet->id); + fprintf(fp, " - flags:"); + if (facet->toporient) + fprintf(fp, " top"); + else + fprintf(fp, " bottom"); + if (facet->simplicial) + fprintf(fp, " simplicial"); + if (facet->tricoplanar) + fprintf(fp, " tricoplanar"); + if (facet->upperdelaunay) + fprintf(fp, " upperDelaunay"); + if (facet->visible) + fprintf(fp, " visible"); + if (facet->newfacet) + fprintf(fp, " new"); + if (facet->tested) + fprintf(fp, " tested"); + if (!facet->good) + fprintf(fp, " notG"); + if (facet->seen) + fprintf(fp, " seen"); + if (facet->coplanar) + fprintf(fp, " coplanar"); + if (facet->mergehorizon) + fprintf(fp, " mergehorizon"); + if (facet->keepcentrum) + fprintf(fp, " keepcentrum"); + if (facet->dupridge) + fprintf(fp, " dupridge"); + if (facet->mergeridge && !facet->mergeridge2) + fprintf(fp, " mergeridge1"); + if (facet->mergeridge2) + fprintf(fp, " mergeridge2"); + if (facet->newmerge) + fprintf(fp, " newmerge"); + if (facet->flipped) + fprintf(fp, " flipped"); + if (facet->notfurthest) + fprintf(fp, " notfurthest"); + if (facet->degenerate) + fprintf(fp, " degenerate"); + if (facet->redundant) + fprintf(fp, " redundant"); + fprintf(fp, "\n"); + if (facet->isarea) + fprintf(fp, " - area: %2.2g\n", facet->f.area); + else if (qh NEWfacets && facet->visible && facet->f.replace) + fprintf(fp, " - replacement: f%d\n", facet->f.replace->id); + else if (facet->newfacet) { + if (facet->f.samecycle && facet->f.samecycle != facet) + fprintf(fp, " - shares same visible/horizon as f%d\n", facet->f.samecycle->id); + }else if (facet->tricoplanar /* !isarea */) { + if (facet->f.triowner) + fprintf(fp, " - owner of normal & centrum is facet f%d\n", facet->f.triowner->id); + }else if (facet->f.newcycle) + fprintf(fp, " - was horizon to f%d\n", facet->f.newcycle->id); + if (facet->nummerge) + fprintf(fp, " - merges: %d\n", facet->nummerge); + qh_printpointid(fp, " - normal: ", qh hull_dim, facet->normal, -1); + fprintf(fp, " - offset: %10.7g\n", facet->offset); + if (qh CENTERtype == qh_ASvoronoi || facet->center) + qh_printcenter (fp, qh_PRINTfacets, " - center: ", facet); +#if qh_MAXoutside + if (facet->maxoutside > qh DISTround) + fprintf(fp, " - maxoutside: %10.7g\n", facet->maxoutside); +#endif + if (!SETempty_(facet->outsideset)) { + furthest= (pointT*)qh_setlast(facet->outsideset); + if (qh_setsize (facet->outsideset) < 6) { + fprintf(fp, " - outside set (furthest p%d):\n", qh_pointid(furthest)); + FOREACHpoint_(facet->outsideset) + qh_printpoint(fp, " ", point); + }else if (qh_setsize (facet->outsideset) < 21) { + qh_printpoints(fp, " - outside set:", facet->outsideset); + }else { + fprintf(fp, " - outside set: %d points.", qh_setsize(facet->outsideset)); + qh_printpoint(fp, " Furthest", furthest); + } +#if !qh_COMPUTEfurthest + fprintf(fp, " - furthest distance= %2.2g\n", facet->furthestdist); +#endif + } + if (!SETempty_(facet->coplanarset)) { + furthest= (pointT*)qh_setlast(facet->coplanarset); + if (qh_setsize (facet->coplanarset) < 6) { + fprintf(fp, " - coplanar set (furthest p%d):\n", qh_pointid(furthest)); + FOREACHpoint_(facet->coplanarset) + qh_printpoint(fp, " ", point); + }else if (qh_setsize (facet->coplanarset) < 21) { + qh_printpoints(fp, " - coplanar set:", facet->coplanarset); + }else { + fprintf(fp, " - coplanar set: %d points.", qh_setsize(facet->coplanarset)); + qh_printpoint(fp, " Furthest", furthest); + } + zinc_(Zdistio); + qh_distplane (furthest, facet, &dist); + fprintf(fp, " furthest distance= %2.2g\n", dist); + } + qh_printvertices (fp, " - vertices:", facet->vertices); + fprintf(fp, " - neighboring facets: "); + FOREACHneighbor_(facet) { + if (neighbor == qh_MERGEridge) + fprintf(fp, " MERGE"); + else if (neighbor == qh_DUPLICATEridge) + fprintf(fp, " DUP"); + else + fprintf(fp, " f%d", neighbor->id); + } + fprintf(fp, "\n"); + qh RANDOMdist= qh old_randomdist; +} /* printfacetheader */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacetridges">-</a> + + qh_printfacetridges( fp, facet ) + prints ridges of a facet to fp + + notes: + ridges printed in neighbor order + assumes the ridges exist + for 'f' output +*/ +void qh_printfacetridges(FILE *fp, facetT *facet) { + facetT *neighbor, **neighborp; + ridgeT *ridge, **ridgep; + int numridges= 0; + + + if (facet->visible && qh NEWfacets) { + fprintf(fp, " - ridges (ids may be garbage):"); + FOREACHridge_(facet->ridges) + fprintf(fp, " r%d", ridge->id); + fprintf(fp, "\n"); + }else { + fprintf(fp, " - ridges:\n"); + FOREACHridge_(facet->ridges) + ridge->seen= False; + if (qh hull_dim == 3) { + ridge= SETfirstt_(facet->ridges, ridgeT); + while (ridge && !ridge->seen) { + ridge->seen= True; + qh_printridge(fp, ridge); + numridges++; + ridge= qh_nextridge3d (ridge, facet, NULL); + } + }else { + FOREACHneighbor_(facet) { + FOREACHridge_(facet->ridges) { + if (otherfacet_(ridge,facet) == neighbor) { + ridge->seen= True; + qh_printridge(fp, ridge); + numridges++; + } + } + } + } + if (numridges != qh_setsize (facet->ridges)) { + fprintf (fp, " - all ridges:"); + FOREACHridge_(facet->ridges) + fprintf (fp, " r%d", ridge->id); + fprintf (fp, "\n"); + } + FOREACHridge_(facet->ridges) { + if (!ridge->seen) + qh_printridge(fp, ridge); + } + } +} /* printfacetridges */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printfacets">-</a> + + qh_printfacets( fp, format, facetlist, facets, printall ) + prints facetlist and/or facet set in output format + + notes: + also used for specialized formats ('FO' and summary) + turns off 'Rn' option since want actual numbers +*/ +void qh_printfacets(FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall) { + int numfacets, numsimplicial, numridges, totneighbors, numcoplanars, numtricoplanars; + facetT *facet, **facetp; + setT *vertices; + coordT *center; + realT outerplane, innerplane; + + qh old_randomdist= qh RANDOMdist; + qh RANDOMdist= False; + if (qh CDDoutput && (format == qh_PRINTcentrums || format == qh_PRINTpointintersect || format == qh_PRINToff)) + fprintf (qh ferr, "qhull warning: CDD format is not available for centrums, halfspace\nintersections, and OFF file format.\n"); + if (format == qh_PRINTnone) + ; /* print nothing */ + else if (format == qh_PRINTaverage) { + vertices= qh_facetvertices (facetlist, facets, printall); + center= qh_getcenter (vertices); + fprintf (fp, "%d 1\n", qh hull_dim); + qh_printpointid (fp, NULL, qh hull_dim, center, -1); + qh_memfree (center, qh normal_size); + qh_settempfree (&vertices); + }else if (format == qh_PRINTextremes) { + if (qh DELAUNAY) + qh_printextremes_d (fp, facetlist, facets, printall); + else if (qh hull_dim == 2) + qh_printextremes_2d (fp, facetlist, facets, printall); + else + qh_printextremes (fp, facetlist, facets, printall); + }else if (format == qh_PRINToptions) + fprintf(fp, "Options selected for Qhull %s:\n%s\n", qh_version, qh qhull_options); + else if (format == qh_PRINTpoints && !qh VORONOI) + qh_printpoints_out (fp, facetlist, facets, printall); + else if (format == qh_PRINTqhull) + fprintf (fp, "%s | %s\n", qh rbox_command, qh qhull_command); + else if (format == qh_PRINTsize) { + fprintf (fp, "0\n2 "); + fprintf (fp, qh_REAL_1, qh totarea); + fprintf (fp, qh_REAL_1, qh totvol); + fprintf (fp, "\n"); + }else if (format == qh_PRINTsummary) { + qh_countfacets (facetlist, facets, printall, &numfacets, &numsimplicial, + &totneighbors, &numridges, &numcoplanars, &numtricoplanars); + vertices= qh_facetvertices (facetlist, facets, printall); + fprintf (fp, "10 %d %d %d %d %d %d %d %d %d %d\n2 ", qh hull_dim, + qh num_points + qh_setsize (qh other_points), + qh num_vertices, qh num_facets - qh num_visible, + qh_setsize (vertices), numfacets, numcoplanars, + numfacets - numsimplicial, zzval_(Zdelvertextot), + numtricoplanars); + qh_settempfree (&vertices); + qh_outerinner (NULL, &outerplane, &innerplane); + fprintf (fp, qh_REAL_2n, outerplane, innerplane); + }else if (format == qh_PRINTvneighbors) + qh_printvneighbors (fp, facetlist, facets, printall); + else if (qh VORONOI && format == qh_PRINToff) + qh_printvoronoi (fp, format, facetlist, facets, printall); + else if (qh VORONOI && format == qh_PRINTgeom) { + qh_printbegin (fp, format, facetlist, facets, printall); + qh_printvoronoi (fp, format, facetlist, facets, printall); + qh_printend (fp, format, facetlist, facets, printall); + }else if (qh VORONOI + && (format == qh_PRINTvertices || format == qh_PRINTinner || format == qh_PRINTouter)) + qh_printvdiagram (fp, format, facetlist, facets, printall); + else { + qh_printbegin (fp, format, facetlist, facets, printall); + FORALLfacet_(facetlist) + qh_printafacet (fp, format, facet, printall); + FOREACHfacet_(facets) + qh_printafacet (fp, format, facet, printall); + qh_printend (fp, format, facetlist, facets, printall); + } + qh RANDOMdist= qh old_randomdist; +} /* printfacets */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printhelp_degenerate">-</a> + + qh_printhelp_degenerate( fp ) + prints descriptive message for precision error + + notes: + no message if qh_QUICKhelp +*/ +void qh_printhelp_degenerate(FILE *fp) { + + if (qh MERGEexact || qh PREmerge || qh JOGGLEmax < REALmax/2) + fprintf(fp, "\n\ +A Qhull error has occurred. Qhull should have corrected the above\n\ +precision error. Please send the input and all of the output to\n\ +qhull_bug@qhull.org\n"); + else if (!qh_QUICKhelp) { + fprintf(fp, "\n\ +Precision problems were detected during construction of the convex hull.\n\ +This occurs because convex hull algorithms assume that calculations are\n\ +exact, but floating-point arithmetic has roundoff errors.\n\ +\n\ +To correct for precision problems, do not use 'Q0'. By default, Qhull\n\ +selects 'C-0' or 'Qx' and merges non-convex facets. With option 'QJ',\n\ +Qhull joggles the input to prevent precision problems. See \"Imprecision\n\ +in Qhull\" (qh-impre.htm).\n\ +\n\ +If you use 'Q0', the output may include\n\ +coplanar ridges, concave ridges, and flipped facets. In 4-d and higher,\n\ +Qhull may produce a ridge with four neighbors or two facets with the same \n\ +vertices. Qhull reports these events when they occur. It stops when a\n\ +concave ridge, flipped facet, or duplicate facet occurs.\n"); +#if REALfloat + fprintf (fp, "\ +\n\ +Qhull is currently using single precision arithmetic. The following\n\ +will probably remove the precision problems:\n\ + - recompile qhull for double precision (#define REALfloat 0 in user.h).\n"); +#endif + if (qh DELAUNAY && !qh SCALElast && qh MAXabs_coord > 1e4) + fprintf( fp, "\ +\n\ +When computing the Delaunay triangulation of coordinates > 1.0,\n\ + - use 'Qbb' to scale the last coordinate to [0,m] (max previous coordinate)\n"); + if (qh DELAUNAY && !qh ATinfinity) + fprintf( fp, "\ +When computing the Delaunay triangulation:\n\ + - use 'Qz' to add a point at-infinity. This reduces precision problems.\n"); + + fprintf(fp, "\ +\n\ +If you need triangular output:\n\ + - use option 'Qt' to triangulate the output\n\ + - use option 'QJ' to joggle the input points and remove precision errors\n\ + - use option 'Ft'. It triangulates non-simplicial facets with added points.\n\ +\n\ +If you must use 'Q0',\n\ +try one or more of the following options. They can not guarantee an output.\n\ + - use 'QbB' to scale the input to a cube.\n\ + - use 'Po' to produce output and prevent partitioning for flipped facets\n\ + - use 'V0' to set min. distance to visible facet as 0 instead of roundoff\n\ + - use 'En' to specify a maximum roundoff error less than %2.2g.\n\ + - options 'Qf', 'Qbb', and 'QR0' may also help\n", + qh DISTround); + fprintf(fp, "\ +\n\ +To guarantee simplicial output:\n\ + - use option 'Qt' to triangulate the output\n\ + - use option 'QJ' to joggle the input points and remove precision errors\n\ + - use option 'Ft' to triangulate the output by adding points\n\ + - use exact arithmetic (see \"Imprecision in Qhull\", qh-impre.htm)\n\ +"); + } +} /* printhelp_degenerate */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printhelp_singular">-</a> + + qh_printhelp_singular( fp ) + prints descriptive message for singular input +*/ +void qh_printhelp_singular(FILE *fp) { + facetT *facet; + vertexT *vertex, **vertexp; + realT min, max, *coord, dist; + int i,k; + + fprintf(fp, "\n\ +The input to qhull appears to be less than %d dimensional, or a\n\ +computation has overflowed.\n\n\ +Qhull could not construct a clearly convex simplex from points:\n", + qh hull_dim); + qh_printvertexlist (fp, "", qh facet_list, NULL, qh_ALL); + if (!qh_QUICKhelp) + fprintf(fp, "\n\ +The center point is coplanar with a facet, or a vertex is coplanar\n\ +with a neighboring facet. The maximum round off error for\n\ +computing distances is %2.2g. The center point, facets and distances\n\ +to the center point are as follows:\n\n", qh DISTround); + qh_printpointid (fp, "center point", qh hull_dim, qh interior_point, -1); + fprintf (fp, "\n"); + FORALLfacets { + fprintf (fp, "facet"); + FOREACHvertex_(facet->vertices) + fprintf (fp, " p%d", qh_pointid(vertex->point)); + zinc_(Zdistio); + qh_distplane(qh interior_point, facet, &dist); + fprintf (fp, " distance= %4.2g\n", dist); + } + if (!qh_QUICKhelp) { + if (qh HALFspace) + fprintf (fp, "\n\ +These points are the dual of the given halfspaces. They indicate that\n\ +the intersection is degenerate.\n"); + fprintf (fp,"\n\ +These points either have a maximum or minimum x-coordinate, or\n\ +they maximize the determinant for k coordinates. Trial points\n\ +are first selected from points that maximize a coordinate.\n"); + if (qh hull_dim >= qh_INITIALmax) + fprintf (fp, "\n\ +Because of the high dimension, the min x-coordinate and max-coordinate\n\ +points are used if the determinant is non-zero. Option 'Qs' will\n\ +do a better, though much slower, job. Instead of 'Qs', you can change\n\ +the points by randomly rotating the input with 'QR0'.\n"); + } + fprintf (fp, "\nThe min and max coordinates for each dimension are:\n"); + for (k=0; k < qh hull_dim; k++) { + min= REALmax; + max= -REALmin; + for (i=qh num_points, coord= qh first_point+k; i--; coord += qh hull_dim) { + maximize_(max, *coord); + minimize_(min, *coord); + } + fprintf (fp, " %d: %8.4g %8.4g difference= %4.4g\n", k, min, max, max-min); + } + if (!qh_QUICKhelp) { + fprintf (fp, "\n\ +If the input should be full dimensional, you have several options that\n\ +may determine an initial simplex:\n\ + - use 'QJ' to joggle the input and make it full dimensional\n\ + - use 'QbB' to scale the points to the unit cube\n\ + - use 'QR0' to randomly rotate the input for different maximum points\n\ + - use 'Qs' to search all points for the initial simplex\n\ + - use 'En' to specify a maximum roundoff error less than %2.2g.\n\ + - trace execution with 'T3' to see the determinant for each point.\n", + qh DISTround); +#if REALfloat + fprintf (fp, "\ + - recompile qhull for double precision (#define REALfloat 0 in qhull.h).\n"); +#endif + fprintf (fp, "\n\ +If the input is lower dimensional:\n\ + - use 'QJ' to joggle the input and make it full dimensional\n\ + - use 'Qbk:0Bk:0' to delete coordinate k from the input. You should\n\ + pick the coordinate with the least range. The hull will have the\n\ + correct topology.\n\ + - determine the flat containing the points, rotate the points\n\ + into a coordinate plane, and delete the other coordinates.\n\ + - add one or more points to make the input full dimensional.\n\ +"); + if (qh DELAUNAY && !qh ATinfinity) + fprintf (fp, "\n\n\ +This is a Delaunay triangulation and the input is co-circular or co-spherical:\n\ + - use 'Qz' to add a point \"at infinity\" (i.e., above the paraboloid)\n\ + - or use 'QJ' to joggle the input and avoid co-circular data\n"); + } +} /* printhelp_singular */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printhyperplaneintersection">-</a> + + qh_printhyperplaneintersection( fp, facet1, facet2, vertices, color ) + print Geomview OFF or 4OFF for the intersection of two hyperplanes in 3-d or 4-d +*/ +void qh_printhyperplaneintersection(FILE *fp, facetT *facet1, facetT *facet2, + setT *vertices, realT color[3]) { + realT costheta, denominator, dist1, dist2, s, t, mindenom, p[4]; + vertexT *vertex, **vertexp; + int i, k; + boolT nearzero1, nearzero2; + + costheta= qh_getangle(facet1->normal, facet2->normal); + denominator= 1 - costheta * costheta; + i= qh_setsize(vertices); + if (qh hull_dim == 3) + fprintf(fp, "VECT 1 %d 1 %d 1 ", i, i); + else if (qh hull_dim == 4 && qh DROPdim >= 0) + fprintf(fp, "OFF 3 1 1 "); + else + qh printoutvar++; + fprintf (fp, "# intersect f%d f%d\n", facet1->id, facet2->id); + mindenom= 1 / (10.0 * qh MAXabs_coord); + FOREACHvertex_(vertices) { + zadd_(Zdistio, 2); + qh_distplane(vertex->point, facet1, &dist1); + qh_distplane(vertex->point, facet2, &dist2); + s= qh_divzero (-dist1 + costheta * dist2, denominator,mindenom,&nearzero1); + t= qh_divzero (-dist2 + costheta * dist1, denominator,mindenom,&nearzero2); + if (nearzero1 || nearzero2) + s= t= 0.0; + for(k= qh hull_dim; k--; ) + p[k]= vertex->point[k] + facet1->normal[k] * s + facet2->normal[k] * t; + if (qh PRINTdim <= 3) { + qh_projectdim3 (p, p); + fprintf(fp, "%8.4g %8.4g %8.4g # ", p[0], p[1], p[2]); + }else + fprintf(fp, "%8.4g %8.4g %8.4g %8.4g # ", p[0], p[1], p[2], p[3]); + if (nearzero1+nearzero2) + fprintf (fp, "p%d (coplanar facets)\n", qh_pointid (vertex->point)); + else + fprintf (fp, "projected p%d\n", qh_pointid (vertex->point)); + } + if (qh hull_dim == 3) + fprintf(fp, "%8.4g %8.4g %8.4g 1.0\n", color[0], color[1], color[2]); + else if (qh hull_dim == 4 && qh DROPdim >= 0) + fprintf(fp, "3 0 1 2 %8.4g %8.4g %8.4g 1.0\n", color[0], color[1], color[2]); +} /* printhyperplaneintersection */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printline3geom">-</a> + + qh_printline3geom( fp, pointA, pointB, color ) + prints a line as a VECT + prints 0's for qh.DROPdim + + notes: + if pointA == pointB, + it's a 1 point VECT +*/ +void qh_printline3geom (FILE *fp, pointT *pointA, pointT *pointB, realT color[3]) { + int k; + realT pA[4], pB[4]; + + qh_projectdim3(pointA, pA); + qh_projectdim3(pointB, pB); + if ((fabs(pA[0] - pB[0]) > 1e-3) || + (fabs(pA[1] - pB[1]) > 1e-3) || + (fabs(pA[2] - pB[2]) > 1e-3)) { + fprintf (fp, "VECT 1 2 1 2 1\n"); + for (k= 0; k < 3; k++) + fprintf (fp, "%8.4g ", pB[k]); + fprintf (fp, " # p%d\n", qh_pointid (pointB)); + }else + fprintf (fp, "VECT 1 1 1 1 1\n"); + for (k=0; k < 3; k++) + fprintf (fp, "%8.4g ", pA[k]); + fprintf (fp, " # p%d\n", qh_pointid (pointA)); + fprintf (fp, "%8.4g %8.4g %8.4g 1\n", color[0], color[1], color[2]); +} + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printneighborhood">-</a> + + qh_printneighborhood( fp, format, facetA, facetB, printall ) + print neighborhood of one or two facets + + notes: + calls qh_findgood_all() + bumps qh.visit_id +*/ +void qh_printneighborhood (FILE *fp, int format, facetT *facetA, facetT *facetB, boolT printall) { + facetT *neighbor, **neighborp, *facet; + setT *facets; + + if (format == qh_PRINTnone) + return; + qh_findgood_all (qh facet_list); + if (facetA == facetB) + facetB= NULL; + facets= qh_settemp (2*(qh_setsize (facetA->neighbors)+1)); + qh visit_id++; + for (facet= facetA; facet; facet= ((facet == facetA) ? facetB : NULL)) { + if (facet->visitid != qh visit_id) { + facet->visitid= qh visit_id; + qh_setappend (&facets, facet); + } + FOREACHneighbor_(facet) { + if (neighbor->visitid == qh visit_id) + continue; + neighbor->visitid= qh visit_id; + if (printall || !qh_skipfacet (neighbor)) + qh_setappend (&facets, neighbor); + } + } + qh_printfacets (fp, format, NULL, facets, printall); + qh_settempfree (&facets); +} /* printneighborhood */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printpoint">-</a> + + qh_printpoint( fp, string, point ) + qh_printpointid( fp, string, dim, point, id ) + prints the coordinates of a point + + returns: + if string is defined + prints 'string p%d' (skips p%d if id=-1) + + notes: + nop if point is NULL + prints id unless it is undefined (-1) +*/ +void qh_printpoint(FILE *fp, char *string, pointT *point) { + int id= qh_pointid( point); + + qh_printpointid( fp, string, qh hull_dim, point, id); +} /* printpoint */ + +void qh_printpointid(FILE *fp, char *string, int dim, pointT *point, int id) { + int k; + realT r; /*bug fix*/ + + if (!point) + return; + if (string) { + fputs (string, fp); + if (id != -1) + fprintf(fp, " p%d: ", id); + } + for(k= dim; k--; ) { + r= *point++; + if (string) + fprintf(fp, " %8.4g", r); + else + fprintf(fp, qh_REAL_1, r); + } + fprintf(fp, "\n"); +} /* printpointid */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printpoint3">-</a> + + qh_printpoint3( fp, point ) + prints 2-d, 3-d, or 4-d point as Geomview 3-d coordinates +*/ +void qh_printpoint3 (FILE *fp, pointT *point) { + int k; + realT p[4]; + + qh_projectdim3 (point, p); + for (k=0; k < 3; k++) + fprintf (fp, "%8.4g ", p[k]); + fprintf (fp, " # p%d\n", qh_pointid (point)); +} /* printpoint3 */ + +/*---------------------------------------- +-printpoints- print pointids for a set of points starting at index + see geom.c +*/ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printpoints_out">-</a> + + qh_printpoints_out( fp, facetlist, facets, printall ) + prints vertices, coplanar/inside points, for facets by their point coordinates + allows qh.CDDoutput + + notes: + same format as qhull input + if no coplanar/interior points, + same order as qh_printextremes +*/ +void qh_printpoints_out (FILE *fp, facetT *facetlist, setT *facets, int printall) { + int allpoints= qh num_points + qh_setsize (qh other_points); + int numpoints=0, point_i, point_n; + setT *vertices, *points; + facetT *facet, **facetp; + pointT *point, **pointp; + vertexT *vertex, **vertexp; + int id; + + points= qh_settemp (allpoints); + qh_setzero (points, 0, allpoints); + vertices= qh_facetvertices (facetlist, facets, printall); + FOREACHvertex_(vertices) { + id= qh_pointid (vertex->point); + if (id >= 0) + SETelem_(points, id)= vertex->point; + } + if (qh KEEPinside || qh KEEPcoplanar || qh KEEPnearinside) { + FORALLfacet_(facetlist) { + if (!printall && qh_skipfacet(facet)) + continue; + FOREACHpoint_(facet->coplanarset) { + id= qh_pointid (point); + if (id >= 0) + SETelem_(points, id)= point; + } + } + FOREACHfacet_(facets) { + if (!printall && qh_skipfacet(facet)) + continue; + FOREACHpoint_(facet->coplanarset) { + id= qh_pointid (point); + if (id >= 0) + SETelem_(points, id)= point; + } + } + } + qh_settempfree (&vertices); + FOREACHpoint_i_(points) { + if (point) + numpoints++; + } + if (qh CDDoutput) + fprintf (fp, "%s | %s\nbegin\n%d %d real\n", qh rbox_command, + qh qhull_command, numpoints, qh hull_dim + 1); + else + fprintf (fp, "%d\n%d\n", qh hull_dim, numpoints); + FOREACHpoint_i_(points) { + if (point) { + if (qh CDDoutput) + fprintf (fp, "1 "); + qh_printpoint (fp, NULL, point); + } + } + if (qh CDDoutput) + fprintf (fp, "end\n"); + qh_settempfree (&points); +} /* printpoints_out */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printpointvect">-</a> + + qh_printpointvect( fp, point, normal, center, radius, color ) + prints a 2-d, 3-d, or 4-d point as 3-d VECT's relative to normal or to center point +*/ +void qh_printpointvect (FILE *fp, pointT *point, coordT *normal, pointT *center, realT radius, realT color[3]) { + realT diff[4], pointA[4]; + int k; + + for (k= qh hull_dim; k--; ) { + if (center) + diff[k]= point[k]-center[k]; + else if (normal) + diff[k]= normal[k]; + else + diff[k]= 0; + } + if (center) + qh_normalize2 (diff, qh hull_dim, True, NULL, NULL); + for (k= qh hull_dim; k--; ) + pointA[k]= point[k]+diff[k] * radius; + qh_printline3geom (fp, point, pointA, color); +} /* printpointvect */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printpointvect2">-</a> + + qh_printpointvect2( fp, point, normal, center, radius ) + prints a 2-d, 3-d, or 4-d point as 2 3-d VECT's for an imprecise point +*/ +void qh_printpointvect2 (FILE *fp, pointT *point, coordT *normal, pointT *center, realT radius) { + realT red[3]={1, 0, 0}, yellow[3]={1, 1, 0}; + + qh_printpointvect (fp, point, normal, center, radius, red); + qh_printpointvect (fp, point, normal, center, -radius, yellow); +} /* printpointvect2 */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printridge">-</a> + + qh_printridge( fp, ridge ) + prints the information in a ridge + + notes: + for qh_printfacetridges() +*/ +void qh_printridge(FILE *fp, ridgeT *ridge) { + + fprintf(fp, " - r%d", ridge->id); + if (ridge->tested) + fprintf (fp, " tested"); + if (ridge->nonconvex) + fprintf (fp, " nonconvex"); + fprintf (fp, "\n"); + qh_printvertices (fp, " vertices:", ridge->vertices); + if (ridge->top && ridge->bottom) + fprintf(fp, " between f%d and f%d\n", + ridge->top->id, ridge->bottom->id); +} /* printridge */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printspheres">-</a> + + qh_printspheres( fp, vertices, radius ) + prints 3-d vertices as OFF spheres + + notes: + inflated octahedron from Stuart Levy earth/mksphere2 +*/ +void qh_printspheres(FILE *fp, setT *vertices, realT radius) { + vertexT *vertex, **vertexp; + + qh printoutnum++; + fprintf (fp, "{appearance {-edge -normal normscale 0} {\n\ +INST geom {define vsphere OFF\n\ +18 32 48\n\ +\n\ +0 0 1\n\ +1 0 0\n\ +0 1 0\n\ +-1 0 0\n\ +0 -1 0\n\ +0 0 -1\n\ +0.707107 0 0.707107\n\ +0 -0.707107 0.707107\n\ +0.707107 -0.707107 0\n\ +-0.707107 0 0.707107\n\ +-0.707107 -0.707107 0\n\ +0 0.707107 0.707107\n\ +-0.707107 0.707107 0\n\ +0.707107 0.707107 0\n\ +0.707107 0 -0.707107\n\ +0 0.707107 -0.707107\n\ +-0.707107 0 -0.707107\n\ +0 -0.707107 -0.707107\n\ +\n\ +3 0 6 11\n\ +3 0 7 6 \n\ +3 0 9 7 \n\ +3 0 11 9\n\ +3 1 6 8 \n\ +3 1 8 14\n\ +3 1 13 6\n\ +3 1 14 13\n\ +3 2 11 13\n\ +3 2 12 11\n\ +3 2 13 15\n\ +3 2 15 12\n\ +3 3 9 12\n\ +3 3 10 9\n\ +3 3 12 16\n\ +3 3 16 10\n\ +3 4 7 10\n\ +3 4 8 7\n\ +3 4 10 17\n\ +3 4 17 8\n\ +3 5 14 17\n\ +3 5 15 14\n\ +3 5 16 15\n\ +3 5 17 16\n\ +3 6 13 11\n\ +3 7 8 6\n\ +3 9 10 7\n\ +3 11 12 9\n\ +3 14 8 17\n\ +3 15 13 14\n\ +3 16 12 15\n\ +3 17 10 16\n} transforms { TLIST\n"); + FOREACHvertex_(vertices) { + fprintf(fp, "%8.4g 0 0 0 # v%d\n 0 %8.4g 0 0\n0 0 %8.4g 0\n", + radius, vertex->id, radius, radius); + qh_printpoint3 (fp, vertex->point); + fprintf (fp, "1\n"); + } + fprintf (fp, "}}}\n"); +} /* printspheres */ + + +/*---------------------------------------------- +-printsummary- + see qhull.c +*/ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvdiagram">-</a> + + qh_printvdiagram( fp, format, facetlist, facets, printall ) + print voronoi diagram + # of pairs of input sites + #indices site1 site2 vertex1 ... + + sites indexed by input point id + point 0 is the first input point + vertices indexed by 'o' and 'p' order + vertex 0 is the 'vertex-at-infinity' + vertex 1 is the first Voronoi vertex + + see: + qh_printvoronoi() + qh_eachvoronoi_all() + + notes: + if all facets are upperdelaunay, + prints upper hull (furthest-site Voronoi diagram) +*/ +void qh_printvdiagram (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall) { + setT *vertices; + int totcount, numcenters; + boolT islower; + qh_RIDGE innerouter= qh_RIDGEall; + printvridgeT printvridge= NULL; + + if (format == qh_PRINTvertices) { + innerouter= qh_RIDGEall; + printvridge= qh_printvridge; + }else if (format == qh_PRINTinner) { + innerouter= qh_RIDGEinner; + printvridge= qh_printvnorm; + }else if (format == qh_PRINTouter) { + innerouter= qh_RIDGEouter; + printvridge= qh_printvnorm; + }else { + fprintf(qh ferr, "qh_printvdiagram: unknown print format %d.\n", format); + qh_errexit (qh_ERRinput, NULL, NULL); + } + vertices= qh_markvoronoi (facetlist, facets, printall, &islower, &numcenters); + totcount= qh_printvdiagram2 (NULL, NULL, vertices, innerouter, False); + fprintf (fp, "%d\n", totcount); + totcount= qh_printvdiagram2 (fp, printvridge, vertices, innerouter, True /* inorder*/); + qh_settempfree (&vertices); +#if 0 /* for testing qh_eachvoronoi_all */ + fprintf (fp, "\n"); + totcount= qh_eachvoronoi_all(fp, printvridge, qh UPPERdelaunay, innerouter, True /* inorder*/); + fprintf (fp, "%d\n", totcount); +#endif +} /* printvdiagram */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvdiagram2">-</a> + + qh_printvdiagram2( fp, printvridge, vertices, innerouter, inorder ) + visit all pairs of input sites (vertices) for selected Voronoi vertices + vertices may include NULLs + + innerouter: + qh_RIDGEall print inner ridges (bounded) and outer ridges (unbounded) + qh_RIDGEinner print only inner ridges + qh_RIDGEouter print only outer ridges + + inorder: + print 3-d Voronoi vertices in order + + assumes: + qh_markvoronoi marked facet->visitid for Voronoi vertices + all facet->seen= False + all facet->seen2= True + + returns: + total number of Voronoi ridges + if printvridge, + calls printvridge( fp, vertex, vertexA, centers) for each ridge + [see qh_eachvoronoi()] + + see: + qh_eachvoronoi_all() +*/ +int qh_printvdiagram2 (FILE *fp, printvridgeT printvridge, setT *vertices, qh_RIDGE innerouter, boolT inorder) { + int totcount= 0; + int vertex_i, vertex_n; + vertexT *vertex; + + FORALLvertices + vertex->seen= False; + FOREACHvertex_i_(vertices) { + if (vertex) { + if (qh GOODvertex > 0 && qh_pointid(vertex->point)+1 != qh GOODvertex) + continue; + totcount += qh_eachvoronoi (fp, printvridge, vertex, !qh_ALL, innerouter, inorder); + } + } + return totcount; +} /* printvdiagram2 */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvertex">-</a> + + qh_printvertex( fp, vertex ) + prints the information in a vertex +*/ +void qh_printvertex(FILE *fp, vertexT *vertex) { + pointT *point; + int k, count= 0; + facetT *neighbor, **neighborp; + realT r; /*bug fix*/ + + if (!vertex) { + fprintf (fp, " NULLvertex\n"); + return; + } + fprintf(fp, "- p%d (v%d):", qh_pointid(vertex->point), vertex->id); + point= vertex->point; + if (point) { + for(k= qh hull_dim; k--; ) { + r= *point++; + fprintf(fp, " %5.2g", r); + } + } + if (vertex->deleted) + fprintf(fp, " deleted"); + if (vertex->delridge) + fprintf (fp, " ridgedeleted"); + fprintf(fp, "\n"); + if (vertex->neighbors) { + fprintf(fp, " neighbors:"); + FOREACHneighbor_(vertex) { + if (++count % 100 == 0) + fprintf (fp, "\n "); + fprintf(fp, " f%d", neighbor->id); + } + fprintf(fp, "\n"); + } +} /* printvertex */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvertexlist">-</a> + + qh_printvertexlist( fp, string, facetlist, facets, printall ) + prints vertices used by a facetlist or facet set + tests qh_skipfacet() if !printall +*/ +void qh_printvertexlist (FILE *fp, char* string, facetT *facetlist, + setT *facets, boolT printall) { + vertexT *vertex, **vertexp; + setT *vertices; + + vertices= qh_facetvertices (facetlist, facets, printall); + fputs (string, fp); + FOREACHvertex_(vertices) + qh_printvertex(fp, vertex); + qh_settempfree (&vertices); +} /* printvertexlist */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvertices">-</a> + + qh_printvertices( fp, string, vertices ) + prints vertices in a set +*/ +void qh_printvertices(FILE *fp, char* string, setT *vertices) { + vertexT *vertex, **vertexp; + + fputs (string, fp); + FOREACHvertex_(vertices) + fprintf (fp, " p%d (v%d)", qh_pointid(vertex->point), vertex->id); + fprintf(fp, "\n"); +} /* printvertices */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvneighbors">-</a> + + qh_printvneighbors( fp, facetlist, facets, printall ) + print vertex neighbors of vertices in facetlist and facets ('FN') + + notes: + qh_countfacets clears facet->visitid for non-printed facets + + design: + collect facet count and related statistics + if necessary, build neighbor sets for each vertex + collect vertices in facetlist and facets + build a point array for point->vertex and point->coplanar facet + for each point + list vertex neighbors or coplanar facet +*/ +void qh_printvneighbors (FILE *fp, facetT* facetlist, setT *facets, boolT printall) { + int numfacets, numsimplicial, numridges, totneighbors, numneighbors, numcoplanars, numtricoplanars; + setT *vertices, *vertex_points, *coplanar_points; + int numpoints= qh num_points + qh_setsize (qh other_points); + vertexT *vertex, **vertexp; + int vertex_i, vertex_n; + facetT *facet, **facetp, *neighbor, **neighborp; + pointT *point, **pointp; + + qh_countfacets (facetlist, facets, printall, &numfacets, &numsimplicial, + &totneighbors, &numridges, &numcoplanars, &numtricoplanars); /* sets facet->visitid */ + fprintf (fp, "%d\n", numpoints); + qh_vertexneighbors(); + vertices= qh_facetvertices (facetlist, facets, printall); + vertex_points= qh_settemp (numpoints); + coplanar_points= qh_settemp (numpoints); + qh_setzero (vertex_points, 0, numpoints); + qh_setzero (coplanar_points, 0, numpoints); + FOREACHvertex_(vertices) + qh_point_add (vertex_points, vertex->point, vertex); + FORALLfacet_(facetlist) { + FOREACHpoint_(facet->coplanarset) + qh_point_add (coplanar_points, point, facet); + } + FOREACHfacet_(facets) { + FOREACHpoint_(facet->coplanarset) + qh_point_add (coplanar_points, point, facet); + } + FOREACHvertex_i_(vertex_points) { + if (vertex) { + numneighbors= qh_setsize (vertex->neighbors); + fprintf (fp, "%d", numneighbors); + if (qh hull_dim == 3) + qh_order_vertexneighbors (vertex); + else if (qh hull_dim >= 4) + qsort (SETaddr_(vertex->neighbors, facetT), numneighbors, + sizeof (facetT *), qh_compare_facetvisit); + FOREACHneighbor_(vertex) + fprintf (fp, " %d", + neighbor->visitid ? neighbor->visitid - 1 : - neighbor->id); + fprintf (fp, "\n"); + }else if ((facet= SETelemt_(coplanar_points, vertex_i, facetT))) + fprintf (fp, "1 %d\n", + facet->visitid ? facet->visitid - 1 : - facet->id); + else + fprintf (fp, "0\n"); + } + qh_settempfree (&coplanar_points); + qh_settempfree (&vertex_points); + qh_settempfree (&vertices); +} /* printvneighbors */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvoronoi">-</a> + + qh_printvoronoi( fp, format, facetlist, facets, printall ) + print voronoi diagram in 'o' or 'G' format + for 'o' format + prints voronoi centers for each facet and for infinity + for each vertex, lists ids of printed facets or infinity + assumes facetlist and facets are disjoint + for 'G' format + prints an OFF object + adds a 0 coordinate to center + prints infinity but does not list in vertices + + see: + qh_printvdiagram() + + notes: + if 'o', + prints a line for each point except "at-infinity" + if all facets are upperdelaunay, + reverses lower and upper hull +*/ +void qh_printvoronoi (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall) { + int k, numcenters, numvertices= 0, numneighbors, numinf, vid=1, vertex_i, vertex_n; + facetT *facet, **facetp, *neighbor, **neighborp; + setT *vertices; + vertexT *vertex; + boolT islower; + unsigned int numfacets= (unsigned int) qh num_facets; + + vertices= qh_markvoronoi (facetlist, facets, printall, &islower, &numcenters); + FOREACHvertex_i_(vertices) { + if (vertex) { + numvertices++; + numneighbors = numinf = 0; + FOREACHneighbor_(vertex) { + if (neighbor->visitid == 0) + numinf= 1; + else if (neighbor->visitid < numfacets) + numneighbors++; + } + if (numinf && !numneighbors) { + SETelem_(vertices, vertex_i)= NULL; + numvertices--; + } + } + } + if (format == qh_PRINTgeom) + fprintf (fp, "{appearance {+edge -face} OFF %d %d 1 # Voronoi centers and cells\n", + numcenters, numvertices); + else + fprintf (fp, "%d\n%d %d 1\n", qh hull_dim-1, numcenters, qh_setsize(vertices)); + if (format == qh_PRINTgeom) { + for (k= qh hull_dim-1; k--; ) + fprintf (fp, qh_REAL_1, 0.0); + fprintf (fp, " 0 # infinity not used\n"); + }else { + for (k= qh hull_dim-1; k--; ) + fprintf (fp, qh_REAL_1, qh_INFINITE); + fprintf (fp, "\n"); + } + FORALLfacet_(facetlist) { + if (facet->visitid && facet->visitid < numfacets) { + if (format == qh_PRINTgeom) + fprintf (fp, "# %d f%d\n", vid++, facet->id); + qh_printcenter (fp, format, NULL, facet); + } + } + FOREACHfacet_(facets) { + if (facet->visitid && facet->visitid < numfacets) { + if (format == qh_PRINTgeom) + fprintf (fp, "# %d f%d\n", vid++, facet->id); + qh_printcenter (fp, format, NULL, facet); + } + } + FOREACHvertex_i_(vertices) { + numneighbors= 0; + numinf=0; + if (vertex) { + if (qh hull_dim == 3) + qh_order_vertexneighbors(vertex); + else if (qh hull_dim >= 4) + qsort (SETaddr_(vertex->neighbors, vertexT), + qh_setsize (vertex->neighbors), + sizeof (facetT *), qh_compare_facetvisit); + FOREACHneighbor_(vertex) { + if (neighbor->visitid == 0) + numinf= 1; + else if (neighbor->visitid < numfacets) + numneighbors++; + } + } + if (format == qh_PRINTgeom) { + if (vertex) { + fprintf (fp, "%d", numneighbors); + if (vertex) { + FOREACHneighbor_(vertex) { + if (neighbor->visitid && neighbor->visitid < numfacets) + fprintf (fp, " %d", neighbor->visitid); + } + } + fprintf (fp, " # p%d (v%d)\n", vertex_i, vertex->id); + }else + fprintf (fp, " # p%d is coplanar or isolated\n", vertex_i); + }else { + if (numinf) + numneighbors++; + fprintf (fp, "%d", numneighbors); + if (vertex) { + FOREACHneighbor_(vertex) { + if (neighbor->visitid == 0) { + if (numinf) { + numinf= 0; + fprintf (fp, " %d", neighbor->visitid); + } + }else if (neighbor->visitid < numfacets) + fprintf (fp, " %d", neighbor->visitid); + } + } + fprintf (fp, "\n"); + } + } + if (format == qh_PRINTgeom) + fprintf (fp, "}\n"); + qh_settempfree (&vertices); +} /* printvoronoi */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvnorm">-</a> + + qh_printvnorm( fp, vertex, vertexA, centers, unbounded ) + print one separating plane of the Voronoi diagram for a pair of input sites + unbounded==True if centers includes vertex-at-infinity + + assumes: + qh_ASvoronoi and qh_vertexneighbors() already set + + see: + qh_printvdiagram() + qh_eachvoronoi() +*/ +void qh_printvnorm (FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded) { + pointT *normal; + realT offset; + int k; + + normal= qh_detvnorm (vertex, vertexA, centers, &offset); + fprintf (fp, "%d %d %d ", + 2+qh hull_dim, qh_pointid (vertex->point), qh_pointid (vertexA->point)); + for (k= 0; k< qh hull_dim-1; k++) + fprintf (fp, qh_REAL_1, normal[k]); + fprintf (fp, qh_REAL_1, offset); + fprintf (fp, "\n"); +} /* printvnorm */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="printvridge">-</a> + + qh_printvridge( fp, vertex, vertexA, centers, unbounded ) + print one ridge of the Voronoi diagram for a pair of input sites + unbounded==True if centers includes vertex-at-infinity + + see: + qh_printvdiagram() + + notes: + the user may use a different function +*/ +void qh_printvridge (FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded) { + facetT *facet, **facetp; + + fprintf (fp, "%d %d %d", qh_setsize (centers)+2, + qh_pointid (vertex->point), qh_pointid (vertexA->point)); + FOREACHfacet_(centers) + fprintf (fp, " %d", facet->visitid); + fprintf (fp, "\n"); +} /* printvridge */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="projectdim3">-</a> + + qh_projectdim3( source, destination ) + project 2-d 3-d or 4-d point to a 3-d point + uses qh.DROPdim and qh.hull_dim + source and destination may be the same + + notes: + allocate 4 elements to destination just in case +*/ +void qh_projectdim3 (pointT *source, pointT *destination) { + int i,k; + + for (k= 0, i=0; k < qh hull_dim; k++) { + if (qh hull_dim == 4) { + if (k != qh DROPdim) + destination[i++]= source[k]; + }else if (k == qh DROPdim) + destination[i++]= 0; + else + destination[i++]= source[k]; + } + while (i < 3) + destination[i++]= 0.0; +} /* projectdim3 */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="readfeasible">-</a> + + qh_readfeasible( dim, remainder ) + read feasible point from remainder string and qh.fin + + returns: + number of lines read from qh.fin + sets qh.FEASIBLEpoint with malloc'd coordinates + + notes: + checks for qh.HALFspace + assumes dim > 1 + + see: + qh_setfeasible +*/ +int qh_readfeasible (int dim, char *remainder) { + boolT isfirst= True; + int linecount= 0, tokcount= 0; + char *s, *t, firstline[qh_MAXfirst+1]; + coordT *coords, value; + + if (!qh HALFspace) { + fprintf (qh ferr, "qhull input error: feasible point (dim 1 coords) is only valid for halfspace intersection\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (qh feasible_string) + fprintf (qh ferr, "qhull input warning: feasible point (dim 1 coords) overrides 'Hn,n,n' feasible point for halfspace intersection\n"); + if (!(qh feasible_point= (coordT*)malloc (dim* sizeof(coordT)))) { + fprintf(qh ferr, "qhull error: insufficient memory for feasible point\n"); + qh_errexit(qh_ERRmem, NULL, NULL); + } + coords= qh feasible_point; + while ((s= (isfirst ? remainder : fgets(firstline, qh_MAXfirst, qh fin)))) { + if (isfirst) + isfirst= False; + else + linecount++; + while (*s) { + while (isspace(*s)) + s++; + value= qh_strtod (s, &t); + if (s == t) + break; + s= t; + *(coords++)= value; + if (++tokcount == dim) { + while (isspace (*s)) + s++; + qh_strtod (s, &t); + if (s != t) { + fprintf (qh ferr, "qhull input error: coordinates for feasible point do not finish out the line: %s\n", + s); + qh_errexit (qh_ERRinput, NULL, NULL); + } + return linecount; + } + } + } + fprintf (qh ferr, "qhull input error: only %d coordinates. Could not read %d-d feasible point.\n", + tokcount, dim); + qh_errexit (qh_ERRinput, NULL, NULL); + return 0; +} /* readfeasible */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="readpoints">-</a> + + qh_readpoints( numpoints, dimension, ismalloc ) + read points from qh.fin into qh.first_point, qh.num_points + qh.fin is lines of coordinates, one per vertex, first line number of points + if 'rbox D4', + gives message + if qh.ATinfinity, + adds point-at-infinity for Delaunay triangulations + + returns: + number of points, array of point coordinates, dimension, ismalloc True + if qh.DELAUNAY & !qh.PROJECTinput, projects points to paraboloid + and clears qh.PROJECTdelaunay + if qh.HALFspace, reads optional feasible point, reads halfspaces, + converts to dual. + + for feasible point in "cdd format" in 3-d: + 3 1 + coordinates + comments + begin + n 4 real/integer + ... + end + + notes: + dimension will change in qh_initqhull_globals if qh.PROJECTinput + uses malloc() since qh_mem not initialized + FIXUP: this routine needs rewriting +*/ +coordT *qh_readpoints(int *numpoints, int *dimension, boolT *ismalloc) { + coordT *points, *coords, *infinity= NULL; + realT paraboloid, maxboloid= -REALmax, value; + realT *coordp= NULL, *offsetp= NULL, *normalp= NULL; + char *s, *t, firstline[qh_MAXfirst+1]; + int diminput=0, numinput=0, dimfeasible= 0, newnum, k, tempi; + int firsttext=0, firstshort=0, firstlong=0, firstpoint=0; + int tokcount= 0, linecount=0, maxcount, coordcount=0; + boolT islong, isfirst= True, wasbegin= False; + boolT isdelaunay= qh DELAUNAY && !qh PROJECTinput; + + if (qh CDDinput) { + while ((s= fgets(firstline, qh_MAXfirst, qh fin))) { + linecount++; + if (qh HALFspace && linecount == 1 && isdigit(*s)) { + dimfeasible= qh_strtol (s, &s); + while (isspace(*s)) + s++; + if (qh_strtol (s, &s) == 1) + linecount += qh_readfeasible (dimfeasible, s); + else + dimfeasible= 0; + }else if (!memcmp (firstline, "begin", 5) || !memcmp (firstline, "BEGIN", 5)) + break; + else if (!*qh rbox_command) + strncat(qh rbox_command, s, sizeof (qh rbox_command)-1); + } + if (!s) { + fprintf (qh ferr, "qhull input error: missing \"begin\" for cdd-formated input\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + } + while(!numinput && (s= fgets(firstline, qh_MAXfirst, qh fin))) { + linecount++; + if (!memcmp (s, "begin", 5) || !memcmp (s, "BEGIN", 5)) + wasbegin= True; + while (*s) { + while (isspace(*s)) + s++; + if (!*s) + break; + if (!isdigit(*s)) { + if (!*qh rbox_command) { + strncat(qh rbox_command, s, sizeof (qh rbox_command)-1); + firsttext= linecount; + } + break; + } + if (!diminput) + diminput= qh_strtol (s, &s); + else { + numinput= qh_strtol (s, &s); + if (numinput == 1 && diminput >= 2 && qh HALFspace && !qh CDDinput) { + linecount += qh_readfeasible (diminput, s); /* checks if ok */ + dimfeasible= diminput; + diminput= numinput= 0; + }else + break; + } + } + } + if (!s) { + fprintf(qh ferr, "qhull input error: short input file. Did not find dimension and number of points\n"); + qh_errexit(qh_ERRinput, NULL, NULL); + } + if (diminput > numinput) { + tempi= diminput; /* exchange dim and n, e.g., for cdd input format */ + diminput= numinput; + numinput= tempi; + } + if (diminput < 2) { + fprintf(qh ferr,"qhull input error: dimension %d (first number) should be at least 2\n", + diminput); + qh_errexit(qh_ERRinput, NULL, NULL); + } + if (isdelaunay) { + qh PROJECTdelaunay= False; + if (qh CDDinput) + *dimension= diminput; + else + *dimension= diminput+1; + *numpoints= numinput; + if (qh ATinfinity) + (*numpoints)++; + }else if (qh HALFspace) { + *dimension= diminput - 1; + *numpoints= numinput; + if (diminput < 3) { + fprintf(qh ferr,"qhull input error: dimension %d (first number, includes offset) should be at least 3 for halfspaces\n", + diminput); + qh_errexit(qh_ERRinput, NULL, NULL); + } + if (dimfeasible) { + if (dimfeasible != *dimension) { + fprintf(qh ferr,"qhull input error: dimension %d of feasible point is not one less than dimension %d for halfspaces\n", + dimfeasible, diminput); + qh_errexit(qh_ERRinput, NULL, NULL); + } + }else + qh_setfeasible (*dimension); + }else { + if (qh CDDinput) + *dimension= diminput-1; + else + *dimension= diminput; + *numpoints= numinput; + } + qh normal_size= *dimension * sizeof(coordT); /* for tracing with qh_printpoint */ + if (qh HALFspace) { + qh half_space= coordp= (coordT*) malloc (qh normal_size + sizeof(coordT)); + if (qh CDDinput) { + offsetp= qh half_space; + normalp= offsetp + 1; + }else { + normalp= qh half_space; + offsetp= normalp + *dimension; + } + } + qh maxline= diminput * (qh_REALdigits + 5); + maximize_(qh maxline, 500); + qh line= (char*)malloc ((qh maxline+1) * sizeof (char)); + *ismalloc= True; /* use malloc since memory not setup */ + coords= points= qh temp_malloc= + (coordT*)malloc((*numpoints)*(*dimension)*sizeof(coordT)); + if (!coords || !qh line || (qh HALFspace && !qh half_space)) { + fprintf(qh ferr, "qhull error: insufficient memory to read %d points\n", + numinput); + qh_errexit(qh_ERRmem, NULL, NULL); + } + if (isdelaunay && qh ATinfinity) { + infinity= points + numinput * (*dimension); + for (k= (*dimension) - 1; k--; ) + infinity[k]= 0.0; + } + maxcount= numinput * diminput; + paraboloid= 0.0; + while ((s= (isfirst ? s : fgets(qh line, qh maxline, qh fin)))) { + if (!isfirst) { + linecount++; + if (*s == 'e' || *s == 'E') { + if (!memcmp (s, "end", 3) || !memcmp (s, "END", 3)) { + if (qh CDDinput ) + break; + else if (wasbegin) + fprintf (qh ferr, "qhull input warning: the input appears to be in cdd format. If so, use 'Fd'\n"); + } + } + } + islong= False; + while (*s) { + while (isspace(*s)) + s++; + value= qh_strtod (s, &t); + if (s == t) { + if (!*qh rbox_command) + strncat(qh rbox_command, s, sizeof (qh rbox_command)-1); + if (*s && !firsttext) + firsttext= linecount; + if (!islong && !firstshort && coordcount) + firstshort= linecount; + break; + } + if (!firstpoint) + firstpoint= linecount; + s= t; + if (++tokcount > maxcount) + continue; + if (qh HALFspace) { + if (qh CDDinput) + *(coordp++)= -value; /* both coefficients and offset */ + else + *(coordp++)= value; + }else { + *(coords++)= value; + if (qh CDDinput && !coordcount) { + if (value != 1.0) { + fprintf (qh ferr, "qhull input error: for cdd format, point at line %d does not start with '1'\n", + linecount); + qh_errexit (qh_ERRinput, NULL, NULL); + } + coords--; + }else if (isdelaunay) { + paraboloid += value * value; + if (qh ATinfinity) { + if (qh CDDinput) + infinity[coordcount-1] += value; + else + infinity[coordcount] += value; + } + } + } + if (++coordcount == diminput) { + coordcount= 0; + if (isdelaunay) { + *(coords++)= paraboloid; + maximize_(maxboloid, paraboloid); + paraboloid= 0.0; + }else if (qh HALFspace) { + if (!qh_sethalfspace (*dimension, coords, &coords, normalp, offsetp, qh feasible_point)) { + fprintf (qh ferr, "The halfspace was on line %d\n", linecount); + if (wasbegin) + fprintf (qh ferr, "The input appears to be in cdd format. If so, you should use option 'Fd'\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + coordp= qh half_space; + } + while (isspace(*s)) + s++; + if (*s) { + islong= True; + if (!firstlong) + firstlong= linecount; + } + } + } + if (!islong && !firstshort && coordcount) + firstshort= linecount; + if (!isfirst && s - qh line >= qh maxline) { + fprintf(qh ferr, "qhull input error: line %d contained more than %d characters\n", + linecount, (int) (s - qh line)); + qh_errexit(qh_ERRinput, NULL, NULL); + } + isfirst= False; + } + if (tokcount != maxcount) { + newnum= fmin_(numinput, tokcount/diminput); + fprintf(qh ferr,"\ +qhull warning: instead of %d %d-dimensional points, input contains\n\ +%d points and %d extra coordinates. Line %d is the first\npoint", + numinput, diminput, tokcount/diminput, tokcount % diminput, firstpoint); + if (firsttext) + fprintf(qh ferr, ", line %d is the first comment", firsttext); + if (firstshort) + fprintf(qh ferr, ", line %d is the first short\nline", firstshort); + if (firstlong) + fprintf(qh ferr, ", line %d is the first long line", firstlong); + fprintf(qh ferr, ". Continue with %d points.\n", newnum); + numinput= newnum; + if (isdelaunay && qh ATinfinity) { + for (k= tokcount % diminput; k--; ) + infinity[k] -= *(--coords); + *numpoints= newnum+1; + }else { + coords -= tokcount % diminput; + *numpoints= newnum; + } + } + if (isdelaunay && qh ATinfinity) { + for (k= (*dimension) -1; k--; ) + infinity[k] /= numinput; + if (coords == infinity) + coords += (*dimension) -1; + else { + for (k= 0; k < (*dimension) -1; k++) + *(coords++)= infinity[k]; + } + *(coords++)= maxboloid * 1.1; + } + if (qh rbox_command[0]) { + qh rbox_command[strlen(qh rbox_command)-1]= '\0'; + if (!strcmp (qh rbox_command, "./rbox D4")) + fprintf (qh ferr, "\n\ +This is the qhull test case. If any errors or core dumps occur,\n\ +recompile qhull with 'make new'. If errors still occur, there is\n\ +an incompatibility. You should try a different compiler. You can also\n\ +change the choices in user.h. If you discover the source of the problem,\n\ +please send mail to qhull_bug@qhull.org.\n\ +\n\ +Type 'qhull' for a short list of options.\n"); + } + free (qh line); + qh line= NULL; + if (qh half_space) { + free (qh half_space); + qh half_space= NULL; + } + qh temp_malloc= NULL; + trace1((qh ferr,"qh_readpoints: read in %d %d-dimensional points\n", + numinput, diminput)); + return(points); +} /* readpoints */ + + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="setfeasible">-</a> + + qh_setfeasible( dim ) + set qh.FEASIBLEpoint from qh.feasible_string in "n,n,n" or "n n n" format + + notes: + "n,n,n" already checked by qh_initflags() + see qh_readfeasible() +*/ +void qh_setfeasible (int dim) { + int tokcount= 0; + char *s; + coordT *coords, value; + + if (!(s= qh feasible_string)) { + fprintf(qh ferr, "\ +qhull input error: halfspace intersection needs a feasible point.\n\ +Either prepend the input with 1 point or use 'Hn,n,n'. See manual.\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (!(qh feasible_point= (pointT*)malloc (dim* sizeof(coordT)))) { + fprintf(qh ferr, "qhull error: insufficient memory for 'Hn,n,n'\n"); + qh_errexit(qh_ERRmem, NULL, NULL); + } + coords= qh feasible_point; + while (*s) { + value= qh_strtod (s, &s); + if (++tokcount > dim) { + fprintf (qh ferr, "qhull input warning: more coordinates for 'H%s' than dimension %d\n", + qh feasible_string, dim); + break; + } + *(coords++)= value; + if (*s) + s++; + } + while (++tokcount <= dim) + *(coords++)= 0.0; +} /* setfeasible */ + +/*-<a href="qh-io.htm#TOC" + >-------------------------------</a><a name="skipfacet">-</a> + + qh_skipfacet( facet ) + returns 'True' if this facet is not to be printed + + notes: + based on the user provided slice thresholds and 'good' specifications +*/ +boolT qh_skipfacet(facetT *facet) { + facetT *neighbor, **neighborp; + + if (qh PRINTneighbors) { + if (facet->good) + return !qh PRINTgood; + FOREACHneighbor_(facet) { + if (neighbor->good) + return False; + } + return True; + }else if (qh PRINTgood) + return !facet->good; + else if (!facet->normal) + return True; + return (!qh_inthresholds (facet->normal, NULL)); +} /* skipfacet */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/io.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/io.h new file mode 100644 index 0000000000000000000000000000000000000000..77bb61d7de764d5c9b097809bd5d3fcecb74b0cb --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/io.h @@ -0,0 +1,154 @@ +/*<html><pre> -<a href="qh-io.htm" + >-------------------------------</a><a name="TOP">-</a> + + io.h + declarations of Input/Output functions + + see README, qhull.h and io.c + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFio +#define qhDEFio 1 + +/*============ constants and flags ==================*/ + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_MAXfirst">-</a> + + qh_MAXfirst + maximum length of first two lines of stdin +*/ +#define qh_MAXfirst 200 + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_MINradius">-</a> + + qh_MINradius + min radius for Gp and Gv, fraction of maxcoord +*/ +#define qh_MINradius 0.02 + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_GEOMepsilon">-</a> + + qh_GEOMepsilon + adjust outer planes for 'lines closer' and geomview roundoff. + This prevents bleed through. +*/ +#define qh_GEOMepsilon 2e-3 + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="qh_WHITESPACE">-</a> + + qh_WHITESPACE + possible values of white space +*/ +#define qh_WHITESPACE " \n\t\v\r\f" + + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="RIDGE">-</a> + + qh_RIDGE + to select which ridges to print in qh_eachvoronoi +*/ +typedef enum +{ + qh_RIDGEall = 0, qh_RIDGEinner, qh_RIDGEouter +} +qh_RIDGE; + +/*-<a href="qh-io.htm#TOC" + >--------------------------------</a><a name="printvridgeT">-</a> + + printvridgeT + prints results of qh_printvdiagram + + see: + <a href="io.c#printvridge">qh_printvridge</a> for an example +*/ +typedef void (*printvridgeT)(FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded); + +/*============== -prototypes in alphabetical order =========*/ + +void dfacet( unsigned id); +void dvertex( unsigned id); +int qh_compare_facetarea(const void *p1, const void *p2); +int qh_compare_facetmerge(const void *p1, const void *p2); +int qh_compare_facetvisit(const void *p1, const void *p2); +int qh_compare_vertexpoint(const void *p1, const void *p2); /* not used */ + +void qh_countfacets (facetT *facetlist, setT *facets, boolT printall, + int *numfacetsp, int *numsimplicialp, int *totneighborsp, + int *numridgesp, int *numcoplanarsp, int *numnumtricoplanarsp); +pointT *qh_detvnorm (vertexT *vertex, vertexT *vertexA, setT *centers, realT *offsetp); +setT *qh_detvridge (vertexT *vertex); +setT *qh_detvridge3 (vertexT *atvertex, vertexT *vertex); +int qh_eachvoronoi (FILE *fp, printvridgeT printvridge, vertexT *atvertex, boolT visitall, qh_RIDGE innerouter, boolT inorder); +int qh_eachvoronoi_all (FILE *fp, printvridgeT printvridge, boolT isupper, qh_RIDGE innerouter, boolT inorder); +void qh_facet2point(facetT *facet, pointT **point0, pointT **point1, realT *mindist); +setT *qh_facetvertices (facetT *facetlist, setT *facets, boolT allfacets); +void qh_geomplanes (facetT *facet, realT *outerplane, realT *innerplane); +void qh_markkeep (facetT *facetlist); +setT *qh_markvoronoi (facetT *facetlist, setT *facets, boolT printall, boolT *islowerp, int *numcentersp); +void qh_order_vertexneighbors(vertexT *vertex); +void qh_printafacet(FILE *fp, int format, facetT *facet, boolT printall); +void qh_printbegin (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printcenter (FILE *fp, int format, char *string, facetT *facet); +void qh_printcentrum (FILE *fp, facetT *facet, realT radius); +void qh_printend (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printend4geom (FILE *fp, facetT *facet, int *num, boolT printall); +void qh_printextremes (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printextremes_2d (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printextremes_d (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printfacet(FILE *fp, facetT *facet); +void qh_printfacet2math(FILE *fp, facetT *facet, int format, int notfirst); +void qh_printfacet2geom(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet2geom_points(FILE *fp, pointT *point1, pointT *point2, + facetT *facet, realT offset, realT color[3]); +void qh_printfacet3math (FILE *fp, facetT *facet, int format, int notfirst); +void qh_printfacet3geom_nonsimplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet3geom_points(FILE *fp, setT *points, facetT *facet, realT offset, realT color[3]); +void qh_printfacet3geom_simplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet3vertex(FILE *fp, facetT *facet, int format); +void qh_printfacet4geom_nonsimplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacet4geom_simplicial(FILE *fp, facetT *facet, realT color[3]); +void qh_printfacetNvertex_nonsimplicial(FILE *fp, facetT *facet, int id, int format); +void qh_printfacetNvertex_simplicial(FILE *fp, facetT *facet, int format); +void qh_printfacetheader(FILE *fp, facetT *facet); +void qh_printfacetridges(FILE *fp, facetT *facet); +void qh_printfacets(FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printhelp_degenerate(FILE *fp); +void qh_printhelp_singular(FILE *fp); +void qh_printhyperplaneintersection(FILE *fp, facetT *facet1, facetT *facet2, + setT *vertices, realT color[3]); +void qh_printneighborhood (FILE *fp, int format, facetT *facetA, facetT *facetB, boolT printall); +void qh_printline3geom (FILE *fp, pointT *pointA, pointT *pointB, realT color[3]); +void qh_printpoint(FILE *fp, char *string, pointT *point); +void qh_printpointid(FILE *fp, char *string, int dim, pointT *point, int id); +void qh_printpoint3 (FILE *fp, pointT *point); +void qh_printpoints_out (FILE *fp, facetT *facetlist, setT *facets, int printall); +void qh_printpointvect (FILE *fp, pointT *point, coordT *normal, pointT *center, realT radius, realT color[3]); +void qh_printpointvect2 (FILE *fp, pointT *point, coordT *normal, pointT *center, realT radius); +void qh_printridge(FILE *fp, ridgeT *ridge); +void qh_printspheres(FILE *fp, setT *vertices, realT radius); +void qh_printvdiagram (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +int qh_printvdiagram2 (FILE *fp, printvridgeT printvridge, setT *vertices, qh_RIDGE innerouter, boolT inorder); +void qh_printvertex(FILE *fp, vertexT *vertex); +void qh_printvertexlist (FILE *fp, char* string, facetT *facetlist, + setT *facets, boolT printall); +void qh_printvertices (FILE *fp, char* string, setT *vertices); +void qh_printvneighbors (FILE *fp, facetT* facetlist, setT *facets, boolT printall); +void qh_printvoronoi (FILE *fp, int format, facetT *facetlist, setT *facets, boolT printall); +void qh_printvnorm (FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded); +void qh_printvridge (FILE *fp, vertexT *vertex, vertexT *vertexA, setT *centers, boolT unbounded); +void qh_produce_output(void); +void qh_projectdim3 (pointT *source, pointT *destination); +int qh_readfeasible (int dim, char *remainder); +coordT *qh_readpoints(int *numpoints, int *dimension, boolT *ismalloc); +void qh_setfeasible (int dim); +boolT qh_skipfacet(facetT *facet); + +#endif /* qhDEFio */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/mem.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/mem.c new file mode 100644 index 0000000000000000000000000000000000000000..f7fdde46cd3ebb6532977f7d0a15be58bfb61815 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/mem.c @@ -0,0 +1,452 @@ +/*<html><pre> -<a href="qh-mem.htm" + >-------------------------------</a><a name="TOP">-</a> + + mem.c + memory management routines for qhull + + This is a standalone program. + + To initialize memory: + + qh_meminit (stderr); + qh_meminitbuffers (qh IStracing, qh_MEMalign, 7, qh_MEMbufsize,qh_MEMinitbuf); + qh_memsize(sizeof(facetT)); + qh_memsize(sizeof(facetT)); + ... + qh_memsetup(); + + To free up all memory buffers: + qh_memfreeshort (&curlong, &totlong); + + if qh_NOmem, + malloc/free is used instead of mem.c + + notes: + uses Quickfit algorithm (freelists for commonly allocated sizes) + assumes small sizes for freelists (it discards the tail of memory buffers) + + see: + qh-mem.htm and mem.h + global.c (qh_initbuffers) for an example of using mem.c + + copyright (c) 1993-2003 The Geometry Center +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include "mem.h" + +#ifndef qhDEFqhull +typedef struct ridgeT ridgeT; +typedef struct facetT facetT; +void qh_errexit(int exitcode, facetT *, ridgeT *); +#endif + +/*============ -global data structure ============== + see mem.h for definition +*/ + +qhmemT qhmem; +/*= {0}; +/ * remove "= {0}" if this causes a compiler error */ + +#ifndef qh_NOmem + +/*============= internal functions ==============*/ + +static int qh_intcompare(const void *i, const void *j); + +/*========== functions in alphabetical order ======== */ + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="intcompare">-</a> + + qh_intcompare( i, j ) + used by qsort and bsearch to compare two integers +*/ +static int qh_intcompare(const void *i, const void *j) { + return(*((int *)i) - *((int *)j)); +} /* intcompare */ + + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="memalloc">-</a> + + qh_memalloc( insize ) + returns object of insize bytes + qhmem is the global memory structure + + returns: + pointer to allocated memory + errors if insufficient memory + + notes: + use explicit type conversion to avoid type warnings on some compilers + actual object may be larger than insize + use qh_memalloc_() for inline code for quick allocations + logs allocations if 'T5' + + design: + if size < qhmem.LASTsize + if qhmem.freelists[size] non-empty + return first object on freelist + else + round up request to size of qhmem.freelists[size] + allocate new allocation buffer if necessary + allocate object from allocation buffer + else + allocate object with malloc() +*/ +void *qh_memalloc(int insize) { + void **freelistp, *newbuffer; + int index, size; + int outsize, bufsize; + void *object; + + if ((unsigned) insize <= (unsigned) qhmem.LASTsize) { + index= qhmem.indextable[insize]; + freelistp= qhmem.freelists+index; + if ((object= *freelistp)) { + qhmem.cntquick++; + *freelistp= *((void **)*freelistp); /* replace freelist with next object */ + return (object); + }else { + outsize= qhmem.sizetable[index]; + qhmem.cntshort++; + if (outsize > qhmem .freesize) { + if (!qhmem.curbuffer) + bufsize= qhmem.BUFinit; + else + bufsize= qhmem.BUFsize; + qhmem.totshort += bufsize; + if (!(newbuffer= malloc(bufsize))) { + fprintf(qhmem.ferr, "qhull error (qh_memalloc): insufficient memory\n"); + qh_errexit(qhmem_ERRmem, NULL, NULL); + } + *((void **)newbuffer)= qhmem.curbuffer; /* prepend newbuffer to curbuffer + list */ + qhmem.curbuffer= newbuffer; + size= (sizeof(void **) + qhmem.ALIGNmask) & ~qhmem.ALIGNmask; + qhmem.freemem= (void *)((char *)newbuffer+size); + qhmem.freesize= bufsize - size; + } + object= qhmem.freemem; + qhmem.freemem= (void *)((char *)qhmem.freemem + outsize); + qhmem.freesize -= outsize; + return object; + } + }else { /* long allocation */ + if (!qhmem.indextable) { + fprintf (qhmem.ferr, "qhull internal error (qh_memalloc): qhmem has not been initialized.\n"); + qh_errexit(qhmem_ERRqhull, NULL, NULL); + } + outsize= insize; + qhmem .cntlong++; + qhmem .curlong++; + qhmem .totlong += outsize; + if (qhmem.maxlong < qhmem.totlong) + qhmem.maxlong= qhmem.totlong; + if (!(object= malloc(outsize))) { + fprintf(qhmem.ferr, "qhull error (qh_memalloc): insufficient memory\n"); + qh_errexit(qhmem_ERRmem, NULL, NULL); + } + if (qhmem.IStracing >= 5) + fprintf (qhmem.ferr, "qh_memalloc long: %d bytes at %p\n", outsize, object); + } + return (object); +} /* memalloc */ + + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="memfree">-</a> + + qh_memfree( object, size ) + free up an object of size bytes + size is insize from qh_memalloc + + notes: + object may be NULL + type checking warns if using (void **)object + use qh_memfree_() for quick free's of small objects + + design: + if size <= qhmem.LASTsize + append object to corresponding freelist + else + call free(object) +*/ +void qh_memfree(void *object, int size) { + void **freelistp; + + if (!object) + return; + if (size <= qhmem.LASTsize) { + qhmem .freeshort++; + freelistp= qhmem.freelists + qhmem.indextable[size]; + *((void **)object)= *freelistp; + *freelistp= object; + }else { + qhmem .freelong++; + qhmem .totlong -= size; + free (object); + if (qhmem.IStracing >= 5) + fprintf (qhmem.ferr, "qh_memfree long: %d bytes at %p\n", size, object); + } +} /* memfree */ + + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="memfreeshort">-</a> + + qh_memfreeshort( curlong, totlong ) + frees up all short and qhmem memory allocations + + returns: + number and size of current long allocations +*/ +void qh_memfreeshort (int *curlong, int *totlong) { + void *buffer, *nextbuffer; + FILE *ferr; + + *curlong= qhmem .cntlong - qhmem .freelong; + *totlong= qhmem .totlong; + for(buffer= qhmem.curbuffer; buffer; buffer= nextbuffer) { + nextbuffer= *((void **) buffer); + free(buffer); + } + qhmem.curbuffer= NULL; + if (qhmem .LASTsize) { + free (qhmem .indextable); + free (qhmem .freelists); + free (qhmem .sizetable); + } + ferr= qhmem.ferr; + memset((char *)&qhmem, 0, sizeof qhmem); /* every field is 0, FALSE, NULL */ + qhmem.ferr= ferr; +} /* memfreeshort */ + + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="meminit">-</a> + + qh_meminit( ferr ) + initialize qhmem and test sizeof( void*) +*/ +void qh_meminit (FILE *ferr) { + + memset((char *)&qhmem, 0, sizeof qhmem); /* every field is 0, FALSE, NULL */ + qhmem.ferr= ferr; + if (sizeof(void*) < sizeof(int)) { + fprintf (ferr, "qhull internal error (qh_meminit): sizeof(void*) < sizeof(int). qset.c will not work\n"); + exit (1); /* can not use qh_errexit() */ + } +} /* meminit */ + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="meminitbuffers">-</a> + + qh_meminitbuffers( tracelevel, alignment, numsizes, bufsize, bufinit ) + initialize qhmem + if tracelevel >= 5, trace memory allocations + alignment= desired address alignment for memory allocations + numsizes= number of freelists + bufsize= size of additional memory buffers for short allocations + bufinit= size of initial memory buffer for short allocations +*/ +void qh_meminitbuffers (int tracelevel, int alignment, int numsizes, int bufsize, int bufinit) { + + qhmem.IStracing= tracelevel; + qhmem.NUMsizes= numsizes; + qhmem.BUFsize= bufsize; + qhmem.BUFinit= bufinit; + qhmem.ALIGNmask= alignment-1; + if (qhmem.ALIGNmask & ~qhmem.ALIGNmask) { + fprintf (qhmem.ferr, "qhull internal error (qh_meminit): memory alignment %d is not a power of 2\n", alignment); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + qhmem.sizetable= (int *) calloc (numsizes, sizeof(int)); + qhmem.freelists= (void **) calloc (numsizes, sizeof(void *)); + if (!qhmem.sizetable || !qhmem.freelists) { + fprintf(qhmem.ferr, "qhull error (qh_meminit): insufficient memory\n"); + qh_errexit (qhmem_ERRmem, NULL, NULL); + } + if (qhmem.IStracing >= 1) + fprintf (qhmem.ferr, "qh_meminitbuffers: memory initialized with alignment %d\n", alignment); +} /* meminitbuffers */ + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="memsetup">-</a> + + qh_memsetup() + set up memory after running memsize() +*/ +void qh_memsetup (void) { + int k,i; + + qsort(qhmem.sizetable, qhmem.TABLEsize, sizeof(int), qh_intcompare); + qhmem.LASTsize= qhmem.sizetable[qhmem.TABLEsize-1]; + if (qhmem .LASTsize >= qhmem .BUFsize || qhmem.LASTsize >= qhmem .BUFinit) { + fprintf (qhmem.ferr, "qhull error (qh_memsetup): largest mem size %d is >= buffer size %d or initial buffer size %d\n", + qhmem .LASTsize, qhmem .BUFsize, qhmem .BUFinit); + qh_errexit(qhmem_ERRmem, NULL, NULL); + } + if (!(qhmem.indextable= (int *)malloc((qhmem.LASTsize+1) * sizeof(int)))) { + fprintf(qhmem.ferr, "qhull error (qh_memsetup): insufficient memory\n"); + qh_errexit(qhmem_ERRmem, NULL, NULL); + } + for(k=qhmem.LASTsize+1; k--; ) + qhmem.indextable[k]= k; + i= 0; + for(k= 0; k <= qhmem.LASTsize; k++) { + if (qhmem.indextable[k] <= qhmem.sizetable[i]) + qhmem.indextable[k]= i; + else + qhmem.indextable[k]= ++i; + } +} /* memsetup */ + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="memsize">-</a> + + qh_memsize( size ) + define a free list for this size +*/ +void qh_memsize(int size) { + int k; + + if (qhmem .LASTsize) { + fprintf (qhmem .ferr, "qhull error (qh_memsize): called after qhmem_setup\n"); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + size= (size + qhmem.ALIGNmask) & ~qhmem.ALIGNmask; + for(k= qhmem.TABLEsize; k--; ) { + if (qhmem.sizetable[k] == size) + return; + } + if (qhmem.TABLEsize < qhmem.NUMsizes) + qhmem.sizetable[qhmem.TABLEsize++]= size; + else + fprintf(qhmem.ferr, "qhull warning (memsize): free list table has room for only %d sizes\n", qhmem.NUMsizes); +} /* memsize */ + + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="memstatistics">-</a> + + qh_memstatistics( fp ) + print out memory statistics + + notes: + does not account for wasted memory at the end of each block +*/ +void qh_memstatistics (FILE *fp) { + int i, count, totfree= 0; + void *object; + + for (i=0; i < qhmem.TABLEsize; i++) { + count=0; + for (object= qhmem .freelists[i]; object; object= *((void **)object)) + count++; + totfree += qhmem.sizetable[i] * count; + } + fprintf (fp, "\nmemory statistics:\n\ +%7d quick allocations\n\ +%7d short allocations\n\ +%7d long allocations\n\ +%7d short frees\n\ +%7d long frees\n\ +%7d bytes of short memory in use\n\ +%7d bytes of short memory in freelists\n\ +%7d bytes of long memory allocated (except for input)\n\ +%7d bytes of long memory in use (in %d pieces)\n\ +%7d bytes per memory buffer (initially %d bytes)\n", + qhmem .cntquick, qhmem.cntshort, qhmem.cntlong, + qhmem .freeshort, qhmem.freelong, + qhmem .totshort - qhmem .freesize - totfree, + totfree, + qhmem .maxlong, qhmem .totlong, qhmem .cntlong - qhmem .freelong, + qhmem .BUFsize, qhmem .BUFinit); + if (qhmem.cntlarger) { + fprintf (fp, "%7d calls to qh_setlarger\n%7.2g average copy size\n", + qhmem.cntlarger, ((float) qhmem.totlarger)/ qhmem.cntlarger); + fprintf (fp, " freelists (bytes->count):"); + } + for (i=0; i < qhmem.TABLEsize; i++) { + count=0; + for (object= qhmem .freelists[i]; object; object= *((void **)object)) + count++; + fprintf (fp, " %d->%d", qhmem.sizetable[i], count); + } + fprintf (fp, "\n\n"); +} /* memstatistics */ + + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="NOmem">-</a> + + qh_NOmem + turn off quick-fit memory allocation + + notes: + uses malloc() and free() instead +*/ +#else /* qh_NOmem */ + +void *qh_memalloc(int insize) { + void *object; + + if (!(object= malloc(insize))) { + fprintf(qhmem.ferr, "qhull error (qh_memalloc): insufficient memory\n"); + qh_errexit(qhmem_ERRmem, NULL, NULL); + } + if (qhmem.IStracing >= 5) + fprintf (qhmem.ferr, "qh_memalloc long: %d bytes at %p\n", insize, object); + return object; +} + +void qh_memfree(void *object, int size) { + + if (!object) + return; + free (object); + if (qhmem.IStracing >= 5) + fprintf (qhmem.ferr, "qh_memfree long: %d bytes at %p\n", size, object); +} + +void qh_memfreeshort (int *curlong, int *totlong) { + + memset((char *)&qhmem, 0, sizeof qhmem); /* every field is 0, FALSE, NULL */ + *curlong= 0; + *totlong= 0; +} + +void qh_meminit (FILE *ferr) { + + memset((char *)&qhmem, 0, sizeof qhmem); /* every field is 0, FALSE, NULL */ + qhmem.ferr= ferr; + if (sizeof(void*) < sizeof(int)) { + fprintf (ferr, "qhull internal error (qh_meminit): sizeof(void*) < sizeof(int). qset.c will not work\n"); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } +} + +void qh_meminitbuffers (int tracelevel, int alignment, int numsizes, int bufsize, int bufinit) { + + qhmem.IStracing= tracelevel; + +} + +void qh_memsetup (void) { + +} + +void qh_memsize(int size) { + +} + +void qh_memstatistics (FILE *fp) { + +} + +#endif /* qh_NOmem */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/mem.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/mem.h new file mode 100644 index 0000000000000000000000000000000000000000..fb141f0c4d717b358c1cecb63345f29f92919c03 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/mem.h @@ -0,0 +1,174 @@ +/*<html><pre> -<a href="qh-mem.htm" + >-------------------------------</a><a name="TOP">-</a> + + mem.h + prototypes for memory management functions + + see qh-mem.htm, mem.c and qset.h + + for error handling, writes message and calls + qh_errexit (qhmem_ERRmem, NULL, NULL) if insufficient memory + and + qh_errexit (qhmem_ERRqhull, NULL, NULL) otherwise + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFmem +#define qhDEFmem + +/*-<a href="qh-mem.htm#TOC" + >-------------------------------</a><a name="NOmem">-</a> + + qh_NOmem + turn off quick-fit memory allocation + + notes: + mem.c implements Quickfit memory allocation for about 20% time + savings. If it fails on your machine, try to locate the + problem, and send the answer to qhull@qhull.org. If this can + not be done, define qh_NOmem to use malloc/free instead. + + #define qh_NOmem +*/ + +/*------------------------------------------- + to avoid bus errors, memory allocation must consider alignment requirements. + malloc() automatically takes care of alignment. Since mem.c manages + its own memory, we need to explicitly specify alignment in + qh_meminitbuffers(). + + A safe choice is sizeof(double). sizeof(float) may be used if doubles + do not occur in data structures and pointers are the same size. Be careful + of machines (e.g., DEC Alpha) with large pointers. If gcc is available, + use __alignof__(double) or fmax_(__alignof__(float), __alignof__(void *)). + + see <a href="user.h#MEMalign">qh_MEMalign</a> in user.h for qhull's alignment +*/ + +#define qhmem_ERRmem 4 /* matches qh_ERRmem in qhull.h */ +#define qhmem_ERRqhull 5 /* matches qh_ERRqhull in qhull.h */ + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="ptr_intT">-</a> + + ptr_intT + for casting a void* to an integer-type + + notes: + On 64-bit machines, a pointer may be larger than an 'int'. + qh_meminit() checks that 'long' holds a 'void*' +*/ +typedef unsigned long ptr_intT; + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="qhmemT">-</a> + + qhmemT + global memory structure for mem.c + + notes: + users should ignore qhmem except for writing extensions + qhmem is allocated in mem.c + + qhmem could be swapable like qh and qhstat, but then + multiple qh's and qhmem's would need to keep in synch. + A swapable qhmem would also waste memory buffers. As long + as memory operations are atomic, there is no problem with + multiple qh structures being active at the same time. + If you need separate address spaces, you can swap the + contents of qhmem. +*/ +typedef struct qhmemT qhmemT; +extern qhmemT qhmem; + +struct qhmemT { /* global memory management variables */ + int BUFsize; /* size of memory allocation buffer */ + int BUFinit; /* initial size of memory allocation buffer */ + int TABLEsize; /* actual number of sizes in free list table */ + int NUMsizes; /* maximum number of sizes in free list table */ + int LASTsize; /* last size in free list table */ + int ALIGNmask; /* worst-case alignment, must be 2^n-1 */ + void **freelists; /* free list table, linked by offset 0 */ + int *sizetable; /* size of each freelist */ + int *indextable; /* size->index table */ + void *curbuffer; /* current buffer, linked by offset 0 */ + void *freemem; /* free memory in curbuffer */ + int freesize; /* size of free memory in bytes */ + void *tempstack; /* stack of temporary memory, managed by users */ + FILE *ferr; /* file for reporting errors */ + int IStracing; /* =5 if tracing memory allocations */ + int cntquick; /* count of quick allocations */ + /* remove statistics doesn't effect speed */ + int cntshort; /* count of short allocations */ + int cntlong; /* count of long allocations */ + int curlong; /* current count of inuse, long allocations */ + int freeshort; /* count of short memfrees */ + int freelong; /* count of long memfrees */ + int totshort; /* total size of short allocations */ + int totlong; /* total size of long allocations */ + int maxlong; /* maximum totlong */ + int cntlarger; /* count of setlarger's */ + int totlarger; /* total copied by setlarger */ +}; + + +/*==================== -macros ====================*/ + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="memalloc_">-</a> + + qh_memalloc_(size, object, type) + returns object of size bytes + assumes size<=qhmem.LASTsize and void **freelistp is a temp +*/ + +#ifdef qh_NOmem +#define qh_memalloc_(size, freelistp, object, type) {\ + object= (type*)qh_memalloc (size); } +#else /* !qh_NOmem */ + +#define qh_memalloc_(size, freelistp, object, type) {\ + freelistp= qhmem.freelists + qhmem.indextable[size];\ + if ((object= (type*)*freelistp)) {\ + qhmem.cntquick++; \ + *freelistp= *((void **)*freelistp);\ + }else object= (type*)qh_memalloc (size);} +#endif + +/*-<a href="qh-mem.htm#TOC" + >--------------------------------</a><a name="memfree_">-</a> + + qh_memfree_(object, size) + free up an object + + notes: + object may be NULL + assumes size<=qhmem.LASTsize and void **freelistp is a temp +*/ +#ifdef qh_NOmem +#define qh_memfree_(object, size, freelistp) {\ + qh_memfree (object, size); } +#else /* !qh_NOmem */ + +#define qh_memfree_(object, size, freelistp) {\ + if (object) { \ + qhmem .freeshort++;\ + freelistp= qhmem.freelists + qhmem.indextable[size];\ + *((void **)object)= *freelistp;\ + *freelistp= object;}} +#endif + +/*=============== prototypes in alphabetical order ============*/ + +void *qh_memalloc(int insize); +void qh_memfree (void *object, int size); +void qh_memfreeshort (int *curlong, int *totlong); +void qh_meminit (FILE *ferr); +void qh_meminitbuffers (int tracelevel, int alignment, int numsizes, + int bufsize, int bufinit); +void qh_memsetup (void); +void qh_memsize(int size); +void qh_memstatistics (FILE *fp); + +#endif /* qhDEFmem */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/merge.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/merge.c new file mode 100644 index 0000000000000000000000000000000000000000..cb683dea1b28cd7944ff36ec15c192d4fb608d52 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/merge.c @@ -0,0 +1,3619 @@ +/*<html><pre> -<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="TOP">-</a> + + merge.c + merges non-convex facets + + see qh-merge.htm and merge.h + + other modules call qh_premerge() and qh_postmerge() + + the user may call qh_postmerge() to perform additional merges. + + To remove deleted facets and vertices (qhull() in qhull.c): + qh_partitionvisible (!qh_ALL, &numoutside); // visible_list, newfacet_list + qh_deletevisible (); // qh.visible_list + qh_resetlists (False, qh_RESETvisible); // qh.visible_list newvertex_list newfacet_list + + assumes qh.CENTERtype= centrum + + merges occur in qh_mergefacet and in qh_mergecycle + vertex->neighbors not set until the first merge occurs + + copyright (c) 1993-2003 The Geometry Center +*/ + +#include "qhull_a.h" + +#ifndef qh_NOmerge + +/*===== functions (alphabetical after premerge and postmerge) ======*/ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="premerge">-</a> + + qh_premerge( apex, maxcentrum ) + pre-merge nonconvex facets in qh.newfacet_list for apex + maxcentrum defines coplanar and concave (qh_test_appendmerge) + + returns: + deleted facets added to qh.visible_list with facet->visible set + + notes: + uses globals, qh.MERGEexact, qh.PREmerge + + design: + mark duplicate ridges in qh.newfacet_list + merge facet cycles in qh.newfacet_list + merge duplicate ridges and concave facets in qh.newfacet_list + check merged facet cycles for degenerate and redundant facets + merge degenerate and redundant facets + collect coplanar and concave facets + merge concave, coplanar, degenerate, and redundant facets +*/ +void qh_premerge (vertexT *apex, realT maxcentrum, realT maxangle) { + boolT othermerge= False; + facetT *newfacet; + + if (qh ZEROcentrum && qh_checkzero(!qh_ALL)) + return; + trace2((qh ferr, "qh_premerge: premerge centrum %2.2g angle %2.2g for apex v%d facetlist f%d\n", + maxcentrum, maxangle, apex->id, getid_(qh newfacet_list))); + if (qh IStracing >= 4 && qh num_facets < 50) + qh_printlists(); + qh centrum_radius= maxcentrum; + qh cos_max= maxangle; + qh degen_mergeset= qh_settemp (qh TEMPsize); + qh facet_mergeset= qh_settemp (qh TEMPsize); + if (qh hull_dim >=3) { + qh_mark_dupridges (qh newfacet_list); /* facet_mergeset */ + qh_mergecycle_all (qh newfacet_list, &othermerge); + qh_forcedmerges (&othermerge /* qh facet_mergeset */); + FORALLnew_facets { /* test samecycle merges */ + if (!newfacet->simplicial && !newfacet->mergeridge) + qh_degen_redundant_neighbors (newfacet, NULL); + } + if (qh_merge_degenredundant()) + othermerge= True; + }else /* qh hull_dim == 2 */ + qh_mergecycle_all (qh newfacet_list, &othermerge); + qh_flippedmerges (qh newfacet_list, &othermerge); + if (!qh MERGEexact || zzval_(Ztotmerge)) { + zinc_(Zpremergetot); + qh POSTmerging= False; + qh_getmergeset_initial (qh newfacet_list); + qh_all_merges (othermerge, False); + } + qh_settempfree(&qh facet_mergeset); + qh_settempfree(&qh degen_mergeset); +} /* premerge */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="postmerge">-</a> + + qh_postmerge( reason, maxcentrum, maxangle, vneighbors ) + post-merge nonconvex facets as defined by maxcentrum and maxangle + 'reason' is for reporting progress + if vneighbors, + calls qh_test_vneighbors at end of qh_all_merge + if firstmerge, + calls qh_reducevertices before qh_getmergeset + + returns: + if first call (qh.visible_list != qh.facet_list), + builds qh.facet_newlist, qh.newvertex_list + deleted facets added to qh.visible_list with facet->visible + qh.visible_list == qh.facet_list + + notes: + + + design: + if first call + set qh.visible_list and qh.newfacet_list to qh.facet_list + add all facets to qh.newfacet_list + mark non-simplicial facets, facet->newmerge + set qh.newvertext_list to qh.vertex_list + add all vertices to qh.newvertex_list + if a pre-merge occured + set vertex->delridge {will retest the ridge} + if qh.MERGEexact + call qh_reducevertices() + if no pre-merging + merge flipped facets + determine non-convex facets + merge all non-convex facets +*/ +void qh_postmerge (char *reason, realT maxcentrum, realT maxangle, + boolT vneighbors) { + facetT *newfacet; + boolT othermerges= False; + vertexT *vertex; + + if (qh REPORTfreq || qh IStracing) { + qh_buildtracing (NULL, NULL); + qh_printsummary (qh ferr); + if (qh PRINTstatistics) + qh_printallstatistics (qh ferr, "reason"); + fprintf (qh ferr, "\n%s with 'C%.2g' and 'A%.2g'\n", + reason, maxcentrum, maxangle); + } + trace2((qh ferr, "qh_postmerge: postmerge. test vneighbors? %d\n", + vneighbors)); + qh centrum_radius= maxcentrum; + qh cos_max= maxangle; + qh POSTmerging= True; + qh degen_mergeset= qh_settemp (qh TEMPsize); + qh facet_mergeset= qh_settemp (qh TEMPsize); + if (qh visible_list != qh facet_list) { /* first call */ + qh NEWfacets= True; + qh visible_list= qh newfacet_list= qh facet_list; + FORALLnew_facets { + newfacet->newfacet= True; + if (!newfacet->simplicial) + newfacet->newmerge= True; + zinc_(Zpostfacets); + } + qh newvertex_list= qh vertex_list; + FORALLvertices + vertex->newlist= True; + if (qh VERTEXneighbors) { /* a merge has occurred */ + FORALLvertices + vertex->delridge= True; /* test for redundant, needed? */ + if (qh MERGEexact) { + if (qh hull_dim <= qh_DIMreduceBuild) + qh_reducevertices(); /* was skipped during pre-merging */ + } + } + if (!qh PREmerge && !qh MERGEexact) + qh_flippedmerges (qh newfacet_list, &othermerges); + } + qh_getmergeset_initial (qh newfacet_list); + qh_all_merges (False, vneighbors); + qh_settempfree(&qh facet_mergeset); + qh_settempfree(&qh degen_mergeset); +} /* post_merge */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="all_merges">-</a> + + qh_all_merges( othermerge, vneighbors ) + merge all non-convex facets + + set othermerge if already merged facets (for qh_reducevertices) + if vneighbors + tests vertex neighbors for convexity at end + qh.facet_mergeset lists the non-convex ridges in qh_newfacet_list + qh.degen_mergeset is defined + if qh.MERGEexact && !qh.POSTmerging, + does not merge coplanar facets + + returns: + deleted facets added to qh.visible_list with facet->visible + deleted vertices added qh.delvertex_list with vertex->delvertex + + notes: + unless !qh.MERGEindependent, + merges facets in independent sets + uses qh.newfacet_list as argument since merges call qh_removefacet() + + design: + while merges occur + for each merge in qh.facet_mergeset + unless one of the facets was already merged in this pass + merge the facets + test merged facets for additional merges + add merges to qh.facet_mergeset + if vertices record neighboring facets + rename redundant vertices + update qh.facet_mergeset + if vneighbors ?? + tests vertex neighbors for convexity at end +*/ +void qh_all_merges (boolT othermerge, boolT vneighbors) { + facetT *facet1, *facet2; + mergeT *merge; + boolT wasmerge= True, isreduce; + void **freelistp; /* used !qh_NOmem */ + vertexT *vertex; + mergeType mergetype; + int numcoplanar=0, numconcave=0, numdegenredun= 0, numnewmerges= 0; + + trace2((qh ferr, "qh_all_merges: starting to merge facets beginning from f%d\n", + getid_(qh newfacet_list))); + while (True) { + wasmerge= False; + while (qh_setsize (qh facet_mergeset)) { + while ((merge= (mergeT*)qh_setdellast(qh facet_mergeset))) { + facet1= merge->facet1; + facet2= merge->facet2; + mergetype= merge->type; + qh_memfree_(merge, sizeof(mergeT), freelistp); + if (facet1->visible || facet2->visible) /*deleted facet*/ + continue; + if ((facet1->newfacet && !facet1->tested) + || (facet2->newfacet && !facet2->tested)) { + if (qh MERGEindependent && mergetype <= MRGanglecoplanar) + continue; /* perform independent sets of merges */ + } + qh_merge_nonconvex (facet1, facet2, mergetype); + numdegenredun += qh_merge_degenredundant(); + numnewmerges++; + wasmerge= True; + if (mergetype == MRGconcave) + numconcave++; + else /* MRGcoplanar or MRGanglecoplanar */ + numcoplanar++; + } /* while setdellast */ + if (qh POSTmerging && qh hull_dim <= qh_DIMreduceBuild + && numnewmerges > qh_MAXnewmerges) { + numnewmerges= 0; + qh_reducevertices(); /* otherwise large post merges too slow */ + } + qh_getmergeset (qh newfacet_list); /* facet_mergeset */ + } /* while mergeset */ + if (qh VERTEXneighbors) { + isreduce= False; + if (qh hull_dim >=4 && qh POSTmerging) { + FORALLvertices + vertex->delridge= True; + isreduce= True; + } + if ((wasmerge || othermerge) && (!qh MERGEexact || qh POSTmerging) + && qh hull_dim <= qh_DIMreduceBuild) { + othermerge= False; + isreduce= True; + } + if (isreduce) { + if (qh_reducevertices()) { + qh_getmergeset (qh newfacet_list); /* facet_mergeset */ + continue; + } + } + } + if (vneighbors && qh_test_vneighbors(/* qh newfacet_list */)) + continue; + break; + } /* while (True) */ + if (qh CHECKfrequently && !qh MERGEexact) { + qh old_randomdist= qh RANDOMdist; + qh RANDOMdist= False; + qh_checkconvex (qh newfacet_list, qh_ALGORITHMfault); + /* qh_checkconnect (); [this is slow and it changes the facet order] */ + qh RANDOMdist= qh old_randomdist; + } + trace1((qh ferr, "qh_all_merges: merged %d coplanar facets %d concave facets and %d degen or redundant facets.\n", + numcoplanar, numconcave, numdegenredun)); + if (qh IStracing >= 4 && qh num_facets < 50) + qh_printlists (); +} /* all_merges */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="appendmergeset">-</a> + + qh_appendmergeset( facet, neighbor, mergetype, angle ) + appends an entry to qh.facet_mergeset or qh.degen_mergeset + + angle ignored if NULL or !qh.ANGLEmerge + + returns: + merge appended to facet_mergeset or degen_mergeset + sets ->degenerate or ->redundant if degen_mergeset + + see: + qh_test_appendmerge() + + design: + allocate merge entry + if regular merge + append to qh.facet_mergeset + else if degenerate merge and qh.facet_mergeset is all degenerate + append to qh.degen_mergeset + else if degenerate merge + prepend to qh.degen_mergeset + else if redundant merge + append to qh.degen_mergeset +*/ +void qh_appendmergeset(facetT *facet, facetT *neighbor, mergeType mergetype, realT *angle) { + mergeT *merge, *lastmerge; + void **freelistp; /* used !qh_NOmem */ + + if (facet->redundant) + return; + if (facet->degenerate && mergetype == MRGdegen) + return; + qh_memalloc_(sizeof(mergeT), freelistp, merge, mergeT); + merge->facet1= facet; + merge->facet2= neighbor; + merge->type= mergetype; + if (angle && qh ANGLEmerge) + merge->angle= *angle; + if (mergetype < MRGdegen) + qh_setappend (&(qh facet_mergeset), merge); + else if (mergetype == MRGdegen) { + facet->degenerate= True; + if (!(lastmerge= (mergeT*)qh_setlast (qh degen_mergeset)) + || lastmerge->type == MRGdegen) + qh_setappend (&(qh degen_mergeset), merge); + else + qh_setaddnth (&(qh degen_mergeset), 0, merge); + }else if (mergetype == MRGredundant) { + facet->redundant= True; + qh_setappend (&(qh degen_mergeset), merge); + }else /* mergetype == MRGmirror */ { + if (facet->redundant || neighbor->redundant) { + fprintf(qh ferr, "qhull error (qh_appendmergeset): facet f%d or f%d is already a mirrored facet\n", + facet->id, neighbor->id); + qh_errexit2 (qh_ERRqhull, facet, neighbor); + } + if (!qh_setequal (facet->vertices, neighbor->vertices)) { + fprintf(qh ferr, "qhull error (qh_appendmergeset): mirrored facets f%d and f%d do not have the same vertices\n", + facet->id, neighbor->id); + qh_errexit2 (qh_ERRqhull, facet, neighbor); + } + facet->redundant= True; + neighbor->redundant= True; + qh_setappend (&(qh degen_mergeset), merge); + } +} /* appendmergeset */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="basevertices">-</a> + + qh_basevertices( samecycle ) + return temporary set of base vertices for samecycle + samecycle is first facet in the cycle + assumes apex is SETfirst_( samecycle->vertices ) + + returns: + vertices (settemp) + all ->seen are cleared + + notes: + uses qh_vertex_visit; + + design: + for each facet in samecycle + for each unseen vertex in facet->vertices + append to result +*/ +setT *qh_basevertices (facetT *samecycle) { + facetT *same; + vertexT *apex, *vertex, **vertexp; + setT *vertices= qh_settemp (qh TEMPsize); + + apex= SETfirstt_(samecycle->vertices, vertexT); + apex->visitid= ++qh vertex_visit; + FORALLsame_cycle_(samecycle) { + if (same->mergeridge) + continue; + FOREACHvertex_(same->vertices) { + if (vertex->visitid != qh vertex_visit) { + qh_setappend (&vertices, vertex); + vertex->visitid= qh vertex_visit; + vertex->seen= False; + } + } + } + trace4((qh ferr, "qh_basevertices: found %d vertices\n", + qh_setsize (vertices))); + return vertices; +} /* basevertices */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="checkconnect">-</a> + + qh_checkconnect() + check that new facets are connected + new facets are on qh.newfacet_list + + notes: + this is slow and it changes the order of the facets + uses qh.visit_id + + design: + move first new facet to end of qh.facet_list + for all newly appended facets + append unvisited neighbors to end of qh.facet_list + for all new facets + report error if unvisited +*/ +void qh_checkconnect (void /* qh newfacet_list */) { + facetT *facet, *newfacet, *errfacet= NULL, *neighbor, **neighborp; + + facet= qh newfacet_list; + qh_removefacet (facet); + qh_appendfacet (facet); + facet->visitid= ++qh visit_id; + FORALLfacet_(facet) { + FOREACHneighbor_(facet) { + if (neighbor->visitid != qh visit_id) { + qh_removefacet (neighbor); + qh_appendfacet (neighbor); + neighbor->visitid= qh visit_id; + } + } + } + FORALLnew_facets { + if (newfacet->visitid == qh visit_id) + break; + fprintf(qh ferr, "qhull error: f%d is not attached to the new facets\n", + newfacet->id); + errfacet= newfacet; + } + if (errfacet) + qh_errexit (qh_ERRqhull, errfacet, NULL); +} /* checkconnect */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="checkzero">-</a> + + qh_checkzero( testall ) + check that facets are clearly convex for qh.DISTround with qh.MERGEexact + + if testall, + test all facets for qh.MERGEexact post-merging + else + test qh.newfacet_list + + if qh.MERGEexact, + allows coplanar ridges + skips convexity test while qh.ZEROall_ok + + returns: + True if all facets !flipped, !dupridge, normal + if all horizon facets are simplicial + if all vertices are clearly below neighbor + if all opposite vertices of horizon are below + clears qh.ZEROall_ok if any problems or coplanar facets + + notes: + uses qh.vertex_visit + horizon facets may define multiple new facets + + design: + for all facets in qh.newfacet_list or qh.facet_list + check for flagged faults (flipped, etc.) + for all facets in qh.newfacet_list or qh.facet_list + for each neighbor of facet + skip horizon facets for qh.newfacet_list + test the opposite vertex + if qh.newfacet_list + test the other vertices in the facet's horizon facet +*/ +boolT qh_checkzero (boolT testall) { + facetT *facet, *neighbor, **neighborp; + facetT *horizon, *facetlist; + int neighbor_i; + vertexT *vertex, **vertexp; + realT dist; + + if (testall) + facetlist= qh facet_list; + else { + facetlist= qh newfacet_list; + FORALLfacet_(facetlist) { + horizon= SETfirstt_(facet->neighbors, facetT); + if (!horizon->simplicial) + goto LABELproblem; + if (facet->flipped || facet->dupridge || !facet->normal) + goto LABELproblem; + } + if (qh MERGEexact && qh ZEROall_ok) { + trace2((qh ferr, "qh_checkzero: skip convexity check until first pre-merge\n")); + return True; + } + } + FORALLfacet_(facetlist) { + qh vertex_visit++; + neighbor_i= 0; + horizon= NULL; + FOREACHneighbor_(facet) { + if (!neighbor_i && !testall) { + horizon= neighbor; + neighbor_i++; + continue; /* horizon facet tested in qh_findhorizon */ + } + vertex= SETelemt_(facet->vertices, neighbor_i++, vertexT); + vertex->visitid= qh vertex_visit; + zzinc_(Zdistzero); + qh_distplane (vertex->point, neighbor, &dist); + if (dist >= -qh DISTround) { + qh ZEROall_ok= False; + if (!qh MERGEexact || testall || dist > qh DISTround) + goto LABELnonconvex; + } + } + if (!testall) { + FOREACHvertex_(horizon->vertices) { + if (vertex->visitid != qh vertex_visit) { + zzinc_(Zdistzero); + qh_distplane (vertex->point, facet, &dist); + if (dist >= -qh DISTround) { + qh ZEROall_ok= False; + if (!qh MERGEexact || dist > qh DISTround) + goto LABELnonconvex; + } + break; + } + } + } + } + trace2((qh ferr, "qh_checkzero: testall %d, facets are %s\n", testall, + (qh MERGEexact && !testall) ? + "not concave, flipped, or duplicate ridged" : "clearly convex")); + return True; + + LABELproblem: + qh ZEROall_ok= False; + trace2((qh ferr, "qh_checkzero: facet f%d needs pre-merging\n", + facet->id)); + return False; + + LABELnonconvex: + trace2((qh ferr, "qh_checkzero: facet f%d and f%d are not clearly convex. v%d dist %.2g\n", + facet->id, neighbor->id, vertex->id, dist)); + return False; +} /* checkzero */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="compareangle">-</a> + + qh_compareangle( angle1, angle2 ) + used by qsort() to order merges by angle +*/ +int qh_compareangle(const void *p1, const void *p2) { + mergeT *a= *((mergeT **)p1), *b= *((mergeT **)p2); + + return ((a->angle > b->angle) ? 1 : -1); +} /* compareangle */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="comparemerge">-</a> + + qh_comparemerge( merge1, merge2 ) + used by qsort() to order merges +*/ +int qh_comparemerge(const void *p1, const void *p2) { + mergeT *a= *((mergeT **)p1), *b= *((mergeT **)p2); + + return (a->type - b->type); +} /* comparemerge */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="comparevisit">-</a> + + qh_comparevisit( vertex1, vertex2 ) + used by qsort() to order vertices by their visitid +*/ +int qh_comparevisit (const void *p1, const void *p2) { + vertexT *a= *((vertexT **)p1), *b= *((vertexT **)p2); + + return (a->visitid - b->visitid); +} /* comparevisit */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="copynonconvex">-</a> + + qh_copynonconvex( atridge ) + set non-convex flag on other ridges (if any) between same neighbors + + notes: + may be faster if use smaller ridge set + + design: + for each ridge of atridge's top facet + if ridge shares the same neighbor + set nonconvex flag +*/ +void qh_copynonconvex (ridgeT *atridge) { + facetT *facet, *otherfacet; + ridgeT *ridge, **ridgep; + + facet= atridge->top; + otherfacet= atridge->bottom; + FOREACHridge_(facet->ridges) { + if (otherfacet == otherfacet_(ridge, facet) && ridge != atridge) { + ridge->nonconvex= True; + trace4((qh ferr, "qh_copynonconvex: moved nonconvex flag from r%d to r%d\n", + atridge->id, ridge->id)); + break; + } + } +} /* copynonconvex */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="degen_redundant_facet">-</a> + + qh_degen_redundant_facet( facet ) + check facet for degen. or redundancy + + notes: + bumps vertex_visit + called if a facet was redundant but no longer is (qh_merge_degenredundant) + qh_appendmergeset() only appends first reference to facet (i.e., redundant) + + see: + qh_degen_redundant_neighbors() + + design: + test for redundant neighbor + test for degenerate facet +*/ +void qh_degen_redundant_facet (facetT *facet) { + vertexT *vertex, **vertexp; + facetT *neighbor, **neighborp; + + trace4((qh ferr, "qh_degen_redundant_facet: test facet f%d for degen/redundant\n", + facet->id)); + FOREACHneighbor_(facet) { + qh vertex_visit++; + FOREACHvertex_(neighbor->vertices) + vertex->visitid= qh vertex_visit; + FOREACHvertex_(facet->vertices) { + if (vertex->visitid != qh vertex_visit) + break; + } + if (!vertex) { + qh_appendmergeset (facet, neighbor, MRGredundant, NULL); + trace2((qh ferr, "qh_degen_redundant_facet: f%d is contained in f%d. merge\n", facet->id, neighbor->id)); + return; + } + } + if (qh_setsize (facet->neighbors) < qh hull_dim) { + qh_appendmergeset (facet, facet, MRGdegen, NULL); + trace2((qh ferr, "qh_degen_redundant_neighbors: f%d is degenerate.\n", facet->id)); + } +} /* degen_redundant_facet */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="degen_redundant_neighbors">-</a> + + qh_degen_redundant_neighbors( facet, delfacet, ) + append degenerate and redundant neighbors to facet_mergeset + if delfacet, + only checks neighbors of both delfacet and facet + also checks current facet for degeneracy + + notes: + bumps vertex_visit + called for each qh_mergefacet() and qh_mergecycle() + merge and statistics occur in merge_nonconvex + qh_appendmergeset() only appends first reference to facet (i.e., redundant) + it appends redundant facets after degenerate ones + + a degenerate facet has fewer than hull_dim neighbors + a redundant facet's vertices is a subset of its neighbor's vertices + tests for redundant merges first (appendmergeset is nop for others) + in a merge, only needs to test neighbors of merged facet + + see: + qh_merge_degenredundant() and qh_degen_redundant_facet() + + design: + test for degenerate facet + test for redundant neighbor + test for degenerate neighbor +*/ +void qh_degen_redundant_neighbors (facetT *facet, facetT *delfacet) { + vertexT *vertex, **vertexp; + facetT *neighbor, **neighborp; + int size; + + trace4((qh ferr, "qh_degen_redundant_neighbors: test neighbors of f%d with delfacet f%d\n", + facet->id, getid_(delfacet))); + if ((size= qh_setsize (facet->neighbors)) < qh hull_dim) { + qh_appendmergeset (facet, facet, MRGdegen, NULL); + trace2((qh ferr, "qh_degen_redundant_neighbors: f%d is degenerate with %d neighbors.\n", facet->id, size)); + } + if (!delfacet) + delfacet= facet; + qh vertex_visit++; + FOREACHvertex_(facet->vertices) + vertex->visitid= qh vertex_visit; + FOREACHneighbor_(delfacet) { + /* uses early out instead of checking vertex count */ + if (neighbor == facet) + continue; + FOREACHvertex_(neighbor->vertices) { + if (vertex->visitid != qh vertex_visit) + break; + } + if (!vertex) { + qh_appendmergeset (neighbor, facet, MRGredundant, NULL); + trace2((qh ferr, "qh_degen_redundant_neighbors: f%d is contained in f%d. merge\n", neighbor->id, facet->id)); + } + } + FOREACHneighbor_(delfacet) { /* redundant merges occur first */ + if (neighbor == facet) + continue; + if ((size= qh_setsize (neighbor->neighbors)) < qh hull_dim) { + qh_appendmergeset (neighbor, neighbor, MRGdegen, NULL); + trace2((qh ferr, "qh_degen_redundant_neighbors: f%d is degenerate with %d neighbors. Neighbor of f%d.\n", neighbor->id, size, facet->id)); + } + } +} /* degen_redundant_neighbors */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="find_newvertex">-</a> + + qh_find_newvertex( oldvertex, vertices, ridges ) + locate new vertex for renaming old vertex + vertices is a set of possible new vertices + vertices sorted by number of deleted ridges + + returns: + newvertex or NULL + each ridge includes both vertex and oldvertex + vertices sorted by number of deleted ridges + + notes: + modifies vertex->visitid + new vertex is in one of the ridges + renaming will not cause a duplicate ridge + renaming will minimize the number of deleted ridges + newvertex may not be adjacent in the dual (though unlikely) + + design: + for each vertex in vertices + set vertex->visitid to number of references in ridges + remove unvisited vertices + set qh.vertex_visit above all possible values + sort vertices by number of references in ridges + add each ridge to qh.hash_table + for each vertex in vertices + look for a vertex that would not cause a duplicate ridge after a rename +*/ +vertexT *qh_find_newvertex (vertexT *oldvertex, setT *vertices, setT *ridges) { + vertexT *vertex, **vertexp; + setT *newridges; + ridgeT *ridge, **ridgep; + int size, hashsize; + int hash; + +#ifndef qh_NOtrace + if (qh IStracing >= 4) { + fprintf (qh ferr, "qh_find_newvertex: find new vertex for v%d from ", + oldvertex->id); + FOREACHvertex_(vertices) + fprintf (qh ferr, "v%d ", vertex->id); + FOREACHridge_(ridges) + fprintf (qh ferr, "r%d ", ridge->id); + fprintf (qh ferr, "\n"); + } +#endif + FOREACHvertex_(vertices) + vertex->visitid= 0; + FOREACHridge_(ridges) { + FOREACHvertex_(ridge->vertices) + vertex->visitid++; + } + FOREACHvertex_(vertices) { + if (!vertex->visitid) { + qh_setdelnth (vertices, SETindex_(vertices,vertex)); + vertexp--; /* repeat since deleted this vertex */ + } + } + qh vertex_visit += qh_setsize (ridges); + if (!qh_setsize (vertices)) { + trace4((qh ferr, "qh_find_newvertex: vertices not in ridges for v%d\n", + oldvertex->id)); + return NULL; + } + qsort (SETaddr_(vertices, vertexT), qh_setsize (vertices), + sizeof (vertexT *), qh_comparevisit); + /* can now use qh vertex_visit */ + if (qh PRINTstatistics) { + size= qh_setsize (vertices); + zinc_(Zintersect); + zadd_(Zintersecttot, size); + zmax_(Zintersectmax, size); + } + hashsize= qh_newhashtable (qh_setsize (ridges)); + FOREACHridge_(ridges) + qh_hashridge (qh hash_table, hashsize, ridge, oldvertex); + FOREACHvertex_(vertices) { + newridges= qh_vertexridges (vertex); + FOREACHridge_(newridges) { + if (qh_hashridge_find (qh hash_table, hashsize, ridge, vertex, oldvertex, &hash)) { + zinc_(Zdupridge); + break; + } + } + qh_settempfree (&newridges); + if (!ridge) + break; /* found a rename */ + } + if (vertex) { + /* counted in qh_renamevertex */ + trace2((qh ferr, "qh_find_newvertex: found v%d for old v%d from %d vertices and %d ridges.\n", + vertex->id, oldvertex->id, qh_setsize (vertices), qh_setsize (ridges))); + }else { + zinc_(Zfindfail); + trace0((qh ferr, "qh_find_newvertex: no vertex for renaming v%d (all duplicated ridges) during p%d\n", + oldvertex->id, qh furthest_id)); + } + qh_setfree (&qh hash_table); + return vertex; +} /* find_newvertex */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="findbest_test">-</a> + + qh_findbest_test( testcentrum, facet, neighbor, bestfacet, dist, mindist, maxdist ) + test neighbor of facet for qh_findbestneighbor() + if testcentrum, + tests centrum (assumes it is defined) + else + tests vertices + + returns: + if a better facet (i.e., vertices/centrum of facet closer to neighbor) + updates bestfacet, dist, mindist, and maxdist +*/ +void qh_findbest_test (boolT testcentrum, facetT *facet, facetT *neighbor, + facetT **bestfacet, realT *distp, realT *mindistp, realT *maxdistp) { + realT dist, mindist, maxdist; + + if (testcentrum) { + zzinc_(Zbestdist); + qh_distplane(facet->center, neighbor, &dist); + dist *= qh hull_dim; /* estimate furthest vertex */ + if (dist < 0) { + maxdist= 0; + mindist= dist; + dist= -dist; + }else + maxdist= dist; + }else + dist= qh_getdistance (facet, neighbor, &mindist, &maxdist); + if (dist < *distp) { + *bestfacet= neighbor; + *mindistp= mindist; + *maxdistp= maxdist; + *distp= dist; + } +} /* findbest_test */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="findbestneighbor">-</a> + + qh_findbestneighbor( facet, dist, mindist, maxdist ) + finds best neighbor (least dist) of a facet for merging + + returns: + returns min and max distances and their max absolute value + + notes: + avoids merging old into new + assumes ridge->nonconvex only set on one ridge between a pair of facets + could use an early out predicate but not worth it + + design: + if a large facet + will test centrum + else + will test vertices + if a large facet + test nonconvex neighbors for best merge + else + test all neighbors for the best merge + if testing centrum + get distance information +*/ +facetT *qh_findbestneighbor(facetT *facet, realT *distp, realT *mindistp, realT *maxdistp) { + facetT *neighbor, **neighborp, *bestfacet= NULL; + ridgeT *ridge, **ridgep; + boolT nonconvex= True, testcentrum= False; + int size= qh_setsize (facet->vertices); + + *distp= REALmax; + if (size > qh_BESTcentrum2 * qh hull_dim + qh_BESTcentrum) { + testcentrum= True; + zinc_(Zbestcentrum); + if (!facet->center) + facet->center= qh_getcentrum (facet); + } + if (size > qh hull_dim + qh_BESTnonconvex) { + FOREACHridge_(facet->ridges) { + if (ridge->nonconvex) { + neighbor= otherfacet_(ridge, facet); + qh_findbest_test (testcentrum, facet, neighbor, + &bestfacet, distp, mindistp, maxdistp); + } + } + } + if (!bestfacet) { + nonconvex= False; + FOREACHneighbor_(facet) + qh_findbest_test (testcentrum, facet, neighbor, + &bestfacet, distp, mindistp, maxdistp); + } + if (!bestfacet) { + fprintf (qh ferr, "qhull internal error (qh_findbestneighbor): no neighbors for f%d\n", facet->id); + + qh_errexit (qh_ERRqhull, facet, NULL); + } + if (testcentrum) + qh_getdistance (facet, bestfacet, mindistp, maxdistp); + trace3((qh ferr, "qh_findbestneighbor: f%d is best neighbor for f%d testcentrum? %d nonconvex? %d dist %2.2g min %2.2g max %2.2g\n", + bestfacet->id, facet->id, testcentrum, nonconvex, *distp, *mindistp, *maxdistp)); + return(bestfacet); +} /* findbestneighbor */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="flippedmerges">-</a> + + qh_flippedmerges( facetlist, wasmerge ) + merge flipped facets into best neighbor + assumes qh.facet_mergeset at top of temporary stack + + returns: + no flipped facets on facetlist + sets wasmerge if merge occurred + degen/redundant merges passed through + + notes: + othermerges not needed since qh.facet_mergeset is empty before & after + keep it in case of change + + design: + append flipped facets to qh.facetmergeset + for each flipped merge + find best neighbor + merge facet into neighbor + merge degenerate and redundant facets + remove flipped merges from qh.facet_mergeset +*/ +void qh_flippedmerges(facetT *facetlist, boolT *wasmerge) { + facetT *facet, *neighbor, *facet1; + realT dist, mindist, maxdist; + mergeT *merge, **mergep; + setT *othermerges; + int nummerge=0; + + trace4((qh ferr, "qh_flippedmerges: begin\n")); + FORALLfacet_(facetlist) { + if (facet->flipped && !facet->visible) + qh_appendmergeset (facet, facet, MRGflip, NULL); + } + othermerges= qh_settemppop(); /* was facet_mergeset */ + qh facet_mergeset= qh_settemp (qh TEMPsize); + qh_settemppush (othermerges); + FOREACHmerge_(othermerges) { + facet1= merge->facet1; + if (merge->type != MRGflip || facet1->visible) + continue; + if (qh TRACEmerge-1 == zzval_(Ztotmerge)) + qhmem.IStracing= qh IStracing= qh TRACElevel; + neighbor= qh_findbestneighbor (facet1, &dist, &mindist, &maxdist); + trace0((qh ferr, "qh_flippedmerges: merge flipped f%d into f%d dist %2.2g during p%d\n", + facet1->id, neighbor->id, dist, qh furthest_id)); + qh_mergefacet (facet1, neighbor, &mindist, &maxdist, !qh_MERGEapex); + nummerge++; + if (qh PRINTstatistics) { + zinc_(Zflipped); + wadd_(Wflippedtot, dist); + wmax_(Wflippedmax, dist); + } + qh_merge_degenredundant(); + } + FOREACHmerge_(othermerges) { + if (merge->facet1->visible || merge->facet2->visible) + qh_memfree (merge, sizeof(mergeT)); + else + qh_setappend (&qh facet_mergeset, merge); + } + qh_settempfree (&othermerges); + if (nummerge) + *wasmerge= True; + trace1((qh ferr, "qh_flippedmerges: merged %d flipped facets into a good neighbor\n", nummerge)); +} /* flippedmerges */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="forcedmerges">-</a> + + qh_forcedmerges( wasmerge ) + merge duplicated ridges + + returns: + removes all duplicate ridges on facet_mergeset + wasmerge set if merge + qh.facet_mergeset may include non-forced merges (none for now) + qh.degen_mergeset includes degen/redun merges + + notes: + duplicate ridges occur when the horizon is pinched, + i.e. a subridge occurs in more than two horizon ridges. + could rename vertices that pinch the horizon + assumes qh_merge_degenredundant() has not be called + othermerges isn't needed since facet_mergeset is empty afterwards + keep it in case of change + + design: + for each duplicate ridge + find current facets by chasing f.replace links + determine best direction for facet + merge one facet into the other + remove duplicate ridges from qh.facet_mergeset +*/ +void qh_forcedmerges(boolT *wasmerge) { + facetT *facet1, *facet2; + mergeT *merge, **mergep; + realT dist1, dist2, mindist1, mindist2, maxdist1, maxdist2; + setT *othermerges; + int nummerge=0, numflip=0; + + if (qh TRACEmerge-1 == zzval_(Ztotmerge)) + qhmem.IStracing= qh IStracing= qh TRACElevel; + trace4((qh ferr, "qh_forcedmerges: begin\n")); + othermerges= qh_settemppop(); /* was facet_mergeset */ + qh facet_mergeset= qh_settemp (qh TEMPsize); + qh_settemppush (othermerges); + FOREACHmerge_(othermerges) { + if (merge->type != MRGridge) + continue; + facet1= merge->facet1; + facet2= merge->facet2; + while (facet1->visible) /* must exist, no qh_merge_degenredunant */ + facet1= facet1->f.replace; /* previously merged facet */ + while (facet2->visible) + facet2= facet2->f.replace; /* previously merged facet */ + if (facet1 == facet2) + continue; + if (!qh_setin (facet2->neighbors, facet1)) { + fprintf (qh ferr, "qhull internal error (qh_forcedmerges): f%d and f%d had a duplicate ridge but as f%d and f%d they are no longer neighbors\n", + merge->facet1->id, merge->facet2->id, facet1->id, facet2->id); + qh_errexit2 (qh_ERRqhull, facet1, facet2); + } + if (qh TRACEmerge-1 == zzval_(Ztotmerge)) + qhmem.IStracing= qh IStracing= qh TRACElevel; + dist1= qh_getdistance (facet1, facet2, &mindist1, &maxdist1); + dist2= qh_getdistance (facet2, facet1, &mindist2, &maxdist2); + trace0((qh ferr, "qh_forcedmerges: duplicate ridge between f%d and f%d, dist %2.2g and reverse dist %2.2g during p%d\n", + facet1->id, facet2->id, dist1, dist2, qh furthest_id)); + if (dist1 < dist2) + qh_mergefacet (facet1, facet2, &mindist1, &maxdist1, !qh_MERGEapex); + else { + qh_mergefacet (facet2, facet1, &mindist2, &maxdist2, !qh_MERGEapex); + dist1= dist2; + facet1= facet2; + } + if (facet1->flipped) { + zinc_(Zmergeflipdup); + numflip++; + }else + nummerge++; + if (qh PRINTstatistics) { + zinc_(Zduplicate); + wadd_(Wduplicatetot, dist1); + wmax_(Wduplicatemax, dist1); + } + } + FOREACHmerge_(othermerges) { + if (merge->type == MRGridge) + qh_memfree (merge, sizeof(mergeT)); + else + qh_setappend (&qh facet_mergeset, merge); + } + qh_settempfree (&othermerges); + if (nummerge) + *wasmerge= True; + trace1((qh ferr, "qh_forcedmerges: merged %d facets and %d flipped facets across duplicated ridges\n", + nummerge, numflip)); +} /* forcedmerges */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="getmergeset">-</a> + + qh_getmergeset( facetlist ) + determines nonconvex facets on facetlist + tests !tested ridges and nonconvex ridges of !tested facets + + returns: + returns sorted qh.facet_mergeset of facet-neighbor pairs to be merged + all ridges tested + + notes: + assumes no nonconvex ridges with both facets tested + uses facet->tested/ridge->tested to prevent duplicate tests + can not limit tests to modified ridges since the centrum changed + uses qh.visit_id + + see: + qh_getmergeset_initial() + + design: + for each facet on facetlist + for each ridge of facet + if untested ridge + test ridge for convexity + if non-convex + append ridge to qh.facet_mergeset + sort qh.facet_mergeset by angle +*/ +void qh_getmergeset(facetT *facetlist) { + facetT *facet, *neighbor, **neighborp; + ridgeT *ridge, **ridgep; + int nummerges; + + nummerges= qh_setsize (qh facet_mergeset); + trace4((qh ferr, "qh_getmergeset: started.\n")); + qh visit_id++; + FORALLfacet_(facetlist) { + if (facet->tested) + continue; + facet->visitid= qh visit_id; + facet->tested= True; /* must be non-simplicial due to merge */ + FOREACHneighbor_(facet) + neighbor->seen= False; + FOREACHridge_(facet->ridges) { + if (ridge->tested && !ridge->nonconvex) + continue; + /* if tested & nonconvex, need to append merge */ + neighbor= otherfacet_(ridge, facet); + if (neighbor->seen) { + ridge->tested= True; + ridge->nonconvex= False; + }else if (neighbor->visitid != qh visit_id) { + ridge->tested= True; + ridge->nonconvex= False; + neighbor->seen= True; /* only one ridge is marked nonconvex */ + if (qh_test_appendmerge (facet, neighbor)) + ridge->nonconvex= True; + } + } + } + nummerges= qh_setsize (qh facet_mergeset); + if (qh ANGLEmerge) + qsort(SETaddr_(qh facet_mergeset, mergeT), nummerges,sizeof(mergeT *),qh_compareangle); + else + qsort(SETaddr_(qh facet_mergeset, mergeT), nummerges,sizeof(mergeT *),qh_comparemerge); + if (qh POSTmerging) { + zadd_(Zmergesettot2, nummerges); + }else { + zadd_(Zmergesettot, nummerges); + zmax_(Zmergesetmax, nummerges); + } + trace2((qh ferr, "qh_getmergeset: %d merges found\n", nummerges)); +} /* getmergeset */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="getmergeset_initial">-</a> + + qh_getmergeset_initial( facetlist ) + determine initial qh.facet_mergeset for facets + tests all facet/neighbor pairs on facetlist + + returns: + sorted qh.facet_mergeset with nonconvex ridges + sets facet->tested, ridge->tested, and ridge->nonconvex + + notes: + uses visit_id, assumes ridge->nonconvex is False + + see: + qh_getmergeset() + + design: + for each facet on facetlist + for each untested neighbor of facet + test facet and neighbor for convexity + if non-convex + append merge to qh.facet_mergeset + mark one of the ridges as nonconvex + sort qh.facet_mergeset by angle +*/ +void qh_getmergeset_initial (facetT *facetlist) { + facetT *facet, *neighbor, **neighborp; + ridgeT *ridge, **ridgep; + int nummerges; + + qh visit_id++; + FORALLfacet_(facetlist) { + facet->visitid= qh visit_id; + facet->tested= True; + FOREACHneighbor_(facet) { + if (neighbor->visitid != qh visit_id) { + if (qh_test_appendmerge (facet, neighbor)) { + FOREACHridge_(neighbor->ridges) { + if (facet == otherfacet_(ridge, neighbor)) { + ridge->nonconvex= True; + break; /* only one ridge is marked nonconvex */ + } + } + } + } + } + FOREACHridge_(facet->ridges) + ridge->tested= True; + } + nummerges= qh_setsize (qh facet_mergeset); + if (qh ANGLEmerge) + qsort(SETaddr_(qh facet_mergeset, mergeT), nummerges,sizeof(mergeT *),qh_compareangle); + else + qsort(SETaddr_(qh facet_mergeset, mergeT), nummerges,sizeof(mergeT *),qh_comparemerge); + if (qh POSTmerging) { + zadd_(Zmergeinittot2, nummerges); + }else { + zadd_(Zmergeinittot, nummerges); + zmax_(Zmergeinitmax, nummerges); + } + trace2((qh ferr, "qh_getmergeset_initial: %d merges found\n", nummerges)); +} /* getmergeset_initial */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="hashridge">-</a> + + qh_hashridge( hashtable, hashsize, ridge, oldvertex ) + add ridge to hashtable without oldvertex + + notes: + assumes hashtable is large enough + + design: + determine hash value for ridge without oldvertex + find next empty slot for ridge +*/ +void qh_hashridge (setT *hashtable, int hashsize, ridgeT *ridge, vertexT *oldvertex) { + int hash; + ridgeT *ridgeA; + + hash= (int)qh_gethash (hashsize, ridge->vertices, qh hull_dim-1, 0, oldvertex); + while (True) { + if (!(ridgeA= SETelemt_(hashtable, hash, ridgeT))) { + SETelem_(hashtable, hash)= ridge; + break; + }else if (ridgeA == ridge) + break; + if (++hash == hashsize) + hash= 0; + } +} /* hashridge */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="hashridge_find">-</a> + + qh_hashridge_find( hashtable, hashsize, ridge, vertex, oldvertex, hashslot ) + returns matching ridge without oldvertex in hashtable + for ridge without vertex + if oldvertex is NULL + matches with any one skip + + returns: + matching ridge or NULL + if no match, + if ridge already in table + hashslot= -1 + else + hashslot= next NULL index + + notes: + assumes hashtable is large enough + can't match ridge to itself + + design: + get hash value for ridge without vertex + for each hashslot + return match if ridge matches ridgeA without oldvertex +*/ +ridgeT *qh_hashridge_find (setT *hashtable, int hashsize, ridgeT *ridge, + vertexT *vertex, vertexT *oldvertex, int *hashslot) { + int hash; + ridgeT *ridgeA; + + *hashslot= 0; + zinc_(Zhashridge); + hash= (int)qh_gethash (hashsize, ridge->vertices, qh hull_dim-1, 0, vertex); + while ((ridgeA= SETelemt_(hashtable, hash, ridgeT))) { + if (ridgeA == ridge) + *hashslot= -1; + else { + zinc_(Zhashridgetest); + if (qh_setequal_except (ridge->vertices, vertex, ridgeA->vertices, oldvertex)) + return ridgeA; + } + if (++hash == hashsize) + hash= 0; + } + if (!*hashslot) + *hashslot= hash; + return NULL; +} /* hashridge_find */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="makeridges">-</a> + + qh_makeridges( facet ) + creates explicit ridges between simplicial facets + + returns: + facet with ridges and without qh_MERGEridge + ->simplicial is False + + notes: + allows qh_MERGEridge flag + uses existing ridges + duplicate neighbors ok if ridges already exist (qh_mergecycle_ridges) + + see: + qh_mergecycle_ridges() + + design: + look for qh_MERGEridge neighbors + mark neighbors that already have ridges + for each unprocessed neighbor of facet + create a ridge for neighbor and facet + if any qh_MERGEridge neighbors + delete qh_MERGEridge flags (already handled by qh_mark_dupridges) +*/ +void qh_makeridges(facetT *facet) { + facetT *neighbor, **neighborp; + ridgeT *ridge, **ridgep; + int neighbor_i, neighbor_n; + boolT toporient, mergeridge= False; + + if (!facet->simplicial) + return; + trace4((qh ferr, "qh_makeridges: make ridges for f%d\n", facet->id)); + facet->simplicial= False; + FOREACHneighbor_(facet) { + if (neighbor == qh_MERGEridge) + mergeridge= True; + else + neighbor->seen= False; + } + FOREACHridge_(facet->ridges) + otherfacet_(ridge, facet)->seen= True; + FOREACHneighbor_i_(facet) { + if (neighbor == qh_MERGEridge) + continue; /* fixed by qh_mark_dupridges */ + else if (!neighbor->seen) { /* no current ridges */ + ridge= qh_newridge(); + ridge->vertices= qh_setnew_delnthsorted (facet->vertices, qh hull_dim, + neighbor_i, 0); + toporient= facet->toporient ^ (neighbor_i & 0x1); + if (toporient) { + ridge->top= facet; + ridge->bottom= neighbor; + }else { + ridge->top= neighbor; + ridge->bottom= facet; + } +#if 0 /* this also works */ + flip= (facet->toporient ^ neighbor->toporient)^(skip1 & 0x1) ^ (skip2 & 0x1); + if (facet->toporient ^ (skip1 & 0x1) ^ flip) { + ridge->top= neighbor; + ridge->bottom= facet; + }else { + ridge->top= facet; + ridge->bottom= neighbor; + } +#endif + qh_setappend(&(facet->ridges), ridge); + qh_setappend(&(neighbor->ridges), ridge); + } + } + if (mergeridge) { + while (qh_setdel (facet->neighbors, qh_MERGEridge)) + ; /* delete each one */ + } +} /* makeridges */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mark_dupridges">-</a> + + qh_mark_dupridges( facetlist ) + add duplicated ridges to qh.facet_mergeset + facet->dupridge is true + + returns: + duplicate ridges on qh.facet_mergeset + ->mergeridge/->mergeridge2 set + duplicate ridges marked by qh_MERGEridge and both sides facet->dupridge + no MERGEridges in neighbor sets + + notes: + duplicate ridges occur when the horizon is pinched, + i.e. a subridge occurs in more than two horizon ridges. + could rename vertices that pinch the horizon + uses qh.visit_id + + design: + for all facets on facetlist + if facet contains a duplicate ridge + for each neighbor of facet + if neighbor marked qh_MERGEridge (one side of the merge) + set facet->mergeridge + else + if neighbor contains a duplicate ridge + and the back link is qh_MERGEridge + append duplicate ridge to qh.facet_mergeset + for each duplicate ridge + make ridge sets in preparation for merging + remove qh_MERGEridge from neighbor set + for each duplicate ridge + restore the missing neighbor from the neighbor set that was qh_MERGEridge + add the missing ridge for this neighbor +*/ +void qh_mark_dupridges(facetT *facetlist) { + facetT *facet, *neighbor, **neighborp; + int nummerge=0; + mergeT *merge, **mergep; + + + trace4((qh ferr, "qh_mark_dupridges: identify duplicate ridges\n")); + FORALLfacet_(facetlist) { + if (facet->dupridge) { + FOREACHneighbor_(facet) { + if (neighbor == qh_MERGEridge) { + facet->mergeridge= True; + continue; + } + if (neighbor->dupridge + && !qh_setin (neighbor->neighbors, facet)) { /* qh_MERGEridge */ + qh_appendmergeset (facet, neighbor, MRGridge, NULL); + facet->mergeridge2= True; + facet->mergeridge= True; + nummerge++; + } + } + } + } + if (!nummerge) + return; + FORALLfacet_(facetlist) { /* gets rid of qh_MERGEridge */ + if (facet->mergeridge && !facet->mergeridge2) + qh_makeridges (facet); + } + FOREACHmerge_(qh facet_mergeset) { /* restore the missing neighbors */ + if (merge->type == MRGridge) { + qh_setappend (&merge->facet2->neighbors, merge->facet1); + qh_makeridges (merge->facet1); /* and the missing ridges */ + } + } + trace1((qh ferr, "qh_mark_dupridges: found %d duplicated ridges\n", + nummerge)); +} /* mark_dupridges */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="maydropneighbor">-</a> + + qh_maydropneighbor( facet ) + drop neighbor relationship if no ridge between facet and neighbor + + returns: + neighbor sets updated + appends degenerate facets to qh.facet_mergeset + + notes: + won't cause redundant facets since vertex inclusion is the same + may drop vertex and neighbor if no ridge + uses qh.visit_id + + design: + visit all neighbors with ridges + for each unvisited neighbor of facet + delete neighbor and facet from the neighbor sets + if neighbor becomes degenerate + append neighbor to qh.degen_mergeset + if facet is degenerate + append facet to qh.degen_mergeset +*/ +void qh_maydropneighbor (facetT *facet) { + ridgeT *ridge, **ridgep; + realT angledegen= qh_ANGLEdegen; + facetT *neighbor, **neighborp; + + qh visit_id++; + trace4((qh ferr, "qh_maydropneighbor: test f%d for no ridges to a neighbor\n", + facet->id)); + FOREACHridge_(facet->ridges) { + ridge->top->visitid= qh visit_id; + ridge->bottom->visitid= qh visit_id; + } + FOREACHneighbor_(facet) { + if (neighbor->visitid != qh visit_id) { + trace0((qh ferr, "qh_maydropneighbor: facets f%d and f%d are no longer neighbors during p%d\n", + facet->id, neighbor->id, qh furthest_id)); + zinc_(Zdropneighbor); + qh_setdel (facet->neighbors, neighbor); + neighborp--; /* repeat, deleted a neighbor */ + qh_setdel (neighbor->neighbors, facet); + if (qh_setsize (neighbor->neighbors) < qh hull_dim) { + zinc_(Zdropdegen); + qh_appendmergeset (neighbor, neighbor, MRGdegen, &angledegen); + trace2((qh ferr, "qh_maydropneighbors: f%d is degenerate.\n", neighbor->id)); + } + } + } + if (qh_setsize (facet->neighbors) < qh hull_dim) { + zinc_(Zdropdegen); + qh_appendmergeset (facet, facet, MRGdegen, &angledegen); + trace2((qh ferr, "qh_maydropneighbors: f%d is degenerate.\n", facet->id)); + } +} /* maydropneighbor */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="merge_degenredundant">-</a> + + qh_merge_degenredundant() + merge all degenerate and redundant facets + qh.degen_mergeset contains merges from qh_degen_redundant_neighbors() + + returns: + number of merges performed + resets facet->degenerate/redundant + if deleted (visible) facet has no neighbors + sets ->f.replace to NULL + + notes: + redundant merges happen before degenerate ones + merging and renaming vertices can result in degen/redundant facets + + design: + for each merge on qh.degen_mergeset + if redundant merge + if non-redundant facet merged into redundant facet + recheck facet for redundancy + else + merge redundant facet into other facet +*/ +int qh_merge_degenredundant (void) { + int size; + mergeT *merge; + facetT *bestneighbor, *facet1, *facet2; + realT dist, mindist, maxdist; + vertexT *vertex, **vertexp; + int nummerges= 0; + mergeType mergetype; + + while ((merge= (mergeT*)qh_setdellast (qh degen_mergeset))) { + facet1= merge->facet1; + facet2= merge->facet2; + mergetype= merge->type; + qh_memfree (merge, sizeof(mergeT)); + if (facet1->visible) + continue; + facet1->degenerate= False; + facet1->redundant= False; + if (qh TRACEmerge-1 == zzval_(Ztotmerge)) + qhmem.IStracing= qh IStracing= qh TRACElevel; + if (mergetype == MRGredundant) { + zinc_(Zneighbor); + while (facet2->visible) { + if (!facet2->f.replace) { + fprintf (qh ferr, "qhull internal error (qh_merge_degenredunant): f%d redundant but f%d has no replacement\n", + facet1->id, facet2->id); + qh_errexit2 (qh_ERRqhull, facet1, facet2); + } + facet2= facet2->f.replace; + } + if (facet1 == facet2) { + qh_degen_redundant_facet (facet1); /* in case of others */ + continue; + } + trace2((qh ferr, "qh_merge_degenredundant: facet f%d is contained in f%d, will merge\n", + facet1->id, facet2->id)); + qh_mergefacet(facet1, facet2, NULL, NULL, !qh_MERGEapex); + /* merge distance is already accounted for */ + nummerges++; + }else { /* mergetype == MRGdegen, other merges may have fixed */ + if (!(size= qh_setsize (facet1->neighbors))) { + zinc_(Zdelfacetdup); + trace2((qh ferr, "qh_merge_degenredundant: facet f%d has no neighbors. Deleted\n", facet1->id)); + qh_willdelete (facet1, NULL); + FOREACHvertex_(facet1->vertices) { + qh_setdel (vertex->neighbors, facet1); + if (!SETfirst_(vertex->neighbors)) { + zinc_(Zdegenvertex); + trace2((qh ferr, "qh_merge_degenredundant: deleted v%d because f%d has no neighbors\n", + vertex->id, facet1->id)); + vertex->deleted= True; + qh_setappend (&qh del_vertices, vertex); + } + } + nummerges++; + }else if (size < qh hull_dim) { + bestneighbor= qh_findbestneighbor(facet1, &dist, &mindist, &maxdist); + trace2((qh ferr, "qh_merge_degenredundant: facet f%d has %d neighbors, merge into f%d dist %2.2g\n", + facet1->id, size, bestneighbor->id, dist)); + qh_mergefacet(facet1, bestneighbor, &mindist, &maxdist, !qh_MERGEapex); + nummerges++; + if (qh PRINTstatistics) { + zinc_(Zdegen); + wadd_(Wdegentot, dist); + wmax_(Wdegenmax, dist); + } + } /* else, another merge fixed the degeneracy and redundancy tested */ + } + } + return nummerges; +} /* merge_degenredundant */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="merge_nonconvex">-</a> + + qh_merge_nonconvex( facet1, facet2, mergetype ) + remove non-convex ridge between facet1 into facet2 + mergetype gives why the facet's are non-convex + + returns: + merges one of the facets into the best neighbor + + design: + if one of the facets is a new facet + prefer merging new facet into old facet + find best neighbors for both facets + merge the nearest facet into its best neighbor + update the statistics +*/ +void qh_merge_nonconvex (facetT *facet1, facetT *facet2, mergeType mergetype) { + facetT *bestfacet, *bestneighbor, *neighbor; + realT dist, dist2, mindist, mindist2, maxdist, maxdist2; + + if (qh TRACEmerge-1 == zzval_(Ztotmerge)) + qhmem.IStracing= qh IStracing= qh TRACElevel; + trace3((qh ferr, "qh_merge_nonconvex: merge #%d for f%d and f%d type %d\n", + zzval_(Ztotmerge) + 1, facet1->id, facet2->id, mergetype)); + /* concave or coplanar */ + if (!facet1->newfacet) { + bestfacet= facet2; /* avoid merging old facet if new is ok */ + facet2= facet1; + facet1= bestfacet; + }else + bestfacet= facet1; + bestneighbor= qh_findbestneighbor(bestfacet, &dist, &mindist, &maxdist); + neighbor= qh_findbestneighbor(facet2, &dist2, &mindist2, &maxdist2); + if (dist < dist2) { + qh_mergefacet(bestfacet, bestneighbor, &mindist, &maxdist, !qh_MERGEapex); + }else if (qh AVOIDold && !facet2->newfacet + && ((mindist >= -qh MAXcoplanar && maxdist <= qh max_outside) + || dist * 1.5 < dist2)) { + zinc_(Zavoidold); + wadd_(Wavoidoldtot, dist); + wmax_(Wavoidoldmax, dist); + trace2((qh ferr, "qh_merge_nonconvex: avoid merging old facet f%d dist %2.2g. Use f%d dist %2.2g instead\n", + facet2->id, dist2, facet1->id, dist2)); + qh_mergefacet(bestfacet, bestneighbor, &mindist, &maxdist, !qh_MERGEapex); + }else { + qh_mergefacet(facet2, neighbor, &mindist2, &maxdist2, !qh_MERGEapex); + dist= dist2; + } + if (qh PRINTstatistics) { + if (mergetype == MRGanglecoplanar) { + zinc_(Zacoplanar); + wadd_(Wacoplanartot, dist); + wmax_(Wacoplanarmax, dist); + }else if (mergetype == MRGconcave) { + zinc_(Zconcave); + wadd_(Wconcavetot, dist); + wmax_(Wconcavemax, dist); + }else { /* MRGcoplanar */ + zinc_(Zcoplanar); + wadd_(Wcoplanartot, dist); + wmax_(Wcoplanarmax, dist); + } + } +} /* merge_nonconvex */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergecycle">-</a> + + qh_mergecycle( samecycle, newfacet ) + merge a cycle of facets starting at samecycle into a newfacet + newfacet is a horizon facet with ->normal + samecycle facets are simplicial from an apex + + returns: + initializes vertex neighbors on first merge + samecycle deleted (placed on qh.visible_list) + newfacet at end of qh.facet_list + deleted vertices on qh.del_vertices + + see: + qh_mergefacet() + called by qh_mergecycle_all() for multiple, same cycle facets + + design: + make vertex neighbors if necessary + make ridges for newfacet + merge neighbor sets of samecycle into newfacet + merge ridges of samecycle into newfacet + merge vertex neighbors of samecycle into newfacet + make apex of samecycle the apex of newfacet + if newfacet wasn't a new facet + add its vertices to qh.newvertex_list + delete samecycle facets a make newfacet a newfacet +*/ +void qh_mergecycle (facetT *samecycle, facetT *newfacet) { + int traceonce= False, tracerestore= 0; + vertexT *apex; +#ifndef qh_NOtrace + facetT *same; +#endif + + if (newfacet->tricoplanar) { + if (!qh TRInormals) { + fprintf (qh ferr, "qh_mergecycle: does not work for tricoplanar facets. Use option 'Q11'\n"); + qh_errexit (qh_ERRqhull, newfacet, NULL); + } + newfacet->tricoplanar= False; + newfacet->keepcentrum= False; + } + if (!qh VERTEXneighbors) + qh_vertexneighbors(); + zzinc_(Ztotmerge); + if (qh REPORTfreq2 && qh POSTmerging) { + if (zzval_(Ztotmerge) > qh mergereport + qh REPORTfreq2) + qh_tracemerging(); + } +#ifndef qh_NOtrace + if (qh TRACEmerge == zzval_(Ztotmerge)) + qhmem.IStracing= qh IStracing= qh TRACElevel; + trace2((qh ferr, "qh_mergecycle: merge #%d for facets from cycle f%d into coplanar horizon f%d\n", + zzval_(Ztotmerge), samecycle->id, newfacet->id)); + if (newfacet == qh tracefacet) { + tracerestore= qh IStracing; + qh IStracing= 4; + fprintf (qh ferr, "qh_mergecycle: ========= trace merge %d of samecycle %d into trace f%d, furthest is p%d\n", + zzval_(Ztotmerge), samecycle->id, newfacet->id, qh furthest_id); + traceonce= True; + } + if (qh IStracing >=4) { + fprintf (qh ferr, " same cycle:"); + FORALLsame_cycle_(samecycle) + fprintf(qh ferr, " f%d", same->id); + fprintf (qh ferr, "\n"); + } + if (qh IStracing >=4) + qh_errprint ("MERGING CYCLE", samecycle, newfacet, NULL, NULL); +#endif /* !qh_NOtrace */ + apex= SETfirstt_(samecycle->vertices, vertexT); + qh_makeridges (newfacet); + qh_mergecycle_neighbors (samecycle, newfacet); + qh_mergecycle_ridges (samecycle, newfacet); + qh_mergecycle_vneighbors (samecycle, newfacet); + if (SETfirstt_(newfacet->vertices, vertexT) != apex) + qh_setaddnth (&newfacet->vertices, 0, apex); /* apex has last id */ + if (!newfacet->newfacet) + qh_newvertices (newfacet->vertices); + qh_mergecycle_facets (samecycle, newfacet); + qh_tracemerge (samecycle, newfacet); + /* check for degen_redundant_neighbors after qh_forcedmerges() */ + if (traceonce) { + fprintf (qh ferr, "qh_mergecycle: end of trace facet\n"); + qh IStracing= tracerestore; + } +} /* mergecycle */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergecycle_all">-</a> + + qh_mergecycle_all( facetlist, wasmerge ) + merge all samecycles of coplanar facets into horizon + don't merge facets with ->mergeridge (these already have ->normal) + all facets are simplicial from apex + all facet->cycledone == False + + returns: + all newfacets merged into coplanar horizon facets + deleted vertices on qh.del_vertices + sets wasmerge if any merge + + see: + calls qh_mergecycle for multiple, same cycle facets + + design: + for each facet on facetlist + skip facets with duplicate ridges and normals + check that facet is in a samecycle (->mergehorizon) + if facet only member of samecycle + sets vertex->delridge for all vertices except apex + merge facet into horizon + else + mark all facets in samecycle + remove facets with duplicate ridges from samecycle + merge samecycle into horizon (deletes facets from facetlist) +*/ +void qh_mergecycle_all (facetT *facetlist, boolT *wasmerge) { + facetT *facet, *same, *prev, *horizon; + facetT *samecycle= NULL, *nextfacet, *nextsame; + vertexT *apex, *vertex, **vertexp; + int cycles=0, total=0, facets, nummerge; + + trace2((qh ferr, "qh_mergecycle_all: begin\n")); + for (facet= facetlist; facet && (nextfacet= facet->next); facet= nextfacet) { + if (facet->normal) + continue; + if (!facet->mergehorizon) { + fprintf (qh ferr, "qh_mergecycle_all: f%d without normal\n", facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + horizon= SETfirstt_(facet->neighbors, facetT); + if (facet->f.samecycle == facet) { + zinc_(Zonehorizon); + /* merge distance done in qh_findhorizon */ + apex= SETfirstt_(facet->vertices, vertexT); + FOREACHvertex_(facet->vertices) { + if (vertex != apex) + vertex->delridge= True; + } + horizon->f.newcycle= NULL; + qh_mergefacet (facet, horizon, NULL, NULL, qh_MERGEapex); + }else { + samecycle= facet; + facets= 0; + prev= facet; + for (same= facet->f.samecycle; same; /* FORALLsame_cycle_(facet) */ + same= (same == facet ? NULL :nextsame)) { /* ends at facet */ + nextsame= same->f.samecycle; + if (same->cycledone || same->visible) + qh_infiniteloop (same); + same->cycledone= True; + if (same->normal) { + prev->f.samecycle= same->f.samecycle; /* unlink ->mergeridge */ + same->f.samecycle= NULL; + }else { + prev= same; + facets++; + } + } + while (nextfacet && nextfacet->cycledone) /* will delete samecycle */ + nextfacet= nextfacet->next; + horizon->f.newcycle= NULL; + qh_mergecycle (samecycle, horizon); + nummerge= horizon->nummerge + facets; + if (nummerge > qh_MAXnummerge) + horizon->nummerge= qh_MAXnummerge; + else + horizon->nummerge= nummerge; + zzinc_(Zcyclehorizon); + total += facets; + zzadd_(Zcyclefacettot, facets); + zmax_(Zcyclefacetmax, facets); + } + cycles++; + } + if (cycles) + *wasmerge= True; + trace1((qh ferr, "qh_mergecycle_all: merged %d same cycles or facets into coplanar horizons\n", cycles)); +} /* mergecycle_all */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergecycle_facets">-</a> + + qh_mergecycle_facets( samecycle, newfacet ) + finish merge of samecycle into newfacet + + returns: + samecycle prepended to visible_list for later deletion and partitioning + each facet->f.replace == newfacet + + newfacet moved to end of qh.facet_list + makes newfacet a newfacet (get's facet1->id if it was old) + sets newfacet->newmerge + clears newfacet->center (unless merging into a large facet) + clears newfacet->tested and ridge->tested for facet1 + + adds neighboring facets to facet_mergeset if redundant or degenerate + + design: + make newfacet a new facet and set its flags + move samecycle facets to qh.visible_list for later deletion + unless newfacet is large + remove its centrum +*/ +void qh_mergecycle_facets (facetT *samecycle, facetT *newfacet) { + facetT *same, *next; + + trace4((qh ferr, "qh_mergecycle_facets: make newfacet new and samecycle deleted\n")); + qh_removefacet(newfacet); /* append as a newfacet to end of qh facet_list */ + qh_appendfacet(newfacet); + newfacet->newfacet= True; + newfacet->simplicial= False; + newfacet->newmerge= True; + + for (same= samecycle->f.samecycle; same; same= (same == samecycle ? NULL : next)) { + next= same->f.samecycle; /* reused by willdelete */ + qh_willdelete (same, newfacet); + } + if (newfacet->center + && qh_setsize (newfacet->vertices) <= qh hull_dim + qh_MAXnewcentrum) { + qh_memfree (newfacet->center, qh normal_size); + newfacet->center= NULL; + } + trace3((qh ferr, "qh_mergecycle_facets: merged facets from cycle f%d into f%d\n", + samecycle->id, newfacet->id)); +} /* mergecycle_facets */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergecycle_neighbors">-</a> + + qh_mergecycle_neighbors( samecycle, newfacet ) + add neighbors for samecycle facets to newfacet + + returns: + newfacet with updated neighbors and vice-versa + newfacet has ridges + all neighbors of newfacet marked with qh.visit_id + samecycle facets marked with qh.visit_id-1 + ridges updated for simplicial neighbors of samecycle with a ridge + + notes: + assumes newfacet not in samecycle + usually, samecycle facets are new, simplicial facets without internal ridges + not so if horizon facet is coplanar to two different samecycles + + see: + qh_mergeneighbors() + + design: + check samecycle + delete neighbors from newfacet that are also in samecycle + for each neighbor of a facet in samecycle + if neighbor is simplicial + if first visit + move the neighbor relation to newfacet + update facet links for its ridges + else + make ridges for neighbor + remove samecycle reference + else + update neighbor sets +*/ +void qh_mergecycle_neighbors(facetT *samecycle, facetT *newfacet) { + facetT *same, *neighbor, **neighborp; + int delneighbors= 0, newneighbors= 0; + unsigned int samevisitid; + ridgeT *ridge, **ridgep; + + samevisitid= ++qh visit_id; + FORALLsame_cycle_(samecycle) { + if (same->visitid == samevisitid || same->visible) + qh_infiniteloop (samecycle); + same->visitid= samevisitid; + } + newfacet->visitid= ++qh visit_id; + trace4((qh ferr, "qh_mergecycle_neighbors: delete shared neighbors from newfacet\n")); + FOREACHneighbor_(newfacet) { + if (neighbor->visitid == samevisitid) { + SETref_(neighbor)= NULL; /* samecycle neighbors deleted */ + delneighbors++; + }else + neighbor->visitid= qh visit_id; + } + qh_setcompact (newfacet->neighbors); + + trace4((qh ferr, "qh_mergecycle_neighbors: update neighbors\n")); + FORALLsame_cycle_(samecycle) { + FOREACHneighbor_(same) { + if (neighbor->visitid == samevisitid) + continue; + if (neighbor->simplicial) { + if (neighbor->visitid != qh visit_id) { + qh_setappend (&newfacet->neighbors, neighbor); + qh_setreplace (neighbor->neighbors, same, newfacet); + newneighbors++; + neighbor->visitid= qh visit_id; + FOREACHridge_(neighbor->ridges) { /* update ridge in case of qh_makeridges */ + if (ridge->top == same) { + ridge->top= newfacet; + break; + }else if (ridge->bottom == same) { + ridge->bottom= newfacet; + break; + } + } + }else { + qh_makeridges (neighbor); + qh_setdel (neighbor->neighbors, same); + /* same can't be horizon facet for neighbor */ + } + }else { /* non-simplicial neighbor */ + qh_setdel (neighbor->neighbors, same); + if (neighbor->visitid != qh visit_id) { + qh_setappend (&neighbor->neighbors, newfacet); + qh_setappend (&newfacet->neighbors, neighbor); + neighbor->visitid= qh visit_id; + newneighbors++; + } + } + } + } + trace2((qh ferr, "qh_mergecycle_neighbors: deleted %d neighbors and added %d\n", + delneighbors, newneighbors)); +} /* mergecycle_neighbors */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergecycle_ridges">-</a> + + qh_mergecycle_ridges( samecycle, newfacet ) + add ridges/neighbors for facets in samecycle to newfacet + all new/old neighbors of newfacet marked with qh.visit_id + facets in samecycle marked with qh.visit_id-1 + newfacet marked with qh.visit_id + + returns: + newfacet has merged ridges + + notes: + ridge already updated for simplicial neighbors of samecycle with a ridge + + see: + qh_mergeridges() + qh_makeridges() + + design: + remove ridges between newfacet and samecycle + for each facet in samecycle + for each ridge in facet + update facet pointers in ridge + skip ridges processed in qh_mergecycle_neighors + free ridges between newfacet and samecycle + free ridges between facets of samecycle (on 2nd visit) + append remaining ridges to newfacet + if simpilicial facet + for each neighbor of facet + if simplicial facet + and not samecycle facet or newfacet + make ridge between neighbor and newfacet +*/ +void qh_mergecycle_ridges(facetT *samecycle, facetT *newfacet) { + facetT *same, *neighbor= NULL; + int numold=0, numnew=0; + int neighbor_i, neighbor_n; + unsigned int samevisitid; + ridgeT *ridge, **ridgep; + boolT toporient; + void **freelistp; /* used !qh_NOmem */ + + trace4((qh ferr, "qh_mergecycle_ridges: delete shared ridges from newfacet\n")); + samevisitid= qh visit_id -1; + FOREACHridge_(newfacet->ridges) { + neighbor= otherfacet_(ridge, newfacet); + if (neighbor->visitid == samevisitid) + SETref_(ridge)= NULL; /* ridge free'd below */ + } + qh_setcompact (newfacet->ridges); + + trace4((qh ferr, "qh_mergecycle_ridges: add ridges to newfacet\n")); + FORALLsame_cycle_(samecycle) { + FOREACHridge_(same->ridges) { + if (ridge->top == same) { + ridge->top= newfacet; + neighbor= ridge->bottom; + }else if (ridge->bottom == same) { + ridge->bottom= newfacet; + neighbor= ridge->top; + }else if (ridge->top == newfacet || ridge->bottom == newfacet) { + qh_setappend (&newfacet->ridges, ridge); + numold++; /* already set by qh_mergecycle_neighbors */ + continue; + }else { + fprintf (qh ferr, "qhull internal error (qh_mergecycle_ridges): bad ridge r%d\n", ridge->id); + qh_errexit (qh_ERRqhull, NULL, ridge); + } + if (neighbor == newfacet) { + qh_setfree(&(ridge->vertices)); + qh_memfree_(ridge, sizeof(ridgeT), freelistp); + numold++; + }else if (neighbor->visitid == samevisitid) { + qh_setdel (neighbor->ridges, ridge); + qh_setfree(&(ridge->vertices)); + qh_memfree_(ridge, sizeof(ridgeT), freelistp); + numold++; + }else { + qh_setappend (&newfacet->ridges, ridge); + numold++; + } + } + if (same->ridges) + qh_settruncate (same->ridges, 0); + if (!same->simplicial) + continue; + FOREACHneighbor_i_(same) { /* note: !newfact->simplicial */ + if (neighbor->visitid != samevisitid && neighbor->simplicial) { + ridge= qh_newridge(); + ridge->vertices= qh_setnew_delnthsorted (same->vertices, qh hull_dim, + neighbor_i, 0); + toporient= same->toporient ^ (neighbor_i & 0x1); + if (toporient) { + ridge->top= newfacet; + ridge->bottom= neighbor; + }else { + ridge->top= neighbor; + ridge->bottom= newfacet; + } + qh_setappend(&(newfacet->ridges), ridge); + qh_setappend(&(neighbor->ridges), ridge); + numnew++; + } + } + } + + trace2((qh ferr, "qh_mergecycle_ridges: found %d old ridges and %d new ones\n", + numold, numnew)); +} /* mergecycle_ridges */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergecycle_vneighbors">-</a> + + qh_mergecycle_vneighbors( samecycle, newfacet ) + create vertex neighbors for newfacet from vertices of facets in samecycle + samecycle marked with visitid == qh.visit_id - 1 + + returns: + newfacet vertices with updated neighbors + marks newfacet with qh.visit_id-1 + deletes vertices that are merged away + sets delridge on all vertices (faster here than in mergecycle_ridges) + + see: + qh_mergevertex_neighbors() + + design: + for each vertex of samecycle facet + set vertex->delridge + delete samecycle facets from vertex neighbors + append newfacet to vertex neighbors + if vertex only in newfacet + delete it from newfacet + add it to qh.del_vertices for later deletion +*/ +void qh_mergecycle_vneighbors (facetT *samecycle, facetT *newfacet) { + facetT *neighbor, **neighborp; + unsigned int mergeid; + vertexT *vertex, **vertexp, *apex; + setT *vertices; + + trace4((qh ferr, "qh_mergecycle_vneighbors: update vertex neighbors for newfacet\n")); + mergeid= qh visit_id - 1; + newfacet->visitid= mergeid; + vertices= qh_basevertices (samecycle); /* temp */ + apex= SETfirstt_(samecycle->vertices, vertexT); + qh_setappend (&vertices, apex); + FOREACHvertex_(vertices) { + vertex->delridge= True; + FOREACHneighbor_(vertex) { + if (neighbor->visitid == mergeid) + SETref_(neighbor)= NULL; + } + qh_setcompact (vertex->neighbors); + qh_setappend (&vertex->neighbors, newfacet); + if (!SETsecond_(vertex->neighbors)) { + zinc_(Zcyclevertex); + trace2((qh ferr, "qh_mergecycle_vneighbors: deleted v%d when merging cycle f%d into f%d\n", + vertex->id, samecycle->id, newfacet->id)); + qh_setdelsorted (newfacet->vertices, vertex); + vertex->deleted= True; + qh_setappend (&qh del_vertices, vertex); + } + } + qh_settempfree (&vertices); + trace3((qh ferr, "qh_mergecycle_vneighbors: merged vertices from cycle f%d into f%d\n", + samecycle->id, newfacet->id)); +} /* mergecycle_vneighbors */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergefacet">-</a> + + qh_mergefacet( facet1, facet2, mindist, maxdist, mergeapex ) + merges facet1 into facet2 + mergeapex==qh_MERGEapex if merging new facet into coplanar horizon + + returns: + qh.max_outside and qh.min_vertex updated + initializes vertex neighbors on first merge + + returns: + facet2 contains facet1's vertices, neighbors, and ridges + facet2 moved to end of qh.facet_list + makes facet2 a newfacet + sets facet2->newmerge set + clears facet2->center (unless merging into a large facet) + clears facet2->tested and ridge->tested for facet1 + + facet1 prepended to visible_list for later deletion and partitioning + facet1->f.replace == facet2 + + adds neighboring facets to facet_mergeset if redundant or degenerate + + notes: + mindist/maxdist may be NULL + traces merge if fmax_(maxdist,-mindist) > TRACEdist + + see: + qh_mergecycle() + + design: + trace merge and check for degenerate simplex + make ridges for both facets + update qh.max_outside, qh.max_vertex, qh.min_vertex + update facet2->maxoutside and keepcentrum + update facet2->nummerge + update tested flags for facet2 + if facet1 is simplicial + merge facet1 into facet2 + else + merge facet1's neighbors into facet2 + merge facet1's ridges into facet2 + merge facet1's vertices into facet2 + merge facet1's vertex neighbors into facet2 + add facet2's vertices to qh.new_vertexlist + unless qh_MERGEapex + test facet2 for degenerate or redundant neighbors + move facet1 to qh.visible_list for later deletion + move facet2 to end of qh.newfacet_list +*/ +void qh_mergefacet(facetT *facet1, facetT *facet2, realT *mindist, realT *maxdist, boolT mergeapex) { + boolT traceonce= False; + vertexT *vertex, **vertexp; + int tracerestore=0, nummerge; + + if (facet1->tricoplanar || facet2->tricoplanar) { + if (!qh TRInormals) { + fprintf (qh ferr, "qh_mergefacet: does not work for tricoplanar facets. Use option 'Q11'\n"); + qh_errexit2 (qh_ERRqhull, facet1, facet2); + } + if (facet2->tricoplanar) { + facet2->tricoplanar= False; + facet2->keepcentrum= False; + } + } + zzinc_(Ztotmerge); + if (qh REPORTfreq2 && qh POSTmerging) { + if (zzval_(Ztotmerge) > qh mergereport + qh REPORTfreq2) + qh_tracemerging(); + } +#ifndef qh_NOtrace + if (qh build_cnt >= qh RERUN) { + if (mindist && (-*mindist > qh TRACEdist || *maxdist > qh TRACEdist)) { + tracerestore= 0; + qh IStracing= qh TRACElevel; + traceonce= True; + fprintf (qh ferr, "qh_mergefacet: ========= trace wide merge #%d (%2.2g) for f%d into f%d, last point was p%d\n", zzval_(Ztotmerge), + fmax_(-*mindist, *maxdist), facet1->id, facet2->id, qh furthest_id); + }else if (facet1 == qh tracefacet || facet2 == qh tracefacet) { + tracerestore= qh IStracing; + qh IStracing= 4; + traceonce= True; + fprintf (qh ferr, "qh_mergefacet: ========= trace merge #%d involving f%d, furthest is p%d\n", + zzval_(Ztotmerge), qh tracefacet_id, qh furthest_id); + } + } + if (qh IStracing >= 2) { + realT mergemin= -2; + realT mergemax= -2; + + if (mindist) { + mergemin= *mindist; + mergemax= *maxdist; + } + fprintf (qh ferr, "qh_mergefacet: #%d merge f%d into f%d, mindist= %2.2g, maxdist= %2.2g\n", + zzval_(Ztotmerge), facet1->id, facet2->id, mergemin, mergemax); + } +#endif /* !qh_NOtrace */ + if (facet1 == facet2 || facet1->visible || facet2->visible) { + fprintf (qh ferr, "qhull internal error (qh_mergefacet): either f%d and f%d are the same or one is a visible facet\n", + facet1->id, facet2->id); + qh_errexit2 (qh_ERRqhull, facet1, facet2); + } + if (qh num_facets - qh num_visible <= qh hull_dim + 1) { + fprintf(qh ferr, "\n\ +qhull precision error: Only %d facets remain. Can not merge another\n\ +pair. The input is too degenerate or the convexity constraints are\n\ +too strong.\n", qh hull_dim+1); + if (qh hull_dim >= 5 && !qh MERGEexact) + fprintf(qh ferr, "Option 'Qx' may avoid this problem.\n"); + qh_errexit(qh_ERRinput, NULL, NULL); + } + if (!qh VERTEXneighbors) + qh_vertexneighbors(); + qh_makeridges(facet1); + qh_makeridges(facet2); + if (qh IStracing >=4) + qh_errprint ("MERGING", facet1, facet2, NULL, NULL); + if (mindist) { + maximize_(qh max_outside, *maxdist); + maximize_(qh max_vertex, *maxdist); +#if qh_MAXoutside + maximize_(facet2->maxoutside, *maxdist); +#endif + minimize_(qh min_vertex, *mindist); + if (!facet2->keepcentrum + && (*maxdist > qh WIDEfacet || *mindist < -qh WIDEfacet)) { + facet2->keepcentrum= True; + zinc_(Zwidefacet); + } + } + nummerge= facet1->nummerge + facet2->nummerge + 1; + if (nummerge >= qh_MAXnummerge) + facet2->nummerge= qh_MAXnummerge; + else + facet2->nummerge= nummerge; + facet2->newmerge= True; + facet2->dupridge= False; + qh_updatetested (facet1, facet2); + if (qh hull_dim > 2 && qh_setsize (facet1->vertices) == qh hull_dim) + qh_mergesimplex (facet1, facet2, mergeapex); + else { + qh vertex_visit++; + FOREACHvertex_(facet2->vertices) + vertex->visitid= qh vertex_visit; + if (qh hull_dim == 2) + qh_mergefacet2d(facet1, facet2); + else { + qh_mergeneighbors(facet1, facet2); + qh_mergevertices(facet1->vertices, &facet2->vertices); + } + qh_mergeridges(facet1, facet2); + qh_mergevertex_neighbors(facet1, facet2); + if (!facet2->newfacet) + qh_newvertices (facet2->vertices); + } + if (!mergeapex) + qh_degen_redundant_neighbors (facet2, facet1); + if (facet2->coplanar || !facet2->newfacet) { + zinc_(Zmergeintohorizon); + }else if (!facet1->newfacet && facet2->newfacet) { + zinc_(Zmergehorizon); + }else { + zinc_(Zmergenew); + } + qh_willdelete (facet1, facet2); + qh_removefacet(facet2); /* append as a newfacet to end of qh facet_list */ + qh_appendfacet(facet2); + facet2->newfacet= True; + facet2->tested= False; + qh_tracemerge (facet1, facet2); + if (traceonce) { + fprintf (qh ferr, "qh_mergefacet: end of wide tracing\n"); + qh IStracing= tracerestore; + } +} /* mergefacet */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergefacet2d">-</a> + + qh_mergefacet2d( facet1, facet2 ) + in 2d, merges neighbors and vertices of facet1 into facet2 + + returns: + build ridges for neighbors if necessary + facet2 looks like a simplicial facet except for centrum, ridges + neighbors are opposite the corresponding vertex + maintains orientation of facet2 + + notes: + qh_mergefacet() retains non-simplicial structures + they are not needed in 2d, but later routines may use them + preserves qh.vertex_visit for qh_mergevertex_neighbors() + + design: + get vertices and neighbors + determine new vertices and neighbors + set new vertices and neighbors and adjust orientation + make ridges for new neighbor if needed +*/ +void qh_mergefacet2d (facetT *facet1, facetT *facet2) { + vertexT *vertex1A, *vertex1B, *vertex2A, *vertex2B, *vertexA, *vertexB; + facetT *neighbor1A, *neighbor1B, *neighbor2A, *neighbor2B, *neighborA, *neighborB; + + vertex1A= SETfirstt_(facet1->vertices, vertexT); + vertex1B= SETsecondt_(facet1->vertices, vertexT); + vertex2A= SETfirstt_(facet2->vertices, vertexT); + vertex2B= SETsecondt_(facet2->vertices, vertexT); + neighbor1A= SETfirstt_(facet1->neighbors, facetT); + neighbor1B= SETsecondt_(facet1->neighbors, facetT); + neighbor2A= SETfirstt_(facet2->neighbors, facetT); + neighbor2B= SETsecondt_(facet2->neighbors, facetT); + if (vertex1A == vertex2A) { + vertexA= vertex1B; + vertexB= vertex2B; + neighborA= neighbor2A; + neighborB= neighbor1A; + }else if (vertex1A == vertex2B) { + vertexA= vertex1B; + vertexB= vertex2A; + neighborA= neighbor2B; + neighborB= neighbor1A; + }else if (vertex1B == vertex2A) { + vertexA= vertex1A; + vertexB= vertex2B; + neighborA= neighbor2A; + neighborB= neighbor1B; + }else { /* 1B == 2B */ + vertexA= vertex1A; + vertexB= vertex2A; + neighborA= neighbor2B; + neighborB= neighbor1B; + } + /* vertexB always from facet2, neighborB always from facet1 */ + if (vertexA->id > vertexB->id) { + SETfirst_(facet2->vertices)= vertexA; + SETsecond_(facet2->vertices)= vertexB; + if (vertexB == vertex2A) + facet2->toporient= !facet2->toporient; + SETfirst_(facet2->neighbors)= neighborA; + SETsecond_(facet2->neighbors)= neighborB; + }else { + SETfirst_(facet2->vertices)= vertexB; + SETsecond_(facet2->vertices)= vertexA; + if (vertexB == vertex2B) + facet2->toporient= !facet2->toporient; + SETfirst_(facet2->neighbors)= neighborB; + SETsecond_(facet2->neighbors)= neighborA; + } + qh_makeridges (neighborB); + qh_setreplace(neighborB->neighbors, facet1, facet2); + trace4((qh ferr, "qh_mergefacet2d: merged v%d and neighbor f%d of f%d into f%d\n", + vertexA->id, neighborB->id, facet1->id, facet2->id)); +} /* mergefacet2d */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergeneighbors">-</a> + + qh_mergeneighbors( facet1, facet2 ) + merges the neighbors of facet1 into facet2 + + see: + qh_mergecycle_neighbors() + + design: + for each neighbor of facet1 + if neighbor is also a neighbor of facet2 + if neighbor is simpilicial + make ridges for later deletion as a degenerate facet + update its neighbor set + else + move the neighbor relation to facet2 + remove the neighbor relation for facet1 and facet2 +*/ +void qh_mergeneighbors(facetT *facet1, facetT *facet2) { + facetT *neighbor, **neighborp; + + trace4((qh ferr, "qh_mergeneighbors: merge neighbors of f%d and f%d\n", + facet1->id, facet2->id)); + qh visit_id++; + FOREACHneighbor_(facet2) { + neighbor->visitid= qh visit_id; + } + FOREACHneighbor_(facet1) { + if (neighbor->visitid == qh visit_id) { + if (neighbor->simplicial) /* is degen, needs ridges */ + qh_makeridges (neighbor); + if (SETfirstt_(neighbor->neighbors, facetT) != facet1) /*keep newfacet->horizon*/ + qh_setdel (neighbor->neighbors, facet1); + else { + qh_setdel(neighbor->neighbors, facet2); + qh_setreplace(neighbor->neighbors, facet1, facet2); + } + }else if (neighbor != facet2) { + qh_setappend(&(facet2->neighbors), neighbor); + qh_setreplace(neighbor->neighbors, facet1, facet2); + } + } + qh_setdel(facet1->neighbors, facet2); /* here for makeridges */ + qh_setdel(facet2->neighbors, facet1); +} /* mergeneighbors */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergeridges">-</a> + + qh_mergeridges( facet1, facet2 ) + merges the ridge set of facet1 into facet2 + + returns: + may delete all ridges for a vertex + sets vertex->delridge on deleted ridges + + see: + qh_mergecycle_ridges() + + design: + delete ridges between facet1 and facet2 + mark (delridge) vertices on these ridges for later testing + for each remaining ridge + rename facet1 to facet2 +*/ +void qh_mergeridges(facetT *facet1, facetT *facet2) { + ridgeT *ridge, **ridgep; + vertexT *vertex, **vertexp; + + trace4((qh ferr, "qh_mergeridges: merge ridges of f%d and f%d\n", + facet1->id, facet2->id)); + FOREACHridge_(facet2->ridges) { + if ((ridge->top == facet1) || (ridge->bottom == facet1)) { + FOREACHvertex_(ridge->vertices) + vertex->delridge= True; + qh_delridge(ridge); /* expensive in high-d, could rebuild */ + ridgep--; /*repeat*/ + } + } + FOREACHridge_(facet1->ridges) { + if (ridge->top == facet1) + ridge->top= facet2; + else + ridge->bottom= facet2; + qh_setappend(&(facet2->ridges), ridge); + } +} /* mergeridges */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergesimplex">-</a> + + qh_mergesimplex( facet1, facet2, mergeapex ) + merge simplicial facet1 into facet2 + mergeapex==qh_MERGEapex if merging samecycle into horizon facet + vertex id is latest (most recently created) + facet1 may be contained in facet2 + ridges exist for both facets + + returns: + facet2 with updated vertices, ridges, neighbors + updated neighbors for facet1's vertices + facet1 not deleted + sets vertex->delridge on deleted ridges + + notes: + special case code since this is the most common merge + called from qh_mergefacet() + + design: + if qh_MERGEapex + add vertices of facet2 to qh.new_vertexlist if necessary + add apex to facet2 + else + for each ridge between facet1 and facet2 + set vertex->delridge + determine the apex for facet1 (i.e., vertex to be merged) + unless apex already in facet2 + insert apex into vertices for facet2 + add vertices of facet2 to qh.new_vertexlist if necessary + add apex to qh.new_vertexlist if necessary + for each vertex of facet1 + if apex + rename facet1 to facet2 in its vertex neighbors + else + delete facet1 from vertex neighors + if only in facet2 + add vertex to qh.del_vertices for later deletion + for each ridge of facet1 + delete ridges between facet1 and facet2 + append other ridges to facet2 after renaming facet to facet2 +*/ +void qh_mergesimplex(facetT *facet1, facetT *facet2, boolT mergeapex) { + vertexT *vertex, **vertexp, *apex; + ridgeT *ridge, **ridgep; + boolT issubset= False; + int vertex_i= -1, vertex_n; + facetT *neighbor, **neighborp, *otherfacet; + + if (mergeapex) { + if (!facet2->newfacet) + qh_newvertices (facet2->vertices); /* apex is new */ + apex= SETfirstt_(facet1->vertices, vertexT); + if (SETfirstt_(facet2->vertices, vertexT) != apex) + qh_setaddnth (&facet2->vertices, 0, apex); /* apex has last id */ + else + issubset= True; + }else { + zinc_(Zmergesimplex); + FOREACHvertex_(facet1->vertices) + vertex->seen= False; + FOREACHridge_(facet1->ridges) { + if (otherfacet_(ridge, facet1) == facet2) { + FOREACHvertex_(ridge->vertices) { + vertex->seen= True; + vertex->delridge= True; + } + break; + } + } + FOREACHvertex_(facet1->vertices) { + if (!vertex->seen) + break; /* must occur */ + } + apex= vertex; + trace4((qh ferr, "qh_mergesimplex: merge apex v%d of f%d into facet f%d\n", + apex->id, facet1->id, facet2->id)); + FOREACHvertex_i_(facet2->vertices) { + if (vertex->id < apex->id) { + break; + }else if (vertex->id == apex->id) { + issubset= True; + break; + } + } + if (!issubset) + qh_setaddnth (&facet2->vertices, vertex_i, apex); + if (!facet2->newfacet) + qh_newvertices (facet2->vertices); + else if (!apex->newlist) { + qh_removevertex (apex); + qh_appendvertex (apex); + } + } + trace4((qh ferr, "qh_mergesimplex: update vertex neighbors of f%d\n", + facet1->id)); + FOREACHvertex_(facet1->vertices) { + if (vertex == apex && !issubset) + qh_setreplace (vertex->neighbors, facet1, facet2); + else { + qh_setdel (vertex->neighbors, facet1); + if (!SETsecond_(vertex->neighbors)) + qh_mergevertex_del (vertex, facet1, facet2); + } + } + trace4((qh ferr, "qh_mergesimplex: merge ridges and neighbors of f%d into f%d\n", + facet1->id, facet2->id)); + qh visit_id++; + FOREACHneighbor_(facet2) + neighbor->visitid= qh visit_id; + FOREACHridge_(facet1->ridges) { + otherfacet= otherfacet_(ridge, facet1); + if (otherfacet == facet2) { + qh_setdel (facet2->ridges, ridge); + qh_setfree(&(ridge->vertices)); + qh_memfree (ridge, sizeof(ridgeT)); + qh_setdel (facet2->neighbors, facet1); + }else { + qh_setappend (&facet2->ridges, ridge); + if (otherfacet->visitid != qh visit_id) { + qh_setappend (&facet2->neighbors, otherfacet); + qh_setreplace (otherfacet->neighbors, facet1, facet2); + otherfacet->visitid= qh visit_id; + }else { + if (otherfacet->simplicial) /* is degen, needs ridges */ + qh_makeridges (otherfacet); + if (SETfirstt_(otherfacet->neighbors, facetT) != facet1) + qh_setdel (otherfacet->neighbors, facet1); + else { /*keep newfacet->neighbors->horizon*/ + qh_setdel(otherfacet->neighbors, facet2); + qh_setreplace(otherfacet->neighbors, facet1, facet2); + } + } + if (ridge->top == facet1) /* wait until after qh_makeridges */ + ridge->top= facet2; + else + ridge->bottom= facet2; + } + } + SETfirst_(facet1->ridges)= NULL; /* it will be deleted */ + trace3((qh ferr, "qh_mergesimplex: merged simplex f%d apex v%d into facet f%d\n", + facet1->id, getid_(apex), facet2->id)); +} /* mergesimplex */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergevertex_del">-</a> + + qh_mergevertex_del( vertex, facet1, facet2 ) + delete a vertex because of merging facet1 into facet2 + + returns: + deletes vertex from facet2 + adds vertex to qh.del_vertices for later deletion +*/ +void qh_mergevertex_del (vertexT *vertex, facetT *facet1, facetT *facet2) { + + zinc_(Zmergevertex); + trace2((qh ferr, "qh_mergevertex_del: deleted v%d when merging f%d into f%d\n", + vertex->id, facet1->id, facet2->id)); + qh_setdelsorted (facet2->vertices, vertex); + vertex->deleted= True; + qh_setappend (&qh del_vertices, vertex); +} /* mergevertex_del */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergevertex_neighbors">-</a> + + qh_mergevertex_neighbors( facet1, facet2 ) + merge the vertex neighbors of facet1 to facet2 + + returns: + if vertex is current qh.vertex_visit + deletes facet1 from vertex->neighbors + else + renames facet1 to facet2 in vertex->neighbors + deletes vertices if only one neighbor + + notes: + assumes vertex neighbor sets are good +*/ +void qh_mergevertex_neighbors(facetT *facet1, facetT *facet2) { + vertexT *vertex, **vertexp; + + trace4((qh ferr, "qh_mergevertex_neighbors: merge vertex neighbors of f%d and f%d\n", + facet1->id, facet2->id)); + if (qh tracevertex) { + fprintf (qh ferr, "qh_mergevertex_neighbors: of f%d and f%d at furthest p%d f0= %p\n", + facet1->id, facet2->id, qh furthest_id, qh tracevertex->neighbors->e[0].p); + qh_errprint ("TRACE", NULL, NULL, NULL, qh tracevertex); + } + FOREACHvertex_(facet1->vertices) { + if (vertex->visitid != qh vertex_visit) + qh_setreplace(vertex->neighbors, facet1, facet2); + else { + qh_setdel(vertex->neighbors, facet1); + if (!SETsecond_(vertex->neighbors)) + qh_mergevertex_del (vertex, facet1, facet2); + } + } + if (qh tracevertex) + qh_errprint ("TRACE", NULL, NULL, NULL, qh tracevertex); +} /* mergevertex_neighbors */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="mergevertices">-</a> + + qh_mergevertices( vertices1, vertices2 ) + merges the vertex set of facet1 into facet2 + + returns: + replaces vertices2 with merged set + preserves vertex_visit for qh_mergevertex_neighbors + updates qh.newvertex_list + + design: + create a merged set of both vertices (in inverse id order) +*/ +void qh_mergevertices(setT *vertices1, setT **vertices2) { + int newsize= qh_setsize(vertices1)+qh_setsize(*vertices2) - qh hull_dim + 1; + setT *mergedvertices; + vertexT *vertex, **vertexp, **vertex2= SETaddr_(*vertices2, vertexT); + + mergedvertices= qh_settemp (newsize); + FOREACHvertex_(vertices1) { + if (!*vertex2 || vertex->id > (*vertex2)->id) + qh_setappend (&mergedvertices, vertex); + else { + while (*vertex2 && (*vertex2)->id > vertex->id) + qh_setappend (&mergedvertices, *vertex2++); + if (!*vertex2 || (*vertex2)->id < vertex->id) + qh_setappend (&mergedvertices, vertex); + else + qh_setappend (&mergedvertices, *vertex2++); + } + } + while (*vertex2) + qh_setappend (&mergedvertices, *vertex2++); + if (newsize < qh_setsize (mergedvertices)) { + fprintf (qh ferr, "qhull internal error (qh_mergevertices): facets did not share a ridge\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + qh_setfree(vertices2); + *vertices2= mergedvertices; + qh_settemppop (); +} /* mergevertices */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="neighbor_intersections">-</a> + + qh_neighbor_intersections( vertex ) + return intersection of all vertices in vertex->neighbors except for vertex + + returns: + returns temporary set of vertices + does not include vertex + NULL if a neighbor is simplicial + NULL if empty set + + notes: + used for renaming vertices + + design: + initialize the intersection set with vertices of the first two neighbors + delete vertex from the intersection + for each remaining neighbor + intersect its vertex set with the intersection set + return NULL if empty + return the intersection set +*/ +setT *qh_neighbor_intersections (vertexT *vertex) { + facetT *neighbor, **neighborp, *neighborA, *neighborB; + setT *intersect; + int neighbor_i, neighbor_n; + + FOREACHneighbor_(vertex) { + if (neighbor->simplicial) + return NULL; + } + neighborA= SETfirstt_(vertex->neighbors, facetT); + neighborB= SETsecondt_(vertex->neighbors, facetT); + zinc_(Zintersectnum); + if (!neighborA) + return NULL; + if (!neighborB) + intersect= qh_setcopy (neighborA->vertices, 0); + else + intersect= qh_vertexintersect_new (neighborA->vertices, neighborB->vertices); + qh_settemppush (intersect); + qh_setdelsorted (intersect, vertex); + FOREACHneighbor_i_(vertex) { + if (neighbor_i >= 2) { + zinc_(Zintersectnum); + qh_vertexintersect (&intersect, neighbor->vertices); + if (!SETfirst_(intersect)) { + zinc_(Zintersectfail); + qh_settempfree (&intersect); + return NULL; + } + } + } + trace3((qh ferr, "qh_neighbor_intersections: %d vertices in neighbor intersection of v%d\n", + qh_setsize (intersect), vertex->id)); + return intersect; +} /* neighbor_intersections */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="newvertices">-</a> + + qh_newvertices( vertices ) + add vertices to end of qh.vertex_list (marks as new vertices) + + returns: + vertices on qh.newvertex_list + vertex->newlist set +*/ +void qh_newvertices (setT *vertices) { + vertexT *vertex, **vertexp; + + FOREACHvertex_(vertices) { + if (!vertex->newlist) { + qh_removevertex (vertex); + qh_appendvertex (vertex); + } + } +} /* newvertices */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="reducevertices">-</a> + + qh_reducevertices() + reduce extra vertices, shared vertices, and redundant vertices + facet->newmerge is set if merged since last call + if !qh.MERGEvertices, only removes extra vertices + + returns: + True if also merged degen_redundant facets + vertices are renamed if possible + clears facet->newmerge and vertex->delridge + + notes: + ignored if 2-d + + design: + merge any degenerate or redundant facets + for each newly merged facet + remove extra vertices + if qh.MERGEvertices + for each newly merged facet + for each vertex + if vertex was on a deleted ridge + rename vertex if it is shared + remove delridge flag from new vertices +*/ +boolT qh_reducevertices (void) { + int numshare=0, numrename= 0; + boolT degenredun= False; + facetT *newfacet; + vertexT *vertex, **vertexp; + + if (qh hull_dim == 2) + return False; + if (qh_merge_degenredundant()) + degenredun= True; + LABELrestart: + FORALLnew_facets { + if (newfacet->newmerge) { + if (!qh MERGEvertices) + newfacet->newmerge= False; + qh_remove_extravertices (newfacet); + } + } + if (!qh MERGEvertices) + return False; + FORALLnew_facets { + if (newfacet->newmerge) { + newfacet->newmerge= False; + FOREACHvertex_(newfacet->vertices) { + if (vertex->delridge) { + if (qh_rename_sharedvertex (vertex, newfacet)) { + numshare++; + vertexp--; /* repeat since deleted vertex */ + } + } + } + } + } + FORALLvertex_(qh newvertex_list) { + if (vertex->delridge && !vertex->deleted) { + vertex->delridge= False; + if (qh hull_dim >= 4 && qh_redundant_vertex (vertex)) { + numrename++; + if (qh_merge_degenredundant()) { + degenredun= True; + goto LABELrestart; + } + } + } + } + trace1((qh ferr, "qh_reducevertices: renamed %d shared vertices and %d redundant vertices. Degen? %d\n", + numshare, numrename, degenredun)); + return degenredun; +} /* reducevertices */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="redundant_vertex">-</a> + + qh_redundant_vertex( vertex ) + detect and rename a redundant vertex + vertices have full vertex->neighbors + + returns: + returns true if find a redundant vertex + deletes vertex (vertex->deleted) + + notes: + only needed if vertex->delridge and hull_dim >= 4 + may add degenerate facets to qh.facet_mergeset + doesn't change vertex->neighbors or create redundant facets + + design: + intersect vertices of all facet neighbors of vertex + determine ridges for these vertices + if find a new vertex for vertex amoung these ridges and vertices + rename vertex to the new vertex +*/ +vertexT *qh_redundant_vertex (vertexT *vertex) { + vertexT *newvertex= NULL; + setT *vertices, *ridges; + + trace3((qh ferr, "qh_redundant_vertex: check if v%d can be renamed\n", vertex->id)); + if ((vertices= qh_neighbor_intersections (vertex))) { + ridges= qh_vertexridges (vertex); + if ((newvertex= qh_find_newvertex (vertex, vertices, ridges))) + qh_renamevertex (vertex, newvertex, ridges, NULL, NULL); + qh_settempfree (&ridges); + qh_settempfree (&vertices); + } + return newvertex; +} /* redundant_vertex */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="remove_extravertices">-</a> + + qh_remove_extravertices( facet ) + remove extra vertices from non-simplicial facets + + returns: + returns True if it finds them + + design: + for each vertex in facet + if vertex not in a ridge (i.e., no longer used) + delete vertex from facet + delete facet from vertice's neighbors + unless vertex in another facet + add vertex to qh.del_vertices for later deletion +*/ +boolT qh_remove_extravertices (facetT *facet) { + ridgeT *ridge, **ridgep; + vertexT *vertex, **vertexp; + boolT foundrem= False; + + trace4((qh ferr, "qh_remove_extravertices: test f%d for extra vertices\n", + facet->id)); + FOREACHvertex_(facet->vertices) + vertex->seen= False; + FOREACHridge_(facet->ridges) { + FOREACHvertex_(ridge->vertices) + vertex->seen= True; + } + FOREACHvertex_(facet->vertices) { + if (!vertex->seen) { + foundrem= True; + zinc_(Zremvertex); + qh_setdelsorted (facet->vertices, vertex); + qh_setdel (vertex->neighbors, facet); + if (!qh_setsize (vertex->neighbors)) { + vertex->deleted= True; + qh_setappend (&qh del_vertices, vertex); + zinc_(Zremvertexdel); + trace2((qh ferr, "qh_remove_extravertices: v%d deleted because it's lost all ridges\n", vertex->id)); + }else + trace3((qh ferr, "qh_remove_extravertices: v%d removed from f%d because it's lost all ridges\n", vertex->id, facet->id)); + vertexp--; /*repeat*/ + } + } + return foundrem; +} /* remove_extravertices */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="rename_sharedvertex">-</a> + + qh_rename_sharedvertex( vertex, facet ) + detect and rename if shared vertex in facet + vertices have full ->neighbors + + returns: + newvertex or NULL + the vertex may still exist in other facets (i.e., a neighbor was pinched) + does not change facet->neighbors + updates vertex->neighbors + + notes: + a shared vertex for a facet is only in ridges to one neighbor + this may undo a pinched facet + + it does not catch pinches involving multiple facets. These appear + to be difficult to detect, since an exhaustive search is too expensive. + + design: + if vertex only has two neighbors + determine the ridges that contain the vertex + determine the vertices shared by both neighbors + if can find a new vertex in this set + rename the vertex to the new vertex +*/ +vertexT *qh_rename_sharedvertex (vertexT *vertex, facetT *facet) { + facetT *neighbor, **neighborp, *neighborA= NULL; + setT *vertices, *ridges; + vertexT *newvertex; + + if (qh_setsize (vertex->neighbors) == 2) { + neighborA= SETfirstt_(vertex->neighbors, facetT); + if (neighborA == facet) + neighborA= SETsecondt_(vertex->neighbors, facetT); + }else if (qh hull_dim == 3) + return NULL; + else { + qh visit_id++; + FOREACHneighbor_(facet) + neighbor->visitid= qh visit_id; + FOREACHneighbor_(vertex) { + if (neighbor->visitid == qh visit_id) { + if (neighborA) + return NULL; + neighborA= neighbor; + } + } + if (!neighborA) { + fprintf (qh ferr, "qhull internal error (qh_rename_sharedvertex): v%d's neighbors not in f%d\n", + vertex->id, facet->id); + qh_errprint ("ERRONEOUS", facet, NULL, NULL, vertex); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + } + /* the vertex is shared by facet and neighborA */ + ridges= qh_settemp (qh TEMPsize); + neighborA->visitid= ++qh visit_id; + qh_vertexridges_facet (vertex, facet, &ridges); + trace2((qh ferr, "qh_rename_sharedvertex: p%d (v%d) is shared by f%d (%d ridges) and f%d\n", + qh_pointid(vertex->point), vertex->id, facet->id, qh_setsize (ridges), neighborA->id)); + zinc_(Zintersectnum); + vertices= qh_vertexintersect_new (facet->vertices, neighborA->vertices); + qh_setdel (vertices, vertex); + qh_settemppush (vertices); + if ((newvertex= qh_find_newvertex (vertex, vertices, ridges))) + qh_renamevertex (vertex, newvertex, ridges, facet, neighborA); + qh_settempfree (&vertices); + qh_settempfree (&ridges); + return newvertex; +} /* rename_sharedvertex */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="renameridgevertex">-</a> + + qh_renameridgevertex( ridge, oldvertex, newvertex ) + renames oldvertex as newvertex in ridge + + returns: + + design: + delete oldvertex from ridge + if newvertex already in ridge + copy ridge->noconvex to another ridge if possible + delete the ridge + else + insert newvertex into the ridge + adjust the ridge's orientation +*/ +void qh_renameridgevertex(ridgeT *ridge, vertexT *oldvertex, vertexT *newvertex) { + int nth= 0, oldnth; + facetT *temp; + vertexT *vertex, **vertexp; + + oldnth= qh_setindex (ridge->vertices, oldvertex); + qh_setdelnthsorted (ridge->vertices, oldnth); + FOREACHvertex_(ridge->vertices) { + if (vertex == newvertex) { + zinc_(Zdelridge); + if (ridge->nonconvex) /* only one ridge has nonconvex set */ + qh_copynonconvex (ridge); + qh_delridge (ridge); + trace2((qh ferr, "qh_renameridgevertex: ridge r%d deleted. It contained both v%d and v%d\n", + ridge->id, oldvertex->id, newvertex->id)); + return; + } + if (vertex->id < newvertex->id) + break; + nth++; + } + qh_setaddnth(&ridge->vertices, nth, newvertex); + if (abs(oldnth - nth)%2) { + trace3((qh ferr, "qh_renameridgevertex: swapped the top and bottom of ridge r%d\n", + ridge->id)); + temp= ridge->top; + ridge->top= ridge->bottom; + ridge->bottom= temp; + } +} /* renameridgevertex */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="renamevertex">-</a> + + qh_renamevertex( oldvertex, newvertex, ridges, oldfacet, neighborA ) + renames oldvertex as newvertex in ridges + gives oldfacet/neighborA if oldvertex is shared between two facets + + returns: + oldvertex may still exist afterwards + + + notes: + can not change neighbors of newvertex (since it's a subset) + + design: + for each ridge in ridges + rename oldvertex to newvertex and delete degenerate ridges + if oldfacet not defined + for each neighbor of oldvertex + delete oldvertex from neighbor's vertices + remove extra vertices from neighbor + add oldvertex to qh.del_vertices + else if oldvertex only between oldfacet and neighborA + delete oldvertex from oldfacet and neighborA + add oldvertex to qh.del_vertices + else oldvertex is in oldfacet and neighborA and other facets (i.e., pinched) + delete oldvertex from oldfacet + delete oldfacet from oldvertice's neighbors + remove extra vertices (e.g., oldvertex) from neighborA +*/ +void qh_renamevertex(vertexT *oldvertex, vertexT *newvertex, setT *ridges, facetT *oldfacet, facetT *neighborA) { + facetT *neighbor, **neighborp; + ridgeT *ridge, **ridgep; + boolT istrace= False; + + if (qh IStracing >= 2 || oldvertex->id == qh tracevertex_id || + newvertex->id == qh tracevertex_id) + istrace= True; + FOREACHridge_(ridges) + qh_renameridgevertex (ridge, oldvertex, newvertex); + if (!oldfacet) { + zinc_(Zrenameall); + if (istrace) + fprintf (qh ferr, "qh_renamevertex: renamed v%d to v%d in several facets\n", + oldvertex->id, newvertex->id); + FOREACHneighbor_(oldvertex) { + qh_maydropneighbor (neighbor); + qh_setdelsorted (neighbor->vertices, oldvertex); + if (qh_remove_extravertices (neighbor)) + neighborp--; /* neighbor may be deleted */ + } + if (!oldvertex->deleted) { + oldvertex->deleted= True; + qh_setappend (&qh del_vertices, oldvertex); + } + }else if (qh_setsize (oldvertex->neighbors) == 2) { + zinc_(Zrenameshare); + if (istrace) + fprintf (qh ferr, "qh_renamevertex: renamed v%d to v%d in oldfacet f%d\n", + oldvertex->id, newvertex->id, oldfacet->id); + FOREACHneighbor_(oldvertex) + qh_setdelsorted (neighbor->vertices, oldvertex); + oldvertex->deleted= True; + qh_setappend (&qh del_vertices, oldvertex); + }else { + zinc_(Zrenamepinch); + if (istrace || qh IStracing) + fprintf (qh ferr, "qh_renamevertex: renamed pinched v%d to v%d between f%d and f%d\n", + oldvertex->id, newvertex->id, oldfacet->id, neighborA->id); + qh_setdelsorted (oldfacet->vertices, oldvertex); + qh_setdel (oldvertex->neighbors, oldfacet); + qh_remove_extravertices (neighborA); + } +} /* renamevertex */ + + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="test_appendmerge">-</a> + + qh_test_appendmerge( facet, neighbor ) + tests facet/neighbor for convexity + appends to mergeset if non-convex + if pre-merging, + nop if qh.SKIPconvex, or qh.MERGEexact and coplanar + + returns: + true if appends facet/neighbor to mergeset + sets facet->center as needed + does not change facet->seen + + design: + if qh.cos_max is defined + if the angle between facet normals is too shallow + append an angle-coplanar merge to qh.mergeset + return True + make facet's centrum if needed + if facet's centrum is above the neighbor + set isconcave + else + if facet's centrum is not below the neighbor + set iscoplanar + make neighbor's centrum if needed + if neighbor's centrum is above the facet + set isconcave + else if neighbor's centrum is not below the facet + set iscoplanar + if isconcave or iscoplanar + get angle if needed + append concave or coplanar merge to qh.mergeset +*/ +boolT qh_test_appendmerge (facetT *facet, facetT *neighbor) { + realT dist, dist2= -REALmax, angle= -REALmax; + boolT isconcave= False, iscoplanar= False, okangle= False; + + if (qh SKIPconvex && !qh POSTmerging) + return False; + if ((!qh MERGEexact || qh POSTmerging) && qh cos_max < REALmax/2) { + angle= qh_getangle(facet->normal, neighbor->normal); + zinc_(Zangletests); + if (angle > qh cos_max) { + zinc_(Zcoplanarangle); + qh_appendmergeset(facet, neighbor, MRGanglecoplanar, &angle); + trace2((qh ferr, "qh_test_appendmerge: coplanar angle %4.4g between f%d and f%d\n", + angle, facet->id, neighbor->id)); + return True; + }else + okangle= True; + } + if (!facet->center) + facet->center= qh_getcentrum (facet); + zzinc_(Zcentrumtests); + qh_distplane(facet->center, neighbor, &dist); + if (dist > qh centrum_radius) + isconcave= True; + else { + if (dist > -qh centrum_radius) + iscoplanar= True; + if (!neighbor->center) + neighbor->center= qh_getcentrum (neighbor); + zzinc_(Zcentrumtests); + qh_distplane(neighbor->center, facet, &dist2); + if (dist2 > qh centrum_radius) + isconcave= True; + else if (!iscoplanar && dist2 > -qh centrum_radius) + iscoplanar= True; + } + if (!isconcave && (!iscoplanar || (qh MERGEexact && !qh POSTmerging))) + return False; + if (!okangle && qh ANGLEmerge) { + angle= qh_getangle(facet->normal, neighbor->normal); + zinc_(Zangletests); + } + if (isconcave) { + zinc_(Zconcaveridge); + if (qh ANGLEmerge) + angle += qh_ANGLEconcave + 0.5; + qh_appendmergeset(facet, neighbor, MRGconcave, &angle); + trace0((qh ferr, "qh_test_appendmerge: concave f%d to f%d dist %4.4g and reverse dist %4.4g angle %4.4g during p%d\n", + facet->id, neighbor->id, dist, dist2, angle, qh furthest_id)); + }else /* iscoplanar */ { + zinc_(Zcoplanarcentrum); + qh_appendmergeset(facet, neighbor, MRGcoplanar, &angle); + trace2((qh ferr, "qh_test_appendmerge: coplanar f%d to f%d dist %4.4g, reverse dist %4.4g angle %4.4g\n", + facet->id, neighbor->id, dist, dist2, angle)); + } + return True; +} /* test_appendmerge */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="test_vneighbors">-</a> + + qh_test_vneighbors() + test vertex neighbors for convexity + tests all facets on qh.newfacet_list + + returns: + true if non-convex vneighbors appended to qh.facet_mergeset + initializes vertex neighbors if needed + + notes: + assumes all facet neighbors have been tested + this can be expensive + this does not guarantee that a centrum is below all facets + but it is unlikely + uses qh.visit_id + + design: + build vertex neighbors if necessary + for all new facets + for all vertices + for each unvisited facet neighbor of the vertex + test new facet and neighbor for convexity +*/ +boolT qh_test_vneighbors (void /* qh newfacet_list */) { + facetT *newfacet, *neighbor, **neighborp; + vertexT *vertex, **vertexp; + int nummerges= 0; + + trace1((qh ferr, "qh_test_vneighbors: testing vertex neighbors for convexity\n")); + if (!qh VERTEXneighbors) + qh_vertexneighbors(); + FORALLnew_facets + newfacet->seen= False; + FORALLnew_facets { + newfacet->seen= True; + newfacet->visitid= qh visit_id++; + FOREACHneighbor_(newfacet) + newfacet->visitid= qh visit_id; + FOREACHvertex_(newfacet->vertices) { + FOREACHneighbor_(vertex) { + if (neighbor->seen || neighbor->visitid == qh visit_id) + continue; + if (qh_test_appendmerge (newfacet, neighbor)) + nummerges++; + } + } + } + zadd_(Ztestvneighbor, nummerges); + trace1((qh ferr, "qh_test_vneighbors: found %d non-convex, vertex neighbors\n", + nummerges)); + return (nummerges > 0); +} /* test_vneighbors */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="tracemerge">-</a> + + qh_tracemerge( facet1, facet2 ) + print trace message after merge +*/ +void qh_tracemerge (facetT *facet1, facetT *facet2) { + boolT waserror= False; + +#ifndef qh_NOtrace + if (qh IStracing >= 4) + qh_errprint ("MERGED", facet2, NULL, NULL, NULL); + if (facet2 == qh tracefacet || (qh tracevertex && qh tracevertex->newlist)) { + fprintf (qh ferr, "qh_tracemerge: trace facet and vertex after merge of f%d and f%d, furthest p%d\n", facet1->id, facet2->id, qh furthest_id); + if (facet2 != qh tracefacet) + qh_errprint ("TRACE", qh tracefacet, + (qh tracevertex && qh tracevertex->neighbors) ? + SETfirstt_(qh tracevertex->neighbors, facetT) : NULL, + NULL, qh tracevertex); + } + if (qh tracevertex) { + if (qh tracevertex->deleted) + fprintf (qh ferr, "qh_tracemerge: trace vertex deleted at furthest p%d\n", + qh furthest_id); + else + qh_checkvertex (qh tracevertex); + } + if (qh tracefacet) { + qh_checkfacet (qh tracefacet, True, &waserror); + if (waserror) + qh_errexit (qh_ERRqhull, qh tracefacet, NULL); + } +#endif /* !qh_NOtrace */ + if (qh CHECKfrequently || qh IStracing >= 4) { /* can't check polygon here */ + qh_checkfacet (facet2, True, &waserror); + if (waserror) + qh_errexit(qh_ERRqhull, NULL, NULL); + } +} /* tracemerge */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="tracemerging">-</a> + + qh_tracemerging() + print trace message during POSTmerging + + returns: + updates qh.mergereport + + notes: + called from qh_mergecycle() and qh_mergefacet() + + see: + qh_buildtracing() +*/ +void qh_tracemerging (void) { + realT cpu; + int total; + time_t timedata; + struct tm *tp; + + qh mergereport= zzval_(Ztotmerge); + time (&timedata); + tp= localtime (&timedata); + cpu= qh_CPUclock; + cpu /= qh_SECticks; + total= zzval_(Ztotmerge) - zzval_(Zcyclehorizon) + zzval_(Zcyclefacettot); + fprintf (qh ferr, "\n\ +At %d:%d:%d & %2.5g CPU secs, qhull has merged %d facets. The hull\n\ + contains %d facets and %d vertices.\n", + tp->tm_hour, tp->tm_min, tp->tm_sec, cpu, + total, qh num_facets - qh num_visible, + qh num_vertices-qh_setsize (qh del_vertices)); +} /* tracemerging */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="updatetested">-</a> + + qh_updatetested( facet1, facet2 ) + clear facet2->tested and facet1->ridge->tested for merge + + returns: + deletes facet2->center unless it's already large + if so, clears facet2->ridge->tested + + design: + clear facet2->tested + clear ridge->tested for facet1's ridges + if facet2 has a centrum + if facet2 is large + set facet2->keepcentrum + else if facet2 has 3 vertices due to many merges, or not large and post merging + clear facet2->keepcentrum + unless facet2->keepcentrum + clear facet2->center to recompute centrum later + clear ridge->tested for facet2's ridges +*/ +void qh_updatetested (facetT *facet1, facetT *facet2) { + ridgeT *ridge, **ridgep; + int size; + + facet2->tested= False; + FOREACHridge_(facet1->ridges) + ridge->tested= False; + if (!facet2->center) + return; + size= qh_setsize (facet2->vertices); + if (!facet2->keepcentrum) { + if (size > qh hull_dim + qh_MAXnewcentrum) { + facet2->keepcentrum= True; + zinc_(Zwidevertices); + } + }else if (size <= qh hull_dim + qh_MAXnewcentrum) { + /* center and keepcentrum was set */ + if (size == qh hull_dim || qh POSTmerging) + facet2->keepcentrum= False; /* if many merges need to recompute centrum */ + } + if (!facet2->keepcentrum) { + qh_memfree (facet2->center, qh normal_size); + facet2->center= NULL; + FOREACHridge_(facet2->ridges) + ridge->tested= False; + } +} /* updatetested */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="vertexridges">-</a> + + qh_vertexridges( vertex ) + return temporary set of ridges adjacent to a vertex + vertex->neighbors defined + + ntoes: + uses qh.visit_id + does not include implicit ridges for simplicial facets + + design: + for each neighbor of vertex + add ridges that include the vertex to ridges +*/ +setT *qh_vertexridges (vertexT *vertex) { + facetT *neighbor, **neighborp; + setT *ridges= qh_settemp (qh TEMPsize); + int size; + + qh visit_id++; + FOREACHneighbor_(vertex) + neighbor->visitid= qh visit_id; + FOREACHneighbor_(vertex) { + if (*neighborp) /* no new ridges in last neighbor */ + qh_vertexridges_facet (vertex, neighbor, &ridges); + } + if (qh PRINTstatistics || qh IStracing) { + size= qh_setsize (ridges); + zinc_(Zvertexridge); + zadd_(Zvertexridgetot, size); + zmax_(Zvertexridgemax, size); + trace3((qh ferr, "qh_vertexridges: found %d ridges for v%d\n", + size, vertex->id)); + } + return ridges; +} /* vertexridges */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="vertexridges_facet">-</a> + + qh_vertexridges_facet( vertex, facet, ridges ) + add adjacent ridges for vertex in facet + neighbor->visitid==qh.visit_id if it hasn't been visited + + returns: + ridges updated + sets facet->visitid to qh.visit_id-1 + + design: + for each ridge of facet + if ridge of visited neighbor (i.e., unprocessed) + if vertex in ridge + append ridge to vertex + mark facet processed +*/ +void qh_vertexridges_facet (vertexT *vertex, facetT *facet, setT **ridges) { + ridgeT *ridge, **ridgep; + facetT *neighbor; + + FOREACHridge_(facet->ridges) { + neighbor= otherfacet_(ridge, facet); + if (neighbor->visitid == qh visit_id + && qh_setin (ridge->vertices, vertex)) + qh_setappend (ridges, ridge); + } + facet->visitid= qh visit_id-1; +} /* vertexridges_facet */ + +/*-<a href="qh-merge.htm#TOC" + >-------------------------------</a><a name="willdelete">-</a> + + qh_willdelete( facet, replace ) + moves facet to visible list + sets facet->f.replace to replace (may be NULL) + + returns: + bumps qh.num_visible +*/ +void qh_willdelete (facetT *facet, facetT *replace) { + + qh_removefacet(facet); + qh_prependfacet (facet, &qh visible_list); + qh num_visible++; + facet->visible= True; + facet->f.replace= replace; +} /* willdelete */ + +#else /* qh_NOmerge */ +void qh_premerge (vertexT *apex, realT maxcentrum, realT maxangle) { +} +void qh_postmerge (char *reason, realT maxcentrum, realT maxangle, + boolT vneighbors) { +} +boolT qh_checkzero (boolT testall) { + } +#endif /* qh_NOmerge */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/merge.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/merge.h new file mode 100644 index 0000000000000000000000000000000000000000..5dc8ac1f17e6a7d5226dfc0241f56e0d2bbdb70d --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/merge.h @@ -0,0 +1,174 @@ +/*<html><pre> -<a href="qh-merge.htm" + >-------------------------------</a><a name="TOP">-</a> + + merge.h + header file for merge.c + + see qh-merge.htm and merge.c + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFmerge +#define qhDEFmerge 1 + + +/*============ -constants- ==============*/ + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_ANGLEredundant">-</a> + + qh_ANGLEredundant + indicates redundant merge in mergeT->angle +*/ +#define qh_ANGLEredundant 6.0 + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_ANGLEdegen">-</a> + + qh_ANGLEdegen + indicates degenerate facet in mergeT->angle +*/ +#define qh_ANGLEdegen 5.0 + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_ANGLEconcave">-</a> + + qh_ANGLEconcave + offset to indicate concave facets in mergeT->angle + + notes: + concave facets are assigned the range of [2,4] in mergeT->angle + roundoff error may make the angle less than 2 +*/ +#define qh_ANGLEconcave 1.5 + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="MRG">-</a> + + MRG... (mergeType) + indicates the type of a merge (mergeT->type) +*/ +typedef enum { /* in sort order for facet_mergeset */ + MRGnone= 0, + MRGcoplanar, /* centrum coplanar */ + MRGanglecoplanar, /* angle coplanar */ + /* could detect half concave ridges */ + MRGconcave, /* concave ridge */ + MRGflip, /* flipped facet. facet1 == facet2 */ + MRGridge, /* duplicate ridge (qh_MERGEridge) */ + /* degen and redundant go onto degen_mergeset */ + MRGdegen, /* degenerate facet (not enough neighbors) facet1 == facet2 */ + MRGredundant, /* redundant facet (vertex subset) */ + /* merge_degenredundant assumes degen < redundant */ + MRGmirror, /* mirror facet from qh_triangulate */ + ENDmrg +} mergeType; + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="qh_MERGEapex">-</a> + + qh_MERGEapex + flag for qh_mergefacet() to indicate an apex merge +*/ +#define qh_MERGEapex True + +/*============ -structures- ====================*/ + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="mergeT">-</a> + + mergeT + structure used to merge facets +*/ + +typedef struct mergeT mergeT; +struct mergeT { /* initialize in qh_appendmergeset */ + realT angle; /* angle between normals of facet1 and facet2 */ + facetT *facet1; /* will merge facet1 into facet2 */ + facetT *facet2; + mergeType type; +}; + + +/*=========== -macros- =========================*/ + +/*-<a href="qh-merge.htm#TOC" + >--------------------------------</a><a name="FOREACHmerge_">-</a> + + FOREACHmerge_( merges ) {...} + assign 'merge' to each merge in merges + + notes: + uses 'mergeT *merge, **mergep;' + if qh_mergefacet(), + restart since qh.facet_mergeset may change + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHmerge_( merges ) FOREACHsetelement_(mergeT, merges, merge) + +/*============ prototypes in alphabetical order after pre/postmerge =======*/ + +void qh_premerge (vertexT *apex, realT maxcentrum, realT maxangle); +void qh_postmerge (char *reason, realT maxcentrum, realT maxangle, + boolT vneighbors); +void qh_all_merges (boolT othermerge, boolT vneighbors); +void qh_appendmergeset(facetT *facet, facetT *neighbor, mergeType mergetype, realT *angle); +setT *qh_basevertices( facetT *samecycle); +void qh_checkconnect (void /* qh new_facets */); +boolT qh_checkzero (boolT testall); +int qh_compareangle(const void *p1, const void *p2); +int qh_comparemerge(const void *p1, const void *p2); +int qh_comparevisit (const void *p1, const void *p2); +void qh_copynonconvex (ridgeT *atridge); +void qh_degen_redundant_facet (facetT *facet); +void qh_degen_redundant_neighbors (facetT *facet, facetT *delfacet); +vertexT *qh_find_newvertex (vertexT *oldvertex, setT *vertices, setT *ridges); +void qh_findbest_test (boolT testcentrum, facetT *facet, facetT *neighbor, + facetT **bestfacet, realT *distp, realT *mindistp, realT *maxdistp); +facetT *qh_findbestneighbor(facetT *facet, realT *distp, realT *mindistp, realT *maxdistp); +void qh_flippedmerges(facetT *facetlist, boolT *wasmerge); +void qh_forcedmerges( boolT *wasmerge); +void qh_getmergeset(facetT *facetlist); +void qh_getmergeset_initial (facetT *facetlist); +void qh_hashridge (setT *hashtable, int hashsize, ridgeT *ridge, vertexT *oldvertex); +ridgeT *qh_hashridge_find (setT *hashtable, int hashsize, ridgeT *ridge, + vertexT *vertex, vertexT *oldvertex, int *hashslot); +void qh_makeridges(facetT *facet); +void qh_mark_dupridges(facetT *facetlist); +void qh_maydropneighbor (facetT *facet); +int qh_merge_degenredundant (void); +void qh_merge_nonconvex( facetT *facet1, facetT *facet2, mergeType mergetype); +void qh_mergecycle (facetT *samecycle, facetT *newfacet); +void qh_mergecycle_all (facetT *facetlist, boolT *wasmerge); +void qh_mergecycle_facets( facetT *samecycle, facetT *newfacet); +void qh_mergecycle_neighbors(facetT *samecycle, facetT *newfacet); +void qh_mergecycle_ridges(facetT *samecycle, facetT *newfacet); +void qh_mergecycle_vneighbors( facetT *samecycle, facetT *newfacet); +void qh_mergefacet(facetT *facet1, facetT *facet2, realT *mindist, realT *maxdist, boolT mergeapex); +void qh_mergefacet2d (facetT *facet1, facetT *facet2); +void qh_mergeneighbors(facetT *facet1, facetT *facet2); +void qh_mergeridges(facetT *facet1, facetT *facet2); +void qh_mergesimplex(facetT *facet1, facetT *facet2, boolT mergeapex); +void qh_mergevertex_del (vertexT *vertex, facetT *facet1, facetT *facet2); +void qh_mergevertex_neighbors(facetT *facet1, facetT *facet2); +void qh_mergevertices(setT *vertices1, setT **vertices); +setT *qh_neighbor_intersections (vertexT *vertex); +void qh_newvertices (setT *vertices); +boolT qh_reducevertices (void); +vertexT *qh_redundant_vertex (vertexT *vertex); +boolT qh_remove_extravertices (facetT *facet); +vertexT *qh_rename_sharedvertex (vertexT *vertex, facetT *facet); +void qh_renameridgevertex(ridgeT *ridge, vertexT *oldvertex, vertexT *newvertex); +void qh_renamevertex(vertexT *oldvertex, vertexT *newvertex, setT *ridges, + facetT *oldfacet, facetT *neighborA); +boolT qh_test_appendmerge (facetT *facet, facetT *neighbor); +boolT qh_test_vneighbors (void /* qh newfacet_list */); +void qh_tracemerge (facetT *facet1, facetT *facet2); +void qh_tracemerging (void); +void qh_updatetested( facetT *facet1, facetT *facet2); +setT *qh_vertexridges (vertexT *vertex); +void qh_vertexridges_facet (vertexT *vertex, facetT *facet, setT **ridges); +void qh_willdelete (facetT *facet, facetT *replace); + +#endif /* qhDEFmerge */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly.c new file mode 100644 index 0000000000000000000000000000000000000000..c2791341d506bf01a64d2d1bcd5a10584f5dbf70 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly.c @@ -0,0 +1,1191 @@ +/*<html><pre> -<a href="qh-poly.htm" + >-------------------------------</a><a name="TOP">-</a> + + poly.c + implements polygons and simplices + + see qh-poly.htm, poly.h and qhull.h + + infrequent code is in poly2.c + (all but top 50 and their callers 12/3/95) + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include "qhull_a.h" + +/*======== functions in alphabetical order ==========*/ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="appendfacet">-</a> + + qh_appendfacet( facet ) + appends facet to end of qh.facet_list, + + returns: + updates qh.newfacet_list, facet_next, facet_list + increments qh.numfacets + + notes: + assumes qh.facet_list/facet_tail is defined (createsimplex) + + see: + qh_removefacet() + +*/ +void qh_appendfacet(facetT *facet) { + facetT *tail= qh facet_tail; + + if (tail == qh newfacet_list) + qh newfacet_list= facet; + if (tail == qh facet_next) + qh facet_next= facet; + facet->previous= tail->previous; + facet->next= tail; + if (tail->previous) + tail->previous->next= facet; + else + qh facet_list= facet; + tail->previous= facet; + qh num_facets++; + trace4((qh ferr, "qh_appendfacet: append f%d to facet_list\n", facet->id)); +} /* appendfacet */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="appendvertex">-</a> + + qh_appendvertex( vertex ) + appends vertex to end of qh.vertex_list, + + returns: + sets vertex->newlist + updates qh.vertex_list, newvertex_list + increments qh.num_vertices + + notes: + assumes qh.vertex_list/vertex_tail is defined (createsimplex) + +*/ +void qh_appendvertex (vertexT *vertex) { + vertexT *tail= qh vertex_tail; + + if (tail == qh newvertex_list) + qh newvertex_list= vertex; + vertex->newlist= True; + vertex->previous= tail->previous; + vertex->next= tail; + if (tail->previous) + tail->previous->next= vertex; + else + qh vertex_list= vertex; + tail->previous= vertex; + qh num_vertices++; + trace4((qh ferr, "qh_appendvertex: append v%d to vertex_list\n", vertex->id)); +} /* appendvertex */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="attachnewfacets">-</a> + + qh_attachnewfacets( ) + attach horizon facets to new facets in qh.newfacet_list + newfacets have neighbor and ridge links to horizon but not vice versa + only needed for qh.ONLYgood + + returns: + set qh.NEWfacets + horizon facets linked to new facets + ridges changed from visible facets to new facets + simplicial ridges deleted + qh.visible_list, no ridges valid + facet->f.replace is a newfacet (if any) + + design: + delete interior ridges and neighbor sets by + for each visible, non-simplicial facet + for each ridge + if last visit or if neighbor is simplicial + if horizon neighbor + delete ridge for horizon's ridge set + delete ridge + erase neighbor set + attach horizon facets and new facets by + for all new facets + if corresponding horizon facet is simplicial + locate corresponding visible facet {may be more than one} + link visible facet to new facet + replace visible facet with new facet in horizon + else it's non-simplicial + for all visible neighbors of the horizon facet + link visible neighbor to new facet + delete visible neighbor from horizon facet + append new facet to horizon's neighbors + the first ridge of the new facet is the horizon ridge + link the new facet into the horizon ridge +*/ +void qh_attachnewfacets (void ) { + facetT *newfacet= NULL, *neighbor, **neighborp, *horizon, *visible; + ridgeT *ridge, **ridgep; + + qh NEWfacets= True; + trace3((qh ferr, "qh_attachnewfacets: delete interior ridges\n")); + qh visit_id++; + FORALLvisible_facets { + visible->visitid= qh visit_id; + if (visible->ridges) { + FOREACHridge_(visible->ridges) { + neighbor= otherfacet_(ridge, visible); + if (neighbor->visitid == qh visit_id + || (!neighbor->visible && neighbor->simplicial)) { + if (!neighbor->visible) /* delete ridge for simplicial horizon */ + qh_setdel (neighbor->ridges, ridge); + qh_setfree (&(ridge->vertices)); /* delete on 2nd visit */ + qh_memfree (ridge, sizeof(ridgeT)); + } + } + SETfirst_(visible->ridges)= NULL; + } + SETfirst_(visible->neighbors)= NULL; + } + trace1((qh ferr, "qh_attachnewfacets: attach horizon facets to new facets\n")); + FORALLnew_facets { + horizon= SETfirstt_(newfacet->neighbors, facetT); + if (horizon->simplicial) { + visible= NULL; + FOREACHneighbor_(horizon) { /* may have more than one horizon ridge */ + if (neighbor->visible) { + if (visible) { + if (qh_setequal_skip (newfacet->vertices, 0, horizon->vertices, + SETindex_(horizon->neighbors, neighbor))) { + visible= neighbor; + break; + } + }else + visible= neighbor; + } + } + if (visible) { + visible->f.replace= newfacet; + qh_setreplace (horizon->neighbors, visible, newfacet); + }else { + fprintf (qh ferr, "qhull internal error (qh_attachnewfacets): couldn't find visible facet for horizon f%d of newfacet f%d\n", + horizon->id, newfacet->id); + qh_errexit2 (qh_ERRqhull, horizon, newfacet); + } + }else { /* non-simplicial, with a ridge for newfacet */ + FOREACHneighbor_(horizon) { /* may hold for many new facets */ + if (neighbor->visible) { + neighbor->f.replace= newfacet; + qh_setdelnth (horizon->neighbors, + SETindex_(horizon->neighbors, neighbor)); + neighborp--; /* repeat */ + } + } + qh_setappend (&horizon->neighbors, newfacet); + ridge= SETfirstt_(newfacet->ridges, ridgeT); + if (ridge->top == horizon) + ridge->bottom= newfacet; + else + ridge->top= newfacet; + } + } /* newfacets */ + if (qh PRINTstatistics) { + FORALLvisible_facets { + if (!visible->f.replace) + zinc_(Zinsidevisible); + } + } +} /* attachnewfacets */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="checkflipped">-</a> + + qh_checkflipped( facet, dist, allerror ) + checks facet orientation to interior point + + if allerror set, + tests against qh.DISTround + else + tests against 0 since tested against DISTround before + + returns: + False if it flipped orientation (sets facet->flipped) + distance if non-NULL +*/ +boolT qh_checkflipped (facetT *facet, realT *distp, boolT allerror) { + realT dist; + + if (facet->flipped && !distp) + return False; + zzinc_(Zdistcheck); + qh_distplane(qh interior_point, facet, &dist); + if (distp) + *distp= dist; + if ((allerror && dist > -qh DISTround)|| (!allerror && dist >= 0.0)) { + facet->flipped= True; + zzinc_(Zflippedfacets); + trace0((qh ferr, "qh_checkflipped: facet f%d is flipped, distance= %6.12g during p%d\n", + facet->id, dist, qh furthest_id)); + qh_precision ("flipped facet"); + return False; + } + return True; +} /* checkflipped */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="delfacet">-</a> + + qh_delfacet( facet ) + removes facet from facet_list and frees up its memory + + notes: + assumes vertices and ridges already freed +*/ +void qh_delfacet(facetT *facet) { + void **freelistp; /* used !qh_NOmem */ + + trace4((qh ferr, "qh_delfacet: delete f%d\n", facet->id)); + if (facet == qh tracefacet) + qh tracefacet= NULL; + if (facet == qh GOODclosest) + qh GOODclosest= NULL; + qh_removefacet(facet); + if (!facet->tricoplanar || facet->keepcentrum) { + qh_memfree_(facet->normal, qh normal_size, freelistp); + if (qh CENTERtype == qh_ASvoronoi) { /* uses macro calls */ + qh_memfree_(facet->center, qh center_size, freelistp); + }else /* AScentrum */ { + qh_memfree_(facet->center, qh normal_size, freelistp); + } + } + qh_setfree(&(facet->neighbors)); + if (facet->ridges) + qh_setfree(&(facet->ridges)); + qh_setfree(&(facet->vertices)); + if (facet->outsideset) + qh_setfree(&(facet->outsideset)); + if (facet->coplanarset) + qh_setfree(&(facet->coplanarset)); + qh_memfree_(facet, sizeof(facetT), freelistp); +} /* delfacet */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="deletevisible">-</a> + + qh_deletevisible() + delete visible facets and vertices + + returns: + deletes each facet and removes from facetlist + at exit, qh.visible_list empty (== qh.newfacet_list) + + notes: + ridges already deleted + horizon facets do not reference facets on qh.visible_list + new facets in qh.newfacet_list + uses qh.visit_id; +*/ +void qh_deletevisible (void /*qh visible_list*/) { + facetT *visible, *nextfacet; + vertexT *vertex, **vertexp; + int numvisible= 0, numdel= qh_setsize(qh del_vertices); + + trace1((qh ferr, "qh_deletevisible: delete %d visible facets and %d vertices\n", + qh num_visible, numdel)); + for (visible= qh visible_list; visible && visible->visible; + visible= nextfacet) { /* deleting current */ + nextfacet= visible->next; + numvisible++; + qh_delfacet(visible); + } + if (numvisible != qh num_visible) { + fprintf (qh ferr, "qhull internal error (qh_deletevisible): qh num_visible %d is not number of visible facets %d\n", + qh num_visible, numvisible); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + qh num_visible= 0; + zadd_(Zvisfacettot, numvisible); + zmax_(Zvisfacetmax, numvisible); + zzadd_(Zdelvertextot, numdel); + zmax_(Zdelvertexmax, numdel); + FOREACHvertex_(qh del_vertices) + qh_delvertex (vertex); + qh_settruncate (qh del_vertices, 0); +} /* deletevisible */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="facetintersect">-</a> + + qh_facetintersect( facetA, facetB, skipa, skipB, prepend ) + return vertices for intersection of two simplicial facets + may include 1 prepended entry (if more, need to settemppush) + + returns: + returns set of qh.hull_dim-1 + prepend vertices + returns skipped index for each test and checks for exactly one + + notes: + does not need settemp since set in quick memory + + see also: + qh_vertexintersect and qh_vertexintersect_new + use qh_setnew_delnthsorted to get nth ridge (no skip information) + + design: + locate skipped vertex by scanning facet A's neighbors + locate skipped vertex by scanning facet B's neighbors + intersect the vertex sets +*/ +setT *qh_facetintersect (facetT *facetA, facetT *facetB, + int *skipA,int *skipB, int prepend) { + setT *intersect; + int dim= qh hull_dim, i, j; + facetT **neighborsA, **neighborsB; + + neighborsA= SETaddr_(facetA->neighbors, facetT); + neighborsB= SETaddr_(facetB->neighbors, facetT); + i= j= 0; + if (facetB == *neighborsA++) + *skipA= 0; + else if (facetB == *neighborsA++) + *skipA= 1; + else if (facetB == *neighborsA++) + *skipA= 2; + else { + for (i= 3; i < dim; i++) { + if (facetB == *neighborsA++) { + *skipA= i; + break; + } + } + } + if (facetA == *neighborsB++) + *skipB= 0; + else if (facetA == *neighborsB++) + *skipB= 1; + else if (facetA == *neighborsB++) + *skipB= 2; + else { + for (j= 3; j < dim; j++) { + if (facetA == *neighborsB++) { + *skipB= j; + break; + } + } + } + if (i >= dim || j >= dim) { + fprintf (qh ferr, "qhull internal error (qh_facetintersect): f%d or f%d not in others neighbors\n", + facetA->id, facetB->id); + qh_errexit2 (qh_ERRqhull, facetA, facetB); + } + intersect= qh_setnew_delnthsorted (facetA->vertices, qh hull_dim, *skipA, prepend); + trace4((qh ferr, "qh_facetintersect: f%d skip %d matches f%d skip %d\n", + facetA->id, *skipA, facetB->id, *skipB)); + return(intersect); +} /* facetintersect */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="gethash">-</a> + + qh_gethash( hashsize, set, size, firstindex, skipelem ) + return hashvalue for a set with firstindex and skipelem + + notes: + assumes at least firstindex+1 elements + assumes skipelem is NULL, in set, or part of hash + + hashes memory addresses which may change over different runs of the same data + using sum for hash does badly in high d +*/ +unsigned qh_gethash (int hashsize, setT *set, int size, int firstindex, void *skipelem) { + void **elemp= SETelemaddr_(set, firstindex, void); + ptr_intT hash = 0, elem; + int i; + // !! PATCHED TO 2010.1 version!! + unsigned result; + // !! PATCHED TO 2010.1 version!! + + switch (size-firstindex) { + case 1: + hash= (ptr_intT)(*elemp) - (ptr_intT) skipelem; + break; + case 2: + hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] - (ptr_intT) skipelem; + break; + case 3: + hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2] + - (ptr_intT) skipelem; + break; + case 4: + hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2] + + (ptr_intT)elemp[3] - (ptr_intT) skipelem; + break; + case 5: + hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2] + + (ptr_intT)elemp[3] + (ptr_intT)elemp[4] - (ptr_intT) skipelem; + break; + case 6: + hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2] + + (ptr_intT)elemp[3] + (ptr_intT)elemp[4]+ (ptr_intT)elemp[5] + - (ptr_intT) skipelem; + break; + default: + hash= 0; + i= 3; + do { /* this is about 10% in 10-d */ + if ((elem= (ptr_intT)*elemp++) != (ptr_intT)skipelem) { + hash ^= (elem << i) + (elem >> (32-i)); + i += 3; + if (i >= 32) + i -= 32; + } + }while(*elemp); + break; + } + + // !! PATCHED TO 2010.1 version!! + //hash %= (ptr_intT) hashsize; + ///* hash= 0; for debugging purposes */ + //return hash; + result= (unsigned)hash; + result %= (unsigned)hashsize; + /* result= 0; for debugging purposes */ + return result; + // !! PATCHED TO 2010.1 version!! + +} /* gethash */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="makenewfacet">-</a> + + qh_makenewfacet( vertices, toporient, horizon ) + creates a toporient? facet from vertices + + returns: + returns newfacet + adds newfacet to qh.facet_list + newfacet->vertices= vertices + if horizon + newfacet->neighbor= horizon, but not vice versa + newvertex_list updated with vertices +*/ +facetT *qh_makenewfacet(setT *vertices, boolT toporient,facetT *horizon) { + facetT *newfacet; + vertexT *vertex, **vertexp; + + FOREACHvertex_(vertices) { + if (!vertex->newlist) { + qh_removevertex (vertex); + qh_appendvertex (vertex); + } + } + newfacet= qh_newfacet(); + newfacet->vertices= vertices; + newfacet->toporient= toporient; + if (horizon) + qh_setappend(&(newfacet->neighbors), horizon); + qh_appendfacet(newfacet); + return(newfacet); +} /* makenewfacet */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="makenewplanes">-</a> + + qh_makenewplanes() + make new hyperplanes for facets on qh.newfacet_list + + returns: + all facets have hyperplanes or are marked for merging + doesn't create hyperplane if horizon is coplanar (will merge) + updates qh.min_vertex if qh.JOGGLEmax + + notes: + facet->f.samecycle is defined for facet->mergehorizon facets +*/ +void qh_makenewplanes (void /* newfacet_list */) { + facetT *newfacet; + + FORALLnew_facets { + if (!newfacet->mergehorizon) + qh_setfacetplane (newfacet); + } + if (qh JOGGLEmax < REALmax/2) + minimize_(qh min_vertex, -wwval_(Wnewvertexmax)); +} /* makenewplanes */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="makenew_nonsimplicial">-</a> + + qh_makenew_nonsimplicial( visible, apex, numnew ) + make new facets for ridges of a visible facet + + returns: + first newfacet, bumps numnew as needed + attaches new facets if !qh.ONLYgood + marks ridge neighbors for simplicial visible + if (qh.ONLYgood) + ridges on newfacet, horizon, and visible + else + ridge and neighbors between newfacet and horizon + visible facet's ridges are deleted + + notes: + qh.visit_id if visible has already been processed + sets neighbor->seen for building f.samecycle + assumes all 'seen' flags initially false + + design: + for each ridge of visible facet + get neighbor of visible facet + if neighbor was already processed + delete the ridge (will delete all visible facets later) + if neighbor is a horizon facet + create a new facet + if neighbor coplanar + adds newfacet to f.samecycle for later merging + else + updates neighbor's neighbor set + (checks for non-simplicial facet with multiple ridges to visible facet) + updates neighbor's ridge set + (checks for simplicial neighbor to non-simplicial visible facet) + (deletes ridge if neighbor is simplicial) + +*/ +#ifndef qh_NOmerge +facetT *qh_makenew_nonsimplicial (facetT *visible, vertexT *apex, int *numnew) { + void **freelistp; /* used !qh_NOmem */ + ridgeT *ridge, **ridgep; + facetT *neighbor, *newfacet= NULL, *samecycle; + setT *vertices; + boolT toporient; + int ridgeid; + + FOREACHridge_(visible->ridges) { + ridgeid= ridge->id; + neighbor= otherfacet_(ridge, visible); + if (neighbor->visible) { + if (!qh ONLYgood) { + if (neighbor->visitid == qh visit_id) { + qh_setfree (&(ridge->vertices)); /* delete on 2nd visit */ + qh_memfree_(ridge, sizeof(ridgeT), freelistp); + } + } + }else { /* neighbor is an horizon facet */ + toporient= (ridge->top == visible); + vertices= qh_setnew (qh hull_dim); /* makes sure this is quick */ + qh_setappend (&vertices, apex); + qh_setappend_set (&vertices, ridge->vertices); + newfacet= qh_makenewfacet(vertices, toporient, neighbor); + (*numnew)++; + if (neighbor->coplanar) { + newfacet->mergehorizon= True; + if (!neighbor->seen) { + newfacet->f.samecycle= newfacet; + neighbor->f.newcycle= newfacet; + }else { + samecycle= neighbor->f.newcycle; + newfacet->f.samecycle= samecycle->f.samecycle; + samecycle->f.samecycle= newfacet; + } + } + if (qh ONLYgood) { + if (!neighbor->simplicial) + qh_setappend(&(newfacet->ridges), ridge); + }else { /* qh_attachnewfacets */ + if (neighbor->seen) { + if (neighbor->simplicial) { + fprintf (qh ferr, "qhull internal error (qh_makenew_nonsimplicial): simplicial f%d sharing two ridges with f%d\n", + neighbor->id, visible->id); + qh_errexit2 (qh_ERRqhull, neighbor, visible); + } + qh_setappend (&(neighbor->neighbors), newfacet); + }else + qh_setreplace (neighbor->neighbors, visible, newfacet); + if (neighbor->simplicial) { + qh_setdel (neighbor->ridges, ridge); + qh_setfree (&(ridge->vertices)); + qh_memfree (ridge, sizeof(ridgeT)); + }else { + qh_setappend(&(newfacet->ridges), ridge); + if (toporient) + ridge->top= newfacet; + else + ridge->bottom= newfacet; + } + trace4((qh ferr, "qh_makenew_nonsimplicial: created facet f%d from v%d and r%d of horizon f%d\n", + newfacet->id, apex->id, ridgeid, neighbor->id)); + } + } + neighbor->seen= True; + } /* for each ridge */ + if (!qh ONLYgood) + SETfirst_(visible->ridges)= NULL; + return newfacet; +} /* makenew_nonsimplicial */ +#else /* qh_NOmerge */ +facetT *qh_makenew_nonsimplicial (facetT *visible, vertexT *apex, int *numnew) { + return NULL; +} +#endif /* qh_NOmerge */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="makenew_simplicial">-</a> + + qh_makenew_simplicial( visible, apex, numnew ) + make new facets for simplicial visible facet and apex + + returns: + attaches new facets if (!qh.ONLYgood) + neighbors between newfacet and horizon + + notes: + nop if neighbor->seen or neighbor->visible (see qh_makenew_nonsimplicial) + + design: + locate neighboring horizon facet for visible facet + determine vertices and orientation + create new facet + if coplanar, + add new facet to f.samecycle + update horizon facet's neighbor list +*/ +facetT *qh_makenew_simplicial (facetT *visible, vertexT *apex, int *numnew) { + facetT *neighbor, **neighborp, *newfacet= NULL; + setT *vertices; + boolT flip, toporient; + int horizonskip, visibleskip; + + FOREACHneighbor_(visible) { + if (!neighbor->seen && !neighbor->visible) { + vertices= qh_facetintersect(neighbor,visible, &horizonskip, &visibleskip, 1); + SETfirst_(vertices)= apex; + flip= ((horizonskip & 0x1) ^ (visibleskip & 0x1)); + if (neighbor->toporient) + toporient= horizonskip & 0x1; + else + toporient= (horizonskip & 0x1) ^ 0x1; + newfacet= qh_makenewfacet(vertices, toporient, neighbor); + (*numnew)++; + if (neighbor->coplanar && (qh PREmerge || qh MERGEexact)) { +#ifndef qh_NOmerge + newfacet->f.samecycle= newfacet; + newfacet->mergehorizon= True; +#endif + } + if (!qh ONLYgood) + SETelem_(neighbor->neighbors, horizonskip)= newfacet; + trace4((qh ferr, "qh_makenew_simplicial: create facet f%d top %d from v%d and horizon f%d skip %d top %d and visible f%d skip %d, flip? %d\n", + newfacet->id, toporient, apex->id, neighbor->id, horizonskip, + neighbor->toporient, visible->id, visibleskip, flip)); + } + } + return newfacet; +} /* makenew_simplicial */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="matchneighbor">-</a> + + qh_matchneighbor( newfacet, newskip, hashsize, hashcount ) + either match subridge of newfacet with neighbor or add to hash_table + + returns: + duplicate ridges are unmatched and marked by qh_DUPLICATEridge + + notes: + ridge is newfacet->vertices w/o newskip vertex + do not allocate memory (need to free hash_table cleanly) + uses linear hash chains + + see also: + qh_matchduplicates + + design: + for each possible matching facet in qh.hash_table + if vertices match + set ismatch, if facets have opposite orientation + if ismatch and matching facet doesn't have a match + match the facets by updating their neighbor sets + else + indicate a duplicate ridge + set facet hyperplane for later testing + add facet to hashtable + unless the other facet was already a duplicate ridge + mark both facets with a duplicate ridge + add other facet (if defined) to hash table +*/ +void qh_matchneighbor (facetT *newfacet, int newskip, int hashsize, int *hashcount) { + boolT newfound= False; /* True, if new facet is already in hash chain */ + boolT same, ismatch; + int hash, scan; + facetT *facet, *matchfacet; + int skip, matchskip; + + hash= (int)qh_gethash (hashsize, newfacet->vertices, qh hull_dim, 1, + SETelem_(newfacet->vertices, newskip)); + trace4((qh ferr, "qh_matchneighbor: newfacet f%d skip %d hash %d hashcount %d\n", + newfacet->id, newskip, hash, *hashcount)); + zinc_(Zhashlookup); + for (scan= hash; (facet= SETelemt_(qh hash_table, scan, facetT)); + scan= (++scan >= hashsize ? 0 : scan)) { + if (facet == newfacet) { + newfound= True; + continue; + } + zinc_(Zhashtests); + if (qh_matchvertices (1, newfacet->vertices, newskip, facet->vertices, &skip, &same)) { + if (SETelem_(newfacet->vertices, newskip) == + SETelem_(facet->vertices, skip)) { + qh_precision ("two facets with the same vertices"); + fprintf (qh ferr, "qhull precision error: Vertex sets are the same for f%d and f%d. Can not force output.\n", + facet->id, newfacet->id); + qh_errexit2 (qh_ERRprec, facet, newfacet); + } + ismatch= (same == (newfacet->toporient ^ facet->toporient)); + matchfacet= SETelemt_(facet->neighbors, skip, facetT); + if (ismatch && !matchfacet) { + SETelem_(facet->neighbors, skip)= newfacet; + SETelem_(newfacet->neighbors, newskip)= facet; + (*hashcount)--; + trace4((qh ferr, "qh_matchneighbor: f%d skip %d matched with new f%d skip %d\n", + facet->id, skip, newfacet->id, newskip)); + return; + } + if (!qh PREmerge && !qh MERGEexact) { + qh_precision ("a ridge with more than two neighbors"); + fprintf (qh ferr, "qhull precision error: facets f%d, f%d and f%d meet at a ridge with more than 2 neighbors. Can not continue.\n", + facet->id, newfacet->id, getid_(matchfacet)); + qh_errexit2 (qh_ERRprec, facet, newfacet); + } + SETelem_(newfacet->neighbors, newskip)= qh_DUPLICATEridge; + newfacet->dupridge= True; + if (!newfacet->normal) + qh_setfacetplane (newfacet); + qh_addhash (newfacet, qh hash_table, hashsize, hash); + (*hashcount)++; + if (!facet->normal) + qh_setfacetplane (facet); + if (matchfacet != qh_DUPLICATEridge) { + SETelem_(facet->neighbors, skip)= qh_DUPLICATEridge; + facet->dupridge= True; + if (!facet->normal) + qh_setfacetplane (facet); + if (matchfacet) { + matchskip= qh_setindex (matchfacet->neighbors, facet); + SETelem_(matchfacet->neighbors, matchskip)= qh_DUPLICATEridge; + matchfacet->dupridge= True; + if (!matchfacet->normal) + qh_setfacetplane (matchfacet); + qh_addhash (matchfacet, qh hash_table, hashsize, hash); + *hashcount += 2; + } + } + trace4((qh ferr, "qh_matchneighbor: new f%d skip %d duplicates ridge for f%d skip %d matching f%d ismatch %d at hash %d\n", + newfacet->id, newskip, facet->id, skip, + (matchfacet == qh_DUPLICATEridge ? -2 : getid_(matchfacet)), + ismatch, hash)); + return; /* end of duplicate ridge */ + } + } + if (!newfound) + SETelem_(qh hash_table, scan)= newfacet; /* same as qh_addhash */ + (*hashcount)++; + trace4((qh ferr, "qh_matchneighbor: no match for f%d skip %d at hash %d\n", + newfacet->id, newskip, hash)); +} /* matchneighbor */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="matchnewfacets">-</a> + + qh_matchnewfacets() + match newfacets in qh.newfacet_list to their newfacet neighbors + + returns: + qh.newfacet_list with full neighbor sets + get vertices with nth neighbor by deleting nth vertex + if qh.PREmerge/MERGEexact or qh.FORCEoutput + sets facet->flippped if flipped normal (also prevents point partitioning) + if duplicate ridges and qh.PREmerge/MERGEexact + sets facet->dupridge + missing neighbor links identifies extra ridges to be merging (qh_MERGEridge) + + notes: + newfacets already have neighbor[0] (horizon facet) + assumes qh.hash_table is NULL + vertex->neighbors has not been updated yet + do not allocate memory after qh.hash_table (need to free it cleanly) + + design: + delete neighbor sets for all new facets + initialize a hash table + for all new facets + match facet with neighbors + if unmatched facets (due to duplicate ridges) + for each new facet with a duplicate ridge + match it with a facet + check for flipped facets +*/ +void qh_matchnewfacets (void /* qh newfacet_list */) { + int numnew=0, hashcount=0, newskip; + facetT *newfacet, *neighbor; + int dim= qh hull_dim, hashsize, neighbor_i, neighbor_n; + setT *neighbors; +#ifndef qh_NOtrace + int facet_i, facet_n, numfree= 0; + facetT *facet; +#endif + + trace1((qh ferr, "qh_matchnewfacets: match neighbors for new facets.\n")); + FORALLnew_facets { + numnew++; + { /* inline qh_setzero (newfacet->neighbors, 1, qh hull_dim); */ + neighbors= newfacet->neighbors; + neighbors->e[neighbors->maxsize].i= dim+1; /*may be overwritten*/ + memset ((char *)SETelemaddr_(neighbors, 1, void), 0, dim * SETelemsize); + } + } + qh_newhashtable (numnew*(qh hull_dim-1)); /* twice what is normally needed, + but every ridge could be DUPLICATEridge */ + hashsize= qh_setsize (qh hash_table); + FORALLnew_facets { + for (newskip=1; newskip<qh hull_dim; newskip++) /* furthest/horizon already matched */ + qh_matchneighbor (newfacet, newskip, hashsize, &hashcount); +#if 0 /* use the following to trap hashcount errors */ + { + int count= 0, k; + facetT *facet, *neighbor; + + count= 0; + FORALLfacet_(qh newfacet_list) { /* newfacet already in use */ + for (k=1; k < qh hull_dim; k++) { + neighbor= SETelemt_(facet->neighbors, k, facetT); + if (!neighbor || neighbor == qh_DUPLICATEridge) + count++; + } + if (facet == newfacet) + break; + } + if (count != hashcount) { + fprintf (qh ferr, "qh_matchnewfacets: after adding facet %d, hashcount %d != count %d\n", + newfacet->id, hashcount, count); + qh_errexit (qh_ERRqhull, newfacet, NULL); + } + } +#endif /* end of trap code */ + } + if (hashcount) { + FORALLnew_facets { + if (newfacet->dupridge) { + FOREACHneighbor_i_(newfacet) { + if (neighbor == qh_DUPLICATEridge) { + qh_matchduplicates (newfacet, neighbor_i, hashsize, &hashcount); + /* this may report MERGEfacet */ + } + } + } + } + } + if (hashcount) { + fprintf (qh ferr, "qhull internal error (qh_matchnewfacets): %d neighbors did not match up\n", + hashcount); + qh_printhashtable (qh ferr); + qh_errexit (qh_ERRqhull, NULL, NULL); + } +#ifndef qh_NOtrace + if (qh IStracing >= 2) { + FOREACHfacet_i_(qh hash_table) { + if (!facet) + numfree++; + } + fprintf (qh ferr, "qh_matchnewfacets: %d new facets, %d unused hash entries . hashsize %d\n", + numnew, numfree, qh_setsize (qh hash_table)); + } +#endif /* !qh_NOtrace */ + qh_setfree (&qh hash_table); + if (qh PREmerge || qh MERGEexact) { + if (qh IStracing >= 4) + qh_printfacetlist (qh newfacet_list, NULL, qh_ALL); + FORALLnew_facets { + if (newfacet->normal) + qh_checkflipped (newfacet, NULL, qh_ALL); + } + }else if (qh FORCEoutput) + qh_checkflipped_all (qh newfacet_list); /* prints warnings for flipped */ +} /* matchnewfacets */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="matchvertices">-</a> + + qh_matchvertices( firstindex, verticesA, skipA, verticesB, skipB, same ) + tests whether vertices match with a single skip + starts match at firstindex since all new facets have a common vertex + + returns: + true if matched vertices + skip index for each set + sets same iff vertices have the same orientation + + notes: + assumes skipA is in A and both sets are the same size + + design: + set up pointers + scan both sets checking for a match + test orientation +*/ +boolT qh_matchvertices (int firstindex, setT *verticesA, int skipA, + setT *verticesB, int *skipB, boolT *same) { + vertexT **elemAp, **elemBp, **skipBp=NULL, **skipAp; + + elemAp= SETelemaddr_(verticesA, firstindex, vertexT); + elemBp= SETelemaddr_(verticesB, firstindex, vertexT); + skipAp= SETelemaddr_(verticesA, skipA, vertexT); + do if (elemAp != skipAp) { + while (*elemAp != *elemBp++) { + if (skipBp) + return False; + skipBp= elemBp; /* one extra like FOREACH */ + } + }while(*(++elemAp)); + if (!skipBp) + skipBp= ++elemBp; + *skipB= SETindex_(verticesB, skipB); + *same= !(((ptr_intT)skipA & 0x1) ^ ((ptr_intT)*skipB & 0x1)); + trace4((qh ferr, "qh_matchvertices: matched by skip %d (v%d) and skip %d (v%d) same? %d\n", + skipA, (*skipAp)->id, *skipB, (*(skipBp-1))->id, *same)); + return (True); +} /* matchvertices */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="newfacet">-</a> + + qh_newfacet() + return a new facet + + returns: + all fields initialized or cleared (NULL) + preallocates neighbors set +*/ +facetT *qh_newfacet(void) { + facetT *facet; + void **freelistp; /* used !qh_NOmem */ + + qh_memalloc_(sizeof(facetT), freelistp, facet, facetT); + memset ((char *)facet, 0, sizeof(facetT)); + if (qh facet_id == qh tracefacet_id) + qh tracefacet= facet; + facet->id= qh facet_id++; + facet->neighbors= qh_setnew(qh hull_dim); +#if !qh_COMPUTEfurthest + facet->furthestdist= 0.0; +#endif +#if qh_MAXoutside + if (qh FORCEoutput && qh APPROXhull) + facet->maxoutside= qh MINoutside; + else + facet->maxoutside= qh DISTround; +#endif + facet->simplicial= True; + facet->good= True; + facet->newfacet= True; + trace4((qh ferr, "qh_newfacet: created facet f%d\n", facet->id)); + return (facet); +} /* newfacet */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="newridge">-</a> + + qh_newridge() + return a new ridge +*/ +ridgeT *qh_newridge(void) { + ridgeT *ridge; + void **freelistp; /* used !qh_NOmem */ + + qh_memalloc_(sizeof(ridgeT), freelistp, ridge, ridgeT); + memset ((char *)ridge, 0, sizeof(ridgeT)); + zinc_(Ztotridges); + if (qh ridge_id == 0xFFFFFF) { + fprintf(qh ferr, "\ +qhull warning: more than %d ridges. ID field overflows and two ridges\n\ +may have the same identifier. Otherwise output ok.\n", 0xFFFFFF); + } + ridge->id= qh ridge_id++; + trace4((qh ferr, "qh_newridge: created ridge r%d\n", ridge->id)); + return (ridge); +} /* newridge */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="pointid">-</a> + + qh_pointid( ) + return id for a point, + returns -3 if null, -2 if interior, or -1 if not known + + alternative code: + unsigned long id; + id= ((unsigned long)point - (unsigned long)qh.first_point)/qh.normal_size; + + notes: + if point not in point array + the code does a comparison of unrelated pointers. +*/ +int qh_pointid (pointT *point) { + long offset, id; + + if (!point) + id= -3; + else if (point == qh interior_point) + id= -2; + else if (point >= qh first_point + && point < qh first_point + qh num_points * qh hull_dim) { + offset= point - qh first_point; + id= offset / qh hull_dim; + }else if ((id= qh_setindex (qh other_points, point)) != -1) + id += qh num_points; + else + id= -1; + return (int) id; +} /* pointid */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="removefacet">-</a> + + qh_removefacet( facet ) + unlinks facet from qh.facet_list, + + returns: + updates qh.facet_list .newfacet_list .facet_next visible_list + decrements qh.num_facets + + see: + qh_appendfacet +*/ +void qh_removefacet(facetT *facet) { + facetT *next= facet->next, *previous= facet->previous; + + if (facet == qh newfacet_list) + qh newfacet_list= next; + if (facet == qh facet_next) + qh facet_next= next; + if (facet == qh visible_list) + qh visible_list= next; + if (previous) { + previous->next= next; + next->previous= previous; + }else { /* 1st facet in qh facet_list */ + qh facet_list= next; + qh facet_list->previous= NULL; + } + qh num_facets--; + trace4((qh ferr, "qh_removefacet: remove f%d from facet_list\n", facet->id)); +} /* removefacet */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="removevertex">-</a> + + qh_removevertex( vertex ) + unlinks vertex from qh.vertex_list, + + returns: + updates qh.vertex_list .newvertex_list + decrements qh.num_vertices +*/ +void qh_removevertex(vertexT *vertex) { + vertexT *next= vertex->next, *previous= vertex->previous; + + if (vertex == qh newvertex_list) + qh newvertex_list= next; + if (previous) { + previous->next= next; + next->previous= previous; + }else { /* 1st vertex in qh vertex_list */ + qh vertex_list= vertex->next; + qh vertex_list->previous= NULL; + } + qh num_vertices--; + trace4((qh ferr, "qh_removevertex: remove v%d from vertex_list\n", vertex->id)); +} /* removevertex */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="updatevertices">-</a> + + qh_updatevertices() + update vertex neighbors and delete interior vertices + + returns: + if qh.VERTEXneighbors, updates neighbors for each vertex + if qh.newvertex_list, + removes visible neighbors from vertex neighbors + if qh.newfacet_list + adds new facets to vertex neighbors + if qh.visible_list + interior vertices added to qh.del_vertices for later partitioning + + design: + if qh.VERTEXneighbors + deletes references to visible facets from vertex neighbors + appends new facets to the neighbor list for each vertex + checks all vertices of visible facets + removes visible facets from neighbor lists + marks unused vertices for deletion +*/ +void qh_updatevertices (void /*qh newvertex_list, newfacet_list, visible_list*/) { + facetT *newfacet= NULL, *neighbor, **neighborp, *visible; + vertexT *vertex, **vertexp; + + trace3((qh ferr, "qh_updatevertices: delete interior vertices and update vertex->neighbors\n")); + if (qh VERTEXneighbors) { + FORALLvertex_(qh newvertex_list) { + FOREACHneighbor_(vertex) { + if (neighbor->visible) + SETref_(neighbor)= NULL; + } + qh_setcompact (vertex->neighbors); + } + FORALLnew_facets { + FOREACHvertex_(newfacet->vertices) + qh_setappend (&vertex->neighbors, newfacet); + } + FORALLvisible_facets { + FOREACHvertex_(visible->vertices) { + if (!vertex->newlist && !vertex->deleted) { + FOREACHneighbor_(vertex) { /* this can happen under merging */ + if (!neighbor->visible) + break; + } + if (neighbor) + qh_setdel (vertex->neighbors, visible); + else { + vertex->deleted= True; + qh_setappend (&qh del_vertices, vertex); + trace2((qh ferr, "qh_updatevertices: delete vertex p%d (v%d) in f%d\n", + qh_pointid(vertex->point), vertex->id, visible->id)); + } + } + } + } + }else { /* !VERTEXneighbors */ + FORALLvisible_facets { + FOREACHvertex_(visible->vertices) { + if (!vertex->newlist && !vertex->deleted) { + vertex->deleted= True; + qh_setappend (&qh del_vertices, vertex); + trace2((qh ferr, "qh_updatevertices: delete vertex p%d (v%d) in f%d\n", + qh_pointid(vertex->point), vertex->id, visible->id)); + } + } + } + } +} /* updatevertices */ + + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly.h new file mode 100644 index 0000000000000000000000000000000000000000..2d49c284ce2de41c09a85ac49f552dba5fce1777 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly.h @@ -0,0 +1,291 @@ +/*<html><pre> -<a href="qh-poly.htm" + >-------------------------------</a><a name="TOP">-</a> + + poly.h + header file for poly.c and poly2.c + + see qh-poly.htm, qhull.h and poly.c + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFpoly +#define qhDEFpoly 1 + +/*=============== constants ========================== */ + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="ALGORITHMfault">-</a> + + ALGORITHMfault + use as argument to checkconvex() to report errors during buildhull +*/ +#define qh_ALGORITHMfault 0 + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="DATAfault">-</a> + + DATAfault + use as argument to checkconvex() to report errors during initialhull +*/ +#define qh_DATAfault 1 + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="DUPLICATEridge">-</a> + + DUPLICATEridge + special value for facet->neighbor to indicate a duplicate ridge + + notes: + set by matchneighbor, used by matchmatch and mark_dupridge +*/ +#define qh_DUPLICATEridge ( facetT * ) 1L + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="MERGEridge">-</a> + + MERGEridge flag in facet + special value for facet->neighbor to indicate a merged ridge + + notes: + set by matchneighbor, used by matchmatch and mark_dupridge +*/ +#define qh_MERGEridge ( facetT * ) 2L + + +/*============ -structures- ====================*/ + +/*=========== -macros- =========================*/ + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLfacet_">-</a> + + FORALLfacet_( facetlist ) { ... } + assign 'facet' to each facet in facetlist + + notes: + uses 'facetT *facet;' + assumes last facet is a sentinel + + see: + FORALLfacets +*/ +#define FORALLfacet_( facetlist ) if ( facetlist ) for( facet=( facetlist );facet && facet->next;facet=facet->next ) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLnew_facets">-</a> + + FORALLnew_facets { ... } + assign 'newfacet' to each facet in qh.newfacet_list + + notes: + uses 'facetT *newfacet;' + at exit, newfacet==NULL +*/ +#define FORALLnew_facets for( newfacet=qh newfacet_list;newfacet && newfacet->next;newfacet=newfacet->next ) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLvertex_">-</a> + + FORALLvertex_( vertexlist ) { ... } + assign 'vertex' to each vertex in vertexlist + + notes: + uses 'vertexT *vertex;' + at exit, vertex==NULL +*/ +#define FORALLvertex_( vertexlist ) for ( vertex=( vertexlist );vertex && vertex->next;vertex= vertex->next ) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLvisible_facets">-</a> + + FORALLvisible_facets { ... } + assign 'visible' to each visible facet in qh.visible_list + + notes: + uses 'vacetT *visible;' + at exit, visible==NULL +*/ +#define FORALLvisible_facets for (visible=qh visible_list; visible && visible->visible; visible= visible->next) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLsame_">-</a> + + FORALLsame_( newfacet ) { ... } + assign 'same' to each facet in newfacet->f.samecycle + + notes: + uses 'facetT *same;' + stops when it returns to newfacet +*/ +#define FORALLsame_(newfacet) for (same= newfacet->f.samecycle; same != newfacet; same= same->f.samecycle) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLsame_cycle_">-</a> + + FORALLsame_cycle_( newfacet ) { ... } + assign 'same' to each facet in newfacet->f.samecycle + + notes: + uses 'facetT *same;' + at exit, same == NULL +*/ +#define FORALLsame_cycle_(newfacet) \ + for (same= newfacet->f.samecycle; \ + same; same= (same == newfacet ? NULL : same->f.samecycle)) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHneighborA_">-</a> + + FOREACHneighborA_( facet ) { ... } + assign 'neighborA' to each neighbor in facet->neighbors + + FOREACHneighborA_( vertex ) { ... } + assign 'neighborA' to each neighbor in vertex->neighbors + + declare: + facetT *neighborA, **neighborAp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHneighborA_(facet) FOREACHsetelement_(facetT, facet->neighbors, neighborA) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvisible_">-</a> + + FOREACHvisible_( facets ) { ... } + assign 'visible' to each facet in facets + + notes: + uses 'facetT *facet, *facetp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvisible_(facets) FOREACHsetelement_(facetT, facets, visible) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHnewfacet_">-</a> + + FOREACHnewfacet_( facets ) { ... } + assign 'newfacet' to each facet in facets + + notes: + uses 'facetT *newfacet, *newfacetp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHnewfacet_(facets) FOREACHsetelement_(facetT, facets, newfacet) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertexA_">-</a> + + FOREACHvertexA_( vertices ) { ... } + assign 'vertexA' to each vertex in vertices + + notes: + uses 'vertexT *vertexA, *vertexAp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvertexA_(vertices) FOREACHsetelement_(vertexT, vertices, vertexA) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertexreverse12_">-</a> + + FOREACHvertexreverse12_( vertices ) { ... } + assign 'vertex' to each vertex in vertices + reverse order of first two vertices + + notes: + uses 'vertexT *vertex, *vertexp;' + see <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvertexreverse12_(vertices) FOREACHsetelementreverse12_(vertexT, vertices, vertex) + + +/*=============== prototypes poly.c in alphabetical order ================*/ + +void qh_appendfacet(facetT *facet); +void qh_appendvertex(vertexT *vertex); +void qh_attachnewfacets (void); +boolT qh_checkflipped (facetT *facet, realT *dist, boolT allerror); +void qh_delfacet(facetT *facet); +void qh_deletevisible(void /*qh visible_list, qh horizon_list*/); +setT *qh_facetintersect (facetT *facetA, facetT *facetB, int *skipAp,int *skipBp, int extra); +unsigned qh_gethash (int hashsize, setT *set, int size, int firstindex, void *skipelem); +facetT *qh_makenewfacet(setT *vertices, boolT toporient, facetT *facet); +void qh_makenewplanes ( void /* newfacet_list */); +facetT *qh_makenew_nonsimplicial (facetT *visible, vertexT *apex, int *numnew); +facetT *qh_makenew_simplicial (facetT *visible, vertexT *apex, int *numnew); +void qh_matchneighbor (facetT *newfacet, int newskip, int hashsize, + int *hashcount); +void qh_matchnewfacets (void); +boolT qh_matchvertices (int firstindex, setT *verticesA, int skipA, + setT *verticesB, int *skipB, boolT *same); +facetT *qh_newfacet(void); +ridgeT *qh_newridge(void); +int qh_pointid (pointT *point); +void qh_removefacet(facetT *facet); +void qh_removevertex(vertexT *vertex); +void qh_updatevertices (void); + + +/*========== -prototypes poly2.c in alphabetical order ===========*/ + +void qh_addhash (void* newelem, setT *hashtable, int hashsize, unsigned hash); +void qh_check_bestdist (void); +void qh_check_maxout (void); +void qh_check_output (void); +void qh_check_point (pointT *point, facetT *facet, realT *maxoutside, realT *maxdist, facetT **errfacet1, facetT **errfacet2); +void qh_check_points(void); +void qh_checkconvex(facetT *facetlist, int fault); +void qh_checkfacet(facetT *facet, boolT newmerge, boolT *waserrorp); +void qh_checkflipped_all (facetT *facetlist); +void qh_checkpolygon(facetT *facetlist); +void qh_checkvertex (vertexT *vertex); +void qh_clearcenters (qh_CENTER type); +void qh_createsimplex(setT *vertices); +void qh_delridge(ridgeT *ridge); +void qh_delvertex (vertexT *vertex); +setT *qh_facet3vertex (facetT *facet); +facetT *qh_findbestfacet (pointT *point, boolT bestoutside, + realT *bestdist, boolT *isoutside); +facetT *qh_findbestlower (facetT *upperfacet, pointT *point, realT *bestdistp, int *numpart); +facetT *qh_findfacet_all (pointT *point, realT *bestdist, boolT *isoutside, + int *numpart); +int qh_findgood (facetT *facetlist, int goodhorizon); +void qh_findgood_all (facetT *facetlist); +void qh_furthestnext (void /* qh facet_list */); +void qh_furthestout (facetT *facet); +void qh_infiniteloop (facetT *facet); +void qh_initbuild(void); +void qh_initialhull(setT *vertices); +setT *qh_initialvertices(int dim, setT *maxpoints, pointT *points, int numpoints); +vertexT *qh_isvertex (pointT *point, setT *vertices); +vertexT *qh_makenewfacets (pointT *point /*horizon_list, visible_list*/); +void qh_matchduplicates (facetT *atfacet, int atskip, int hashsize, int *hashcount); +void qh_nearcoplanar ( void /* qh.facet_list */); +vertexT *qh_nearvertex (facetT *facet, pointT *point, realT *bestdistp); +int qh_newhashtable(int newsize); +vertexT *qh_newvertex(pointT *point); +ridgeT *qh_nextridge3d (ridgeT *atridge, facetT *facet, vertexT **vertexp); +void qh_outcoplanar (void /* facet_list */); +pointT *qh_point (int id); +void qh_point_add (setT *set, pointT *point, void *elem); +setT *qh_pointfacet (void /*qh facet_list*/); +setT *qh_pointvertex (void /*qh facet_list*/); +void qh_prependfacet(facetT *facet, facetT **facetlist); +void qh_printhashtable(FILE *fp); +void qh_printlists (void); +void qh_resetlists (boolT stats, boolT resetVisible /*qh newvertex_list newfacet_list visible_list*/); +void qh_setvoronoi_all (void); +void qh_triangulate (void /*qh facet_list*/); +void qh_triangulate_facet (facetT *facetA, vertexT **first_vertex); +void qh_triangulate_link (facetT *oldfacetA, facetT *facetA, facetT *oldfacetB, facetT *facetB); +void qh_triangulate_mirror (facetT *facetA, facetT *facetB); +void qh_triangulate_null (facetT *facetA); +void qh_vertexintersect(setT **vertexsetA,setT *vertexsetB); +setT *qh_vertexintersect_new(setT *vertexsetA,setT *vertexsetB); +void qh_vertexneighbors (void /*qh facet_list*/); +boolT qh_vertexsubset(setT *vertexsetA, setT *vertexsetB); + + +#endif /* qhDEFpoly */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly2.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly2.c new file mode 100644 index 0000000000000000000000000000000000000000..79ce4afa83a92b4836101a782ecb417a4ce8c5df --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/poly2.c @@ -0,0 +1,3137 @@ +/*<html><pre> -<a href="qh-poly.htm" + >-------------------------------</a><a name="TOP">-</a> + + poly2.c + implements polygons and simplices + + see qh-poly.htm, poly.h and qhull.h + + frequently used code is in poly.c + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include "qhull_a.h" + +/*======== functions in alphabetical order ==========*/ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="addhash">-</a> + + qh_addhash( newelem, hashtable, hashsize, hash ) + add newelem to linear hash table at hash if not already there +*/ +void qh_addhash (void* newelem, setT *hashtable, int hashsize, unsigned hash) { + int scan; + void *elem; + + for (scan= (int)hash; (elem= SETelem_(hashtable, scan)); + scan= (++scan >= hashsize ? 0 : scan)) { + if (elem == newelem) + break; + } + /* loop terminates because qh_HASHfactor >= 1.1 by qh_initbuffers */ + if (!elem) + SETelem_(hashtable, scan)= newelem; +} /* addhash */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="check_bestdist">-</a> + + qh_check_bestdist() + check that all points are within max_outside of the nearest facet + if qh.ONLYgood, + ignores !good facets + + see: + qh_check_maxout(), qh_outerinner() + + notes: + only called from qh_check_points() + seldom used since qh.MERGING is almost always set + if notverified>0 at end of routine + some points were well inside the hull. If the hull contains + a lens-shaped component, these points were not verified. Use + options 'Qi Tv' to verify all points. (Exhaustive check also verifies) + + design: + determine facet for each point (if any) + for each point + start with the assigned facet or with the first facet + find the best facet for the point and check all coplanar facets + error if point is outside of facet +*/ +void qh_check_bestdist (void) { + boolT waserror= False, unassigned; + facetT *facet, *bestfacet, *errfacet1= NULL, *errfacet2= NULL; + facetT *facetlist; + realT dist, maxoutside, maxdist= -REALmax; + pointT *point; + int numpart= 0, facet_i, facet_n, notgood= 0, notverified= 0; + setT *facets; + + trace1((qh ferr, "qh_check_bestdist: check points below nearest facet. Facet_list f%d\n", + qh facet_list->id)); + maxoutside= qh_maxouter(); + maxoutside += qh DISTround; + /* one more qh.DISTround for check computation */ + trace1((qh ferr, "qh_check_bestdist: check that all points are within %2.2g of best facet\n", maxoutside)); + facets= qh_pointfacet (/*qh facet_list*/); + if (!qh_QUICKhelp && qh PRINTprecision) + fprintf (qh ferr, "\n\ +qhull output completed. Verifying that %d points are\n\ +below %2.2g of the nearest %sfacet.\n", + qh_setsize(facets), maxoutside, (qh ONLYgood ? "good " : "")); + FOREACHfacet_i_(facets) { /* for each point with facet assignment */ + if (facet) + unassigned= False; + else { + unassigned= True; + facet= qh facet_list; + } + point= qh_point(facet_i); + if (point == qh GOODpointp) + continue; + qh_distplane(point, facet, &dist); + numpart++; + bestfacet= qh_findbesthorizon (!qh_IScheckmax, point, facet, qh_NOupper, &dist, &numpart); + /* occurs after statistics reported */ + maximize_(maxdist, dist); + if (dist > maxoutside) { + if (qh ONLYgood && !bestfacet->good + && !((bestfacet= qh_findgooddist (point, bestfacet, &dist, &facetlist)) + && dist > maxoutside)) + notgood++; + else { + waserror= True; + fprintf(qh ferr, "qhull precision error: point p%d is outside facet f%d, distance= %6.8g maxoutside= %6.8g\n", + facet_i, bestfacet->id, dist, maxoutside); + if (errfacet1 != bestfacet) { + errfacet2= errfacet1; + errfacet1= bestfacet; + } + } + }else if (unassigned && dist < -qh MAXcoplanar) + notverified++; + } + qh_settempfree (&facets); + if (notverified && !qh DELAUNAY && !qh_QUICKhelp && qh PRINTprecision) + fprintf(qh ferr, "\n%d points were well inside the hull. If the hull contains\n\ +a lens-shaped component, these points were not verified. Use\n\ +options 'Qci Tv' to verify all points.\n", notverified); + if (maxdist > qh outside_err) { + fprintf( qh ferr, "qhull precision error (qh_check_bestdist): a coplanar point is %6.2g from convex hull. The maximum value (qh.outside_err) is %6.2g\n", + maxdist, qh outside_err); + qh_errexit2 (qh_ERRprec, errfacet1, errfacet2); + }else if (waserror && qh outside_err > REALmax/2) + qh_errexit2 (qh_ERRprec, errfacet1, errfacet2); + else if (waserror) + ; /* the error was logged to qh.ferr but does not effect the output */ + trace0((qh ferr, "qh_check_bestdist: max distance outside %2.2g\n", maxdist)); +} /* check_bestdist */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="check_maxout">-</a> + + qh_check_maxout() + updates qh.max_outside by checking all points against bestfacet + if qh.ONLYgood, ignores !good facets + + returns: + updates facet->maxoutside via qh_findbesthorizon() + sets qh.maxoutdone + if printing qh.min_vertex (qh_outerinner), + it is updated to the current vertices + removes inside/coplanar points from coplanarset as needed + + notes: + defines coplanar as min_vertex instead of MAXcoplanar + may not need to check near-inside points because of qh.MAXcoplanar + and qh.KEEPnearinside (before it was -DISTround) + + see also: + qh_check_bestdist() + + design: + if qh.min_vertex is needed + for all neighbors of all vertices + test distance from vertex to neighbor + determine facet for each point (if any) + for each point with an assigned facet + find the best facet for the point and check all coplanar facets + (updates outer planes) + remove near-inside points from coplanar sets +*/ +#ifndef qh_NOmerge +void qh_check_maxout (void) { + facetT *facet, *bestfacet, *neighbor, **neighborp, *facetlist; + realT dist, maxoutside, minvertex, old_maxoutside; + pointT *point; + int numpart= 0, facet_i, facet_n, notgood= 0; + setT *facets, *vertices; + vertexT *vertex; + + trace1((qh ferr, "qh_check_maxout: check and update maxoutside for each facet.\n")); + maxoutside= minvertex= 0; + if (qh VERTEXneighbors + && (qh PRINTsummary || qh KEEPinside || qh KEEPcoplanar + || qh TRACElevel || qh PRINTstatistics + || qh PRINTout[0] == qh_PRINTsummary || qh PRINTout[0] == qh_PRINTnone)) { + trace1((qh ferr, "qh_check_maxout: determine actual maxoutside and minvertex\n")); + vertices= qh_pointvertex (/*qh facet_list*/); + FORALLvertices { + FOREACHneighbor_(vertex) { + zinc_(Zdistvertex); /* distance also computed by main loop below */ + qh_distplane (vertex->point, neighbor, &dist); + minimize_(minvertex, dist); + if (-dist > qh TRACEdist || dist > qh TRACEdist + || neighbor == qh tracefacet || vertex == qh tracevertex) + fprintf (qh ferr, "qh_check_maxout: p%d (v%d) is %.2g from f%d\n", + qh_pointid (vertex->point), vertex->id, dist, neighbor->id); + } + } + if (qh MERGING) { + wmin_(Wminvertex, qh min_vertex); + } + qh min_vertex= minvertex; + qh_settempfree (&vertices); + } + facets= qh_pointfacet (/*qh facet_list*/); + do { + old_maxoutside= fmax_(qh max_outside, maxoutside); + FOREACHfacet_i_(facets) { /* for each point with facet assignment */ + if (facet) { + point= qh_point(facet_i); + if (point == qh GOODpointp) + continue; + zinc_(Ztotcheck); + qh_distplane(point, facet, &dist); + numpart++; + bestfacet= qh_findbesthorizon (qh_IScheckmax, point, facet, !qh_NOupper, &dist, &numpart); + if (bestfacet && dist > maxoutside) { + if (qh ONLYgood && !bestfacet->good + && !((bestfacet= qh_findgooddist (point, bestfacet, &dist, &facetlist)) + && dist > maxoutside)) + notgood++; + else + maxoutside= dist; + } + if (dist > qh TRACEdist || (bestfacet && bestfacet == qh tracefacet)) + fprintf (qh ferr, "qh_check_maxout: p%d is %.2g above f%d\n", + qh_pointid (point), dist, bestfacet->id); + } + } + }while + (maxoutside > 2*old_maxoutside); + /* if qh.maxoutside increases substantially, qh_SEARCHdist is not valid + e.g., RBOX 5000 s Z1 G1e-13 t1001200614 | qhull */ + zzadd_(Zcheckpart, numpart); + qh_settempfree (&facets); + wval_(Wmaxout)= maxoutside - qh max_outside; + wmax_(Wmaxoutside, qh max_outside); + qh max_outside= maxoutside; + qh_nearcoplanar (/*qh.facet_list*/); + qh maxoutdone= True; + trace1((qh ferr, "qh_check_maxout: maxoutside %2.2g, min_vertex %2.2g, outside of not good %d\n", + maxoutside, qh min_vertex, notgood)); +} /* check_maxout */ +#else /* qh_NOmerge */ +void qh_check_maxout (void) { +} +#endif + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="check_output">-</a> + + qh_check_output() + performs the checks at the end of qhull algorithm + Maybe called after voronoi output. Will recompute otherwise centrums are Voronoi centers instead +*/ +void qh_check_output (void) { + int i; + + if (qh STOPcone) + return; + if (qh VERIFYoutput | qh IStracing | qh CHECKfrequently) { + qh_checkpolygon (qh facet_list); + qh_checkflipped_all (qh facet_list); + qh_checkconvex (qh facet_list, qh_ALGORITHMfault); + }else if (!qh MERGING && qh_newstats (qhstat precision, &i)) { + qh_checkflipped_all (qh facet_list); + qh_checkconvex (qh facet_list, qh_ALGORITHMfault); + } +} /* check_output */ + + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="check_point">-</a> + + qh_check_point( point, facet, maxoutside, maxdist, errfacet1, errfacet2 ) + check that point is less than maxoutside from facet +*/ +void qh_check_point (pointT *point, facetT *facet, realT *maxoutside, realT *maxdist, facetT **errfacet1, facetT **errfacet2) { + realT dist; + + /* occurs after statistics reported */ + qh_distplane(point, facet, &dist); + if (dist > *maxoutside) { + if (*errfacet1 != facet) { + *errfacet2= *errfacet1; + *errfacet1= facet; + } + fprintf(qh ferr, "qhull precision error: point p%d is outside facet f%d, distance= %6.8g maxoutside= %6.8g\n", + qh_pointid(point), facet->id, dist, *maxoutside); + } + maximize_(*maxdist, dist); +} /* qh_check_point */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="check_points">-</a> + + qh_check_points() + checks that all points are inside all facets + + notes: + if many points and qh_check_maxout not called (i.e., !qh.MERGING), + calls qh_findbesthorizon (seldom done). + ignores flipped facets + maxoutside includes 2 qh.DISTrounds + one qh.DISTround for the computed distances in qh_check_points + qh_printafacet and qh_printsummary needs only one qh.DISTround + the computation for qh.VERIFYdirect does not account for qh.other_points + + design: + if many points + use qh_check_bestdist() + else + for all facets + for all points + check that point is inside facet +*/ +void qh_check_points (void) { + facetT *facet, *errfacet1= NULL, *errfacet2= NULL; + realT total, maxoutside, maxdist= -REALmax; + pointT *point, **pointp, *pointtemp; + boolT testouter; + + maxoutside= qh_maxouter(); + maxoutside += qh DISTround; + /* one more qh.DISTround for check computation */ + trace1((qh ferr, "qh_check_points: check all points below %2.2g of all facet planes\n", + maxoutside)); + if (qh num_good) /* miss counts other_points and !good facets */ + total= (float) qh num_good * qh num_points; + else + total= (float) qh num_facets * qh num_points; + if (total >= qh_VERIFYdirect && !qh maxoutdone) { + if (!qh_QUICKhelp && qh SKIPcheckmax && qh MERGING) + fprintf (qh ferr, "\n\ +qhull input warning: merging without checking outer planes ('Q5' or 'Po').\n\ +Verify may report that a point is outside of a facet.\n"); + qh_check_bestdist(); + }else { + if (qh_MAXoutside && qh maxoutdone) + testouter= True; + else + testouter= False; + if (!qh_QUICKhelp) { + if (qh MERGEexact) + fprintf (qh ferr, "\n\ +qhull input warning: exact merge ('Qx'). Verify may report that a point\n\ +is outside of a facet. See qh-optq.htm#Qx\n"); + else if (qh SKIPcheckmax || qh NOnearinside) + fprintf (qh ferr, "\n\ +qhull input warning: no outer plane check ('Q5') or no processing of\n\ +near-inside points ('Q8'). Verify may report that a point is outside\n\ +of a facet.\n"); + } + if (qh PRINTprecision) { + if (testouter) + fprintf (qh ferr, "\n\ +Output completed. Verifying that all points are below outer planes of\n\ +all %sfacets. Will make %2.0f distance computations.\n", + (qh ONLYgood ? "good " : ""), total); + else + fprintf (qh ferr, "\n\ +Output completed. Verifying that all points are below %2.2g of\n\ +all %sfacets. Will make %2.0f distance computations.\n", + maxoutside, (qh ONLYgood ? "good " : ""), total); + } + FORALLfacets { + if (!facet->good && qh ONLYgood) + continue; + if (facet->flipped) + continue; + if (!facet->normal) { + fprintf( qh ferr, "qhull warning (qh_check_points): missing normal for facet f%d\n", facet->id); + continue; + } + if (testouter) { +#if qh_MAXoutside + maxoutside= facet->maxoutside + 2* qh DISTround; + /* one DISTround to actual point and another to computed point */ +#endif + } + FORALLpoints { + if (point != qh GOODpointp) + qh_check_point (point, facet, &maxoutside, &maxdist, &errfacet1, &errfacet2); + } + FOREACHpoint_(qh other_points) { + if (point != qh GOODpointp) + qh_check_point (point, facet, &maxoutside, &maxdist, &errfacet1, &errfacet2); + } + } + if (maxdist > qh outside_err) { + fprintf( qh ferr, "qhull precision error (qh_check_points): a coplanar point is %6.2g from convex hull. The maximum value (qh.outside_err) is %6.2g\n", + maxdist, qh outside_err ); + qh_errexit2( qh_ERRprec, errfacet1, errfacet2 ); + }else if (errfacet1 && qh outside_err > REALmax/2) + qh_errexit2( qh_ERRprec, errfacet1, errfacet2 ); + else if (errfacet1) + ; /* the error was logged to qh.ferr but does not effect the output */ + trace0((qh ferr, "qh_check_points: max distance outside %2.2g\n", maxdist)); + } +} /* check_points */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="checkconvex">-</a> + + qh_checkconvex( facetlist, fault ) + check that each ridge in facetlist is convex + fault = qh_DATAfault if reporting errors + = qh_ALGORITHMfault otherwise + + returns: + counts Zconcaveridges and Zcoplanarridges + errors if concaveridge or if merging an coplanar ridge + + note: + if not merging, + tests vertices for neighboring simplicial facets + else if ZEROcentrum, + tests vertices for neighboring simplicial facets + else + tests centrums of neighboring facets + + design: + for all facets + report flipped facets + if ZEROcentrum and simplicial neighbors + test vertices for neighboring simplicial facets + else + test centrum against all neighbors +*/ +void qh_checkconvex(facetT *facetlist, int fault) { + facetT *facet, *neighbor, **neighborp, *errfacet1=NULL, *errfacet2=NULL; + vertexT *vertex; + realT dist; + pointT *centrum; + boolT waserror= False, centrum_warning= False, tempcentrum= False, allsimplicial; + int neighbor_i; + + trace1((qh ferr, "qh_checkconvex: check all ridges are convex\n")); + if (!qh RERUN) { + zzval_(Zconcaveridges)= 0; + zzval_(Zcoplanarridges)= 0; + } + FORALLfacet_(facetlist) { + if (facet->flipped) { + qh_precision ("flipped facet"); + fprintf (qh ferr, "qhull precision error: f%d is flipped (interior point is outside)\n", + facet->id); + errfacet1= facet; + waserror= True; + continue; + } + if (qh MERGING && (!qh ZEROcentrum || !facet->simplicial || facet->tricoplanar)) + allsimplicial= False; + else { + allsimplicial= True; + neighbor_i= 0; + FOREACHneighbor_(facet) { + vertex= SETelemt_(facet->vertices, neighbor_i++, vertexT); + if (!neighbor->simplicial || neighbor->tricoplanar) { + allsimplicial= False; + continue; + } + qh_distplane (vertex->point, neighbor, &dist); + if (dist > -qh DISTround) { + if (fault == qh_DATAfault) { + qh_precision ("coplanar or concave ridge"); + fprintf (qh ferr, "qhull precision error: initial simplex is not convex. Distance=%.2g\n", dist); + qh_errexit(qh_ERRsingular, NULL, NULL); + } + if (dist > qh DISTround) { + zzinc_(Zconcaveridges); + qh_precision ("concave ridge"); + fprintf (qh ferr, "qhull precision error: f%d is concave to f%d, since p%d (v%d) is %6.4g above\n", + facet->id, neighbor->id, qh_pointid(vertex->point), vertex->id, dist); + errfacet1= facet; + errfacet2= neighbor; + waserror= True; + }else if (qh ZEROcentrum) { + if (dist > 0) { /* qh_checkzero checks that dist < - qh DISTround */ + zzinc_(Zcoplanarridges); + qh_precision ("coplanar ridge"); + fprintf (qh ferr, "qhull precision error: f%d is clearly not convex to f%d, since p%d (v%d) is %6.4g above\n", + facet->id, neighbor->id, qh_pointid(vertex->point), vertex->id, dist); + errfacet1= facet; + errfacet2= neighbor; + waserror= True; + } + }else { + zzinc_(Zcoplanarridges); + qh_precision ("coplanar ridge"); + trace0((qh ferr, "qhull precision error: f%d may be coplanar to f%d, since p%d (v%d) is within %6.4g during p%d\n", + facet->id, neighbor->id, qh_pointid(vertex->point), vertex->id, dist, qh furthest_id)); + } + } + } + } + if (!allsimplicial) { + if (qh CENTERtype == qh_AScentrum) { + if (!facet->center) + facet->center= qh_getcentrum (facet); + centrum= facet->center; + }else { + if (!centrum_warning && (!facet->simplicial || facet->tricoplanar)) { + centrum_warning= True; + fprintf (qh ferr, "qhull note: recomputing centrums for convexity test. This may lead to false, precision errors.\n"); + } + centrum= qh_getcentrum(facet); + tempcentrum= True; + } + FOREACHneighbor_(facet) { + if (qh ZEROcentrum && facet->simplicial && neighbor->simplicial) + continue; + if (facet->tricoplanar || neighbor->tricoplanar) + continue; + zzinc_(Zdistconvex); + qh_distplane (centrum, neighbor, &dist); + if (dist > qh DISTround) { + zzinc_(Zconcaveridges); + qh_precision ("concave ridge"); + fprintf (qh ferr, "qhull precision error: f%d is concave to f%d. Centrum of f%d is %6.4g above f%d\n", + facet->id, neighbor->id, facet->id, dist, neighbor->id); + errfacet1= facet; + errfacet2= neighbor; + waserror= True; + }else if (dist >= 0.0) { /* if arithmetic always rounds the same, + can test against centrum radius instead */ + zzinc_(Zcoplanarridges); + qh_precision ("coplanar ridge"); + fprintf (qh ferr, "qhull precision error: f%d is coplanar or concave to f%d. Centrum of f%d is %6.4g above f%d\n", + facet->id, neighbor->id, facet->id, dist, neighbor->id); + errfacet1= facet; + errfacet2= neighbor; + waserror= True; + } + } + if (tempcentrum) + qh_memfree(centrum, qh normal_size); + } + } + if (waserror && !qh FORCEoutput) + qh_errexit2 (qh_ERRprec, errfacet1, errfacet2); +} /* checkconvex */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="checkfacet">-</a> + + qh_checkfacet( facet, newmerge, waserror ) + checks for consistency errors in facet + newmerge set if from merge.c + + returns: + sets waserror if any error occurs + + checks: + vertex ids are inverse sorted + unless newmerge, at least hull_dim neighbors and vertices (exactly if simplicial) + if non-simplicial, at least as many ridges as neighbors + neighbors are not duplicated + ridges are not duplicated + in 3-d, ridges=verticies + (qh.hull_dim-1) ridge vertices + neighbors are reciprocated + ridge neighbors are facet neighbors and a ridge for every neighbor + simplicial neighbors match facetintersect + vertex intersection matches vertices of common ridges + vertex neighbors and facet vertices agree + all ridges have distinct vertex sets + + notes: + uses neighbor->seen + + design: + check sets + check vertices + check sizes of neighbors and vertices + check for qh_MERGEridge and qh_DUPLICATEridge flags + check neighbor set + check ridge set + check ridges, neighbors, and vertices +*/ +void qh_checkfacet(facetT *facet, boolT newmerge, boolT *waserrorp) { + facetT *neighbor, **neighborp, *errother=NULL; + ridgeT *ridge, **ridgep, *errridge= NULL, *ridge2; + vertexT *vertex, **vertexp; + unsigned previousid= INT_MAX; + int numneighbors, numvertices, numridges=0, numRvertices=0; + boolT waserror= False; + int skipA, skipB, ridge_i, ridge_n, i; + setT *intersection; + + if (facet->visible) { + fprintf (qh ferr, "qhull internal error (qh_checkfacet): facet f%d is on the visible_list\n", + facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + if (!facet->normal) { + fprintf (qh ferr, "qhull internal error (qh_checkfacet): facet f%d does not have a normal\n", + facet->id); + waserror= True; + } + qh_setcheck (facet->vertices, "vertices for f", facet->id); + qh_setcheck (facet->ridges, "ridges for f", facet->id); + qh_setcheck (facet->outsideset, "outsideset for f", facet->id); + qh_setcheck (facet->coplanarset, "coplanarset for f", facet->id); + qh_setcheck (facet->neighbors, "neighbors for f", facet->id); + FOREACHvertex_(facet->vertices) { + if (vertex->deleted) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): deleted vertex v%d in f%d\n", vertex->id, facet->id); + qh_errprint ("ERRONEOUS", NULL, NULL, NULL, vertex); + waserror= True; + } + if (vertex->id >= previousid) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): vertices of f%d are not in descending id order at v%d\n", facet->id, vertex->id); + waserror= True; + break; + } + previousid= vertex->id; + } + numneighbors= qh_setsize(facet->neighbors); + numvertices= qh_setsize(facet->vertices); + numridges= qh_setsize(facet->ridges); + if (facet->simplicial) { + if (numvertices+numneighbors != 2*qh hull_dim + && !facet->degenerate && !facet->redundant) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): for simplicial facet f%d, #vertices %d + #neighbors %d != 2*qh hull_dim\n", + facet->id, numvertices, numneighbors); + qh_setprint (qh ferr, "", facet->neighbors); + waserror= True; + } + }else { /* non-simplicial */ + if (!newmerge + &&(numvertices < qh hull_dim || numneighbors < qh hull_dim) + && !facet->degenerate && !facet->redundant) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): for facet f%d, #vertices %d or #neighbors %d < qh hull_dim\n", + facet->id, numvertices, numneighbors); + waserror= True; + } + /* in 3-d, can get a vertex twice in an edge list, e.g., RBOX 1000 s W1e-13 t995849315 D2 | QHULL d Tc Tv TP624 TW1e-13 T4 */ + if (numridges < numneighbors + ||(qh hull_dim == 3 && numvertices > numridges && !qh NEWfacets) + ||(qh hull_dim == 2 && numridges + numvertices + numneighbors != 6)) { + if (!facet->degenerate && !facet->redundant) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): for facet f%d, #ridges %d < #neighbors %d or (3-d) > #vertices %d or (2-d) not all 2\n", + facet->id, numridges, numneighbors, numvertices); + waserror= True; + } + } + } + FOREACHneighbor_(facet) { + if (neighbor == qh_MERGEridge || neighbor == qh_DUPLICATEridge) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): facet f%d still has a MERGE or DUP neighbor\n", facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + neighbor->seen= True; + } + FOREACHneighbor_(facet) { + if (!qh_setin(neighbor->neighbors, facet)) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): facet f%d has neighbor f%d, but f%d does not have neighbor f%d\n", + facet->id, neighbor->id, neighbor->id, facet->id); + errother= neighbor; + waserror= True; + } + if (!neighbor->seen) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): facet f%d has a duplicate neighbor f%d\n", + facet->id, neighbor->id); + errother= neighbor; + waserror= True; + } + neighbor->seen= False; + } + FOREACHridge_(facet->ridges) { + qh_setcheck (ridge->vertices, "vertices for r", ridge->id); + ridge->seen= False; + } + FOREACHridge_(facet->ridges) { + if (ridge->seen) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): facet f%d has a duplicate ridge r%d\n", + facet->id, ridge->id); + errridge= ridge; + waserror= True; + } + ridge->seen= True; + numRvertices= qh_setsize(ridge->vertices); + if (numRvertices != qh hull_dim - 1) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): ridge between f%d and f%d has %d vertices\n", + ridge->top->id, ridge->bottom->id, numRvertices); + errridge= ridge; + waserror= True; + } + neighbor= otherfacet_(ridge, facet); + neighbor->seen= True; + if (!qh_setin(facet->neighbors, neighbor)) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): for facet f%d, neighbor f%d of ridge r%d not in facet\n", + facet->id, neighbor->id, ridge->id); + errridge= ridge; + waserror= True; + } + } + if (!facet->simplicial) { + FOREACHneighbor_(facet) { + if (!neighbor->seen) { + fprintf(qh ferr, "qhull internal error (qh_checkfacet): facet f%d does not have a ridge for neighbor f%d\n", + facet->id, neighbor->id); + errother= neighbor; + waserror= True; + } + intersection= qh_vertexintersect_new(facet->vertices, neighbor->vertices); + qh_settemppush (intersection); + FOREACHvertex_(facet->vertices) { + vertex->seen= False; + vertex->seen2= False; + } + FOREACHvertex_(intersection) + vertex->seen= True; + FOREACHridge_(facet->ridges) { + if (neighbor != otherfacet_(ridge, facet)) + continue; + FOREACHvertex_(ridge->vertices) { + if (!vertex->seen) { + fprintf (qh ferr, "qhull internal error (qh_checkfacet): vertex v%d in r%d not in f%d intersect f%d\n", + vertex->id, ridge->id, facet->id, neighbor->id); + qh_errexit (qh_ERRqhull, facet, ridge); + } + vertex->seen2= True; + } + } + if (!newmerge) { + FOREACHvertex_(intersection) { + if (!vertex->seen2) { + if (qh IStracing >=3 || !qh MERGING) { + fprintf (qh ferr, "qhull precision error (qh_checkfacet): vertex v%d in f%d intersect f%d but\n\ + not in a ridge. This is ok under merging. Last point was p%d\n", + vertex->id, facet->id, neighbor->id, qh furthest_id); + if (!qh FORCEoutput && !qh MERGING) { + qh_errprint ("ERRONEOUS", facet, neighbor, NULL, vertex); + if (!qh MERGING) + qh_errexit (qh_ERRqhull, NULL, NULL); + } + } + } + } + } + qh_settempfree (&intersection); + } + }else { /* simplicial */ + FOREACHneighbor_(facet) { + if (neighbor->simplicial) { + skipA= SETindex_(facet->neighbors, neighbor); + skipB= qh_setindex (neighbor->neighbors, facet); + if (!qh_setequal_skip (facet->vertices, skipA, neighbor->vertices, skipB)) { + fprintf (qh ferr, "qhull internal error (qh_checkfacet): facet f%d skip %d and neighbor f%d skip %d do not match \n", + facet->id, skipA, neighbor->id, skipB); + errother= neighbor; + waserror= True; + } + } + } + } + if (qh hull_dim < 5 && (qh IStracing > 2 || qh CHECKfrequently)) { + FOREACHridge_i_(facet->ridges) { /* expensive */ + for (i= ridge_i+1; i < ridge_n; i++) { + ridge2= SETelemt_(facet->ridges, i, ridgeT); + if (qh_setequal (ridge->vertices, ridge2->vertices)) { + fprintf (qh ferr, "qh_checkfacet: ridges r%d and r%d have the same vertices\n", + ridge->id, ridge2->id); + errridge= ridge; + waserror= True; + } + } + } + } + if (waserror) { + qh_errprint("ERRONEOUS", facet, errother, errridge, NULL); + *waserrorp= True; + } +} /* checkfacet */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="checkflipped_all">-</a> + + qh_checkflipped_all( facetlist ) + checks orientation of facets in list against interior point +*/ +void qh_checkflipped_all (facetT *facetlist) { + facetT *facet; + boolT waserror= False; + realT dist; + + if (facetlist == qh facet_list) + zzval_(Zflippedfacets)= 0; + FORALLfacet_(facetlist) { + if (facet->normal && !qh_checkflipped (facet, &dist, !qh_ALL)) { + fprintf(qh ferr, "qhull precision error: facet f%d is flipped, distance= %6.12g\n", + facet->id, dist); + if (!qh FORCEoutput) { + qh_errprint("ERRONEOUS", facet, NULL, NULL, NULL); + waserror= True; + } + } + } + if (waserror) { + fprintf (qh ferr, "\n\ +A flipped facet occurs when its distance to the interior point is\n\ +greater than %2.2g, the maximum roundoff error.\n", -qh DISTround); + qh_errexit(qh_ERRprec, NULL, NULL); + } +} /* checkflipped_all */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="checkpolygon">-</a> + + qh_checkpolygon( facetlist ) + checks the correctness of the structure + + notes: + call with either qh.facet_list or qh.newfacet_list + checks num_facets and num_vertices if qh.facet_list + + design: + for each facet + checks facet and outside set + initializes vertexlist + for each facet + checks vertex set + if checking all facets (qh.facetlist) + check facet count + if qh.VERTEXneighbors + check vertex neighbors and count + check vertex count +*/ +void qh_checkpolygon(facetT *facetlist) { + facetT *facet; + vertexT *vertex, **vertexp, *vertexlist; + int numfacets= 0, numvertices= 0, numridges= 0; + int totvneighbors= 0, totvertices= 0; + boolT waserror= False, nextseen= False, visibleseen= False; + + trace1((qh ferr, "qh_checkpolygon: check all facets from f%d\n", facetlist->id)); + if (facetlist != qh facet_list || qh ONLYgood) + nextseen= True; + FORALLfacet_(facetlist) { + if (facet == qh visible_list) + visibleseen= True; + if (!facet->visible) { + if (!nextseen) { + if (facet == qh facet_next) + nextseen= True; + else if (qh_setsize (facet->outsideset)) { + if (!qh NARROWhull +#if !qh_COMPUTEfurthest + || facet->furthestdist >= qh MINoutside +#endif + ) { + fprintf (qh ferr, "qhull internal error (qh_checkpolygon): f%d has outside points before qh facet_next\n", + facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + } + } + numfacets++; + qh_checkfacet(facet, False, &waserror); + } + } + if (qh visible_list && !visibleseen && facetlist == qh facet_list) { + fprintf (qh ferr, "qhull internal error (qh_checkpolygon): visible list f%d no longer on facet list\n", qh visible_list->id); + qh_printlists(); + qh_errexit (qh_ERRqhull, qh visible_list, NULL); + } + if (facetlist == qh facet_list) + vertexlist= qh vertex_list; + else if (facetlist == qh newfacet_list) + vertexlist= qh newvertex_list; + else + vertexlist= NULL; + FORALLvertex_(vertexlist) { + vertex->seen= False; + vertex->visitid= 0; + } + FORALLfacet_(facetlist) { + if (facet->visible) + continue; + if (facet->simplicial) + numridges += qh hull_dim; + else + numridges += qh_setsize (facet->ridges); + FOREACHvertex_(facet->vertices) { + vertex->visitid++; + if (!vertex->seen) { + vertex->seen= True; + numvertices++; + if (qh_pointid (vertex->point) == -1) { + fprintf (qh ferr, "qhull internal error (qh_checkpolygon): unknown point %p for vertex v%d first_point %p\n", + vertex->point, vertex->id, qh first_point); + waserror= True; + } + } + } + } + qh vertex_visit += numfacets; + if (facetlist == qh facet_list) { + if (numfacets != qh num_facets - qh num_visible) { + fprintf(qh ferr, "qhull internal error (qh_checkpolygon): actual number of facets is %d, cumulative facet count is %d - %d visible facets\n", + numfacets, qh num_facets, qh num_visible); + waserror= True; + } + qh vertex_visit++; + if (qh VERTEXneighbors) { + FORALLvertices { + qh_setcheck (vertex->neighbors, "neighbors for v", vertex->id); + if (vertex->deleted) + continue; + totvneighbors += qh_setsize (vertex->neighbors); + } + FORALLfacet_(facetlist) + totvertices += qh_setsize (facet->vertices); + if (totvneighbors != totvertices) { + fprintf(qh ferr, "qhull internal error (qh_checkpolygon): vertex neighbors inconsistent. Totvneighbors %d, totvertices %d\n", + totvneighbors, totvertices); + waserror= True; + } + } + if (numvertices != qh num_vertices - qh_setsize(qh del_vertices)) { + fprintf(qh ferr, "qhull internal error (qh_checkpolygon): actual number of vertices is %d, cumulative vertex count is %d\n", + numvertices, qh num_vertices - qh_setsize(qh del_vertices)); + waserror= True; + } + if (qh hull_dim == 2 && numvertices != numfacets) { + fprintf (qh ferr, "qhull internal error (qh_checkpolygon): #vertices %d != #facets %d\n", + numvertices, numfacets); + waserror= True; + } + if (qh hull_dim == 3 && numvertices + numfacets - numridges/2 != 2) { + fprintf (qh ferr, "qhull warning: #vertices %d + #facets %d - #edges %d != 2\n\ + A vertex appears twice in a edge list. May occur during merging.", + numvertices, numfacets, numridges/2); + /* occurs if lots of merging and a vertex ends up twice in an edge list. e.g., RBOX 1000 s W1e-13 t995849315 D2 | QHULL d Tc Tv */ + } + } + if (waserror) + qh_errexit(qh_ERRqhull, NULL, NULL); +} /* checkpolygon */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="checkvertex">-</a> + + qh_checkvertex( vertex ) + check vertex for consistency + checks vertex->neighbors + + notes: + neighbors checked efficiently in checkpolygon +*/ +void qh_checkvertex (vertexT *vertex) { + boolT waserror= False; + facetT *neighbor, **neighborp, *errfacet=NULL; + + if (qh_pointid (vertex->point) == -1) { + fprintf (qh ferr, "qhull internal error (qh_checkvertex): unknown point id %p\n", vertex->point); + waserror= True; + } + if (vertex->id >= qh vertex_id) { + fprintf (qh ferr, "qhull internal error (qh_checkvertex): unknown vertex id %d\n", vertex->id); + waserror= True; + } + if (!waserror && !vertex->deleted) { + if (qh_setsize (vertex->neighbors)) { + FOREACHneighbor_(vertex) { + if (!qh_setin (neighbor->vertices, vertex)) { + fprintf (qh ferr, "qhull internal error (qh_checkvertex): neighbor f%d does not contain v%d\n", neighbor->id, vertex->id); + errfacet= neighbor; + waserror= True; + } + } + } + } + if (waserror) { + qh_errprint ("ERRONEOUS", NULL, NULL, NULL, vertex); + qh_errexit (qh_ERRqhull, errfacet, NULL); + } +} /* checkvertex */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="clearcenters">-</a> + + qh_clearcenters( type ) + clear old data from facet->center + + notes: + sets new centertype + nop if CENTERtype is the same +*/ +void qh_clearcenters (qh_CENTER type) { + facetT *facet; + + if (qh CENTERtype != type) { + FORALLfacets { + if (qh CENTERtype == qh_ASvoronoi){ + if (facet->center) { + qh_memfree (facet->center, qh center_size); + facet->center= NULL; + } + }else /* qh CENTERtype == qh_AScentrum */ { + if (facet->center) { + qh_memfree (facet->center, qh normal_size); + facet->center= NULL; + } + } + } + qh CENTERtype= type; + } + trace2((qh ferr, "qh_clearcenters: switched to center type %d\n", type)); +} /* clearcenters */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="createsimplex">-</a> + + qh_createsimplex( vertices ) + creates a simplex from a set of vertices + + returns: + initializes qh.facet_list to the simplex + initializes qh.newfacet_list, .facet_tail + initializes qh.vertex_list, .newvertex_list, .vertex_tail + + design: + initializes lists + for each vertex + create a new facet + for each new facet + create its neighbor set +*/ +void qh_createsimplex(setT *vertices) { + facetT *facet= NULL, *newfacet; + boolT toporient= True; + int vertex_i, vertex_n, nth; + setT *newfacets= qh_settemp (qh hull_dim+1); + vertexT *vertex; + + qh facet_list= qh newfacet_list= qh facet_tail= qh_newfacet(); + qh num_facets= qh num_vertices= qh num_visible= 0; + qh vertex_list= qh newvertex_list= qh vertex_tail= qh_newvertex(NULL); + FOREACHvertex_i_(vertices) { + newfacet= qh_newfacet(); + newfacet->vertices= qh_setnew_delnthsorted (vertices, vertex_n, + vertex_i, 0); + newfacet->toporient= toporient; + qh_appendfacet(newfacet); + newfacet->newfacet= True; + qh_appendvertex (vertex); + qh_setappend (&newfacets, newfacet); + toporient ^= True; + } + FORALLnew_facets { + nth= 0; + FORALLfacet_(qh newfacet_list) { + if (facet != newfacet) + SETelem_(newfacet->neighbors, nth++)= facet; + } + qh_settruncate (newfacet->neighbors, qh hull_dim); + } + qh_settempfree (&newfacets); + trace1((qh ferr, "qh_createsimplex: created simplex\n")); +} /* createsimplex */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="delridge">-</a> + + qh_delridge( ridge ) + deletes ridge from data structures it belongs to + frees up its memory + + notes: + in merge.c, caller sets vertex->delridge for each vertex + ridges also freed in qh_freeqhull +*/ +void qh_delridge(ridgeT *ridge) { + void **freelistp; /* used !qh_NOmem */ + + qh_setdel(ridge->top->ridges, ridge); + qh_setdel(ridge->bottom->ridges, ridge); + qh_setfree(&(ridge->vertices)); + qh_memfree_(ridge, sizeof(ridgeT), freelistp); +} /* delridge */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="delvertex">-</a> + + qh_delvertex( vertex ) + deletes a vertex and frees its memory + + notes: + assumes vertex->adjacencies have been updated if needed + unlinks from vertex_list +*/ +void qh_delvertex (vertexT *vertex) { + + if (vertex == qh tracevertex) + qh tracevertex= NULL; + qh_removevertex (vertex); + qh_setfree (&vertex->neighbors); + qh_memfree(vertex, sizeof(vertexT)); +} /* delvertex */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="facet3vertex">-</a> + + qh_facet3vertex( ) + return temporary set of 3-d vertices in qh_ORIENTclock order + + design: + if simplicial facet + build set from facet->vertices with facet->toporient + else + for each ridge in order + build set from ridge's vertices +*/ +setT *qh_facet3vertex (facetT *facet) { + ridgeT *ridge, *firstridge; + vertexT *vertex; + int cntvertices, cntprojected=0; + setT *vertices; + + cntvertices= qh_setsize(facet->vertices); + vertices= qh_settemp (cntvertices); + if (facet->simplicial) { + if (cntvertices != 3) { + fprintf (qh ferr, "qhull internal error (qh_facet3vertex): only %d vertices for simplicial facet f%d\n", + cntvertices, facet->id); + qh_errexit(qh_ERRqhull, facet, NULL); + } + qh_setappend (&vertices, SETfirst_(facet->vertices)); + if (facet->toporient ^ qh_ORIENTclock) + qh_setappend (&vertices, SETsecond_(facet->vertices)); + else + qh_setaddnth (&vertices, 0, SETsecond_(facet->vertices)); + qh_setappend (&vertices, SETelem_(facet->vertices, 2)); + }else { + ridge= firstridge= SETfirstt_(facet->ridges, ridgeT); /* no infinite */ + while ((ridge= qh_nextridge3d (ridge, facet, &vertex))) { + qh_setappend (&vertices, vertex); + if (++cntprojected > cntvertices || ridge == firstridge) + break; + } + if (!ridge || cntprojected != cntvertices) { + fprintf (qh ferr, "qhull internal error (qh_facet3vertex): ridges for facet %d don't match up. got at least %d\n", + facet->id, cntprojected); + qh_errexit(qh_ERRqhull, facet, ridge); + } + } + return vertices; +} /* facet3vertex */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="findbestfacet">-</a> + + qh_findbestfacet( point, bestoutside, bestdist, isoutside ) + find facet that is furthest below a point + + for Delaunay triangulations, + Use qh_setdelaunay() to lift point to paraboloid and scale by 'Qbb' if needed + Do not use options 'Qbk', 'QBk', or 'QbB' since they scale the coordinates. + + returns: + if bestoutside is set (e.g., qh_ALL) + returns best facet that is not upperdelaunay + if Delaunay and inside, point is outside circumsphere of bestfacet + else + returns first facet below point + if point is inside, returns nearest, !upperdelaunay facet + distance to facet + isoutside set if outside of facet + + notes: + For tricoplanar facets, this finds one of the tricoplanar facets closest + to the point. For Delaunay triangulations, the point may be inside a + different tricoplanar facet. See <a href="../html/qh-in.htm#findfacet">locate a facet with qh_findbestfacet()</a> + + If inside, qh_findbestfacet performs an exhaustive search + this may be too conservative. Sometimes it is clearly required. + + qh_findbestfacet is not used by qhull. + uses qh.visit_id and qh.coplanarset + + see: + <a href="geom.c#findbest">qh_findbest</a> +*/ +facetT *qh_findbestfacet (pointT *point, boolT bestoutside, + realT *bestdist, boolT *isoutside) { + facetT *bestfacet= NULL; + int numpart, totpart= 0; + + bestfacet= qh_findbest (point, qh facet_list, + bestoutside, !qh_ISnewfacets, bestoutside /* qh_NOupper */, + bestdist, isoutside, &totpart); + if (*bestdist < -qh DISTround) { + bestfacet= qh_findfacet_all (point, bestdist, isoutside, &numpart); + totpart += numpart; + if ((isoutside && bestoutside) + || (!isoutside && bestfacet->upperdelaunay)) { + bestfacet= qh_findbest (point, bestfacet, + bestoutside, False, bestoutside, + bestdist, isoutside, &totpart); + totpart += numpart; + } + } + trace3((qh ferr, "qh_findbestfacet: f%d dist %2.2g isoutside %d totpart %d\n", + bestfacet->id, *bestdist, *isoutside, totpart)); + return bestfacet; +} /* findbestfacet */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="findbestlower">-</a> + + qh_findbestlower( facet, point, bestdist, numpart ) + returns best non-upper, non-flipped neighbor of facet for point + if needed, searches vertex neighbors + + returns: + returns bestdist and updates numpart + + notes: + if Delaunay and inside, point is outside of circumsphere of bestfacet + called by qh_findbest() for points above an upperdelaunay facet + +*/ +facetT *qh_findbestlower (facetT *upperfacet, pointT *point, realT *bestdistp, int *numpart) { + facetT *neighbor, **neighborp, *bestfacet= NULL; + realT bestdist= -REALmax/2 /* avoid underflow */; + realT dist; + vertexT *vertex; + + zinc_(Zbestlower); + FOREACHneighbor_(upperfacet) { + if (neighbor->upperdelaunay || neighbor->flipped) + continue; + (*numpart)++; + qh_distplane (point, neighbor, &dist); + if (dist > bestdist) { + bestfacet= neighbor; + bestdist= dist; + } + } + if (!bestfacet) { + zinc_(Zbestlowerv); + /* rarely called, numpart does not count nearvertex computations */ + vertex= qh_nearvertex (upperfacet, point, &dist); + qh_vertexneighbors(); + FOREACHneighbor_(vertex) { + if (neighbor->upperdelaunay || neighbor->flipped) + continue; + (*numpart)++; + qh_distplane (point, neighbor, &dist); + if (dist > bestdist) { + bestfacet= neighbor; + bestdist= dist; + } + } + } + if (!bestfacet) { + fprintf(qh ferr, "\n\ +qh_findbestlower: all neighbors of facet %d are flipped or upper Delaunay.\n\ +Please report this error to qhull_bug@qhull.org with the input and all of the output.\n", + upperfacet->id); + qh_errexit (qh_ERRqhull, upperfacet, NULL); + } + *bestdistp= bestdist; + trace3((qh ferr, "qh_findbestlower: f%d dist %2.2g for f%d p%d\n", + bestfacet->id, bestdist, upperfacet->id, qh_pointid(point))); + return bestfacet; +} /* findbestlower */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="findfacet_all">-</a> + + qh_findfacet_all( point, bestdist, isoutside, numpart ) + exhaustive search for facet below a point + + for Delaunay triangulations, + Use qh_setdelaunay() to lift point to paraboloid and scale by 'Qbb' if needed + Do not use options 'Qbk', 'QBk', or 'QbB' since they scale the coordinates. + + returns: + returns first facet below point + if point is inside, + returns nearest facet + distance to facet + isoutside if point is outside of the hull + number of distance tests +*/ +facetT *qh_findfacet_all (pointT *point, realT *bestdist, boolT *isoutside, + int *numpart) { + facetT *bestfacet= NULL, *facet; + realT dist; + int totpart= 0; + + *bestdist= REALmin; + *isoutside= False; + FORALLfacets { + if (facet->flipped || !facet->normal) + continue; + totpart++; + qh_distplane (point, facet, &dist); + if (dist > *bestdist) { + *bestdist= dist; + bestfacet= facet; + if (dist > qh MINoutside) { + *isoutside= True; + break; + } + } + } + *numpart= totpart; + trace3((qh ferr, "qh_findfacet_all: f%d dist %2.2g isoutside %d totpart %d\n", + getid_(bestfacet), *bestdist, *isoutside, totpart)); + return bestfacet; +} /* findfacet_all */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="findgood">-</a> + + qh_findgood( facetlist, goodhorizon ) + identify good facets for qh.PRINTgood + if qh.GOODvertex>0 + facet includes point as vertex + if !match, returns goodhorizon + inactive if qh.MERGING + if qh.GOODpoint + facet is visible or coplanar (>0) or not visible (<0) + if qh.GOODthreshold + facet->normal matches threshold + if !goodhorizon and !match, + selects facet with closest angle + sets GOODclosest + + returns: + number of new, good facets found + determines facet->good + may update qh.GOODclosest + + notes: + qh_findgood_all further reduces the good region + + design: + count good facets + mark good facets for qh.GOODpoint + mark good facets for qh.GOODthreshold + if necessary + update qh.GOODclosest +*/ +int qh_findgood (facetT *facetlist, int goodhorizon) { + facetT *facet, *bestfacet= NULL; + realT angle, bestangle= REALmax, dist; + int numgood=0; + + FORALLfacet_(facetlist) { + if (facet->good) + numgood++; + } + if (qh GOODvertex>0 && !qh MERGING) { + FORALLfacet_(facetlist) { + if (!qh_isvertex (qh GOODvertexp, facet->vertices)) { + facet->good= False; + numgood--; + } + } + } + if (qh GOODpoint && numgood) { + FORALLfacet_(facetlist) { + if (facet->good && facet->normal) { + zinc_(Zdistgood); + qh_distplane (qh GOODpointp, facet, &dist); + if ((qh GOODpoint > 0) ^ (dist > 0.0)) { + facet->good= False; + numgood--; + } + } + } + } + if (qh GOODthreshold && (numgood || goodhorizon || qh GOODclosest)) { + FORALLfacet_(facetlist) { + if (facet->good && facet->normal) { + if (!qh_inthresholds (facet->normal, &angle)) { + facet->good= False; + numgood--; + if (angle < bestangle) { + bestangle= angle; + bestfacet= facet; + } + } + } + } + if (!numgood && (!goodhorizon || qh GOODclosest)) { + if (qh GOODclosest) { + if (qh GOODclosest->visible) + qh GOODclosest= NULL; + else { + qh_inthresholds (qh GOODclosest->normal, &angle); + if (angle < bestangle) + bestfacet= qh GOODclosest; + } + } + if (bestfacet && bestfacet != qh GOODclosest) { + if (qh GOODclosest) + qh GOODclosest->good= False; + qh GOODclosest= bestfacet; + bestfacet->good= True; + numgood++; + trace2((qh ferr, "qh_findgood: f%d is closest (%2.2g) to thresholds\n", + bestfacet->id, bestangle)); + return numgood; + } + }else if (qh GOODclosest) { /* numgood > 0 */ + qh GOODclosest->good= False; + qh GOODclosest= NULL; + } + } + zadd_(Zgoodfacet, numgood); + trace2((qh ferr, "qh_findgood: found %d good facets with %d good horizon\n", + numgood, goodhorizon)); + if (!numgood && qh GOODvertex>0 && !qh MERGING) + return goodhorizon; + return numgood; +} /* findgood */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="findgood_all">-</a> + + qh_findgood_all( facetlist ) + apply other constraints for good facets (used by qh.PRINTgood) + if qh.GOODvertex + facet includes (>0) or doesn't include (<0) point as vertex + if last good facet and ONLYgood, prints warning and continues + if qh.SPLITthresholds + facet->normal matches threshold, or if none, the closest one + calls qh_findgood + nop if good not used + + returns: + clears facet->good if not good + sets qh.num_good + + notes: + this is like qh_findgood but more restrictive + + design: + uses qh_findgood to mark good facets + marks facets for qh.GOODvertex + marks facets for qh.SPLITthreholds +*/ +void qh_findgood_all (facetT *facetlist) { + facetT *facet, *bestfacet=NULL; + realT angle, bestangle= REALmax; + int numgood=0, startgood; + + if (!qh GOODvertex && !qh GOODthreshold && !qh GOODpoint + && !qh SPLITthresholds) + return; + if (!qh ONLYgood) + qh_findgood (qh facet_list, 0); + FORALLfacet_(facetlist) { + if (facet->good) + numgood++; + } + if (qh GOODvertex <0 || (qh GOODvertex > 0 && qh MERGING)) { + FORALLfacet_(facetlist) { + if (facet->good && ((qh GOODvertex > 0) ^ !!qh_isvertex (qh GOODvertexp, facet->vertices))) { + if (!--numgood) { + if (qh ONLYgood) { + fprintf (qh ferr, "qhull warning: good vertex p%d does not match last good facet f%d. Ignored.\n", + qh_pointid(qh GOODvertexp), facet->id); + return; + }else if (qh GOODvertex > 0) + fprintf (qh ferr, "qhull warning: point p%d is not a vertex ('QV%d').\n", + qh GOODvertex-1, qh GOODvertex-1); + else + fprintf (qh ferr, "qhull warning: point p%d is a vertex for every facet ('QV-%d').\n", + -qh GOODvertex - 1, -qh GOODvertex - 1); + } + facet->good= False; + } + } + } + startgood= numgood; + if (qh SPLITthresholds) { + FORALLfacet_(facetlist) { + if (facet->good) { + if (!qh_inthresholds (facet->normal, &angle)) { + facet->good= False; + numgood--; + if (angle < bestangle) { + bestangle= angle; + bestfacet= facet; + } + } + } + } + if (!numgood && bestfacet) { + bestfacet->good= True; + numgood++; + trace0((qh ferr, "qh_findgood_all: f%d is closest (%2.2g) to thresholds\n", + bestfacet->id, bestangle)); + return; + } + } + qh num_good= numgood; + trace0((qh ferr, "qh_findgood_all: %d good facets remain out of %d facets\n", + numgood, startgood)); +} /* findgood_all */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="furthestnext">-</a> + + qh_furthestnext() + set qh.facet_next to facet with furthest of all furthest points + searches all facets on qh.facet_list + + notes: + this may help avoid precision problems +*/ +void qh_furthestnext (void /* qh facet_list */) { + facetT *facet, *bestfacet= NULL; + realT dist, bestdist= -REALmax; + + FORALLfacets { + if (facet->outsideset) { +#if qh_COMPUTEfurthest + pointT *furthest; + furthest= (pointT*)qh_setlast (facet->outsideset); + zinc_(Zcomputefurthest); + qh_distplane (furthest, facet, &dist); +#else + dist= facet->furthestdist; +#endif + if (dist > bestdist) { + bestfacet= facet; + bestdist= dist; + } + } + } + if (bestfacet) { + qh_removefacet (bestfacet); + qh_prependfacet (bestfacet, &qh facet_next); + trace1((qh ferr, "qh_furthestnext: made f%d next facet (dist %.2g)\n", + bestfacet->id, bestdist)); + } +} /* furthestnext */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="furthestout">-</a> + + qh_furthestout( facet ) + make furthest outside point the last point of outsideset + + returns: + updates facet->outsideset + clears facet->notfurthest + sets facet->furthestdist + + design: + determine best point of outsideset + make it the last point of outsideset +*/ +void qh_furthestout (facetT *facet) { + pointT *point, **pointp, *bestpoint= NULL; + realT dist, bestdist= -REALmax; + + FOREACHpoint_(facet->outsideset) { + qh_distplane (point, facet, &dist); + zinc_(Zcomputefurthest); + if (dist > bestdist) { + bestpoint= point; + bestdist= dist; + } + } + if (bestpoint) { + qh_setdel (facet->outsideset, point); + qh_setappend (&facet->outsideset, point); +#if !qh_COMPUTEfurthest + facet->furthestdist= bestdist; +#endif + } + facet->notfurthest= False; + trace3((qh ferr, "qh_furthestout: p%d is furthest outside point of f%d\n", + qh_pointid (point), facet->id)); +} /* furthestout */ + + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="infiniteloop">-</a> + + qh_infiniteloop( facet ) + report infinite loop error due to facet +*/ +void qh_infiniteloop (facetT *facet) { + + fprintf (qh ferr, "qhull internal error (qh_infiniteloop): potential infinite loop detected\n"); + qh_errexit (qh_ERRqhull, facet, NULL); +} /* qh_infiniteloop */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="initbuild">-</a> + + qh_initbuild() + initialize hull and outside sets with point array + qh.FIRSTpoint/qh.NUMpoints is point array + if qh.GOODpoint + adds qh.GOODpoint to initial hull + + returns: + qh_facetlist with initial hull + points partioned into outside sets, coplanar sets, or inside + initializes qh.GOODpointp, qh.GOODvertexp, + + design: + initialize global variables used during qh_buildhull + determine precision constants and points with max/min coordinate values + if qh.SCALElast, scale last coordinate (for 'd') + build initial simplex + partition input points into facets of initial simplex + set up lists + if qh.ONLYgood + check consistency + add qh.GOODvertex if defined +*/ +void qh_initbuild( void) { + setT *maxpoints, *vertices; + facetT *facet; + int i, numpart; + realT dist; + boolT isoutside; + + qh furthest_id= -1; + qh lastreport= 0; + qh facet_id= qh vertex_id= qh ridge_id= 0; + qh visit_id= qh vertex_visit= 0; + qh maxoutdone= False; + + if (qh GOODpoint > 0) + qh GOODpointp= qh_point (qh GOODpoint-1); + else if (qh GOODpoint < 0) + qh GOODpointp= qh_point (-qh GOODpoint-1); + if (qh GOODvertex > 0) + qh GOODvertexp= qh_point (qh GOODvertex-1); + else if (qh GOODvertex < 0) + qh GOODvertexp= qh_point (-qh GOODvertex-1); + if ((qh GOODpoint + && (qh GOODpointp < qh first_point /* also catches !GOODpointp */ + || qh GOODpointp > qh_point (qh num_points-1))) + || (qh GOODvertex + && (qh GOODvertexp < qh first_point /* also catches !GOODvertexp */ + || qh GOODvertexp > qh_point (qh num_points-1)))) { + fprintf (qh ferr, "qhull input error: either QGn or QVn point is > p%d\n", + qh num_points-1); + qh_errexit (qh_ERRinput, NULL, NULL); + } + maxpoints= qh_maxmin(qh first_point, qh num_points, qh hull_dim); + if (qh SCALElast) + qh_scalelast (qh first_point, qh num_points, qh hull_dim, + qh MINlastcoord, qh MAXlastcoord, qh MAXwidth); + qh_detroundoff(); + if (qh DELAUNAY && qh upper_threshold[qh hull_dim-1] > REALmax/2 + && qh lower_threshold[qh hull_dim-1] < -REALmax/2) { + for (i= qh_PRINTEND; i--; ) { + if (qh PRINTout[i] == qh_PRINTgeom && qh DROPdim < 0 + && !qh GOODthreshold && !qh SPLITthresholds) + break; /* in this case, don't set upper_threshold */ + } + if (i < 0) { + if (qh UPPERdelaunay) { /* matches qh.upperdelaunay in qh_setfacetplane */ + qh lower_threshold[qh hull_dim-1]= qh ANGLEround * qh_ZEROdelaunay; + qh GOODthreshold= True; + }else { + qh upper_threshold[qh hull_dim-1]= -qh ANGLEround * qh_ZEROdelaunay; + if (!qh GOODthreshold) + qh SPLITthresholds= True; /* build upper-convex hull even if Qg */ + /* qh_initqhull_globals errors if Qg without Pdk/etc. */ + } + } + } + vertices= qh_initialvertices(qh hull_dim, maxpoints, qh first_point, qh num_points); + qh_initialhull (vertices); /* initial qh facet_list */ + qh_partitionall (vertices, qh first_point, qh num_points); + if (qh PRINToptions1st || qh TRACElevel || qh IStracing) { + if (qh TRACElevel || qh IStracing) + fprintf (qh ferr, "\nTrace level %d for %s | %s\n", + qh IStracing ? qh IStracing : qh TRACElevel, qh rbox_command, qh qhull_command); + fprintf (qh ferr, "Options selected for Qhull %s:\n%s\n", qh_version, qh qhull_options); + } + qh_resetlists (False, qh_RESETvisible /*qh visible_list newvertex_list newfacet_list */); + qh facet_next= qh facet_list; + qh_furthestnext (/* qh facet_list */); + if (qh PREmerge) { + qh cos_max= qh premerge_cos; + qh centrum_radius= qh premerge_centrum; + } + if (qh ONLYgood) { + if (qh GOODvertex > 0 && qh MERGING) { + fprintf (qh ferr, "qhull input error: 'Qg QVn' (only good vertex) does not work with merging.\nUse 'QJ' to joggle the input or 'Q0' to turn off merging.\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (!(qh GOODthreshold || qh GOODpoint + || (!qh MERGEexact && !qh PREmerge && qh GOODvertexp))) { + fprintf (qh ferr, "qhull input error: 'Qg' (ONLYgood) needs a good threshold ('Pd0D0'), a\n\ +good point (QGn or QG-n), or a good vertex with 'QJ' or 'Q0' (QVn).\n"); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (qh GOODvertex > 0 && !qh MERGING /* matches qh_partitionall */ + && !qh_isvertex (qh GOODvertexp, vertices)) { + facet= qh_findbestnew (qh GOODvertexp, qh facet_list, + &dist, !qh_ALL, &isoutside, &numpart); + zadd_(Zdistgood, numpart); + if (!isoutside) { + fprintf (qh ferr, "qhull input error: point for QV%d is inside initial simplex. It can not be made a vertex.\n", + qh_pointid(qh GOODvertexp)); + qh_errexit (qh_ERRinput, NULL, NULL); + } + if (!qh_addpoint (qh GOODvertexp, facet, False)) { + qh_settempfree(&vertices); + qh_settempfree(&maxpoints); + return; + } + } + qh_findgood (qh facet_list, 0); + } + qh_settempfree(&vertices); + qh_settempfree(&maxpoints); + trace1((qh ferr, "qh_initbuild: initial hull created and points partitioned\n")); +} /* initbuild */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="initialhull">-</a> + + qh_initialhull( vertices ) + constructs the initial hull as a DIM3 simplex of vertices + + design: + creates a simplex (initializes lists) + determines orientation of simplex + sets hyperplanes for facets + doubles checks orientation (in case of axis-parallel facets with Gaussian elimination) + checks for flipped facets and qh.NARROWhull + checks the result +*/ +void qh_initialhull(setT *vertices) { + facetT *facet, *firstfacet, *neighbor, **neighborp; + realT dist, angle, minangle= REALmax; +#ifndef qh_NOtrace + int k; +#endif + + qh_createsimplex(vertices); /* qh facet_list */ + qh_resetlists (False, qh_RESETvisible); + qh facet_next= qh facet_list; /* advance facet when processed */ + qh interior_point= qh_getcenter(vertices); + firstfacet= qh facet_list; + qh_setfacetplane(firstfacet); + zinc_(Znumvisibility); /* needs to be in printsummary */ + qh_distplane(qh interior_point, firstfacet, &dist); + if (dist > 0) { + FORALLfacets + facet->toporient ^= True; + } + FORALLfacets + qh_setfacetplane(facet); + FORALLfacets { + if (!qh_checkflipped (facet, NULL, qh_ALL)) {/* due to axis-parallel facet */ + trace1((qh ferr, "qh_initialhull: initial orientation incorrect. Correct all facets\n")); + facet->flipped= False; + FORALLfacets { + facet->toporient ^= True; + qh_orientoutside (facet); + } + break; + } + } + FORALLfacets { + if (!qh_checkflipped (facet, NULL, !qh_ALL)) { /* can happen with 'R0.1' */ + qh_precision ("initial facet is coplanar with interior point"); + fprintf (qh ferr, "qhull precision error: initial facet %d is coplanar with the interior point\n", + facet->id); + qh_errexit (qh_ERRsingular, facet, NULL); + } + FOREACHneighbor_(facet) { + angle= qh_getangle (facet->normal, neighbor->normal); + minimize_( minangle, angle); + } + } + if (minangle < qh_MAXnarrow && !qh NOnarrow) { + realT diff= 1.0 + minangle; + + qh NARROWhull= True; + qh_option ("_narrow-hull", NULL, &diff); + if (minangle < qh_WARNnarrow && !qh RERUN && qh PRINTprecision) + fprintf (qh ferr, "qhull precision warning: \n\ +The initial hull is narrow (cosine of min. angle is %.16f).\n\ +A coplanar point may lead to a wide facet. Options 'QbB' (scale to unit box)\n\ +or 'Qbb' (scale last coordinate) may remove this warning. Use 'Pp' to skip\n\ +this warning. See 'Limitations' in qh-impre.htm.\n", + -minangle); /* convert from angle between normals to angle between facets */ + } + zzval_(Zprocessed)= qh hull_dim+1; + qh_checkpolygon (qh facet_list); + qh_checkconvex(qh facet_list, qh_DATAfault); +#ifndef qh_NOtrace + if (qh IStracing >= 1) { + fprintf(qh ferr, "qh_initialhull: simplex constructed, interior point:"); + for (k=0; k < qh hull_dim; k++) + fprintf (qh ferr, " %6.4g", qh interior_point[k]); + fprintf (qh ferr, "\n"); + } +#endif +} /* initialhull */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="initialvertices">-</a> + + qh_initialvertices( dim, maxpoints, points, numpoints ) + determines a non-singular set of initial vertices + maxpoints may include duplicate points + + returns: + temporary set of dim+1 vertices in descending order by vertex id + if qh.RANDOMoutside && !qh.ALLpoints + picks random points + if dim >= qh_INITIALmax, + uses min/max x and max points with non-zero determinants + + notes: + unless qh.ALLpoints, + uses maxpoints as long as determinate is non-zero +*/ +setT *qh_initialvertices(int dim, setT *maxpoints, pointT *points, int numpoints) { + pointT *point, **pointp; + setT *vertices, *simplex, *tested; + realT randr; + int index, point_i, point_n, k; + boolT nearzero= False; + + vertices= qh_settemp (dim + 1); + simplex= qh_settemp (dim+1); + if (qh ALLpoints) + qh_maxsimplex (dim, NULL, points, numpoints, &simplex); + else if (qh RANDOMoutside) { + while (qh_setsize (simplex) != dim+1) { + randr= qh_RANDOMint; + randr= randr/(qh_RANDOMmax+1); + index= (int)floor(qh num_points * randr); + while (qh_setin (simplex, qh_point (index))) { + index++; /* in case qh_RANDOMint always returns the same value */ + index= index < qh num_points ? index : 0; + } + qh_setappend (&simplex, qh_point (index)); + } + }else if (qh hull_dim >= qh_INITIALmax) { + tested= qh_settemp (dim+1); + qh_setappend (&simplex, SETfirst_(maxpoints)); /* max and min X coord */ + qh_setappend (&simplex, SETsecond_(maxpoints)); + qh_maxsimplex (fmin_(qh_INITIALsearch, dim), maxpoints, points, numpoints, &simplex); + k= qh_setsize (simplex); + FOREACHpoint_i_(maxpoints) { + if (point_i & 0x1) { /* first pick up max. coord. points */ + if (!qh_setin (simplex, point) && !qh_setin (tested, point)){ + qh_detsimplex(point, simplex, k, &nearzero); + if (nearzero) + qh_setappend (&tested, point); + else { + qh_setappend (&simplex, point); + if (++k == dim) /* use search for last point */ + break; + } + } + } + } + while (k != dim && (point= (pointT*)qh_setdellast (maxpoints))) { + if (!qh_setin (simplex, point) && !qh_setin (tested, point)){ + qh_detsimplex (point, simplex, k, &nearzero); + if (nearzero) + qh_setappend (&tested, point); + else { + qh_setappend (&simplex, point); + k++; + } + } + } + index= 0; + while (k != dim && (point= qh_point (index++))) { + if (!qh_setin (simplex, point) && !qh_setin (tested, point)){ + qh_detsimplex (point, simplex, k, &nearzero); + if (!nearzero){ + qh_setappend (&simplex, point); + k++; + } + } + } + qh_settempfree (&tested); + qh_maxsimplex (dim, maxpoints, points, numpoints, &simplex); + }else + qh_maxsimplex (dim, maxpoints, points, numpoints, &simplex); + FOREACHpoint_(simplex) + qh_setaddnth (&vertices, 0, qh_newvertex(point)); /* descending order */ + qh_settempfree (&simplex); + return vertices; +} /* initialvertices */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="isvertex">-</a> + + qh_isvertex( ) + returns vertex if point is in vertex set, else returns NULL + + notes: + for qh.GOODvertex +*/ +vertexT *qh_isvertex (pointT *point, setT *vertices) { + vertexT *vertex, **vertexp; + + FOREACHvertex_(vertices) { + if (vertex->point == point) + return vertex; + } + return NULL; +} /* isvertex */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="makenewfacets">-</a> + + qh_makenewfacets( point ) + make new facets from point and qh.visible_list + + returns: + qh.newfacet_list= list of new facets with hyperplanes and ->newfacet + qh.newvertex_list= list of vertices in new facets with ->newlist set + + if (qh.ONLYgood) + newfacets reference horizon facets, but not vice versa + ridges reference non-simplicial horizon ridges, but not vice versa + does not change existing facets + else + sets qh.NEWfacets + new facets attached to horizon facets and ridges + for visible facets, + visible->r.replace is corresponding new facet + + see also: + qh_makenewplanes() -- make hyperplanes for facets + qh_attachnewfacets() -- attachnewfacets if not done here (qh ONLYgood) + qh_matchnewfacets() -- match up neighbors + qh_updatevertices() -- update vertex neighbors and delvertices + qh_deletevisible() -- delete visible facets + qh_checkpolygon() --check the result + qh_triangulate() -- triangulate a non-simplicial facet + + design: + for each visible facet + make new facets to its horizon facets + update its f.replace + clear its neighbor set +*/ +vertexT *qh_makenewfacets (pointT *point /*visible_list*/) { + facetT *visible, *newfacet= NULL, *newfacet2= NULL, *neighbor, **neighborp; + vertexT *apex; + int numnew=0; + + qh newfacet_list= qh facet_tail; + qh newvertex_list= qh vertex_tail; + apex= qh_newvertex(point); + qh_appendvertex (apex); + qh visit_id++; + if (!qh ONLYgood) + qh NEWfacets= True; + FORALLvisible_facets { + FOREACHneighbor_(visible) + neighbor->seen= False; + if (visible->ridges) { + visible->visitid= qh visit_id; + newfacet2= qh_makenew_nonsimplicial (visible, apex, &numnew); + } + if (visible->simplicial) + newfacet= qh_makenew_simplicial (visible, apex, &numnew); + if (!qh ONLYgood) { + if (newfacet2) /* newfacet is null if all ridges defined */ + newfacet= newfacet2; + if (newfacet) + visible->f.replace= newfacet; + else + zinc_(Zinsidevisible); + SETfirst_(visible->neighbors)= NULL; + } + } + trace1((qh ferr, "qh_makenewfacets: created %d new facets from point p%d to horizon\n", + numnew, qh_pointid(point))); + if (qh IStracing >= 4) + qh_printfacetlist (qh newfacet_list, NULL, qh_ALL); + return apex; +} /* makenewfacets */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="matchduplicates">-</a> + + qh_matchduplicates( atfacet, atskip, hashsize, hashcount ) + match duplicate ridges in qh.hash_table for atfacet/atskip + duplicates marked with ->dupridge and qh_DUPLICATEridge + + returns: + picks match with worst merge (min distance apart) + updates hashcount + + see also: + qh_matchneighbor + + notes: + + design: + compute hash value for atfacet and atskip + repeat twice -- once to make best matches, once to match the rest + for each possible facet in qh.hash_table + if it is a matching facet and pass 2 + make match + unless tricoplanar, mark match for merging (qh_MERGEridge) + [e.g., tricoplanar RBOX s 1000 t993602376 | QHULL C-1e-3 d Qbb FA Qt] + if it is a matching facet and pass 1 + test if this is a better match + if pass 1, + make best match (it will not be merged) +*/ +#ifndef qh_NOmerge +void qh_matchduplicates (facetT *atfacet, int atskip, int hashsize, int *hashcount) { + boolT same, ismatch; + int hash, scan; + facetT *facet, *newfacet, *maxmatch= NULL, *maxmatch2= NULL, *nextfacet; + int skip, newskip, nextskip= 0, maxskip= 0, maxskip2= 0, makematch; + realT maxdist= -REALmax, mindist, dist2, low, high; + + hash= (int)qh_gethash (hashsize, atfacet->vertices, qh hull_dim, 1, + SETelem_(atfacet->vertices, atskip)); + trace2((qh ferr, "qh_matchduplicates: find duplicate matches for f%d skip %d hash %d hashcount %d\n", + atfacet->id, atskip, hash, *hashcount)); + for (makematch= 0; makematch < 2; makematch++) { + qh visit_id++; + for (newfacet= atfacet, newskip= atskip; newfacet; newfacet= nextfacet, newskip= nextskip) { + zinc_(Zhashlookup); + nextfacet= NULL; + newfacet->visitid= qh visit_id; + for (scan= hash; (facet= SETelemt_(qh hash_table, scan, facetT)); + scan= (++scan >= hashsize ? 0 : scan)) { + if (!facet->dupridge || facet->visitid == qh visit_id) + continue; + zinc_(Zhashtests); + if (qh_matchvertices (1, newfacet->vertices, newskip, facet->vertices, &skip, &same)) { + ismatch= (same == (newfacet->toporient ^ facet->toporient)); + if (SETelemt_(facet->neighbors, skip, facetT) != qh_DUPLICATEridge) { + if (!makematch) { + fprintf (qh ferr, "qhull internal error (qh_matchduplicates): missing dupridge at f%d skip %d for new f%d skip %d hash %d\n", + facet->id, skip, newfacet->id, newskip, hash); + qh_errexit2 (qh_ERRqhull, facet, newfacet); + } + }else if (ismatch && makematch) { + if (SETelemt_(newfacet->neighbors, newskip, facetT) == qh_DUPLICATEridge) { + SETelem_(facet->neighbors, skip)= newfacet; + if (newfacet->tricoplanar) + SETelem_(newfacet->neighbors, newskip)= facet; + else + SETelem_(newfacet->neighbors, newskip)= qh_MERGEridge; + *hashcount -= 2; /* removed two unmatched facets */ + trace4((qh ferr, "qh_matchduplicates: duplicate f%d skip %d matched with new f%d skip %d merge\n", + facet->id, skip, newfacet->id, newskip)); + } + }else if (ismatch) { + mindist= qh_getdistance (facet, newfacet, &low, &high); + dist2= qh_getdistance (newfacet, facet, &low, &high); + minimize_(mindist, dist2); + if (mindist > maxdist) { + maxdist= mindist; + maxmatch= facet; + maxskip= skip; + maxmatch2= newfacet; + maxskip2= newskip; + } + trace3((qh ferr, "qh_matchduplicates: duplicate f%d skip %d new f%d skip %d at dist %2.2g, max is now f%d f%d\n", + facet->id, skip, newfacet->id, newskip, mindist, + maxmatch->id, maxmatch2->id)); + }else { /* !ismatch */ + nextfacet= facet; + nextskip= skip; + } + } + if (makematch && !facet + && SETelemt_(facet->neighbors, skip, facetT) == qh_DUPLICATEridge) { + fprintf (qh ferr, "qhull internal error (qh_matchduplicates): no MERGEridge match for duplicate f%d skip %d at hash %d\n", + newfacet->id, newskip, hash); + qh_errexit (qh_ERRqhull, newfacet, NULL); + } + } + } /* end of for each new facet at hash */ + if (!makematch) { + if (!maxmatch) { + fprintf (qh ferr, "qhull internal error (qh_matchduplicates): no maximum match at duplicate f%d skip %d at hash %d\n", + atfacet->id, atskip, hash); + qh_errexit (qh_ERRqhull, atfacet, NULL); + } + SETelem_(maxmatch->neighbors, maxskip)= maxmatch2; + SETelem_(maxmatch2->neighbors, maxskip2)= maxmatch; + *hashcount -= 2; /* removed two unmatched facets */ + zzinc_(Zmultiridge); + trace0((qh ferr, "qh_matchduplicates: duplicate f%d skip %d matched with new f%d skip %d keep\n", + maxmatch->id, maxskip, maxmatch2->id, maxskip2)); + qh_precision ("ridge with multiple neighbors"); + if (qh IStracing >= 4) + qh_errprint ("DUPLICATED/MATCH", maxmatch, maxmatch2, NULL, NULL); + } + } +} /* matchduplicates */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="nearcoplanar">-</a> + + qh_nearcoplanar() + for all facets, remove near-inside points from facet->coplanarset</li> + coplanar points defined by innerplane from qh_outerinner() + + returns: + if qh KEEPcoplanar && !qh KEEPinside + facet->coplanarset only contains coplanar points + if qh.JOGGLEmax + drops inner plane by another qh.JOGGLEmax diagonal since a + vertex could shift out while a coplanar point shifts in + + notes: + used for qh.PREmerge and qh.JOGGLEmax + must agree with computation of qh.NEARcoplanar in qh_detroundoff() + design: + if not keeping coplanar or inside points + free all coplanar sets + else if not keeping both coplanar and inside points + remove !coplanar or !inside points from coplanar sets +*/ +void qh_nearcoplanar ( void /* qh.facet_list */) { + facetT *facet; + pointT *point, **pointp; + int numpart; + realT dist, innerplane; + + if (!qh KEEPcoplanar && !qh KEEPinside) { + FORALLfacets { + if (facet->coplanarset) + qh_setfree( &facet->coplanarset); + } + }else if (!qh KEEPcoplanar || !qh KEEPinside) { + qh_outerinner (NULL, NULL, &innerplane); + if (qh JOGGLEmax < REALmax/2) + innerplane -= qh JOGGLEmax * sqrt (qh hull_dim); + numpart= 0; + FORALLfacets { + if (facet->coplanarset) { + FOREACHpoint_(facet->coplanarset) { + numpart++; + qh_distplane (point, facet, &dist); + if (dist < innerplane) { + if (!qh KEEPinside) + SETref_(point)= NULL; + }else if (!qh KEEPcoplanar) + SETref_(point)= NULL; + } + qh_setcompact (facet->coplanarset); + } + } + zzadd_(Zcheckpart, numpart); + } +} /* nearcoplanar */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="nearvertex">-</a> + + qh_nearvertex( facet, point, bestdist ) + return nearest vertex in facet to point + + returns: + vertex and its distance + + notes: + if qh.DELAUNAY + distance is measured in the input set + searches neighboring tricoplanar facets (requires vertexneighbors) + Slow implementation. Recomputes vertex set for each point. + The vertex set could be stored in the qh.keepcentrum facet. +*/ +vertexT *qh_nearvertex (facetT *facet, pointT *point, realT *bestdistp) { + realT bestdist= REALmax, dist; + vertexT *bestvertex= NULL, *vertex, **vertexp, *apex; + coordT *center; + facetT *neighbor, **neighborp; + setT *vertices; + int dim= qh hull_dim; + + if (qh DELAUNAY) + dim--; + if (facet->tricoplanar) { + if (!qh VERTEXneighbors || !facet->center) { + fprintf(qh ferr, "qhull internal error (qh_nearvertex): qh.VERTEXneighbors and facet->center required for tricoplanar facets\n"); + qh_errexit(qh_ERRqhull, facet, NULL); + } + vertices= qh_settemp (qh TEMPsize); + apex= SETfirst_(facet->vertices); + center= facet->center; + FOREACHneighbor_(apex) { + if (neighbor->center == center) { + FOREACHvertex_(neighbor->vertices) + qh_setappend(&vertices, vertex); + } + } + }else + vertices= facet->vertices; + FOREACHvertex_(vertices) { + dist= qh_pointdist (vertex->point, point, -dim); + if (dist < bestdist) { + bestdist= dist; + bestvertex= vertex; + } + } + if (facet->tricoplanar) + qh_settempfree (&vertices); + *bestdistp= sqrt (bestdist); + trace3((qh ferr, "qh_nearvertex: v%d dist %2.2g for f%d p%d\n", + bestvertex->id, *bestdistp, facet->id, qh_pointid(point))); + return bestvertex; +} /* nearvertex */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="newhashtable">-</a> + + qh_newhashtable( newsize ) + returns size of qh.hash_table of at least newsize slots + + notes: + assumes qh.hash_table is NULL + qh_HASHfactor determines the number of extra slots + size is not divisible by 2, 3, or 5 +*/ +int qh_newhashtable(int newsize) { + int size; + + size= ((newsize+1)*qh_HASHfactor) | 0x1; /* odd number */ + while (True) { + if ((size%3) && (size%5)) + break; + size += 2; + /* loop terminates because there is an infinite number of primes */ + } + qh hash_table= qh_setnew (size); + qh_setzero (qh hash_table, 0, size); + return size; +} /* newhashtable */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="newvertex">-</a> + + qh_newvertex( point ) + returns a new vertex for point +*/ +vertexT *qh_newvertex(pointT *point) { + vertexT *vertex; + + zinc_(Ztotvertices); + vertex= (vertexT *)qh_memalloc(sizeof(vertexT)); + memset ((char *) vertex, 0, sizeof (vertexT)); + if (qh vertex_id == 0xFFFFFF) { + fprintf(qh ferr, "qhull input error: more than %d vertices. ID field overflows and two vertices\n\ +may have the same identifier. Vertices not sorted correctly.\n", 0xFFFFFF); + qh_errexit(qh_ERRinput, NULL, NULL); + } + if (qh vertex_id == qh tracevertex_id) + qh tracevertex= vertex; + vertex->id= qh vertex_id++; + vertex->point= point; + trace4((qh ferr, "qh_newvertex: vertex p%d (v%d) created\n", qh_pointid(vertex->point), + vertex->id)); + return (vertex); +} /* newvertex */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="nextridge3d">-</a> + + qh_nextridge3d( atridge, facet, vertex ) + return next ridge and vertex for a 3d facet + + notes: + in qh_ORIENTclock order + this is a O(n^2) implementation to trace all ridges + be sure to stop on any 2nd visit + + design: + for each ridge + exit if it is the ridge after atridge +*/ +ridgeT *qh_nextridge3d (ridgeT *atridge, facetT *facet, vertexT **vertexp) { + vertexT *atvertex, *vertex, *othervertex; + ridgeT *ridge, **ridgep; + + if ((atridge->top == facet) ^ qh_ORIENTclock) + atvertex= SETsecondt_(atridge->vertices, vertexT); + else + atvertex= SETfirstt_(atridge->vertices, vertexT); + FOREACHridge_(facet->ridges) { + if (ridge == atridge) + continue; + if ((ridge->top == facet) ^ qh_ORIENTclock) { + othervertex= SETsecondt_(ridge->vertices, vertexT); + vertex= SETfirstt_(ridge->vertices, vertexT); + }else { + vertex= SETsecondt_(ridge->vertices, vertexT); + othervertex= SETfirstt_(ridge->vertices, vertexT); + } + if (vertex == atvertex) { + if (vertexp) + *vertexp= othervertex; + return ridge; + } + } + return NULL; +} /* nextridge3d */ +#else /* qh_NOmerge */ +void qh_matchduplicates (facetT *atfacet, int atskip, int hashsize, int *hashcount) { +} +ridgeT *qh_nextridge3d (ridgeT *atridge, facetT *facet, vertexT **vertexp) { + + return NULL; +} +#endif /* qh_NOmerge */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="outcoplanar">-</a> + + qh_outcoplanar() + move points from all facets' outsidesets to their coplanarsets + + notes: + for post-processing under qh.NARROWhull + + design: + for each facet + for each outside point for facet + partition point into coplanar set +*/ +void qh_outcoplanar (void /* facet_list */) { + pointT *point, **pointp; + facetT *facet; + realT dist; + + trace1((qh ferr, "qh_outcoplanar: move outsideset to coplanarset for qh NARROWhull\n")); + FORALLfacets { + FOREACHpoint_(facet->outsideset) { + qh num_outside--; + if (qh KEEPcoplanar || qh KEEPnearinside) { + qh_distplane (point, facet, &dist); + zinc_(Zpartition); + qh_partitioncoplanar (point, facet, &dist); + } + } + qh_setfree (&facet->outsideset); + } +} /* outcoplanar */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="point">-</a> + + qh_point( id ) + return point for a point id, or NULL if unknown + + alternative code: + return ((pointT *)((unsigned long)qh.first_point + + (unsigned long)((id)*qh.normal_size))); +*/ +pointT *qh_point (int id) { + + if (id < 0) + return NULL; + if (id < qh num_points) + return qh first_point + id * qh hull_dim; + id -= qh num_points; + if (id < qh_setsize (qh other_points)) + return SETelemt_(qh other_points, id, pointT); + return NULL; +} /* point */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="point_add">-</a> + + qh_point_add( set, point, elem ) + stores elem at set[point.id] + + returns: + access function for qh_pointfacet and qh_pointvertex + + notes: + checks point.id +*/ +void qh_point_add (setT *set, pointT *point, void *elem) { + int id, size; + + SETreturnsize_(set, size); + if ((id= qh_pointid(point)) < 0) + fprintf (qh ferr, "qhull internal warning (point_add): unknown point %p id %d\n", + point, id); + else if (id >= size) { + fprintf (qh ferr, "qhull internal errror (point_add): point p%d is out of bounds (%d)\n", + id, size); + qh_errexit (qh_ERRqhull, NULL, NULL); + }else + SETelem_(set, id)= elem; +} /* point_add */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="pointfacet">-</a> + + qh_pointfacet() + return temporary set of facet for each point + the set is indexed by point id + + notes: + vertices assigned to one of the facets + coplanarset assigned to the facet + outside set assigned to the facet + NULL if no facet for point (inside) + includes qh.GOODpointp + + access: + FOREACHfacet_i_(facets) { ... } + SETelem_(facets, i) + + design: + for each facet + add each vertex + add each coplanar point + add each outside point +*/ +setT *qh_pointfacet (void /*qh facet_list*/) { + int numpoints= qh num_points + qh_setsize (qh other_points); + setT *facets; + facetT *facet; + vertexT *vertex, **vertexp; + pointT *point, **pointp; + + facets= qh_settemp (numpoints); + qh_setzero (facets, 0, numpoints); + qh vertex_visit++; + FORALLfacets { + FOREACHvertex_(facet->vertices) { + if (vertex->visitid != qh vertex_visit) { + vertex->visitid= qh vertex_visit; + qh_point_add (facets, vertex->point, facet); + } + } + FOREACHpoint_(facet->coplanarset) + qh_point_add (facets, point, facet); + FOREACHpoint_(facet->outsideset) + qh_point_add (facets, point, facet); + } + return facets; +} /* pointfacet */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="pointvertex">-</a> + + qh_pointvertex( ) + return temporary set of vertices indexed by point id + entry is NULL if no vertex for a point + this will include qh.GOODpointp + + access: + FOREACHvertex_i_(vertices) { ... } + SETelem_(vertices, i) +*/ +setT *qh_pointvertex (void /*qh facet_list*/) { + int numpoints= qh num_points + qh_setsize (qh other_points); + setT *vertices; + vertexT *vertex; + + vertices= qh_settemp (numpoints); + qh_setzero (vertices, 0, numpoints); + FORALLvertices + qh_point_add (vertices, vertex->point, vertex); + return vertices; +} /* pointvertex */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="prependfacet">-</a> + + qh_prependfacet( facet, facetlist ) + prepend facet to the start of a facetlist + + returns: + increments qh.numfacets + updates facetlist, qh.facet_list, facet_next + + notes: + be careful of prepending since it can lose a pointer. + e.g., can lose _next by deleting and then prepending before _next +*/ +void qh_prependfacet(facetT *facet, facetT **facetlist) { + facetT *prevfacet, *list; + + + trace4((qh ferr, "qh_prependfacet: prepend f%d before f%d\n", + facet->id, getid_(*facetlist))); + if (!*facetlist) + (*facetlist)= qh facet_tail; + list= *facetlist; + prevfacet= list->previous; + facet->previous= prevfacet; + if (prevfacet) + prevfacet->next= facet; + list->previous= facet; + facet->next= *facetlist; + if (qh facet_list == list) /* this may change *facetlist */ + qh facet_list= facet; + if (qh facet_next == list) + qh facet_next= facet; + *facetlist= facet; + qh num_facets++; +} /* prependfacet */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="printhashtable">-</a> + + qh_printhashtable( fp ) + print hash table to fp + + notes: + not in I/O to avoid bringing io.c in + + design: + for each hash entry + if defined + if unmatched or will merge (NULL, qh_MERGEridge, qh_DUPLICATEridge) + print entry and neighbors +*/ +void qh_printhashtable(FILE *fp) { + facetT *facet, *neighbor; + int id, facet_i, facet_n, neighbor_i= 0, neighbor_n= 0; + vertexT *vertex, **vertexp; + + FOREACHfacet_i_(qh hash_table) { + if (facet) { + FOREACHneighbor_i_(facet) { + if (!neighbor || neighbor == qh_MERGEridge || neighbor == qh_DUPLICATEridge) + break; + } + if (neighbor_i == neighbor_n) + continue; + fprintf (fp, "hash %d f%d ", facet_i, facet->id); + FOREACHvertex_(facet->vertices) + fprintf (fp, "v%d ", vertex->id); + fprintf (fp, "\n neighbors:"); + FOREACHneighbor_i_(facet) { + if (neighbor == qh_MERGEridge) + id= -3; + else if (neighbor == qh_DUPLICATEridge) + id= -2; + else + id= getid_(neighbor); + fprintf (fp, " %d", id); + } + fprintf (fp, "\n"); + } + } +} /* printhashtable */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="printlists">-</a> + + qh_printlists( fp ) + print out facet and vertex list for debugging (without 'f/v' tags) +*/ +void qh_printlists (void) { + facetT *facet; + vertexT *vertex; + int count= 0; + + fprintf (qh ferr, "qh_printlists: facets:"); + FORALLfacets { + if (++count % 100 == 0) + fprintf (qh ferr, "\n "); + fprintf (qh ferr, " %d", facet->id); + } + fprintf (qh ferr, "\n new facets %d visible facets %d next facet for qh_addpoint %d\n vertices (new %d):", + getid_(qh newfacet_list), getid_(qh visible_list), getid_(qh facet_next), + getid_(qh newvertex_list)); + count = 0; + FORALLvertices { + if (++count % 100 == 0) + fprintf (qh ferr, "\n "); + fprintf (qh ferr, " %d", vertex->id); + } + fprintf (qh ferr, "\n"); +} /* printlists */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="resetlists">-</a> + + qh_resetlists( stats, qh_RESETvisible ) + reset newvertex_list, newfacet_list, visible_list + if stats, + maintains statistics + + returns: + visible_list is empty if qh_deletevisible was called +*/ +void qh_resetlists (boolT stats, boolT resetVisible /*qh newvertex_list newfacet_list visible_list*/) { + vertexT *vertex; + facetT *newfacet, *visible; + int totnew=0, totver=0; + + if (stats) { + FORALLvertex_(qh newvertex_list) + totver++; + FORALLnew_facets + totnew++; + zadd_(Zvisvertextot, totver); + zmax_(Zvisvertexmax, totver); + zadd_(Znewfacettot, totnew); + zmax_(Znewfacetmax, totnew); + } + FORALLvertex_(qh newvertex_list) + vertex->newlist= False; + qh newvertex_list= NULL; + FORALLnew_facets + newfacet->newfacet= False; + qh newfacet_list= NULL; + if (resetVisible) { + FORALLvisible_facets { + visible->f.replace= NULL; + visible->visible= False; + } + qh num_visible= 0; + } + qh visible_list= NULL; /* may still have visible facets via qh_triangulate */ + qh NEWfacets= False; +} /* resetlists */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="setvoronoi_all">-</a> + + qh_setvoronoi_all() + compute Voronoi centers for all facets + includes upperDelaunay facets if qh.UPPERdelaunay ('Qu') + + returns: + facet->center is the Voronoi center + + notes: + this is unused/untested code + please email bradb@shore.net if this works ok for you + + use: + FORALLvertices {...} to locate the vertex for a point. + FOREACHneighbor_(vertex) {...} to visit the Voronoi centers for a Voronoi cell. +*/ +void qh_setvoronoi_all (void) { + facetT *facet; + + qh_clearcenters (qh_ASvoronoi); + qh_vertexneighbors(); + + FORALLfacets { + if (!facet->normal || !facet->upperdelaunay || qh UPPERdelaunay) { + if (!facet->center) + facet->center= qh_facetcenter (facet->vertices); + } + } +} /* setvoronoi_all */ + +#ifndef qh_NOmerge + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="triangulate">-</a> + + qh_triangulate() + triangulate non-simplicial facets on qh.facet_list, + if qh.CENTERtype=qh_ASvoronoi, sets Voronoi centers of non-simplicial facets + + returns: + all facets simplicial + each tricoplanar facet has ->f.triowner == owner of ->center,normal,etc. + + notes: + call after qh_check_output since may switch to Voronoi centers + Output may overwrite ->f.triowner with ->f.area +*/ +void qh_triangulate (void /*qh facet_list*/) { + facetT *facet, *nextfacet, *owner; + int onlygood= qh ONLYgood; + facetT *neighbor, *visible= NULL, *facet1, *facet2, *new_facet_list= NULL; + facetT *orig_neighbor= NULL, *otherfacet; + vertexT *new_vertex_list= NULL; + mergeT *merge; + mergeType mergetype; + int neighbor_i, neighbor_n; + + trace1((qh ferr, "qh_triangulate: triangulate non-simplicial facets\n")); + if (qh hull_dim == 2) + return; + if (qh VORONOI) { /* otherwise lose Voronoi centers [could rebuild vertex set from tricoplanar] */ + qh_clearcenters (qh_ASvoronoi); + qh_vertexneighbors(); + } + qh ONLYgood= False; /* for makenew_nonsimplicial */ + qh visit_id++; + qh NEWfacets= True; + qh degen_mergeset= qh_settemp (qh TEMPsize); + qh newvertex_list= qh vertex_tail; + for (facet= qh facet_list; facet && facet->next; facet= nextfacet) { /* non-simplicial facets moved to end */ + nextfacet= facet->next; + if (facet->visible || facet->simplicial) + continue; + /* triangulate all non-simplicial facets, otherwise merging does not work, e.g., RBOX c P-0.1 P+0.1 P+0.1 D3 | QHULL d Qt Tv */ + if (!new_facet_list) + new_facet_list= facet; /* will be moved to end */ + qh_triangulate_facet (facet, &new_vertex_list); + } + trace2((qh ferr, "qh_triangulate: delete null facets from f%d -- apex same as second vertex\n", getid_(new_facet_list))); + for (facet= new_facet_list; facet && facet->next; facet= nextfacet) { /* null facets moved to end */ + nextfacet= facet->next; + if (facet->visible) + continue; + if (facet->ridges) { + if (qh_setsize(facet->ridges) > 0) { + fprintf( qh ferr, "qhull error (qh_triangulate): ridges still defined for f%d\n", facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + qh_setfree (&facet->ridges); + } + if (SETfirst_(facet->vertices) == SETsecond_(facet->vertices)) { + zinc_(Ztrinull); + qh_triangulate_null (facet); + } + } + trace2((qh ferr, "qh_triangulate: delete %d or more mirror facets -- same vertices and neighbors\n", qh_setsize(qh degen_mergeset))); + qh visible_list= qh facet_tail; + while ((merge= (mergeT*)qh_setdellast (qh degen_mergeset))) { + facet1= merge->facet1; + facet2= merge->facet2; + mergetype= merge->type; + qh_memfree (merge, sizeof(mergeT)); + if (mergetype == MRGmirror) { + zinc_(Ztrimirror); + qh_triangulate_mirror (facet1, facet2); + } + } + qh_settempfree(&qh degen_mergeset); + trace2((qh ferr, "qh_triangulate: update neighbor lists for vertices from v%d\n", getid_(new_vertex_list))); + qh newvertex_list= new_vertex_list; /* all vertices of new facets */ + qh visible_list= NULL; + qh_updatevertices(/*qh newvertex_list, empty newfacet_list and visible_list*/); + qh_resetlists (False, !qh_RESETvisible /*qh newvertex_list, empty newfacet_list and visible_list*/); + + trace2((qh ferr, "qh_triangulate: identify degenerate tricoplanar facets from f%d\n", getid_(new_facet_list))); + trace2((qh ferr, "qh_triangulate: and replace facet->f.triowner with tricoplanar facets that own center, normal, etc.\n")); + FORALLfacet_(new_facet_list) { + if (facet->tricoplanar && !facet->visible) { + FOREACHneighbor_i_(facet) { + if (neighbor_i == 0) { /* first iteration */ + if (neighbor->tricoplanar) + orig_neighbor= neighbor->f.triowner; + else + orig_neighbor= neighbor; + }else { + if (neighbor->tricoplanar) + otherfacet= neighbor->f.triowner; + else + otherfacet= neighbor; + if (orig_neighbor == otherfacet) { + zinc_(Ztridegen); + facet->degenerate= True; + break; + } + } + } + } + } + + trace2((qh ferr, "qh_triangulate: delete visible facets -- non-simplicial, null, and mirrored facets\n")); + owner= NULL; + visible= NULL; + for (facet= new_facet_list; facet && facet->next; facet= nextfacet) { /* may delete facet */ + nextfacet= facet->next; + if (facet->visible) { + if (facet->tricoplanar) { /* a null or mirrored facet */ + qh_delfacet(facet); + qh num_visible--; + }else { /* a non-simplicial facet followed by its tricoplanars */ + if (visible && !owner) { + /* RBOX 200 s D5 t1001471447 | QHULL Qt C-0.01 Qx Qc Tv Qt -- f4483 had 6 vertices/neighbors and 8 ridges */ + trace2((qh ferr, "qh_triangulate: all tricoplanar facets degenerate for non-simplicial facet f%d\n", + visible->id)); + qh_delfacet(visible); + qh num_visible--; + } + visible= facet; + owner= NULL; + } + }else if (facet->tricoplanar) { + if (facet->f.triowner != visible) { + fprintf( qh ferr, "qhull error (qh_triangulate): tricoplanar facet f%d not owned by its visible, non-simplicial facet f%d\n", facet->id, getid_(visible)); + qh_errexit2 (qh_ERRqhull, facet, visible); + } + if (owner) + facet->f.triowner= owner; + else if (!facet->degenerate) { + owner= facet; + nextfacet= visible->next; /* rescan tricoplanar facets with owner */ + facet->keepcentrum= True; /* one facet owns ->normal, etc. */ + facet->coplanarset= visible->coplanarset; + facet->outsideset= visible->outsideset; + visible->coplanarset= NULL; + visible->outsideset= NULL; + if (!qh TRInormals) { /* center and normal copied to tricoplanar facets */ + visible->center= NULL; + visible->normal= NULL; + } + qh_delfacet(visible); + qh num_visible--; + } + } + } + if (visible && !owner) { + trace2((qh ferr, "qh_triangulate: all tricoplanar facets degenerate for last non-simplicial facet f%d\n", + visible->id)); + qh_delfacet(visible); + qh num_visible--; + } + qh NEWfacets= False; + qh ONLYgood= onlygood; /* restore value */ + if (qh CHECKfrequently) + qh_checkpolygon (qh facet_list); +} /* triangulate */ + + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="triangulate_facet">-</a> + + qh_triangulate_facet (facetA) + triangulate a non-simplicial facet + if qh.CENTERtype=qh_ASvoronoi, sets its Voronoi center + returns: + qh.newfacet_list == simplicial facets + facet->tricoplanar set and ->keepcentrum false + facet->degenerate set if duplicated apex + facet->f.trivisible set to facetA + facet->center copied from facetA (created if qh_ASvoronoi) + qh_eachvoronoi, qh_detvridge, qh_detvridge3 assume centers copied + facet->normal,offset,maxoutside copied from facetA + + notes: + qh_makenew_nonsimplicial uses neighbor->seen for the same + + see also: + qh_addpoint() -- add a point + qh_makenewfacets() -- construct a cone of facets for a new vertex + + design: + if qh_ASvoronoi, + compute Voronoi center (facet->center) + select first vertex (highest ID to preserve ID ordering of ->vertices) + triangulate from vertex to ridges + copy facet->center, normal, offset + update vertex neighbors +*/ +void qh_triangulate_facet (facetT *facetA, vertexT **first_vertex) { + facetT *newfacet; + facetT *neighbor, **neighborp; + vertexT *apex; + int numnew=0; + + trace3((qh ferr, "qh_triangulate_facet: triangulate facet f%d\n", facetA->id)); + + if (qh IStracing >= 4) + qh_printfacet (qh ferr, facetA); + FOREACHneighbor_(facetA) { + neighbor->seen= False; + neighbor->coplanar= False; + } + if (qh CENTERtype == qh_ASvoronoi && !facetA->center /* matches upperdelaunay in qh_setfacetplane() */ + && fabs_(facetA->normal[qh hull_dim -1]) >= qh ANGLEround * qh_ZEROdelaunay) { + facetA->center= qh_facetcenter (facetA->vertices); + } + qh_willdelete (facetA, NULL); + qh newfacet_list= qh facet_tail; + facetA->visitid= qh visit_id; + apex= SETfirst_(facetA->vertices); + qh_makenew_nonsimplicial (facetA, apex, &numnew); + SETfirst_(facetA->neighbors)= NULL; + FORALLnew_facets { + newfacet->tricoplanar= True; + newfacet->f.trivisible= facetA; + newfacet->degenerate= False; + newfacet->upperdelaunay= facetA->upperdelaunay; + newfacet->good= facetA->good; + if (qh TRInormals) { + newfacet->keepcentrum= True; + newfacet->normal= qh_copypoints (facetA->normal, 1, qh hull_dim); + if (qh CENTERtype == qh_AScentrum) + newfacet->center= qh_getcentrum (newfacet); + else + newfacet->center= qh_copypoints (facetA->center, 1, qh hull_dim); + }else { + newfacet->keepcentrum= False; + newfacet->normal= facetA->normal; + newfacet->center= facetA->center; + } + newfacet->offset= facetA->offset; +#if qh_MAXoutside + newfacet->maxoutside= facetA->maxoutside; +#endif + } + qh_matchnewfacets(/*qh newfacet_list*/); + zinc_(Ztricoplanar); + zadd_(Ztricoplanartot, numnew); + zmax_(Ztricoplanarmax, numnew); + qh visible_list= NULL; + if (!(*first_vertex)) + (*first_vertex)= qh newvertex_list; + qh newvertex_list= NULL; + qh_updatevertices(/*qh newfacet_list, empty visible_list and newvertex_list*/); + qh_resetlists (False, !qh_RESETvisible /*qh newfacet_list, empty visible_list and newvertex_list*/); +} /* triangulate_facet */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="triangulate_link">-</a> + + qh_triangulate_link (oldfacetA, facetA, oldfacetB, facetB) + relink facetA to facetB via oldfacets + returns: + adds mirror facets to qh degen_mergeset (4-d and up only) + design: + if they are already neighbors, the opposing neighbors become MRGmirror facets +*/ +void qh_triangulate_link (facetT *oldfacetA, facetT *facetA, facetT *oldfacetB, facetT *facetB) { + int errmirror= False; + + trace3((qh ferr, "qh_triangulate_link: relink old facets f%d and f%d between neighbors f%d and f%d\n", + oldfacetA->id, oldfacetB->id, facetA->id, facetB->id)); + if (qh_setin (facetA->neighbors, facetB)) { + if (!qh_setin (facetB->neighbors, facetA)) + errmirror= True; + else + qh_appendmergeset (facetA, facetB, MRGmirror, NULL); + }else if (qh_setin (facetB->neighbors, facetA)) + errmirror= True; + if (errmirror) { + fprintf( qh ferr, "qhull error (qh_triangulate_link): mirror facets f%d and f%d do not match for old facets f%d and f%d\n", + facetA->id, facetB->id, oldfacetA->id, oldfacetB->id); + qh_errexit2 (qh_ERRqhull, facetA, facetB); + } + qh_setreplace (facetB->neighbors, oldfacetB, facetA); + qh_setreplace (facetA->neighbors, oldfacetA, facetB); +} /* triangulate_link */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="triangulate_mirror">-</a> + + qh_triangulate_mirror (facetA, facetB) + delete mirrored facets from qh_triangulate_null() and qh_triangulate_mirror + a mirrored facet shares the same vertices of a logical ridge + design: + since a null facet duplicates the first two vertices, the opposing neighbors absorb the null facet + if they are already neighbors, the opposing neighbors become MRGmirror facets +*/ +void qh_triangulate_mirror (facetT *facetA, facetT *facetB) { + facetT *neighbor, *neighborB; + int neighbor_i, neighbor_n; + + trace3((qh ferr, "qh_triangulate_mirror: delete mirrored facets f%d and f%d\n", + facetA->id, facetB->id)); + FOREACHneighbor_i_(facetA) { + neighborB= SETelemt_(facetB->neighbors, neighbor_i, facetT); + if (neighbor == neighborB) + continue; /* occurs twice */ + qh_triangulate_link (facetA, neighbor, facetB, neighborB); + } + qh_willdelete (facetA, NULL); + qh_willdelete (facetB, NULL); +} /* triangulate_mirror */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="triangulate_null">-</a> + + qh_triangulate_null (facetA) + remove null facetA from qh_triangulate_facet() + a null facet has vertex #1 (apex) == vertex #2 + returns: + adds facetA to ->visible for deletion after qh_updatevertices + qh degen_mergeset contains mirror facets (4-d and up only) + design: + since a null facet duplicates the first two vertices, the opposing neighbors absorb the null facet + if they are already neighbors, the opposing neighbors become MRGmirror facets +*/ +void qh_triangulate_null (facetT *facetA) { + facetT *neighbor, *otherfacet; + + trace3((qh ferr, "qh_triangulate_null: delete null facet f%d\n", facetA->id)); + neighbor= SETfirst_(facetA->neighbors); + otherfacet= SETsecond_(facetA->neighbors); + qh_triangulate_link (facetA, neighbor, facetA, otherfacet); + qh_willdelete (facetA, NULL); +} /* triangulate_null */ + +#else /* qh_NOmerge */ +void qh_triangulate (void) { +} +#endif /* qh_NOmerge */ + + /*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="vertexintersect">-</a> + + qh_vertexintersect( vertexsetA, vertexsetB ) + intersects two vertex sets (inverse id ordered) + vertexsetA is a temporary set at the top of qhmem.tempstack + + returns: + replaces vertexsetA with the intersection + + notes: + could overwrite vertexsetA if currently too slow +*/ +void qh_vertexintersect(setT **vertexsetA,setT *vertexsetB) { + setT *intersection; + + intersection= qh_vertexintersect_new (*vertexsetA, vertexsetB); + qh_settempfree (vertexsetA); + *vertexsetA= intersection; + qh_settemppush (intersection); +} /* vertexintersect */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="vertexintersect_new">-</a> + + qh_vertexintersect_new( ) + intersects two vertex sets (inverse id ordered) + + returns: + a new set +*/ +setT *qh_vertexintersect_new (setT *vertexsetA,setT *vertexsetB) { + setT *intersection= qh_setnew (qh hull_dim - 1); + vertexT **vertexA= SETaddr_(vertexsetA, vertexT); + vertexT **vertexB= SETaddr_(vertexsetB, vertexT); + + while (*vertexA && *vertexB) { + if (*vertexA == *vertexB) { + qh_setappend(&intersection, *vertexA); + vertexA++; vertexB++; + }else { + if ((*vertexA)->id > (*vertexB)->id) + vertexA++; + else + vertexB++; + } + } + return intersection; +} /* vertexintersect_new */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="vertexneighbors">-</a> + + qh_vertexneighbors() + for each vertex in qh.facet_list, + determine its neighboring facets + + returns: + sets qh.VERTEXneighbors + nop if qh.VERTEXneighbors already set + qh_addpoint() will maintain them + + notes: + assumes all vertex->neighbors are NULL + + design: + for each facet + for each vertex + append facet to vertex->neighbors +*/ +void qh_vertexneighbors (void /*qh facet_list*/) { + facetT *facet; + vertexT *vertex, **vertexp; + + if (qh VERTEXneighbors) + return; + trace1((qh ferr, "qh_vertexneighbors: determing neighboring facets for each vertex\n")); + qh vertex_visit++; + FORALLfacets { + if (facet->visible) + continue; + FOREACHvertex_(facet->vertices) { + if (vertex->visitid != qh vertex_visit) { + vertex->visitid= qh vertex_visit; + vertex->neighbors= qh_setnew (qh hull_dim); + } + qh_setappend (&vertex->neighbors, facet); + } + } + qh VERTEXneighbors= True; +} /* vertexneighbors */ + +/*-<a href="qh-poly.htm#TOC" + >-------------------------------</a><a name="vertexsubset">-</a> + + qh_vertexsubset( vertexsetA, vertexsetB ) + returns True if vertexsetA is a subset of vertexsetB + assumes vertexsets are sorted + + note: + empty set is a subset of any other set +*/ +boolT qh_vertexsubset(setT *vertexsetA, setT *vertexsetB) { + vertexT **vertexA= (vertexT **) SETaddr_(vertexsetA, vertexT); + vertexT **vertexB= (vertexT **) SETaddr_(vertexsetB, vertexT); + + while (True) { + if (!*vertexA) + return True; + if (!*vertexB) + return False; + if ((*vertexA)->id > (*vertexB)->id) + return False; + if (*vertexA == *vertexB) + vertexA++; + vertexB++; + } + return False; /* avoid warnings */ +} /* vertexsubset */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-geom.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-geom.htm new file mode 100644 index 0000000000000000000000000000000000000000..0f16f4518e72054b2d99412fa660cca5eb5946ab --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-geom.htm @@ -0,0 +1,291 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>geom.c, geom2.c -- geometric and floating point routines</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="../src/index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm#TOC">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> + +<hr> +<!-- Main text of document. --> + +<h2>geom.c, geom2.c -- geometric and floating point routines</h2> +<blockquote> +<p>Geometrically, a vertex is a point with <em>d</em> coordinates +and a facet is a halfspace. A <em>halfspace</em> is defined by an +oriented hyperplane through the facet's vertices. A <em>hyperplane</em> +is defined by <em>d</em> normalized coefficients and an offset. A +point is <em>above</em> a facet if its distance to the facet is +positive.</p> + +<p>Qhull uses floating point coordinates for input points, +vertices, halfspace equations, centrums, and an interior point.</p> + +<p>Qhull may be configured for single precision or double +precision floating point arithmetic (see <a href="user.h#realT">realT</a> +). </p> + +<p>Each floating point operation may incur round-off error (see +<a href="qh-merge.htm#TOC">Merge</a>). The maximum error for distance +computations is determined at initialization. The roundoff error +in halfspace computation is accounted for by computing the +distance from vertices to the halfspace. </p> +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <b>Geom</b> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> • +<a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> • +<a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> • +<a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> • +<a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm#TOC">User</a> </p> + +<h3>Index to <a href="geom.c">geom.c</a>, +<a href="geom2.c">geom2.c</a>, and <a href="geom.h">geom.h</a></h3> + +<ul> +<li><a href="#gtype">geometric data types and constants</a> </li> +<li><a href="#gmacro">mathematical macros</a> +</li> +<li><a href="#gmath">mathematical functions</a> </li> +<li><a href="#gcomp">computational geometry functions</a> </li> +<li><a href="#gpoint">point array functions</a> </li> +<li><a href="#gfacet">geometric facet functions</a> </li> +<li><a href="#ground">geometric roundoff functions</a></li> +</ul> + +<h3><a href="qh-geom.htm#TOC">»</a><a name="gtype">geometric data types +and constants</a></h3> + +<ul> +<li><a href="qhull.h#coordT">coordT</a> coordinates and +coefficients are stored as realT</li> +<li><a href="qhull.h#pointT">pointT</a> a point is an array +of <tt>DIM3</tt> coordinates </li> +</ul> + +<h3><a href="qh-geom.htm#TOC">»</a><a name="gmacro">mathematical macros</a></h3> + +<ul> +<li><a href="geom.h#fabs_">fabs_</a> returns the absolute +value of a </li> +<li><a href="geom.h#fmax_">fmax_</a> returns the maximum +value of a and b </li> +<li><a href="geom.h#fmin_">fmin_</a> returns the minimum +value of a and b </li> +<li><a href="geom.h#maximize_">maximize_</a> maximize a value +</li> +<li><a href="geom.h#minimize_">minimize_</a> minimize a value +</li> +<li><a href="geom.h#det2_">det2_</a> compute a 2-d +determinate </li> +<li><a href="geom.h#det3_">det3_</a> compute a 3-d +determinate </li> +<li><a href="geom.h#dX">dX, dY, dZ</a> compute the difference +between two coordinates </li> +</ul> + +<h3><a href="qh-geom.htm#TOC">»</a><a name="gmath">mathematical functions</a></h3> + +<ul> +<li><a href="geom.c#backnormal">qh_backnormal</a> solve for +normal using back substitution </li> +<li><a href="geom2.c#crossproduct">qh_crossproduct</a> +compute the cross product of two 3-d vectors </li> +<li><a href="geom2.c#determinant">qh_determinant</a> compute +the determinant of a square matrix </li> +<li><a href="geom.c#gausselim">qh_gausselim</a> Gaussian +elimination with partial pivoting </li> +<li><a href="geom2.c#gram_schmidt">qh_gram_schmidt</a> +implements Gram-Schmidt orthogonalization by rows </li> +<li><a href="geom2.c#maxabsval">qh_maxabsval</a> return max +absolute value of a vector </li> +<li><a href="geom2.c#minabsval">qh_minabsval</a> return min +absolute value of a dim vector </li> +<li><a href="geom2.c#mindiff">qh_mindiff</a> return index of +min absolute difference of two vectors </li> +<li><a href="geom.c#normalize">qh_normalize</a> normalize a +vector </li> +<li><a href="geom.c#normalize2">qh_normalize2</a> normalize a +vector and report if too small </li> +<li><a href="geom2.c#printmatrix">qh_printmatrix</a> print +matrix given by row vectors </li> +<li><a href="geom2.c#rand">qh_rand/srand</a> generate random +numbers </li> +<li><a href="geom2.c#randomfactor">qh_randomfactor</a> return +a random factor near 1.0 </li> +<li><a href="geom2.c#randommatrix">qh_randommatrix</a> +generate a random dimXdim matrix in range (-1,1) </li> +</ul> + +<h3><a href="qh-geom.htm#TOC">»</a><a name="gcomp">computational geometry functions</a></h3> + +<ul> +<li><a href="geom2.c#detsimplex">qh_detsimplex</a> compute +determinate of a simplex of points </li> +<li><a href="io.c#detvnorm">qh_detvnorm</a> determine normal for Voronoi ridge </li> +<li><a href="geom2.c#distnorm">qh_distnorm</a> compute +distance from point to hyperplane as defined by normal and offset</li> +<li><a href="geom2.c#facetarea_simplex">qh_facetarea_simplex</a> +return area of a simplex</li> +<li><a href="geom.c#getangle">qh_getangle</a> return cosine +of angle (i.e., dot product) </li> +<li><a href="geom.c#getcenter">qh_getcenter</a> return +arithmetic center for a set of vertices </li> +<li><a href="geom2.c#pointdist">qh_pointdist</a> return +distance between two points </li> +<li><a href="geom2.c#rotatepoints">qh_rotatepoints</a> rotate +numpoints points by a row matrix </li> +<li><a href="geom2.c#sethalfspace">qh_sethalfspace</a> set +coords to dual of halfspace relative to an interior point </li> +<li><a href="geom.c#sethyperplane_det">qh_sethyperplane_det</a> +return hyperplane for oriented simplex using determinates +</li> +<li><a href="geom.c#sethyperplane_gauss">qh_sethyperplane_gauss</a> +return hyperplane for oriented simplex using Gaussian +elimination </li> +<li><a href="geom2.c#voronoi_center">qh_voronoi_center</a> +return Voronoi center for a set of points </li> +</ul> + +<h3><a href="qh-geom.htm#TOC">»</a><a name="gpoint">point array functions</a></h3> +<ul> +<li><a href="geom2.c#copypoints">qh_copypoints</a> return +malloc'd copy of points</li> +<li><a href="geom2.c#joggleinput">qh_joggleinput</a> joggle +input points by qh.JOGGLEmax </li> +<li><a href="geom2.c#maxmin">qh_maxmin</a> return max/min +points for each dimension</li> +<li><a href="geom2.c#maxsimplex">qh_maxsimplex</a> determines +maximum simplex for a set of points </li> +<li><a href="geom2.c#printpoints">qh_printpoints</a> print ids for a +set of points </li> +<li><a href="geom2.c#projectinput">qh_projectinput</a> project +input using qh DELAUNAY and qh low_bound/high_bound </li> +<li><a href="geom2.c#projectpoints">qh_projectpoints</a> +project points along one or more dimensions </li> +<li><a href="geom2.c#rotateinput">qh_rotateinput</a> rotate +input points using row matrix </li> +<li><a href="geom2.c#scaleinput">qh_scaleinput</a> scale +input points using qh low_bound/high_bound </li> +<li><a href="geom2.c#scalelast">qh_scalelast</a> scale last +coordinate to [0,m] for Delaunay triangulations </li> +<li><a href="geom2.c#scalepoints">qh_scalepoints</a> scale +points to new lowbound and highbound </li> +<li><a href="geom2.c#setdelaunay">qh_setdelaunay</a> project +points to paraboloid for Delaunay triangulation </li> +<li><a href="geom2.c#sethalfspace_all">qh_sethalfspace_all</a> +generate dual for halfspace intersection with interior +point </li> +</ul> + +<h3><a href="qh-geom.htm#TOC">»</a><a name="gfacet">geometric facet functions</a></h3> +<ul> +<li><a href="geom.c#distplane">qh_distplane</a> return +distance from point to facet </li> +<li><a href="geom2.c#facetarea">qh_facetarea</a> return area +of a facet </li> +<li><a href="geom2.c#facetcenter">qh_facetcenter</a> return +Voronoi center for a facet's vertices </li> +<li><a href="geom.c#findbest">qh_findbest</a> find visible +facet or best facet for a point </li> +<li><a href="geom.c#findbesthorizon">qh_findbesthorizon</a> +update best new facet with horizon facets</li> +<li><a href="geom.c#findbestnew">qh_findbestnew</a> find best +new facet for point </li> +<li><a href="geom2.c#getarea">qh_getarea</a> get area of all +facets in facetlist, collect statistics </li> +<li><a href="geom.c#getcentrum">qh_getcentrum</a> return +centrum for a facet </li> +<li><a href="geom.c#getdistance">qh_getdistance</a> returns +the max and min distance of a facet's vertices to a +neighboring facet</li> +<li><a href="geom2.c#findgooddist">qh_findgooddist</a> find +best good facet visible for point from facet </li> +<li><a href="geom2.c#inthresholds">qh_inthresholds</a> return +True if facet normal within 'Pdn' and 'PDn'</li> +<li><a href="geom2.c#orientoutside">qh_orientoutside</a> +orient facet so that <tt>qh.interior_point</tt> is inside</li> +<li><a href="geom.c#projectpoint">qh_projectpoint</a> project +point onto a facet </li> +<li><a href="geom.c#setfacetplane">qh_setfacetplane</a> sets +the hyperplane for a facet </li> +<li><a href="geom2.c#sharpnewfacets">qh_sharpnewfacets</a> true +if new facets contains a sharp corner</li> +</ul> + +<h3><a href="qh-geom.htm#TOC">»</a><a name="ground">geometric roundoff functions</a></h3> +<ul> +<li><a href="geom2.c#detjoggle">qh_detjoggle</a> determine +default joggle for points and distance roundoff error</li> +<li><a href="geom2.c#detroundoff">qh_detroundoff</a> +determine maximum roundoff error and other precision constants</li> +<li><a href="geom2.c#distround">qh_distround</a> compute +maximum roundoff error due to a distance computation to a +normalized hyperplane</li> +<li><a href="geom2.c#divzero">qh_divzero</a> divide by a +number that is nearly zero </li> +<li><a href="geom2.c#maxouter">qh_maxouter</a> return maximum outer +plane</li> +<li><a href="geom2.c#outerinner">qh_outerinner</a> return actual +outer and inner planes +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> + + +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-globa.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-globa.htm new file mode 100644 index 0000000000000000000000000000000000000000..26a75b5763c2af1b5b46878d255a08605f3bbb52 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-globa.htm @@ -0,0 +1,159 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>global.c -- global variables and their functions</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm#TOC">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> + +<hr> +<!-- Main text of document. --> + +<h2>global.c -- global variables and their functions</h2> +<blockquote> +<p>Qhull uses a global data structure, <tt>qh</tt>, to store +globally defined constants, lists, sets, and variables. This +allows multiple instances of Qhull to execute at the same time. +The structure may be statically allocated or +dynamically allocated with malloc(). See +<a href="user.h#QHpointer">QHpointer</a>. +</p> +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <b>Global</b> • +<a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> • +<a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> • +<a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> • +<a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm#TOC">User</a> </p> + +<h3>Index to <a href="global.c">global.c</a> and +<a href="qhull.h">qhull.h</a></h3> + +<ul> +<li><a href="#ovar">Qhull's global variables</a> </li> +<li><a href="#ofunc">Global variable and initialization +routines</a> </li> +</ul> + +<h3><a href="qh-globa.htm#TOC">»</a><a name="ovar">Qhull's global +variables</a></h3> + +<ul> +<li><a href=global.c#qh_version>qh_version</a> version string +<li><a href="qhull.h#qh">qh</a> all global variables for +qhull are in <tt>qh,qhmem</tt>, and <tt>qhstat</tt></li> +<li><a href="qhull.h#qh-const">qh constants</a> configuration +flags and constants for Qhull </li> +<li><a href="qhull.h#qh-prec">qh precision constants</a> +precision constants for Qhull </li> +<li><a href="qhull.h#qh-intern">qh internal constants</a> +internal constants for Qhull </li> +<li><a href="qhull.h#qh-lists">qh facet and vertex lists</a> +lists of facets and vertices </li> +<li><a href="qhull.h#qh-var">qh global variables</a> minimum +and maximum distances, next visit ids, several flags, and +other global variables. </li> +<li><a href="qhull.h#qh-set">qh global sets</a> global sets +for merging, hashing, input, etc. </li> +<li><a href="qhull.h#qh-buf">qh global buffers</a> buffers +for matrix operations and input </li> +<li><a href="qhull.h#qh-static">qh static variables</a> +static variables for individual functions </li> +</ul> + +<h3><a href="qh-globa.htm#TOC">»</a><a name="ofunc">Global variable and +initialization routines</a></h3> + +<ul> +<li><a href="global.c#appendprint">qh_appendprint</a> append +output format to <tt>qh.PRINTout</tt> </li> +<li><a href="global.c#freebuffers">qh_freebuffers</a> free +global memory buffers </li> +<li><a href="global.c#freeqhull">qh_freeqhull</a> free memory +used by qhull </li> +<li><a href="global.c#init_A">qh_init_A</a> called before +error handling initialized </li> +<li><a href="global.c#init_B">qh_init_B</a> called after +points are defined </li> +<li><a href="global.c#init_qhull_command">qh_init_qhull_command</a> +build <tt>qh.qhull_command</tt> from <tt>argc/argv</tt></li> +<li><a href="global.c#initflags">qh_initflags</a> set flags +and constants from command line </li> +<li><a href="global.c#initqhull_buffers">qh_initqhull_buffers</a> +initialize global memory buffers </li> +<li><a href="global.c#initqhull_globals">qh_initqhull_globals</a> +initialize global variables </li> +<li><a href="global.c#initqhull_mem">qh_initqhull_mem</a> +initialize Qhull memory management </li> +<li><a href="global.c#initqhull_start">qh_initqhull_start</a> +initialize default values at Qhull startup </li> +<li><a href="global.c#initthresholds">qh_initthresholds</a> +initialize 'Pdn' and 'PDn' thresholds </li> +<li><a href="global.c#option">qh_option</a> append option +description to <tt>qh.global_options</tt> </li> +<li><a href="global.c#restore_qhull">qh_restore_qhull</a> +restores a previously saved qhull </li> +<li><a href="global.c#save_qhull">qh_save_qhull</a> saves +qhull for a later qh_restore_qhull() </li> +<li><a href="global.c#strtol">qh_strtol</a> duplicates +strtod() and strtol() </li> +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-io.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-io.htm new file mode 100644 index 0000000000000000000000000000000000000000..2a0c256b628f6fb1804a4b5c2367c740b0181271 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-io.htm @@ -0,0 +1,295 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>io.c -- input and output operations</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> +<hr> + +<h2>io.c -- input and output operations</h2> +<blockquote> + +<p>Qhull provides a wide range of input +and output options. To organize the code, most output formats use +the same driver: </p> + +<pre> + qh_printbegin( fp, format, facetlist, facets, printall ); + + FORALLfacet_( facetlist ) + qh_printafacet( fp, format, facet, printall ); + + FOREACHfacet_( facets ) + qh_printafacet( fp, format, facet, printall ); + + qh_printend( fp, format ); +</pre> + +<p>Note the 'printall' flag. It selects whether or not +qh_skipfacet() is tested. </p> + +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> <a name="TOC">•</a> +<a href="qh-globa.htm#TOC">Global</a> • <b>Io</b> • +<a href="qh-mem.htm#TOC">Mem</a> • <a href="qh-merge.htm#TOC">Merge</a> • +<a href="qh-poly.htm#TOC">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> • +<a href="qh-set.htm#TOC">Set</a> • <a href="qh-stat.htm#TOC">Stat</a> • +<a href="qh-user.htm#TOC">User</a> </p> + +<h3>Index to <a href="io.c">io.c</a> and <a href="io.h">io.h</a></h3> + +<ul> +<li><a href="#iconst">io.h constants and types</a> </li> +<li><a href="#ilevel">User level functions</a> </li> +<li><a href="#iprint">Print functions for all output formats</a></li> +<li><a href="#itext">Text output functions</a> </li> +<li><a href="#iutil">Text utility functions</a></li> +<li><a href="#igeom">Geomview output functions</a> </li> +<li><a href="#iview">Geomview utility functions</a></li> +</ul> + +<h3><a href="qh-io.htm#TOC">»</a><a name="iconst">io.h constants and types</a></h3> + +<ul> +<li><a href="io.h#qh_MAXfirst">qh_MAXfirst</a> maximum length +of first two lines of stdin </li> +<li><a href="io.h#qh_WHITESPACE">qh_WHITESPACE</a> possible +values of white space </li> +<li><a href="io.h#printvridgeT">printvridgeT</a> function to +print results of qh_printvdiagram or qh_eachvoronoi</li> +</ul> + +<h3><a href="qh-io.htm#TOC">»</a><a name="ilevel">User level functions</a></h3> + +<ul> +<li><a href="io.c#eachvoronoi_all">qh_eachvoronoi_all</a> +visit each Voronoi ridge of the Voronoi diagram +<li><a href="io.c#printhelp_degenerate">qh_printhelp_degenerate</a> +prints descriptive message for precision error </li> +<li><a href="io.c#printhelp_singular">qh_printhelp_singular</a> +print help message for singular data </li> +<li><a href="qhull.c#printsummary">qh_printsummary</a> print +summary ('s')</li> +<li><a href="io.c#produce_output">qh_produce_output</a> +prints out the result of qhull()</li> +<li><a href="io.c#readfeasible">qh_readfeasible</a> read +interior point from remainder and qh fin ('H')</li> +<li><a href="io.c#readpoints">qh_readpoints</a> read input +points </li> +<li><a href="io.c#setfeasible">qh_setfeasible</a> set +interior point from qh feasible_string ('Hn,n,n')</li> +</ul> + +<h3><a href="qh-io.htm#TOC">»</a><a name="iprint">Print functions for all +output formats</a></h3> + +<ul> +<li><a href="io.c#countfacets">qh_countfacets</a> count good +facets for printing and set visitid </li> +<li><a href="io.c#markkeep">qh_markkeep</a> mark good facets +that meet qh.KEEParea ('PAn'), qh.KEEPmerge ('PMn'), and qh.KEEPminArea ('PFn')</li> +<li><a href="io.c#order_vertexneighbors">qh_order_vertexneighbors</a> +order neighbors for a 3-d vertex by adjacency ('i', 'o')</li> +<li><a href="io.c#printafacet">qh_printafacet</a> print facet +in an output format </li> +<li><a href="io.c#printbegin">qh_printbegin</a> print header +for an output format </li> +<li><a href="io.c#printend">qh_printend</a> print trailer for +an output format </li> +<li><a href="user.c#printfacetlist">qh_printfacetlist</a> +print facets in a facetlist</li> +<li><a href="io.c#printfacets">qh_printfacets</a> print +facetlist and/or facet set in an output format </li> +<li><a href="io.c#printneighborhood">qh_printneighborhood</a> +print neighborhood of one or two facets ('Po')</li> +<li><a href="io.c#produce_output">qh_produce_output</a> +print the results of qh_qhull() </li> +<li><a href="io.c#skipfacet">qh_skipfacet</a> True if not +printing this facet ('Pdk:n', 'QVn', 'QGn')</li> +<li><a href="io.c#facetvertices">qh_facetvertices</a> return +vertices in a set of facets ('p')</li> +</ul> + +<h3><a href="qh-io.htm#TOC">»</a><a name="itext">Text output functions</a></h3> +<ul> +<li><a href="io.c#eachvoronoi">qh_eachvoronoi</a> +print or visit each Voronoi ridge for an input site of the Voronoi diagram +<li><a href="io.c#printextremes">qh_printextremes</a> print +extreme points by point ID (vertices of convex hull) ('Fx')</li> +<li><a href="io.c#printextremes_2d">qh_printextremes_2d</a> print +2-d extreme points by point ID ('Fx')</li> +<li><a href="io.c#printextremes_d">qh_printextremes_d</a> print +extreme points of input sites for Delaunay triangulations ('Fx')</li> +<li><a href="io.c#printfacet">qh_printfacet</a> print all +fields of a facet ('f')</li> +<li><a href="io.c#printfacet2math">qh_printfacet2math</a> print +2-d Maple or Mathematica output for a facet ('FM' or 'm')</li> +<li><a href="io.c#printfacet3math">qh_printfacet3math</a> +print 3-d Maple or Mathematica facet ('FM' or 'm')</li> +<li><a href="io.c#printfacet3vertex">qh_printfacet3vertex</a> +print vertices for a 3-d facet ('i', 'o')</li> +<li><a href="io.c#printfacetheader">qh_printfacetheader</a> +prints header fields of a facet ('f')</li> +<li><a href="io.c#printfacetNvertex_nonsimplicial">qh_printfacetNvertex_nonsimplicial</a> +print vertices for an N-d non-simplicial facet ('i', 'Ft')</li> +<li><a href="io.c#printfacetNvertex_simplicial">qh_printfacetNvertex_simplicial</a> +print vertices for an N-d simplicial facet ('i', 'o', 'Ft')</li> +<li><a href="io.c#printfacetridges">qh_printfacetridges</a> +prints ridges of a facet ('f')</li> +<li><a href="io.c#printpoints_out">qh_printpoints_out</a> prints +vertices for facets by their point coordinates ('p')</li> +<li><a href="io.c#printridge">qh_printridge</a> print all +fields for a ridge ('f')</li> +<li><a href="io.c#printvdiagram">qh_printvdiagram</a> print +voronoi diagram as Voronoi vertices for each input pair</li> +<li><a href="io.c#printvertex">qh_printvertex</a> print all +fields for a vertex ('f')</li> +<li><a href="io.c#printvertexlist">qh_printvertexlist</a> +print vertices used by a list or set of facets ('f')</li> +<li><a href="io.c#printvertices">qh_printvertices</a> print a +set of vertices ('f')</li> +<li><a href="io.c#printvneighbors">qh_printvneighbors</a> +print vertex neighbors of vertices ('FN')</li> +<li><a href="io.c#printvoronoi">qh_printvoronoi</a> print +voronoi diagram in 'o' or 'G' format</li> +</ul> + +<h3><a href="qh-io.htm#TOC">»</a><a name="iutil">Text utility functions</a></h3> +<ul> +<li><a href="io.c#dfacet">dfacet</a> print facet by ID </li> +<li><a href="io.c#dvertex">dvertex</a> print vertex by ID </li> +<li><a href="io.c#compare_facetarea">qh_compare_facetarea</a> +used by qsort() to order facets by area </li> +<li><a href="io.c#compare_facetmerge">qh_compare_facetmerge</a> +used by qsort() to order facets by number of merges </li> +<li><a href="io.c#compare_facetvisit">qh_compare_facetvisit</a> +used by qsort() to order facets by visit ID or ID </li> +<li><a href="io.c#compare_vertexpoint">qh_compare_vertexpoint</a> +used by qsort() to order vertices by point ID </li> +<li><a href="io.c#detvnorm">qh_detvnorm</a> determine normal for Voronoi ridge </li> +<li><a href="io.c#detvridge">qh_detvridge</a> determine Voronoi +ridge for an input site +<li><a href="io.c#detvridge3">qh_detvridge3</a> determine 3-d Voronoi +ridge for an input site +<li><a href="io.c#facet2point">qh_facet2point</a> return two +projected temporary vertices for a 2-d facet ('m', 'G')</li> +<li><a href="io.c#markvoronoi">qh_markvoronoi</a> mark Voronoi +vertices for printing +<li><a href="io.c#printcenter">qh_printcenter</a> print +facet->center as centrum or Voronoi center ('Ft', 'v p', 'FC', 'f') </li> +<li><a href="io.c#printpoint">qh_printpoint</a>, qh_printpointid, print +coordinates of a point ('p', 'o', 'Fp', 'G', 'f')</li> +<li><a href="io.c#printpoint3">qh_printpoint3</a> prints 2-d, +3-d, or 4-d point as 3-d coordinates ('G')</li> +<li><a href="io.c#printvdiagram2">qh_printvdiagram2</a> print +voronoi diagram for each ridge of each vertex from qh_markvoronoi</li> +<li><a href="io.c#printvnorm">qh_printvnorm</a> print +separating plane of the Voronoi diagram for a pair of input sites</li> +<li><a href="io.c#printvridge">qh_printvridge</a> print +ridge of the Voronoi diagram for a pair of input sites</li> +<li><a href="io.c#projectdim3">qh_projectdim3</a> project 2-d +3-d or 4-d point to a 3-d point ('G')</li> +</ul> + +<h3><a href="qh-io.htm#TOC">»</a><a name="igeom">Geomview output functions</a></h3> +<ul> +<li><a href="io.c#printfacet2geom">qh_printfacet2geom</a> +print facet as a 2-d VECT object </li> +<li><a href="io.c#printfacet2geom_points">qh_printfacet2geom_points</a> +print points as a 2-d VECT object with offset </li> +<li><a href="io.c#printfacet3geom_nonsimplicial">qh_printfacet3geom_nonsimplicial</a> +print Geomview OFF for a 3-d nonsimplicial facet. </li> +<li><a href="io.c#printfacet3geom_points">qh_printfacet3geom_points</a> +prints a 3-d facet as OFF Geomview object. </li> +<li><a href="io.c#printfacet3geom_simplicial">qh_printfacet3geom_simplicial</a> +print Geomview OFF for a 3-d simplicial facet. </li> +<li><a href="io.c#printfacet4geom_nonsimplicial">qh_printfacet4geom_nonsimplicial</a> +print Geomview 4OFF file for a 4d nonsimplicial facet </li> +<li><a href="io.c#printfacet4geom_simplicial">qh_printfacet4geom_simplicial</a> +print Geomview 4OFF file for a 4d simplicial facet </li> +<li><a href="io.c#printhyperplaneintersection">qh_printhyperplaneintersection</a> +print hyperplane intersection as OFF or 4OFF </li> +<li><a href="io.c#printvoronoi">qh_printvoronoi</a> print +voronoi diagram in 'o' or 'G' format</li> +</ul> +<h3><a href="qh-io.htm#TOC">»</a><a name="iview">Geomview utility functions</a></h3> +<ul> +<li><a href="io.c#geomplanes">qh_geomplanes</a> + return outer and inner planes for Geomview</li> +<li><a href="io.c#printcentrum">qh_printcentrum</a> print +centrum for a facet in OOGL format </li> +<li><a href="io.c#printend4geom">qh_printend4geom</a> helper +function for qh_printbegin/printend </li> +<li><a href="io.c#printhyperplaneintersection">qh_printhyperplaneintersection</a> +print Geomview OFF or 4OFF for the intersection of two +hyperplanes in 3-d or 4-d </li> +<li><a href="io.c#printline3geom">qh_printline3geom</a> prints a +line as a VECT </li> +<li><a href="io.c#printpointvect">qh_printpointvect</a> +prints a 2-d or 3-d point as 3-d VECT's </li> +<li><a href="io.c#printpointvect2">qh_printpointvect2</a> +prints a 2-d or 3-d point as 2 3-d VECT's </li> +<li><a href="io.c#printspheres">qh_printspheres</a> prints 3-d +vertices as OFF spheres </li> +</ul> +<p> +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-mem.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-mem.htm new file mode 100644 index 0000000000000000000000000000000000000000..cfc414a7750f859eaf14e11e01910e5cae594d3f --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-mem.htm @@ -0,0 +1,140 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>mem.c -- memory operations</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> +<hr> + +<h2>mem.c -- memory operations</h2> +<blockquote> +<p>Qhull uses quick-fit memory allocation. It maintains a +set of free lists for a variety of small allocations. A +small request returns a block from the best fitting free +list. If the free list is empty, Qhull allocates a block +from a reserved buffer. </p> +<p>Use 'T5' to trace memory allocations.</p> + +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> • +<a href="qh-io.htm#TOC">Io</a> • <b>Mem</b> +• <a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> +• <a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> +• <a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm#TOC">User</a> +</p> +<h3>Index to <a href="mem.c">mem.c</a> and +<a href="mem.h">mem.h</a></h3> +<ul> +<li><a href="#etype">mem.h data types</a> </li> +<li><a href="#emacro">mem.h macros</a> </li> +<li><a href="#efunc">User level functions</a> </li> +</ul> +<h3><a href="qh-mem.htm#TOC">»</a><a name="etype">mem.h data types and constants</a></h3> +<ul> +<li><a href="mem.h#ptr_intT">ptr_intT</a> for casting +a void* to an integer-type </li> +<li><a href="mem.h#qhmemT">qhmemT</a> global memory +structure for mem.c </li> +<li><a href="mem.h#NOmem">qh_NOmem</a> disable memory allocation</li> +</ul> +<h3><a href="qh-mem.htm#TOC">»</a><a name="emacro">mem.h macros</a></h3> +<ul> +<li><a href="mem.h#memalloc_">qh_memalloc_</a> +allocate memory</li> +<li><a href="mem.h#memfree_">qh_memfree_</a> free +memory</li> +</ul> +<h3><a href="qh-mem.htm#TOC">»</a><a name="efunc">User level +functions</a></h3> +<ul> +<li><a href="mem.c#memalloc">qh_memalloc</a> allocate +memory </li> +<li><a href="mem.c#memfree">qh_memfree</a> free +memory </li> +<li><a href="mem.c#meminit">qh_meminit</a> initialize +memory </li> +<li><a href="mem.c#memstatistics">qh_memstatistics</a> +print memory statistics </li> +<li><a href="mem.c#NOmem">qh_NOmem</a> allocation routines with malloc() and free() +</ul> + +<h3><a href="qh-mem.htm#TOC">»</a><a name="m">Initialization and +termination functions</a></h3> +<ul> +<li><a href="mem.c#intcompare">qh_intcompare</a> used by +qsort and bsearch to compare two integers </li> +<li><a href="mem.c#memfreeshort">qh_memfreeshort</a> +frees up all short and qhmem memory allocations </li> +<li><a href="mem.c#meminit">qh_meminit</a> initialize +memory </li> +<li><a href="mem.c#meminitbuffers">qh_meminitbuffers</a> +initialize qhmem </li> +<li><a href="mem.c#memsetup">qh_memsetup</a> set up +memory after running memsize() </li> +<li><a href="mem.c#memsize">qh_memsize</a> define a free +list for this size </li> +<li><a href="mem.c#memstatistics">qh_memstatistics</a> +print out memory statistics </li> +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-merge.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-merge.htm new file mode 100644 index 0000000000000000000000000000000000000000..0c17b5dd35fd0eced4450551e8e4fcdcd333dfbb --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-merge.htm @@ -0,0 +1,364 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>merge.c -- facet merge operations</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> +<hr> + +<h2>merge.c -- facet merge operations</h2> +<blockquote> +<p>Qhull handles precision problems by merged facets or joggled input. +Except for redundant vertices, it corrects a problem by +merging two facets. When done, all facets are clearly +convex. See <a href="../html/qh-impre.htm">Imprecision in Qhull</a> +for further information. </p> +<p>Users may joggle the input ('<a href="../html/qh-optq.htm#QJn">QJn</a>') +instead of merging facets. </p> +<p>Qhull detects and corrects the following problems: </p> +<ul> +<li><b>More than two facets meeting at a ridge. </b>When +Qhull creates facets, it creates an even number +of facets for each ridge. A convex hull always +has two facets for each ridge. More than two +facets may be created if non-adjacent facets +share a vertex. This is called a <em>duplicate +ridge</em>. In 2-d, a duplicate ridge would +create a loop of facets. </li> +</ul> +<ul> +<li><b>A facet contained in another facet. </b>Facet +merging may leave all vertices of one facet as a +subset of the vertices of another facet. This is +called a <em>redundant facet</em>. </li> +</ul> +<ul> +<li><b>A facet with fewer than three neighbors. </b>Facet +merging may leave a facet with one or two +neighbors. This is called a <em>degenerate facet</em>. +</li> +</ul> +<ul> +<li><b>A facet with flipped orientation. </b>A +facet's hyperplane may define a halfspace that +does not include the interior point.This is +called a <em>flipped facet</em>. </li> +</ul> +<ul> +<li><strong>A coplanar horizon facet.</strong> A +newly processed point may be coplanar with an +horizon facet. Qhull creates a new facet without +a hyperplane. It links new facets for the same +horizon facet together. This is called a <em>samecycle</em>. +The new facet or samecycle is merged into the +horizon facet. </li> +</ul> +<ul> +<li><b>Concave facets. </b>A facet's centrum may be +above a neighboring facet. If so, the facets meet +at a concave angle. </li> +</ul> +<ul> +<li><b>Coplanar facets. </b>A facet's centrum may be +coplanar with a neighboring facet (i.e., it is +neither clearly below nor clearly above the +facet's hyperplane). Qhull removes coplanar +facets in independent sets sorted by angle.</li> +</ul> +<ul> +<li><b>Redundant vertex. </b>A vertex may have fewer +than three neighboring facets. If so, it is +redundant and may be renamed to an adjacent +vertex without changing the topological +structure.This is called a <em>redundant vertex</em>. +</li> +</ul> +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> +• <a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> +• <b>Merge</b> • <a href="qh-poly.htm#TOC">Poly</a> +• <a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> +• <a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm#TOC">User</a> +</p> +<h3>Index to <a href="merge.c">merge.c</a> and +<a href="merge.h">merge.h</a></h3> +<ul> +<li><a href="#mtype">merge.h data types, macros, and +global sets</a> </li> +<li><a href="#mconst">merge.h constants</a> </li> +</ul> +<ul> +<li><a href="#mtop">top-level merge functions</a> </li> +<li><a href="#mset">functions for identifying merges</a></li> +<li><a href="#mbest">functions for determining the +best merge</a> </li> +<li><a href="#mmerge">functions for merging facets</a> +</li> +<li><a href="#mcycle">functions for merging a cycle +of facets</a> </li> +<li><a href="#mrename">functions for renaming a +vertex</a> </li> +<li><a href="#mvertex">functions for identifying +vertices for renaming</a> </li> +<li><a href="#mcheck">functions for check and trace</a> </li> +</ul> +<h3><a href="qh-merge.htm#TOC">»</a><a name="mtype">merge.h data +types, macros, and global sets</a></h3> +<ul> +<li><a href="merge.h#mergeT">mergeT</a> structure to +identify a merge of two facets</li> +<li><a href="merge.h#FOREACHmerge_">FOREACHmerge_</a> +assign 'merge' to each merge in merges </li> +<li><a href="qhull.h#qh-set">qh global sets</a> +qh.facet_mergeset contains non-convex merges +while qh.degen_mergeset contains degenerate and +redundant facets</li> +</ul> +<h3><a href="qh-merge.htm#TOC">»</a><a name="mconst">merge.h +constants</a></h3> +<ul> +<li><a href="qhull.h#qh-prec">qh precision constants</a> +precision constants for Qhull </li> +<li><a href="merge.h#MRG">MRG...</a> indicates the +type of a merge (mergeT->type)</li> +<li><a href="merge.h#qh_ANGLEredundant">qh_ANGLEredundant</a> +indicates redundant merge in mergeT->angle </li> +<li><a href="merge.h#qh_ANGLEdegen">qh_ANGLEdegen</a> +indicates degenerate facet in mergeT->angle </li> +<li><a href="merge.h#qh_ANGLEconcave">qh_ANGLEconcave</a> +offset to indicate concave facets in +mergeT->angle </li> +<li><a href="merge.h#qh_MERGEapex">qh_MERGEapex</a> +flag for qh_mergefacet() to indicate an apex +merge </li> +</ul> +<h3><a href="qh-merge.htm#TOC">»</a><a name="mtop">top-level merge +functions</a></h3> +<ul> +<li><a href="merge.c#all_merges">qh_all_merges</a> +merge all non-convex facets </li> +<li><a href="merge.c#checkzero">qh_checkzero</a> +check that facets are clearly convex </li> +<li><a href="merge.c#flippedmerges">qh_flippedmerges</a> +merge flipped facets into best neighbor </li> +<li><a href="merge.c#forcedmerges">qh_forcedmerges</a> +merge all duplicate ridges </li> +<li><a href="merge.c#merge_degenredundant">qh_merge_degenredundant</a> +merge degenerate and redundant facets </li> +<li><a href="merge.c#merge_nonconvex">qh_merge_nonconvex</a> +merge a non-convex ridge </li> +<li><a href="merge.c#premerge">qh_premerge</a> +pre-merge non-convex facets </li> +<li><a href="merge.c#postmerge">qh_postmerge</a> +post-merge nonconvex facets as defined by +maxcentrum/maxangle </li> +</ul> + +<h3><a href="qh-merge.htm#TOC">»</a><a name="mset">functions for +identifying merges</a></h3> +<ul> +<li><a href="merge.c#appendmergeset">qh_appendmergeset</a> +appends an entry to qh.facet_mergeset</li> +<li><a href="merge.c#compareangle">qh_compareangle</a> +used by qsort() to order merges </li> +<li><a href="merge.c#comparemerge">qh_comparemerge</a> +used by qsort() to order merges </li> +<li><a href="merge.c#degen_redundant_facet">qh_degen_redundant_facet</a> +check for a degenerate and redundant facet</li> +<li><a href="merge.c#degen_redundant_neighbors">qh_degen_redundant_neighbors</a> +append degenerate and redundant neighbors to +qh.degen_mergeset </li> +<li><a href="merge.c#getmergeset_initial">qh_getmergeset_initial</a> +build initial qh.facet_mergeset </li> +<li><a href="merge.c#getmergeset">qh_getmergeset</a> +update qh.facet_mergeset </li> +<li><a href="merge.c#mark_dupridges">qh_mark_dupridges</a> +add duplicated ridges to qh.facet_mergeset</li> +<li><a href="merge.c#maydropneighbor">qh_maydropneighbor</a> +drop neighbor relationship if no ridge between +facet and neighbor </li> +<li><a href="merge.c#test_appendmerge">qh_test_appendmerge</a> +test a pair of facets for convexity and append to +qh.facet_mergeset if non-convex </li> +<li><a href="merge.c#test_vneighbors">qh_test_vneighbors</a> +test vertex neighbors for convexity </li> +</ul> + +<h3><a href="qh-merge.htm#TOC">»</a><a name="mbest">functions for +determining the best merge</a></h3> +<ul> +<li><a href="merge.c#findbest_test">qh_findbest_test</a> +test neighbor for best merge </li> +<li><a href="merge.c#findbestneighbor">qh_findbestneighbor</a> +finds best neighbor of a facet for merging (i.e., +closest hyperplane) </li> +</ul> + +<h3><a href="qh-merge.htm#TOC">»</a><a name="mmerge">functions for +merging facets</a></h3> +<ul> +<li><a href="merge.c#copynonconvex">qh_copynonconvex</a> +copy non-convex flag to another ridge for the +same neighbor </li> +<li><a href="merge.c#makeridges">qh_makeridges</a> +creates explicit ridges between simplicial facets +</li> +<li><a href="merge.c#mergefacet">qh_mergefacet</a> +merges one facet into another facet</li> +<li><a href="merge.c#mergeneighbors">qh_mergeneighbors</a> +merges the neighbors of two facets </li> +<li><a href="merge.c#mergeridges">qh_mergeridges</a> +merges the ridge sets of two facets </li> +<li><a href="merge.c#mergesimplex">qh_mergesimplex</a> +merge a simplicial facet into another simplicial +facet </li> +<li><a href="merge.c#mergevertex_del">qh_mergevertex_del</a> +delete a vertex due to merging one facet into +another facet </li> +<li><a href="merge.c#mergevertex_neighbors">qh_mergevertex_neighbors</a> +merge the vertex neighbors of two facets </li> +<li><a href="merge.c#mergevertices">qh_mergevertices</a> +merge the vertex sets of two facets </li> +<li><a href="merge.c#newvertices">qh_newvertices</a> +register all vertices as new vertices </li> +<li><a href="merge.c#updatetested">qh_updatetested</a> +clear tested flags and centrums involved in a +merge </li> +<li><a href="merge.c#willdelete">qh_willdelete</a> +moves facet to qh.visible_list; sets replacement +or NULL </li> +</ul> + +<h3><a href="qh-merge.htm#TOC">»</a><a name="mcycle">functions for +merging a cycle of facets</a></h3> +<p>If a point is coplanar with an horizon facet, the +corresponding new facets are linked together (a <em>samecycle</em>) +for merging.</p> +<ul> +<li><a href="merge.c#basevertices">qh_basevertices</a> +return temporary set of base vertices for a +samecycle </li> +<li><a href="merge.c#mergecycle">qh_mergecycle</a> +merge a samecycle into a horizon facet </li> +<li><a href="merge.c#mergecycle_all">qh_mergecycle_all</a> +merge all samecycles into horizon facets</li> +<li><a href="merge.c#mergecycle_facets">qh_mergecycle_facets</a> +finish merge of samecycle </li> +<li><a href="merge.c#mergecycle_neighbors">qh_mergecycle_neighbors</a> +merge neighbor sets for samecycle </li> +<li><a href="merge.c#mergecycle_ridges">qh_mergecycle_ridges</a> +merge ridge sets for samecycle </li> +<li><a href="merge.c#mergecycle_vneighbors">qh_mergecycle_vneighbors</a> +merge vertex neighbor sets for samecycle </li> +</ul> +<h3><a href="qh-merge.htm#TOC">»</a><a name="mrename">functions +for renaming a vertex</a></h3> +<ul> +<li><a href="merge.c#comparevisit">qh_comparevisit</a> +used by qsort() to order vertices by visitid</li> +<li><a href="merge.c#reducevertices">qh_reducevertices</a> +reduce vertex sets </li> +<li><a href="merge.c#redundant_vertex">qh_redundant_vertex</a> +returns true if detect and rename redundant +vertex </li> +<li><a href="merge.c#rename_sharedvertex">qh_rename_sharedvertex</a> +detect and rename a shared vertex </li> +<li><a href="merge.c#renameridgevertex">qh_renameridgevertex</a> +rename oldvertex to newvertex in a ridge </li> +<li><a href="merge.c#renamevertex">qh_renamevertex</a> +rename oldvertex to newvertex in ridges </li> +<li><a href="merge.c#remove_extravertices">qh_remove_extravertices</a> +remove extra vertices in non-simplicial facets </li> +</ul> + +<h3><a href="qh-merge.htm#TOC">»</a><a name="mvertex">functions +for identifying vertices for renaming</a></h3> +<ul> +<li><a href="merge.c#find_newvertex">qh_find_newvertex</a> +locate new vertex for renaming old vertex </li> +<li><a href="merge.c#hashridge">qh_hashridge</a> add +ridge to hashtable </li> +<li><a href="merge.c#hashridge_find">qh_hashridge_find</a> +returns matching ridge in hashtable</li> +<li><a href="merge.c#neighbor_intersections">qh_neighbor_intersections</a> +return intersection of vertex sets for +neighboring facets </li> +<li><a href="merge.c#vertexridges">qh_vertexridges</a> +return temporary set of ridges adjacent to a +vertex </li> +<li><a href="merge.c#vertexridges_facet">qh_vertexridges_facet</a> +add adjacent ridges for a vertex in facet </li> +</ul> + +<h3><a href="qh-merge.htm#TOC">»</a><a name="mcheck">functions for check and +trace</a></h3> +<ul> +<li><a href="merge.c#checkconnect">qh_checkconnect</a> +check that new facets are connected </li> +<li><a href="merge.c#tracemerge">qh_tracemerge</a> +print trace message after merge </li> +<li><a href="merge.c#tracemerging">qh_tracemerging</a> +print trace message during post-merging </li> +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-poly.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-poly.htm new file mode 100644 index 0000000000000000000000000000000000000000..a28b370da4557f3c4b0659f3ee8abe65ba921e69 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-poly.htm @@ -0,0 +1,481 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>poly.c, poly2.c -- polyhedron operations</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> +<hr> + +<h2>poly.c, poly2.c -- polyhedron operations</h2> +<blockquote> + +<p>Qhull uses dimension-free terminology. Qhull builds a +polyhedron in dimension <em>d. </em>A <em>polyhedron</em> is a +simplicial complex of faces with geometric information for the +top and bottom-level faces. A (<em>d-1</em>)-face is a <em>facet</em>, +a (<em>d-2</em>)-face is a <em>ridge</em>, and a <em>0</em>-face +is a <em>vertex</em>. For example in 3-d, a facet is a polygon +and a ridge is an edge. A facet is built from a ridge (the <em>base</em>) +and a vertex (the <em>apex</em>). See +<a href="../html/index.htm#structure">Qhull's data structures</a>.</p> + +<p>Qhull's primary data structure is a polyhedron. A +polyhedron is a list of facets. Each facet has a set of +neighboring facets and a set of vertices. Each facet has a +hyperplane. For example, a tetrahedron has four facets. +If its vertices are <em>a, b, c, d</em>, and its facets +are <em>1, 2, 3, 4,</em> the tetrahedron is </p> +<blockquote> +<ul> +<li>facet 1 <ul> + <li>vertices: b c d </li> + <li>neighbors: 2 3 4 </li> +</ul> +</li> +<li>facet 2 <ul> + <li>vertices: a c d </li> + <li>neighbors: 1 3 4 </li> +</ul> +</li> +<li>facet 3 <ul> + <li>vertices: a b d </li> + <li>neighbors: 1 2 4 </li> +</ul> +</li> +<li>facet 4 <ul> + <li>vertices: a b c </li> + <li>neighbors: 1 2 3 </li> +</ul> +</li> +</ul> +</blockquote> +<p>A facet may be simplicial or non-simplicial. In 3-d, a +<i>simplicial facet</i> has three vertices and three +neighbors. A <i>nonsimplicial facet</i> has more than +three vertices and more than three neighbors. A +nonsimplicial facet has a set of ridges and a centrum. </p> +<p> +A simplicial facet has an orientation. An <i>orientation</i> +is either <i>top</i> or <i>bottom</i>. +The flag, <tt>facet->toporient,</tt> +defines the orientation of the facet's vertices. For example in 3-d, +'top' is left-handed orientation (i.e., the vertex order follows the direction +of the left-hand fingers when the thumb is pointing away from the center). +Except for axis-parallel facets in 5-d and higher, topological orientation +determines the geometric orientation of the facet's hyperplane. + +<p>A nonsimplicial facet is due to merging two or more +facets. The facet's ridge set determine a simplicial +decomposition of the facet. Each ridge is a 1-face (i.e., +it has two vertices and two neighboring facets). The +orientation of a ridge is determined by the order of the +neighboring facets. The flag, <tt>facet->toporient,</tt>is +ignored. </p> +<p>A nonsimplicial facet has a centrum for testing +convexity. A <i>centrum</i> is a point on the facet's +hyperplane that is near the center of the facet. Except +for large facets, it is the arithmetic average of the +facet's vertices. </p> +<p>A nonsimplicial facet is an approximation that is +defined by offsets from the facet's hyperplane. When +Qhull finishes, the <i>outer plane</i> is above all +points while the <i>inner plane</i> is below the facet's +vertices. This guarantees that any exact convex hull +passes between the inner and outer planes. The outer +plane is defined by <tt>facet->maxoutside</tt> while +the inner plane is computed from the facet's vertices.</p> + +<p>Qhull 3.1 includes triangulation of non-simplicial facets +('<A href="../html/qh-optq.htm#Qt">Qt</A>'). +These facets, +called <i>tricoplanar</i>, share the same normal. centrum, and Voronoi center. +One facet (keepcentrum) owns these data structures. +While tricoplanar facets are more accurate than the simplicial facets from +joggled input, they +may have zero area or flipped orientation. + +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> +• <a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> +• <a href="qh-merge.htm#TOC">Merge</a> • <b>Poly</b> +• <a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> +• <a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm#TOC">User</a> +</p> +<h3>Index to <a href="poly.c">poly.c</a>, +<a href="poly2.c">poly2.c</a>, <a href="poly.h">poly.h</a>, +and <a href="qhull.h">qhull.h</a></h3> +<ul> +<li><a href="#ptype">Data types and global +lists for polyhedrons</a> </li> +<li><a href="#pconst">poly.h constants</a> </li> +<li><a href="#pgall">Global FORALL macros</a> </li> +<li><a href="#pall">FORALL macros</a> </li> +<li><a href="#peach">FOREACH macros</a> </li> +<li><a href="#pieach">Indexed FOREACH macros</a> </li> +<li><a href="#pmacro">Other macros for polyhedrons</a><p> </li> +<li><a href="#plist">Facetlist functions</a> </li> +<li><a href="#pfacet">Facet functions</a> </li> +<li><a href="#pvertex">Vertex, ridge, and point +functions</a> </li> +<li><a href="#phash">Hashtable functions</a> </li> +<li><a href="#pnew">Allocation and deallocation +functions</a> </li> +<li><a href="#pcheck">Check functions</a> </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="ptype">Data +types and global lists for polyhedrons</a></h3> +<ul> +<li><a href="qhull.h#facetT">facetT</a> defines a +facet </li> +<li><a href="qhull.h#ridgeT">ridgeT</a> defines a +ridge </li> +<li><a href="qhull.h#vertexT">vertexT</a> defines a +vertex </li> +<li><a href="qhull.h#qh-lists">qh facet and vertex +lists</a> lists of facets and vertices </li> +<li><a href="qhull.h#qh-set">qh global sets</a> +global sets for merging, hashing, input, etc. </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pconst">poly.h constants</a></h3> +<ul> +<li><a href="poly.h#ALGORITHMfault">ALGORITHMfault</a> +flag to not report errors in qh_checkconvex() </li> +<li><a href="poly.h#DATAfault">DATAfault</a> flag to +report errors in qh_checkconvex() </li> +<li><a href="poly.h#DUPLICATEridge">DUPLICATEridge</a> +special value for facet->neighbor to indicate +a duplicate ridge </li> +<li><a href="poly.h#MERGEridge">MERGEridge</a> +special value for facet->neighbor to indicate +a merged ridge </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pgall">Global FORALL +macros</a></h3> +<ul> +<li><a href="qhull.h#FORALLfacets">FORALLfacets</a> +assign 'facet' to each facet in qh.facet_list </li> +<li><a href="poly.h#FORALLnew_facets">FORALLnew_facets</a> +assign 'facet' to each facet in qh.newfacet_list </li> +<li><a href="poly.h#FORALLvisible_facets">FORALLvisible_facets</a> +assign 'visible' to each visible facet in +qh.visible_list </li> +<li><a href="qhull.h#FORALLpoints">FORALLpoints</a> +assign 'point' to each point in qh.first_point, +qh.num_points </li> +<li><a href="qhull.h#FORALLvertices">FORALLvertices</a> +assign 'vertex' to each vertex in qh.vertex_list </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pall">FORALL macros</a></h3> +<ul> +<li><a href="poly.h#FORALLfacet_">FORALLfacet_</a> +assign 'facet' to each facet in facetlist </li> +<li><a href="qhull.h#FORALLpoint_">FORALLpoint_</a> +assign 'point' to each point in points array</li> +<li><a href="poly.h#FORALLsame_">FORALLsame_</a> +assign 'same' to each facet in samecycle</li> +<li><a href="poly.h#FORALLsame_cycle_">FORALLsame_cycle_</a> +assign 'same' to each facet in samecycle</li> +<li><a href="poly.h#FORALLvertex_">FORALLvertex_</a> +assign 'vertex' to each vertex in vertexlist </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="peach">FOREACH macros</a></h3> +<ul> +<li><a href="qhull.h#FOREACHfacet_">FOREACHfacet_</a> +assign 'facet' to each facet in facets </li> +<li><a href="qhull.h#FOREACHneighbor_">FOREACHneighbor_</a> +assign 'neighbor' to each facet in +facet->neighbors or vertex->neighbors</li> +<li><a href="poly.h#FOREACHnewfacet_">FOREACHnewfacet_</a> +assign 'newfacet' to each facet in facet set </li> +<li><a href="qhull.h#FOREACHpoint_">FOREACHpoint_</a> +assign 'point' to each point in points set </li> +<li><a href="qhull.h#FOREACHridge_">FOREACHridge_</a> +assign 'ridge' to each ridge in ridge set </li> +<li><a href="qhull.h#FOREACHvertex_">FOREACHvertex_</a> +assign 'vertex' to each vertex in vertex set </li> +<li><a href="poly.h#FOREACHvertexA_">FOREACHvertexA_</a> +assign 'vertexA' to each vertex in vertex set</li> +<li><a href="poly.h#FOREACHvisible_">FOREACHvisible_</a> +assign 'visible' to each facet in facet set </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pieach">Indexed +FOREACH macros</a></h3> +<ul> +<li><a href="qhull.h#FOREACHfacet_i_">FOREACHfacet_i_</a> +assign 'facet' and 'facet_i' to each facet in +facet set </li> +<li><a href="qhull.h#FOREACHneighbor_i_">FOREACHneighbor_i_</a> +assign 'neighbor' and 'neighbor_i' to each facet +in facet->neighbors or vertex->neighbors</li> +<li><a href="qhull.h#FOREACHpoint_i_">FOREACHpoint_i_</a> +assign 'point' and 'point_i' to each point in +points set </li> +<li><a href="qhull.h#FOREACHridge_i_">FOREACHridge_i_</a> +assign 'ridge' and 'ridge_i' to each ridge in +ridges set </li> +<li><a href="qhull.h#FOREACHvertex_i_">FOREACHvertex_i_</a> +assign 'vertex' and 'vertex_i' to each vertex in +vertices set </li> +<li><a href="poly.h#FOREACHvertexreverse12_">FOREACHvertexreverse12_</a> +assign 'vertex' to each vertex in vertex set; +reverse the order of first two vertices </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pmacro">Other macros for polyhedrons</a></h3> +<ul> +<li><a href="qhull.h#getid_">getid_</a> return ID for +a facet, ridge, or vertex </li> +<li><a href="qhull.h#otherfacet_">otherfacet_</a> +return neighboring facet for a ridge in a facet </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="plist">Facetlist +functions</a></h3> +<ul> +<li><a href="poly.c#appendfacet">qh_appendfacet</a> +appends facet to end of qh.facet_list</li> +<li><a href="poly.c#attachnewfacets">qh_attachnewfacets</a> +attach new facets in qh.newfacet_list to the +horizon </li> +<li><a href="poly2.c#findgood">qh_findgood</a> +identify good facets for qh.PRINTgood </li> +<li><a href="poly2.c#findgood_all">qh_findgood_all</a> +identify more good facets for qh.PRINTgood </li> +<li><a href="poly2.c#furthestnext">qh_furthestnext</a> +move facet with furthest of furthest points to +facet_next </li> +<li><a href="poly2.c#initialhull">qh_initialhull</a> +construct the initial hull as a simplex of +vertices </li> +<li><a href="poly2.c#nearcoplanar">qh_nearcoplanar</a> + remove near-inside points from coplanar sets</li> +<li><a href="poly2.c#prependfacet">qh_prependfacet</a> +prepends facet to start of facetlist </li> +<li><a href="user.c#printfacetlist">qh_printfacetlist</a> +print facets in a facetlist</li> +<li><a href="poly2.c#printlists">qh_printlists</a> +print out facet list for debugging </li> +<li><a href="poly.c#removefacet">qh_removefacet</a> +unlinks facet from qh.facet_list</li> +<li><a href="poly2.c#resetlists">qh_resetlists</a> +reset qh.newvertex_list, qh.newfacet_list, and +qh.visible_list </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pfacet">Facet +functions</a></h3> +<ul> +<li><a href="poly2.c#createsimplex">qh_createsimplex</a> +create a simplex of facets from a set of vertices +</li> +<li><a href="poly2.c#findbestlower">qh_findbestlower</a> find best +non-upper, non-flipped facet for point at upperfacet</li> +<li><a href="poly2.c#furthestout">qh_furthestout</a> +make furthest outside point the last point of a +facet's outside set </li> +<li><a href="poly.c#makenew_nonsimplicial">qh_makenew_nonsimplicial</a> +make new facets from ridges of visible facets </li> +<li><a href="poly.c#makenew_simplicial">qh_makenew_simplicial</a> +make new facets for horizon neighbors </li> +<li><a href="poly.c#makenewfacet">qh_makenewfacet</a> +create a facet from vertices and apex </li> +<li><a href="poly2.c#makenewfacets">qh_makenewfacets</a> +make new facets from vertex, horizon facets, and +visible facets </li> +<li><a href="poly.c#makenewplanes">qh_makenewplanes</a> +make new hyperplanes for facets </li> +<li><a href="poly2.c#outcoplanar">qh_outcoplanar</a> +move points from outside set to coplanar set </li> +<li><a href="poly2.c#setvoronoi_all">qh_setvoronoi_all</a> +compute Voronoi centers for all facets </li> +<li><a href="poly2.c#triangulate">qh_triangulate</a> +triangulate non-simplicial facets</li> +<li><a href="poly2.c#triangulate_facet">qh_triangulate_facet</a> +triangulate a non-simplicial facet</li> +<li><a href="poly2.c#triangulate_link">qh_triangulate_link</a> +link facets together from qh_triangulate</li> +<li><a href="poly2.c#triangulate_mirror">qh_triangulate_mirror</a> +delete mirrored facets from qh_triangulate</li> +<li><a href="poly2.c#triangulate_null">qh_triangulate_null</a> +delete null facet from qh_triangulate</li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pvertex">Vertex, +ridge, and point functions</a></h3> +<ul> +<li><a href="poly.c#appendvertex">qh_appendvertex</a> +append vertex to end of qh.vertex_list, </li> +<li><a href="io.c#detvridge">qh_detvridge</a> determine Voronoi +ridge for an input site +<li><a href="io.c#detvridge3">qh_detvridge3</a> determine 3-d Voronoi +ridge for an input site +<li><a href="poly2.c#facet3vertex">qh_facet3vertex</a> +return an oriented vertex set for a 3-d facet </li> +<li><a href="poly.c#facetintersect">qh_facetintersect</a> +return intersection of simplicial facets </li> +<li><a href="poly2.c#initialvertices">qh_initialvertices</a> +return non-singular set of initial vertices </li> +<li><a href="poly2.c#isvertex">qh_isvertex</a> true +if point is in a vertex set </li> +<li><a href="poly2.c#nearvertex">qh_nearvertex</a> +return nearest vertex to point </li> +<li><a href="poly2.c#nextridge3d">qh_nextridge3d</a> +iterate over each ridge and vertex for a 3-d +facet </li> +<li><a href="poly2.c#point">qh_point</a> return point +for a point ID </li> +<li><a href="poly2.c#pointfacet">qh_pointfacet</a> +return temporary set of facets indexed by point +ID </li> +<li><a href="poly.c#pointid">qh_pointid</a> return ID +for a point</li> +<li><a href="poly2.c#pointvertex">qh_pointvertex</a> +return temporary set of vertices indexed by point +ID </li> +<li><a href="poly.c#removevertex">qh_removevertex</a> +unlink vertex from qh.vertex_list, </li> +<li><a href="poly.c#updatevertices">qh_updatevertices</a> +update vertex neighbors and delete interior +vertices </li> +<li><a href="poly2.c#vertexintersect">qh_vertexintersect</a> +intersect two vertex sets </li> +<li><a href="poly2.c#vertexintersect_new">qh_vertexintersect_new</a> +return intersection of two vertex sets </li> +<li><a href="poly2.c#vertexneighbors">qh_vertexneighbors</a> +for each vertex in hull, determine facet +neighbors </li> +<li><a href="poly2.c#vertexsubset">qh_vertexsubset</a> +returns True if vertexsetA is a subset of +vertexsetB </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="phash">Hashtable functions</a></h3> +<ul> +<li><a href="poly2.c#addhash">qh_addhash</a> add hash +element to linear hash table</li> +<li><a href="poly.c#gethash">qh_gethash</a> return +hash value for a set</li> +<li><a href="poly2.c#matchduplicates">qh_matchduplicates</a> +match duplicate ridges in hash table </li> +<li><a href="poly.c#matchneighbor">qh_matchneighbor</a> +try to match subridge of new facet with a +neighbor </li> +<li><a href="poly.c#matchnewfacets">qh_matchnewfacets</a> +match new facets with their new facet neighbors </li> +<li><a href="poly.c#matchvertices">qh_matchvertices</a> +tests whether a facet and hash entry match at a +ridge </li> +<li><a href="poly2.c#newhashtable">qh_newhashtable</a> +allocate a new qh.hash_table </li> +<li><a href="poly2.c#printhashtable">qh_printhashtable</a> +print hash table </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pnew">Allocation and +deallocation functions</a></h3> +<ul> +<li><a href="poly2.c#clearcenters">qh_clearcenters</a> +clear old data from facet->center </li> +<li><a href="poly.c#deletevisible">qh_deletevisible</a> +delete visible facets and vertices </li> +<li><a href="poly.c#delfacet">qh_delfacet</a> free up +the memory occupied by a facet </li> +<li><a href="poly2.c#delridge">qh_delridge</a> delete +ridge</li> +<li><a href="poly2.c#delvertex">qh_delvertex</a> +delete vertex </li> +<li><a href="poly.c#newfacet">qh_newfacet</a> create +and allocate space for a facet </li> +<li><a href="poly.c#newridge">qh_newridge</a> create +and allocate space for a ridge </li> +<li><a href="poly2.c#newvertex">qh_newvertex</a> +create and allocate space for a vertex </li> +</ul> +<h3><a href="qh-poly.htm#TOC">»</a><a name="pcheck">Check +functions</a></h3> +<ul> +<li><a href="poly2.c#check_bestdist">qh_check_bestdist</a> +check that points are not outside of facets </li> +<li><a href="poly2.c#check_maxout">qh_check_maxout</a> +updates qh.max_outside and checks all points +against bestfacet </li> +<li><a href="poly2.c#check_output">qh_check_output</a> +check topological and geometric output</li> +<li><a href="poly2.c#check_point">qh_check_point</a> +check that point is not outside of facet </li> +<li><a href="poly2.c#check_points">qh_check_points</a> +check that all points are inside all facets </li> +<li><a href="poly2.c#checkconvex">qh_checkconvex</a> +check that each ridge in facetlist is convex </li> +<li><a href="poly2.c#checkfacet">qh_checkfacet</a> +check for consistency errors in facet </li> +<li><a href="poly.c#checkflipped">qh_checkflipped</a> +check facet orientation to the interior point </li> +<li><a href="poly2.c#checkflipped_all">qh_checkflipped_all</a> +check facet orientation for a facet list </li> +<li><a href="poly2.c#checkpolygon">qh_checkpolygon</a> +check topological structure </li> +<li><a href="poly2.c#checkvertex">qh_checkvertex</a> +check vertex for consistency </li> +<li><a href="poly2.c#infiniteloop">qh_infiniteloop</a> +report error for a loop of facets </li> +<li><a href="poly2.c#printlists">qh_printlists</a> +print out facet list for debugging </li> +</ul> + + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-qhull.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-qhull.htm new file mode 100644 index 0000000000000000000000000000000000000000..3494d25e52b4c56e5aa80c08b05ce728bc110ee4 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-qhull.htm @@ -0,0 +1,271 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>qhull.c -- top-level functions and basic data types</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> +<hr> + +<h2>qhull.c -- top-level functions and basic data types</h2> +<blockquote> +<p>Qhull implements the Quickhull algorithm for computing +the convex hull. The Quickhull algorithm combines two +well-known algorithms: the 2-d quickhull algorithm and +the n-d beneath-beyond algorithm. See +<a href="../html/index.htm#description">Description of Qhull</a>. </p> +<p>This section provides an index to the top-level +functions and base data types. The top-level header file, <tt>qhull.h</tt>, +contains prototypes for these functions.</p> +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> +• <a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> +• <a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> +• <b>Qhull</b> • <a href="qh-set.htm#TOC">Set</a> +• <a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm#TOC">User</a> +</p> +<h3>Index to <a href="qhull.c">qhull.c</a>, +<a href="qhull.h">qhull.h</a>, and +<a href="unix.c">unix.c</a></h3> +<ul> +<li><a href="#qtype">qhull.h and unix.c data types and +constants</a> </li> +<li><a href="#qmacro">qhull.h other macros</a> </li> +<li><a href="#qfunc">Quickhull routines in call order</a> </li> +<li><a href="#qinit">Top-level routines for initializing and terminating Qhull</a></li> +<li><a href="#qin">Top-level routines for reading and modifying the input</a></li> +<li><a href="#qcall">Top-level routines for calling Qhull</a></li> +<li><a href="#qout">Top-level routines for returning results</a></li> +<li><a href="#qtest">Top-level routines for testing and debugging</a></li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qtype">qhull.h and unix.c +data types and constants</a></h3> +<ul> +<li><a href="qhull.h#flagT">flagT</a> Boolean flag as +a bit </li> +<li><a href="qhull.h#boolT">boolT</a> boolean value, +either True or False </li> +<li><a href="qhull.h#CENTERtype">CENTERtype</a> to +distinguish facet->center </li> +<li><a href="qhull.h#qh_PRINT">qh_PRINT</a> output +formats for printing (qh.PRINTout) </li> +<li><a href="qhull.h#qh_ALL">qh_ALL</a> argument flag +for selecting everything </li> +<li><a href="qhull.h#qh_ERR">qh_ERR</a> Qhull exit +codes for indicating errors </li> +<li><a href="unix.c#prompt">qh_prompt</a> version and long prompt for Qhull</li> +<li><a href="unix.c#prompt2">qh_prompt2</a> synopsis for Qhull</li> +<li><a href="unix.c#prompt3">qh_prompt3</a> concise prompt for Qhull</li> +<li><a href="global.c#qh_version">qh_version</a> version stamp</li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qmacro">qhull.h other +macros</a></h3> +<ul> +<li><a href="qhull_a.h#traceN">traceN</a> print trace +message if <em>qh.IStracing >= N</em>. </li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qfunc">Quickhull +routines in call order</a></h3> +<ul> +<li><a href="unix.c#main">main</a> processes the +command line, calls qhull() to do the work, and +exits </li> +<li><a href="qhull.c#qhull">qh_qhull</a> construct +the convex hull of a set of points </li> +<li><a href="qhull.c#build_withrestart">qh_build_withrestart</a> +allow restarts while calling qh_buildhull</li> +<li><a href="poly2.c#initbuild">qh_initbuild</a> +initialize hull and outside sets with point array</li> +<li><a href="qhull.c#partitionall">qh_partitionall</a> +partition all points into outside sets </li> +<li><a href="qhull.c#buildhull">qh_buildhull</a> +construct a convex hull by adding points one at a +time </li> +<li><a href="qhull.c#nextfurthest">qh_nextfurthest</a> +return next furthest point for processing </li> +<li><a href="qhull.c#buildtracing">qh_buildtracing</a> +trace an iteration of buildhull </li> +<li><a href="qhull.c#addpoint">qh_addpoint</a> add a +point to the convex hull </li> +<li><a href="qhull.c#findhorizon">qh_findhorizon</a> +find the horizon and visible facets for a point </li> +<li><a href="qhull.c#partitionvisible">qh_partitionvisible</a> +partition points from facets in qh.visible_list +to facets in qh.newfacet_list </li> +<li><a href="qhull.c#partitionpoint">qh_partitionpoint</a> +partition a point as inside, coplanar with, or +outside a facet </li> +<li><a href="qhull.c#partitioncoplanar">qh_partitioncoplanar</a> +partition coplanar point into a facet </li> +<li><a href="qhull.c#precision">qh_precision</a> restart on precision errors if not merging and if 'QJn'</li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qinit">Top-level routines for initializing and terminating Qhull (in other modules)</a></h3> +<ul> +<li><a href="global.c#freebuild">qh_freebuild</a> +free memory used by qh_initbuild and qh_buildhull +</li> +<li><a href="global.c#checkflags">qh_checkflags</a> +check flags for multiple frontends to qhull +<li><a href="global.c#freeqhull">qh_freeqhull</a> +free memory used by qhull </li> +<li><a href="global.c#init_A">qh_init_A</a> called +before error handling initialized </li> +<li><a href="global.c#init_B">qh_init_B</a> called +after points are defined </li> +<li><a href="global.c#initflags">qh_initflags</a> set +flags and constants from command line </li> +<li><a href="global.c#restore_qhull">qh_restore_qhull</a> +restores a saved qhull </li> +<li><a href="global.c#save_qhull">qh_save_qhull</a> +saves qhull for later restoring </li> +<li><a href="user.c#user_memsizes">qh_user_memsizes</a> +define additional quick allocation sizes +</li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qin">Top-level routines for reading and modifying the input (in other modules)</a></h3> +<ul> +<li><a href="geom2.c#gram_schmidt">qh_gram_schmidt</a> +implements Gram-Schmidt orthogonalization by rows </li> +<li><a href="geom2.c#projectinput">qh_projectinput</a> +project input along one or more dimensions + +Delaunay projection </li> +<li><a href="geom2.c#randommatrix">qh_randommatrix</a> +generate a random dimXdim matrix in range (-1,1) </li> +<li><a href="io.c#readpoints">qh_readpoints</a> read +points from input </li> +<li><a href="geom2.c#rotateinput">qh_rotateinput</a> rotate +input points using row matrix </li> +<li><a href="geom2.c#scaleinput">qh_scaleinput</a> scale +input points using qh low_bound/high_bound </li> +<li><a href="geom2.c#setdelaunay">qh_setdelaunay</a> project +points to paraboloid for Delaunay triangulation </li> +<li><a href="geom2.c#sethalfspace_all">qh_sethalfspace_all</a> +generate dual for halfspace intersection with interior +point </li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qcall">Top-level routines for calling Qhull (in other modules)</a></h3> +<ul> +<li><a href="qhull.c#addpoint">qh_addpoint</a> add +point to convex hull </li> +<li><a href="poly2.c#findbestfacet">qh_findbestfacet</a> +find facet that is furthest below a point </li> +<li><a href="poly2.c#findfacet_all">qh_findfacet_all</a> +exhaustive search for facet below a point </li> +<li><a href="qhull.c#qhull">qh_qhull</a> construct +the convex hull of a set of points </li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qout">Top-level routines for returning results (in other modules)</a></h3> +<ul> +<li><a href="stat.c#collectstatistics">qh_collectstatistics</a> +collect statistics for qh.facet_list </li> +<li><a href="poly2.c#nearvertex">qh_nearvertex</a> +return nearest vertex to point </li> +<li><a href="poly2.c#point">qh_point</a> return point +for a point ID </li> +<li><a href="poly2.c#pointfacet">qh_pointfacet</a> +return temporary set of facets indexed by point +ID </li> +<li><a href="poly.c#pointid">qh_pointid</a> return ID +for a point</li> +<li><a href="poly2.c#pointvertex">qh_pointvertex</a> +return vertices (if any) for all points</li> +<li><a href="stat.c#printallstatistics">qh_printallstatistics</a> +print all statistics </li> +<li><a href="io.c#printneighborhood">qh_printneighborhood</a> +print neighborhood of one or two facets </li> +<li><a href="qhull.c#printsummary">qh_printsummary</a> +print summary </li> +<li><a href="io.c#produce_output">qh_produce_output</a> +print the results of qh_qhull() </li> +<li><a href="poly2.c#setvoronoi_all">qh_setvoronoi_all</a> +compute Voronoi centers for all facets </li> +</ul> + +<h3><a href="qh-qhull.htm#TOC">»</a><a name="qtest">Top-level routines for testing and debugging (in other modules)</a></h3> +<ul> +<li><a href="io.c#dfacet">dfacet</a> print facet by +ID from debugger </li> +<li><a href="io.c#dvertex">dvertex</a> print vertex +by ID from debugger </li> +<li><a href="poly2.c#check_output">qh_check_output</a> +check output </li> +<li><a href="poly2.c#check_points">qh_check_points</a> +verify that all points are inside the convex hull +</li> +<li><a href="user.c#errexit">qh_errexit</a> report +error with a facet and a ridge</li> +<li><a href="qhull.c#errexit2">qh_errexit2</a> report +error with two facets </li> +<li><a href="user.c#errprint">qh_errprint</a> print +erroneous facets, ridge, and vertex </li> +<li><a href="user.c#printfacetlist">qh_printfacetlist</a> +print all fields for a list of facets </li> +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-set.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-set.htm new file mode 100644 index 0000000000000000000000000000000000000000..20cf4324eb89f4dc341cb736a7c588621e270655 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-set.htm @@ -0,0 +1,304 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>qset.c -- set data type and operations</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm">User</a> +</p> +<hr> + +<h2>qset.c -- set data type and operations</h2> +<blockquote> +<p>Qhull's data structures are constructed from sets. The +functions and macros in qset.c construct, iterate, and +modify these sets. They are the most frequently called +functions in Qhull. For this reason, efficiency is the +primary concern. </p> +<p>In Qhull, a <i>set</i> is represented by an unordered +array of pointers with a maximum size and a NULL +terminator (<a href="qset.h#setT">setT</a>). +Most sets correspond to mathematical sets +(i.e., the pointers are unique). Some sets are sorted to +enforce uniqueness. Some sets are ordered. For example, +the order of vertices in a ridge determine the ridge's +orientation. If you reverse the order of adjacent +vertices, the orientation reverses. Some sets are not +mathematical sets. They may be indexed as an array and +they may include NULL pointers. </p> +<p>The most common operation on a set is to iterate its +members. This is done with a 'FOREACH...' macro. Each set +has a custom macro. For example, 'FOREACHvertex_' +iterates over a set of vertices. Each vertex is assigned +to the variable 'vertex' from the pointer 'vertexp'. </p> +<p>Most sets are constructed by appending elements to the +set. The last element of a set is either NULL or the +index of the terminating NULL for a partially full set. +If a set is full, appending an element copies the set to +a larger array. </p> + +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> • +<a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> • +<a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> +• <a href="qh-qhull.htm#TOC">Qhull</a> • <b>Set</b> +• <a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm#TOC">User</a> +</p> +<h3>Index to <a href="qset.c">qset.c</a> and +<a href="qset.h">qset.h</a></h3> +<ul> +<li><a href="#stype">Data types and constants</a> </li> +<li><a href="#seach">FOREACH macros</a> </li> +<li><a href="#saccess">access and size macros</a> </li> +<li><a href="#sint">internal macros</a> </li> +<li><a href="#saddr">address macros</a><p> </li> + +<li><a href="#snew">Allocation and deallocation functions</a> </li> +<li><a href="#spred">Access and predicate functions</a> +</li> +<li><a href="#sadd">Add functions</a> </li> +<li><a href="#scheck">Check and print functions</a></li> +<li><a href="#scopy">Copy, compact, and zero functions</a></li> +<li><a href="#sdel">Delete functions</a> </li> +<li><a href="#stemp">Temporary set functions</a> </li> +</ul> +<h3><a href="qh-set.htm#TOC">»</a><a name="stype">Data types and +constants</a></h3> +<ul> +<li><a href="qset.h#SETelemsize">SETelemsize</a> size +of a set element in bytes </li> +<li><a href="qset.h#setT">setT</a> a set with a +maximum size and a current size</li> +<li><a href="qhull.h#qh-set">qh global sets</a> +global sets for temporary sets, etc. </li> +</ul> +<h3><a href="qh-set.htm#TOC">»</a><a name="seach">FOREACH macros</a></h3> +<ul> +<li><a href="qset.h#FOREACHelem_">FOREACHelem_</a> +assign 'elem' to each element in a set </li> +<li><a href="qset.h#FOREACHset_">FOREACHset_</a> +assign 'set' to each set in a set of sets </li> +<li><a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +define a FOREACH iterator </li> +<li><a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +define an indexed FOREACH iterator </li> +<li><a href="qset.h#FOREACHsetelementreverse_">FOREACHsetelementreverse_</a> +define a reversed FOREACH iterator </li> +<li><a href="qset.h#FOREACHsetelementreverse12_">FOREACHsetelementreverse12_</a> +define a FOREACH iterator with e[1] and e[0] +reversed </li> +</ul> +<h3><a href="qh-set.htm#TOC">»</a><a name="saccess">Access and +size macros</a></h3> +<ul> +<li><a href="qset.h#SETelem_">SETelem_</a> return the +n'th element of set </li> +<li><a href="qset.h#SETelemt_">SETelemt_</a> return +the n'th element of set as a type</li> +<li><a href="qset.h#SETempty_">SETempty_</a> return +true (1) if set is empty </li> +<li><a href="qset.h#SETfirst_">SETfirst_</a> return +first element of set </li> +<li><a href="qset.h#SETfirstt_">SETfirstt_</a> return +first element of set as a type</li> +<li><a href="qset.h#SETindex_">SETindex_</a> return +index of elem in set </li> +<li><a href="qset.h#SETreturnsize_">SETreturnsize_</a> +return size of a set (normally use <a href="qset.c#setsize">qh_setsize</a>) </li> +<li><a href="qset.h#SETsecond_">SETsecond_</a> return +second element of set </li> +<li><a href="qset.h#SETsecondt_">SETsecondt_</a> +return second element of set as a type</li> +<li><a href="qset.h#SETtruncate_">SETtruncate_</a> +truncate set to size, i.e., qh_settruncate()</li> +</ul> +<h3><a href="qh-set.htm#TOC">»</a><a name="sint">Internal macros</a></h3> +<ul> +<li><a href="qset.c#SETsizeaddr_">SETsizeaddr_</a> +return pointer to end element of a set (indicates +current size) </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="saddr">address macros</a></h3> +<ul> +<li><a href="qset.h#SETaddr_">SETaddr_</a> return +address of a set's elements </li> +<li><a href="qset.h#SETelemaddr_">SETelemaddr_</a> +return address of the n'th element of a set </li> +<li><a href="qset.h#SETref_">SETref_</a> l.h.s. for +modifying the current element in a FOREACH +iteration </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="snew">Allocation and +deallocation functions</a></h3> +<ul> +<li><a href="qset.c#setfree">qh_setfree</a> free the +space occupied by a set </li> +<li><a href="qset.c#setfree2">qh_setfree2</a> free a +set and its elements </li> +<li><a href="qset.c#setfreelong">qh_setfreelong</a> +free a set only if it is in long memory </li> +<li><a href="qset.c#setnew">qh_setnew</a> create a new +set </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="spred">Access and +predicate functions </a></h3> +<ul> +<li><a href="qset.c#setequal">qh_setequal</a> return 1 +if two sorted sets are equal </li> +<li><a href="qset.c#setequal_except">qh_setequal_except</a> +return 1 if two sorted sets are equal except for +an element </li> +<li><a href="qset.c#setequal_skip">qh_setequal_skip</a> +return 1 if two sorted sets are equal except for +a pair of skipped elements </li> +<li><a href="qset.c#setequal_skip">qh_setequal_skip</a> +return 1 if two sorted sets are equal except for +a pair of skipped elements </li> +<li><a href="qset.c#setin">qh_setin</a> return 1 if an +element is in a set </li> +<li><a href="qset.c#setindex">qh_setindex</a> return +the index of an element in a set </li> +<li><a href="qset.c#setlast">qh_setlast</a> return +last element of a set</li> +<li><a href="qset.c#setsize">qh_setsize</a> returns +the size of a set </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="sadd">Add functions</a></h3> +<ul> +<li><a href="qset.c#setaddnth">qh_setaddnth</a> add a +element as n'th element of sorted or unsorted set +</li> +<li><a href="qset.c#setaddsorted">qh_setaddsorted</a> +add an element to a sorted set </li> +<li><a href="qset.c#setappend">qh_setappend</a> append +an element to a set </li> +<li><a href="qset.c#setappend_set">qh_setappend_set</a> +append a set of elements to a set </li> +<li><a href="qset.c#setappend2ndlast">qh_setappend2ndlast</a> +add an element as the next to the last element in +a set </li> +<li><a href="qset.c#setlarger">qh_setlarger</a> return +a larger set with the same elements</li> +<li><a href="qset.c#setreplace">qh_setreplace</a> +replace one element with another in a set</li> +<li><a href="qset.c#setunique">qh_setunique</a> add an +element if it is not already in a set </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="scheck">Check and print functions</a></h3> +<ul> +<li><a href="qset.c#setcheck">qh_setcheck</a> check a +set for validity </li> +<li><a href="qset.c#setprint">qh_setprint</a> print a +set's elements to fp </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="scopy">Copy, compact, and zero functions</a></h3> +<ul> +<li><a href="qset.c#setcompact">qh_setcompact</a> +compact NULLs from an unsorted set </li> +<li><a href="qset.c#setcopy">qh_setcopy</a> make a +copy of a sorted or unsorted set </li> +<li><a href="qset.c#setduplicate">qh_setduplicate</a> +duplicate a set and its elements </li> +<li><a href="qset.c#settruncate">qh_settruncate</a> +truncate a set to size elements </li> +<li><a href="qset.c#setzero">qh_setzero</a> zero the +remainder of a set </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="sdel">Delete functions</a></h3> +<ul> +<li><a href="qset.c#setdel">qh_setdel</a> delete an +element from an unsorted set. </li> +<li><a href="qset.c#setdellast">qh_setdellast</a> +delete and return last element from a set</li> +<li><a href="qset.c#setdelnth">qh_setdelnth</a> delete +and return nth element from an unsorted set </li> +<li><a href="qset.c#setdelnthsorted">qh_setdelnthsorted</a> +delete and return nth element from a sorted set </li> +<li><a href="qset.c#setdelsorted">qh_setdelsorted</a> +delete an element from a sorted set </li> +<li><a href="qset.c#setnew_delnthsorted">qh_setnew_delnthsorted</a> +create a sorted set not containing the nth +element </li> +</ul> + +<h3><a href="qh-set.htm#TOC">»</a><a name="stemp">Temporary set functions</a></h3> +<ul> +<li><a href="qset.c#settemp">qh_settemp</a> return a +temporary set and append it qhmem.tempstack</li> +<li><a href="qset.c#settempfree">qh_settempfree</a> +free and pop a set from qhmem.tempstack</li> +<li><a href="qset.c#settempfree_all">qh_settempfree_all</a> +free all sets in qhmem.tempstack </li> +<li><a href="qset.c#settemppop">qh_settemppop</a> pop +a set from qhmem.tempstack (makes it permanent) </li> +<li><a href="qset.c#settemppush">qh_settemppush</a> +push a set unto qhmem.tempstack (makes it +temporary) </li> +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-stat.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-stat.htm new file mode 100644 index 0000000000000000000000000000000000000000..ec5fb115a6465eec02e755db22c063888a504bbd --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-stat.htm @@ -0,0 +1,161 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>stat.c -- statistical operations</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm#TOC">Stat</a> • <a href="qh-user.htm">User</a> +</p> +<hr> + +<h2>stat.c -- statistical operations</h2> +<blockquote> +<p>Qhull records many statistics. These functions and +macros make it inexpensive to add a statistic. +<p>As with Qhull's global variables, the statistics data structure is +accessed by a macro, 'qhstat'. If qh_QHpointer is defined, the macro +is 'qh_qhstat->', otherwise the macro is 'qh_qhstat.'. +Statistics +may be turned off in user.h. If so, all but the 'zz' +statistics are ignored.</p> +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> +• <a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> +• <a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> +• <a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> +• <b>Stat</b> • <a href="qh-user.htm#TOC">User</a> +</p> +<h3>Index to <a href="stat.c">stat.c</a> and +<a href="stat.h">stat.h</a></h3> +<ul> +<li><a href="#ttype">stat.h types</a> </li> +<li><a href="#tconst">stat.h constants</a> </li> +<li><a href="#tmacro">stat.h macros</a> </li> +<li><a href="#tfunc">stat.c functions</a> </li> +</ul> + +<h3><a href="qh-stat.htm#TOC">»</a><a name="ttype">stat.h types</a></h3> +<ul> +<li><a href="stat.h#intrealT">intrealT</a> union of +integer and real</li> +<li><a href="stat.h#qhstat">qhstat</a> global data +structure for statistics </li> +</ul> +<h3><a href="qh-stat.htm#TOC">»</a><a name="tconst">stat.h +constants</a></h3> +<ul> +<li><a href="stat.h#KEEPstatistics">qh_KEEPstatistics</a> 0 turns off most statistics</li> +<li><a href="stat.h#statistics">Z..., W...</a> integer (Z) and real (W) statistics +</li> +<li><a href="stat.h#ZZstat">ZZstat</a> Z.../W... statistics that +remain defined if qh_KEEPstatistics=0 +</li> +<li><a href="stat.h#ztype">ztype</a> zdoc, zinc, etc. +for definining statistics </li> +</ul> +<h3><a href="qh-stat.htm#TOC">»</a><a name="tmacro">stat.h macros</a></h3> +<ul> +<li><a href="stat.h#MAYdebugx">MAYdebugx</a> called +frequently for error trapping </li> +<li><a href="stat.h#zadd_">zadd_/wadd_</a> add value +to an integer or real statistic </li> +<li><a href="stat.h#zdef_">zdef_</a> define a +statistic </li> +<li><a href="stat.h#zinc_">zinc_</a> increment an +integer statistic </li> +<li><a href="stat.h#zmax_">zmax_/wmax_</a> update +integer or real maximum statistic </li> +<li><a href="stat.h#zmin_">zmin_/wmin_</a> update +integer or real minimum statistic </li> +<li><a href="stat.h#zval_">zval_/wval_</a> set or +return value of a statistic </li> +</ul> + +<h3><a href="qh-stat.htm#TOC">»</a><a name="tfunc">stat.c +functions</a></h3> +<ul> +<li><a href="stat.c#allstatA">qh_allstatA</a> define +statistics in groups of 20 </li> +<li><a href="stat.c#allstatistics">qh_allstatistics</a> +reset printed flag for all statistics </li> +<li><a href="stat.c#collectstatistics">qh_collectstatistics</a> +collect statistics for qh.facet_list </li> +<li><a href="stat.c#freestatistics">qh_freestatistics</a> +free memory used for statistics </li> +<li><a href="stat.c#initstatistics">qh_initstatistics</a> +allocate and initialize statistics </li> +<li><a href="stat.c#newstats">qh_newstats</a> returns +True if statistics for zdoc </li> +<li><a href="stat.c#nostatistic">qh_nostatistic</a> +true if no statistic to print </li> +<li><a href="stat.c#printallstatistics">qh_printallstatistics</a> +print all statistics </li> +<li><a href="stat.c#printstatistics">qh_printstatistics</a> +print statistics to a file </li> +<li><a href="stat.c#printstatlevel">qh_printstatlevel</a> +print level information for a statistic </li> +<li><a href="stat.c#printstats">qh_printstats</a> +print statistics for a zdoc group </li> +<li><a href="stat.c#stddev">qh_stddev</a> compute the +standard deviation and average from statistics </li> +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-user.htm b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-user.htm new file mode 100644 index 0000000000000000000000000000000000000000..4c92321719957adc0b0c508fd3cccc076e95c86c --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qh-user.htm @@ -0,0 +1,234 @@ +<!-- Do not edit with Front Page, it adds too many spaces --> +<html> +<head> +<meta http-equiv="Content-Type" +content="text/html; charset=iso-8859-1"> +<title>user.c -- user-definable operations</title> +</head> + +<body> +<!-- Navigation links --> +<p><a name="TOP"><b>Up:</b></a> <a +href="http://www.qhull.org">Home page</a> for Qhull<br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual</a>: Table of Contents <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a><br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • <a href="qh-globa.htm">Global</a> +• <a href="qh-io.htm">Io</a> • <a href="qh-mem.htm">Mem</a> +• <a href="qh-merge.htm">Merge</a> • <a href="qh-poly.htm">Poly</a> +• <a href="qh-qhull.htm">Qhull</a> • <a href="qh-set.htm">Set</a> +• <a href="qh-stat.htm">Stat</a> • <a href="qh-user.htm#TOC">User</a> +</p> +<hr> +<h2>user.c -- user-definable operations</h2> +<blockquote> +<p>This section contains functions and constants that the +user may want to change. </p> + +</blockquote> +<p><b>Copyright © 1995-2003 The Geometry Center, Minneapolis MN</b></p> +<hr> +<p><a href="#TOP">»</a> <a href="qh-geom.htm#TOC">Geom</a> +<a name="TOC">•</a> <a href="qh-globa.htm#TOC">Global</a> +• <a href="qh-io.htm#TOC">Io</a> • <a href="qh-mem.htm#TOC">Mem</a> +• <a href="qh-merge.htm#TOC">Merge</a> • <a href="qh-poly.htm#TOC">Poly</a> +• <a href="qh-qhull.htm#TOC">Qhull</a> • <a href="qh-set.htm#TOC">Set</a> +• <a href="qh-stat.htm#TOC">Stat</a> • <b>User</b> +</p> +<h3>Index to <a href="user.c">user.c</a> and +<a href="user.h">user.h</a></h3> +<ul> +<li><a href="#utype">user.h data types and +configuration macros</a> </li> +<li><a href="#ujoggle">joggle constants</a></li> +<li><a href="#uperform">performance related constants</a></li> +<li><a href="#umemory">memory constants</a></li> +<li><a href="#ucond">conditional compilation</a></li> +<li><a href="#umerge">merge constants</a> </li> +<li><a href="#ufunc">user.c functions</a> </li> +</ul> +<h3><a href="qh-user.htm#TOC">»</a><a name="utype">user.h data +types and configuration macros</a></h3> +<ul> +<li><a href="user.h#realT">realT, qh_REAL...</a> size +of floating point numbers </li> +<li><a href="user.h#CPUclock">qh_CPUclock</a> clock() +function for reporting the total time spent by +Qhull </li> +<li><a href="user.h#RANDOM">qh_RANDOM...</a> random +number generator </li> +</ul> + +<h3><a href="qh-user.htm#TOC">»</a><a name="udef">definition constants</a></h3> +<ul> +<li><a href="user.h#DEFAULTbox">qh_DEFAULTbox</a> +define default box size for rbox, 'Qbb', and 'QbB' (Geomview expects 0.5) </li> +<li><a href="user.h#INFINITE">qh_INFINITE</a> on +output, indicates Voronoi center at infinity </li> +<li><a href="user.h#ORIENTclock">qh_ORIENTclock</a> +define convention for orienting facets</li> +<li><a href="user.h#ZEROdelaunay">qh_ZEROdelaunay</a> +define facets that are ignored in Delaunay triangulations</li> +</ul> + +<h3><a href="qh-user.htm#TOC">»</a><a name="ujoggle">joggle constants</a></h3> +<ul> +<li><a href="user.h#JOGGLEagain">qh_JOGGLEagain</a> +how often to retry before using qh_JOGGLEmaxincrease +again </li> +<li><a href="user.h#JOGGLEdefault">qh_JOGGLEdefault</a> +default value for qh.JOGGLEmax for 'QP' </li> +<li><a href="user.h#JOGGLEincrease">qh_JOGGLEincrease</a> +factor to increase qh.JOGGLEmax on retrys for +'QPn' </li> +<li><a href="user.h#JOGGLEmaxincrease">qh_JOGGLEmaxincrease</a> max +for increasing qh.JOGGLEmax relative to +qh.MAXwidth </li> +<li><a href="user.h#JOGGLEretry">qh_JOGGLEmaxretry</a> +report error if this many retries </li> +<li><a href="user.h#JOGGLEretry">qh_JOGGLEretry</a> +how often to retry before using qh_JOGGLEmax </li> +</ul> + +<h3><a href="qh-user.htm#TOC">»</a><a name="uperform">performance +related constants</a></h3> +<ul> +<li><a href="user.h#HASHfactor">qh_HASHfactor</a> +total/used hash slots </li> +<li><a href="user.h#INITIALmax">qh_INITIALmax</a> if +dim >= qh_INITIALmax, use min/max coordinate +points for initial simplex </li> +<li><a href="user.h#INITIALsearch">qh_INITIALsearch</a> +if qh.INITIALmax, search points up to this +dimension </li> +<li><a href="user.h#NOtrace">qh_NOtrace</a> disallow +tracing </li> +<li><a href="user.h#VERIFYdirect">qh_VERIFYdirect</a> +'Tv' verifies all <em>points X facets</em> if op +count is smaller </li> +</ul> + +<h3><a href="qh-user.htm#TOC">»</a><a name="umemory">memory constants</a></h3> +<ul> +<li><a href="user.h#MEMalign">qh_MEMalign</a> memory +alignment for qh_meminitbuffers() in global.c </li> +<li><a href="user.h#MEMbufsize">qh_MEMbufsize</a> +size of additional memory buffers </li> +<li><a href="user.h#MEMinitbuf">qh_MEMinitbuf</a> +size of initial memory buffer </li> +</ul> + +<h3><a href="qh-user.htm#TOC">»</a><a name="ucond">conditional compilation</a></h3> +<ul> +<li><a href="user.h#compiler">compiler</a> defined symbols, +e.g., _STDC_ and _cplusplus + +<li><a href="user.h#COMPUTEfurthest">qh_COMPUTEfurthest</a> + compute furthest distance to an outside point instead of storing it with the facet +<li><a href="user.h#KEEPstatistics">qh_KEEPstatistics</a> + enable statistic gathering and reporting with option 'Ts' +<li><a href="user.h#MAXoutside">qh_MAXoutside</a> +record outer plane for each facet +<li><a href="user.h#NOmerge">qh_NOmerge</a> +disable facet merging +<li><a href="user.h#NOtrace">qh_NOtrace</a> +disable tracing with option 'T4' +<li><a href="user.h#QHpointer">qh_QHpointer</a> +access global data with pointer or static structure +<li><a href="user.h#QUICKhelp">qh_QUICKhelp</a> +use abbreviated help messages, e.g., for degenerate inputs +</ul> + +<h3><a href="qh-user.htm#TOC">»</a><a name="umerge">merge +constants</a></h3> +<ul> +<li><a href="user.h#BESTcentrum">qh_BESTcentrum</a> +when does qh_findbestneighbor() test centrums? </li> +<li><a href="user.h#BESTnonconvex">qh_BESTnonconvex</a> +when does qh_findbestneighbor() test nonconvex +ridges only? </li> +<li><a href="user.h#COPLANARratio">qh_COPLANARratio</a> +what is qh.MINvisible? </li> +<li><a href="user.h#DIMreduceBuild">qh_DIMreduceBuild</a> +max dimension for vertex reduction </li> +<li><a href="user.h#DIMmergeVertex">qh_DIMmergeVertex</a> +max dimension for vertex merging </li> +<li><a href="user.h#DISToutside">qh_DISToutside</a> +when is a point clearly outside of a facet for qh_findbestnew and qh_partitionall</li> +<li><a href="user.h#MAXnarrow">qh_MAXnarrow</a> max. +cosine for qh.NARROWhull </li> +<li><a href="user.h#MAXnewcentrum">qh_MAXnewcentrum</a> +when does qh_reducevertices_centrum() reset the +centrum? </li> +<li><a href="user.h#MAXnewmerges">qh_MAXnewmerges</a> +when does qh_merge_nonconvex() call +qh_reducevertices_centrums? </li> +<li><a href="user.h#RATIOnearinside">qh_RATIOnearinside</a> +ratio for retaining inside points for +qh_check_maxout() </li> +<li><a href="user.h#SEARCHdist">qh_SEARCHdist</a> +when is facet coplanar with the best facet for qh_findbesthorizon</li> +<li><a href="user.h#USEfindbestnew">qh_USEfindbestnew</a> +when to use qh_findbestnew for qh_partitionpoint()</li> +<li><a href="user.h#WIDEcoplanar">qh_WIDEcoplanar</a> +what is a WIDEfacet? </li> +<li><a href="user.h#WARNnarrow">qh_WARNnarrow</a> +max. cosine to warn about qh.NARROWhull </li> +</ul> + +<h3><a href="qh-user.htm#TOC">»</a><a name="ufunc">user.c +functions</a></h3> +<ul> +<li><a href="user.c#errexit">qh_errexit</a> report +error and exit qhull()</li> +<li><a href="user.c#errprint">qh_errprint</a> print +information about facets and ridges </li> +<li><a href="user.c#new_qhull">qh_new_qhull</a> call qhull on an array +of points</li> +<li><a href="user.c#printfacetlist">qh_printfacetlist</a> +print all fields of all facets </li> +</ul> + +<p><!-- Navigation links --> </p> +<hr> +<p><b>Up:</b> +<a href="http://www.qhull.org">Home page for +Qhull</a> <br> +<b>Up:</b> <a href="../html/index.htm#TOC">Qhull manual: Table of Contents</a> <br> +<b>Up:</b> <a href="../html/qh-quick.htm#programs">Programs</a> +• <a href="../html/qh-quick.htm#options">Options</a> +• <a href="../html/qh-opto.htm#output">Output</a> +• <a href="../html/qh-optf.htm#format">Formats</a> +• <a href="../html/qh-optg.htm#geomview">Geomview</a> +• <a href="../html/qh-optp.htm#print">Print</a> +• <a href="../html/qh-optq.htm#qhull">Qhull</a> +• <a href="../html/qh-optc.htm#prec">Precision</a> +• <a href="../html/qh-optt.htm#trace">Trace</a><br> +<b>Up:</b> <a href="../html/qh-in.htm#TOC">Qhull internals: Table of Contents</a> <br> +<b>To:</b> <a href="index.htm">Qhull functions</a>, macros, and data structures<br> +<b>To:</b> <a href="qh-geom.htm">Geom</a> • +<a href="qh-globa.htm">Global</a> • <a href="qh-io.htm">Io</a> +• <a href="qh-mem.htm">Mem</a> • <a href="qh-merge.htm">Merge</a> +• <a href="qh-poly.htm">Poly</a> • <a href="qh-qhull.htm#TOC">Qhull</a> +• <a href="qh-set.htm">Set</a> • <a href="qh-stat.htm">Stat</a> +• <a href="qh-user.htm">User</a><br> +</p> +<p><!-- GC common information --> </p> +<hr> +<p><a href="http://www.geom.uiuc.edu/"><img +src="../html/qh--geom.gif" align="middle" width="40" height="40"></a><i>The +Geometry Center Home Page </i></p> +<p>Comments to: <a href=mailto:qhull@qhull.org>qhull@qhull.org</a> +</a><br> +Created: May 2, 1997 --- <!-- hhmts start --> Last modified: see top <!-- hhmts end --> </p> +</body> +</html> diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull.c new file mode 100644 index 0000000000000000000000000000000000000000..55a152103227f4827db814905e7275f68ae0b92d --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull.c @@ -0,0 +1,1396 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qhull.c + Quickhull algorithm for convex hulls + + qhull() and top-level routines + + see qh-qhull.htm, qhull.h, unix.c + + see qhull_a.h for internal functions + + copyright (c) 1993-2003 The Geometry Center +*/ + +#include "qhull_a.h" + +/*============= functions in alphabetic order after qhull() =======*/ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="qhull">-</a> + + qh_qhull() + compute DIM3 convex hull of qh.num_points starting at qh.first_point + qh contains all global options and variables + + returns: + returns polyhedron + qh.facet_list, qh.num_facets, qh.vertex_list, qh.num_vertices, + + returns global variables + qh.hulltime, qh.max_outside, qh.interior_point, qh.max_vertex, qh.min_vertex + + returns precision constants + qh.ANGLEround, centrum_radius, cos_max, DISTround, MAXabs_coord, ONEmerge + + notes: + unless needed for output + qh.max_vertex and qh.min_vertex are max/min due to merges + + see: + to add individual points to either qh.num_points + use qh_addpoint() + + if qh.GETarea + qh_produceoutput() returns qh.totarea and qh.totvol via qh_getarea() + + design: + record starting time + initialize hull and partition points + build convex hull + unless early termination + update facet->maxoutside for vertices, coplanar, and near-inside points + error if temporary sets exist + record end time +*/ + +void qh_qhull (void) { + int numoutside; + + qh hulltime= qh_CPUclock; + if (qh RERUN || qh JOGGLEmax < REALmax/2) + qh_build_withrestart(); + else { + qh_initbuild(); + qh_buildhull(); + } + if (!qh STOPpoint && !qh STOPcone) { + if (qh ZEROall_ok && !qh TESTvneighbors && qh MERGEexact) + qh_checkzero( qh_ALL); + if (qh ZEROall_ok && !qh TESTvneighbors && !qh WAScoplanar) { + trace2((qh ferr, "qh_qhull: all facets are clearly convex and no coplanar points. Post-merging and check of maxout not needed.\n")); + qh DOcheckmax= False; + }else { + if (qh MERGEexact || (qh hull_dim > qh_DIMreduceBuild && qh PREmerge)) + qh_postmerge ("First post-merge", qh premerge_centrum, qh premerge_cos, + (qh POSTmerge ? False : qh TESTvneighbors)); + else if (!qh POSTmerge && qh TESTvneighbors) + qh_postmerge ("For testing vertex neighbors", qh premerge_centrum, + qh premerge_cos, True); + if (qh POSTmerge) + qh_postmerge ("For post-merging", qh postmerge_centrum, + qh postmerge_cos, qh TESTvneighbors); + if (qh visible_list == qh facet_list) { /* i.e., merging done */ + qh findbestnew= True; + qh_partitionvisible (/*visible_list, newfacet_list*/ !qh_ALL, &numoutside); + qh findbestnew= False; + qh_deletevisible (/*qh visible_list*/); + qh_resetlists (False, qh_RESETvisible /*qh visible_list newvertex_list newfacet_list */); + } + } + if (qh DOcheckmax){ + if (qh REPORTfreq) { + qh_buildtracing (NULL, NULL); + fprintf (qh ferr, "\nTesting all coplanar points.\n"); + } + qh_check_maxout(); + } + if (qh KEEPnearinside && !qh maxoutdone) + qh_nearcoplanar(); + } + if (qh_setsize ((setT*)qhmem.tempstack) != 0) { + fprintf (qh ferr, "qhull internal error (qh_qhull): temporary sets not empty (%d)\n", + qh_setsize ((setT*)qhmem.tempstack)); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + qh hulltime= qh_CPUclock - qh hulltime; + qh QHULLfinished= True; + trace1((qh ferr, "qh_qhull: algorithm completed\n")); +} /* qhull */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="addpoint">-</a> + + qh_addpoint( furthest, facet, checkdist ) + add point (usually furthest point) above facet to hull + if checkdist, + check that point is above facet. + if point is not outside of the hull, uses qh_partitioncoplanar() + assumes that facet is defined by qh_findbestfacet() + else if facet specified, + assumes that point is above facet (major damage if below) + for Delaunay triangulations, + Use qh_setdelaunay() to lift point to paraboloid and scale by 'Qbb' if needed + Do not use options 'Qbk', 'QBk', or 'QbB' since they scale the coordinates. + + returns: + returns False if user requested an early termination + qh.visible_list, newfacet_list, delvertex_list, NEWfacets may be defined + updates qh.facet_list, qh.num_facets, qh.vertex_list, qh.num_vertices + clear qh.maxoutdone (will need to call qh_check_maxout() for facet->maxoutside) + if unknown point, adds a pointer to qh.other_points + do not deallocate the point's coordinates + + notes: + assumes point is near its best facet and not at a local minimum of a lens + distributions. Use qh_findbestfacet to avoid this case. + uses qh.visible_list, qh.newfacet_list, qh.delvertex_list, qh.NEWfacets + + see also: + qh_triangulate() -- triangulate non-simplicial facets + + design: + check point in qh.first_point/.num_points + if checkdist + if point not above facet + partition coplanar point + exit + exit if pre STOPpoint requested + find horizon and visible facets for point + make new facets for point to horizon + make hyperplanes for point + compute balance statistics + match neighboring new facets + update vertex neighbors and delete interior vertices + exit if STOPcone requested + merge non-convex new facets + if merge found, many merges, or 'Qf' + use qh_findbestnew() instead of qh_findbest() + partition outside points from visible facets + delete visible facets + check polyhedron if requested + exit if post STOPpoint requested + reset working lists of facets and vertices +*/ +boolT qh_addpoint (pointT *furthest, facetT *facet, boolT checkdist) { + int goodvisible, goodhorizon; + vertexT *vertex; + facetT *newfacet; + realT dist, newbalance, pbalance; + boolT isoutside= False; + int numpart, numpoints, numnew, firstnew; + + qh maxoutdone= False; + if (qh_pointid (furthest) == -1) + qh_setappend (&qh other_points, furthest); + if (!facet) { + fprintf (qh ferr, "qh_addpoint: NULL facet. Need to call qh_findbestfacet first\n"); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + if (checkdist) { + facet= qh_findbest (furthest, facet, !qh_ALL, !qh_ISnewfacets, !qh_NOupper, + &dist, &isoutside, &numpart); + zzadd_(Zpartition, numpart); + if (!isoutside) { + zinc_(Znotmax); /* last point of outsideset is no longer furthest. */ + facet->notfurthest= True; + qh_partitioncoplanar (furthest, facet, &dist); + return True; + } + } + qh_buildtracing (furthest, facet); + if (qh STOPpoint < 0 && qh furthest_id == -qh STOPpoint-1) { + facet->notfurthest= True; + return False; + } + qh_findhorizon (furthest, facet, &goodvisible, &goodhorizon); + if (qh ONLYgood && !(goodvisible+goodhorizon) && !qh GOODclosest) { + zinc_(Znotgood); + facet->notfurthest= True; + /* last point of outsideset is no longer furthest. This is ok + since all points of the outside are likely to be bad */ + qh_resetlists (False, qh_RESETvisible /*qh visible_list newvertex_list newfacet_list */); + return True; + } + zzinc_(Zprocessed); + firstnew= qh facet_id; + vertex= qh_makenewfacets (furthest /*visible_list, attaches if !ONLYgood */); + qh_makenewplanes (/* newfacet_list */); + numnew= qh facet_id - firstnew; + newbalance= numnew - (realT) (qh num_facets-qh num_visible) + * qh hull_dim/qh num_vertices; + wadd_(Wnewbalance, newbalance); + wadd_(Wnewbalance2, newbalance * newbalance); + if (qh ONLYgood + && !qh_findgood (qh newfacet_list, goodhorizon) && !qh GOODclosest) { + FORALLnew_facets + qh_delfacet (newfacet); + qh_delvertex (vertex); + qh_resetlists (True, qh_RESETvisible /*qh visible_list newvertex_list newfacet_list */); + zinc_(Znotgoodnew); + facet->notfurthest= True; + return True; + } + if (qh ONLYgood) + qh_attachnewfacets(/*visible_list*/); + qh_matchnewfacets(); + qh_updatevertices(); + if (qh STOPcone && qh furthest_id == qh STOPcone-1) { + facet->notfurthest= True; + return False; /* visible_list etc. still defined */ + } + qh findbestnew= False; + if (qh PREmerge || qh MERGEexact) { + qh_premerge (vertex, qh premerge_centrum, qh premerge_cos); + if (qh_USEfindbestnew) + qh findbestnew= True; + else { + FORALLnew_facets { + if (!newfacet->simplicial) { + qh findbestnew= True; /* use qh_findbestnew instead of qh_findbest*/ + break; + } + } + } + }else if (qh BESToutside) + qh findbestnew= True; + qh_partitionvisible (/*visible_list, newfacet_list*/ !qh_ALL, &numpoints); + qh findbestnew= False; + qh findbest_notsharp= False; + zinc_(Zpbalance); + pbalance= numpoints - (realT) qh hull_dim /* assumes all points extreme */ + * (qh num_points - qh num_vertices)/qh num_vertices; + wadd_(Wpbalance, pbalance); + wadd_(Wpbalance2, pbalance * pbalance); + qh_deletevisible (/*qh visible_list*/); + zmax_(Zmaxvertex, qh num_vertices); + qh NEWfacets= False; + if (qh IStracing >= 4) { + if (qh num_facets < 2000) + qh_printlists(); + qh_printfacetlist (qh newfacet_list, NULL, True); + qh_checkpolygon (qh facet_list); + }else if (qh CHECKfrequently) { + if (qh num_facets < 50) + qh_checkpolygon (qh facet_list); + else + qh_checkpolygon (qh newfacet_list); + } + if (qh STOPpoint > 0 && qh furthest_id == qh STOPpoint-1) + return False; + qh_resetlists (True, qh_RESETvisible /*qh visible_list newvertex_list newfacet_list */); + /* qh_triangulate(); to test qh.TRInormals */ + trace2((qh ferr, "qh_addpoint: added p%d new facets %d new balance %2.2g point balance %2.2g\n", + qh_pointid (furthest), numnew, newbalance, pbalance)); + return True; +} /* addpoint */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="build_withrestart">-</a> + + qh_build_withrestart() + allow restarts due to qh.JOGGLEmax while calling qh_buildhull() + qh.FIRSTpoint/qh.NUMpoints is point array + it may be moved by qh_joggleinput() +*/ +void qh_build_withrestart (void) { + int restart; + + qh ALLOWrestart= True; + while (True) { + restart= setjmp (qh restartexit); /* simple statement for CRAY J916 */ + if (restart) { /* only from qh_precision() */ + zzinc_(Zretry); + wmax_(Wretrymax, qh JOGGLEmax); + qh ERREXITcalled= False; + qh STOPcone= True; /* if break, prevents normal output */ + } + if (!qh RERUN && qh JOGGLEmax < REALmax/2) { + if (qh build_cnt > qh_JOGGLEmaxretry) { + fprintf(qh ferr, "\n\ +qhull precision error: %d attempts to construct a convex hull\n\ + with joggled input. Increase joggle above 'QJ%2.2g'\n\ + or modify qh_JOGGLE... parameters in user.h\n", + qh build_cnt, qh JOGGLEmax); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + if (qh build_cnt && !restart) + break; + }else if (qh build_cnt && qh build_cnt >= qh RERUN) + break; + qh STOPcone= False; + qh_freebuild (True); /* first call is a nop */ + qh build_cnt++; + if (!qh qhull_optionsiz) + qh qhull_optionsiz= strlen (qh qhull_options); + else { + qh qhull_options [qh qhull_optionsiz]= '\0'; + qh qhull_optionlen= 80; + } + qh_option("_run", &qh build_cnt, NULL); + if (qh build_cnt == qh RERUN) { + qh IStracing= qh TRACElastrun; /* duplicated from qh_initqhull_globals */ + if (qh TRACEpoint != -1 || qh TRACEdist < REALmax/2 || qh TRACEmerge) { + qh TRACElevel= (qh IStracing? qh IStracing : 3); + qh IStracing= 0; + } + qhmem.IStracing= qh IStracing; + } + if (qh JOGGLEmax < REALmax/2) + qh_joggleinput(); + qh_initbuild(); + qh_buildhull(); + if (qh JOGGLEmax < REALmax/2 && !qh MERGING) + qh_checkconvex (qh facet_list, qh_ALGORITHMfault); + } + qh ALLOWrestart= False; +} /* qh_build_withrestart */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="buildhull">-</a> + + qh_buildhull() + construct a convex hull by adding outside points one at a time + + returns: + + notes: + may be called multiple times + checks facet and vertex lists for incorrect flags + to recover from STOPcone, call qh_deletevisible and qh_resetlists + + design: + check visible facet and newfacet flags + check newlist vertex flags and qh.STOPcone/STOPpoint + for each facet with a furthest outside point + add point to facet + exit if qh.STOPcone or qh.STOPpoint requested + if qh.NARROWhull for initial simplex + partition remaining outside points to coplanar sets +*/ +void qh_buildhull(void) { + facetT *facet; + pointT *furthest; + vertexT *vertex; + int id; + + trace1((qh ferr, "qh_buildhull: start build hull\n")); + FORALLfacets { + if (facet->visible || facet->newfacet) { + fprintf (qh ferr, "qhull internal error (qh_buildhull): visible or new facet f%d in facet list\n", + facet->id); + qh_errexit (qh_ERRqhull, facet, NULL); + } + } + FORALLvertices { + if (vertex->newlist) { + fprintf (qh ferr, "qhull internal error (qh_buildhull): new vertex f%d in vertex list\n", + vertex->id); + qh_errprint ("ERRONEOUS", NULL, NULL, NULL, vertex); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + id= qh_pointid (vertex->point); + if ((qh STOPpoint>0 && id == qh STOPpoint-1) || + (qh STOPpoint<0 && id == -qh STOPpoint-1) || + (qh STOPcone>0 && id == qh STOPcone-1)) { + trace1((qh ferr,"qh_buildhull: stop point or cone P%d in initial hull\n", id)); + return; + } + } + qh facet_next= qh facet_list; /* advance facet when processed */ + while ((furthest= qh_nextfurthest (&facet))) { + qh num_outside--; /* if ONLYmax, furthest may not be outside */ + if (!qh_addpoint (furthest, facet, qh ONLYmax)) + break; + } + if (qh NARROWhull) /* move points from outsideset to coplanarset */ + qh_outcoplanar( /* facet_list */ ); + if (qh num_outside && !furthest) { + fprintf (qh ferr, "qhull internal error (qh_buildhull): %d outside points were never processed.\n", qh num_outside); + qh_errexit (qh_ERRqhull, NULL, NULL); + } + trace1((qh ferr, "qh_buildhull: completed the hull construction\n")); +} /* buildhull */ + + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="buildtracing">-</a> + + qh_buildtracing( furthest, facet ) + trace an iteration of qh_buildhull() for furthest point and facet + if !furthest, prints progress message + + returns: + tracks progress with qh.lastreport + updates qh.furthest_id (-3 if furthest is NULL) + also resets visit_id, vertext_visit on wrap around + + see: + qh_tracemerging() + + design: + if !furthest + print progress message + exit + if 'TFn' iteration + print progress message + else if tracing + trace furthest point and facet + reset qh.visit_id and qh.vertex_visit if overflow may occur + set qh.furthest_id for tracing +*/ +void qh_buildtracing (pointT *furthest, facetT *facet) { + realT dist= 0; + float cpu; + int total, furthestid; + time_t timedata; + struct tm *tp; + vertexT *vertex; + + qh old_randomdist= qh RANDOMdist; + qh RANDOMdist= False; + if (!furthest) { + time (&timedata); + tp= localtime (&timedata); + cpu= qh_CPUclock - qh hulltime; + cpu /= qh_SECticks; + total= zzval_(Ztotmerge) - zzval_(Zcyclehorizon) + zzval_(Zcyclefacettot); + fprintf (qh ferr, "\n\ +At %02d:%02d:%02d & %2.5g CPU secs, qhull has created %d facets and merged %d.\n\ + The current hull contains %d facets and %d vertices. Last point was p%d\n", + tp->tm_hour, tp->tm_min, tp->tm_sec, cpu, qh facet_id -1, + total, qh num_facets, qh num_vertices, qh furthest_id); + return; + } + furthestid= qh_pointid (furthest); + if (qh TRACEpoint == furthestid) { + qh IStracing= qh TRACElevel; + qhmem.IStracing= qh TRACElevel; + }else if (qh TRACEpoint != -1 && qh TRACEdist < REALmax/2) { + qh IStracing= 0; + qhmem.IStracing= 0; + } + if (qh REPORTfreq && (qh facet_id-1 > qh lastreport+qh REPORTfreq)) { + qh lastreport= qh facet_id-1; + time (&timedata); + tp= localtime (&timedata); + cpu= qh_CPUclock - qh hulltime; + cpu /= qh_SECticks; + total= zzval_(Ztotmerge) - zzval_(Zcyclehorizon) + zzval_(Zcyclefacettot); + zinc_(Zdistio); + qh_distplane (furthest, facet, &dist); + fprintf (qh ferr, "\n\ +At %02d:%02d:%02d & %2.5g CPU secs, qhull has created %d facets and merged %d.\n\ + The current hull contains %d facets and %d vertices. There are %d\n\ + outside points. Next is point p%d (v%d), %2.2g above f%d.\n", + tp->tm_hour, tp->tm_min, tp->tm_sec, cpu, qh facet_id -1, + total, qh num_facets, qh num_vertices, qh num_outside+1, + furthestid, qh vertex_id, dist, getid_(facet)); + }else if (qh IStracing >=1) { + cpu= qh_CPUclock - qh hulltime; + cpu /= qh_SECticks; + qh_distplane (furthest, facet, &dist); + fprintf (qh ferr, "qh_addpoint: add p%d (v%d) to hull of %d facets (%2.2g above f%d) and %d outside at %4.4g CPU secs. Previous was p%d.\n", + furthestid, qh vertex_id, qh num_facets, dist, + getid_(facet), qh num_outside+1, cpu, qh furthest_id); + } + if (qh visit_id > (unsigned) INT_MAX) { + qh visit_id= 0; + FORALLfacets + facet->visitid= qh visit_id; + } + if (qh vertex_visit > (unsigned) INT_MAX) { + qh vertex_visit= 0; + FORALLvertices + vertex->visitid= qh vertex_visit; + } + qh furthest_id= furthestid; + qh RANDOMdist= qh old_randomdist; +} /* buildtracing */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="errexit2">-</a> + + qh_errexit2( exitcode, facet, otherfacet ) + return exitcode to system after an error + report two facets + + returns: + assumes exitcode non-zero + + see: + normally use qh_errexit() in user.c (reports a facet and a ridge) +*/ +void qh_errexit2(int exitcode, facetT *facet, facetT *otherfacet) { + + qh_errprint("ERRONEOUS", facet, otherfacet, NULL, NULL); + qh_errexit (exitcode, NULL, NULL); +} /* errexit2 */ + + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="findhorizon">-</a> + + qh_findhorizon( point, facet, goodvisible, goodhorizon ) + given a visible facet, find the point's horizon and visible facets + for all facets, !facet-visible + + returns: + returns qh.visible_list/num_visible with all visible facets + marks visible facets with ->visible + updates count of good visible and good horizon facets + updates qh.max_outside, qh.max_vertex, facet->maxoutside + + see: + similar to qh_delpoint() + + design: + move facet to qh.visible_list at end of qh.facet_list + for all visible facets + for each unvisited neighbor of a visible facet + compute distance of point to neighbor + if point above neighbor + move neighbor to end of qh.visible_list + else if point is coplanar with neighbor + update qh.max_outside, qh.max_vertex, neighbor->maxoutside + mark neighbor coplanar (will create a samecycle later) + update horizon statistics +*/ +void qh_findhorizon(pointT *point, facetT *facet, int *goodvisible, int *goodhorizon) { + facetT *neighbor, **neighborp, *visible; + int numhorizon= 0, coplanar= 0; + realT dist; + + trace1((qh ferr,"qh_findhorizon: find horizon for point p%d facet f%d\n",qh_pointid(point),facet->id)); + *goodvisible= *goodhorizon= 0; + zinc_(Ztotvisible); + qh_removefacet(facet); /* visible_list at end of qh facet_list */ + qh_appendfacet(facet); + qh num_visible= 1; + if (facet->good) + (*goodvisible)++; + qh visible_list= facet; + facet->visible= True; + facet->f.replace= NULL; + if (qh IStracing >=4) + qh_errprint ("visible", facet, NULL, NULL, NULL); + qh visit_id++; + FORALLvisible_facets { + if (visible->tricoplanar && !qh TRInormals) { + fprintf (qh ferr, "qh_findhorizon: does not work for tricoplanar facets. Use option 'Q11'\n"); + qh_errexit (qh_ERRqhull, visible, NULL); + } + visible->visitid= qh visit_id; + FOREACHneighbor_(visible) { + if (neighbor->visitid == qh visit_id) + continue; + neighbor->visitid= qh visit_id; + zzinc_(Znumvisibility); + qh_distplane(point, neighbor, &dist); + if (dist > qh MINvisible) { + zinc_(Ztotvisible); + qh_removefacet(neighbor); /* append to end of qh visible_list */ + qh_appendfacet(neighbor); + neighbor->visible= True; + neighbor->f.replace= NULL; + qh num_visible++; + if (neighbor->good) + (*goodvisible)++; + if (qh IStracing >=4) + qh_errprint ("visible", neighbor, NULL, NULL, NULL); + }else { + if (dist > - qh MAXcoplanar) { + neighbor->coplanar= True; + zzinc_(Zcoplanarhorizon); + qh_precision ("coplanar horizon"); + coplanar++; + if (qh MERGING) { + if (dist > 0) { + maximize_(qh max_outside, dist); + maximize_(qh max_vertex, dist); +#if qh_MAXoutside + maximize_(neighbor->maxoutside, dist); +#endif + }else + minimize_(qh min_vertex, dist); /* due to merge later */ + } + trace2((qh ferr, "qh_findhorizon: point p%d is coplanar to horizon f%d, dist=%2.7g < qh MINvisible (%2.7g)\n", + qh_pointid(point), neighbor->id, dist, qh MINvisible)); + }else + neighbor->coplanar= False; + zinc_(Ztothorizon); + numhorizon++; + if (neighbor->good) + (*goodhorizon)++; + if (qh IStracing >=4) + qh_errprint ("horizon", neighbor, NULL, NULL, NULL); + } + } + } + if (!numhorizon) { + qh_precision ("empty horizon"); + fprintf(qh ferr, "qhull precision error (qh_findhorizon): empty horizon\n\ +Point p%d was above all facets.\n", qh_pointid(point)); + qh_printfacetlist (qh facet_list, NULL, True); + qh_errexit(qh_ERRprec, NULL, NULL); + } + trace1((qh ferr, "qh_findhorizon: %d horizon facets (good %d), %d visible (good %d), %d coplanar\n", + numhorizon, *goodhorizon, qh num_visible, *goodvisible, coplanar)); + if (qh IStracing >= 4 && qh num_facets < 50) + qh_printlists (); +} /* findhorizon */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="nextfurthest">-</a> + + qh_nextfurthest( visible ) + returns next furthest point and visible facet for qh_addpoint() + starts search at qh.facet_next + + returns: + removes furthest point from outside set + NULL if none available + advances qh.facet_next over facets with empty outside sets + + design: + for each facet from qh.facet_next + if empty outside set + advance qh.facet_next + else if qh.NARROWhull + determine furthest outside point + if furthest point is not outside + advance qh.facet_next (point will be coplanar) + remove furthest point from outside set +*/ +pointT *qh_nextfurthest (facetT **visible) { + facetT *facet; + int size, index; + realT randr, dist; + pointT *furthest; + + while ((facet= qh facet_next) != qh facet_tail) { + if (!facet->outsideset) { + qh facet_next= facet->next; + continue; + } + SETreturnsize_(facet->outsideset, size); + if (!size) { + qh_setfree (&facet->outsideset); + qh facet_next= facet->next; + continue; + } + if (qh NARROWhull) { + if (facet->notfurthest) + qh_furthestout (facet); + furthest= (pointT*)qh_setlast (facet->outsideset); +#if qh_COMPUTEfurthest + qh_distplane (furthest, facet, &dist); + zinc_(Zcomputefurthest); +#else + dist= facet->furthestdist; +#endif + if (dist < qh MINoutside) { /* remainder of outside set is coplanar for qh_outcoplanar */ + qh facet_next= facet->next; + continue; + } + } + if (!qh RANDOMoutside && !qh VIRTUALmemory) { + if (qh PICKfurthest) { + qh_furthestnext (/* qh facet_list */); + facet= qh facet_next; + } + *visible= facet; + return ((pointT*)qh_setdellast (facet->outsideset)); + } + if (qh RANDOMoutside) { + int outcoplanar = 0; + if (qh NARROWhull) { + FORALLfacets { + if (facet == qh facet_next) + break; + if (facet->outsideset) + outcoplanar += qh_setsize( facet->outsideset); + } + } + randr= qh_RANDOMint; + randr= randr/(qh_RANDOMmax+1); + index= (int)floor((qh num_outside - outcoplanar) * randr); + FORALLfacet_(qh facet_next) { + if (facet->outsideset) { + SETreturnsize_(facet->outsideset, size); + if (!size) + qh_setfree (&facet->outsideset); + else if (size > index) { + *visible= facet; + return ((pointT*)qh_setdelnth (facet->outsideset, index)); + }else + index -= size; + } + } + fprintf (qh ferr, "qhull internal error (qh_nextfurthest): num_outside %d is too low\nby at least %d, or a random real %g >= 1.0\n", + qh num_outside, index+1, randr); + qh_errexit (qh_ERRqhull, NULL, NULL); + }else { /* VIRTUALmemory */ + facet= qh facet_tail->previous; + if (!(furthest= (pointT*)qh_setdellast(facet->outsideset))) { + if (facet->outsideset) + qh_setfree (&facet->outsideset); + qh_removefacet (facet); + qh_prependfacet (facet, &qh facet_list); + continue; + } + *visible= facet; + return furthest; + } + } + return NULL; +} /* nextfurthest */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="partitionall">-</a> + + qh_partitionall( vertices, points, numpoints ) + partitions all points in points/numpoints to the outsidesets of facets + vertices= vertices in qh.facet_list (not partitioned) + + returns: + builds facet->outsideset + does not partition qh.GOODpoint + if qh.ONLYgood && !qh.MERGING, + does not partition qh.GOODvertex + + notes: + faster if qh.facet_list sorted by anticipated size of outside set + + design: + initialize pointset with all points + remove vertices from pointset + remove qh.GOODpointp from pointset (unless it's qh.STOPcone or qh.STOPpoint) + for all facets + for all remaining points in pointset + compute distance from point to facet + if point is outside facet + remove point from pointset (by not reappending) + update bestpoint + append point or old bestpoint to facet's outside set + append bestpoint to facet's outside set (furthest) + for all points remaining in pointset + partition point into facets' outside sets and coplanar sets +*/ +void qh_partitionall(setT *vertices, pointT *points, int numpoints){ + setT *pointset; + vertexT *vertex, **vertexp; + pointT *point, **pointp, *bestpoint; + int size, point_i, point_n, point_end, remaining, i, id; + facetT *facet; + realT bestdist= -REALmax, dist, distoutside; + + trace1((qh ferr, "qh_partitionall: partition all points into outside sets\n")); + pointset= qh_settemp (numpoints); + qh num_outside= 0; + pointp= SETaddr_(pointset, pointT); + for (i=numpoints, point= points; i--; point += qh hull_dim) + *(pointp++)= point; + qh_settruncate (pointset, numpoints); + FOREACHvertex_(vertices) { + if ((id= qh_pointid(vertex->point)) >= 0) + SETelem_(pointset, id)= NULL; + } + id= qh_pointid (qh GOODpointp); + if (id >=0 && qh STOPcone-1 != id && -qh STOPpoint-1 != id) + SETelem_(pointset, id)= NULL; + if (qh GOODvertexp && qh ONLYgood && !qh MERGING) { /* matches qhull()*/ + if ((id= qh_pointid(qh GOODvertexp)) >= 0) + SETelem_(pointset, id)= NULL; + } + if (!qh BESToutside) { /* matches conditional for qh_partitionpoint below */ + distoutside= qh_DISToutside; /* multiple of qh.MINoutside & qh.max_outside, see user.h */ + zval_(Ztotpartition)= qh num_points - qh hull_dim - 1; /*misses GOOD... */ + remaining= qh num_facets; + point_end= numpoints; + FORALLfacets { + size= point_end/(remaining--) + 100; + facet->outsideset= qh_setnew (size); + bestpoint= NULL; + point_end= 0; + FOREACHpoint_i_(pointset) { + if (point) { + zzinc_(Zpartitionall); + qh_distplane (point, facet, &dist); + if (dist < distoutside) + SETelem_(pointset, point_end++)= point; + else { + qh num_outside++; + if (!bestpoint) { + bestpoint= point; + bestdist= dist; + }else if (dist > bestdist) { + qh_setappend (&facet->outsideset, bestpoint); + bestpoint= point; + bestdist= dist; + }else + qh_setappend (&facet->outsideset, point); + } + } + } + if (bestpoint) { + qh_setappend (&facet->outsideset, bestpoint); +#if !qh_COMPUTEfurthest + facet->furthestdist= bestdist; +#endif + }else + qh_setfree (&facet->outsideset); + qh_settruncate (pointset, point_end); + } + } + /* if !qh BESToutside, pointset contains points not assigned to outsideset */ + if (qh BESToutside || qh MERGING || qh KEEPcoplanar || qh KEEPinside) { + qh findbestnew= True; + FOREACHpoint_i_(pointset) { + if (point) + qh_partitionpoint(point, qh facet_list); + } + qh findbestnew= False; + } + zzadd_(Zpartitionall, zzval_(Zpartition)); + zzval_(Zpartition)= 0; + qh_settempfree(&pointset); + if (qh IStracing >= 4) + qh_printfacetlist (qh facet_list, NULL, True); +} /* partitionall */ + + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="partitioncoplanar">-</a> + + qh_partitioncoplanar( point, facet, dist ) + partition coplanar point to a facet + dist is distance from point to facet + if dist NULL, + searches for bestfacet and does nothing if inside + if qh.findbestnew set, + searches new facets instead of using qh_findbest() + + returns: + qh.max_ouside updated + if qh.KEEPcoplanar or qh.KEEPinside + point assigned to best coplanarset + + notes: + facet->maxoutside is updated at end by qh_check_maxout + + design: + if dist undefined + find best facet for point + if point sufficiently below facet (depends on qh.NEARinside and qh.KEEPinside) + exit + if keeping coplanar/nearinside/inside points + if point is above furthest coplanar point + append point to coplanar set (it is the new furthest) + update qh.max_outside + else + append point one before end of coplanar set + else if point is clearly outside of qh.max_outside and bestfacet->coplanarset + and bestfacet is more than perpendicular to facet + repartition the point using qh_findbest() -- it may be put on an outsideset + else + update qh.max_outside +*/ +void qh_partitioncoplanar (pointT *point, facetT *facet, realT *dist) { + facetT *bestfacet; + pointT *oldfurthest; + realT bestdist, dist2, angle; + int numpart= 0, oldfindbest; + boolT isoutside; + + qh WAScoplanar= True; + if (!dist) { + if (qh findbestnew) + bestfacet= qh_findbestnew (point, facet, &bestdist, qh_ALL, &isoutside, &numpart); + else + bestfacet= qh_findbest (point, facet, qh_ALL, !qh_ISnewfacets, qh DELAUNAY, + &bestdist, &isoutside, &numpart); + zinc_(Ztotpartcoplanar); + zzadd_(Zpartcoplanar, numpart); + if (!qh DELAUNAY && !qh KEEPinside) { /* for 'd', bestdist skips upperDelaunay facets */ + if (qh KEEPnearinside) { + if (bestdist < -qh NEARinside) { + zinc_(Zcoplanarinside); + trace4((qh ferr, "qh_partitioncoplanar: point p%d is more than near-inside facet f%d dist %2.2g findbestnew %d\n", + qh_pointid(point), bestfacet->id, bestdist, qh findbestnew)); + return; + } + }else if (bestdist < -qh MAXcoplanar) { + trace4((qh ferr, "qh_partitioncoplanar: point p%d is inside facet f%d dist %2.2g findbestnew %d\n", + qh_pointid(point), bestfacet->id, bestdist, qh findbestnew)); + zinc_(Zcoplanarinside); + return; + } + } + }else { + bestfacet= facet; + bestdist= *dist; + } + if (bestdist > qh max_outside) { + if (!dist && facet != bestfacet) { + zinc_(Zpartangle); + angle= qh_getangle(facet->normal, bestfacet->normal); + if (angle < 0) { + /* typically due to deleted vertex and coplanar facets, e.g., + RBOX 1000 s Z1 G1e-13 t1001185205 | QHULL Tv */ + zinc_(Zpartflip); + trace2((qh ferr, "qh_partitioncoplanar: repartition point p%d from f%d. It is above flipped facet f%d dist %2.2g\n", + qh_pointid(point), facet->id, bestfacet->id, bestdist)); + oldfindbest= qh findbestnew; + qh findbestnew= False; + qh_partitionpoint(point, bestfacet); + qh findbestnew= oldfindbest; + return; + } + } + qh max_outside= bestdist; + if (bestdist > qh TRACEdist) { + fprintf (qh ferr, "qh_partitioncoplanar: ====== p%d from f%d increases max_outside to %2.2g of f%d last p%d\n", + qh_pointid(point), facet->id, bestdist, bestfacet->id, qh furthest_id); + qh_errprint ("DISTANT", facet, bestfacet, NULL, NULL); + } + } + if (qh KEEPcoplanar + qh KEEPinside + qh KEEPnearinside) { + oldfurthest= (pointT*)qh_setlast (bestfacet->coplanarset); + if (oldfurthest) { + zinc_(Zcomputefurthest); + qh_distplane (oldfurthest, bestfacet, &dist2); + } + if (!oldfurthest || dist2 < bestdist) + qh_setappend(&bestfacet->coplanarset, point); + else + qh_setappend2ndlast(&bestfacet->coplanarset, point); + } + trace4((qh ferr, "qh_partitioncoplanar: point p%d is coplanar with facet f%d (or inside) dist %2.2g\n", + qh_pointid(point), bestfacet->id, bestdist)); +} /* partitioncoplanar */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="partitionpoint">-</a> + + qh_partitionpoint( point, facet ) + assigns point to an outside set, coplanar set, or inside set (i.e., dropt) + if qh.findbestnew + uses qh_findbestnew() to search all new facets + else + uses qh_findbest() + + notes: + after qh_distplane(), this and qh_findbest() are most expensive in 3-d + + design: + find best facet for point + (either exhaustive search of new facets or directed search from facet) + if qh.NARROWhull + retain coplanar and nearinside points as outside points + if point is outside bestfacet + if point above furthest point for bestfacet + append point to outside set (it becomes the new furthest) + if outside set was empty + move bestfacet to end of qh.facet_list (i.e., after qh.facet_next) + update bestfacet->furthestdist + else + append point one before end of outside set + else if point is coplanar to bestfacet + if keeping coplanar points or need to update qh.max_outside + partition coplanar point into bestfacet + else if near-inside point + partition as coplanar point into bestfacet + else is an inside point + if keeping inside points + partition as coplanar point into bestfacet +*/ +void qh_partitionpoint (pointT *point, facetT *facet) { + realT bestdist; + boolT isoutside; + facetT *bestfacet; + int numpart; +#if qh_COMPUTEfurthest + realT dist; +#endif + + if (qh findbestnew) + bestfacet= qh_findbestnew (point, facet, &bestdist, qh BESToutside, &isoutside, &numpart); + else + bestfacet= qh_findbest (point, facet, qh BESToutside, qh_ISnewfacets, !qh_NOupper, + &bestdist, &isoutside, &numpart); + zinc_(Ztotpartition); + zzadd_(Zpartition, numpart); + if (qh NARROWhull) { + if (qh DELAUNAY && !isoutside && bestdist >= -qh MAXcoplanar) + qh_precision ("nearly incident point (narrow hull)"); + if (qh KEEPnearinside) { + if (bestdist >= -qh NEARinside) + isoutside= True; + }else if (bestdist >= -qh MAXcoplanar) + isoutside= True; + } + + if (isoutside) { + if (!bestfacet->outsideset + || !qh_setlast (bestfacet->outsideset)) { + qh_setappend(&(bestfacet->outsideset), point); + if (!bestfacet->newfacet) { + qh_removefacet (bestfacet); /* make sure it's after qh facet_next */ + qh_appendfacet (bestfacet); + } +#if !qh_COMPUTEfurthest + bestfacet->furthestdist= bestdist; +#endif + }else { +#if qh_COMPUTEfurthest + zinc_(Zcomputefurthest); + qh_distplane (oldfurthest, bestfacet, &dist); + if (dist < bestdist) + qh_setappend(&(bestfacet->outsideset), point); + else + qh_setappend2ndlast(&(bestfacet->outsideset), point); +#else + if (bestfacet->furthestdist < bestdist) { + qh_setappend(&(bestfacet->outsideset), point); + bestfacet->furthestdist= bestdist; + }else + qh_setappend2ndlast(&(bestfacet->outsideset), point); +#endif + } + qh num_outside++; + trace4((qh ferr, "qh_partitionpoint: point p%d is outside facet f%d new? %d(or narrowhull)\n", + qh_pointid(point), bestfacet->id, bestfacet->newfacet)); + }else if (qh DELAUNAY || bestdist >= -qh MAXcoplanar) { /* for 'd', bestdist skips upperDelaunay facets */ + zzinc_(Zcoplanarpart); + if (qh DELAUNAY) + qh_precision ("nearly incident point"); + if ((qh KEEPcoplanar + qh KEEPnearinside) || bestdist > qh max_outside) + qh_partitioncoplanar (point, bestfacet, &bestdist); + else { + trace4((qh ferr, "qh_partitionpoint: point p%d is coplanar to facet f%d (dropped)\n", + qh_pointid(point), bestfacet->id)); + } + }else if (qh KEEPnearinside && bestdist > -qh NEARinside) { + zinc_(Zpartnear); + qh_partitioncoplanar (point, bestfacet, &bestdist); + }else { + zinc_(Zpartinside); + trace4((qh ferr, "qh_partitionpoint: point p%d is inside all facets, closest to f%d dist %2.2g\n", + qh_pointid(point), bestfacet->id, bestdist)); + if (qh KEEPinside) + qh_partitioncoplanar (point, bestfacet, &bestdist); + } +} /* partitionpoint */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="partitionvisible">-</a> + + qh_partitionvisible( allpoints, numoutside ) + partitions points in visible facets to qh.newfacet_list + qh.visible_list= visible facets + for visible facets + 1st neighbor (if any) points to a horizon facet or a new facet + if allpoints (not used), + repartitions coplanar points + + returns: + updates outside sets and coplanar sets of qh.newfacet_list + updates qh.num_outside (count of outside points) + + notes: + qh.findbest_notsharp should be clear (extra work if set) + + design: + for all visible facets with outside set or coplanar set + select a newfacet for visible facet + if outside set + partition outside set into new facets + if coplanar set and keeping coplanar/near-inside/inside points + if allpoints + partition coplanar set into new facets, may be assigned outside + else + partition coplanar set into coplanar sets of new facets + for each deleted vertex + if allpoints + partition vertex into new facets, may be assigned outside + else + partition vertex into coplanar sets of new facets +*/ +void qh_partitionvisible(/*visible_list*/ boolT allpoints, int *numoutside) { + facetT *visible, *newfacet; + pointT *point, **pointp; + int coplanar=0, size; + unsigned count; + vertexT *vertex, **vertexp; + + if (qh ONLYmax) + maximize_(qh MINoutside, qh max_vertex); + *numoutside= 0; + FORALLvisible_facets { + if (!visible->outsideset && !visible->coplanarset) + continue; + newfacet= visible->f.replace; + count= 0; + while (newfacet && newfacet->visible) { + newfacet= newfacet->f.replace; + if (count++ > qh facet_id) + qh_infiniteloop (visible); + } + if (!newfacet) + newfacet= qh newfacet_list; + if (newfacet == qh facet_tail) { + fprintf (qh ferr, "qhull precision error (qh_partitionvisible): all new facets deleted as\n degenerate facets. Can not continue.\n"); + qh_errexit (qh_ERRprec, NULL, NULL); + } + if (visible->outsideset) { + size= qh_setsize (visible->outsideset); + *numoutside += size; + qh num_outside -= size; + FOREACHpoint_(visible->outsideset) + qh_partitionpoint (point, newfacet); + } + if (visible->coplanarset && (qh KEEPcoplanar + qh KEEPinside + qh KEEPnearinside)) { + size= qh_setsize (visible->coplanarset); + coplanar += size; + FOREACHpoint_(visible->coplanarset) { + if (allpoints) /* not used */ + qh_partitionpoint (point, newfacet); + else + qh_partitioncoplanar (point, newfacet, NULL); + } + } + } + FOREACHvertex_(qh del_vertices) { + if (vertex->point) { + if (allpoints) /* not used */ + qh_partitionpoint (vertex->point, qh newfacet_list); + else + qh_partitioncoplanar (vertex->point, qh newfacet_list, NULL); + } + } + trace1((qh ferr,"qh_partitionvisible: partitioned %d points from outsidesets and %d points from coplanarsets\n", *numoutside, coplanar)); +} /* partitionvisible */ + + + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="precision">-</a> + + qh_precision( reason ) + restart on precision errors if not merging and if 'QJn' +*/ +void qh_precision (char *reason) { + + if (qh ALLOWrestart && !qh PREmerge && !qh MERGEexact) { + if (qh JOGGLEmax < REALmax/2) { + trace0((qh ferr, "qh_precision: qhull restart because of %s\n", reason)); + longjmp(qh restartexit, qh_ERRprec); + } + } +} /* qh_precision */ + +/*-<a href="qh-qhull.htm#TOC" + >-------------------------------</a><a name="printsummary">-</a> + + qh_printsummary( fp ) + prints summary to fp + + notes: + not in io.c so that user_eg.c can prevent io.c from loading + qh_printsummary and qh_countfacets must match counts + + design: + determine number of points, vertices, and coplanar points + print summary +*/ +void qh_printsummary(FILE *fp) { + realT ratio, outerplane, innerplane; + float cpu; + int size, id, nummerged, numvertices, numcoplanars= 0, nonsimplicial=0; + int goodused; + facetT *facet; + char *s; + int numdel= zzval_(Zdelvertextot); + int numtricoplanars= 0; + + size= qh num_points + qh_setsize (qh other_points); + numvertices= qh num_vertices - qh_setsize (qh del_vertices); + id= qh_pointid (qh GOODpointp); + FORALLfacets { + if (facet->coplanarset) + numcoplanars += qh_setsize( facet->coplanarset); + if (facet->good) { + if (facet->simplicial) { + if (facet->keepcentrum && facet->tricoplanar) + numtricoplanars++; + }else if (qh_setsize(facet->vertices) != qh hull_dim) + nonsimplicial++; + } + } + if (id >=0 && qh STOPcone-1 != id && -qh STOPpoint-1 != id) + size--; + if (qh STOPcone || qh STOPpoint) + fprintf (fp, "\nAt a premature exit due to 'TVn', 'TCn', 'TRn', or precision error."); + if (qh UPPERdelaunay) + goodused= qh GOODvertex + qh GOODpoint + qh SPLITthresholds; + else if (qh DELAUNAY) + goodused= qh GOODvertex + qh GOODpoint + qh GOODthreshold; + else + goodused= qh num_good; + nummerged= zzval_(Ztotmerge) - zzval_(Zcyclehorizon) + zzval_(Zcyclefacettot); + if (qh VORONOI) { + if (qh UPPERdelaunay) + fprintf (fp, "\n\ +Furthest-site Voronoi vertices by the convex hull of %d points in %d-d:\n\n", size, qh hull_dim); + else + fprintf (fp, "\n\ +Voronoi diagram by the convex hull of %d points in %d-d:\n\n", size, qh hull_dim); + fprintf(fp, " Number of Voronoi regions%s: %d\n", + qh ATinfinity ? " and at-infinity" : "", numvertices); + if (numdel) + fprintf(fp, " Total number of deleted points due to merging: %d\n", numdel); + if (numcoplanars - numdel > 0) + fprintf(fp, " Number of nearly incident points: %d\n", numcoplanars - numdel); + else if (size - numvertices - numdel > 0) + fprintf(fp, " Total number of nearly incident points: %d\n", size - numvertices - numdel); + fprintf(fp, " Number of%s Voronoi vertices: %d\n", + goodused ? " 'good'" : "", qh num_good); + if (nonsimplicial) + fprintf(fp, " Number of%s non-simplicial Voronoi vertices: %d\n", + goodused ? " 'good'" : "", nonsimplicial); + }else if (qh DELAUNAY) { + if (qh UPPERdelaunay) + fprintf (fp, "\n\ +Furthest-site Delaunay triangulation by the convex hull of %d points in %d-d:\n\n", size, qh hull_dim); + else + fprintf (fp, "\n\ +Delaunay triangulation by the convex hull of %d points in %d-d:\n\n", size, qh hull_dim); + fprintf(fp, " Number of input sites%s: %d\n", + qh ATinfinity ? " and at-infinity" : "", numvertices); + if (numdel) + fprintf(fp, " Total number of deleted points due to merging: %d\n", numdel); + if (numcoplanars - numdel > 0) + fprintf(fp, " Number of nearly incident points: %d\n", numcoplanars - numdel); + else if (size - numvertices - numdel > 0) + fprintf(fp, " Total number of nearly incident points: %d\n", size - numvertices - numdel); + fprintf(fp, " Number of%s Delaunay regions: %d\n", + goodused ? " 'good'" : "", qh num_good); + if (nonsimplicial) + fprintf(fp, " Number of%s non-simplicial Delaunay regions: %d\n", + goodused ? " 'good'" : "", nonsimplicial); + }else if (qh HALFspace) { + fprintf (fp, "\n\ +Halfspace intersection by the convex hull of %d points in %d-d:\n\n", size, qh hull_dim); + fprintf(fp, " Number of halfspaces: %d\n", size); + fprintf(fp, " Number of non-redundant halfspaces: %d\n", numvertices); + if (numcoplanars) { + if (qh KEEPinside && qh KEEPcoplanar) + s= "similar and redundant"; + else if (qh KEEPinside) + s= "redundant"; + else + s= "similar"; + fprintf(fp, " Number of %s halfspaces: %d\n", s, numcoplanars); + } + fprintf(fp, " Number of intersection points: %d\n", qh num_facets - qh num_visible); + if (goodused) + fprintf(fp, " Number of 'good' intersection points: %d\n", qh num_good); + if (nonsimplicial) + fprintf(fp, " Number of%s non-simplicial intersection points: %d\n", + goodused ? " 'good'" : "", nonsimplicial); + }else { + fprintf (fp, "\n\ +Convex hull of %d points in %d-d:\n\n", size, qh hull_dim); + fprintf(fp, " Number of vertices: %d\n", numvertices); + if (numcoplanars) { + if (qh KEEPinside && qh KEEPcoplanar) + s= "coplanar and interior"; + else if (qh KEEPinside) + s= "interior"; + else + s= "coplanar"; + fprintf(fp, " Number of %s points: %d\n", s, numcoplanars); + } + fprintf(fp, " Number of facets: %d\n", qh num_facets - qh num_visible); + if (goodused) + fprintf(fp, " Number of 'good' facets: %d\n", qh num_good); + if (nonsimplicial) + fprintf(fp, " Number of%s non-simplicial facets: %d\n", + goodused ? " 'good'" : "", nonsimplicial); + } + if (numtricoplanars) + fprintf(fp, " Number of triangulated facets: %d\n", numtricoplanars); + fprintf(fp, "\nStatistics for: %s | %s", + qh rbox_command, qh qhull_command); + if (qh ROTATErandom != INT_MIN) + fprintf(fp, " QR%d\n\n", qh ROTATErandom); + else + fprintf(fp, "\n\n"); + fprintf(fp, " Number of points processed: %d\n", zzval_(Zprocessed)); + fprintf(fp, " Number of hyperplanes created: %d\n", zzval_(Zsetplane)); + if (qh DELAUNAY) + fprintf(fp, " Number of facets in hull: %d\n", qh num_facets - qh num_visible); + fprintf(fp, " Number of distance tests for qhull: %d\n", zzval_(Zpartition)+ + zzval_(Zpartitionall)+zzval_(Znumvisibility)+zzval_(Zpartcoplanar)); +#if 0 /* NOTE: must print before printstatistics() */ + {realT stddev, ave; + fprintf(fp, " average new facet balance: %2.2g\n", + wval_(Wnewbalance)/zval_(Zprocessed)); + stddev= qh_stddev (zval_(Zprocessed), wval_(Wnewbalance), + wval_(Wnewbalance2), &ave); + fprintf(fp, " new facet standard deviation: %2.2g\n", stddev); + fprintf(fp, " average partition balance: %2.2g\n", + wval_(Wpbalance)/zval_(Zpbalance)); + stddev= qh_stddev (zval_(Zpbalance), wval_(Wpbalance), + wval_(Wpbalance2), &ave); + fprintf(fp, " partition standard deviation: %2.2g\n", stddev); + } +#endif + if (nummerged) { + fprintf(fp," Number of distance tests for merging: %d\n",zzval_(Zbestdist)+ + zzval_(Zcentrumtests)+zzval_(Zdistconvex)+zzval_(Zdistcheck)+ + zzval_(Zdistzero)); + fprintf(fp," Number of distance tests for checking: %d\n",zzval_(Zcheckpart)); + fprintf(fp," Number of merged facets: %d\n", nummerged); + } + if (!qh RANDOMoutside && qh QHULLfinished) { + cpu= qh hulltime; + cpu /= qh_SECticks; + wval_(Wcpu)= cpu; + fprintf (fp, " CPU seconds to compute hull (after input): %2.4g\n", cpu); + } + if (qh RERUN) { + if (!qh PREmerge && !qh MERGEexact) + fprintf(fp, " Percentage of runs with precision errors: %4.1f\n", + zzval_(Zretry)*100.0/qh build_cnt); /* careful of order */ + }else if (qh JOGGLEmax < REALmax/2) { + if (zzval_(Zretry)) + fprintf(fp, " After %d retries, input joggled by: %2.2g\n", + zzval_(Zretry), qh JOGGLEmax); + else + fprintf(fp, " Input joggled by: %2.2g\n", qh JOGGLEmax); + } + if (qh totarea != 0.0) + fprintf(fp, " %s facet area: %2.8g\n", + zzval_(Ztotmerge) ? "Approximate" : "Total", qh totarea); + if (qh totvol != 0.0) + fprintf(fp, " %s volume: %2.8g\n", + zzval_(Ztotmerge) ? "Approximate" : "Total", qh totvol); + if (qh MERGING) { + qh_outerinner (NULL, &outerplane, &innerplane); + if (outerplane > 2 * qh DISTround) { + fprintf(fp, " Maximum distance of %spoint above facet: %2.2g", + (qh QHULLfinished ? "" : "merged "), outerplane); + ratio= outerplane/(qh ONEmerge + qh DISTround); + /* don't report ratio if MINoutside is large */ + if (ratio > 0.05 && 2* qh ONEmerge > qh MINoutside && qh JOGGLEmax > REALmax/2) + fprintf (fp, " (%.1fx)\n", ratio); + else + fprintf (fp, "\n"); + } + if (innerplane < -2 * qh DISTround) { + fprintf(fp, " Maximum distance of %svertex below facet: %2.2g", + (qh QHULLfinished ? "" : "merged "), innerplane); + ratio= -innerplane/(qh ONEmerge+qh DISTround); + if (ratio > 0.05 && qh JOGGLEmax > REALmax/2) + fprintf (fp, " (%.1fx)\n", ratio); + else + fprintf (fp, "\n"); + } + } + fprintf(fp, "\n"); +} /* printsummary */ + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull.h new file mode 100644 index 0000000000000000000000000000000000000000..657f5e44d1657c294ebd86db6e134cbbd8a06c1e --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull.h @@ -0,0 +1,1030 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qhull.h + user-level header file for using qhull.a library + + see qh-qhull.htm, qhull_a.h + + copyright (c) 1993-2003, The Geometry Center + + NOTE: access to qh_qh is via the 'qh' macro. This allows + qh_qh to be either a pointer or a structure. An example + of using qh is "qh DROPdim" which accesses the DROPdim + field of qh_qh. Similarly, access to qh_qhstat is via + the 'qhstat' macro. + + includes function prototypes for qhull.c, geom.c, global.c, io.c, user.c + + use mem.h for mem.c + use qset.h for qset.c + + see unix.c for an example of using qhull.h + + recompile qhull if you change this file +*/ + +#ifndef qhDEFqhull +#define qhDEFqhull 1 + +/*=========================== -included files ==============*/ + +#include <setjmp.h> +#include <float.h> +#include <time.h> + +#if __MWERKS__ && __POWERPC__ +#include <SIOUX.h> +#include <Files.h> +#include <Desk.h> +#endif + +#ifndef __STDC__ +#ifndef __cplusplus +#if !_MSC_VER +#error Neither __STDC__ nor __cplusplus is defined. Please use strict ANSI C or C++ to compile +#error Qhull. You may need to turn off compiler extensions in your project configuration. If +#error your compiler is a standard C compiler, you can delete this warning from qhull.h +#endif +#endif +#endif + +#include "user.h" /* user defineable constants */ + +/*============ constants and basic types ====================*/ + +extern char *qh_version; /* defined in global.c */ + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="coordT">-</a> + + coordT + coordinates and coefficients are stored as realT (i.e., double) + + notes: + could use 'float' for data and 'double' for calculations (realT vs. coordT) + This requires many type casts, and adjusted error bounds. + Also C compilers may do expressions in double anyway. +*/ +#define coordT realT + +/*-<a href="qh-geom.htm#TOC" + >--------------------------------</a><a name="pointT">-</a> + + pointT + a point is an array of DIM3 coordinates +*/ +#define pointT coordT + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="flagT">-</a> + + flagT + Boolean flag as a bit +*/ +#define flagT unsigned int + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="boolT">-</a> + + boolT + boolean value, either True or False + + notes: + needed for portability +*/ +#define boolT unsigned int +#ifdef False +#undef False +#endif +#ifdef True +#undef True +#endif +#define False 0 +#define True 1 + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="CENTERtype">-</a> + + qh_CENTER + to distinguish facet->center +*/ +typedef enum +{ + qh_ASnone = 0, qh_ASvoronoi, qh_AScentrum +} +qh_CENTER; + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="qh_PRINT">-</a> + + qh_PRINT + output formats for printing (qh.PRINTout). + 'Fa' 'FV' 'Fc' 'FC' + + + notes: + some of these names are similar to qh names. The similar names are only + used in switch statements in qh_printbegin() etc. +*/ +typedef enum {qh_PRINTnone= 0, + qh_PRINTarea, qh_PRINTaverage, /* 'Fa' 'FV' 'Fc' 'FC' */ + qh_PRINTcoplanars, qh_PRINTcentrums, + qh_PRINTfacets, qh_PRINTfacets_xridge, /* 'f' 'FF' 'G' 'FI' 'Fi' 'Fn' */ + qh_PRINTgeom, qh_PRINTids, qh_PRINTinner, qh_PRINTneighbors, + qh_PRINTnormals, qh_PRINTouter, qh_PRINTmaple, /* 'n' 'Fo' 'i' 'm' 'Fm' 'FM', 'o' */ + qh_PRINTincidences, qh_PRINTmathematica, qh_PRINTmerges, qh_PRINToff, + qh_PRINToptions, qh_PRINTpointintersect, /* 'FO' 'Fp' 'FP' 'p' 'FQ' 'FS' */ + qh_PRINTpointnearest, qh_PRINTpoints, qh_PRINTqhull, qh_PRINTsize, + qh_PRINTsummary, qh_PRINTtriangles, /* 'Fs' 'Ft' 'Fv' 'FN' 'Fx' */ + qh_PRINTvertices, qh_PRINTvneighbors, qh_PRINTextremes, + qh_PRINTEND} qh_PRINT; + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="qh_ALL">-</a> + + qh_ALL + argument flag for selecting everything +*/ +#define qh_ALL True +#define qh_NOupper True /* argument for qh_findbest */ +#define qh_IScheckmax True /* argument for qh_findbesthorizon */ +#define qh_ISnewfacets True /* argument for qh_findbest */ +#define qh_RESETvisible True /* argument for qh_resetlists */ + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="qh_ERR">-</a> + + qh_ERR + Qhull exit codes, for indicating errors +*/ +#define qh_ERRnone 0 /* no error occurred during qhull */ +#define qh_ERRinput 1 /* input inconsistency */ +#define qh_ERRsingular 2 /* singular input data */ +#define qh_ERRprec 3 /* precision error */ +#define qh_ERRmem 4 /* insufficient memory, matches mem.h */ +#define qh_ERRqhull 5 /* internal error detected, matches mem.h */ + +/* ============ -structures- ==================== + each of the following structures is defined by a typedef + all realT and coordT fields occur at the beginning of a structure + (otherwise space may be wasted due to alignment) + define all flags together and pack into 32-bit number +*/ + +typedef struct vertexT vertexT; +typedef struct ridgeT ridgeT; +typedef struct facetT facetT; +#ifndef DEFsetT +#define DEFsetT 1 +typedef struct setT setT; /* defined in qset.h */ +#endif + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="facetT">-</a> + + facetT + defines a facet + + notes: + qhull() generates the hull as a list of facets. + + topological information: + f.previous,next doubly-linked list of facets + f.vertices set of vertices + f.ridges set of ridges + f.neighbors set of neighbors + f.toporient True if facet has top-orientation (else bottom) + + geometric information: + f.offset,normal hyperplane equation + f.maxoutside offset to outer plane -- all points inside + f.center centrum for testing convexity + f.simplicial True if facet is simplicial + f.flipped True if facet does not include qh.interior_point + + for constructing hull: + f.visible True if facet on list of visible facets (will be deleted) + f.newfacet True if facet on list of newly created facets + f.coplanarset set of points coplanar with this facet + (includes near-inside points for later testing) + f.outsideset set of points outside of this facet + f.furthestdist distance to furthest point of outside set + f.visitid marks visited facets during a loop + f.replace replacement facet for to-be-deleted, visible facets + f.samecycle,newcycle cycle of facets for merging into horizon facet + + see below for other flags and fields +*/ +struct facetT { +#if !qh_COMPUTEfurthest + coordT furthestdist;/* distance to furthest point of outsideset */ +#endif +#if qh_MAXoutside + coordT maxoutside; /* max computed distance of point to facet + Before QHULLfinished this is an approximation + since maxdist not always set for mergefacet + Actual outer plane is +DISTround and + computed outer plane is +2*DISTround */ +#endif + coordT offset; /* exact offset of hyperplane from origin */ + coordT *normal; /* normal of hyperplane, hull_dim coefficients */ + /* if tricoplanar, shared with a neighbor */ + union { /* in order of testing */ + realT area; /* area of facet, only in io.c if ->isarea */ + facetT *replace; /* replacement facet if ->visible and NEWfacets + is NULL only if qh_mergedegen_redundant or interior */ + facetT *samecycle; /* cycle of facets from the same visible/horizon intersection, + if ->newfacet */ + facetT *newcycle; /* in horizon facet, current samecycle of new facets */ + facetT *trivisible; /* visible facet for ->tricoplanar facets during qh_triangulate() */ + facetT *triowner; /* owner facet for ->tricoplanar, !isarea facets w/ ->keepcentrum */ + }f; + coordT *center; /* centrum for convexity, qh CENTERtype == qh_AScentrum */ + /* Voronoi center, qh CENTERtype == qh_ASvoronoi */ + /* if tricoplanar, shared with a neighbor */ + facetT *previous; /* previous facet in the facet_list */ + facetT *next; /* next facet in the facet_list */ + setT *vertices; /* vertices for this facet, inverse sorted by ID + if simplicial, 1st vertex was apex/furthest */ + setT *ridges; /* explicit ridges for nonsimplicial facets. + for simplicial facets, neighbors defines ridge */ + setT *neighbors; /* neighbors of the facet. If simplicial, the kth + neighbor is opposite the kth vertex, and the first + neighbor is the horizon facet for the first vertex*/ + setT *outsideset; /* set of points outside this facet + if non-empty, last point is furthest + if NARROWhull, includes coplanars for partitioning*/ + setT *coplanarset; /* set of points coplanar with this facet + > qh.min_vertex and <= facet->max_outside + a point is assigned to the furthest facet + if non-empty, last point is furthest away */ + unsigned visitid; /* visit_id, for visiting all neighbors, + all uses are independent */ + unsigned id; /* unique identifier from qh facet_id */ + unsigned nummerge:9; /* number of merges */ +#define qh_MAXnummerge 511 /* 2^9-1, 32 flags total, see "flags:" in io.c */ + flagT tricoplanar:1; /* True if TRIangulate and simplicial and coplanar with a neighbor */ + /* all tricoplanars share the same ->center, ->normal, ->offset, ->maxoutside */ + /* all tricoplanars share the same apex */ + /* if ->degenerate, does not span facet (one logical ridge) */ + /* one tricoplanar has ->keepcentrum and ->coplanarset */ + /* during qh_triangulate, f.trivisible points to original facet */ + flagT newfacet:1; /* True if facet on qh newfacet_list (new or merged) */ + flagT visible:1; /* True if visible facet (will be deleted) */ + flagT toporient:1; /* True if created with top orientation + after merging, use ridge orientation */ + flagT simplicial:1;/* True if simplicial facet, ->ridges may be implicit */ + flagT seen:1; /* used to perform operations only once, like visitid */ + flagT seen2:1; /* used to perform operations only once, like visitid */ + flagT flipped:1; /* True if facet is flipped */ + flagT upperdelaunay:1; /* True if facet is upper envelope of Delaunay triangulation */ + flagT notfurthest:1; /* True if last point of outsideset is not furthest*/ + +/*-------- flags primarily for output ---------*/ + flagT good:1; /* True if a facet marked good for output */ + flagT isarea:1; /* True if facet->f.area is defined */ + +/*-------- flags for merging ------------------*/ + flagT dupridge:1; /* True if duplicate ridge in facet */ + flagT mergeridge:1; /* True if facet or neighbor contains a qh_MERGEridge + ->normal defined (also defined for mergeridge2) */ + flagT mergeridge2:1; /* True if neighbor contains a qh_MERGEridge (mark_dupridges */ + flagT coplanar:1; /* True if horizon facet is coplanar at last use */ + flagT mergehorizon:1; /* True if will merge into horizon (->coplanar) */ + flagT cycledone:1;/* True if mergecycle_all already done */ + flagT tested:1; /* True if facet convexity has been tested (false after merge */ + flagT keepcentrum:1; /* True if keep old centrum after a merge, or marks owner for ->tricoplanar */ + flagT newmerge:1; /* True if facet is newly merged for reducevertices */ + flagT degenerate:1; /* True if facet is degenerate (degen_mergeset or ->tricoplanar) */ + flagT redundant:1; /* True if facet is redundant (degen_mergeset) */ +}; + + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="ridgeT">-</a> + + ridgeT + defines a ridge + + notes: + a ridge is DIM3-1 simplex between two neighboring facets. If the + facets are non-simplicial, there may be more than one ridge between + two facets. E.G. a 4-d hypercube has two triangles between each pair + of neighboring facets. + + topological information: + vertices a set of vertices + top,bottom neighboring facets with orientation + + geometric information: + tested True if ridge is clearly convex + nonconvex True if ridge is non-convex +*/ +struct ridgeT { + setT *vertices; /* vertices belonging to this ridge, inverse sorted by ID + NULL if a degen ridge (matchsame) */ + facetT *top; /* top facet this ridge is part of */ + facetT *bottom; /* bottom facet this ridge is part of */ + unsigned id:24; /* unique identifier, =>room for 8 flags */ + flagT seen:1; /* used to perform operations only once */ + flagT tested:1; /* True when ridge is tested for convexity */ + flagT nonconvex:1; /* True if getmergeset detected a non-convex neighbor + only one ridge between neighbors may have nonconvex */ +}; + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="vertexT">-</a> + + vertexT + defines a vertex + + topological information: + next,previous doubly-linked list of all vertices + neighbors set of adjacent facets (only if qh.VERTEXneighbors) + + geometric information: + point array of DIM3 coordinates +*/ +struct vertexT { + vertexT *next; /* next vertex in vertex_list */ + vertexT *previous; /* previous vertex in vertex_list */ + pointT *point; /* hull_dim coordinates (coordT) */ + setT *neighbors; /* neighboring facets of vertex, qh_vertexneighbors() + inits in io.c or after first merge */ + unsigned visitid; /* for use with qh vertex_visit */ + unsigned id:24; /* unique identifier, =>room for 8 flags */ + flagT seen:1; /* used to perform operations only once */ + flagT seen2:1; /* another seen flag */ + flagT delridge:1; /* vertex was part of a deleted ridge */ + flagT deleted:1; /* true if vertex on qh del_vertices */ + flagT newlist:1; /* true if vertex on qh newvertex_list */ +}; + +/*======= -global variables -qh ============================*/ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh">-</a> + + qh + all global variables for qhull are in qh, qhmem, and qhstat + + notes: + qhmem is defined in mem.h and qhstat is defined in stat.h + Access to qh_qh is via the "qh" macro. See qh_QHpointer in user.h +*/ +typedef struct qhT qhT; +#if qh_QHpointer +#define qh qh_qh-> +extern qhT *qh_qh; /* allocated in global.c */ +#else +#define qh qh_qh. +extern qhT qh_qh; +#endif + +struct qhT { + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-const">-</a> + + qh constants + configuration flags and constants for Qhull + + notes: + The user configures Qhull by defining flags. They are + copied into qh by qh_setflags(). qh-quick.htm#options defines the flags. +*/ + boolT ALLpoints; /* true 'Qs' if search all points for initial simplex */ + boolT ANGLEmerge; /* true 'Qa' if sort potential merges by angle */ + boolT APPROXhull; /* true 'Wn' if MINoutside set */ + realT MINoutside; /* 'Wn' min. distance for an outside point */ + boolT ATinfinity; /* true 'Qz' if point num_points-1 is "at-infinity" + for improving precision in Delaunay triangulations */ + boolT AVOIDold; /* true 'Q4' if avoid old->new merges */ + boolT BESToutside; /* true 'Qf' if partition points into best outsideset */ + boolT CDDinput; /* true 'Pc' if input uses CDD format (1.0/offset first) */ + boolT CDDoutput; /* true 'PC' if print normals in CDD format (offset first) */ + boolT CHECKfrequently; /* true 'Tc' if checking frequently */ + realT premerge_cos; /* 'A-n' cos_max when pre merging */ + realT postmerge_cos; /* 'An' cos_max when post merging */ + boolT DELAUNAY; /* true 'd' if computing DELAUNAY triangulation */ + boolT DOintersections; /* true 'Gh' if print hyperplane intersections */ + int DROPdim; /* drops dim 'GDn' for 4-d -> 3-d output */ + boolT FORCEoutput; /* true 'Po' if forcing output despite degeneracies */ + int GOODpoint; /* 1+n for 'QGn', good facet if visible/not(-) from point n*/ + pointT *GOODpointp; /* the actual point */ + boolT GOODthreshold; /* true if qh lower_threshold/upper_threshold defined + false if qh SPLITthreshold */ + int GOODvertex; /* 1+n, good facet if vertex for point n */ + pointT *GOODvertexp; /* the actual point */ + boolT HALFspace; /* true 'Hn,n,n' if halfspace intersection */ + int IStracing; /* trace execution, 0=none, 1=least, 4=most, -1=events */ + int KEEParea; /* 'PAn' number of largest facets to keep */ + boolT KEEPcoplanar; /* true 'Qc' if keeping nearest facet for coplanar points */ + boolT KEEPinside; /* true 'Qi' if keeping nearest facet for inside points + set automatically if 'd Qc' */ + int KEEPmerge; /* 'PMn' number of facets to keep with most merges */ + realT KEEPminArea; /* 'PFn' minimum facet area to keep */ + realT MAXcoplanar; /* 'Un' max distance below a facet to be coplanar*/ + boolT MERGEexact; /* true 'Qx' if exact merges (coplanar, degen, dupridge, flipped) */ + boolT MERGEindependent; /* true 'Q2' if merging independent sets */ + boolT MERGING; /* true if exact-, pre- or post-merging, with angle and centrum tests */ + realT premerge_centrum; /* 'C-n' centrum_radius when pre merging. Default is round-off */ + realT postmerge_centrum; /* 'Cn' centrum_radius when post merging. Default is round-off */ + boolT MERGEvertices; /* true 'Q3' if merging redundant vertices */ + realT MINvisible; /* 'Vn' min. distance for a facet to be visible */ + boolT NOnarrow; /* true 'Q10' if no special processing for narrow distributions */ + boolT NOnearinside; /* true 'Q8' if ignore near-inside points when partitioning */ + boolT NOpremerge; /* true 'Q0' if no defaults for C-0 or Qx */ + boolT ONLYgood; /* true 'Qg' if process points with good visible or horizon facets */ + boolT ONLYmax; /* true 'Qm' if only process points that increase max_outside */ + boolT PICKfurthest; /* true 'Q9' if process furthest of furthest points*/ + boolT POSTmerge; /* true if merging after buildhull (Cn or An) */ + boolT PREmerge; /* true if merging during buildhull (C-n or A-n) */ + /* NOTE: some of these names are similar to qh_PRINT names */ + boolT PRINTcentrums; /* true 'Gc' if printing centrums */ + boolT PRINTcoplanar; /* true 'Gp' if printing coplanar points */ + int PRINTdim; /* print dimension for Geomview output */ + boolT PRINTdots; /* true 'Ga' if printing all points as dots */ + boolT PRINTgood; /* true 'Pg' if printing good facets */ + boolT PRINTinner; /* true 'Gi' if printing inner planes */ + boolT PRINTneighbors; /* true 'PG' if printing neighbors of good facets */ + boolT PRINTnoplanes; /* true 'Gn' if printing no planes */ + boolT PRINToptions1st; /* true 'FO' if printing options to stderr */ + boolT PRINTouter; /* true 'Go' if printing outer planes */ + boolT PRINTprecision; /* false 'Pp' if not reporting precision problems */ + qh_PRINT PRINTout[qh_PRINTEND]; /* list of output formats to print */ + boolT PRINTridges; /* true 'Gr' if print ridges */ + boolT PRINTspheres; /* true 'Gv' if print vertices as spheres */ + boolT PRINTstatistics; /* true 'Ts' if printing statistics to stderr */ + boolT PRINTsummary; /* true 's' if printing summary to stderr */ + boolT PRINTtransparent; /* true 'Gt' if print transparent outer ridges */ + boolT PROJECTdelaunay; /* true if DELAUNAY, no readpoints() and + need projectinput() for Delaunay in qh_init_B */ + int PROJECTinput; /* number of projected dimensions 'bn:0Bn:0' */ + boolT QUICKhelp; /* true if quick help message for degen input */ + boolT RANDOMdist; /* true if randomly change distplane and setfacetplane */ + realT RANDOMfactor; /* maximum random perturbation */ + realT RANDOMa; /* qh_randomfactor is randr * RANDOMa + RANDOMb */ + realT RANDOMb; + boolT RANDOMoutside; /* true if select a random outside point */ + int REPORTfreq; /* buildtracing reports every n facets */ + int REPORTfreq2; /* tracemerging reports every REPORTfreq/2 facets */ + int RERUN; /* 'TRn' rerun qhull n times (qh.build_cnt) */ + int ROTATErandom; /* 'QRn' seed, 0 time, >= rotate input */ + boolT SCALEinput; /* true 'Qbk' if scaling input */ + boolT SCALElast; /* true 'Qbb' if scale last coord to max prev coord */ + boolT SETroundoff; /* true 'E' if qh DISTround is predefined */ + boolT SKIPcheckmax; /* true 'Q5' if skip qh_check_maxout */ + boolT SKIPconvex; /* true 'Q6' if skip convexity testing during pre-merge */ + boolT SPLITthresholds; /* true if upper_/lower_threshold defines a region + used only for printing (not for qh ONLYgood) */ + int STOPcone; /* 'TCn' 1+n for stopping after cone for point n*/ + /* also used by qh_build_withresart for err exit*/ + int STOPpoint; /* 'TVn' 'TV-n' 1+n for stopping after/before(-) + adding point n */ + int TESTpoints; /* 'QTn' num of test points after qh.num_points. Test points always coplanar. */ + boolT TESTvneighbors; /* true 'Qv' if test vertex neighbors at end */ + int TRACElevel; /* 'Tn' conditional IStracing level */ + int TRACElastrun; /* qh.TRACElevel applies to last qh.RERUN */ + int TRACEpoint; /* 'TPn' start tracing when point n is a vertex */ + realT TRACEdist; /* 'TWn' start tracing when merge distance too big */ + int TRACEmerge; /* 'TMn' start tracing before this merge */ + boolT TRIangulate; /* true 'Qt' if triangulate non-simplicial facets */ + boolT TRInormals; /* true 'Q11' if triangulate duplicates normals (sets Qt) */ + boolT UPPERdelaunay; /* true 'Qu' if computing furthest-site Delaunay */ + boolT VERIFYoutput; /* true 'Tv' if verify output at end of qhull */ + boolT VIRTUALmemory; /* true 'Q7' if depth-first processing in buildhull */ + boolT VORONOI; /* true 'v' if computing Voronoi diagram */ + + /*--------input constants ---------*/ + realT AREAfactor; /* 1/(hull_dim-1)! for converting det's to area */ + boolT DOcheckmax; /* true if calling qh_check_maxout (qh_initqhull_globals) */ + char *feasible_string; /* feasible point 'Hn,n,n' for halfspace intersection */ + coordT *feasible_point; /* as coordinates, both malloc'd */ + boolT GETarea; /* true 'Fa', 'FA', 'FS', 'PAn', 'PFn' if compute facet area/Voronoi volume in io.c */ + boolT KEEPnearinside; /* true if near-inside points in coplanarset */ + int hull_dim; /* dimension of hull, set by initbuffers */ + int input_dim; /* dimension of input, set by initbuffers */ + int num_points; /* number of input points */ + pointT *first_point; /* array of input points, see POINTSmalloc */ + boolT POINTSmalloc; /* true if qh first_point/num_points allocated */ + pointT *input_points; /* copy of original qh.first_point for input points for qh_joggleinput */ + boolT input_malloc; /* true if qh input_points malloc'd */ + char qhull_command[256];/* command line that invoked this program */ + char rbox_command[256]; /* command line that produced the input points */ + char qhull_options[512];/* descriptive list of options */ + int qhull_optionlen; /* length of last line */ + int qhull_optionsiz; /* size of qhull_options before qh_initbuild */ + boolT VERTEXneighbors; /* true if maintaining vertex neighbors */ + boolT ZEROcentrum; /* true if 'C-0' or 'C-0 Qx'. sets ZEROall_ok */ + realT *upper_threshold; /* don't print if facet->normal[k]>=upper_threshold[k] + must set either GOODthreshold or SPLITthreshold + if Delaunay, default is 0.0 for upper envelope */ + realT *lower_threshold; /* don't print if facet->normal[k] <=lower_threshold[k] */ + realT *upper_bound; /* scale point[k] to new upper bound */ + realT *lower_bound; /* scale point[k] to new lower bound + project if both upper_ and lower_bound == 0 */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-prec">-</a> + + qh precision constants + precision constants for Qhull + + notes: + qh_detroundoff() computes the maximum roundoff error for distance + and other computations. It also sets default values for the + qh constants above. +*/ + realT ANGLEround; /* max round off error for angles */ + realT centrum_radius; /* max centrum radius for convexity (roundoff added) */ + realT cos_max; /* max cosine for convexity (roundoff added) */ + realT DISTround; /* max round off error for distances, 'E' overrides */ + realT MAXabs_coord; /* max absolute coordinate */ + realT MAXlastcoord; /* max last coordinate for qh_scalelast */ + realT MAXsumcoord; /* max sum of coordinates */ + realT MAXwidth; /* max rectilinear width of point coordinates */ + realT MINdenom_1; /* min. abs. value for 1/x */ + realT MINdenom; /* use divzero if denominator < MINdenom */ + realT MINdenom_1_2; /* min. abs. val for 1/x that allows normalization */ + realT MINdenom_2; /* use divzero if denominator < MINdenom_2 */ + realT MINlastcoord; /* min. last coordinate for qh_scalelast */ + boolT NARROWhull; /* set in qh_initialhull if angle < qh_MAXnarrow */ + realT *NEARzero; /* hull_dim array for near zero in gausselim */ + realT NEARinside; /* keep points for qh_check_maxout if close to facet */ + realT ONEmerge; /* max distance for merging simplicial facets */ + realT outside_err; /* application's epsilon for coplanar points + qh_check_bestdist() qh_check_points() reports error if point outside */ + realT WIDEfacet; /* size of wide facet for skipping ridge in + area computation and locking centrum */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-intern">-</a> + + qh internal constants + internal constants for Qhull +*/ + char qhull[sizeof("qhull")]; /* for checking ownership */ + void *old_stat; /* pointer to saved qh_qhstat, qh_save_qhull */ + jmp_buf errexit; /* exit label for qh_errexit, defined by setjmp() */ + char jmpXtra[40]; /* extra bytes in case jmp_buf is defined wrong by compiler */ + jmp_buf restartexit; /* restart label for qh_errexit, defined by setjmp() */ + char jmpXtra2[40]; /* extra bytes in case jmp_buf is defined wrong by compiler*/ + FILE *fin; /* pointer to input file, init by qh_meminit */ + FILE *fout; /* pointer to output file */ + FILE *ferr; /* pointer to error file */ + pointT *interior_point; /* center point of the initial simplex*/ + int normal_size; /* size in bytes for facet normals and point coords*/ + int center_size; /* size in bytes for Voronoi centers */ + int TEMPsize; /* size for small, temporary sets (in quick mem) */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-lists">-</a> + + qh facet and vertex lists + defines lists of facets, new facets, visible facets, vertices, and + new vertices. Includes counts, next ids, and trace ids. + see: + qh_resetlists() +*/ + facetT *facet_list; /* first facet */ + facetT *facet_tail; /* end of facet_list (dummy facet) */ + facetT *facet_next; /* next facet for buildhull() + previous facets do not have outside sets + NARROWhull: previous facets may have coplanar outside sets for qh_outcoplanar */ + facetT *newfacet_list; /* list of new facets to end of facet_list */ + facetT *visible_list; /* list of visible facets preceeding newfacet_list, + facet->visible set */ + int num_visible; /* current number of visible facets */ + unsigned tracefacet_id; /* set at init, then can print whenever */ + facetT *tracefacet; /* set in newfacet/mergefacet, undone in delfacet*/ + unsigned tracevertex_id; /* set at buildtracing, can print whenever */ + vertexT *tracevertex; /* set in newvertex, undone in delvertex*/ + vertexT *vertex_list; /* list of all vertices, to vertex_tail */ + vertexT *vertex_tail; /* end of vertex_list (dummy vertex) */ + vertexT *newvertex_list; /* list of vertices in newfacet_list, to vertex_tail + all vertices have 'newlist' set */ + int num_facets; /* number of facets in facet_list + includes visble faces (num_visible) */ + int num_vertices; /* number of vertices in facet_list */ + int num_outside; /* number of points in outsidesets (for tracing and RANDOMoutside) + includes coplanar outsideset points for NARROWhull/qh_outcoplanar() */ + int num_good; /* number of good facets (after findgood_all) */ + unsigned facet_id; /* ID of next, new facet from newfacet() */ + unsigned ridge_id; /* ID of next, new ridge from newridge() */ + unsigned vertex_id; /* ID of next, new vertex from newvertex() */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-var">-</a> + + qh global variables + defines minimum and maximum distances, next visit ids, several flags, + and other global variables. + initialize in qh_initbuild or qh_maxmin if used in qh_buildhull +*/ + unsigned long hulltime; /* ignore time to set up input and randomize */ + /* use unsigned to avoid wrap-around errors */ + boolT ALLOWrestart; /* true if qh_precision can use qh.restartexit */ + int build_cnt; /* number of calls to qh_initbuild */ + qh_CENTER CENTERtype; /* current type of facet->center, qh_CENTER */ + int furthest_id; /* pointid of furthest point, for tracing */ + facetT *GOODclosest; /* closest facet to GOODthreshold in qh_findgood */ + realT JOGGLEmax; /* set 'QJn' if randomly joggle input */ + boolT maxoutdone; /* set qh_check_maxout(), cleared by qh_addpoint() */ + realT max_outside; /* maximum distance from a point to a facet, + before roundoff, not simplicial vertices + actual outer plane is +DISTround and + computed outer plane is +2*DISTround */ + realT max_vertex; /* maximum distance (>0) from vertex to a facet, + before roundoff, due to a merge */ + realT min_vertex; /* minimum distance (<0) from vertex to a facet, + before roundoff, due to a merge + if qh.JOGGLEmax, qh_makenewplanes sets it + recomputed if qh.DOcheckmax, default -qh.DISTround */ + boolT NEWfacets; /* true while visible facets invalid due to new or merge + from makecone/attachnewfacets to deletevisible */ + boolT findbestnew; /* true if partitioning calls qh_findbestnew */ + boolT findbest_notsharp; /* true if new facets are at least 90 degrees */ + boolT NOerrexit; /* true if qh.errexit is not available */ + realT PRINTcradius; /* radius for printing centrums */ + realT PRINTradius; /* radius for printing vertex spheres and points */ + boolT POSTmerging; /* true when post merging */ + int printoutvar; /* temporary variable for qh_printbegin, etc. */ + int printoutnum; /* number of facets printed */ + boolT QHULLfinished; /* True after qhull() is finished */ + realT totarea; /* 'FA': total facet area computed by qh_getarea */ + realT totvol; /* 'FA': total volume computed by qh_getarea */ + unsigned int visit_id; /* unique ID for searching neighborhoods, */ + unsigned int vertex_visit; /* unique ID for searching vertices */ + boolT ZEROall_ok; /* True if qh_checkzero always succeeds */ + boolT WAScoplanar; /* True if qh_partitioncoplanar (qh_check_maxout) */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-set">-</a> + + qh global sets + defines sets for merging, initial simplex, hashing, extra input points, + and deleted vertices +*/ + setT *facet_mergeset; /* temporary set of merges to be done */ + setT *degen_mergeset; /* temporary set of degenerate and redundant merges */ + setT *hash_table; /* hash table for matching ridges in qh_matchfacets + size is setsize() */ + setT *other_points; /* additional points (first is qh interior_point) */ + setT *del_vertices; /* vertices to partition and delete with visible + facets. Have deleted set for checkfacet */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-buf">-</a> + + qh global buffers + defines buffers for maxtrix operations, input, and error messages +*/ + coordT *gm_matrix; /* (dim+1)Xdim matrix for geom.c */ + coordT **gm_row; /* array of gm_matrix rows */ + char* line; /* malloc'd input line of maxline+1 chars */ + int maxline; + coordT *half_space; /* malloc'd input array for halfspace (qh normal_size+coordT) */ + coordT *temp_malloc; /* malloc'd input array for points */ + +/*-<a href="qh-globa.htm#TOC" + >--------------------------------</a><a name="qh-static">-</a> + + qh static variables + defines static variables for individual functions + + notes: + do not use 'static' within a function. Multiple instances of qhull + may exist. + + do not assume zero initialization, 'QPn' may cause a restart +*/ + boolT ERREXITcalled; /* true during errexit (prevents duplicate calls */ + boolT firstcentrum; /* for qh_printcentrum */ + realT last_low; /* qh_scalelast parameters for qh_setdelaunay */ + realT last_high; + realT last_newhigh; + unsigned lastreport; /* for qh_buildtracing */ + int mergereport; /* for qh_tracemerging */ + boolT old_randomdist; /* save RANDOMdist when io, tracing, or statistics */ + int ridgeoutnum; /* number of ridges in 4OFF output */ + void *old_qhstat; /* for saving qh_qhstat in save_qhull() */ + setT *old_tempstack; /* for saving qhmem.tempstack in save_qhull */ + setT *coplanarset; /* set of coplanar facets for searching qh_findbesthorizon() */ +}; + +/*=========== -macros- =========================*/ + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="otherfacet_">-</a> + + otherfacet_(ridge, facet) + return neighboring facet for a ridge in facet +*/ +#define otherfacet_(ridge, facet) \ + (((ridge)->top == (facet)) ? (ridge)->bottom : (ridge)->top) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="getid_">-</a> + + getid_(p) + return ID for facet, ridge, or vertex + return MAXINT if NULL (-1 causes type conversion error ) +*/ +#define getid_(p) ((p) ? (p)->id : -1) + +/*============== FORALL macros ===================*/ + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLfacets">-</a> + + FORALLfacets { ... } + assign 'facet' to each facet in qh.facet_list + + notes: + uses 'facetT *facet;' + assumes last facet is a sentinel + + see: + FORALLfacet_( facetlist ) +*/ +#define FORALLfacets for (facet=qh facet_list;facet && facet->next;facet=facet->next) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLpoints">-</a> + + FORALLpoints { ... } + assign 'point' to each point in qh.first_point, qh.num_points + + declare: + coordT *point, *pointtemp; +*/ +#define FORALLpoints FORALLpoint_(qh first_point, qh num_points) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLpoint_">-</a> + + FORALLpoint_( points, num) { ... } + assign 'point' to each point in points array of num points + + declare: + coordT *point, *pointtemp; +*/ +#define FORALLpoint_(points, num) for(point= (points), \ + pointtemp= (points)+qh hull_dim*(num); point < pointtemp; point += qh hull_dim) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FORALLvertices">-</a> + + FORALLvertices { ... } + assign 'vertex' to each vertex in qh.vertex_list + + declare: + vertexT *vertex; + + notes: + assumes qh.vertex_list terminated with a sentinel +*/ +#define FORALLvertices for (vertex=qh vertex_list;vertex && vertex->next;vertex= vertex->next) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHfacet_">-</a> + + FOREACHfacet_( facets ) { ... } + assign 'facet' to each facet in facets + + declare: + facetT *facet, **facetp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHfacet_(facets) FOREACHsetelement_(facetT, facets, facet) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHneighbor_">-</a> + + FOREACHneighbor_( facet ) { ... } + assign 'neighbor' to each neighbor in facet->neighbors + + FOREACHneighbor_( vertex ) { ... } + assign 'neighbor' to each neighbor in vertex->neighbors + + declare: + facetT *neighbor, **neighborp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHneighbor_(facet) FOREACHsetelement_(facetT, facet->neighbors, neighbor) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHpoint_">-</a> + + FOREACHpoint_( points ) { ... } + assign 'point' to each point in points set + + declare: + pointT *point, **pointp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHpoint_(points) FOREACHsetelement_(pointT, points, point) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHridge_">-</a> + + FOREACHridge_( ridges ) { ... } + assign 'ridge' to each ridge in ridges set + + declare: + ridgeT *ridge, **ridgep; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHridge_(ridges) FOREACHsetelement_(ridgeT, ridges, ridge) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertex_">-</a> + + FOREACHvertex_( vertices ) { ... } + assign 'vertex' to each vertex in vertices set + + declare: + vertexT *vertex, **vertexp; + + see: + <a href="qset.h#FOREACHsetelement_">FOREACHsetelement_</a> +*/ +#define FOREACHvertex_(vertices) FOREACHsetelement_(vertexT, vertices,vertex) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHfacet_i_">-</a> + + FOREACHfacet_i_( facets ) { ... } + assign 'facet' and 'facet_i' for each facet in facets set + + declare: + facetT *facet; + int facet_n, facet_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHfacet_i_(facets) FOREACHsetelement_i_(facetT, facets, facet) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHneighbor_i_">-</a> + + FOREACHneighbor_i_( facet ) { ... } + assign 'neighbor' and 'neighbor_i' for each neighbor in facet->neighbors + + FOREACHneighbor_i_( vertex ) { ... } + assign 'neighbor' and 'neighbor_i' for each neighbor in vertex->neighbors + + declare: + facetT *neighbor; + int neighbor_n, neighbor_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHneighbor_i_(facet) FOREACHsetelement_i_(facetT, facet->neighbors, neighbor) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHpoint_i_">-</a> + + FOREACHpoint_i_( points ) { ... } + assign 'point' and 'point_i' for each point in points set + + declare: + pointT *point; + int point_n, point_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHpoint_i_(points) FOREACHsetelement_i_(pointT, points, point) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHridge_i_">-</a> + + FOREACHridge_i_( ridges ) { ... } + assign 'ridge' and 'ridge_i' for each ridge in ridges set + + declare: + ridgeT *ridge; + int ridge_n, ridge_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> +*/ +#define FOREACHridge_i_(ridges) FOREACHsetelement_i_(ridgeT, ridges, ridge) + +/*-<a href="qh-poly.htm#TOC" + >--------------------------------</a><a name="FOREACHvertex_i_">-</a> + + FOREACHvertex_i_( vertices ) { ... } + assign 'vertex' and 'vertex_i' for each vertex in vertices set + + declare: + vertexT *vertex; + int vertex_n, vertex_i; + + see: + <a href="qset.h#FOREACHsetelement_i_">FOREACHsetelement_i_</a> + */ +#define FOREACHvertex_i_(vertices) FOREACHsetelement_i_(vertexT, vertices,vertex) + +/********* -qhull.c prototypes (duplicated from qhull_a.h) **********************/ + +void qh_qhull (void); +boolT qh_addpoint (pointT *furthest, facetT *facet, boolT checkdist); +void qh_printsummary(FILE *fp); + +/********* -user.c prototypes (alphabetical) **********************/ + +void qh_errexit(int exitcode, facetT *facet, ridgeT *ridge); +void qh_errprint(char* string, facetT *atfacet, facetT *otherfacet, ridgeT *atridge, vertexT *atvertex); +int qh_new_qhull (int dim, int numpoints, coordT *points, boolT ismalloc, + char *qhull_cmd, FILE *outfile, FILE *errfile); +void qh_printfacetlist(facetT *facetlist, setT *facets, boolT printall); +void qh_user_memsizes (void); + +/***** -geom.c/geom2.c prototypes (duplicated from geom.h) ****************/ + +facetT *qh_findbest (pointT *point, facetT *startfacet, + boolT bestoutside, boolT newfacets, boolT noupper, + realT *dist, boolT *isoutside, int *numpart); +facetT *qh_findbestnew (pointT *point, facetT *startfacet, + realT *dist, boolT bestoutside, boolT *isoutside, int *numpart); +boolT qh_gram_schmidt(int dim, realT **rows); +void qh_outerinner (facetT *facet, realT *outerplane, realT *innerplane); +void qh_printsummary(FILE *fp); +void qh_projectinput (void); +void qh_randommatrix (realT *buffer, int dim, realT **row); +void qh_rotateinput (realT **rows); +void qh_scaleinput (void); +void qh_setdelaunay (int dim, int count, pointT *points); +coordT *qh_sethalfspace_all (int dim, int count, coordT *halfspaces, pointT *feasible); + +/***** -global.c prototypes (alphabetical) ***********************/ + +unsigned long qh_clock (void); +void qh_checkflags (char *command, char *hiddenflags); +void qh_freebuffers (void); +void qh_freeqhull (boolT allmem); +void qh_init_A (FILE *infile, FILE *outfile, FILE *errfile, int argc, char *argv[]); +void qh_init_B (coordT *points, int numpoints, int dim, boolT ismalloc); +void qh_init_qhull_command (int argc, char *argv[]); +void qh_initbuffers (coordT *points, int numpoints, int dim, boolT ismalloc); +void qh_initflags (char *command); +void qh_initqhull_buffers (void); +void qh_initqhull_globals (coordT *points, int numpoints, int dim, boolT ismalloc); +void qh_initqhull_mem (void); +void qh_initqhull_start (FILE *infile, FILE *outfile, FILE *errfile); +void qh_initthresholds (char *command); +void qh_option (char *option, int *i, realT *r); +#if qh_QHpointer +void qh_restore_qhull (qhT **oldqh); +qhT *qh_save_qhull (void); +#endif + +/***** -io.c prototypes (duplicated from io.h) ***********************/ + +void dfacet( unsigned id); +void dvertex( unsigned id); +void qh_printneighborhood (FILE *fp, int format, facetT *facetA, facetT *facetB, boolT printall); +void qh_produce_output(void); +coordT *qh_readpoints(int *numpoints, int *dimension, boolT *ismalloc); + + +/********* -mem.c prototypes (duplicated from mem.h) **********************/ + +void qh_meminit (FILE *ferr); +void qh_memfreeshort (int *curlong, int *totlong); + +/********* -poly.c/poly2.c prototypes (duplicated from poly.h) **********************/ + +void qh_check_output (void); +void qh_check_points (void); +setT *qh_facetvertices (facetT *facetlist, setT *facets, boolT allfacets); +facetT *qh_findbestfacet (pointT *point, boolT bestoutside, + realT *bestdist, boolT *isoutside); +vertexT *qh_nearvertex (facetT *facet, pointT *point, realT *bestdistp); +pointT *qh_point (int id); +setT *qh_pointfacet (void /*qh.facet_list*/); +int qh_pointid (pointT *point); +setT *qh_pointvertex (void /*qh.facet_list*/); +void qh_setvoronoi_all (void); +void qh_triangulate (void /*qh facet_list*/); + +/********* -stat.c prototypes (duplicated from stat.h) **********************/ + +void qh_collectstatistics (void); +void qh_printallstatistics (FILE *fp, char *string); + +#endif /* qhDEFqhull */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull_a.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull_a.h new file mode 100644 index 0000000000000000000000000000000000000000..027cafa78380c60f8d170dec25628df6c88b484c --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull_a.h @@ -0,0 +1,127 @@ +/*<html><pre> -<a href="qh-qhull.htm" + >-------------------------------</a><a name="TOP">-</a> + + qhull_a.h + all header files for compiling qhull + + see qh-qhull.htm + + see qhull.h for user-level definitions + + see user.h for user-defineable constants + + defines internal functions for qhull.c global.c + + copyright (c) 1993-2003, The Geometry Center + + Notes: grep for ((" and (" to catch fprintf("lkasdjf"); + full parens around (x?y:z) + use '#include qhull/qhull_a.h' to avoid name clashes +*/ + +#ifndef qhDEFqhulla +#define qhDEFqhulla + +#include <stdio.h> +#include <stdlib.h> +#include <setjmp.h> +#include <string.h> +#include <math.h> +#include <float.h> /* some compilers will not need float.h */ +#include <limits.h> +#include <time.h> +#include <ctype.h> +/*** uncomment here and qset.c + if string.h does not define memcpy() +#include <memory.h> +*/ +#include "qhull.h" +#include "mem.h" +#include "qset.h" +#include "geom.h" +#include "merge.h" +#include "poly.h" +#include "io.h" +#include "stat.h" + +#if qh_CLOCKtype == 2 /* defined in user.h from qhull.h */ +#include <sys/types.h> +#include <sys/times.h> +#include <unistd.h> +#endif + +#ifdef _MSC_VER /* Microsoft Visual C++ */ +#pragma warning( disable : 4056) /* float constant expression. Looks like a compiler bug */ +#pragma warning( disable : 4146) /* unary minus applied to unsigned type */ +#pragma warning( disable : 4244) /* conversion from 'unsigned long' to 'real' */ +#pragma warning( disable : 4305) /* conversion from 'const double' to 'float' */ +#endif + +/* ======= -macros- =========== */ + +/*-<a href="qh-qhull.htm#TOC" + >--------------------------------</a><a name="traceN">-</a> + + traceN((fp.ferr, "format\n", vars)); + calls fprintf if qh.IStracing >= N + + notes: + removing tracing reduces code size but doesn't change execution speed +*/ +#ifndef qh_NOtrace +#define trace0(args) {if (qh IStracing) fprintf args;} +#define trace1(args) {if (qh IStracing >= 1) fprintf args;} +#define trace2(args) {if (qh IStracing >= 2) fprintf args;} +#define trace3(args) {if (qh IStracing >= 3) fprintf args;} +#define trace4(args) {if (qh IStracing >= 4) fprintf args;} +#define trace5(args) {if (qh IStracing >= 5) fprintf args;} +#else /* qh_NOtrace */ +#define trace0(args) {} +#define trace1(args) {} +#define trace2(args) {} +#define trace3(args) {} +#define trace4(args) {} +#define trace5(args) {} +#endif /* qh_NOtrace */ + +/***** -qhull.c prototypes (alphabetical after qhull) ********************/ + +void qh_qhull (void); +boolT qh_addpoint (pointT *furthest, facetT *facet, boolT checkdist); +void qh_buildhull(void); +void qh_buildtracing (pointT *furthest, facetT *facet); +void qh_build_withrestart (void); +void qh_errexit2(int exitcode, facetT *facet, facetT *otherfacet); +void qh_findhorizon(pointT *point, facetT *facet, int *goodvisible,int *goodhorizon); +pointT *qh_nextfurthest (facetT **visible); +void qh_partitionall(setT *vertices, pointT *points,int npoints); +void qh_partitioncoplanar (pointT *point, facetT *facet, realT *dist); +void qh_partitionpoint (pointT *point, facetT *facet); +void qh_partitionvisible(boolT allpoints, int *numpoints); +void qh_precision (char *reason); +void qh_printsummary(FILE *fp); + +/***** -global.c internal prototypes (alphabetical) ***********************/ + +void qh_appendprint (qh_PRINT format); +void qh_freebuild (boolT allmem); +void qh_freebuffers (void); +void qh_initbuffers (coordT *points, int numpoints, int dim, boolT ismalloc); +int qh_strtol (const char *s, char **endp); +double qh_strtod (const char *s, char **endp); + +/***** -stat.c internal prototypes (alphabetical) ***********************/ + +void qh_allstatA (void); +void qh_allstatB (void); +void qh_allstatC (void); +void qh_allstatD (void); +void qh_allstatE (void); +void qh_allstatE2 (void); +void qh_allstatF (void); +void qh_allstatG (void); +void qh_allstatH (void); +void qh_freebuffers (void); +void qh_initbuffers (coordT *points, int numpoints, int dim, boolT ismalloc); + +#endif /* qhDEFqhulla */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull_interface.cpp b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull_interface.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6ecc640e82b8718a64ccdcb5b3ba40ed795cfeaa --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qhull_interface.cpp @@ -0,0 +1,96 @@ +/*<html><pre> -<a href="qh-user.htm" + >-------------------------------</a><a name="TOP">-</a> +*/ + +#include <iostream.h> +#include <conio.h> + +//--- Include qhull, so it works from with in a C++ source file +//--- +//--- In MVC one cannot just do: +//--- +//--- extern "C" +//--- { +//--- #include "qhull_a.h" +//--- } +//--- +//--- Because qhull_a.h includes math.h, which can not appear +//--- inside a extern "C" declaration. +//--- +//--- Maybe that why Numerical recipes in C avoid this problem, by removing +//--- standard include headers from its header files and add them in the +//--- respective source files instead. +//--- +//--- [K. Erleben] + +#if defined(__cplusplus) +extern "C" +{ +#endif +#include <stdio.h> +#include <stdlib.h> +#include <qhull/qhull.h> +#include <qhull/mem.h> +#include <qhull/qset.h> +#include <qhull/geom.h> +#include <qhull/merge.h> +#include <qhull/poly.h> +#include <qhull/io.h> +#include <qhull/stat.h> +#if defined(__cplusplus) +} +#endif + +/*********************************************************************/ +/* */ +/* */ +/* */ +/* */ +/*********************************************************************/ + +void compute_convex_hull(void) +{ + int dim; /* dimension of points */ + int numpoints; /* number of points */ + coordT *points; /* array of coordinates for each point */ + boolT ismalloc; /* True if qhull should free points in qh_freeqhull() or reallocation */ + char flags[]= "qhull Tv"; /* option flags for qhull, see qh_opt.htm */ + FILE *outfile= stdout; /* output from qh_produce_output() + use NULL to skip qh_produce_output() */ + FILE *errfile= stderr; /* error messages from qhull code */ + int exitcode; /* 0 if no error from qhull */ + facetT *facet; /* set by FORALLfacets */ + int curlong, totlong; /* memory remaining after qh_memfreeshort */ + + /* initialize dim, numpoints, points[], ismalloc here */ + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, + flags, outfile, errfile); + if (!exitcode) { /* if no error */ + /* 'qh facet_list' contains the convex hull */ + FORALLfacets { + /* ... your code ... */ + } + } + qh_freeqhull(!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (errfile, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", + totlong, curlong); +}; + +/*********************************************************************/ +/* */ +/* */ +/* */ +/* */ +/*********************************************************************/ + +void main() +{ + cout << "Hello world" << endl; + + cout << "Press any key..." << endl; + + while(!_kbhit()); + +}; diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qset.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qset.c new file mode 100644 index 0000000000000000000000000000000000000000..f902ea7a6cc7cf0c3f064471d9d83564f9008f6c --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qset.c @@ -0,0 +1,1301 @@ +/*<html><pre> -<a href="qh-set.htm" + >-------------------------------</a><a name="TOP">-</a> + + qset.c + implements set manipulations needed for quickhull + + see qh-set.htm and qset.h + + copyright (c) 1993-2003 The Geometry Center +*/ + +#include <stdio.h> +#include <string.h> +/*** uncomment here and qhull_a.h + if string.h does not define memcpy() +#include <memory.h> +*/ +#include "qset.h" +#include "mem.h" + +#ifndef qhDEFqhull +typedef struct ridgeT ridgeT; +typedef struct facetT facetT; +void qh_errexit(int exitcode, facetT *, ridgeT *); +#endif + +/*=============== internal macros ===========================*/ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="SETsizeaddr_">-</a> + + SETsizeaddr_(set) + return pointer to actual size+1 of set (set CANNOT be NULL!!) + + notes: + *SETsizeaddr==NULL or e[*SETsizeaddr-1].p==NULL +*/ +#define SETsizeaddr_(set) (&((set)->e[(set)->maxsize].i)) + +/*============ functions in alphabetical order ===================*/ + +/*-<a href="qh-set.htm#TOC" + >--------------------------------<a name="setaddnth">-</a> + + qh_setaddnth( setp, nth, newelem) + adds newelem as n'th element of sorted or unsorted *setp + + notes: + *setp and newelem must be defined + *setp may be a temp set + nth=0 is first element + errors if nth is out of bounds + + design: + expand *setp if empty or full + move tail of *setp up one + insert newelem +*/ +void qh_setaddnth(setT **setp, int nth, void *newelem) { + int *sizep, oldsize, i; + void **oldp, **newp; + + if (!*setp || !*(sizep= SETsizeaddr_(*setp))) { + qh_setlarger(setp); + sizep= SETsizeaddr_(*setp); + } + oldsize= *sizep - 1; + if (nth < 0 || nth > oldsize) { + fprintf (qhmem.ferr, "qhull internal error (qh_setaddnth): nth %d is out-of-bounds for set:\n", nth); + qh_setprint (qhmem.ferr, "", *setp); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + (*sizep)++; + oldp= SETelemaddr_(*setp, oldsize, void); /* NULL */ + newp= oldp+1; + for (i= oldsize-nth+1; i--; ) /* move at least NULL */ + *(newp--)= *(oldp--); /* may overwrite *sizep */ + *newp= newelem; +} /* setaddnth */ + + +/*-<a href="qh-set.htm#TOC" + >--------------------------------<a name="setaddsorted">-</a> + + setaddsorted( setp, newelem ) + adds an newelem into sorted *setp + + notes: + *setp and newelem must be defined + *setp may be a temp set + nop if newelem already in set + + design: + find newelem's position in *setp + insert newelem +*/ +void qh_setaddsorted(setT **setp, void *newelem) { + int newindex=0; + void *elem, **elemp; + + FOREACHelem_(*setp) { /* could use binary search instead */ + if (elem < newelem) + newindex++; + else if (elem == newelem) + return; + else + break; + } + qh_setaddnth(setp, newindex, newelem); +} /* setaddsorted */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setappend">-</a> + + qh_setappend( setp, newelem) + append newelem to *setp + + notes: + *setp may be a temp set + *setp and newelem may be NULL + + design: + expand *setp if empty or full + append newelem to *setp + +*/ +void qh_setappend(setT **setp, void *newelem) { + int *sizep; + void **endp; + + if (!newelem) + return; + if (!*setp || !*(sizep= SETsizeaddr_(*setp))) { + qh_setlarger(setp); + sizep= SETsizeaddr_(*setp); + } + *(endp= &((*setp)->e[(*sizep)++ - 1].p))= newelem; + *(++endp)= NULL; +} /* setappend */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setappend_set">-</a> + + qh_setappend_set( setp, setA) + appends setA to *setp + + notes: + *setp can not be a temp set + *setp and setA may be NULL + + design: + setup for copy + expand *setp if it is too small + append all elements of setA to *setp +*/ +void qh_setappend_set(setT **setp, setT *setA) { + int *sizep, sizeA, size; + setT *oldset; + + if (!setA) + return; + SETreturnsize_(setA, sizeA); + if (!*setp) + *setp= qh_setnew (sizeA); + sizep= SETsizeaddr_(*setp); + if (!(size= *sizep)) + size= (*setp)->maxsize; + else + size--; + if (size + sizeA > (*setp)->maxsize) { + oldset= *setp; + *setp= qh_setcopy (oldset, sizeA); + qh_setfree (&oldset); + sizep= SETsizeaddr_(*setp); + } + *sizep= size+sizeA+1; /* memcpy may overwrite */ + if (sizeA > 0) + memcpy((char *)&((*setp)->e[size].p), (char *)&(setA->e[0].p), SETelemsize *(sizeA+1)); +} /* setappend_set */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setappend2ndlast">-</a> + + qh_setappend2ndlast( setp, newelem ) + makes newelem the next to the last element in *setp + + notes: + *setp must have at least one element + newelem must be defined + *setp may be a temp set + + design: + expand *setp if empty or full + move last element of *setp up one + insert newelem +*/ +void qh_setappend2ndlast(setT **setp, void *newelem) { + int *sizep; + void **endp, **lastp; + + if (!*setp || !*(sizep= SETsizeaddr_(*setp))) { + qh_setlarger(setp); + sizep= SETsizeaddr_(*setp); + } + endp= SETelemaddr_(*setp, (*sizep)++ -1, void); /* NULL */ + lastp= endp-1; + *(endp++)= *lastp; + *endp= NULL; /* may overwrite *sizep */ + *lastp= newelem; +} /* setappend2ndlast */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setcheck">-</a> + + qh_setcheck( set, typename, id ) + check set for validity + report errors with typename and id + + design: + checks that maxsize, actual size, and NULL terminator agree +*/ +void qh_setcheck(setT *set, char *tname, int id) { + int maxsize, size; + int waserr= 0; + + if (!set) + return; + SETreturnsize_(set, size); + maxsize= set->maxsize; + if (size > maxsize || !maxsize) { + fprintf (qhmem.ferr, "qhull internal error (qh_setcheck): actual size %d of %s%d is greater than max size %d\n", + size, tname, id, maxsize); + waserr= 1; + }else if (set->e[size].p) { + fprintf (qhmem.ferr, "qhull internal error (qh_setcheck): %s%d (size %d max %d) is not null terminated.\n", + tname, id, maxsize, size-1); + waserr= 1; + } + if (waserr) { + qh_setprint (qhmem.ferr, "ERRONEOUS", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } +} /* setcheck */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setcompact">-</a> + + qh_setcompact( set ) + remove internal NULLs from an unsorted set + + returns: + updated set + + notes: + set may be NULL + it would be faster to swap tail of set into holes, like qh_setdel + + design: + setup pointers into set + skip NULLs while copying elements to start of set + update the actual size +*/ +void qh_setcompact(setT *set) { + int size; + void **destp, **elemp, **endp, **firstp; + + if (!set) + return; + SETreturnsize_(set, size); + destp= elemp= firstp= SETaddr_(set, void); + endp= destp + size; + while (1) { + if (!(*destp++ = *elemp++)) { + destp--; + if (elemp > endp) + break; + } + } + qh_settruncate (set, destp-firstp); +} /* setcompact */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setcopy">-</a> + + qh_setcopy( set, extra ) + make a copy of a sorted or unsorted set with extra slots + + returns: + new set + + design: + create a newset with extra slots + copy the elements to the newset + +*/ +setT *qh_setcopy(setT *set, int extra) { + setT *newset; + int size; + + if (extra < 0) + extra= 0; + SETreturnsize_(set, size); + newset= qh_setnew(size+extra); + *SETsizeaddr_(newset)= size+1; /* memcpy may overwrite */ + memcpy((char *)&(newset->e[0].p), (char *)&(set->e[0].p), SETelemsize *(size+1)); + return (newset); +} /* setcopy */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setdel">-</a> + + qh_setdel( set, oldelem ) + delete oldelem from an unsorted set + + returns: + returns oldelem if found + returns NULL otherwise + + notes: + set may be NULL + oldelem must not be NULL; + only deletes one copy of oldelem in set + + design: + locate oldelem + update actual size if it was full + move the last element to the oldelem's location +*/ +void *qh_setdel(setT *set, void *oldelem) { + void **elemp, **lastp; + int *sizep; + + if (!set) + return NULL; + elemp= SETaddr_(set, void); + while (*elemp != oldelem && *elemp) + elemp++; + if (*elemp) { + sizep= SETsizeaddr_(set); + if (!(*sizep)--) /* if was a full set */ + *sizep= set->maxsize; /* *sizep= (maxsize-1)+ 1 */ + lastp= SETelemaddr_(set, *sizep-1, void); + *elemp= *lastp; /* may overwrite itself */ + *lastp= NULL; + return oldelem; + } + return NULL; +} /* setdel */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setdellast">-</a> + + qh_setdellast( set) + return last element of set or NULL + + notes: + deletes element from set + set may be NULL + + design: + return NULL if empty + if full set + delete last element and set actual size + else + delete last element and update actual size +*/ +void *qh_setdellast(setT *set) { + int setsize; /* actually, actual_size + 1 */ + int maxsize; + int *sizep; + void *returnvalue; + + if (!set || !(set->e[0].p)) + return NULL; + sizep= SETsizeaddr_(set); + if ((setsize= *sizep)) { + returnvalue= set->e[setsize - 2].p; + set->e[setsize - 2].p= NULL; + (*sizep)--; + }else { + maxsize= set->maxsize; + returnvalue= set->e[maxsize - 1].p; + set->e[maxsize - 1].p= NULL; + *sizep= maxsize; + } + return returnvalue; +} /* setdellast */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setdelnth">-</a> + + qh_setdelnth( set, nth ) + deletes nth element from unsorted set + 0 is first element + + returns: + returns the element (needs type conversion) + + notes: + errors if nth invalid + + design: + setup points and check nth + delete nth element and overwrite with last element +*/ +void *qh_setdelnth(setT *set, int nth) { + void **elemp, **lastp, *elem; + int *sizep; + + + elemp= SETelemaddr_(set, nth, void); + sizep= SETsizeaddr_(set); + if (!(*sizep)--) /* if was a full set */ + *sizep= set->maxsize; /* *sizep= (maxsize-1)+ 1 */ + if (nth < 0 || nth >= *sizep) { + fprintf (qhmem.ferr, "qhull internal error (qh_setaddnth): nth %d is out-of-bounds for set:\n", nth); + qh_setprint (qhmem.ferr, "", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + lastp= SETelemaddr_(set, *sizep-1, void); + elem= *elemp; + *elemp= *lastp; /* may overwrite itself */ + *lastp= NULL; + return elem; +} /* setdelnth */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setdelnthsorted">-</a> + + qh_setdelnthsorted( set, nth ) + deletes nth element from sorted set + + returns: + returns the element (use type conversion) + + notes: + errors if nth invalid + + see also: + setnew_delnthsorted + + design: + setup points and check nth + copy remaining elements down one + update actual size +*/ +void *qh_setdelnthsorted(setT *set, int nth) { + void **newp, **oldp, *elem; + int *sizep; + + sizep= SETsizeaddr_(set); + if (nth < 0 || (*sizep && nth >= *sizep-1) || nth >= set->maxsize) { + fprintf (qhmem.ferr, "qhull internal error (qh_setaddnth): nth %d is out-of-bounds for set:\n", nth); + qh_setprint (qhmem.ferr, "", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + newp= SETelemaddr_(set, nth, void); + elem= *newp; + oldp= newp+1; + while ((*(newp++)= *(oldp++))) + ; /* copy remaining elements and NULL */ + if (!(*sizep)--) /* if was a full set */ + *sizep= set->maxsize; /* *sizep= (max size-1)+ 1 */ + return elem; +} /* setdelnthsorted */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setdelsorted">-</a> + + qh_setdelsorted( set, oldelem ) + deletes oldelem from sorted set + + returns: + returns oldelem if it was deleted + + notes: + set may be NULL + + design: + locate oldelem in set + copy remaining elements down one + update actual size +*/ +void *qh_setdelsorted(setT *set, void *oldelem) { + void **newp, **oldp; + int *sizep; + + if (!set) + return NULL; + newp= SETaddr_(set, void); + while(*newp != oldelem && *newp) + newp++; + if (*newp) { + oldp= newp+1; + while ((*(newp++)= *(oldp++))) + ; /* copy remaining elements */ + sizep= SETsizeaddr_(set); + if (!(*sizep)--) /* if was a full set */ + *sizep= set->maxsize; /* *sizep= (max size-1)+ 1 */ + return oldelem; + } + return NULL; +} /* setdelsorted */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setduplicate">-</a> + + qh_setduplicate( set, elemsize ) + duplicate a set of elemsize elements + + notes: + use setcopy if retaining old elements + + design: + create a new set + for each elem of the old set + create a newelem + append newelem to newset +*/ +setT *qh_setduplicate (setT *set, int elemsize) { + void *elem, **elemp, *newElem; + setT *newSet; + int size; + + if (!(size= qh_setsize (set))) + return NULL; + newSet= qh_setnew (size); + FOREACHelem_(set) { + newElem= qh_memalloc (elemsize); + memcpy (newElem, elem, elemsize); + qh_setappend (&newSet, newElem); + } + return newSet; +} /* setduplicate */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setequal">-</a> + + qh_setequal( ) + returns 1 if two sorted sets are equal, otherwise returns 0 + + notes: + either set may be NULL + + design: + check size of each set + setup pointers + compare elements of each set +*/ +int qh_setequal(setT *setA, setT *setB) { + void **elemAp, **elemBp; + int sizeA, sizeB; + + SETreturnsize_(setA, sizeA); + SETreturnsize_(setB, sizeB); + if (sizeA != sizeB) + return 0; + if (!sizeA) + return 1; + elemAp= SETaddr_(setA, void); + elemBp= SETaddr_(setB, void); + if (!memcmp((char *)elemAp, (char *)elemBp, sizeA*SETelemsize)) + return 1; + return 0; +} /* setequal */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setequal_except">-</a> + + qh_setequal_except( setA, skipelemA, setB, skipelemB ) + returns 1 if sorted setA and setB are equal except for skipelemA & B + + returns: + false if either skipelemA or skipelemB are missing + + notes: + neither set may be NULL + + if skipelemB is NULL, + can skip any one element of setB + + design: + setup pointers + search for skipelemA, skipelemB, and mismatches + check results +*/ +int qh_setequal_except (setT *setA, void *skipelemA, setT *setB, void *skipelemB) { + void **elemA, **elemB; + int skip=0; + + elemA= SETaddr_(setA, void); + elemB= SETaddr_(setB, void); + while (1) { + if (*elemA == skipelemA) { + skip++; + elemA++; + } + if (skipelemB) { + if (*elemB == skipelemB) { + skip++; + elemB++; + } + }else if (*elemA != *elemB) { + skip++; + if (!(skipelemB= *elemB++)) + return 0; + } + if (!*elemA) + break; + if (*elemA++ != *elemB++) + return 0; + } + if (skip != 2 || *elemB) + return 0; + return 1; +} /* setequal_except */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setequal_skip">-</a> + + qh_setequal_skip( setA, skipA, setB, skipB ) + returns 1 if sorted setA and setB are equal except for elements skipA & B + + returns: + false if different size + + notes: + neither set may be NULL + + design: + setup pointers + search for mismatches while skipping skipA and skipB +*/ +int qh_setequal_skip (setT *setA, int skipA, setT *setB, int skipB) { + void **elemA, **elemB, **skipAp, **skipBp; + + elemA= SETaddr_(setA, void); + elemB= SETaddr_(setB, void); + skipAp= SETelemaddr_(setA, skipA, void); + skipBp= SETelemaddr_(setB, skipB, void); + while (1) { + if (elemA == skipAp) + elemA++; + if (elemB == skipBp) + elemB++; + if (!*elemA) + break; + if (*elemA++ != *elemB++) + return 0; + } + if (*elemB) + return 0; + return 1; +} /* setequal_skip */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setfree">-</a> + + qh_setfree( setp ) + frees the space occupied by a sorted or unsorted set + + returns: + sets setp to NULL + + notes: + set may be NULL + + design: + free array + free set +*/ +void qh_setfree(setT **setp) { + int size; + void **freelistp; /* used !qh_NOmem */ + + if (*setp) { + size= sizeof(setT) + ((*setp)->maxsize)*SETelemsize; + if (size <= qhmem.LASTsize) { + qh_memfree_(*setp, size, freelistp); + }else + qh_memfree (*setp, size); + *setp= NULL; + } +} /* setfree */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setfree2">-</a> + + qh_setfree2( setp, elemsize ) + frees the space occupied by a set and its elements + + notes: + set may be NULL + + design: + free each element + free set +*/ +void qh_setfree2 (setT **setp, int elemsize) { + void *elem, **elemp; + + FOREACHelem_(*setp) + qh_memfree (elem, elemsize); + qh_setfree (setp); +} /* setfree2 */ + + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setfreelong">-</a> + + qh_setfreelong( setp ) + frees a set only if it's in long memory + + returns: + sets setp to NULL if it is freed + + notes: + set may be NULL + + design: + if set is large + free it +*/ +void qh_setfreelong(setT **setp) { + int size; + + if (*setp) { + size= sizeof(setT) + ((*setp)->maxsize)*SETelemsize; + if (size > qhmem.LASTsize) { + qh_memfree (*setp, size); + *setp= NULL; + } + } +} /* setfreelong */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setin">-</a> + + qh_setin( set, setelem ) + returns 1 if setelem is in a set, 0 otherwise + + notes: + set may be NULL or unsorted + + design: + scans set for setelem +*/ +int qh_setin(setT *set, void *setelem) { + void *elem, **elemp; + + FOREACHelem_(set) { + if (elem == setelem) + return 1; + } + return 0; +} /* setin */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setindex">-</a> + + qh_setindex( set, atelem ) + returns the index of atelem in set. + returns -1, if not in set or maxsize wrong + + notes: + set may be NULL and may contain nulls. + + design: + checks maxsize + scans set for atelem +*/ +int qh_setindex(setT *set, void *atelem) { + void **elem; + int size, i; + + SETreturnsize_(set, size); + if (size > set->maxsize) + return -1; + elem= SETaddr_(set, void); + for (i=0; i < size; i++) { + if (*elem++ == atelem) + return i; + } + return -1; +} /* setindex */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setlarger">-</a> + + qh_setlarger( oldsetp ) + returns a larger set that contains all elements of *oldsetp + + notes: + the set is at least twice as large + if temp set, updates qhmem.tempstack + + design: + creates a new set + copies the old set to the new set + updates pointers in tempstack + deletes the old set +*/ +void qh_setlarger(setT **oldsetp) { + int size= 1, *sizep; + setT *newset, *set, **setp, *oldset; + void **oldp, **newp; + + if (*oldsetp) { + oldset= *oldsetp; + SETreturnsize_(oldset, size); + qhmem.cntlarger++; + qhmem.totlarger += size+1; + newset= qh_setnew(2 * size); + oldp= SETaddr_(oldset, void); + newp= SETaddr_(newset, void); + memcpy((char *)newp, (char *)oldp, (size+1) * SETelemsize); + sizep= SETsizeaddr_(newset); + *sizep= size+1; + FOREACHset_((setT *)qhmem.tempstack) { + if (set == oldset) + *(setp-1)= newset; + } + qh_setfree(oldsetp); + }else + newset= qh_setnew(3); + *oldsetp= newset; +} /* setlarger */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setlast">-</a> + + qh_setlast( ) + return last element of set or NULL (use type conversion) + + notes: + set may be NULL + + design: + return last element +*/ +void *qh_setlast(setT *set) { + int size; + + if (set) { + size= *SETsizeaddr_(set); + if (!size) + return SETelem_(set, set->maxsize - 1); + else if (size > 1) + return SETelem_(set, size - 2); + } + return NULL; +} /* setlast */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setnew">-</a> + + qh_setnew( setsize ) + creates and allocates space for a set + + notes: + setsize means the number of elements (NOT including the NULL terminator) + use qh_settemp/qh_setfreetemp if set is temporary + + design: + allocate memory for set + roundup memory if small set + initialize as empty set +*/ +setT *qh_setnew(int setsize) { + setT *set; + int sizereceived; /* used !qh_NOmem */ + int size; + void **freelistp; /* used !qh_NOmem */ + + if (!setsize) + setsize++; + size= sizeof(setT) + setsize * SETelemsize; + if ((unsigned) size <= (unsigned) qhmem.LASTsize) { + qh_memalloc_(size, freelistp, set, setT); +#ifndef qh_NOmem + sizereceived= qhmem.sizetable[ qhmem.indextable[size]]; + if (sizereceived > size) + setsize += (sizereceived - size)/SETelemsize; +#endif + }else + set= (setT*)qh_memalloc (size); + set->maxsize= setsize; + set->e[setsize].i= 1; + set->e[0].p= NULL; + return (set); +} /* setnew */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setnew_delnthsorted">-</a> + + qh_setnew_delnthsorted( set, size, nth, prepend ) + creates a sorted set not containing nth element + if prepend, the first prepend elements are undefined + + notes: + set must be defined + checks nth + see also: setdelnthsorted + + design: + create new set + setup pointers and allocate room for prepend'ed entries + append head of old set to new set + append tail of old set to new set +*/ +setT *qh_setnew_delnthsorted(setT *set, int size, int nth, int prepend) { + setT *newset; + void **oldp, **newp; + int tailsize= size - nth -1, newsize; + + if (tailsize < 0) { + fprintf (qhmem.ferr, "qhull internal error (qh_setaddnth): nth %d is out-of-bounds for set:\n", nth); + qh_setprint (qhmem.ferr, "", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + newsize= size-1 + prepend; + newset= qh_setnew(newsize); + newset->e[newset->maxsize].i= newsize+1; /* may be overwritten */ + oldp= SETaddr_(set, void); + newp= SETaddr_(newset, void) + prepend; + switch (nth) { + case 0: + break; + case 1: + *(newp++)= *oldp++; + break; + case 2: + *(newp++)= *oldp++; + *(newp++)= *oldp++; + break; + case 3: + *(newp++)= *oldp++; + *(newp++)= *oldp++; + *(newp++)= *oldp++; + break; + case 4: + *(newp++)= *oldp++; + *(newp++)= *oldp++; + *(newp++)= *oldp++; + *(newp++)= *oldp++; + break; + default: + memcpy((char *)newp, (char *)oldp, nth * SETelemsize); + newp += nth; + oldp += nth; + break; + } + oldp++; + switch (tailsize) { + case 0: + break; + case 1: + *(newp++)= *oldp++; + break; + case 2: + *(newp++)= *oldp++; + *(newp++)= *oldp++; + break; + case 3: + *(newp++)= *oldp++; + *(newp++)= *oldp++; + *(newp++)= *oldp++; + break; + case 4: + *(newp++)= *oldp++; + *(newp++)= *oldp++; + *(newp++)= *oldp++; + *(newp++)= *oldp++; + break; + default: + memcpy((char *)newp, (char *)oldp, tailsize * SETelemsize); + newp += tailsize; + } + *newp= NULL; + return(newset); +} /* setnew_delnthsorted */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setprint">-</a> + + qh_setprint( fp, string, set ) + print set elements to fp with identifying string + + notes: + never errors +*/ +void qh_setprint(FILE *fp, char* string, setT *set) { + int size, k; + + if (!set) + fprintf (fp, "%s set is null\n", string); + else { + SETreturnsize_(set, size); + fprintf (fp, "%s set=%p maxsize=%d size=%d elems=", + string, set, set->maxsize, size); + if (size > set->maxsize) + size= set->maxsize+1; + for (k=0; k < size; k++) + fprintf(fp, " %p", set->e[k].p); + fprintf(fp, "\n"); + } +} /* setprint */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setreplace">-</a> + + qh_setreplace( set, oldelem, newelem ) + replaces oldelem in set with newelem + + notes: + errors if oldelem not in the set + newelem may be NULL, but it turns the set into an indexed set (no FOREACH) + + design: + find oldelem + replace with newelem +*/ +void qh_setreplace(setT *set, void *oldelem, void *newelem) { + void **elemp; + + elemp= SETaddr_(set, void); + while(*elemp != oldelem && *elemp) + elemp++; + if (*elemp) + *elemp= newelem; + else { + fprintf (qhmem.ferr, "qhull internal error (qh_setreplace): elem %p not found in set\n", + oldelem); + qh_setprint (qhmem.ferr, "", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } +} /* setreplace */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setsize">-</a> + + qh_setsize( set ) + returns the size of a set + + notes: + errors if set's maxsize is incorrect + same as SETreturnsize_(set) + + design: + determine actual size of set from maxsize +*/ +int qh_setsize(setT *set) { + int size, *sizep; + + if (!set) + return (0); + sizep= SETsizeaddr_(set); + if ((size= *sizep)) { + size--; + if (size > set->maxsize) { + fprintf (qhmem.ferr, "qhull internal error (qh_setsize): current set size %d is greater than maximum size %d\n", + size, set->maxsize); + qh_setprint (qhmem.ferr, "set: ", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + }else + size= set->maxsize; + return size; +} /* setsize */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="settemp">-</a> + + qh_settemp( setsize ) + return a stacked, temporary set of upto setsize elements + + notes: + use settempfree or settempfree_all to release from qhmem.tempstack + see also qh_setnew + + design: + allocate set + append to qhmem.tempstack + +*/ +setT *qh_settemp(int setsize) { + setT *newset; + + newset= qh_setnew (setsize); + qh_setappend ((setT **)&qhmem.tempstack, newset); + if (qhmem.IStracing >= 5) + fprintf (qhmem.ferr, "qh_settemp: temp set %p of %d elements, depth %d\n", + newset, newset->maxsize, qh_setsize ((setT*)qhmem.tempstack)); + return newset; +} /* settemp */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="settempfree">-</a> + + qh_settempfree( set ) + free temporary set at top of qhmem.tempstack + + notes: + nop if set is NULL + errors if set not from previous qh_settemp + + to locate errors: + use 'T2' to find source and then find mis-matching qh_settemp + + design: + check top of qhmem.tempstack + free it +*/ +void qh_settempfree(setT **set) { + setT *stackedset; + + if (!*set) + return; + stackedset= qh_settemppop (); + if (stackedset != *set) { + qh_settemppush(stackedset); + fprintf (qhmem.ferr, "qhull internal error (qh_settempfree): set %p (size %d) was not last temporary allocated (depth %d, set %p, size %d)\n", + *set, qh_setsize(*set), qh_setsize((setT*)qhmem.tempstack)+1, + stackedset, qh_setsize(stackedset)); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + qh_setfree (set); +} /* settempfree */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="settempfree_all">-</a> + + qh_settempfree_all( ) + free all temporary sets in qhmem.tempstack + + design: + for each set in tempstack + free set + free qhmem.tempstack +*/ +void qh_settempfree_all(void) { + setT *set, **setp; + + FOREACHset_((setT *)qhmem.tempstack) + qh_setfree(&set); + qh_setfree((setT **)&qhmem.tempstack); +} /* settempfree_all */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="settemppop">-</a> + + qh_settemppop( ) + pop and return temporary set from qhmem.tempstack + + notes: + the returned set is permanent + + design: + pop and check top of qhmem.tempstack +*/ +setT *qh_settemppop(void) { + setT *stackedset; + + stackedset= (setT*)qh_setdellast((setT *)qhmem.tempstack); + if (!stackedset) { + fprintf (qhmem.ferr, "qhull internal error (qh_settemppop): pop from empty temporary stack\n"); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + if (qhmem.IStracing >= 5) + fprintf (qhmem.ferr, "qh_settemppop: depth %d temp set %p of %d elements\n", + qh_setsize((setT*)qhmem.tempstack)+1, stackedset, qh_setsize(stackedset)); + return stackedset; +} /* settemppop */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="settemppush">-</a> + + qh_settemppush( set ) + push temporary set unto qhmem.tempstack (makes it temporary) + + notes: + duplicates settemp() for tracing + + design: + append set to tempstack +*/ +void qh_settemppush(setT *set) { + + qh_setappend ((setT**)&qhmem.tempstack, set); + if (qhmem.IStracing >= 5) + fprintf (qhmem.ferr, "qh_settemppush: depth %d temp set %p of %d elements\n", + qh_setsize((setT*)qhmem.tempstack), set, qh_setsize(set)); +} /* settemppush */ + + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="settruncate">-</a> + + qh_settruncate( set, size ) + truncate set to size elements + + notes: + set must be defined + + see: + SETtruncate_ + + design: + check size + update actual size of set +*/ +void qh_settruncate (setT *set, int size) { + + if (size < 0 || size > set->maxsize) { + fprintf (qhmem.ferr, "qhull internal error (qh_settruncate): size %d out of bounds for set:\n", size); + qh_setprint (qhmem.ferr, "", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + set->e[set->maxsize].i= size+1; /* maybe overwritten */ + set->e[size].p= NULL; +} /* settruncate */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setunique">-</a> + + qh_setunique( set, elem ) + add elem to unsorted set unless it is already in set + + notes: + returns 1 if it is appended + + design: + if elem not in set + append elem to set +*/ +int qh_setunique (setT **set, void *elem) { + + if (!qh_setin (*set, elem)) { + qh_setappend (set, elem); + return 1; + } + return 0; +} /* setunique */ + +/*-<a href="qh-set.htm#TOC" + >-------------------------------<a name="setzero">-</a> + + qh_setzero( set, index, size ) + zero elements from index on + set actual size of set to size + + notes: + set must be defined + the set becomes an indexed set (can not use FOREACH...) + + see also: + qh_settruncate + + design: + check index and size + update actual size + zero elements starting at e[index] +*/ +void qh_setzero (setT *set, int index, int size) { + int count; + + if (index < 0 || index >= size || size > set->maxsize) { + fprintf (qhmem.ferr, "qhull internal error (qh_setzero): index %d or size %d out of bounds for set:\n", index, size); + qh_setprint (qhmem.ferr, "", set); + qh_errexit (qhmem_ERRqhull, NULL, NULL); + } + set->e[set->maxsize].i= size+1; /* may be overwritten */ + count= size - index + 1; /* +1 for NULL terminator */ + memset ((char *)SETelemaddr_(set, index, void), 0, count * SETelemsize); +} /* setzero */ + + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qset.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qset.h new file mode 100644 index 0000000000000000000000000000000000000000..57524984e2b81840920475f2567ec4c0e0f6456b --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/qset.h @@ -0,0 +1,468 @@ +/*<html><pre> -<a href="qh-set.htm" + >-------------------------------</a><a name="TOP">-</a> + + qset.h + header file for qset.c that implements set + + see qh-set.htm and qset.c + + only uses mem.c, malloc/free + + for error handling, writes message and calls + qh_errexit (qhmem_ERRqhull, NULL, NULL); + + set operations satisfy the following properties: + - sets have a max size, the actual size (if different) is stored at the end + - every set is NULL terminated + - sets may be sorted or unsorted, the caller must distinguish this + + copyright (c) 1993-2003, The Geometry Center +*/ + +#ifndef qhDEFset +#define qhDEFset 1 + +/*================= -structures- ===============*/ + +#ifndef DEFsetT +#define DEFsetT 1 +typedef struct setT setT; /* a set is a sorted or unsorted array of pointers */ +#endif + +/*-<a href="qh-set.htm#TOC" +>----------------------------------------</a><a name="setT">-</a> + +setT + a set or list of pointers with maximum size and actual size. + +variations: + unsorted, unique -- a list of unique pointers with NULL terminator + user guarantees uniqueness + sorted -- a sorted list of unique pointers with NULL terminator + qset.c guarantees uniqueness + unsorted -- a list of pointers terminated with NULL + indexed -- an array of pointers with NULL elements + +structure for set of n elements: + + -------------- + | maxsize + -------------- + | e[0] - a pointer, may be NULL for indexed sets + -------------- + | e[1] + + -------------- + | ... + -------------- + | e[n-1] + -------------- + | e[n] = NULL + -------------- + | ... + -------------- + | e[maxsize] - n+1 or NULL (determines actual size of set) + -------------- + +*/ + +/*-- setelemT -- internal type to allow both pointers and indices +*/ +typedef union setelemT setelemT; +union setelemT { + void *p; + int i; /* integer used for e[maxSize] */ +}; + +struct setT { + int maxsize; /* maximum number of elements (except NULL) */ + setelemT e[1]; /* array of pointers, tail is NULL */ + /* last slot (unless NULL) is actual size+1 + e[maxsize]==NULL or e[e[maxsize]-1]==NULL */ + /* this may generate a warning since e[] contains + maxsize elements */ +}; + +/*=========== -constants- =========================*/ + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="SETelemsize">-</a> + + SETelemsize + size of a set element in bytes +*/ +#define SETelemsize sizeof(setelemT) + + +/*=========== -macros- =========================*/ + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHsetelement_">-</a> + + FOREACHsetelement_(type, set, variable) + define FOREACH iterator + + declare: + assumes *variable and **variablep are declared + no space in "variable)" [DEC Alpha cc compiler] + + each iteration: + variable is set element + variablep is one beyond variable. + + to repeat an element: + variablep--; / *repeat* / + + at exit: + variable is NULL at end of loop + + example: + #define FOREACHfacet_( facets ) FOREACHsetelement_( facetT, facets, facet ) + + notes: + use FOREACHsetelement_i_() if need index or include NULLs + + WARNING: + nested loops can't use the same variable (define another FOREACH) + + needs braces if nested inside another FOREACH + this includes intervening blocks, e.g. FOREACH...{ if () FOREACH...} ) +*/ +#define FOREACHsetelement_(type, set, variable) \ + if (((variable= NULL), set)) for(\ + variable##p= (type **)&((set)->e[0].p); \ + (variable= *variable##p++);) + +/*-<a href="qh-set.htm#TOC" + >----------------------------------------</a><a name="FOREACHsetelement_i_">-</a> + + FOREACHsetelement_i_(type, set, variable) + define indexed FOREACH iterator + + declare: + type *variable, variable_n, variable_i; + + each iteration: + variable is set element, may be NULL + variable_i is index, variable_n is qh_setsize() + + to repeat an element: + variable_i--; variable_n-- repeats for deleted element + + at exit: + variable==NULL and variable_i==variable_n + + example: + #define FOREACHfacet_i_( facets ) FOREACHsetelement_i_( facetT, facets, facet ) + + WARNING: + nested loops can't use the same variable (define another FOREACH) + + needs braces if nested inside another FOREACH + this includes intervening blocks, e.g. FOREACH...{ if () FOREACH...} ) +*/ +#define FOREACHsetelement_i_(type, set, variable) \ + if (((variable= NULL), set)) for (\ + variable##_i= 0, variable= (type *)((set)->e[0].p), \ + variable##_n= qh_setsize(set);\ + variable##_i < variable##_n;\ + variable= (type *)((set)->e[++variable##_i].p) ) + +/*-<a href="qh-set.htm#TOC" + >--------------------------------------</a><a name="FOREACHsetelementreverse_">-</a> + + FOREACHsetelementreverse_(type, set, variable)- + define FOREACH iterator in reverse order + + declare: + assumes *variable and **variablep are declared + also declare 'int variabletemp' + + each iteration: + variable is set element + + to repeat an element: + variabletemp++; / *repeat* / + + at exit: + variable is NULL + + example: + #define FOREACHvertexreverse_( vertices ) FOREACHsetelementreverse_( vertexT, vertices, vertex ) + + notes: + use FOREACHsetelementreverse12_() to reverse first two elements + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHsetelementreverse_(type, set, variable) \ + if (((variable= NULL), set)) for(\ + variable##temp= qh_setsize(set)-1, variable= qh_setlast(set);\ + variable; variable= \ + ((--variable##temp >= 0) ? SETelemt_(set, variable##temp, type) : NULL)) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHsetelementreverse12_">-</a> + + FOREACHsetelementreverse12_(type, set, variable)- + define FOREACH iterator with e[1] and e[0] reversed + + declare: + assumes *variable and **variablep are declared + + each iteration: + variable is set element + variablep is one after variable. + + to repeat an element: + variablep--; / *repeat* / + + at exit: + variable is NULL at end of loop + + example + #define FOREACHvertexreverse12_( vertices ) FOREACHsetelementreverse12_( vertexT, vertices, vertex ) + + notes: + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHsetelementreverse12_(type, set, variable) \ + if (((variable= NULL), set)) for(\ + variable##p= (type **)&((set)->e[1].p); \ + (variable= *variable##p); \ + variable##p == ((type **)&((set)->e[0].p))?variable##p += 2: \ + (variable##p == ((type **)&((set)->e[1].p))?variable##p--:variable##p++)) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHelem_">-</a> + + FOREACHelem_( set )- + iterate elements in a set + + declare: + void *elem, *elemp; + + each iteration: + elem is set element + elemp is one beyond + + to repeat an element: + elemp--; / *repeat* / + + at exit: + elem == NULL at end of loop + + example: + FOREACHelem_(set) { + + notes: + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHelem_(set) FOREACHsetelement_(void, set, elem) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------</a><a name="FOREACHset_">-</a> + + FOREACHset_( set )- + iterate a set of sets + + declare: + setT *set, **setp; + + each iteration: + set is set element + setp is one beyond + + to repeat an element: + setp--; / *repeat* / + + at exit: + set == NULL at end of loop + + example + FOREACHset_(sets) { + + notes: + WARNING: needs braces if nested inside another FOREACH +*/ +#define FOREACHset_(sets) FOREACHsetelement_(setT, sets, set) + +/*-<a href="qh-set.htm#TOC" + >-----------------------------------------</a><a name="SETindex_">-</a> + + SETindex_( set, elem ) + return index of elem in set + + notes: + for use with FOREACH iteration + + example: + i= SETindex_(ridges, ridge) +*/ +#define SETindex_(set, elem) ((void **)elem##p - (void **)&(set)->e[1].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETref_">-</a> + + SETref_( elem ) + l.h.s. for modifying the current element in a FOREACH iteration + + example: + SETref_(ridge)= anotherridge; +*/ +#define SETref_(elem) (elem##p[-1]) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETelem_">-</a> + + SETelem_(set, n) + return the n'th element of set + + notes: + assumes that n is valid [0..size] and that set is defined + use SETelemt_() for type cast +*/ +#define SETelem_(set, n) ((set)->e[n].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETelemt_">-</a> + + SETelemt_(set, n, type) + return the n'th element of set as a type + + notes: + assumes that n is valid [0..size] and that set is defined +*/ +#define SETelemt_(set, n, type) ((type*)((set)->e[n].p)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETelemaddr_">-</a> + + SETelemaddr_(set, n, type) + return address of the n'th element of a set + + notes: + assumes that n is valid [0..size] and set is defined +*/ +#define SETelemaddr_(set, n, type) ((type **)(&((set)->e[n].p))) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETfirst_">-</a> + + SETfirst_(set) + return first element of set + +*/ +#define SETfirst_(set) ((set)->e[0].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETfirstt_">-</a> + + SETfirstt_(set, type) + return first element of set as a type + +*/ +#define SETfirstt_(set, type) ((type*)((set)->e[0].p)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETsecond_">-</a> + + SETsecond_(set) + return second element of set + +*/ +#define SETsecond_(set) ((set)->e[1].p) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETsecondt_">-</a> + + SETsecondt_(set, type) + return second element of set as a type +*/ +#define SETsecondt_(set, type) ((type*)((set)->e[1].p)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETaddr_">-</a> + + SETaddr_(set, type) + return address of set's elements +*/ +#define SETaddr_(set,type) ((type **)(&((set)->e[0].p))) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETreturnsize_">-</a> + + SETreturnsize_(set, size) + return size of a set + + notes: + set must be defined + use qh_setsize(set) unless speed is critical +*/ +#define SETreturnsize_(set, size) (((size)= ((set)->e[(set)->maxsize].i))?(--(size)):((size)= (set)->maxsize)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETempty_">-</a> + + SETempty_(set) + return true (1) if set is empty + + notes: + set may be NULL +*/ +#define SETempty_(set) (!set || (SETfirst_(set) ? 0:1)) + +/*-<a href="qh-set.htm#TOC" + >---------------------------------------</a><a name="SETtruncate_">-</a> + + SETtruncate_(set) + return first element of set + + see: + qh_settruncate() + +*/ +#define SETtruncate_(set, size) {set->e[set->maxsize].i= size+1; /* maybe overwritten */ \ + set->e[size].p= NULL;} + +/*======= prototypes in alphabetical order ============*/ + +void qh_setaddsorted(setT **setp, void *elem); +void qh_setaddnth(setT **setp, int nth, void *newelem); +void qh_setappend(setT **setp, void *elem); +void qh_setappend_set(setT **setp, setT *setA); +void qh_setappend2ndlast(setT **setp, void *elem); +void qh_setcheck(setT *set, char *tname, int id); +void qh_setcompact(setT *set); +setT *qh_setcopy(setT *set, int extra); +void *qh_setdel(setT *set, void *elem); +void *qh_setdellast(setT *set); +void *qh_setdelnth(setT *set, int nth); +void *qh_setdelnthsorted(setT *set, int nth); +void *qh_setdelsorted(setT *set, void *newelem); +setT *qh_setduplicate( setT *set, int elemsize); +int qh_setequal(setT *setA, setT *setB); +int qh_setequal_except (setT *setA, void *skipelemA, setT *setB, void *skipelemB); +int qh_setequal_skip (setT *setA, int skipA, setT *setB, int skipB); +void qh_setfree(setT **set); +void qh_setfree2( setT **setp, int elemsize); +void qh_setfreelong(setT **set); +int qh_setin(setT *set, void *setelem); +int qh_setindex(setT *set, void *setelem); +void qh_setlarger(setT **setp); +void *qh_setlast(setT *set); +setT *qh_setnew(int size); +setT *qh_setnew_delnthsorted(setT *set, int size, int nth, int prepend); +void qh_setprint(FILE *fp, char* string, setT *set); +void qh_setreplace(setT *set, void *oldelem, void *newelem); +int qh_setsize(setT *set); +setT *qh_settemp(int setsize); +void qh_settempfree(setT **set); +void qh_settempfree_all(void); +setT *qh_settemppop(void); +void qh_settemppush(setT *set); +void qh_settruncate (setT *set, int size); +int qh_setunique (setT **set, void *elem); +void qh_setzero (setT *set, int index, int size); + + +#endif /* qhDEFset */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/stat.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/stat.c new file mode 100644 index 0000000000000000000000000000000000000000..3a1aa051ff21225c498744003e4c3becc9eebc4c --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/stat.c @@ -0,0 +1,702 @@ +/*<html><pre> -<a href="qh-stat.htm" + >-------------------------------</a><a name="TOP">-</a> + + stat.c + contains all statistics that are collected for qhull + + see qh-stat.htm and stat.h + + copyright (c) 1993-2003, The Geometry Center +*/ + +#include "qhull_a.h" + +/*============ global data structure ==========*/ + +#if qh_QHpointer +qhstatT *qh_qhstat=NULL; /* global data structure */ +#else +qhstatT qh_qhstat; /* add "={0}" if this causes a compiler error */ +#endif + +/*========== functions in alphabetic order ================*/ + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="allstatA">-</a> + + qh_allstatA() + define statistics in groups of 20 + + notes: + (otherwise, 'gcc -O2' uses too much memory) + uses qhstat.next +*/ +void qh_allstatA (void) { + + /* zdef_(type,name,doc,average) */ + zzdef_(zdoc, Zdoc2, "precision statistics", -1); + zdef_(zinc, Znewvertex, NULL, -1); + zdef_(wadd, Wnewvertex, "ave. distance of a new vertex to a facet (not 0s)", Znewvertex); + zzdef_(wmax, Wnewvertexmax, "max. distance of a new vertex to a facet", -1); + zdef_(wmax, Wvertexmax, "max. distance of an output vertex to a facet", -1); + zdef_(wmin, Wvertexmin, "min. distance of an output vertex to a facet", -1); + zdef_(wmin, Wmindenom, "min. denominator in hyperplane computation", -1); + + qhstat precision= qhstat next; /* call qh_precision for each of these */ + zzdef_(zdoc, Zdoc3, "precision problems (corrected unless 'Q0' or an error)", -1); + zzdef_(zinc, Zcoplanarridges, "coplanar half ridges in output", -1); + zzdef_(zinc, Zconcaveridges, "concave half ridges in output", -1); + zzdef_(zinc, Zflippedfacets, "flipped facets", -1); + zzdef_(zinc, Zcoplanarhorizon, "coplanar horizon facets for new vertices", -1); + zzdef_(zinc, Zcoplanarpart, "coplanar points during partitioning", -1); + zzdef_(zinc, Zminnorm, "degenerate hyperplanes recomputed with gaussian elimination", -1); + zzdef_(zinc, Znearlysingular, "nearly singular or axis-parallel hyperplanes", -1); + zzdef_(zinc, Zback0, "zero divisors during back substitute", -1); + zzdef_(zinc, Zgauss0, "zero divisors during gaussian elimination", -1); + zzdef_(zinc, Zmultiridge, "ridges with multiple neighbors", -1); +} +void qh_allstatB (void) { + zzdef_(zdoc, Zdoc1, "summary information", -1); + zdef_(zinc, Zvertices, "number of vertices in output", -1); + zdef_(zinc, Znumfacets, "number of facets in output", -1); + zdef_(zinc, Znonsimplicial, "number of non-simplicial facets in output", -1); + zdef_(zinc, Znowsimplicial, "number of simplicial facets that were merged", -1); + zdef_(zinc, Znumridges, "number of ridges in output", -1); + zdef_(zadd, Znumridges, "average number of ridges per facet", Znumfacets); + zdef_(zmax, Zmaxridges, "maximum number of ridges", -1); + zdef_(zadd, Znumneighbors, "average number of neighbors per facet", Znumfacets); + zdef_(zmax, Zmaxneighbors, "maximum number of neighbors", -1); + zdef_(zadd, Znumvertices, "average number of vertices per facet", Znumfacets); + zdef_(zmax, Zmaxvertices, "maximum number of vertices", -1); + zdef_(zadd, Znumvneighbors, "average number of neighbors per vertex", Zvertices); + zdef_(zmax, Zmaxvneighbors, "maximum number of neighbors", -1); + zdef_(wadd, Wcpu, "cpu seconds for qhull after input", -1); + zdef_(zinc, Ztotvertices, "vertices created altogether", -1); + zzdef_(zinc, Zsetplane, "facets created altogether", -1); + zdef_(zinc, Ztotridges, "ridges created altogether", -1); + zdef_(zinc, Zpostfacets, "facets before post merge", -1); + zdef_(zadd, Znummergetot, "average merges per facet (at most 511)", Znumfacets); + zdef_(zmax, Znummergemax, " maximum merges for a facet (at most 511)", -1); + zdef_(zinc, Zangle, NULL, -1); + zdef_(wadd, Wangle, "average angle (cosine) of facet normals for all ridges", Zangle); + zdef_(wmax, Wanglemax, " maximum angle (cosine) of facet normals across a ridge", -1); + zdef_(wmin, Wanglemin, " minimum angle (cosine) of facet normals across a ridge", -1); + zdef_(wadd, Wareatot, "total area of facets", -1); + zdef_(wmax, Wareamax, " maximum facet area", -1); + zdef_(wmin, Wareamin, " minimum facet area", -1); +} +void qh_allstatC (void) { + zdef_(zdoc, Zdoc9, "build hull statistics", -1); + zzdef_(zinc, Zprocessed, "points processed", -1); + zzdef_(zinc, Zretry, "retries due to precision problems", -1); + zdef_(wmax, Wretrymax, " max. random joggle", -1); + zdef_(zmax, Zmaxvertex, "max. vertices at any one time", -1); + zdef_(zinc, Ztotvisible, "ave. visible facets per iteration", Zprocessed); + zdef_(zinc, Zinsidevisible, " ave. visible facets without an horizon neighbor", Zprocessed); + zdef_(zadd, Zvisfacettot, " ave. facets deleted per iteration", Zprocessed); + zdef_(zmax, Zvisfacetmax, " maximum", -1); + zdef_(zadd, Zvisvertextot, "ave. visible vertices per iteration", Zprocessed); + zdef_(zmax, Zvisvertexmax, " maximum", -1); + zdef_(zinc, Ztothorizon, "ave. horizon facets per iteration", Zprocessed); + zdef_(zadd, Znewfacettot, "ave. new or merged facets per iteration", Zprocessed); + zdef_(zmax, Znewfacetmax, " maximum (includes initial simplex)", -1); + zdef_(wadd, Wnewbalance, "average new facet balance", Zprocessed); + zdef_(wadd, Wnewbalance2, " standard deviation", -1); + zdef_(wadd, Wpbalance, "average partition balance", Zpbalance); + zdef_(wadd, Wpbalance2, " standard deviation", -1); + zdef_(zinc, Zpbalance, " number of trials", -1); + zdef_(zinc, Zsearchpoints, "searches of all points for initial simplex", -1); + zdef_(zinc, Zdetsimplex, "determinants computed (area & initial hull)", -1); + zdef_(zinc, Znoarea, "determinants not computed because vertex too low", -1); + zdef_(zinc, Znotmax, "points ignored (not above max_outside)", -1); + zdef_(zinc, Znotgood, "points ignored (not above a good facet)", -1); + zdef_(zinc, Znotgoodnew, "points ignored (didn't create a good new facet)", -1); + zdef_(zinc, Zgoodfacet, "good facets found", -1); + zzdef_(zinc, Znumvisibility, "distance tests for facet visibility", -1); + zdef_(zinc, Zdistvertex, "distance tests to report minimum vertex", -1); + zdef_(zinc, Ztotcheck, "points checked for facets' outer planes", -1); + zzdef_(zinc, Zcheckpart, " ave. distance tests per check", Ztotcheck); +} +void qh_allstatD(void) { + zdef_(zdoc, Zdoc4, "partitioning statistics (see previous for outer planes)", -1); + zzdef_(zadd, Zdelvertextot, "total vertices deleted", -1); + zdef_(zmax, Zdelvertexmax, " maximum vertices deleted per iteration", -1); + zdef_(zinc, Zfindbest, "calls to findbest", -1); + zdef_(zadd, Zfindbesttot, " ave. facets tested", Zfindbest); + zdef_(zmax, Zfindbestmax, " max. facets tested", -1); + zdef_(zadd, Zfindcoplanar, " ave. coplanar search", Zfindbest); + zdef_(zinc, Zfindnew, "calls to findbestnew", -1); + zdef_(zadd, Zfindnewtot, " ave. facets tested", Zfindnew); + zdef_(zmax, Zfindnewmax, " max. facets tested", -1); + zdef_(zinc, Zfindnewjump, " ave. clearly better", Zfindnew); + zdef_(zinc, Zfindnewsharp, " calls due to qh_sharpnewfacets", -1); + zdef_(zinc, Zfindhorizon, "calls to findhorizon", -1); + zdef_(zadd, Zfindhorizontot, " ave. facets tested", Zfindhorizon); + zdef_(zmax, Zfindhorizonmax, " max. facets tested", -1); + zdef_(zinc, Zfindjump, " ave. clearly better", Zfindhorizon); + zdef_(zinc, Zparthorizon, " horizon facets better than bestfacet", -1); + zdef_(zinc, Zpartangle, "angle tests for repartitioned coplanar points", -1); + zdef_(zinc, Zpartflip, " repartitioned coplanar points for flipped orientation", -1); +} +void qh_allstatE(void) { + zdef_(zinc, Zpartinside, "inside points", -1); + zdef_(zinc, Zpartnear, " inside points kept with a facet", -1); + zdef_(zinc, Zcoplanarinside, " inside points that were coplanar with a facet", -1); + zdef_(zinc, Zbestlower, "calls to findbestlower", -1); + zdef_(zinc, Zbestlowerv, " with search of vertex neighbors", -1); + zdef_(wadd, Wmaxout, "difference in max_outside at final check", -1); + zzdef_(zinc, Zpartitionall, "distance tests for initial partition", -1); + zdef_(zinc, Ztotpartition, "partitions of a point", -1); + zzdef_(zinc, Zpartition, "distance tests for partitioning", -1); + zzdef_(zinc, Zdistcheck, "distance tests for checking flipped facets", -1); + zzdef_(zinc, Zdistconvex, "distance tests for checking convexity", -1); + zdef_(zinc, Zdistgood, "distance tests for checking good point", -1); + zdef_(zinc, Zdistio, "distance tests for output", -1); + zdef_(zinc, Zdiststat, "distance tests for statistics", -1); + zdef_(zinc, Zdistplane, "total number of distance tests", -1); + zdef_(zinc, Ztotpartcoplanar, "partitions of coplanar points or deleted vertices", -1); + zzdef_(zinc, Zpartcoplanar, " distance tests for these partitions", -1); + zdef_(zinc, Zcomputefurthest, "distance tests for computing furthest", -1); +} +void qh_allstatE2(void) { + zdef_(zdoc, Zdoc5, "statistics for matching ridges", -1); + zdef_(zinc, Zhashlookup, "total lookups for matching ridges of new facets", -1); + zdef_(zinc, Zhashtests, "average number of tests to match a ridge", Zhashlookup); + zdef_(zinc, Zhashridge, "total lookups of subridges (duplicates and boundary)", -1); + zdef_(zinc, Zhashridgetest, "average number of tests per subridge", Zhashridge); + zdef_(zinc, Zdupsame, "duplicated ridges in same merge cycle", -1); + zdef_(zinc, Zdupflip, "duplicated ridges with flipped facets", -1); + + zdef_(zdoc, Zdoc6, "statistics for determining merges", -1); + zdef_(zinc, Zangletests, "angles computed for ridge convexity", -1); + zdef_(zinc, Zbestcentrum, "best merges used centrum instead of vertices",-1); + zzdef_(zinc, Zbestdist, "distance tests for best merge", -1); + zzdef_(zinc, Zcentrumtests, "distance tests for centrum convexity", -1); + zzdef_(zinc, Zdistzero, "distance tests for checking simplicial convexity", -1); + zdef_(zinc, Zcoplanarangle, "coplanar angles in getmergeset", -1); + zdef_(zinc, Zcoplanarcentrum, "coplanar centrums in getmergeset", -1); + zdef_(zinc, Zconcaveridge, "concave ridges in getmergeset", -1); +} +void qh_allstatF(void) { + zdef_(zdoc, Zdoc7, "statistics for merging", -1); + zdef_(zinc, Zpremergetot, "merge iterations", -1); + zdef_(zadd, Zmergeinittot, "ave. initial non-convex ridges per iteration", Zpremergetot); + zdef_(zadd, Zmergeinitmax, " maximum", -1); + zdef_(zadd, Zmergesettot, " ave. additional non-convex ridges per iteration", Zpremergetot); + zdef_(zadd, Zmergesetmax, " maximum additional in one pass", -1); + zdef_(zadd, Zmergeinittot2, "initial non-convex ridges for post merging", -1); + zdef_(zadd, Zmergesettot2, " additional non-convex ridges", -1); + zdef_(wmax, Wmaxoutside, "max distance of vertex or coplanar point above facet (w/roundoff)", -1); + zdef_(wmin, Wminvertex, "max distance of merged vertex below facet (or roundoff)", -1); + zdef_(zinc, Zwidefacet, "centrums frozen due to a wide merge", -1); + zdef_(zinc, Zwidevertices, "centrums frozen due to extra vertices", -1); + zzdef_(zinc, Ztotmerge, "total number of facets or cycles of facets merged", -1); + zdef_(zinc, Zmergesimplex, "merged a simplex", -1); + zdef_(zinc, Zonehorizon, "simplices merged into coplanar horizon", -1); + zzdef_(zinc, Zcyclehorizon, "cycles of facets merged into coplanar horizon", -1); + zzdef_(zadd, Zcyclefacettot, " ave. facets per cycle", Zcyclehorizon); + zdef_(zmax, Zcyclefacetmax, " max. facets", -1); + zdef_(zinc, Zmergeintohorizon, "new facets merged into horizon", -1); + zdef_(zinc, Zmergenew, "new facets merged", -1); + zdef_(zinc, Zmergehorizon, "horizon facets merged into new facets", -1); + zdef_(zinc, Zmergevertex, "vertices deleted by merging", -1); + zdef_(zinc, Zcyclevertex, "vertices deleted by merging into coplanar horizon", -1); + zdef_(zinc, Zdegenvertex, "vertices deleted by degenerate facet", -1); + zdef_(zinc, Zmergeflipdup, "merges due to flipped facets in duplicated ridge", -1); + zdef_(zinc, Zneighbor, "merges due to redundant neighbors", -1); + zdef_(zadd, Ztestvneighbor, "non-convex vertex neighbors", -1); +} +void qh_allstatG(void) { + zdef_(zinc, Zacoplanar, "merges due to angle coplanar facets", -1); + zdef_(wadd, Wacoplanartot, " average merge distance", Zacoplanar); + zdef_(wmax, Wacoplanarmax, " maximum merge distance", -1); + zdef_(zinc, Zcoplanar, "merges due to coplanar facets", -1); + zdef_(wadd, Wcoplanartot, " average merge distance", Zcoplanar); + zdef_(wmax, Wcoplanarmax, " maximum merge distance", -1); + zdef_(zinc, Zconcave, "merges due to concave facets", -1); + zdef_(wadd, Wconcavetot, " average merge distance", Zconcave); + zdef_(wmax, Wconcavemax, " maximum merge distance", -1); + zdef_(zinc, Zavoidold, "coplanar/concave merges due to avoiding old merge", -1); + zdef_(wadd, Wavoidoldtot, " average merge distance", Zavoidold); + zdef_(wmax, Wavoidoldmax, " maximum merge distance", -1); + zdef_(zinc, Zdegen, "merges due to degenerate facets", -1); + zdef_(wadd, Wdegentot, " average merge distance", Zdegen); + zdef_(wmax, Wdegenmax, " maximum merge distance", -1); + zdef_(zinc, Zflipped, "merges due to removing flipped facets", -1); + zdef_(wadd, Wflippedtot, " average merge distance", Zflipped); + zdef_(wmax, Wflippedmax, " maximum merge distance", -1); + zdef_(zinc, Zduplicate, "merges due to duplicated ridges", -1); + zdef_(wadd, Wduplicatetot, " average merge distance", Zduplicate); + zdef_(wmax, Wduplicatemax, " maximum merge distance", -1); +} +void qh_allstatH(void) { + zdef_(zdoc, Zdoc8, "renamed vertex statistics", -1); + zdef_(zinc, Zrenameshare, "renamed vertices shared by two facets", -1); + zdef_(zinc, Zrenamepinch, "renamed vertices in a pinched facet", -1); + zdef_(zinc, Zrenameall, "renamed vertices shared by multiple facets", -1); + zdef_(zinc, Zfindfail, "rename failures due to duplicated ridges", -1); + zdef_(zinc, Zdupridge, " duplicate ridges detected", -1); + zdef_(zinc, Zdelridge, "deleted ridges due to renamed vertices", -1); + zdef_(zinc, Zdropneighbor, "dropped neighbors due to renamed vertices", -1); + zdef_(zinc, Zdropdegen, "degenerate facets due to dropped neighbors", -1); + zdef_(zinc, Zdelfacetdup, " facets deleted because of no neighbors", -1); + zdef_(zinc, Zremvertex, "vertices removed from facets due to no ridges", -1); + zdef_(zinc, Zremvertexdel, " deleted", -1); + zdef_(zinc, Zintersectnum, "vertex intersections for locating redundant vertices", -1); + zdef_(zinc, Zintersectfail, "intersections failed to find a redundant vertex", -1); + zdef_(zinc, Zintersect, "intersections found redundant vertices", -1); + zdef_(zadd, Zintersecttot, " ave. number found per vertex", Zintersect); + zdef_(zmax, Zintersectmax, " max. found for a vertex", -1); + zdef_(zinc, Zvertexridge, NULL, -1); + zdef_(zadd, Zvertexridgetot, " ave. number of ridges per tested vertex", Zvertexridge); + zdef_(zmax, Zvertexridgemax, " max. number of ridges per tested vertex", -1); + + zdef_(zdoc, Zdoc10, "memory usage statistics (in bytes)", -1); + zdef_(zadd, Zmemfacets, "for facets and their normals, neighbor and vertex sets", -1); + zdef_(zadd, Zmemvertices, "for vertices and their neighbor sets", -1); + zdef_(zadd, Zmempoints, "for input points and outside and coplanar sets",-1); + zdef_(zadd, Zmemridges, "for ridges and their vertex sets", -1); +} /* allstat */ + +void qh_allstatI(void) { + qhstat vridges= qhstat next; + zzdef_(zdoc, Zdoc11, "Voronoi ridge statistics", -1); + zzdef_(zinc, Zridge, "non-simplicial Voronoi vertices for all ridges", -1); + zzdef_(wadd, Wridge, " ave. distance to ridge", Zridge); + zzdef_(wmax, Wridgemax, " max. distance to ridge", -1); + zzdef_(zinc, Zridgemid, "bounded ridges", -1); + zzdef_(wadd, Wridgemid, " ave. distance of midpoint to ridge", Zridgemid); + zzdef_(wmax, Wridgemidmax, " max. distance of midpoint to ridge", -1); + zzdef_(zinc, Zridgeok, "bounded ridges with ok normal", -1); + zzdef_(wadd, Wridgeok, " ave. angle to ridge", Zridgeok); + zzdef_(wmax, Wridgeokmax, " max. angle to ridge", -1); + zzdef_(zinc, Zridge0, "bounded ridges with near-zero normal", -1); + zzdef_(wadd, Wridge0, " ave. angle to ridge", Zridge0); + zzdef_(wmax, Wridge0max, " max. angle to ridge", -1); + + zdef_(zdoc, Zdoc12, "Triangulation statistics (Qt)", -1); + zdef_(zinc, Ztricoplanar, "non-simplicial facets triangulated", -1); + zdef_(zadd, Ztricoplanartot, " ave. new facets created (may be deleted)", Ztricoplanar); + zdef_(zmax, Ztricoplanarmax, " max. new facets created", -1); + zdef_(zinc, Ztrinull, "null new facets deleted (duplicated vertex)", -1); + zdef_(zinc, Ztrimirror, "mirrored pairs of new facets deleted (same vertices)", -1); + zdef_(zinc, Ztridegen, "degenerate new facets in output (same ridge)", -1); +} /* allstat */ + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="allstatistics">-</a> + + qh_allstatistics() + reset printed flag for all statistics +*/ +void qh_allstatistics (void) { + int i; + + for (i=ZEND; i--; ) + qhstat printed[i]= False; +} /* allstatistics */ + +#if qh_KEEPstatistics +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="collectstatistics">-</a> + + qh_collectstatistics() + collect statistics for qh.facet_list + +*/ +void qh_collectstatistics (void) { + facetT *facet, *neighbor, **neighborp; + vertexT *vertex, **vertexp; + realT dotproduct, dist; + int sizneighbors, sizridges, sizvertices, i; + + qh old_randomdist= qh RANDOMdist; + qh RANDOMdist= False; + zval_(Zmempoints)= qh num_points * qh normal_size + + sizeof (qhT) + sizeof (qhstatT); + zval_(Zmemfacets)= 0; + zval_(Zmemridges)= 0; + zval_(Zmemvertices)= 0; + zval_(Zangle)= 0; + wval_(Wangle)= 0.0; + zval_(Znumridges)= 0; + zval_(Znumfacets)= 0; + zval_(Znumneighbors)= 0; + zval_(Znumvertices)= 0; + zval_(Znumvneighbors)= 0; + zval_(Znummergetot)= 0; + zval_(Znummergemax)= 0; + zval_(Zvertices)= qh num_vertices - qh_setsize (qh del_vertices); + if (qh MERGING || qh APPROXhull || qh JOGGLEmax < REALmax/2) + wmax_(Wmaxoutside, qh max_outside); + if (qh MERGING) + wmin_(Wminvertex, qh min_vertex); + FORALLfacets + facet->seen= False; + if (qh DELAUNAY) { + FORALLfacets { + if (facet->upperdelaunay != qh UPPERdelaunay) + facet->seen= True; /* remove from angle statistics */ + } + } + FORALLfacets { + if (facet->visible && qh NEWfacets) + continue; + sizvertices= qh_setsize (facet->vertices); + sizneighbors= qh_setsize (facet->neighbors); + sizridges= qh_setsize (facet->ridges); + zinc_(Znumfacets); + zadd_(Znumvertices, sizvertices); + zmax_(Zmaxvertices, sizvertices); + zadd_(Znumneighbors, sizneighbors); + zmax_(Zmaxneighbors, sizneighbors); + zadd_(Znummergetot, facet->nummerge); + i= facet->nummerge; /* avoid warnings */ + zmax_(Znummergemax, i); + if (!facet->simplicial) { + if (sizvertices == qh hull_dim) { + zinc_(Znowsimplicial); + }else { + zinc_(Znonsimplicial); + } + } + if (sizridges) { + zadd_(Znumridges, sizridges); + zmax_(Zmaxridges, sizridges); + } + zadd_(Zmemfacets, sizeof (facetT) + qh normal_size + 2*sizeof (setT) + + SETelemsize * (sizneighbors + sizvertices)); + if (facet->ridges) { + zadd_(Zmemridges, + sizeof (setT) + SETelemsize * sizridges + sizridges * + (sizeof (ridgeT) + sizeof (setT) + SETelemsize * (qh hull_dim-1))/2); + } + if (facet->outsideset) + zadd_(Zmempoints, sizeof (setT) + SETelemsize * qh_setsize (facet->outsideset)); + if (facet->coplanarset) + zadd_(Zmempoints, sizeof (setT) + SETelemsize * qh_setsize (facet->coplanarset)); + if (facet->seen) /* Delaunay upper envelope */ + continue; + facet->seen= True; + FOREACHneighbor_(facet) { + if (neighbor == qh_DUPLICATEridge || neighbor == qh_MERGEridge + || neighbor->seen || !facet->normal || !neighbor->normal) + continue; + dotproduct= qh_getangle(facet->normal, neighbor->normal); + zinc_(Zangle); + wadd_(Wangle, dotproduct); + wmax_(Wanglemax, dotproduct) + wmin_(Wanglemin, dotproduct) + } + if (facet->normal) { + FOREACHvertex_(facet->vertices) { + zinc_(Zdiststat); + qh_distplane(vertex->point, facet, &dist); + wmax_(Wvertexmax, dist); + wmin_(Wvertexmin, dist); + } + } + } + FORALLvertices { + if (vertex->deleted) + continue; + zadd_(Zmemvertices, sizeof (vertexT)); + if (vertex->neighbors) { + sizneighbors= qh_setsize (vertex->neighbors); + zadd_(Znumvneighbors, sizneighbors); + zmax_(Zmaxvneighbors, sizneighbors); + zadd_(Zmemvertices, sizeof (vertexT) + SETelemsize * sizneighbors); + } + } + qh RANDOMdist= qh old_randomdist; +} /* collectstatistics */ +#endif /* qh_KEEPstatistics */ + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="freestatistics">-</a> + + qh_freestatistics( ) + free memory used for statistics +*/ +void qh_freestatistics (void) { + +#if qh_QHpointer + free (qh_qhstat); + qh_qhstat= NULL; +#endif +} /* freestatistics */ + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="initstatistics">-</a> + + qh_initstatistics( ) + allocate and initialize statistics + + notes: + uses malloc() instead of qh_memalloc() since mem.c not set up yet +*/ +void qh_initstatistics (void) { + int i; + realT realx; + int intx; + +#if qh_QHpointer + if (!(qh_qhstat= (qhstatT *)malloc (sizeof(qhstatT)))) { + fprintf (qhmem.ferr, "qhull error (qh_initstatistics): insufficient memory\n"); + exit (1); /* can not use qh_errexit() */ + } +#endif + + qhstat next= 0; + qh_allstatA(); + qh_allstatB(); + qh_allstatC(); + qh_allstatD(); + qh_allstatE(); + qh_allstatE2(); + qh_allstatF(); + qh_allstatG(); + qh_allstatH(); + qh_allstatI(); + if (qhstat next > sizeof(qhstat id)) { + fprintf (qhmem.ferr, "qhull error (qh_initstatistics): increase size of qhstat.id[].\n\ + qhstat.next %d should be <= sizeof(qhstat id) %d\n", qhstat next, sizeof(qhstat id)); +#if 0 /* for locating error, Znumridges should be duplicated */ + for (i=0; i < ZEND; i++) { + int j; + for (j=i+1; j < ZEND; j++) { + if (qhstat id[i] == qhstat id[j]) { + fprintf (qhmem.ferr, "qhull error (qh_initstatistics): duplicated statistic %d at indices %d and %d\n", + qhstat id[i], i, j); + } + } + } +#endif + exit (1); /* can not use qh_errexit() */ + } + qhstat init[zinc].i= 0; + qhstat init[zadd].i= 0; + qhstat init[zmin].i= INT_MAX; + qhstat init[zmax].i= INT_MIN; + qhstat init[wadd].r= 0; + qhstat init[wmin].r= REALmax; + qhstat init[wmax].r= -REALmax; + for (i=0; i < ZEND; i++) { + if (qhstat type[i] > ZTYPEreal) { + realx= qhstat init[(unsigned char)(qhstat type[i])].r; + qhstat stats[i].r= realx; + }else if (qhstat type[i] != zdoc) { + intx= qhstat init[(unsigned char)(qhstat type[i])].i; + qhstat stats[i].i= intx; + } + } +} /* initstatistics */ + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="newstats">-</a> + + qh_newstats( ) + returns True if statistics for zdoc + + returns: + next zdoc +*/ +boolT qh_newstats (int index, int *nextindex) { + boolT isnew= False; + int start, i; + + if (qhstat type[qhstat id[index]] == zdoc) + start= index+1; + else + start= index; + for (i= start; i < qhstat next && qhstat type[qhstat id[i]] != zdoc; i++) { + if (!qh_nostatistic(qhstat id[i]) && !qhstat printed[qhstat id[i]]) + isnew= True; + } + *nextindex= i; + return isnew; +} /* newstats */ + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="nostatistic">-</a> + + qh_nostatistic( index ) + true if no statistic to print +*/ +boolT qh_nostatistic (int i) { + + if ((qhstat type[i] > ZTYPEreal + &&qhstat stats[i].r == qhstat init[(unsigned char)(qhstat type[i])].r) + || (qhstat type[i] < ZTYPEreal + &&qhstat stats[i].i == qhstat init[(unsigned char)(qhstat type[i])].i)) + return True; + return False; +} /* nostatistic */ + +#if qh_KEEPstatistics +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="printallstatistics">-</a> + + qh_printallstatistics( fp, string ) + print all statistics with header 'string' +*/ +void qh_printallstatistics (FILE *fp, char *string) { + + qh_allstatistics(); + qh_collectstatistics(); + qh_printstatistics (fp, string); + qh_memstatistics (fp); +} + + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="printstatistics">-</a> + + qh_printstatistics( fp, string ) + print statistics to a file with header 'string' + skips statistics with qhstat.printed[] (reset with qh_allstatistics) + + see: + qh_printallstatistics() +*/ +void qh_printstatistics (FILE *fp, char *string) { + int i, k; + realT ave; + + if (qh num_points != qh num_vertices) { + wval_(Wpbalance)= 0; + wval_(Wpbalance2)= 0; + }else + wval_(Wpbalance2)= qh_stddev (zval_(Zpbalance), wval_(Wpbalance), + wval_(Wpbalance2), &ave); + wval_(Wnewbalance2)= qh_stddev (zval_(Zprocessed), wval_(Wnewbalance), + wval_(Wnewbalance2), &ave); + fprintf (fp, "\n\ +%s\n\ + qhull invoked by: %s | %s\n%s with options:\n%s\n", string, qh rbox_command, + qh qhull_command, qh_version, qh qhull_options); + fprintf (fp, "\nprecision constants:\n\ + %6.2g max. abs. coordinate in the (transformed) input ('Qbd:n')\n\ + %6.2g max. roundoff error for distance computation ('En')\n\ + %6.2g max. roundoff error for angle computations\n\ + %6.2g min. distance for outside points ('Wn')\n\ + %6.2g min. distance for visible facets ('Vn')\n\ + %6.2g max. distance for coplanar facets ('Un')\n\ + %6.2g max. facet width for recomputing centrum and area\n\ +", + qh MAXabs_coord, qh DISTround, qh ANGLEround, qh MINoutside, + qh MINvisible, qh MAXcoplanar, qh WIDEfacet); + if (qh KEEPnearinside) + fprintf(fp, "\ + %6.2g max. distance for near-inside points\n", qh NEARinside); + if (qh premerge_cos < REALmax/2) fprintf (fp, "\ + %6.2g max. cosine for pre-merge angle\n", qh premerge_cos); + if (qh PREmerge) fprintf (fp, "\ + %6.2g radius of pre-merge centrum\n", qh premerge_centrum); + if (qh postmerge_cos < REALmax/2) fprintf (fp, "\ + %6.2g max. cosine for post-merge angle\n", qh postmerge_cos); + if (qh POSTmerge) fprintf (fp, "\ + %6.2g radius of post-merge centrum\n", qh postmerge_centrum); + fprintf (fp, "\ + %6.2g max. distance for merging two simplicial facets\n\ + %6.2g max. roundoff error for arithmetic operations\n\ + %6.2g min. denominator for divisions\n\ + zero diagonal for Gauss: ", qh ONEmerge, REALepsilon, qh MINdenom); + for (k=0; k < qh hull_dim; k++) + fprintf (fp, "%6.2e ", qh NEARzero[k]); + fprintf (fp, "\n\n"); + for (i=0 ; i < qhstat next; ) + qh_printstats (fp, i, &i); +} /* printstatistics */ +#endif /* qh_KEEPstatistics */ + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="printstatlevel">-</a> + + qh_printstatlevel( fp, id ) + print level information for a statistic + + notes: + nop if id >= ZEND, printed, or same as initial value +*/ +void qh_printstatlevel (FILE *fp, int id, int start) { +#define NULLfield " " + + if (id >= ZEND || qhstat printed[id]) + return; + if (qhstat type[id] == zdoc) { + fprintf (fp, "%s\n", qhstat doc[id]); + return; + } + start= 0; /* not used */ + if (qh_nostatistic(id) || !qhstat doc[id]) + return; + qhstat printed[id]= True; + if (qhstat count[id] != -1 + && qhstat stats[(unsigned char)(qhstat count[id])].i == 0) + fprintf (fp, " *0 cnt*"); + else if (qhstat type[id] >= ZTYPEreal && qhstat count[id] == -1) + fprintf (fp, "%7.2g", qhstat stats[id].r); + else if (qhstat type[id] >= ZTYPEreal && qhstat count[id] != -1) + fprintf (fp, "%7.2g", qhstat stats[id].r/ qhstat stats[(unsigned char)(qhstat count[id])].i); + else if (qhstat type[id] < ZTYPEreal && qhstat count[id] == -1) + fprintf (fp, "%7d", qhstat stats[id].i); + else if (qhstat type[id] < ZTYPEreal && qhstat count[id] != -1) + fprintf (fp, "%7.3g", (realT) qhstat stats[id].i / qhstat stats[(unsigned char)(qhstat count[id])].i); + fprintf (fp, " %s\n", qhstat doc[id]); +} /* printstatlevel */ + + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="printstats">-</a> + + qh_printstats( fp, index, nextindex ) + print statistics for a zdoc group + + returns: + next zdoc if non-null +*/ +void qh_printstats (FILE *fp, int index, int *nextindex) { + int j, nexti; + + if (qh_newstats (index, &nexti)) { + fprintf (fp, "\n"); + for (j=index; j<nexti; j++) + qh_printstatlevel (fp, qhstat id[j], 0); + } + if (nextindex) + *nextindex= nexti; +} /* printstats */ + +#if qh_KEEPstatistics + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="stddev">-</a> + + qh_stddev( num, tot, tot2, ave ) + compute the standard deviation and average from statistics + + tot2 is the sum of the squares + notes: + computes r.m.s.: + (x-ave)^2 + == x^2 - 2x tot/num + (tot/num)^2 + == tot2 - 2 tot tot/num + tot tot/num + == tot2 - tot ave +*/ +realT qh_stddev (int num, realT tot, realT tot2, realT *ave) { + realT stddev; + + *ave= tot/num; + stddev= sqrt (tot2/num - *ave * *ave); + return stddev; +} /* stddev */ + +#endif /* qh_KEEPstatistics */ + +#if !qh_KEEPstatistics +void qh_collectstatistics (void) {} +void qh_printallstatistics (FILE *fp, char *string) {}; +void qh_printstatistics (FILE *fp, char *string) {} +#endif + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/stat.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/stat.h new file mode 100644 index 0000000000000000000000000000000000000000..96c4e6035992bfe6205862b16269a4402f6ffe87 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/stat.h @@ -0,0 +1,522 @@ + /*<html><pre> -<a href="qh-stat.htm" + >-------------------------------</a><a name="TOP">-</a> + + stat.h + contains all statistics that are collected for qhull + + see qh-stat.htm and stat.c + + copyright (c) 1993-2003, The Geometry Center + + recompile qhull if you change this file + + Integer statistics are Z* while real statistics are W*. + + define maydebugx to call a routine at every statistic event + +*/ + +#ifndef qhDEFstat +#define qhDEFstat 1 + + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="KEEPstatistics">-</a> + + qh_KEEPstatistics + 0 turns off statistic gathering (except zzdef/zzinc/zzadd/zzval/wwval) +*/ +#ifndef qh_KEEPstatistics +#define qh_KEEPstatistics 1 +#endif + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="statistics">-</a> + + Zxxx for integers, Wxxx for reals + + notes: + be sure that all statistics are defined in stat.c + otherwise initialization may core dump + can pick up all statistics by: + grep '[zw].*_[(][ZW]' *.c >z.x + remove trailers with query">-</a> + remove leaders with query-replace-regexp [ ^I]+ ( +*/ +#if qh_KEEPstatistics +enum statistics { /* alphabetical after Z/W */ + Zacoplanar, + Wacoplanarmax, + Wacoplanartot, + Zangle, + Wangle, + Wanglemax, + Wanglemin, + Zangletests, + Wareatot, + Wareamax, + Wareamin, + Zavoidold, + Wavoidoldmax, + Wavoidoldtot, + Zback0, + Zbestcentrum, + Zbestdist, + Zbestlower, + Zbestlowerv, + Zcentrumtests, + Zcheckpart, + Zcomputefurthest, + Zconcave, + Wconcavemax, + Wconcavetot, + Zconcaveridges, + Zconcaveridge, + Zcoplanar, + Wcoplanarmax, + Wcoplanartot, + Zcoplanarangle, + Zcoplanarcentrum, + Zcoplanarhorizon, + Zcoplanarinside, + Zcoplanarpart, + Zcoplanarridges, + Wcpu, + Zcyclefacetmax, + Zcyclefacettot, + Zcyclehorizon, + Zcyclevertex, + Zdegen, + Wdegenmax, + Wdegentot, + Zdegenvertex, + Zdelfacetdup, + Zdelridge, + Zdelvertextot, + Zdelvertexmax, + Zdetsimplex, + Zdistcheck, + Zdistconvex, + Zdistgood, + Zdistio, + Zdistplane, + Zdiststat, + Zdistvertex, + Zdistzero, + Zdoc1, + Zdoc2, + Zdoc3, + Zdoc4, + Zdoc5, + Zdoc6, + Zdoc7, + Zdoc8, + Zdoc9, + Zdoc10, + Zdoc11, + Zdoc12, + Zdropdegen, + Zdropneighbor, + Zdupflip, + Zduplicate, + Wduplicatemax, + Wduplicatetot, + Zdupridge, + Zdupsame, + Zflipped, + Wflippedmax, + Wflippedtot, + Zflippedfacets, + Zfindbest, + Zfindbestmax, + Zfindbesttot, + Zfindcoplanar, + Zfindfail, + Zfindhorizon, + Zfindhorizonmax, + Zfindhorizontot, + Zfindjump, + Zfindnew, + Zfindnewmax, + Zfindnewtot, + Zfindnewjump, + Zfindnewsharp, + Zgauss0, + Zgoodfacet, + Zhashlookup, + Zhashridge, + Zhashridgetest, + Zhashtests, + Zinsidevisible, + Zintersect, + Zintersectfail, + Zintersectmax, + Zintersectnum, + Zintersecttot, + Zmaxneighbors, + Wmaxout, + Wmaxoutside, + Zmaxridges, + Zmaxvertex, + Zmaxvertices, + Zmaxvneighbors, + Zmemfacets, + Zmempoints, + Zmemridges, + Zmemvertices, + Zmergeflipdup, + Zmergehorizon, + Zmergeinittot, + Zmergeinitmax, + Zmergeinittot2, + Zmergeintohorizon, + Zmergenew, + Zmergesettot, + Zmergesetmax, + Zmergesettot2, + Zmergesimplex, + Zmergevertex, + Wmindenom, + Wminvertex, + Zminnorm, + Zmultiridge, + Znearlysingular, + Zneighbor, + Wnewbalance, + Wnewbalance2, + Znewfacettot, + Znewfacetmax, + Znewvertex, + Wnewvertex, + Wnewvertexmax, + Znoarea, + Znonsimplicial, + Znowsimplicial, + Znotgood, + Znotgoodnew, + Znotmax, + Znumfacets, + Znummergemax, + Znummergetot, + Znumneighbors, + Znumridges, + Znumvertices, + Znumvisibility, + Znumvneighbors, + Zonehorizon, + Zpartangle, + Zpartcoplanar, + Zpartflip, + Zparthorizon, + Zpartinside, + Zpartition, + Zpartitionall, + Zpartnear, + Zpbalance, + Wpbalance, + Wpbalance2, + Zpostfacets, + Zpremergetot, + Zprocessed, + Zremvertex, + Zremvertexdel, + Zrenameall, + Zrenamepinch, + Zrenameshare, + Zretry, + Wretrymax, + Zridge, + Wridge, + Wridgemax, + Zridge0, + Wridge0, + Wridge0max, + Zridgemid, + Wridgemid, + Wridgemidmax, + Zridgeok, + Wridgeok, + Wridgeokmax, + Zsearchpoints, + Zsetplane, + Ztestvneighbor, + Ztotcheck, + Ztothorizon, + Ztotmerge, + Ztotpartcoplanar, + Ztotpartition, + Ztotridges, + Ztotvertices, + Ztotvisible, + Ztricoplanar, + Ztricoplanarmax, + Ztricoplanartot, + Ztridegen, + Ztrimirror, + Ztrinull, + Wvertexmax, + Wvertexmin, + Zvertexridge, + Zvertexridgetot, + Zvertexridgemax, + Zvertices, + Zvisfacettot, + Zvisfacetmax, + Zvisvertextot, + Zvisvertexmax, + Zwidefacet, + Zwidevertices, + ZEND}; + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="ZZstat">-</a> + + Zxxx/Wxxx statistics that remain defined if qh_KEEPstatistics=0 + + notes: + be sure to use zzdef, zzinc, etc. with these statistics (no double checking!) +*/ +#else +enum statistics { /* for zzdef etc. macros */ + Zback0, + Zbestdist, + Zcentrumtests, + Zcheckpart, + Zconcaveridges, + Zcoplanarhorizon, + Zcoplanarpart, + Zcoplanarridges, + Zcyclefacettot, + Zcyclehorizon, + Zdelvertextot, + Zdistcheck, + Zdistconvex, + Zdistzero, + Zdoc1, + Zdoc2, + Zdoc3, + Zdoc11, + Zflippedfacets, + Zgauss0, + Zminnorm, + Zmultiridge, + Znearlysingular, + Wnewvertexmax, + Znumvisibility, + Zpartcoplanar, + Zpartition, + Zpartitionall, + Zprocessed, + Zretry, + Zridge, + Wridge, + Wridgemax, + Zridge0, + Wridge0, + Wridge0max, + Zridgemid, + Wridgemid, + Wridgemidmax, + Zridgeok, + Wridgeok, + Wridgeokmax, + Zsetplane, + Ztotmerge, + ZEND}; +#endif + +/*-<a href="qh-stat.htm#TOC" + >-------------------------------</a><a name="ztype">-</a> + + ztype + the type of a statistic sets its initial value. + + notes: + The type should be the same as the macro for collecting the statistic +*/ +enum ztypes {zdoc,zinc,zadd,zmax,zmin,ZTYPEreal,wadd,wmax,wmin,ZTYPEend}; + +/*========== macros and constants =============*/ + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="MAYdebugx">-</a> + + MAYdebugx + define as maydebug() to be called frequently for error trapping +*/ +#define MAYdebugx + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zdef_">-</a> + + zzdef_, zdef_( type, name, doc, -1) + define a statistic (assumes 'qhstat.next= 0;') + + zdef_( type, name, doc, count) + define an averaged statistic + printed as name/count +*/ +#define zzdef_(stype,name,string,cnt) qhstat id[qhstat next++]=name; \ + qhstat doc[name]= string; qhstat count[name]= cnt; qhstat type[name]= stype +#if qh_KEEPstatistics +#define zdef_(stype,name,string,cnt) qhstat id[qhstat next++]=name; \ + qhstat doc[name]= string; qhstat count[name]= cnt; qhstat type[name]= stype +#else +#define zdef_(type,name,doc,count) +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zinc_">-</a> + + zzinc_( name ), zinc_( name) + increment an integer statistic +*/ +#define zzinc_(id) {MAYdebugx; qhstat stats[id].i++;} +#if qh_KEEPstatistics +#define zinc_(id) {MAYdebugx; qhstat stats[id].i++;} +#else +#define zinc_(id) {} +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zadd_">-</a> + + zzadd_( name, value ), zadd_( name, value ), wadd_( name, value ) + add value to an integer or real statistic +*/ +#define zzadd_(id, val) {MAYdebugx; qhstat stats[id].i += (val);} +#define wwadd_(id, val) {MAYdebugx; qhstat stats[id].r += (val);} +#if qh_KEEPstatistics +#define zadd_(id, val) {MAYdebugx; qhstat stats[id].i += (val);} +#define wadd_(id, val) {MAYdebugx; qhstat stats[id].r += (val);} +#else +#define zadd_(id, val) {} +#define wadd_(id, val) {} +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zval_">-</a> + + zzval_( name ), zval_( name ), wwval_( name ) + set or return value of a statistic +*/ +#define zzval_(id) ((qhstat stats[id]).i) +#define wwval_(id) ((qhstat stats[id]).r) +#if qh_KEEPstatistics +#define zval_(id) ((qhstat stats[id]).i) +#define wval_(id) ((qhstat stats[id]).r) +#else +#define zval_(id) qhstat tempi +#define wval_(id) qhstat tempr +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zmax_">-</a> + + zmax_( id, val ), wmax_( id, value ) + maximize id with val +*/ +#define wwmax_(id, val) {MAYdebugx; maximize_(qhstat stats[id].r,(val));} +#if qh_KEEPstatistics +#define zmax_(id, val) {MAYdebugx; maximize_(qhstat stats[id].i,(val));} +#define wmax_(id, val) {MAYdebugx; maximize_(qhstat stats[id].r,(val));} +#else +#define zmax_(id, val) {} +#define wmax_(id, val) {} +#endif + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="zmin_">-</a> + + zmin_( id, val ), wmin_( id, value ) + minimize id with val +*/ +#if qh_KEEPstatistics +#define zmin_(id, val) {MAYdebugx; minimize_(qhstat stats[id].i,(val));} +#define wmin_(id, val) {MAYdebugx; minimize_(qhstat stats[id].r,(val));} +#else +#define zmin_(id, val) {} +#define wmin_(id, val) {} +#endif + +/*================== stat.h types ==============*/ + + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="intrealT">-</a> + + intrealT + union of integer and real, used for statistics +*/ +typedef union intrealT intrealT; /* union of int and realT */ +union intrealT { + int i; + realT r; +}; + +/*-<a href="qh-stat.htm#TOC" + >--------------------------------</a><a name="qhstat">-</a> + + qhstat + global data structure for statistics + + notes: + access to qh_qhstat is via the "qhstat" macro. There are two choices + qh_QHpointer = 1 access globals via a pointer + enables qh_saveqhull() and qh_restoreqhull() + = 0 qh_qhstat is a static data structure + only one instance of qhull() can be active at a time + default value + qh_QHpointer is defined in qhull.h + + allocated in stat.c +*/ +typedef struct qhstatT qhstatT; +#if qh_QHpointer +#define qhstat qh_qhstat-> +extern qhstatT *qh_qhstat; +#else +#define qhstat qh_qhstat. +extern qhstatT qh_qhstat; +#endif +struct qhstatT { + intrealT stats[ZEND]; /* integer and real statistics */ + unsigned char id[ZEND+10]; /* id's in print order */ + char *doc[ZEND]; /* array of documentation strings */ + short int count[ZEND]; /* -1 if none, else index of count to use */ + char type[ZEND]; /* type, see ztypes above */ + char printed[ZEND]; /* true, if statistic has been printed */ + intrealT init[ZTYPEend]; /* initial values by types, set initstatistics */ + + int next; /* next index for zdef_ */ + int precision; /* index for precision problems */ + int vridges; /* index for Voronoi ridges */ + int tempi; + realT tempr; +}; + +/*========== function prototypes ===========*/ + +void qh_allstatA(void); +void qh_allstatB(void); +void qh_allstatC(void); +void qh_allstatD(void); +void qh_allstatE(void); +void qh_allstatE2(void); +void qh_allstatF(void); +void qh_allstatG(void); +void qh_allstatH(void); +void qh_allstatI(void); +void qh_allstatistics (void); +void qh_collectstatistics (void); +void qh_freestatistics (void); +void qh_initstatistics (void); +boolT qh_newstats (int index, int *nextindex); +boolT qh_nostatistic (int i); +void qh_printallstatistics (FILE *fp, char *string); +void qh_printstatistics (FILE *fp, char *string); +void qh_printstatlevel (FILE *fp, int id, int start); +void qh_printstats (FILE *fp, int index, int *nextindex); +realT qh_stddev (int num, realT tot, realT tot2, realT *ave); + +#endif /* qhDEFstat */ diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/user.c b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/user.c new file mode 100644 index 0000000000000000000000000000000000000000..b9a75a0b15c3d3786dff33afefe3b3d81301a6c7 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/user.c @@ -0,0 +1,324 @@ +/*<html><pre> -<a href="qh-user.htm" + >-------------------------------</a><a name="TOP">-</a> + + user.c + user redefinable functions + + see README.txt see COPYING.txt for copyright information. + + see qhull.h for data structures, macros, and user-callable functions. + + see user_eg.c, unix.c, and qhull_interface.cpp for examples. + + see user.h for user-definable constants + + use qh_NOmem in mem.h to turn off memory management + use qh_NOmerge in user.h to turn off facet merging + set qh_KEEPstatistics in user.h to 0 to turn off statistics + + This is unsupported software. You're welcome to make changes, + but you're on your own if something goes wrong. Use 'Tc' to + check frequently. Usually qhull will report an error if + a data structure becomes inconsistent. If so, it also reports + the last point added to the hull, e.g., 102. You can then trace + the execution of qhull with "T4P102". + + Please report any errors that you fix to qhull@qhull.org + + call_qhull is a template for calling qhull from within your application + + if you recompile and load this module, then user.o will not be loaded + from qhull.a + + you can add additional quick allocation sizes in qh_user_memsizes + + if the other functions here are redefined to not use qh_print..., + then io.o will not be loaded from qhull.a. See user_eg.c for an + example. We recommend keeping io.o for the extra debugging + information it supplies. +*/ + +#include "qhull_a.h" + +/*-<a href="qh-user.htm#TOC" + >-------------------------------</a><a name="call_qhull">-</a> + + qh_call_qhull( void ) + template for calling qhull from inside your program + remove #if 0, #endif to compile + + returns: + exit code (see qh_ERR... in qhull.h) + all memory freed + + notes: + This can be called any number of times. + + see: + qh_call_qhull_once() + +*/ +#if 0 +{ + int dim; /* dimension of points */ + int numpoints; /* number of points */ + coordT *points; /* array of coordinates for each point */ + boolT ismalloc; /* True if qhull should free points in qh_freeqhull() or reallocation */ + char flags[]= "qhull Tv"; /* option flags for qhull, see qh_opt.htm */ + FILE *outfile= stdout; /* output from qh_produce_output() + use NULL to skip qh_produce_output() */ + FILE *errfile= stderr; /* error messages from qhull code */ + int exitcode; /* 0 if no error from qhull */ + facetT *facet; /* set by FORALLfacets */ + int curlong, totlong; /* memory remaining after qh_memfreeshort */ + + /* initialize dim, numpoints, points[], ismalloc here */ + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, + flags, outfile, errfile); + if (!exitcode) { /* if no error */ + /* 'qh facet_list' contains the convex hull */ + FORALLfacets { + /* ... your code ... */ + } + } + qh_freeqhull(!qh_ALL); + qh_memfreeshort (&curlong, &totlong); + if (curlong || totlong) + fprintf (errfile, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", totlong, curlong); +} +#endif + +/*-<a href="qh-user.htm#TOC" + >-------------------------------</a><a name="new_qhull">-</a> + + qh_new_qhull( dim, numpoints, points, ismalloc, qhull_cmd, outfile, errfile ) + build new qhull data structure and return exitcode (0 if no errors) + + notes: + do not modify points until finished with results. + The qhull data structure contains pointers into the points array. + do not call qhull functions before qh_new_qhull(). + The qhull data structure is not initialized until qh_new_qhull(). + + outfile may be null + qhull_cmd must start with "qhull " + projects points to a new point array for Delaunay triangulations ('d' and 'v') + transforms points into a new point array for halfspace intersection ('H') + + + To allow multiple, concurrent calls to qhull() + - set qh_QHpointer in user.h + - use qh_save_qhull and qh_restore_qhull to swap the global data structure between calls. + - use qh_freeqhull(qh_ALL) to free intermediate convex hulls + + see: + user_eg.c for an example +*/ +int qh_new_qhull (int dim, int numpoints, coordT *points, boolT ismalloc, + char *qhull_cmd, FILE *outfile, FILE *errfile) { + int exitcode, hulldim; + boolT new_ismalloc; + static boolT firstcall = True; + coordT *new_points; + + if (firstcall) { + qh_meminit (errfile); + firstcall= False; + } + if (strncmp (qhull_cmd,"qhull ", 6)) { + fprintf (errfile, "qh_new_qhull: start qhull_cmd argument with \"qhull \"\n"); + exit(1); + } + qh_initqhull_start (NULL, outfile, errfile); + trace1(( qh ferr, "qh_new_qhull: build new Qhull for %d %d-d points with %s\n", numpoints, dim, qhull_cmd)); + exitcode = setjmp (qh errexit); + if (!exitcode) + { + qh NOerrexit = False; + qh_initflags (qhull_cmd); + if (qh DELAUNAY) + qh PROJECTdelaunay= True; + if (qh HALFspace) { + /* points is an array of halfspaces, + the last coordinate of each halfspace is its offset */ + hulldim= dim-1; + qh_setfeasible (hulldim); + new_points= qh_sethalfspace_all (dim, numpoints, points, qh feasible_point); + new_ismalloc= True; + if (ismalloc) + free (points); + }else { + hulldim= dim; + new_points= points; + new_ismalloc= ismalloc; + } + qh_init_B (new_points, numpoints, hulldim, new_ismalloc); + qh_qhull(); + qh_check_output(); + if (outfile) + qh_produce_output(); + if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone) + qh_check_points(); + } + qh NOerrexit = True; + return exitcode; +} /* new_qhull */ + +/*-<a href="qh-user.htm#TOC" + >-------------------------------</a><a name="errexit">-</a> + + qh_errexit( exitcode, facet, ridge ) + report and exit from an error + report facet and ridge if non-NULL + reports useful information such as last point processed + set qh.FORCEoutput to print neighborhood of facet + + see: + qh_errexit2() in qhull.c for printing 2 facets + + design: + check for error within error processing + compute qh.hulltime + print facet and ridge (if any) + report commandString, options, qh.furthest_id + print summary and statistics (including precision statistics) + if qh_ERRsingular + print help text for singular data set + exit program via long jump (if defined) or exit() +*/ +void qh_errexit(int exitcode, facetT *facet, ridgeT *ridge) { + + if (qh ERREXITcalled) { + fprintf (qh ferr, "\nqhull error while processing previous error. Exit program\n"); + exit(1); + } + qh ERREXITcalled= True; + if (!qh QHULLfinished) + qh hulltime= qh_CPUclock - qh hulltime; + qh_errprint("ERRONEOUS", facet, NULL, ridge, NULL); + fprintf (qh ferr, "\nWhile executing: %s | %s\n", qh rbox_command, qh qhull_command); + fprintf(qh ferr, "Options selected for Qhull %s:\n%s\n", qh_version, qh qhull_options); + if (qh furthest_id >= 0) { + fprintf(qh ferr, "Last point added to hull was p%d.", qh furthest_id); + if (zzval_(Ztotmerge)) + fprintf(qh ferr, " Last merge was #%d.", zzval_(Ztotmerge)); + if (qh QHULLfinished) + fprintf(qh ferr, "\nQhull has finished constructing the hull."); + else if (qh POSTmerging) + fprintf(qh ferr, "\nQhull has started post-merging."); + fprintf (qh ferr, "\n"); + } + if (qh FORCEoutput && (qh QHULLfinished || (!facet && !ridge))) + qh_produce_output(); + else { + if (exitcode != qh_ERRsingular && zzval_(Zsetplane) > qh hull_dim+1) { + fprintf (qh ferr, "\nAt error exit:\n"); + qh_printsummary (qh ferr); + if (qh PRINTstatistics) { + qh_collectstatistics(); + qh_printstatistics(qh ferr, "at error exit"); + qh_memstatistics (qh ferr); + } + } + if (qh PRINTprecision) + qh_printstats (qh ferr, qhstat precision, NULL); + } + if (!exitcode) + exitcode= qh_ERRqhull; + else if (exitcode == qh_ERRsingular) + qh_printhelp_singular(qh ferr); + else if (exitcode == qh_ERRprec && !qh PREmerge) + qh_printhelp_degenerate (qh ferr); + if (qh NOerrexit) { + fprintf (qh ferr, "qhull error while ending program. Exit program\n"); + exit(1); + } + qh NOerrexit= True; + longjmp(qh errexit, exitcode); +} /* errexit */ + + +/*-<a href="qh-user.htm#TOC" + >-------------------------------</a><a name="errprint">-</a> + + qh_errprint( fp, string, atfacet, otherfacet, atridge, atvertex ) + prints out the information of facets and ridges to fp + also prints neighbors and geomview output + + notes: + except for string, any parameter may be NULL +*/ +void qh_errprint(char *string, facetT *atfacet, facetT *otherfacet, ridgeT *atridge, vertexT *atvertex) { + int i; + + if (atfacet) { + fprintf(qh ferr, "%s FACET:\n", string); + qh_printfacet(qh ferr, atfacet); + } + if (otherfacet) { + fprintf(qh ferr, "%s OTHER FACET:\n", string); + qh_printfacet(qh ferr, otherfacet); + } + if (atridge) { + fprintf(qh ferr, "%s RIDGE:\n", string); + qh_printridge(qh ferr, atridge); + if (atridge->top && atridge->top != atfacet && atridge->top != otherfacet) + qh_printfacet(qh ferr, atridge->top); + if (atridge->bottom + && atridge->bottom != atfacet && atridge->bottom != otherfacet) + qh_printfacet(qh ferr, atridge->bottom); + if (!atfacet) + atfacet= atridge->top; + if (!otherfacet) + otherfacet= otherfacet_(atridge, atfacet); + } + if (atvertex) { + fprintf(qh ferr, "%s VERTEX:\n", string); + qh_printvertex (qh ferr, atvertex); + } + if (qh fout && qh FORCEoutput && atfacet && !qh QHULLfinished && !qh IStracing) { + fprintf(qh ferr, "ERRONEOUS and NEIGHBORING FACETS to output\n"); + for (i= 0; i < qh_PRINTEND; i++) /* use fout for geomview output */ + qh_printneighborhood (qh fout, qh PRINTout[i], atfacet, otherfacet, + !qh_ALL); + } +} /* errprint */ + + +/*-<a href="qh-user.htm#TOC" + >-------------------------------</a><a name="printfacetlist">-</a> + + qh_printfacetlist( fp, facetlist, facets, printall ) + print all fields for a facet list and/or set of facets to fp + if !printall, + only prints good facets + + notes: + also prints all vertices +*/ +void qh_printfacetlist(facetT *facetlist, setT *facets, boolT printall) { + facetT *facet, **facetp; + + qh_printbegin (qh ferr, qh_PRINTfacets, facetlist, facets, printall); + FORALLfacet_(facetlist) + qh_printafacet(qh ferr, qh_PRINTfacets, facet, printall); + FOREACHfacet_(facets) + qh_printafacet(qh ferr, qh_PRINTfacets, facet, printall); + qh_printend (qh ferr, qh_PRINTfacets, facetlist, facets, printall); +} /* printfacetlist */ + + +/*-<a href="qh-globa.htm#TOC" + >-------------------------------</a><a name="user_memsizes">-</a> + + qh_user_memsizes() + allocate up to 10 additional, quick allocation sizes + + notes: + increase maximum number of allocations in qh_initqhull_mem() +*/ +void qh_user_memsizes (void) { + + /* qh_memsize (size); */ +} /* user_memsizes */ + diff --git a/GraspPlanning/ExternalDependencies/qhull-2003.1/src/user.h b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/user.h new file mode 100644 index 0000000000000000000000000000000000000000..79558967a52a5555028d9a16e2cf856c8d3f45e7 --- /dev/null +++ b/GraspPlanning/ExternalDependencies/qhull-2003.1/src/user.h @@ -0,0 +1,762 @@ +/*<html><pre> -<a href="qh-user.htm" + >-------------------------------</a><a name="TOP">-</a> + + user.h + user redefinable constants + + see qh-user.htm. see COPYING for copyright information. + + before reading any code, review qhull.h for data structure definitions and + the "qh" macro. +*/ + +#ifndef qhDEFuser +#define qhDEFuser 1 + +/*============= data types and configuration macros ==========*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="realT">-</a> + + realT + set the size of floating point numbers + + qh_REALdigits + maximimum number of significant digits + + qh_REAL_1, qh_REAL_2n, qh_REAL_3n + format strings for printf + + qh_REALmax, qh_REALmin + maximum and minimum (near zero) values + + qh_REALepsilon + machine roundoff. Maximum roundoff error for addition and multiplication. + + notes: + Select whether to store floating point numbers in single precision (float) + or double precision (double). + + Use 'float' to save about 8% in time and 25% in space. This is particularly + help if high-d where convex hulls are space limited. Using 'float' also + reduces the printed size of Qhull's output since numbers have 8 digits of + precision. + + Use 'double' when greater arithmetic precision is needed. This is needed + for Delaunay triangulations and Voronoi diagrams when you are not merging + facets. + + If 'double' gives insufficient precision, your data probably includes + degeneracies. If so you should use facet merging (done by default) + or exact arithmetic (see imprecision section of manual, qh-impre.htm). + You may also use option 'Po' to force output despite precision errors. + + You may use 'long double', but many format statements need to be changed + and you may need a 'long double' square root routine. S. Grundmann + (sg@eeiwzb.et.tu-dresden.de) has done this. He reports that the code runs + much slower with little gain in precision. + + WARNING: on some machines, int f(){realT a= REALmax;return (a == REALmax);} + returns False. Use (a > REALmax/2) instead of (a == REALmax). + + REALfloat = 1 all numbers are 'float' type + = 0 all numbers are 'double' type +*/ +#define REALfloat 0 + +#if (REALfloat == 1) +#define realT float +#define REALmax FLT_MAX +#define REALmin FLT_MIN +#define REALepsilon FLT_EPSILON +#define qh_REALdigits 8 /* maximum number of significant digits */ +#define qh_REAL_1 "%6.8g " +#define qh_REAL_2n "%6.8g %6.8g\n" +#define qh_REAL_3n "%6.8g %6.8g %6.8g\n" + +#elif (REALfloat == 0) +#define realT double +#define REALmax DBL_MAX +#define REALmin DBL_MIN +#define REALepsilon DBL_EPSILON +#define qh_REALdigits 16 /* maximum number of significant digits */ +#define qh_REAL_1 "%6.16g " +#define qh_REAL_2n "%6.16g %6.16g\n" +#define qh_REAL_3n "%6.16g %6.16g %6.16g\n" + +#else +#error unknown float option +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="CPUclock">-</a> + + qh_CPUclock + define the clock() function for reporting the total time spent by Qhull + returns CPU ticks as a 'long int' + qh_CPUclock is only used for reporting the total time spent by Qhull + + qh_SECticks + the number of clock ticks per second + + notes: + looks for CLOCKS_PER_SEC, CLOCKS_PER_SECOND, or assumes microseconds + to define a custom clock, set qh_CLOCKtype to 0 + + if your system does not use clock() to return CPU ticks, replace + qh_CPUclock with the corresponding function. It is converted + to unsigned long to prevent wrap-around during long runs. + + + Set qh_CLOCKtype to + + 1 for CLOCKS_PER_SEC, CLOCKS_PER_SECOND, or microsecond + Note: may fail if more than 1 hour elapsed time + + 2 use qh_clock() with POSIX times() (see global.c) +*/ +#define qh_CLOCKtype 1 /* change to the desired number */ + +#if (qh_CLOCKtype == 1) + +#if defined (CLOCKS_PER_SECOND) +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks CLOCKS_PER_SECOND + +#elif defined (CLOCKS_PER_SEC) +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks CLOCKS_PER_SEC + +#elif defined (CLK_TCK) +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks CLK_TCK + +#else +#define qh_CPUclock ((unsigned long)clock()) /* return CPU clock */ +#define qh_SECticks 1E6 +#endif + +#elif (qh_CLOCKtype == 2) +#define qh_CPUclock qh_clock() /* return CPU clock */ +#define qh_SECticks 100 + +#else /* qh_CLOCKtype == ? */ +#error unknown clock option +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="RANDOM">-</a> + + qh_RANDOMtype, qh_RANDOMmax, qh_RANDOMseed + define random number generator + + qh_RANDOMint generates a random integer between 0 and qh_RANDOMmax. + qh_RANDOMseed sets the random number seed for qh_RANDOMint + + Set qh_RANDOMtype (default 5) to: + 1 for random() with 31 bits (UCB) + 2 for rand() with RAND_MAX or 15 bits (system 5) + 3 for rand() with 31 bits (Sun) + 4 for lrand48() with 31 bits (Solaris) + 5 for qh_rand() with 31 bits (included with Qhull) + + notes: + Random numbers are used by rbox to generate point sets. Random + numbers are used by Qhull to rotate the input ('QRn' option), + simulate a randomized algorithm ('Qr' option), and to simulate + roundoff errors ('Rn' option). + + Random number generators differ between systems. Most systems provide + rand() but the period varies. The period of rand() is not critical + since qhull does not normally use random numbers. + + The default generator is Park & Miller's minimal standard random + number generator [CACM 31:1195 '88]. It is included with Qhull. + + If qh_RANDOMmax is wrong, qhull will report a warning and Geomview + output will likely be invisible. +*/ +#define qh_RANDOMtype 5 /* *** change to the desired number *** */ + +#if (qh_RANDOMtype == 1) +#define qh_RANDOMmax ((realT)0x7fffffffUL) /* 31 bits, random()/MAX */ +#define qh_RANDOMint random() +#define qh_RANDOMseed_(seed) srandom(seed); + +#elif (qh_RANDOMtype == 2) +#ifdef RAND_MAX +#define qh_RANDOMmax ((realT)RAND_MAX) +#else +#define qh_RANDOMmax ((realT)32767) /* 15 bits (System 5) */ +#endif +#define qh_RANDOMint rand() +#define qh_RANDOMseed_(seed) srand((unsigned)seed); + +#elif (qh_RANDOMtype == 3) +#define qh_RANDOMmax ((realT)0x7fffffffUL) /* 31 bits, Sun */ +#define qh_RANDOMint rand() +#define qh_RANDOMseed_(seed) srand((unsigned)seed); + +#elif (qh_RANDOMtype == 4) +#define qh_RANDOMmax ((realT)0x7fffffffUL) /* 31 bits, lrand38()/MAX */ +#define qh_RANDOMint lrand48() +#define qh_RANDOMseed_(seed) srand48(seed); + +#elif (qh_RANDOMtype == 5) +#define qh_RANDOMmax ((realT)2147483646UL) /* 31 bits, qh_rand/MAX */ +#define qh_RANDOMint qh_rand() +#define qh_RANDOMseed_(seed) qh_srand(seed); +/* unlike rand(), never returns 0 */ + +#else +#error: unknown random option +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="ORIENTclock">-</a> + + qh_ORIENTclock + 0 for inward pointing normals by Geomview convention +*/ +#define qh_ORIENTclock 0 + + +/*========= performance related constants =========*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="HASHfactor">-</a> + + qh_HASHfactor + total hash slots / used hash slots. Must be at least 1.1. + + notes: + =2 for at worst 50% occupancy for qh hash_table and normally 25% occupancy +*/ +#define qh_HASHfactor 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="VERIFYdirect">-</a> + + qh_VERIFYdirect + with 'Tv' verify all points against all facets if op count is smaller + + notes: + if greater, calls qh_check_bestdist() instead +*/ +#define qh_VERIFYdirect 1000000 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="INITIALsearch">-</a> + + qh_INITIALsearch + if qh_INITIALmax, search points up to this dimension +*/ +#define qh_INITIALsearch 6 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="INITIALmax">-</a> + + qh_INITIALmax + if dim >= qh_INITIALmax, use min/max coordinate points for initial simplex + + notes: + from points with non-zero determinants + use option 'Qs' to override (much slower) +*/ +#define qh_INITIALmax 8 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEdefault">-</a> + + qh_JOGGLEdefault + default qh.JOGGLEmax is qh.DISTround * qh_JOGGLEdefault + + notes: + rbox s r 100 | qhull QJ1e-15 QR0 generates 90% faults at distround 7e-16 + rbox s r 100 | qhull QJ1e-14 QR0 generates 70% faults + rbox s r 100 | qhull QJ1e-13 QR0 generates 35% faults + rbox s r 100 | qhull QJ1e-12 QR0 generates 8% faults + rbox s r 100 | qhull QJ1e-11 QR0 generates 1% faults + rbox s r 100 | qhull QJ1e-10 QR0 generates 0% faults + rbox 1000 W0 | qhull QJ1e-12 QR0 generates 86% faults + rbox 1000 W0 | qhull QJ1e-11 QR0 generates 20% faults + rbox 1000 W0 | qhull QJ1e-10 QR0 generates 2% faults + the later have about 20 points per facet, each of which may interfere + + pick a value large enough to avoid retries on most inputs +*/ +#define qh_JOGGLEdefault 30000.0 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEincrease">-</a> + + qh_JOGGLEincrease + factor to increase qh.JOGGLEmax on qh_JOGGLEretry or qh_JOGGLEagain +*/ +#define qh_JOGGLEincrease 10.0 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEretry">-</a> + + qh_JOGGLEretry + if ZZretry = qh_JOGGLEretry, increase qh.JOGGLEmax + + notes: + try twice at the original value in case of bad luck the first time +*/ +#define qh_JOGGLEretry 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEagain">-</a> + + qh_JOGGLEagain + every following qh_JOGGLEagain, increase qh.JOGGLEmax + + notes: + 1 is OK since it's already failed qh_JOGGLEretry times +*/ +#define qh_JOGGLEagain 1 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEmaxincrease">-</a> + + qh_JOGGLEmaxincrease + maximum qh.JOGGLEmax due to qh_JOGGLEincrease + relative to qh.MAXwidth + + notes: + qh.joggleinput will retry at this value until qh_JOGGLEmaxretry +*/ +#define qh_JOGGLEmaxincrease 1e-2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="JOGGLEmaxretry">-</a> + + qh_JOGGLEmaxretry + stop after qh_JOGGLEmaxretry attempts +*/ +#define qh_JOGGLEmaxretry 100 + +/*========= memory constants =========*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MEMalign">-</a> + + qh_MEMalign + memory alignment for qh_meminitbuffers() in global.c + + notes: + to avoid bus errors, memory allocation must consider alignment requirements. + malloc() automatically takes care of alignment. Since mem.c manages + its own memory, we need to explicitly specify alignment in + qh_meminitbuffers(). + + A safe choice is sizeof(double). sizeof(float) may be used if doubles + do not occur in data structures and pointers are the same size. Be careful + of machines (e.g., DEC Alpha) with large pointers. + + If using gcc, best alignment is + #define qh_MEMalign fmax_(__alignof__(realT),__alignof__(void *)) +*/ +#define qh_MEMalign fmax_(sizeof(realT), sizeof(void *)) + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MEMbufsize">-</a> + + qh_MEMbufsize + size of additional memory buffers + + notes: + used for qh_meminitbuffers() in global.c +*/ +#define qh_MEMbufsize 0x10000 /* allocate 64K memory buffers */ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MEMinitbuf">-</a> + + qh_MEMinitbuf + size of initial memory buffer + + notes: + use for qh_meminitbuffers() in global.c +*/ +#define qh_MEMinitbuf 0x20000 /* initially allocate 128K buffer */ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="INFINITE">-</a> + + qh_INFINITE + on output, indicates Voronoi center at infinity +*/ +#define qh_INFINITE -10.101 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DEFAULTbox">-</a> + + qh_DEFAULTbox + default box size (Geomview expects 0.5) +*/ +#define qh_DEFAULTbox 0.5 + +/*======= conditional compilation ============================*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="compiler">-</a> + + __cplusplus + defined by C++ compilers + + __MSC_VER + defined by Microsoft Visual C++ + + __MWERKS__ && __POWERPC__ + defined by Metrowerks when compiling for the Power Macintosh + + __STDC__ + defined for strict ANSI C +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="COMPUTEfurthest">-</a> + + qh_COMPUTEfurthest + compute furthest distance to an outside point instead of storing it with the facet + =1 to compute furthest + + notes: + computing furthest saves memory but costs time + about 40% more distance tests for partitioning + removes facet->furthestdist +*/ +#define qh_COMPUTEfurthest 0 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="KEEPstatistics">-</a> + + qh_KEEPstatistics + =0 removes most of statistic gathering and reporting + + notes: + if 0, code size is reduced by about 4%. +*/ +#define qh_KEEPstatistics 1 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXoutside">-</a> + + qh_MAXoutside + record outer plane for each facet + =1 to record facet->maxoutside + + notes: + this takes a realT per facet and slightly slows down qhull + it produces better outer planes for geomview output +*/ +#define qh_MAXoutside 1 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="NOmerge">-</a> + + qh_NOmerge + disables facet merging if defined + + notes: + This saves about 10% space. + + Unless 'Q0' + qh_NOmerge sets 'QJ' to avoid precision errors + + #define qh_NOmerge + + see: + <a href="mem.h#NOmem">qh_NOmem</a> in mem.c + + see user.c/user_eg.c for removing io.o +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="NOtrace">-</a> + + qh_NOtrace + no tracing if defined + + notes: + This saves about 5% space. + + #define qh_NOtrace +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="QHpointer">-</a> + + qh_QHpointer + access global data with pointer or static structure + + qh_QHpointer = 1 access globals via a pointer to allocated memory + enables qh_saveqhull() and qh_restoreqhull() + costs about 8% in time and 2% in space + + = 0 qh_qh and qh_qhstat are static data structures + only one instance of qhull() can be active at a time + default value + + notes: + all global variables for qhull are in qh, qhmem, and qhstat + qh is defined in qhull.h + qhmem is defined in mem.h + qhstat is defined in stat.h + + see: + user_eg.c for an example +*/ +#define qh_QHpointer 0 +#if 0 /* sample code */ + qhT *oldqhA, *oldqhB; + + exitcode= qh_new_qhull (dim, numpoints, points, ismalloc, + flags, outfile, errfile); + /* use results from first call to qh_new_qhull */ + oldqhA= qh_save_qhull(); + exitcode= qh_new_qhull (dimB, numpointsB, pointsB, ismalloc, + flags, outfile, errfile); + /* use results from second call to qh_new_qhull */ + oldqhB= qh_save_qhull(); + qh_restore_qhull (&oldqhA); + /* use results from first call to qh_new_qhull */ + qh_freeqhull (qh_ALL); /* frees all memory used by first call */ + qh_restore_qhull (&oldqhB); + /* use results from second call to qh_new_qhull */ + qh_freeqhull (!qh_ALL); /* frees long memory used by second call */ + qh_memfreeshort (&curlong, &totlong); /* frees short memory and memory allocator */ +#endif + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="QUICKhelp">-</a> + + qh_QUICKhelp + =1 to use abbreviated help messages, e.g., for degenerate inputs +*/ +#define qh_QUICKhelp 0 + +/* ============ -merge constants- ==================== + + These constants effect facet merging. You probably will not need + to modify these. They effect the performance of facet merging. +*/ + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DIMmergeVertex">-</a> + + qh_DIMmergeVertex + max dimension for vertex merging (it is not effective in high-d) +*/ +#define qh_DIMmergeVertex 6 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DIMreduceBuild">-</a> + + qh_DIMreduceBuild + max dimension for vertex reduction during build (slow in high-d) +*/ +#define qh_DIMreduceBuild 5 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="BESTcentrum">-</a> + + qh_BESTcentrum + if > 2*dim+n vertices, qh_findbestneighbor() tests centrums (faster) + else, qh_findbestneighbor() tests all vertices (much better merges) + + qh_BESTcentrum2 + if qh_BESTcentrum2 * DIM3 + BESTcentrum < #vertices tests centrums +*/ +#define qh_BESTcentrum 20 +#define qh_BESTcentrum2 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="BESTnonconvex">-</a> + + qh_BESTnonconvex + if > dim+n neighbors, qh_findbestneighbor() tests nonconvex ridges. + + notes: + It is needed because qh_findbestneighbor is slow for large facets +*/ +#define qh_BESTnonconvex 15 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXnewmerges">-</a> + + qh_MAXnewmerges + if >n newmerges, qh_merge_nonconvex() calls qh_reducevertices_centrums. + + notes: + It is needed because postmerge can merge many facets at once +*/ +#define qh_MAXnewmerges 2 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXnewcentrum">-</a> + + qh_MAXnewcentrum + if <= dim+n vertices (n approximates the number of merges), + reset the centrum in qh_updatetested() and qh_mergecycle_facets() + + notes: + needed to reduce cost and because centrums may move too much if + many vertices in high-d +*/ +#define qh_MAXnewcentrum 5 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="COPLANARratio">-</a> + + qh_COPLANARratio + for 3-d+ merging, qh.MINvisible is n*premerge_centrum + + notes: + for non-merging, it's DISTround +*/ +#define qh_COPLANARratio 3 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="DISToutside">-</a> + + qh_DISToutside + When is a point clearly outside of a facet? + Stops search in qh_findbestnew or qh_partitionall + qh_findbest uses qh.MINoutside since since it is only called if no merges. + + notes: + 'Qf' always searches for best facet + if !qh.MERGING, same as qh.MINoutside. + if qh_USEfindbestnew, increase value since neighboring facets may be ill-behaved + [Note: Zdelvertextot occurs normally with interior points] + RBOX 1000 s Z1 G1e-13 t1001188774 | QHULL Tv + When there is a sharp edge, need to move points to a + clearly good facet; otherwise may be lost in another partitioning. + if too big then O(n^2) behavior for partitioning in cone + if very small then important points not processed + Needed in qh_partitionall for + RBOX 1000 s Z1 G1e-13 t1001032651 | QHULL Tv + Needed in qh_findbestnew for many instances of + RBOX 1000 s Z1 G1e-13 t | QHULL Tv + + See: + qh_DISToutside -- when is a point clearly outside of a facet + qh_SEARCHdist -- when is facet coplanar with the best facet? + qh_USEfindbestnew -- when to use qh_findbestnew for qh_partitionpoint() +*/ +#define qh_DISToutside ((qh_USEfindbestnew ? 2 : 1) * \ + fmax_((qh MERGING ? 2 : 1)*qh MINoutside, qh max_outside)) + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="RATIOnearinside">-</a> + + qh_RATIOnearinside + ratio of qh.NEARinside to qh.ONEmerge for retaining inside points for + qh_check_maxout(). + + notes: + This is overkill since do not know the correct value. + It effects whether 'Qc' reports all coplanar points + Not used for 'd' since non-extreme points are coplanar +*/ +#define qh_RATIOnearinside 5 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="SEARCHdist">-</a> + + qh_SEARCHdist + When is a facet coplanar with the best facet? + qh_findbesthorizon: all coplanar facets of the best facet need to be searched. + + See: + qh_DISToutside -- when is a point clearly outside of a facet + qh_SEARCHdist -- when is facet coplanar with the best facet? + qh_USEfindbestnew -- when to use qh_findbestnew for qh_partitionpoint() +*/ +#define qh_SEARCHdist ((qh_USEfindbestnew ? 2 : 1) * \ + (qh max_outside + 2 * qh DISTround + fmax_( qh MINvisible, qh MAXcoplanar))); + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="USEfindbestnew">-</a> + + qh_USEfindbestnew + Always use qh_findbestnew for qh_partitionpoint, otherwise use + qh_findbestnew if merged new facet or sharpnewfacets. + + See: + qh_DISToutside -- when is a point clearly outside of a facet + qh_SEARCHdist -- when is facet coplanar with the best facet? + qh_USEfindbestnew -- when to use qh_findbestnew for qh_partitionpoint() +*/ +#define qh_USEfindbestnew (zzval_(Ztotmerge) > 50) + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="WIDEcoplanar">-</a> + + qh_WIDEcoplanar + n*MAXcoplanar or n*MINvisible for a WIDEfacet + + if vertex is further than qh.WIDEfacet from the hyperplane + then its ridges are not counted in computing the area, and + the facet's centrum is frozen. + + notes: + qh.WIDEfacet= max(qh.MAXoutside,qh_WIDEcoplanar*qh.MAXcoplanar, + qh_WIDEcoplanar * qh.MINvisible); +*/ +#define qh_WIDEcoplanar 6 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="MAXnarrow">-</a> + + qh_MAXnarrow + max. cosine in initial hull that sets qh.NARROWhull + + notes: + If qh.NARROWhull, the initial partition does not make + coplanar points. If narrow, a coplanar point can be + coplanar to two facets of opposite orientations and + distant from the exact convex hull. + + Conservative estimate. Don't actually see problems until it is -1.0 +*/ +#define qh_MAXnarrow -0.99999999 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="WARNnarrow">-</a> + + qh_WARNnarrow + max. cosine in initial hull to warn about qh.NARROWhull + + notes: + this is a conservative estimate. + Don't actually see problems until it is -1.0. See qh-impre.htm +*/ +#define qh_WARNnarrow -0.999999999999999 + +/*-<a href="qh-user.htm#TOC" + >--------------------------------</a><a name="ZEROdelaunay">-</a> + + qh_ZEROdelaunay + a zero Delaunay facet occurs for input sites coplanar with their convex hull + the last normal coefficient of a zero Delaunay facet is within + qh_ZEROdelaunay * qh.ANGLEround of 0 + + notes: + qh_ZEROdelaunay does not allow for joggled input ('QJ'). + + You can avoid zero Delaunay facets by surrounding the input with a box. + + Use option 'PDk:-n' to explicitly define zero Delaunay facets + k= dimension of input sites (e.g., 3 for 3-d Delaunay triangulation) + n= the cutoff for zero Delaunay facets (e.g., 'PD3:-1e-12') +*/ +#define qh_ZEROdelaunay 2 + +#endif /* qh_DEFuser */ + + + diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..82a93c3c38f217937788a031d3de03508b3c443e --- /dev/null +++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp @@ -0,0 +1,125 @@ +#include "GenericGraspPlanner.h" +#include <VirtualRobot/Grasp.h> +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <iostream> +#include <sstream> +#include "GraspQuality/GraspQualityMeasureWrenchSpace.h" +#include "GraspQuality/GraspQualityMeasure.h" +#include "ApproachMovementGenerator.h" + +using namespace std; + +namespace GraspStudio +{ + + +GenericGraspPlanner::GenericGraspPlanner( VirtualRobot::GraspSetPtr graspSet, GraspStudio::GraspQualityMeasurePtr graspQuality, GraspStudio::ApproachMovementGeneratorPtr approach, float minQuality, bool forceClosure ) +:GraspPlanner(graspSet), graspQuality(graspQuality), approach(approach), minQuality(minQuality), forceClosure(forceClosure) +{ + THROW_VR_EXCEPTION_IF(!graspQuality, "NULL grasp quality..."); + THROW_VR_EXCEPTION_IF(!approach, "NULL approach..."); + THROW_VR_EXCEPTION_IF(!graspQuality->getObject(), "no object..."); + THROW_VR_EXCEPTION_IF(graspQuality->getObject() != approach->getObject(), "graspQuality and approach have to use the same object."); + object = graspQuality->getObject(); + eef = approach->getEEF(); + THROW_VR_EXCEPTION_IF(!eef, "NULL eef in approach..."); + THROW_VR_EXCEPTION_IF(!graspSet, "NULL graspSet..."); + verbose = true; +} + +GenericGraspPlanner::~GenericGraspPlanner() +{ +} + +int GenericGraspPlanner::plan(int nrGrasps, int timeOutMS) +{ + startTime = clock(); + this->timeOutMS = timeOutMS; + + int nLoop = 0; + int nGraspsCreated = 0; + + if (verbose) + { + GRASPSTUDIO_INFO << ": Searching " << nrGrasps << " grasps for EEF:" << approach->getEEF()->getName() << " and object:" << graspQuality->getObject()->getName() << ".\n"; + GRASPSTUDIO_INFO << ": Approach movements are generated with " << approach->getName() << endl; + GRASPSTUDIO_INFO << ": Grasps are evaluated with " << graspQuality->getName() << endl; + } + + while (!timeout() && nGraspsCreated < nrGrasps) + { + VirtualRobot::GraspPtr g = planGrasp(); + if (g) + { + if (graspSet) + graspSet->addGrasp(g); + plannedGrasps.push_back(g); + nGraspsCreated++; + } + nLoop++; + } + if (verbose) + GRASPSTUDIO_INFO << ": created " << nGraspsCreated << " valid grasps in " << nLoop << " loops" << endl; + return nGraspsCreated; +} + +VirtualRobot::GraspPtr GenericGraspPlanner::planGrasp() +{ + + std::string sGraspPlanner("Simox - GraspStudio - "); + sGraspPlanner += graspQuality->getName(); + std::string sGraspNameBase = "Grasp "; + + VirtualRobot::RobotPtr robot = approach->getEEFOriginal()->getRobot(); + VirtualRobot::RobotNodePtr tcp = eef->getTcp(); + + VR_ASSERT(robot); + VR_ASSERT(tcp); + + + bool bRes = approach->setEEFToRandomApproachPose(); + if (!bRes) + return VirtualRobot::GraspPtr(); + + std::vector< VirtualRobot::EndEffector::ContactInfo > contacts; + contacts = eef->closeActors(object); + + if (contacts.size()<2) + { + if (verbose) + GRASPSTUDIO_INFO << ": ignoring grasp hypothesis, low number of contacts" << endl; + return VirtualRobot::GraspPtr(); + } + + graspQuality->setContactPoints(contacts); + float score = graspQuality->getGraspQuality(); + if (score<minQuality) + return VirtualRobot::GraspPtr(); + if (forceClosure && !graspQuality->isGraspForceClosure()) + return VirtualRobot::GraspPtr(); + + // found valid grasp + if (verbose) + { + GRASPSTUDIO_INFO << ": Found grasp with " << contacts.size() << " contacts, score: " << score << endl; + } + std::stringstream ss; + ss << sGraspNameBase << (graspSet->getSize()+1); + std::string sGraspName = ss.str(); + Eigen::Matrix4f objP = object->getGlobalPose(); + Eigen::Matrix4f pLocal = tcp->toLocalCoordinateSystem(objP); + VirtualRobot::GraspPtr g(new VirtualRobot::Grasp(sGraspName,robot->getType(),eef->getName(),pLocal,sGraspPlanner,score)); + return g; +} + +bool GenericGraspPlanner::timeout() +{ + if (timeOutMS<=0) + return false; + clock_t endTime = clock(); + int timeMS = (int)(((float)(endTime - startTime) / (float)CLOCKS_PER_SEC) * 1000.0); + return (timeMS > timeOutMS); +} + +} // namespace diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h new file mode 100644 index 0000000000000000000000000000000000000000..72d4b6ba19ef62512c76bbfca7bd7e4631dce755 --- /dev/null +++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h @@ -0,0 +1,85 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __GENERIC_GRASP_PLANNER_H__ +#define __GENERIC_GRASP_PLANNER_H__ + +#include "GraspStudio.h" +#include "GraspPlanner.h" +#include "ApproachMovementGenerator.h" +#include "GraspQuality/GraspQualityMeasure.h" + +namespace GraspStudio +{ +/*! +* +* +* A general grasp planning class that utilizes ApprachMovementGenerator for generating grasp hypothesis and +* GraspQualityMeasure to score them. +* +*/ +class GRASPSTUDIO_IMPORT_EXPORT GenericGraspPlanner : public GraspPlanner +{ +public: + + /*! + Constructor + \param graspSet All planned grasps are added to this GraspSet. + \param graspQuality The quality of generated grasps is evaluated with this object + \param approach Approach movements are generated by this object. + \param minQuality The quality that must be achieved at minimum by the GraspQualityMesurement module + \param forceClosure When true, only force closure grasps are generated. + */ + GenericGraspPlanner(VirtualRobot::GraspSetPtr graspSet, GraspStudio::GraspQualityMeasurePtr graspQuality, GraspStudio::ApproachMovementGeneratorPtr approach, float minQuality = 0.0f, bool forceClosure = true ); + + // destructor + virtual ~GenericGraspPlanner(); + + /*! + Creates new grasps. + \param nrGrasps The number of grasps to be planned. + \param timeOutMS The time out in milliseconds. Planning is stopped when this time is exceeded. Disabled when zero. + \return Number of generated grasps. + */ + virtual int plan(int nrGrasps, int timeOutMS = 0); + + +protected: + + bool timeout(); + + VirtualRobot::GraspPtr planGrasp(); + + VirtualRobot::SceneObjectPtr object; + VirtualRobot::EndEffectorPtr eef; + + clock_t startTime; + int timeOutMS; + + GraspStudio::GraspQualityMeasurePtr graspQuality; + GraspStudio::ApproachMovementGeneratorPtr approach; + float minQuality; + bool forceClosure; + }; +} + +#endif /* __GENERIC_GRASP_PLANNER_H__ */ diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.cpp b/GraspPlanning/GraspPlanner/GraspPlanner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..57d545219d48845a401e87cde3e1ca120c67c98f --- /dev/null +++ b/GraspPlanning/GraspPlanner/GraspPlanner.cpp @@ -0,0 +1,26 @@ +#include "GraspPlanner.h" + +namespace GraspStudio +{ +GraspPlanner::GraspPlanner( VirtualRobot::GraspSetPtr graspSet ) + : graspSet(graspSet) +{ + verbose = false; +} + +GraspPlanner::~GraspPlanner() +{ + +} + +void GraspPlanner::setVerbose(bool enable) +{ + verbose = enable; +} + +std::vector<VirtualRobot::GraspPtr> GraspPlanner::getPlannedGrasps() +{ + return plannedGrasps; +} + +} diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.h b/GraspPlanning/GraspPlanner/GraspPlanner.h new file mode 100644 index 0000000000000000000000000000000000000000..91fe8bf101e1e4f55e444100c9011e863f83d42c --- /dev/null +++ b/GraspPlanning/GraspPlanner/GraspPlanner.h @@ -0,0 +1,76 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __GENERAL_GRASP_PLANNER_H__ +#define __GENERAL_GRASP_PLANNER_H__ + +#include "GraspStudio.h" +#include <VirtualRobot/EndEffector/EndEffector.h> +#include <VirtualRobot/SceneObject.h> +#include <VirtualRobot/GraspSet.h> +#include <vector> + +namespace GraspStudio +{ + +/*! +* +* \brief An interface for grasp planners. +* +*/ +class GRASPSTUDIO_IMPORT_EXPORT GraspPlanner +{ +public: + + /*! + Constructor + \param graspSet Append planned grasps to this set. + */ + GraspPlanner(VirtualRobot::GraspSetPtr graspSet); + + //! destructor + virtual ~GraspPlanner(); + + void setVerbose(bool enable); + + + /*! + Creates new grasps. + \param nrGrasps The number of grasps to be planned. + \param timeOutMS The time out in milliseconds. Planning is stopped when this time is exceeded. Disabled when zero. + \return Number of planned grasps. + */ + virtual int plan(int nrGrasps, int timeOutMS = 0) = 0; + + /*! + Returns all grasps that have been generated. These grasps are also stored in the graspSet that was specified on construction. + */ + std::vector<VirtualRobot::GraspPtr> getPlannedGrasps(); + +protected: + bool verbose; + VirtualRobot::GraspSetPtr graspSet; + std::vector<VirtualRobot::GraspPtr> plannedGrasps; +}; +} + +#endif /* __GENERAL_GRASP_PLANNER_H__ */ diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d45881f87661fc4527d537803f2ad8a1b4ca1792 --- /dev/null +++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp @@ -0,0 +1,213 @@ +// ************************************************************** +// Implementation of class GraspQualityMeasure +// ************************************************************** +// Author: Niko Vahrenkamp +// Date: 26.10.2011 +// ************************************************************** + + +// ************************************************************** +// includes +// ************************************************************** + +#include "GraspQualityMeasure.h" +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/SceneObject.h> +#include <VirtualRobot/CollisionDetection/CollisionModel.h> +#include <VirtualRobot/Visualization/TriMeshModel.h> + +using namespace std; +using namespace VirtualRobot; + +namespace GraspStudio +{ + + +GraspQualityMeasure::GraspQualityMeasure(VirtualRobot::SceneObjectPtr object, float unitForce, float frictionConeCoeff, int frictionConeSamples ) + : object(object), unitForce(unitForce),frictionCoeff(frictionConeCoeff),frictionConeSamples(frictionConeSamples) +{ + THROW_VR_EXCEPTION_IF(!object,"Need an object"); + THROW_VR_EXCEPTION_IF(!object->getCollisionModel(),"Need an object with collision model"); + THROW_VR_EXCEPTION_IF(!object->getCollisionModel()->getTriMeshModel(),"Need an object with trimeshmodel"); + + coneGenerator.reset(new ContactConeGenerator(frictionConeSamples, frictionCoeff, unitForce)); + + //Member variable representing Grasp Quality ranging from 1 to 0 + graspQuality = 0.0; + verbose = false; + objectLength = 0.0f; + + centerOfModel = object->getCollisionModel()->getTriMeshModel()->getCOM(); + + Eigen::Vector3f minS,maxS; + object->getCollisionModel()->getTriMeshModel()->getSize(minS,maxS); + maxS = maxS - minS; + objectLength = maxS(0); + if (maxS(1)>objectLength) + objectLength = maxS(1); + if (maxS(2)>objectLength) + objectLength = maxS(2); +} + +GraspQualityMeasure::~GraspQualityMeasure() +{ +} + +void GraspQualityMeasure::setContactPoints( const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints6d ) +{ + //if (contactPoints.size() < 2) + // return; + this->contactPoints.clear(); + this->contactPointsM.clear(); + std::vector<MathTools::ContactPoint>::const_iterator objPointsIter; + for (objPointsIter = contactPoints.begin(); objPointsIter != contactPoints.end(); objPointsIter++) + { + MathTools::ContactPoint point = (*objPointsIter); + point.p -= centerOfModel; + point.n.normalize(); + + this->contactPoints.push_back(point); + } + MathTools::convertMM2M(this->contactPoints,this->contactPointsM); + if (verbose) + { + GRASPSTUDIO_INFO << ": Nr of contact points:" << this->contactPoints.size() << endl; + } +} + +void GraspQualityMeasure::setContactPoints( const std::vector<EndEffector::ContactInfo> &contactPoints ) +{ + //if (contactPoints.size() < 2) + // return; + this->contactPoints.clear(); + this->contactPointsM.clear(); + std::vector<EndEffector::ContactInfo>::const_iterator objPointsIter; + for (objPointsIter = contactPoints.begin(); objPointsIter != contactPoints.end(); objPointsIter++) + { + MathTools::ContactPoint point; + + point.p = objPointsIter->contactPointObstacleLocal; + point.p -= centerOfModel; + + point.n = objPointsIter->contactPointFingerLocal - objPointsIter->contactPointObstacleLocal; + point.n.normalize(); + + this->contactPoints.push_back(point); + } + VirtualRobot::MathTools::convertMM2M(this->contactPoints,this->contactPointsM); + + if (verbose) + { + GRASPSTUDIO_INFO << ": Nr of contact points:" << this->contactPoints.size() << endl; + } +} + + +bool GraspQualityMeasure::sampleObjectPoints(int nMaxFaces) +{ + sampledObjectPoints.clear(); + sampledObjectPointsM.clear(); + + TriMeshModelPtr model = object->getCollisionModel()->getTriMeshModel(); + //Eigen::Vector3f _com = model->getCOM(); + MathTools::ContactPoint objectPoint; + if (verbose) + GRASPSTUDIO_INFO << ": object COM: << " << centerOfModel[0] << "," << centerOfModel[1] << "," << centerOfModel[2] << endl; + + int nFaces = (int)model->faces.size(); + if (nFaces > nMaxFaces) + { + nFaces = nMaxFaces; + } + int nLoopCount = 0; + std::vector<MathTools::TriangleFace> vFaceCopy = model->faces; + std::vector<MathTools::TriangleFace>::iterator iFaceIter; + + while (nLoopCount<nFaces && vFaceCopy.size()>0) + { + int nRnd = rand() % vFaceCopy.size(); + iFaceIter = vFaceCopy.begin(); + iFaceIter += nRnd; + + objectPoint.p = (model->vertices[iFaceIter->id1] + model->vertices[iFaceIter->id2] + model->vertices[iFaceIter->id3]) / 3.0f; + objectPoint.n = iFaceIter->normal; + objectPoint.n.normalize(); + objectPoint.n *= unitForce; + + // move points so that object is located at origin + objectPoint.p -= centerOfModel; + sampledObjectPoints.push_back(objectPoint); + + vFaceCopy.erase(iFaceIter); + nLoopCount++; + } + + MathTools::convertMM2M(sampledObjectPoints,sampledObjectPointsM); + if (verbose) + { + GRASPSTUDIO_INFO << ": Nr of sample object points:" << sampledObjectPoints.size() << endl; + } + + return (sampledObjectPoints.size()>0); +} + + +MathTools::ContactPoint GraspQualityMeasure::getContactPointsCenter() +{ + MathTools::ContactPoint p; + p.p.setZero(); + p.n.setZero(); + if (contactPoints.size()==0) + return p; + for (int i=0;i<(int)contactPoints.size();i++) + { + p.p += contactPoints[i].p; + p.n += contactPoints[i].n; + + } + p.p /= (float)contactPoints.size(); + p.n /= (float)contactPoints.size(); + + return p; +} + +MathTools::ContactPoint GraspQualityMeasure::getSampledObjectPointsCenter() +{ + MathTools::ContactPoint p; + p.p.setZero(); + p.n.setZero(); + if (sampledObjectPoints.size()==0) + return p; + for (int i=0;i<(int)sampledObjectPoints.size();i++) + { + p.p += sampledObjectPoints[i].p; + p.n += sampledObjectPoints[i].n; + } + p.p /= (float)sampledObjectPoints.size(); + p.n /= (float)sampledObjectPoints.size(); + return p; +} + +void GraspQualityMeasure::setVerbose( bool enable ) +{ + verbose = enable; +} + +std::string GraspQualityMeasure::getName() +{ + std::string sName("GraspQualityMeasure"); + return sName; +} + +Eigen::Vector3f GraspQualityMeasure::getCoM() +{ + return centerOfModel; +} + +VirtualRobot::SceneObjectPtr GraspQualityMeasure::getObject() +{ + return object; +} + + +} // namespace diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.h b/GraspPlanning/GraspQuality/GraspQualityMeasure.h new file mode 100644 index 0000000000000000000000000000000000000000..40be65fac9c9f46262680c4523464b03a8144588 --- /dev/null +++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.h @@ -0,0 +1,117 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __GRASP_QUALTIY_MEASURE_H__ +#define __GRASP_QUALTIY_MEASURE_H__ + +#include "../GraspStudio.h" +#include "../ConvexHullGenerator.h" +#include "../ContactConeGenerator.h" +#include <VirtualRobot/EndEffector/EndEffector.h> +#include <VirtualRobot/MathTools.h> +#include <vector> +#include <Eigen/Core> + +namespace GraspStudio +{ +/*! + \brief An interface class for grasp quality algorithms. + + @see GraspQualityMeasureWrenchSpace +*/ +class GRASPSTUDIO_IMPORT_EXPORT GraspQualityMeasure +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + GraspQualityMeasure(VirtualRobot::SceneObjectPtr object, float unitForce = 1.0f, float frictionConeCoeff = 0.35f, int frictionConeSamples = 8); + + // destructor + ~GraspQualityMeasure(); + + /*! + setup contact information + the contact points are normalized by subtracting the COM + the contact normals are normalize to unit length + */ + virtual void setContactPoints(const std::vector<VirtualRobot::EndEffector::ContactInfo> &contactPoints); + virtual void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints6d); + + /*! + Returns calculated grasp quality + */ + virtual float getGraspQuality() = 0; + + /* + Checks if grasp is force closure + */ + virtual bool isGraspForceClosure() = 0; + + + //! This method is used to compute a reference value that describes a perfect grasp + virtual bool calculateObjectProperties() = 0; + + //! Compute the grasp quality for the given contact points + virtual bool calculateGraspQuality() = 0; + + virtual Eigen::Vector3f getCoM(); + + virtual VirtualRobot::MathTools::ContactPoint getContactPointsCenter(); + virtual VirtualRobot::MathTools::ContactPoint getSampledObjectPointsCenter(); + + virtual void setVerbose(bool enable); + + //! Returns description of this object + virtual std::string getName(); + + VirtualRobot::SceneObjectPtr getObject(); + +protected: + + //Methods + bool sampleObjectPoints(int nMaxFaces = 400); + + //Object relevant parameters + Eigen::Vector3f centerOfModel; + float objectLength; + float graspQuality; + + //Friction cone relevant parameters + float unitForce; + float frictionCoeff; + int frictionConeSamples; + ContactConeGeneratorPtr coneGenerator; + + VirtualRobot::SceneObjectPtr object; + + //For Object and Grasp Wrench Space Calculation + std::vector<VirtualRobot::MathTools::ContactPoint> contactPoints; // in MM + std::vector<VirtualRobot::MathTools::ContactPoint> contactPointsM; // converted to M + std::vector<VirtualRobot::MathTools::ContactPoint> sampledObjectPoints; // in MM + std::vector<VirtualRobot::MathTools::ContactPoint> sampledObjectPointsM; // converted to M + + bool verbose; +}; + +} // namespace + +#endif /* __GRASP_QUALTIY_MEASURE_H__ */ diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1590f0d8dadc15ddcf597b27d781aac372987e36 --- /dev/null +++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp @@ -0,0 +1,500 @@ +// ************************************************************** +// Implementation of class GraspQualityMeasure +// ************************************************************** +// Author: Niko Vahrenkamp +// Date: 26.10.2011 +// ************************************************************** + + +// ************************************************************** +// includes +// ************************************************************** + +#include "GraspQualityMeasureWrenchSpace.h" +#include <cmath> +#include <cstdio> +#include <cassert> +#include <iostream> +#include <sstream> +#include <fstream> +#include <string> +#include <float.h> +// if defined, the inverted contact normals are used +//#define INVERT_NORMALS + +using namespace std; +using namespace VirtualRobot; + +namespace GraspStudio +{ + + +GraspQualityMeasureWrenchSpace::GraspQualityMeasureWrenchSpace(VirtualRobot::SceneObjectPtr object, float unitForce, float frictionConeCoeff, int frictionConeSamples) +:GraspQualityMeasure(object,unitForce,frictionConeCoeff,frictionConeSamples) +{ + OWSCalculated = false; + GWSCalculated = false; + minOffsetOWS = 0.0f; + volumeOWS = 0.0f; +} + +GraspQualityMeasureWrenchSpace::~GraspQualityMeasureWrenchSpace() +{ +} + + +void GraspQualityMeasureWrenchSpace::setContactPoints( const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints ) +{ + GraspQualityMeasure::setContactPoints(contactPoints); + GWSCalculated = false; +} + +void GraspQualityMeasureWrenchSpace::setContactPoints( const std::vector<EndEffector::ContactInfo> &contactPoints ) +{ + GraspQualityMeasure::setContactPoints(contactPoints); + GWSCalculated = false; +} + + +float GraspQualityMeasureWrenchSpace::getGraspQuality() +{ + calculateObjectProperties(); + calculateGraspQuality(); + return graspQuality; +} + +bool GraspQualityMeasureWrenchSpace::isGraspForceClosure() +{ + return isOriginInGWSHull(); +} + +void GraspQualityMeasureWrenchSpace::calculateOWS() + { + bool printAll = false; + bool bRes = sampleObjectPoints(); + if (!bRes) + return; + //Rotate generic friction cone to align with object normals + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator objPointsIter; + std::vector<VirtualRobot::MathTools::ContactPoint> conePoints; + if (verbose && printAll) + cout << "OWS contact points:" << endl; + for (objPointsIter = sampledObjectPointsM.begin(); objPointsIter != sampledObjectPointsM.end(); objPointsIter++) + { + if (verbose && printAll) + MathTools::print( (*objPointsIter) ); + coneGenerator->computeConePoints((*objPointsIter),conePoints); + } + + //Generate convex hull from rotated object friction cones + convexHullOWS = calculateConvexHull(conePoints); + //calculateHullCenter(convexHullOWS, convexHullCenterOWS); + convexHullCenterOWS = convexHullOWS->center; + if (verbose && printAll) + { + GRASPSTUDIO_INFO << " CENTER of OWS: " << endl; + MathTools::print(convexHullCenterOWS); + } + + minOffsetOWS = minOffset(convexHullOWS); + volumeOWS = convexHullOWS->volume; + if (verbose) + { + GRASPSTUDIO_INFO << ": MinDistance to Center of OWS: "<< minOffsetOWS << endl; + GRASPSTUDIO_INFO << ": Volume of OWS: "<< volumeOWS << endl; + } + OWSCalculated = true; +} + +void GraspQualityMeasureWrenchSpace::calculateGWS() +{ + bool printAll = false; + if (contactPointsM.empty()) + { + printf("Contact points not set.\n"); + return; + } + + //Rotate generic friction cone to align with object normals + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator objPointsIter; + std::vector<VirtualRobot::MathTools::ContactPoint> conePoints; + if (verbose && printAll) + cout << "GWS contact points:" << endl; + for (objPointsIter = contactPointsM.begin(); objPointsIter != contactPointsM.end(); objPointsIter++) + { + if (verbose && printAll) + MathTools::print((*objPointsIter)); + coneGenerator->computeConePoints((*objPointsIter),conePoints); + } + + //Generate convex hull from rotated contact friction cones + convexHullGWS = calculateConvexHull(conePoints); + //calculateHullCenter(convexHullGWS, convexHullCenterGWS); + convexHullCenterGWS = convexHullGWS->center; + if (verbose && printAll) + { + GRASPSTUDIO_INFO << " CENTER of GWS: " << endl; + MathTools::print(convexHullCenterGWS); + } + + GWSCalculated = true; +} + +VirtualRobot::MathTools::ConvexHull6DPtr GraspQualityMeasureWrenchSpace::calculateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint> &points) +{ + bool printAll = true; + if (verbose && printAll) + { + cout << "Convex hull points for wrench calculation:" << endl; + printContacts(points); + } + // create wrench + std::vector<VirtualRobot::MathTools::ContactPoint> wrenchPoints = createWrenchPoints(points,Eigen::Vector3f::Zero(),objectLength); // contact points are already moved so that com is at origin + if (verbose && printAll) + { + cout << "Wrench points:" << endl; + printContacts(wrenchPoints); + } + return ConvexHullGenerator::CreateConvexHull(wrenchPoints); +} + +std::vector<VirtualRobot::MathTools::ContactPoint> GraspQualityMeasureWrenchSpace::createWrenchPoints(std::vector<VirtualRobot::MathTools::ContactPoint> &points, const Eigen::Vector3f ¢erOfModel, float objectLengthMM) +{ + std::vector<VirtualRobot::MathTools::ContactPoint> result; + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter = points.begin(); + VirtualRobot::MathTools::ContactPoint p; + Eigen::Vector3f normal; + + // convert objLength from mm to m + bool convertMM2M = true; + + float factor = 1.0f; + if (objectLengthMM!=0) + { + if (convertMM2M) + factor = 2.0f / (objectLengthMM/1000.0f); // == max distance from center ( 1 / (length/2) ) + else + factor = 2.0f / objectLengthMM; // == max distance from center ( 1 / (length/2) ) + } + VirtualRobot::MathTools::ContactPoint tmpP; + + while (iter!=points.end()) + { +#ifdef INVERT_NORMALS + /*p.p(0) = -(*iter).n(0); + p.p(1) = -(*iter).n(1); + p.p(2) = -(*iter).n(2);*/ + p.p = -1.0f*(iter->n); +#else + p.p = iter->n; +#endif + tmpP.p = iter->p - centerOfModel; + tmpP.n = -(iter->n); + + //normal = crossProductPosNormalInv(tmpP); + //p.n = factor * normal; + p.n = factor * tmpP.p.cross(tmpP.n); + + result.push_back(p); + iter++; + } + return result; +} + +void GraspQualityMeasureWrenchSpace::printContacts(std::vector<VirtualRobot::MathTools::ContactPoint> &points) +{ + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter = points.begin(); + while (iter!=points.end()) + { + cout << "# "; + MathTools::print( (*iter) ); + iter++; + } +} + +VirtualRobot::MathTools::ContactPoint GraspQualityMeasureWrenchSpace::calculateHullCenter(VirtualRobot::MathTools::ConvexHull6DPtr hull) +{ + if (!hull) + { + GRASPSTUDIO_ERROR << "NULL data?!" << endl; + return VirtualRobot::MathTools::ContactPoint(); + } + VirtualRobot::MathTools::ContactPoint resultCenter; + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter; + resultCenter.p.setZero(); + resultCenter.n.setZero(); + if (hull->vertices.size()==0) + { + cout << __FUNCTION__ << ": error, no vertices..." << endl; + return resultCenter; + } + for (iter = hull->vertices.begin();iter != hull->vertices.end(); iter++) + { + resultCenter.p += iter->p; + resultCenter.n += iter->n; + } + resultCenter.p /= (float)hull->vertices.size(); + resultCenter.n /= (float)hull->vertices.size(); + return resultCenter; +} + +float GraspQualityMeasureWrenchSpace::minDistanceToGWSHull(VirtualRobot::MathTools::ContactPoint &point) +{ + float minDist = FLT_MAX; + float dist[6]; + float currentDist2; + std::vector<MathTools::TriangleFace6D>::iterator faceIter; + for (faceIter = convexHullGWS->faces.begin(); faceIter != convexHullGWS->faces.end(); faceIter++) + { + VirtualRobot::MathTools::ContactPoint faceCenter; + faceCenter.p.setZero(); + faceCenter.n.setZero(); + + for (int j=0;j<6;j++) + { + faceCenter.p += (convexHullGWS->vertices)[faceIter->id[j]].p; + faceCenter.n += (convexHullGWS->vertices)[faceIter->id[j]].n; + } + faceCenter.p /= 6.0f; + faceCenter.n /= 6.0f; + + currentDist2 = 0; + for (int j=0;j<3;j++) + { + dist[j] = (faceCenter.p(j)-point.p(j)); + dist[j+3] = (faceCenter.n(j)-point.n(j)); + currentDist2 += dist[j]*dist[j]; + currentDist2 += dist[j+3]*dist[j+3]; + } + if (currentDist2<minDist) + minDist = currentDist2; + + + /* + faceCenter.p(0) = (((convexHullGWS.Vertices)[(*faceIter).id1]).p(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id2]).p(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id3]).p(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id4]).p(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id5]).p(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id6]).p(0))/6.0; + faceCenter.p(1) = (((convexHullGWS.Vertices)[(*faceIter).id1]).p(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id2]).p(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id3]).p(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id4]).p(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id5]).p(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id6]).p(1))/6.0; + faceCenter.p(2) = (((convexHullGWS.Vertices)[(*faceIter).id1]).p(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id2]).p(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id3]).p(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id4]).p(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id5]).p(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id6]).p(2))/6.0; + faceCenter.n(0) = (((convexHullGWS.Vertices)[(*faceIter).id1]).n(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id2]).n(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id3]).n(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id4]).n(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id5]).n(0)+ + ((convexHullGWS.Vertices)[(*faceIter).id6]).n(0))/6.0; + faceCenter.n(1) = (((convexHullGWS.Vertices)[(*faceIter).id1]).n(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id2]).n(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id3]).n(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id4]).n(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id5]).n(1)+ + ((convexHullGWS.Vertices)[(*faceIter).id6]).n(1))/6.0; + faceCenter.n(2) = (((convexHullGWS.Vertices)[(*faceIter).id1]).n(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id2]).n(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id3]).n(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id4]).n(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id5]).n(2)+ + ((convexHullGWS.Vertices)[(*faceIter).id6]).n(2))/6.0; + + fDist1 = (faceCenter.p(0)-point.p(0)); + fDist2 = (faceCenter.p(1)-point.p(1)); + fDist3 = (faceCenter.p(2)-point.p(2)); + fDist4 = (faceCenter.n(0)-point.n(0)); + fDist5 = (faceCenter.n(1)-point.n(1)); + fDist6 = (faceCenter.n(2)-point.n(2)); + fDist1 = fDist1*fDist1 + fDist2*fDist2 + fDist3*fDist3 + fDist4*fDist4 + fDist5*fDist5 + fDist6*fDist6; + + if (fDist1<fRes) + fRes = fDist1; */ + } + return sqrtf(minDist); +} +bool GraspQualityMeasureWrenchSpace::isOriginInGWSHull() +{ + if (!GWSCalculated || !convexHullGWS) + return false; + std::vector<MathTools::TriangleFace6D>::iterator faceIter; + for (faceIter = convexHullGWS->faces.begin(); faceIter != convexHullGWS->faces.end(); faceIter++) + { + // ignore rounding errors + if (faceIter->distPlaneZero > 1e-4 ) + return false; + } + return true; +} + + + +float GraspQualityMeasureWrenchSpace::minOffset(VirtualRobot::MathTools::ConvexHull6DPtr ch) +{ + if (!ch) + return 0.0f; + float fRes = FLT_MAX; + int nWrongFacets = 0; + for (size_t i=0;i<(int)ch->faces.size();i++) + { + if (ch->faces[i].distNormCenter>0) + { + nWrongFacets++; + } else + if (-(ch->faces[i].distNormCenter) < fRes) + fRes = -(ch->faces[i].distNormCenter); + } + if (nWrongFacets>0) + { + cout << __FUNCTION__ << " Warning: offset of " << nWrongFacets << " facets >0 (# of facets:" << ch->faces.size() << ")" << endl; + } + return fRes; +} + +float GraspQualityMeasureWrenchSpace::getVolumeGraspMeasure() +{ + if (!GWSCalculated) + calculateGWS(); + + if (!convexHullGWS || convexHullGWS->vertices.size()==0) + { + cout << __FUNCTION__ << "No vertices in Grasp Wrench Space?! Maybe I was not initialized correctly..." << endl; + return 0.0; + } + if (volumeOWS<=0 || convexHullGWS->volume<=0) + { + cout << __FUNCTION__ << "No Volumes in Wrench Space?! Maybe I was not initialized correctly..." << endl; + return 0.0; + } + + float fRes = convexHullGWS->volume; + if (volumeOWS>1e-10) + fRes /= volumeOWS; + return fRes; +} + +void GraspQualityMeasureWrenchSpace::preCalculatedOWS( float fMinDist, float volume ) +{ + minOffsetOWS = fMinDist; + volumeOWS = volume; + OWSCalculated = true; +} + +bool GraspQualityMeasureWrenchSpace::calculateObjectProperties() +{ + if (!OWSCalculated) + calculateOWS(); + return true; +} + +bool GraspQualityMeasureWrenchSpace::calculateGraspQuality() +{ + if (!GWSCalculated) + calculateGWS(); + + graspQuality = 0.0f; + if (!convexHullGWS || convexHullGWS->vertices.size()==0) + { + cout << __FUNCTION__ << "No vertices in Grasp Wrench Space?! Maybe I was not initialized correctly..." << endl; + return 0.0; + } + if (minOffsetOWS<=0 || convexHullGWS->volume<=0) + { + cout << __FUNCTION__ << "No Volumes in Wrench Space?! Maybe I was not initialized correctly..." << endl; + return 0.0; + } + + float fResOffsetGWS = minOffset(convexHullGWS); + float fResOffsetOWS = minOffsetOWS; + + + if (fResOffsetOWS!=0) + graspQuality = fResOffsetGWS / fResOffsetOWS; + else + graspQuality = fResOffsetGWS; + if (verbose) + { + GRASPSTUDIO_INFO << endl; + cout << ": GWS volume : " << convexHullGWS->volume << endl; + cout << ": GWS min Offset: " << fResOffsetGWS << endl; + cout << ": OWS min Offset: " << fResOffsetOWS << endl; + cout << ": GraspQuality : " << graspQuality << endl; + } + return true; +} +/* +SoSeparator* GraspQualityMeasureWrenchSpace::GetVisualizationGWS() +{ + SoSeparator *pSep = new SoSeparator(); + if (!m_GWSCalculated) + { + GRASPSTUDIO_INFO << ": Warning no GWS calculate..." << endl; + return pSep; + } + SoSeparator *pSep2 = new SoSeparator(); + SoTranslation *pTrans = new SoTranslation(); + pTrans->translation.setValue(m_CoM.p(0),m_CoM.p(1),m_CoM.p(2)); + SoScale *pSc = new SoScale(); + pSc->scaleFactor.setValue(100.0f,100.0f,100.0f); + SoMaterial *pMat = new SoMaterial(); + pMat->transparency.setValue(0.5f); + pMat->diffuseColor.setValue(1.0f,1.0f,0.1f); + pSep->addChild(pMat); + pSep->addChild(pTrans); + pSep->addChild(pSc); + CConvexHullGenerator::CreateIVModel(convexHullGWS,pSep2,true); + pSep->addChild(pSep2); + return pSep; +} + +SoSeparator* GraspQualityMeasureWrenchSpace::GetVisualizationOWS() +{ + SoSeparator *pSep = new SoSeparator(); + if (!m_OWSCalculated) + { + GRASPSTUDIO_INFO << ": Warning no OWS calculate..." << endl; + return pSep; + } + //VirtualRobot::MathTools::ConvexHull6d gws = m_pGraspQualityMeasureWrench->getConvexHullOWS(); + //VirtualRobot::MathTools::ContactPoint pCenter = m_pGraspQualityMeasureWrench->GetSampledObjectPointsCenter(); + SoTranslation *pTransl = new SoTranslation(); + pTransl->translation.setValue(m_CoM.p(0),m_CoM.p(1),m_CoM.p(2)); + SoScale *pSc = new SoScale(); + pSc->scaleFactor.setValue(100.0f,100.0f,100.0f); + SoSeparator *pSep2 = new SoSeparator(); + SoMaterial *pMat = new SoMaterial(); + pMat->transparency.setValue(0.5f); + pMat->diffuseColor.setValue(1.0f,1.0f,0.1f); + pSep->addChild(pMat); + pSep->addChild(pTransl); + pSep->addChild(pSc); + CConvexHullGenerator::CreateIVModel(convexHullOWS,pSep2,true); + pSep->addChild(pSep2); + return pSep; +}*/ + +std::string GraspQualityMeasureWrenchSpace::getName() +{ + std::string sName("GraspWrenchSpace"); + return sName; +} + + +Eigen::Vector3f GraspQualityMeasureWrenchSpace::crossProductPosNormalInv( const VirtualRobot::MathTools::ContactPoint &v1 ) +{ + Eigen::Vector3f res; + res(0) = v1.p(1)*(-v1.n(2)) - v1.p(2)*(-v1.n(1)); + res(1) = v1.p(2)*(-v1.n(0)) - v1.p(0)*(-v1.n(2)); + res(2) = v1.p(0)*(-v1.n(1)) - v1.p(1)*(-v1.n(0)); + return res; +} + +} // namespace diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h new file mode 100644 index 0000000000000000000000000000000000000000..c4d085fabeb1efa851fa68e993d5fdd664174b1f --- /dev/null +++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h @@ -0,0 +1,150 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __GRASP_QUALTIY_MEASURE_WRENCH_H__ +#define __GRASP_QUALTIY_MEASURE_WRENCH_H__ + +#include "GraspStudio.h" +#include "GraspQualityMeasure.h" + +#include <Eigen/Core> + +namespace GraspStudio +{ + +/*! + \brief An efficient implementation of the grasp wrench space algorithm for grasp quality evaluation. + + the grasp wrench space algorithm is widely used in the context of grasp planning. By analyzing + the grasp wrench space (GWS) of a given set of contact points, a quality score of a grasp + can be evaluated. + In this implementation, additionally an object specific wrench space (WS) is calculated, which + approximatevly represents a "perfect" grasp. This object is used to normalize the quality score. +*/ +class GRASPSTUDIO_IMPORT_EXPORT GraspQualityMeasureWrenchSpace : public GraspQualityMeasure +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + GraspQualityMeasureWrenchSpace(VirtualRobot::SceneObjectPtr object, float unitForce = 1.0f, float frictionConeCoeff = 0.35f, int frictionConeSamples = 8); + // destructor + ~GraspQualityMeasureWrenchSpace(); + + + /*! + The OWS should not change for an object, so if you have the data calculated once, you can set them here + (The values for the OWS can change slightly, since the facets used for generation of the OWS are sampled randomly, + in case there are more than 400 of them) + */ + virtual void preCalculatedOWS(float minDist, float volume); + + /*! + Returns f_max_gws / f_max_OWS + with f_max_gws = max distance of GWS hull center to one of its facets + with f_max_ows = max distance of OWS hull center to one of its facets + -> also known as "epsilon" quality == radius of larges enclosing 6D ball + */ + virtual float getGraspQuality(); + + /*! + Volume grasp quality ratio of GWS volume / OWS volume + -> also known as "v" quality + */ + virtual float getVolumeGraspMeasure(); + + /* + Checks if wrench space origin is inside GWS-Hull + */ + virtual bool isGraspForceClosure(); + + /* + Returns the internally calculated convex hull object (ObjectWrenchSpace) + This hull depends on the object + */ + virtual VirtualRobot::MathTools::ConvexHull6DPtr getConvexHullOWS(){return convexHullOWS;} + /* + Returns the internally calculated convex hull object (GraspWrenchSpace) + This hull depends on the contacts + */ + virtual VirtualRobot::MathTools::ConvexHull6DPtr getConvexHullGWS(){return convexHullGWS;} + + void calculateOWS(); + void calculateGWS(); + bool OWSExists(){return OWSCalculated;} + bool GWSExists(){return GWSCalculated;} + + VirtualRobot::MathTools::ContactPoint getCenterGWS(){return convexHullCenterGWS;} + VirtualRobot::MathTools::ContactPoint getCenterOWS(){return convexHullCenterOWS;} + + /*! + setup contact information + the contact points are normalized by subtracting the COM + the contact normals are normalize to unit length + */ + virtual void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint> &contactPoints); + virtual void setContactPoints(const std::vector<VirtualRobot::EndEffector::ContactInfo> &contactPoints); + + virtual bool calculateGraspQuality(); + virtual bool calculateObjectProperties(); + + //! Returns description of this object + virtual std::string getName(); + + virtual float getOWSMinOffset(){return minOffsetOWS;} + virtual float getOWSVolume(){return volumeOWS;} + + static std::vector<VirtualRobot::MathTools::ContactPoint> createWrenchPoints(std::vector<VirtualRobot::MathTools::ContactPoint> &points, const Eigen::Vector3f ¢erOfModel, float objectLengthMM); + + //! Goes through all facets of convex hull and searches the minimum distance to it's center + static float minOffset(VirtualRobot::MathTools::ConvexHull6DPtr ch); + +protected: + + //Methods + VirtualRobot::MathTools::ConvexHull6DPtr calculateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint> &points); + VirtualRobot::MathTools::ContactPoint calculateHullCenter(VirtualRobot::MathTools::ConvexHull6DPtr hull); + + float minDistanceToGWSHull(VirtualRobot::MathTools::ContactPoint &point); + + + bool isOriginInGWSHull(); + void printContacts(std::vector<VirtualRobot::MathTools::ContactPoint> &points); + static Eigen::Vector3f crossProductPosNormalInv( const VirtualRobot::MathTools::ContactPoint &v1); + + //For safety + bool OWSCalculated; + bool GWSCalculated; + + //For Object and Grasp Wrench Space Calculation + VirtualRobot::MathTools::ConvexHull6DPtr convexHullOWS; + VirtualRobot::MathTools::ConvexHull6DPtr convexHullGWS; + VirtualRobot::MathTools::ContactPoint convexHullCenterGWS; + VirtualRobot::MathTools::ContactPoint convexHullCenterOWS; + + // the minimal distance from center of OWS to one of it's facets + float minOffsetOWS; + float volumeOWS; +}; + +} // namespace + +#endif /* __GRASP_QUALTIY_MEASURE_WRENCH_H__ */ diff --git a/GraspPlanning/GraspStudio.h b/GraspPlanning/GraspStudio.h new file mode 100644 index 0000000000000000000000000000000000000000..f89a863caa32b374bc1a51c42540a646b305dbb5 --- /dev/null +++ b/GraspPlanning/GraspStudio.h @@ -0,0 +1,112 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _GraspStudio_h_ +#define _GraspStudio_h_ + + +/*! \defgroup GraspStudio The Grasp Planning Library +GraspStudio offers algorithms for grasp scoring and planning covering 3D force space calculations and a full +implementation of the 6d wrench space algorithm for grasp quality measurement. +*/ + +#ifdef WIN32 + +// needed to have M_PI etc defined +#if !defined(_USE_MATH_DEFINES) +#define _USE_MATH_DEFINES +#endif + +// eigen wants this on windows +#if !defined(NOMINMAX) +#define NOMINMAX +#endif + +#endif + +#include "VirtualRobot/VirtualRobot.h" +#include "VirtualRobot/VirtualRobotException.h" +#include <boost/shared_ptr.hpp> +#include <boost/weak_ptr.hpp> +#include <iostream> +#include <sstream> +#include <cmath> +#include <Eigen/Core> +#include <Eigen/Geometry> + +#ifdef WIN32 +# include <winsock2.h> +# include <windows.h> +# pragma warning ( disable : 4251 ) +# if defined(GraspStudio_EXPORTS) +# define GRASPSTUDIO_IMPORT_EXPORT __declspec(dllexport) +# else +# define GRASPSTUDIO_IMPORT_EXPORT __declspec(dllimport) +# endif +#else +# define GRASPSTUDIO_IMPORT_EXPORT +#endif + + +namespace GraspStudio +{ + // only valid within the GraspStudio namespace + using std::cout; + using std::endl; + + class GraspQualityMeasure; + class GraspQualityMeasureWrenchSpace; + class ContactConeGenerator; + + class GraspQualityMeasure; + class ApproachMovementGenerator; + class ApproachMovementSurfaceNormal; + class GraspPlanner; + class GenericGraspPlanner; + + typedef boost::shared_ptr<GraspQualityMeasure> GraspQualityMeasurePtr; + typedef boost::shared_ptr<GraspQualityMeasureWrenchSpace> GraspQualityMeasureWrenchSpacePtr; + typedef boost::shared_ptr<ContactConeGenerator> ContactConeGeneratorPtr; + typedef boost::shared_ptr<GraspQualityMeasure> GraspQualityMeasurePtr; + typedef boost::shared_ptr<ApproachMovementGenerator> ApproachMovementGeneratorPtr; + typedef boost::shared_ptr<ApproachMovementSurfaceNormal> ApproachMovementSurfaceNormalPtr; + typedef boost::shared_ptr<GraspPlanner> GraspPlannerPtr; + typedef boost::shared_ptr<GenericGraspPlanner> GenericGraspPlannerPtr; + +#define GRASPSTUDIO_INFO VR_INFO +#define GRASPSTUDIO_WARNING VR_WARNING +#define GRASPSTUDIO_ERROR VR_ERROR + +#define THROW_GRASPSTUDIO_EXCEPTION(a) THROW_VR_EXCEPTION(a) + +#ifdef _DEBUG +#define GRASPSTUDIO_ASSERT(a) if (!(a)) {cout << "ASSERT failed (" << #a <<")"<<endl; THROW_GRASPSTUDIO_EXCEPTION( "ASSERT failed (" << #a << ")" )}; +#define GRASPSTUDIO_ASSERT_MESSAGE(a,b) if (!(a)) {cout << "ASSERT failed (" << #a <<"): "<<b<<endl; THROW_GRASPSTUDIO_EXCEPTION( "ASSERT failed (" << #a << "): " << b )}; + +#else +#define GRASPSTUDIO_ASSERT(a) +#define GRASPSTUDIO_ASSERT_MESSAGE(a,b) +#endif + +} + +#endif // _GraspStudio_h_ diff --git a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ee7c0e0c2e1263bc5e47dff8eb0e8dc6ee6212ca --- /dev/null +++ b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp @@ -0,0 +1,325 @@ + +#include "CoinConvexHullVisualization.h" +#include "../../ConvexHullGenerator.h" + +#include <Inventor/SoPrimitiveVertex.h> +#include <Inventor/SbLinear.h> +#include <Inventor/nodes/SoShape.h> +#include <Inventor/actions/SoCallbackAction.h> +#include <Inventor/nodes/SoSeparator.h> +#include <Inventor/nodes/SoMatrixTransform.h> +#include <Inventor/nodes/SoScale.h> + + +#include <Inventor/nodes/SoComplexity.h> +#include <Inventor/nodes/SoMaterial.h> +#include <Inventor/nodes/SoDrawStyle.h> +#include <Inventor/nodes/SoLineSet.h> +#include <Inventor/nodes/SoFaceSet.h> + +#include <Inventor/nodes/SoCoordinate3.h> +#include <Inventor/nodes/SoSphere.h> +#include <Inventor/nodes/SoBaseColor.h> +#include <Inventor/nodes/SoTranslation.h> +#include <Inventor/nodes/SoUnits.h> + + +namespace GraspStudio { + +CoinConvexHullVisualization::CoinConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr convHull, bool useFirst3Coords) : +ConvexHullVisualization(convHull,useFirst3Coords) +{ + buildVisu(); +} + +CoinConvexHullVisualization::CoinConvexHullVisualization(VirtualRobot::MathTools::ConvexHull3DPtr convHull) : +ConvexHullVisualization(convHull) +{ + buildVisu(); +} + +void CoinConvexHullVisualization::buildVisu() +{ + visualization = new SoSeparator(); + visualization->ref(); + if (convHull3D) + { + SoSeparator *vi = createConvexHullVisualization(convHull3D); + if (vi) + visualization->addChild(vi); + + } + if (convHull6D) + { + SoSeparator *vi = createConvexHullVisualization(convHull6D, useFirst3Coords); + if (vi) + visualization->addChild(vi); + } +} + +/** + * If CoinConvexHullVisualization::visualization is a valid object call SoNode::unref() + * on it. + */ +CoinConvexHullVisualization::~CoinConvexHullVisualization() +{ + if (visualization) + visualization->unref(); + +} + + + +/** + * This mehtod returns the internal CoinConvexHullVisualization::visualization. + */ +SoSeparator* CoinConvexHullVisualization::getCoinVisualization() +{ + return visualization; +} + + +SoSeparator* CoinConvexHullVisualization::createConvexHullVisualization(VirtualRobot::MathTools::ConvexHull3DPtr &convHull) +{ + SoSeparator* result = new SoSeparator; + if (!convHull || convHull->vertices.size()==0 || convHull->faces.size()==0) + return result; + result->ref(); + + SoCoordinate3* pCoords = new SoCoordinate3(); + SoFaceSet* pFaceSet = new SoFaceSet(); + + int nFaces = (int)convHull->faces.size(); + int nVertices = nFaces*3; + //Face3D f; + //Vec3D v1,v2,v3; + + Eigen::Vector3f v1,v2,v3; + SbVec3f *pVertexArray = new SbVec3f[nVertices]; + + int nVertexCount = 0; + + for (int i=0;i<nFaces;i++) + { + v1 = convHull->vertices.at(convHull->faces[i].id1); + v2 = convHull->vertices.at(convHull->faces[i].id2); + v3 = convHull->vertices.at(convHull->faces[i].id3); + + bool bNeedFlip = ConvexHullGenerator::checkVerticeOrientation(v1,v2,v3,convHull->faces[i].normal); + + // COUNTER CLOCKWISE + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v3(0),(float)v3(1),(float)v3(2)); + else + pVertexArray[nVertexCount].setValue((float)v1(0),(float)v1(1),(float)v1(2)); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v2(0),(float)v2(1),(float)v2(2)); + nVertexCount++; + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v1(0),(float)v1(1),(float)v1(2)); + else + pVertexArray[nVertexCount].setValue((float)v3(0),(float)v3(1),(float)v3(2)); + nVertexCount++; + + } + pCoords->point.setValues(0,nVertices,pVertexArray); + long *nNumVertices = new long[nFaces]; + for (int i=0;i<nFaces;i++) + nNumVertices[i] = 3; + pFaceSet->numVertices.setValues(0,nFaces,(const int32_t*)nNumVertices); + + result->addChild(pCoords); + result->addChild(pFaceSet); + delete []pVertexArray; + delete []nNumVertices; + result->unrefNoDelete(); + + return result; +} + +/* +void addVertex(Vec3D &v1,Vec3D &v2,Vec3D &v3,Vec3D &normal,SbVec3f *pVertexArray, int& nVertexCount) +{ + bool bNeedFlip = GraspStudioHelpers::checkVerticeOrientation(v1,v2,v3,normal); + + // COUNTER CLOCKWISE + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + else + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + nVertexCount++; + + pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z); + nVertexCount++; + + if (bNeedFlip) + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + else + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + nVertexCount++; +}*/ + + +SoSeparator* CoinConvexHullVisualization::createConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr &convHull, bool buseFirst3Coords) +{ + SoSeparator* result = new SoSeparator; + if (!convHull || convHull->vertices.size()==0 || convHull->faces.size()==0) + return result; + result->ref(); + SoScale *sc = new SoScale(); + float fc = 400.0f; + sc->scaleFactor.setValue(fc,fc,fc); + result->addChild(sc); + + + int nFaces = (int)convHull->faces.size(); + + + //Eigen::Vector3f v[6]; + // project points to 3d, then create hull of these points to visualize it + std::vector<Eigen::Vector3f> vProjectedPoints; + for (int i=0;i<nFaces;i++) + { + for (int j=0;j<6;j++) + { + if (buseFirst3Coords) + { + //v[j] = convHull->vertices.at(convHull->faces[i].id[j]).p; + vProjectedPoints.push_back( convHull->vertices[ convHull->faces[i].id[j] ].p ); + } else + { + //v[j] = convHull->vertices.at(convHull->faces[i].id[j]).n; + vProjectedPoints.push_back( convHull->vertices[ convHull->faces[i].id[j] ].n ); + } + + } + } + VirtualRobot::MathTools::ConvexHull3DPtr projectedHull = ConvexHullGenerator::CreateConvexHull(vProjectedPoints); + if (!projectedHull) + { + GRASPSTUDIO_ERROR << " Could not create hull of projected points, aborting..." << endl; + return result; + } + SoSeparator *hullV = createConvexHullVisualization(projectedHull); + if (!hullV) + { + GRASPSTUDIO_ERROR << " Could not create visualization of projected points, aborting..." << endl; + return result; + } + result->addChild(hullV); + result->unrefNoDelete(); + return result; + /* + // creates 3d-projection of all 6d facets + int nVertices = nFaces*12; + SoCoordinate3* pCoords = new SoCoordinate3(); + SoFaceSet* pFaceSet = new SoFaceSet(); + Face6d f; + Vec3d v1,v2,v3,v4,v5,v6; + Vec3d normal; + + SbVec3f *pVertexArray = new SbVec3f[nVertices]; + + int nVertexCount = 0; + bool bNeedFlip = false; + + for (int i=0;i<nFaces;i++) + { + f = convHull.faces.at(i); + if (buseFirst3Coords) + { + v1.x = convHull.vertices.at(f.id[0]).x; + v1.y = convHull.vertices.at(f.id[0]).y; + v1.z = convHull.vertices.at(f.id[0]).z; + v2.x = convHull.vertices.at(f.id[1]).x; + v2.y = convHull.vertices.at(f.id[1]).y; + v2.z = convHull.vertices.at(f.id[1]).z; + v3.x = convHull.vertices.at(f.id[2]).x; + v3.y = convHull.vertices.at(f.id[2]).y; + v3.z = convHull.vertices.at(f.id[2]).z; + v4.x = convHull.vertices.at(f.id[3]).x; + v4.y = convHull.vertices.at(f.id[3]).y; + v4.z = convHull.vertices.at(f.id[3]).z; + v5.x = convHull.vertices.at(f.id[4]).x; + v5.y = convHull.vertices.at(f.id[4]).y; + v5.z = convHull.vertices.at(f.id[4]).z; + v6.x = convHull.vertices.at(f.id[5]).x; + v6.y = convHull.vertices.at(f.id[5]).y; + v6.z = convHull.vertices.at(f.id[5]).z; + normal.x = f.normal.x; + normal.y = f.normal.y; + normal.z = f.normal.z; + } else + { + v1.x = convHull.vertices.at(f.id[0]).nx; + v1.y = convHull.vertices.at(f.id[0]).ny; + v1.z = convHull.vertices.at(f.id[0]).nz; + v2.x = convHull.vertices.at(f.id[1]).nx; + v2.y = convHull.vertices.at(f.id[1]).ny; + v2.z = convHull.vertices.at(f.id[1]).nz; + v3.x = convHull.vertices.at(f.id[2]).nx; + v3.y = convHull.vertices.at(f.id[2]).ny; + v3.z = convHull.vertices.at(f.id[2]).nz; + v4.x = convHull.vertices.at(f.id[3]).nx; + v4.y = convHull.vertices.at(f.id[3]).ny; + v4.z = convHull.vertices.at(f.id[3]).nz; + v5.x = convHull.vertices.at(f.id[4]).nx; + v5.y = convHull.vertices.at(f.id[4]).ny; + v5.z = convHull.vertices.at(f.id[4]).nz; + v6.x = convHull.vertices.at(f.id[5]).nx; + v6.y = convHull.vertices.at(f.id[5]).ny; + v6.z = convHull.vertices.at(f.id[5]).nz; + normal.x = f.normal.nx; + normal.y = f.normal.ny; + normal.z = f.normal.nz; + } + bool bNeedFlip = GraspStudioHelpers::checkVerticeOrientation(v1,v2,v3,normal); + if (bNeedFlip) + { + pVertexArray[nVertexCount].setValue((float)v6.x,(float)v6.y,(float)v6.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v5.x,(float)v5.y,(float)v5.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v4.x,(float)v4.y,(float)v4.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + nVertexCount++; + } else + { + pVertexArray[nVertexCount].setValue((float)v1.x,(float)v1.y,(float)v1.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v2.x,(float)v2.y,(float)v2.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v3.x,(float)v3.y,(float)v3.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v4.x,(float)v4.y,(float)v4.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v5.x,(float)v5.y,(float)v5.z); + nVertexCount++; + pVertexArray[nVertexCount].setValue((float)v6.x,(float)v6.y,(float)v6.z); + nVertexCount++; + } + } + pCoords->point.setValues(0,nVertices,pVertexArray); + long *nNumVertices = new long[nFaces]; + for (int i=0;i<nFaces;i++) + nNumVertices[i] = 6; + pFaceSet->numVertices.setValues(0,nFaces,(const int32_t*)nNumVertices); + + pStoreResult->addChild(pCoords); + pStoreResult->addChild(pFaceSet); + delete []pVertexArray; + delete []nNumVertices; + + qhull_mutex.unlock(); + + return true; + */ +} + + +} // namespace Saba diff --git a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h new file mode 100644 index 0000000000000000000000000000000000000000..c8049cb548ea316a89a088f10212b4d419acd9fd --- /dev/null +++ b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h @@ -0,0 +1,74 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_CoinConvexHullVisualization_h_ +#define _Saba_CoinConvexHullVisualization_h_ + + +#include "../../GraspStudio.h" +#include "../ConvexHullVisualization.h" + +#include <boost/shared_ptr.hpp> + +class SoNode; +class SoSeparator; +class SoCallbackAction; +class SoPrimitiveVertex; + +namespace GraspStudio +{ +/*! +* +* \class CoinConvexHullVisualization +* +* A Coin3D related visualization of a convex hull +*/ +class GRASPSTUDIO_IMPORT_EXPORT CoinConvexHullVisualization : virtual public ConvexHullVisualization +{ +public: + /*! + Constructor + */ + CoinConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr convHull, bool useFirst3Coords = true); + CoinConvexHullVisualization(VirtualRobot::MathTools::ConvexHull3DPtr convHull); + + ~CoinConvexHullVisualization(); + + SoSeparator* getCoinVisualization(); + +protected: + + void buildVisu(); + SoSeparator* createConvexHullVisualization(VirtualRobot::MathTools::ConvexHull3DPtr &convHull); + SoSeparator* createConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr &convHull, bool buseFirst3Coords); + + + SoSeparator* visualization; + +}; + + typedef boost::shared_ptr<CoinConvexHullVisualization> CoinConvexHullVisualizationPtr; + + +} // namespace Saba + +#endif // _Saba_CoinConvexHullVisualization_h_ diff --git a/GraspPlanning/Visualization/ConvexHullVisualization.cpp b/GraspPlanning/Visualization/ConvexHullVisualization.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4a5d0ee3acba2c8d22436a2bae8c723df9df1f1f --- /dev/null +++ b/GraspPlanning/Visualization/ConvexHullVisualization.cpp @@ -0,0 +1,25 @@ + +#include "ConvexHullVisualization.h" + + +namespace GraspStudio { + +ConvexHullVisualization::ConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr convHull, bool useFirst3Coords) +{ + this->convHull6D = convHull; + this->useFirst3Coords = useFirst3Coords; + +} + +ConvexHullVisualization::ConvexHullVisualization(VirtualRobot::MathTools::ConvexHull3DPtr convHull) +{ + this->convHull3D = convHull; + this->useFirst3Coords = true; +} + + +ConvexHullVisualization::~ConvexHullVisualization() +{ +} + +} // namespace GraspStudio diff --git a/GraspPlanning/Visualization/ConvexHullVisualization.h b/GraspPlanning/Visualization/ConvexHullVisualization.h new file mode 100644 index 0000000000000000000000000000000000000000..deeb142b145f6b26da05467e2bc90e962ad6f3e9 --- /dev/null +++ b/GraspPlanning/Visualization/ConvexHullVisualization.h @@ -0,0 +1,65 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _GraspStudio_ConvexHullVisualization_h_ +#define _GraspStudio_ConvexHullVisualization_h_ + +#include "../GraspStudio.h" +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/MathTools.h> +#include <Eigen/Core> +#include <Eigen/Geometry> + +namespace GraspStudio +{ +/*! + * + * \brief A visualization of a convex hull + * @see CoinConvexHullVisualization + * + */ +class GRASPSTUDIO_IMPORT_EXPORT ConvexHullVisualization +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + Constructor + */ + ConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr convHull, bool useFirst3Coords = true); + ConvexHullVisualization(VirtualRobot::MathTools::ConvexHull3DPtr convHull); + + /*! + */ + virtual ~ConvexHullVisualization(); + +protected: + + VirtualRobot::MathTools::ConvexHull3DPtr convHull3D; + VirtualRobot::MathTools::ConvexHull6DPtr convHull6D; + bool useFirst3Coords; + +}; + +} // namespace GraspStudio + +#endif // _GraspStudio_ConvexHullVisualization_h_ diff --git a/GraspPlanning/config.cmake b/GraspPlanning/config.cmake new file mode 100644 index 0000000000000000000000000000000000000000..83de9626d7d494bb5aefdd9b622d5a768cd5cc79 --- /dev/null +++ b/GraspPlanning/config.cmake @@ -0,0 +1,50 @@ +GET_FILENAME_COMPONENT (CurrentGSPath ${CMAKE_CURRENT_LIST_FILE} PATH) +SET(GRASPSTUDIO_DIR ${CurrentGSPath}) + +############################# SETUP PATHS TO Simox ############################# +SET(SIMOX_DIR_STANDARD "${CurrentGSPath}/..") +# be sure to have the absolute path +get_filename_component(SIMOX_DIR_STANDARD ${SIMOX_DIR_STANDARD} ABSOLUTE) + +SET (GRASPSTUDIO_SimoxDir ${SIMOX_DIR_STANDARD} CACHE STRING "Path to Simox used by GraspStudio") + +INCLUDE(${GRASPSTUDIO_SimoxDir}/config.cmake) +INCLUDE(${GRASPSTUDIO_SimoxDir}/SimoxProject.cmake) + +IF(NOT DEFINED SIMOX_BUILD_DIRECTORY) + get_filename_component(SIMOX_BUILD_DIRECTORY ${SIMOX_BUILD_DIRECTORY} ABSOLUTE) + SET(SIMOX_BUILD_DIRECTORY "${GRASPSTUDIO_SimoxDir}/build" CACHE STRING "Simox build directory used by GraspStudio") + SET(SIMOX_LIB_DIR ${SIMOX_BUILD_DIRECTORY}/lib) + SET(SIMOX_BIN_DIR ${SIMOX_BUILD_DIRECTORY}/bin) +ENDIF() + +############################# SETUP PATHS ############################# +ADD_DEFINITIONS(-DGRASPSTUDIO_BASE_DIR="${GRASPSTUDIO_DIR}") + +# Define, where to put the binaries +SET(GRASPSTUDIO_LIB_DIR ${SIMOX_LIB_DIR}) +SET(GRASPSTUDIO_BIN_DIR ${SIMOX_BIN_DIR}) +MESSAGE(STATUS "** GRASPSTUDIO_LIB_DIR: ${GRASPSTUDIO_LIB_DIR}") +MESSAGE(STATUS "** GRASPSTUDIO_BIN_DIR: ${GRASPSTUDIO_BIN_DIR}") + + +# Define, where to install the binaries +# Define, where to install the binaries +SET(GRASPSTUDIO_INSTALL_LIB_DIR ${SIMOX_INSTALL_LIB_DIR}) +SET(GRASPSTUDIO_INSTALL_BIN_DIR ${SIMOX_INSTALL_BIN_DIR}) +SET(GRASPSTUDIO_INSTALL_HEADER_DIR ${SIMOX_INSTALL_HEADER_DIR}) + +####################################################################### +# Setup for testing +####################################################################### +ENABLE_TESTING() +INCLUDE(CTest) + +MACRO(ADD_GRASPSTUDIO_TEST TEST_NAME) + ADD_EXECUTABLE(${TEST_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_NAME}.cpp) + TARGET_LINK_LIBRARIES(${TEST_NAME} VirtualRobot Saba GraspStudio ${VIRTUAL_ROBOT_LINK_LIBRARIES}) + IF(NOT UNIX) + SET_TARGET_PROPERTIES(${TEST_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${SIMOX_BIN_DIR}) + ENDIF(NOT UNIX) + ADD_TEST( GraspStudio_${TEST_NAME} ${TEST_NAME} --output_format=XML --log_level=all --report_level=no) +ENDMACRO(ADD_GRASPSTUDIO_TEST) diff --git a/GraspPlanning/examples/CMakeLists.txt b/GraspPlanning/examples/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..fecde0b4b7b9ff6c7613ba7aa4dcf19b0ce1dcf5 --- /dev/null +++ b/GraspPlanning/examples/CMakeLists.txt @@ -0,0 +1,5 @@ + +if (SOQT_FOUND) + ADD_SUBDIRECTORY(GraspQuality) + ADD_SUBDIRECTORY(GraspPlanner) +endif() diff --git a/GraspPlanning/examples/GraspPlanner/CMakeLists.txt b/GraspPlanning/examples/GraspPlanner/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..d54102fa3050a4b5d05276a29661a020540ab08b --- /dev/null +++ b/GraspPlanning/examples/GraspPlanner/CMakeLists.txt @@ -0,0 +1,30 @@ +PROJECT ( GraspPlanner ) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4) + +CMAKE_POLICY(VERSION 2.6) + +IF(NOT DEFINED SIMOX_CONFIGURED) + INCLUDE(../../config.cmake) +ENDIF() + +IF(SIMOX_USE_COIN_VISUALIZATION) + + + # the variable "demo_SRCS" contains all .cpp files of this project + FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/GraspPlanner.cpp ${PROJECT_SOURCE_DIR}/GraspPlannerWindow.cpp) + FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/GraspPlannerWindow.h) + + set(GUI_MOC_HDRS + ${PROJECT_SOURCE_DIR}/GraspPlannerWindow.h + ) + set(GUI_UIS + ${PROJECT_SOURCE_DIR}/GraspPlanner.ui + ) + + # create the executable + simox_add_executable(${PROJECT_NAME} "${demo_SRCS}" "${demo_INCS}" "${GUI_MOC_HDRS}" "${GUI_UIS}") + + # we need the GraspStudio lib + TARGET_LINK_LIBRARIES(${PROJECT_NAME} GraspStudio) +ENDIF(SIMOX_USE_COIN_VISUALIZATION) diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlanner.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlanner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eaffbaedfc6ee707250cd269d394263b39cbfbc2 --- /dev/null +++ b/GraspPlanning/examples/GraspPlanner/GraspPlanner.cpp @@ -0,0 +1,62 @@ +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/RuntimeEnvironment.h> + +#include <string> +#include <iostream> + +using std::cout; +using std::endl; +using namespace VirtualRobot; + +#include <Eigen/Core> +#include <Eigen/Geometry> + +#include "GraspPlannerWindow.h" + + +int main(int argc, char *argv[]) +{ + SoDB::init(); + SoQt::init(argc,argv,"GraspPlanner"); + cout << " --- START --- " << endl; + + std::string robot(SIMOX_BASE_DIR "/VirtualRobot/data/robots/ArmarIII/ArmarIII.xml"); + std::string eef("Hand L"); + //std::string object(SIMOX_BASE_DIR "/VirtualRobot/data/objects/wok.xml"); + std::string object(SIMOX_BASE_DIR "/VirtualRobot/data/objects/riceBox.xml"); + std::string preshape(""); + + VirtualRobot::RuntimeEnvironment::considerKey("robot"); + VirtualRobot::RuntimeEnvironment::considerKey("object"); + VirtualRobot::RuntimeEnvironment::considerKey("endeffector"); + VirtualRobot::RuntimeEnvironment::considerKey("preshape"); + VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv); + VirtualRobot::RuntimeEnvironment::print(); + + std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot"); + if (!robFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile)) + robot = robFile; + + std::string objFile = VirtualRobot::RuntimeEnvironment::getValue("object"); + if (!objFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(objFile)) + object = objFile; + + std::string eefname = VirtualRobot::RuntimeEnvironment::getValue("endeffector"); + if (!eefname.empty()) + eef = eefname; + + std::string ps = VirtualRobot::RuntimeEnvironment::getValue("preshape"); + if (!ps.empty()) + preshape = ps; + + + cout << "Using robot from " << robot << endl; + cout << "End effector:" << eef << ", preshape:" << preshape << endl; + cout << "Using object from " << object << endl; + + GraspPlannerWindow rw(robot,eef,preshape,object); + + rw.main(); + + return 0; +} diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlanner.ui b/GraspPlanning/examples/GraspPlanner/GraspPlanner.ui new file mode 100644 index 0000000000000000000000000000000000000000..1ea8291630aa25282258490678a8d51e84219939 --- /dev/null +++ b/GraspPlanning/examples/GraspPlanner/GraspPlanner.ui @@ -0,0 +1,322 @@ +<?xml version="1.0" encoding="UTF-8"?> +<ui version="4.0"> + <class>GraspPlanner</class> + <widget class="QMainWindow" name="GraspPlanner"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1079</width> + <height>819</height> + </rect> + </property> + <property name="windowTitle"> + <string>GraspStudio - GraspPlanner</string> + </property> + <widget class="QWidget" name="centralwidget"> + <layout class="QGridLayout" name="gridLayout"> + <item row="0" column="0"> + <widget class="QFrame" name="frameViewer"> + <property name="frameShape"> + <enum>QFrame::StyledPanel</enum> + </property> + <property name="frameShadow"> + <enum>QFrame::Raised</enum> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QGroupBox" name="groupBox"> + <property name="maximumSize"> + <size> + <width>200</width> + <height>16777215</height> + </size> + </property> + <property name="title"> + <string/> + </property> + <widget class="QPushButton" name="pushButtonReset"> + <property name="geometry"> + <rect> + <x>20</x> + <y>20</y> + <width>171</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Reset</string> + </property> + </widget> + <widget class="QLabel" name="label_10"> + <property name="geometry"> + <rect> + <x>10</x> + <y>450</y> + <width>81</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Visualization</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxColModel"> + <property name="geometry"> + <rect> + <x>20</x> + <y>480</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Collision Model</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxCones"> + <property name="geometry"> + <rect> + <x>20</x> + <y>500</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Friction Cones</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxGrasps"> + <property name="geometry"> + <rect> + <x>20</x> + <y>520</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show grasps</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonPlan"> + <property name="geometry"> + <rect> + <x>30</x> + <y>290</y> + <width>141</width> + <height>31</height> + </rect> + </property> + <property name="text"> + <string>Plan grasp(s)</string> + </property> + </widget> + <widget class="QGroupBox" name="groupBox_2"> + <property name="geometry"> + <rect> + <x>10</x> + <y>70</y> + <width>181</width> + <height>201</height> + </rect> + </property> + <property name="title"> + <string>Grasp Planning Options</string> + </property> + <widget class="QCheckBox" name="checkBoxFoceClosure"> + <property name="geometry"> + <rect> + <x>30</x> + <y>170</y> + <width>111</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Force closure</string> + </property> + </widget> + <widget class="QSpinBox" name="spinBoxGraspNumber"> + <property name="geometry"> + <rect> + <x>30</x> + <y>40</y> + <width>111</width> + <height>22</height> + </rect> + </property> + <property name="minimum"> + <number>1</number> + </property> + <property name="maximum"> + <number>10000000</number> + </property> + </widget> + <widget class="QLabel" name="label_11"> + <property name="geometry"> + <rect> + <x>10</x> + <y>20</y> + <width>121</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Number of Grasps</string> + </property> + </widget> + <widget class="QLabel" name="label_12"> + <property name="geometry"> + <rect> + <x>10</x> + <y>70</y> + <width>171</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Timeout, seconds (0=endless)</string> + </property> + </widget> + <widget class="QSpinBox" name="spinBoxTimeOut"> + <property name="geometry"> + <rect> + <x>30</x> + <y>90</y> + <width>111</width> + <height>22</height> + </rect> + </property> + <property name="minimum"> + <number>0</number> + </property> + <property name="maximum"> + <number>10000000</number> + </property> + <property name="value"> + <number>30</number> + </property> + </widget> + <widget class="QLabel" name="label_13"> + <property name="geometry"> + <rect> + <x>10</x> + <y>120</y> + <width>171</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Min quality</string> + </property> + </widget> + <widget class="QDoubleSpinBox" name="doubleSpinBoxQuality"> + <property name="geometry"> + <rect> + <x>30</x> + <y>140</y> + <width>111</width> + <height>22</height> + </rect> + </property> + <property name="decimals"> + <number>3</number> + </property> + <property name="maximum"> + <double>1.000000000000000</double> + </property> + <property name="singleStep"> + <double>0.001000000000000</double> + </property> + </widget> + </widget> + <widget class="QPushButton" name="pushButtonSave"> + <property name="geometry"> + <rect> + <x>30</x> + <y>400</y> + <width>141</width> + <height>31</height> + </rect> + </property> + <property name="text"> + <string>Save</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonClose"> + <property name="geometry"> + <rect> + <x>30</x> + <y>340</y> + <width>71</width> + <height>31</height> + </rect> + </property> + <property name="text"> + <string>Close</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonOpen"> + <property name="geometry"> + <rect> + <x>110</x> + <y>340</y> + <width>71</width> + <height>31</height> + </rect> + </property> + <property name="text"> + <string>Open</string> + </property> + </widget> + <widget class="QLabel" name="labelInfo"> + <property name="geometry"> + <rect> + <x>20</x> + <y>620</y> + <width>161</width> + <height>111</height> + </rect> + </property> + <property name="text"> + <string>Grasps: 0</string> + </property> + <property name="alignment"> + <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxHighlight"> + <property name="geometry"> + <rect> + <x>20</x> + <y>540</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Highlight</string> + </property> + </widget> + </widget> + </item> + </layout> + </widget> + <widget class="QMenuBar" name="menubar"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1079</width> + <height>21</height> + </rect> + </property> + </widget> + <widget class="QStatusBar" name="statusbar"/> + </widget> + <resources/> + <connections/> +</ui> diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp new file mode 100644 index 0000000000000000000000000000000000000000..35b37fab329c11266d3d2df1cab84b5601781c04 --- /dev/null +++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp @@ -0,0 +1,411 @@ + +#include "GraspPlannerWindow.h" +#include "GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h" +#include "VirtualRobot/EndEffector/EndEffector.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/ManipulationObject.h" +#include "VirtualRobot/Grasp.h" +#include "VirtualRobot/IK/GenericIKSolver.h" +#include "VirtualRobot/GraspSet.h" +#include "VirtualRobot/CollisionDetection/CDManager.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/XML/ObjectIO.h" +#include "VirtualRobot/XML/RobotIO.h" +#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h" +#include "VirtualRobot/Visualization/TriMeshModel.h" +#include <QFileDialog> +#include <Eigen/Geometry> + +#include <time.h> +#include <vector> +#include <iostream> +#include <cmath> + + + +#include "Inventor/actions/SoLineHighlightRenderAction.h" +#include <Inventor/nodes/SoShapeHints.h> +#include <Inventor/nodes/SoLightModel.h> +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/nodes/SoMatrixTransform.h> +#include <Inventor/nodes/SoScale.h> + +#include <sstream> +using namespace std; +using namespace VirtualRobot; + +float TIMER_MS = 30.0f; + +GraspPlannerWindow::GraspPlannerWindow(std::string &robFile, std::string &eefName, std::string &preshape, std::string &objFile, Qt::WFlags flags) +:QMainWindow(NULL) +{ + VR_INFO << " start " << endl; + + this->robotFile = robFile; + this->objectFile = objFile; + this->eefName = eefName; + this->preshape = preshape; + eefVisu = NULL; + + sceneSep = new SoSeparator; + sceneSep->ref(); + robotSep = new SoSeparator; + objectSep = new SoSeparator; + frictionConeSep = new SoSeparator; + graspsSep = new SoSeparator; + graspsSep->ref(); + + sceneSep->addChild(robotSep); + sceneSep->addChild(objectSep); + sceneSep->addChild(frictionConeSep); + //sceneSep->addChild(graspsSep); + + setupUI(); + + + loadRobot(); + loadObject(); + + buildVisu(); + + + + m_pExViewer->viewAll(); + + /*SoSensorManager *sensor_mgr = SoDB::getSensorManager(); + SoTimerSensor *timer = new SoTimerSensor(timerCB, this); + timer->setInterval(SbTime(TIMER_MS/1000.0f)); + sensor_mgr->insertTimerSensor(timer);*/ +} + + +GraspPlannerWindow::~GraspPlannerWindow() +{ + sceneSep->unref(); + graspsSep->unref(); + if (eefVisu) + eefVisu->unref(); +} + + +/*void GraspPlannerWindow::timerCB(void * data, SoSensor * sensor) +{ + GraspPlannerWindow *ikWindow = static_cast<GraspPlannerWindow*>(data); + float x[6]; + x[0] = (float)ikWindow->UI.horizontalSliderX->value(); + x[1] = (float)ikWindow->UI.horizontalSliderY->value(); + x[2] = (float)ikWindow->UI.horizontalSliderZ->value(); + x[3]= (float)ikWindow->UI.horizontalSliderRo->value(); + x[4] = (float)ikWindow->UI.horizontalSliderPi->value(); + x[5] = (float)ikWindow->UI.horizontalSliderYa->value(); + x[0] /= 10.0f; + x[1] /= 10.0f; + x[2] /= 10.0f; + x[3] /= 300.0f; + x[4] /= 300.0f; + x[5] /= 300.0f; + + if (x[0]!=0 || x[1]!=0 || x[2]!=0 || x[3]!=0 || x[4]!=0 || x[5]!=0) + ikWindow->updateObject(x); +}*/ + + +void GraspPlannerWindow::setupUI() +{ + UI.setupUi(this); + m_pExViewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP); + + // setup + m_pExViewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f)); + m_pExViewer->setAccumulationBuffer(true); +#ifdef WIN32 +#ifndef _DEBUG + m_pExViewer->setAntialiasing(true, 4); +#endif +#endif + m_pExViewer->setGLRenderAction(new SoLineHighlightRenderAction); + m_pExViewer->setTransparencyType(SoGLRenderAction::BLEND); + m_pExViewer->setFeedbackVisibility(true); + m_pExViewer->setSceneGraph(sceneSep); + m_pExViewer->viewAll(); + + connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll())); + connect(UI.pushButtonPlan, SIGNAL(clicked()), this, SLOT(plan())); + connect(UI.pushButtonSave, SIGNAL(clicked()), this, SLOT(save())); + connect(UI.pushButtonOpen, SIGNAL(clicked()), this, SLOT(openEEF())); + connect(UI.pushButtonClose, SIGNAL(clicked()), this, SLOT(closeEEF())); + + + + connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(colModel())); + connect(UI.checkBoxCones, SIGNAL(clicked()), this, SLOT(frictionConeVisu())); + connect(UI.checkBoxGrasps, SIGNAL(clicked()), this, SLOT(showGrasps())); +} + + +void GraspPlannerWindow::resetSceneryAll() +{ + grasps->removeAllGrasps(); + graspsSep->removeAllChildren(); + + //if (rns) + // rns->setJointValues(startConfig); +} + + +void GraspPlannerWindow::closeEvent(QCloseEvent *event) +{ + quit(); + QMainWindow::closeEvent(event); +} + + +void GraspPlannerWindow::buildVisu() +{ + + robotSep->removeAllChildren(); + SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked())?SceneObject::Collision:SceneObject::Full; + + if (eefCloned) + { + visualizationRobot = eefCloned->getVisualization<CoinVisualization>(colModel); + SoNode* visualisationNode = visualizationRobot->getCoinVisualization(); + if (visualisationNode) + { + robotSep->addChild(visualisationNode); + visualizationRobot->highlight(UI.checkBoxHighlight->isChecked()); + } + } + /* + if (robot) + { + visualizationRobot = robot->getVisualization<CoinVisualization>(colModel); + SoNode* visualisationNode = visualizationRobot->getCoinVisualization(); + if (visualisationNode) + { + robotSep->addChild(visualisationNode); + //visualizationRobot->highlight(true); + } + } + */ + objectSep->removeAllChildren(); + if (object) + { + SceneObject::VisualizationType colModel2 = (UI.checkBoxColModel->isChecked())?SceneObject::CollisionData:SceneObject::Full; + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(object,colModel2); + if (visualisationNode) + objectSep->addChild(visualisationNode); + + /*SoNode *s = CoinVisualizationFactory::getCoinVisualization(object->getCollisionModel()->getTriMeshModel(),true); + if (s) + objectSep->addChild(s); */ + } + + frictionConeSep->removeAllChildren(); + bool fc = (UI.checkBoxCones->isChecked()); + if (fc && contacts.size()>0) + { + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(contacts); + if (visualisationNode) + frictionConeSep->addChild(visualisationNode); + } + + if (UI.checkBoxGrasps->isChecked() && sceneSep->findChild(graspsSep)<0) + sceneSep->addChild(graspsSep); + if (!UI.checkBoxGrasps->isChecked() && sceneSep->findChild(graspsSep)>=0) + sceneSep->removeChild(graspsSep); + + m_pExViewer->scheduleRedraw(); +} + +int GraspPlannerWindow::main() +{ + SoQt::show(this); + SoQt::mainLoop(); + return 0; +} + + +void GraspPlannerWindow::quit() +{ + std::cout << "GraspPlannerWindow: Closing" << std::endl; + this->close(); + SoQt::exitMainLoop(); +} + +void GraspPlannerWindow::loadObject() +{ + if (!objectFile.empty()) + { + object = ObjectIO::loadManipulationObject(objectFile); + } + if (!object) + { + object = Obstacle::createBox(50.0f,50.0f,10.0f); + } + qualityMeasure.reset(new GraspStudio::GraspQualityMeasureWrenchSpace(object)); + qualityMeasure->calculateObjectProperties(); + approach.reset(new GraspStudio::ApproachMovementSurfaceNormal(object,eef)); + eefCloned = approach->getEEFRobotClone(); + if (robot && eef) + { + std::string name = "Grasp Planner - "; + name += eef->getName(); + grasps.reset(new GraspSet(name,robot->getType(),eefName)); + } + + planner.reset(new GraspStudio::GenericGraspPlanner(grasps,qualityMeasure,approach)); + planner->setVerbose(true); +} + +void GraspPlannerWindow::loadRobot() +{ + robot.reset(); + robot = RobotIO::loadRobot(robotFile); + if (!robot) + { + VR_ERROR << " no robot at " << robotFile << endl; + return; + } + eef = robot->getEndEffector(eefName); + if (!preshape.empty()) + { + eef->setPreshape(preshape); + } + + eefVisu = CoinVisualizationFactory::CreateEndEffectorVisualization(eef); + eefVisu->ref(); +} + +void GraspPlannerWindow::plan() +{ + + float timeout = UI.spinBoxTimeOut->value() * 1000.0f; + bool forceClosure = UI.checkBoxFoceClosure->isChecked(); + float quality = (float)UI.doubleSpinBoxQuality->value(); + int nrGrasps = UI.spinBoxGraspNumber->value(); + planner.reset(new GraspStudio::GenericGraspPlanner(grasps,qualityMeasure,approach,quality,forceClosure)); + + int nr = planner->plan(nrGrasps,timeout); + VR_INFO << " Grasp planned:" << nr << endl; + int start = (int)grasps->getSize()-nrGrasps-1; + if (start<0) + start = 0; + for (int i=start; i<(int)grasps->getSize()-1; i++) + { + Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose()); + SoSeparator *sep1 = new SoSeparator(); + SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransform(m); + sep1->addChild(mt); + sep1->addChild(eefVisu); + graspsSep->addChild(sep1); + } + // set to last valid grasp + if (grasps->getSize()>0 && eefCloned && eefCloned->getEndEffector(eefName)) + { + Eigen::Matrix4f mGrasp = grasps->getGrasp(grasps->getSize()-1)->getTcpPoseGlobal(object->getGlobalPose()); + eefCloned->setGlobalPoseForRobotNode(eefCloned->getEndEffector(eefName)->getTcp(),mGrasp); + } + if (nrGrasps>0) + { + openEEF(); + closeEEF(); + } +} + + + +void GraspPlannerWindow::closeEEF() +{ +#if 0 + static int pp=0; + static Eigen::Vector3f approachDir; + /*object->getCollisionModel()->getTriMeshModel()->print(); + object->getCollisionModel()->getTriMeshModel()->checkAndCorrectNormals(false);*/ + object->getCollisionModel()->getTriMeshModel()->print(); + if (pp==0) + { + Eigen::Vector3f position; + approach->getPositionOnObject(position,approachDir); + + // set new pose + approach->setEEFToApproachPose(position,approachDir); + //eefCloned->getEndEffector(eefName)->getGCP()->showCoordinateSystem(true); + } else + { + approach->moveEEFAway(approachDir,3.0f,5); + } + pp = (pp+1) % 5; + return; +#endif + contacts.clear(); + if (eefCloned && eefCloned->getEndEffector(eefName)) + { + contacts = eefCloned->getEndEffector(eefName)->closeActors(object); + float qual = qualityMeasure->getGraspQuality(); + bool isFC = qualityMeasure->isGraspForceClosure(); + std::stringstream ss; + ss << std::setprecision(3); + ss << "Grasp Nr " << grasps->getSize() << "\nQuality (wrench space): " << qual << "\nForce closure: "; + if (isFC) + ss << "yes"; + else + ss << "no"; + UI.labelInfo->setText(QString (ss.str().c_str())); + } + buildVisu(); +} + +void GraspPlannerWindow::openEEF() +{ + contacts.clear(); + if (eefCloned && eefCloned->getEndEffector(eefName)) + { + eefCloned->getEndEffector(eefName)->openActors(); + } + buildVisu(); +} + +void GraspPlannerWindow::frictionConeVisu() +{ + buildVisu(); +} + +void GraspPlannerWindow::colModel() +{ + buildVisu(); +} + +void GraspPlannerWindow::showGrasps() +{ + buildVisu(); +} + +void GraspPlannerWindow::save() +{ + if (!object) + return; + + ManipulationObjectPtr objectM(new ManipulationObject(object->getName(),object->getVisualization()->clone(),object->getCollisionModel()->clone())); + objectM->addGraspSet(grasps); + QString fi = QFileDialog::getSaveFileName(this, tr("Save ManipulationObject"), QString(), tr("XML Files (*.xml)")); + objectFile = std::string(fi.toAscii()); + bool ok = false; + try + { + ok = ObjectIO::saveManipulationObject(objectM, objectFile); + } + catch (VirtualRobotException &e) + { + cout << " ERROR while saving object" << endl; + cout << e.what(); + return; + } + + if (!ok) + { + cout << " ERROR while saving object" << endl; + return; + } +} diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..76cab3036ca559d7bb26f8debf235b4e95c77d69 --- /dev/null +++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h @@ -0,0 +1,111 @@ + +#ifndef __GraspPlanner_WINDOW_H_ +#define __GraspPlanner_WINDOW_H_ + +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/XML/SceneIO.h> +#include <VirtualRobot/Visualization/VisualizationFactory.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <VirtualRobot/Obstacle.h> +#include <VirtualRobot/ManipulationObject.h> +#include <VirtualRobot/Obstacle.h> + +#include "GraspPlanning/GraspStudio.h" +#include "GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h" +#include "GraspPlanning/GraspPlanner/GenericGraspPlanner.h" +#include "GraspPlanning/ApproachMovementSurfaceNormal.h" + +#include <string.h> +#include <QtCore/QtGlobal> +#include <QtGui/QtGui> +#include <QtCore/QtCore> + +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/Qt/SoQt.h> +#include <Inventor/nodes/SoSeparator.h> + + +#include <vector> + +#include "ui_GraspPlanner.h" + +class GraspPlannerWindow : public QMainWindow +{ + Q_OBJECT +public: + GraspPlannerWindow(std::string &robotFile, std::string &eefName, std::string &preshape, std::string &objectFile, Qt::WFlags flags = 0); + ~GraspPlannerWindow(); + + /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */ + int main(); + +public slots: + /*! Closes the window and exits SoQt runloop. */ + void quit(); + + /*!< Overriding the close event, so we know when the window was closed by the user. */ + void closeEvent(QCloseEvent *event); + + void resetSceneryAll(); + + + void closeEEF(); + void openEEF(); + void colModel(); + void frictionConeVisu(); + void showGrasps(); + + void buildVisu(); + + void plan(); + void save(); + +protected: + + void loadRobot(); + void loadObject(); + + void setupUI(); + + static void timerCB(void * data, SoSensor * sensor); + Ui::GraspPlanner UI; + SoQtExaminerViewer *m_pExViewer; /*!< Viewer to display the 3D model of the robot and the environment. */ + + SoSeparator *sceneSep; + SoSeparator *robotSep; + SoSeparator *objectSep; + SoSeparator *frictionConeSep; + SoSeparator *graspsSep; + + VirtualRobot::RobotPtr robot; + VirtualRobot::RobotPtr eefCloned; + VirtualRobot::ObstaclePtr object; + VirtualRobot::EndEffectorPtr eef; + + VirtualRobot::GraspSetPtr grasps; + + + std::vector< VirtualRobot::EndEffector::ContactInfo > contacts; + + + std::string robotFile; + std::string objectFile; + std::string eefName; + std::string preshape; + + SoSeparator *eefVisu; + + GraspStudio::GraspQualityMeasureWrenchSpacePtr qualityMeasure; + GraspStudio::ApproachMovementSurfaceNormalPtr approach; + GraspStudio::GenericGraspPlannerPtr planner; + + boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationRobot; + boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject; +}; + +#endif // __GraspPlanner_WINDOW_H_ diff --git a/GraspPlanning/examples/GraspQuality/CMakeLists.txt b/GraspPlanning/examples/GraspQuality/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..eaf257549dbd2a848510b46c3d1ac87d0f5fc86a --- /dev/null +++ b/GraspPlanning/examples/GraspQuality/CMakeLists.txt @@ -0,0 +1,32 @@ +PROJECT ( GraspQuality ) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4) + +CMAKE_POLICY(VERSION 2.6) + +IF(NOT DEFINED SIMOX_CONFIGURED) + INCLUDE(../../config.cmake) +ENDIF() + +IF(SIMOX_USE_COIN_VISUALIZATION) + + + # the variable "demo_SRCS" contains all .cpp files of this project + FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/GraspQualityDemo.cpp ${PROJECT_SOURCE_DIR}/GraspQualityWindow.cpp) + FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/GraspQualityWindow.h) + + set(GUI_MOC_HDRS + ${PROJECT_SOURCE_DIR}/GraspQualityWindow.h + ) + set(GUI_UIS + ${PROJECT_SOURCE_DIR}/GraspQuality.ui + ) + + + + # create the executable + simox_add_executable(${PROJECT_NAME} "${demo_SRCS}" "${demo_INCS}" "${GUI_MOC_HDRS}" "${GUI_UIS}") + + # we need the GraspStudio lib + TARGET_LINK_LIBRARIES(${PROJECT_NAME} GraspStudio) +ENDIF(SIMOX_USE_COIN_VISUALIZATION) diff --git a/GraspPlanning/examples/GraspQuality/GraspQuality.ui b/GraspPlanning/examples/GraspQuality/GraspQuality.ui new file mode 100644 index 0000000000000000000000000000000000000000..ecab528b83ed412371c3ef62b831619cceea36bb --- /dev/null +++ b/GraspPlanning/examples/GraspQuality/GraspQuality.ui @@ -0,0 +1,454 @@ +<?xml version="1.0" encoding="UTF-8"?> +<ui version="4.0"> + <class>MainWindowGraspQuality</class> + <widget class="QMainWindow" name="MainWindowGraspQuality"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1079</width> + <height>819</height> + </rect> + </property> + <property name="windowTitle"> + <string>GraspStudio - GraspQuality</string> + </property> + <widget class="QWidget" name="centralwidget"> + <layout class="QGridLayout" name="gridLayout"> + <item row="0" column="0"> + <widget class="QFrame" name="frameViewer"> + <property name="frameShape"> + <enum>QFrame::StyledPanel</enum> + </property> + <property name="frameShadow"> + <enum>QFrame::Raised</enum> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QGroupBox" name="groupBox"> + <property name="maximumSize"> + <size> + <width>200</width> + <height>16777215</height> + </size> + </property> + <property name="title"> + <string/> + </property> + <widget class="QPushButton" name="pushButtonReset"> + <property name="geometry"> + <rect> + <x>20</x> + <y>20</y> + <width>171</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Reset</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonClose"> + <property name="geometry"> + <rect> + <x>20</x> + <y>150</y> + <width>81</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Close EEF</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonOpen"> + <property name="geometry"> + <rect> + <x>110</x> + <y>150</y> + <width>81</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Open EEF</string> + </property> + </widget> + <widget class="QLabel" name="label_10"> + <property name="geometry"> + <rect> + <x>10</x> + <y>510</y> + <width>81</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Visualization</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxColModel"> + <property name="geometry"> + <rect> + <x>20</x> + <y>540</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Collision Model</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxEEF"> + <property name="geometry"> + <rect> + <x>20</x> + <y>100</y> + <width>171</width> + <height>22</height> + </rect> + </property> + </widget> + <widget class="QLabel" name="label"> + <property name="geometry"> + <rect> + <x>22</x> + <y>80</y> + <width>121</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Select end effector</string> + </property> + </widget> + <widget class="QGroupBox" name="groupBox_2"> + <property name="geometry"> + <rect> + <x>10</x> + <y>260</y> + <width>181</width> + <height>211</height> + </rect> + </property> + <property name="title"> + <string>Object</string> + </property> + <widget class="QSlider" name="horizontalSliderRo"> + <property name="geometry"> + <rect> + <x>30</x> + <y>100</y> + <width>141</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_6"> + <property name="geometry"> + <rect> + <x>11</x> + <y>80</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Z</string> + </property> + </widget> + <widget class="QLabel" name="label_3"> + <property name="geometry"> + <rect> + <x>11</x> + <y>20</y> + <width>81</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Move Object</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderZ"> + <property name="geometry"> + <rect> + <x>30</x> + <y>80</y> + <width>141</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderX"> + <property name="geometry"> + <rect> + <x>30</x> + <y>40</y> + <width>141</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_7"> + <property name="geometry"> + <rect> + <x>11</x> + <y>100</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Ro</string> + </property> + </widget> + <widget class="QLabel" name="label_8"> + <property name="geometry"> + <rect> + <x>11</x> + <y>120</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Pi</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderPi"> + <property name="geometry"> + <rect> + <x>30</x> + <y>120</y> + <width>141</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_9"> + <property name="geometry"> + <rect> + <x>11</x> + <y>140</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Ya</string> + </property> + </widget> + <widget class="QLabel" name="label_5"> + <property name="geometry"> + <rect> + <x>11</x> + <y>60</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Y</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderYa"> + <property name="geometry"> + <rect> + <x>30</x> + <y>140</y> + <width>141</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_4"> + <property name="geometry"> + <rect> + <x>11</x> + <y>40</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>X</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderY"> + <property name="geometry"> + <rect> + <x>30</x> + <y>60</y> + <width>141</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QPushButton" name="pushButtonToTCP"> + <property name="geometry"> + <rect> + <x>50</x> + <y>170</y> + <width>101</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>to TCP</string> + </property> + </widget> + </widget> + <widget class="QCheckBox" name="checkBoxCones"> + <property name="geometry"> + <rect> + <x>20</x> + <y>560</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Friction Cones</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonQuality"> + <property name="geometry"> + <rect> + <x>20</x> + <y>190</y> + <width>171</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Grasp Quality</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxGWS1"> + <property name="geometry"> + <rect> + <x>20</x> + <y>580</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show GWS (force)</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxGWS2"> + <property name="geometry"> + <rect> + <x>20</x> + <y>600</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show GWS (torque)</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxOWS2"> + <property name="geometry"> + <rect> + <x>20</x> + <y>640</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show OWS (torque)</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxOWS1"> + <property name="geometry"> + <rect> + <x>20</x> + <y>620</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show OWS (force)</string> + </property> + </widget> + </widget> + </item> + </layout> + </widget> + <widget class="QMenuBar" name="menubar"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1079</width> + <height>21</height> + </rect> + </property> + </widget> + <widget class="QStatusBar" name="statusbar"/> + </widget> + <resources/> + <connections/> +</ui> diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityDemo.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityDemo.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6bcf92d86c3b35d345574e0f43d3f201a87ee91f --- /dev/null +++ b/GraspPlanning/examples/GraspQuality/GraspQualityDemo.cpp @@ -0,0 +1,50 @@ +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/RuntimeEnvironment.h> + +#include <string> +#include <iostream> + +using std::cout; +using std::endl; +using namespace VirtualRobot; + +#include <Eigen/Core> +#include <Eigen/Geometry> + +#include "GraspQualityWindow.h" + + +int main(int argc, char *argv[]) +{ + SoDB::init(); + SoQt::init(argc,argv,"IKRRT"); + cout << " --- START --- " << endl; + + //std::string filename1(VR_BASE_DIR "/data/objects/plate.xml"); + std::string robot(SIMOX_BASE_DIR "/VirtualRobot/data/robots/ArmarIII/ArmarIII.xml"); + //std::string object(SIMOX_BASE_DIR "/VirtualRobot/data/objects/wok.xml"); + std::string object(SIMOX_BASE_DIR "/VirtualRobot/data/objects/riceBox.xml"); + + VirtualRobot::RuntimeEnvironment::considerKey("robot"); + VirtualRobot::RuntimeEnvironment::considerKey("object"); + VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv); + VirtualRobot::RuntimeEnvironment::print(); + + std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot"); + if (!robFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile)) + robot = robFile; + + std::string objFile = VirtualRobot::RuntimeEnvironment::getValue("object"); + if (!objFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(objFile)) + object = objFile; + + + cout << "Using robot from " << robot << endl; + cout << "Using object from " << object << endl; + + GraspQualityWindow rw(robot,object); + + rw.main(); + + return 0; +} diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5e58fff3d085c3885e42b85144f05ff658dfbf64 --- /dev/null +++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp @@ -0,0 +1,591 @@ + +#include "GraspQualityWindow.h" +#include "GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h" +#include "VirtualRobot/EndEffector/EndEffector.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/ManipulationObject.h" +#include "VirtualRobot/Grasp.h" +#include "VirtualRobot/IK/GenericIKSolver.h" +#include "VirtualRobot/GraspSet.h" +#include "VirtualRobot/CollisionDetection/CDManager.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/XML/ObjectIO.h" +#include "VirtualRobot/XML/RobotIO.h" +#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h" +#include <QFileDialog> +#include <Eigen/Geometry> + +#include <time.h> +#include <vector> +#include <iostream> +#include <cmath> + + + +#include "Inventor/actions/SoLineHighlightRenderAction.h" +#include <Inventor/nodes/SoShapeHints.h> +#include <Inventor/nodes/SoLightModel.h> +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/nodes/SoMatrixTransform.h> +#include <Inventor/nodes/SoScale.h> + +#include <sstream> +using namespace std; +using namespace VirtualRobot; + +float TIMER_MS = 30.0f; + +GraspQualityWindow::GraspQualityWindow(std::string &robFile, std::string &objFile, Qt::WFlags flags) +:QMainWindow(NULL) +{ + VR_INFO << " start " << endl; + + this->robotFile = robFile; + this->objectFile = objFile; + + sceneSep = new SoSeparator; + sceneSep->ref(); + robotSep = new SoSeparator; + objectSep = new SoSeparator; + frictionConeSep = new SoSeparator; + gws1Sep = new SoSeparator; + gws2Sep = new SoSeparator; + ows1Sep = new SoSeparator; + ows2Sep = new SoSeparator; + + sceneSep->addChild(robotSep); + sceneSep->addChild(objectSep); + sceneSep->addChild(frictionConeSep); + sceneSep->addChild(gws1Sep); + sceneSep->addChild(gws2Sep); + sceneSep->addChild(ows1Sep); + sceneSep->addChild(ows2Sep); + + setupUI(); + + + loadObject(); + loadRobot(); + + + m_pExViewer->viewAll(); + + SoSensorManager *sensor_mgr = SoDB::getSensorManager(); + SoTimerSensor *timer = new SoTimerSensor(timerCB, this); + timer->setInterval(SbTime(TIMER_MS/1000.0f)); + sensor_mgr->insertTimerSensor(timer); +} + + +GraspQualityWindow::~GraspQualityWindow() +{ + sceneSep->unref(); +} + + +void GraspQualityWindow::timerCB(void * data, SoSensor * sensor) +{ + GraspQualityWindow *ikWindow = static_cast<GraspQualityWindow*>(data); + float x[6]; + x[0] = (float)ikWindow->UI.horizontalSliderX->value(); + x[1] = (float)ikWindow->UI.horizontalSliderY->value(); + x[2] = (float)ikWindow->UI.horizontalSliderZ->value(); + x[3]= (float)ikWindow->UI.horizontalSliderRo->value(); + x[4] = (float)ikWindow->UI.horizontalSliderPi->value(); + x[5] = (float)ikWindow->UI.horizontalSliderYa->value(); + x[0] /= 10.0f; + x[1] /= 10.0f; + x[2] /= 10.0f; + x[3] /= 300.0f; + x[4] /= 300.0f; + x[5] /= 300.0f; + + if (x[0]!=0 || x[1]!=0 || x[2]!=0 || x[3]!=0 || x[4]!=0 || x[5]!=0) + ikWindow->updateObject(x); +} + + +void GraspQualityWindow::setupUI() +{ + UI.setupUi(this); + m_pExViewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP); + + // setup + m_pExViewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f)); + m_pExViewer->setAccumulationBuffer(true); +#ifdef WIN32 +#ifndef _DEBUG + m_pExViewer->setAntialiasing(true, 4); +#endif +#endif + m_pExViewer->setGLRenderAction(new SoLineHighlightRenderAction); + m_pExViewer->setTransparencyType(SoGLRenderAction::BLEND); + m_pExViewer->setFeedbackVisibility(true); + m_pExViewer->setSceneGraph(sceneSep); + m_pExViewer->viewAll(); + + connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll())); + connect(UI.pushButtonClose, SIGNAL(clicked()), this, SLOT(closeEEF())); + connect(UI.pushButtonOpen, SIGNAL(clicked()), this, SLOT(openEEF())); + connect(UI.pushButtonToTCP, SIGNAL(clicked()), this, SLOT(objectToTCP())); + connect(UI.pushButtonQuality, SIGNAL(clicked()), this, SLOT(graspQuality())); + + connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(colModel())); + connect(UI.checkBoxCones, SIGNAL(clicked()), this, SLOT(frictionConeVisu())); + connect(UI.checkBoxGWS1, SIGNAL(clicked()), this, SLOT(showGWS())); + connect(UI.checkBoxGWS2, SIGNAL(clicked()), this, SLOT(showGWS())); + connect(UI.checkBoxOWS1, SIGNAL(clicked()), this, SLOT(showOWS())); + connect(UI.checkBoxOWS2, SIGNAL(clicked()), this, SLOT(showOWS())); + + connect(UI.horizontalSliderX, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectX())); + connect(UI.horizontalSliderY, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectY())); + connect(UI.horizontalSliderZ, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectZ())); + connect(UI.horizontalSliderRo, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectA())); + connect(UI.horizontalSliderPi, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectB())); + connect(UI.horizontalSliderYa, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectG())); + + connect(UI.comboBoxEEF, SIGNAL(activated(int)), this, SLOT(selectEEF(int))); +} + + +void GraspQualityWindow::resetSceneryAll() +{ + //if (rns) + // rns->setJointValues(startConfig); +} + + +void GraspQualityWindow::closeEvent(QCloseEvent *event) +{ + quit(); + QMainWindow::closeEvent(event); +} + + +void GraspQualityWindow::buildVisu() +{ + + robotSep->removeAllChildren(); + //bool colModel = (UI.checkBoxColModel->isChecked()); + SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked())?SceneObject::Collision:SceneObject::Full; + + if (robot) + { + visualizationRobot = robot->getVisualization<CoinVisualization>(colModel); + SoNode* visualisationNode = visualizationRobot->getCoinVisualization(); + if (visualisationNode) + { + robotSep->addChild(visualisationNode); + //visualizationRobot->highlight(true); + } + } + + objectSep->removeAllChildren(); + if (object) + { + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(object,colModel); + if (visualisationNode) + objectSep->addChild(visualisationNode); + } + + frictionConeSep->removeAllChildren(); + bool fc = (UI.checkBoxCones->isChecked()); + if (fc && contacts.size()>0) + { + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(contacts); + if (visualisationNode) + frictionConeSep->addChild(visualisationNode); + + } + + m_pExViewer->scheduleRedraw(); +} + +int GraspQualityWindow::main() +{ + SoQt::show(this); + SoQt::mainLoop(); + return 0; +} + + +void GraspQualityWindow::quit() +{ + std::cout << "GraspQualityWindow: Closing" << std::endl; + this->close(); + SoQt::exitMainLoop(); +} + +void GraspQualityWindow::loadObject() +{ + if (!objectFile.empty()) + { + object = ObjectIO::loadManipulationObject(objectFile); + } + if (!object) + { + object = Obstacle::createBox(50.0f,50.0f,10.0f); + } + qualityMeasure.reset(new GraspStudio::GraspQualityMeasureWrenchSpace(object)); + qualityMeasure->calculateObjectProperties(); +} + +void GraspQualityWindow::loadRobot() +{ + eefs.clear(); + robot.reset(); + robot = RobotIO::loadRobot(robotFile); + if (!robot) + { + VR_ERROR << " no robot at " << robotFile << endl; + return; + } + + + setEEFComboBox(); + selectEEF(0); + objectToTCP(); + + buildVisu(); +} + +void GraspQualityWindow::objectToTCP() +{ + if (object && eef && eef->getTcp()) + { + Eigen::Matrix4f pos = eef->getTcp()->getGlobalPose(); + object->setGlobalPose(pos); + } +} + +void GraspQualityWindow::graspQuality() +{ + if (qualityMeasure && object && contacts.size()>0) + { + qualityMeasure->setContactPoints(contacts); + float volume = qualityMeasure->getVolumeGraspMeasure(); + float epsilon = qualityMeasure->getGraspQuality(); + bool fc = qualityMeasure->isGraspForceClosure(); + cout << "Grasp Quality (epsilon measure):" << epsilon << endl; + cout << "v measure:" << volume << endl; + cout << "Force closure:"; + if (fc) + cout << "yes" << endl; + else + cout << "no" << endl; + + } +} +void GraspQualityWindow::selectEEF(int nr) +{ + eef.reset(); + if (nr<0 || nr>=(int)eefs.size()) + return; + eef = eefs[nr]; +} + +void GraspQualityWindow::setEEFComboBox() +{ + UI.comboBoxEEF->clear(); + eef.reset(); + eefs.clear(); + if (!robot) + return; + + robot->getEndEffectors(eefs); + for (size_t i=0;i<eefs.size();i++) + { + QString nameEEF(eefs[i]->getName().c_str()); + UI.comboBoxEEF->addItem(nameEEF); + } +} + +void GraspQualityWindow::closeEEF() +{ + contacts.clear(); + if (eef) + { + contacts = eef->closeActors(object); + } + buildVisu(); +} + +void GraspQualityWindow::openEEF() +{ + contacts.clear(); + if (eef) + { + eef->openActors(); + } + buildVisu(); +} + + + +void GraspQualityWindow::updateObject( float x[6] ) +{ + if (object) + { + //cout << "getGlobalPose robot:" << endl << robotEEF->getGlobalPose() << endl; + //cout << "getGlobalPose TCP:" << endl << robotEEF_EEF->getTcp()->getGlobalPose() << endl; + Eigen::Matrix4f m; + MathTools::posrpy2eigen4f(x,m); + + m = object->getGlobalPose() * m; + object->setGlobalPose(m); + cout << "object " << endl; + cout << m << endl; + + } + m_pExViewer->scheduleRedraw(); +} + +void GraspQualityWindow::sliderReleased_ObjectX() +{ + UI.horizontalSliderX->setValue(0); +} + +void GraspQualityWindow::sliderReleased_ObjectY() +{ + UI.horizontalSliderY->setValue(0); +} + +void GraspQualityWindow::sliderReleased_ObjectZ() +{ + UI.horizontalSliderZ->setValue(0); +} + +void GraspQualityWindow::sliderReleased_ObjectA() +{ + UI.horizontalSliderRo->setValue(0); +} + +void GraspQualityWindow::sliderReleased_ObjectB() +{ + UI.horizontalSliderPi->setValue(0); +} + +void GraspQualityWindow::sliderReleased_ObjectG() +{ + UI.horizontalSliderYa->setValue(0); +} + +void GraspQualityWindow::frictionConeVisu() +{ + buildVisu(); +} + +void GraspQualityWindow::colModel() +{ + buildVisu(); +} + +void GraspQualityWindow::showGWS() +{ + gws1Sep->removeAllChildren(); + gws2Sep->removeAllChildren(); + ows1Sep->removeAllChildren(); + ows2Sep->removeAllChildren(); + SoMatrixTransform *mt = new SoMatrixTransform(); + Eigen::Matrix4f globalPose = object->getGlobalPose(); + SbMatrix m(reinterpret_cast<SbMat*>(globalPose.data())); + mt->matrix.setValue(m); + +#if 0 + // test + + GraspStudio::ContactConeGeneratorPtr cgMM(new GraspStudio::ContactConeGenerator(8,0.25f,100.0f)); + std::vector<MathTools::ContactPoint> resultsMM; + std::vector<EndEffector::ContactInfo>::const_iterator objPointsIter; + for (objPointsIter = contacts.begin(); objPointsIter != contacts.end(); objPointsIter++) + { + MathTools::ContactPoint point; + + point.p = objPointsIter->contactPointObstacleLocal; + //point.p -= centerOfModel; + + point.n = objPointsIter->contactPointFingerLocal - objPointsIter->contactPointObstacleLocal; + point.n.normalize(); + + cgMM->computeConePoints(point,resultsMM); + + SoSeparator* pV2 = CoinVisualizationFactory::CreatePointVisualization(point,true); + gws1Sep->addChild(pV2); + + } + SoSeparator* pV = CoinVisualizationFactory::CreatePointsVisualization(resultsMM,true); + SoSeparator *scaledPoints = new SoSeparator; + SoScale *sc0 = new SoScale; + float sf0 = 1.0f; + sc0->scaleFactor.setValue(sf0,sf0,sf0); + scaledPoints->addChild(sc0); + scaledPoints->addChild(pV); + gws1Sep->addChild(scaledPoints); + + +#endif + +#if 0 + // test + robotSep->removeAllChildren(); + // plane + Eigen::Vector3f posZero(0,0,0); + Eigen::Vector3f posZeroN(0,0,1.0f); + SoSeparator* pV3 = CoinVisualizationFactory::CreatePlaneVisualization(posZero,posZeroN,1000.0f,0.5f); + gws1Sep->addChild(pV3); + + + std::vector<MathTools::ContactPoint> resultsM; + std::vector<MathTools::ContactPoint> resultsMM; + MathTools::ContactPoint pointM; + MathTools::ContactPoint pointMM; + float pos = 0.0f; + float length = 0.2f; //m + float lengthMM = length * 1000.0f; //m + pointM.p(0) = pos; + pointM.p(1) = pos; + pointM.p(2) = pos; + pointM.n(0) = 1.0f; + pointM.n(1) = 1.0f; + pointM.n(2) = 1.0f; + pointM.n.normalize(); + pointMM.n = pointM.n; + pointMM.p = pointM.p * 1000.0f; + MathTools::ContactPoint pointM2; + MathTools::ContactPoint pointMM2; + pointM2.p(0) = pos; + pointM2.p(1) = pos-length; + pointM2.p(2) = pos; + pointM2.n(0) = 0.0f; + pointM2.n(1) = -1.0f; + pointM2.n(2) = 0.0f; + pointMM2.n = pointM2.n; + pointMM2.p = pointM2.p * 1000.0f; + MathTools::ContactPoint pointM3; + MathTools::ContactPoint pointMM3; + pointM3.p(0) = pos; + pointM3.p(1) = pos; + pointM3.p(2) = pos+length; + pointM3.n(0) = 0.0f; + pointM3.n(1) = 0.0f; + pointM3.n(2) = 1.0f; + pointMM3.n = pointM3.n; + pointMM3.p = pointM3.p * 1000.0f; + GraspStudio::ContactConeGeneratorPtr cg(new GraspStudio::ContactConeGenerator(8,0.25f,1.0f)); + GraspStudio::ContactConeGeneratorPtr cgMM(new GraspStudio::ContactConeGenerator(8,0.25f,100.0f)); + cg->computeConePoints(pointM,resultsM); + cg->computeConePoints(pointM2,resultsM); + cg->computeConePoints(pointM3,resultsM); + cgMM->computeConePoints(pointMM,resultsMM); + cgMM->computeConePoints(pointMM2,resultsMM); + cgMM->computeConePoints(pointMM3,resultsMM); + + SoSeparator* pV = CoinVisualizationFactory::CreatePointsVisualization(resultsMM,true); + SoSeparator *scaledPoints = new SoSeparator; + SoScale *sc0 = new SoScale; + float sf0 = 1.0f; + sc0->scaleFactor.setValue(sf0,sf0,sf0); + scaledPoints->addChild(sc0); + scaledPoints->addChild(pV); + gws1Sep->addChild(scaledPoints); + SoSeparator* pV2 = CoinVisualizationFactory::CreatePointVisualization(pointMM,true); + SoSeparator* pV2b = CoinVisualizationFactory::CreatePointVisualization(pointMM2,true); + SoSeparator* pV2c = CoinVisualizationFactory::CreatePointVisualization(pointMM3,true); + gws1Sep->addChild(pV2); + gws1Sep->addChild(pV2b); + gws1Sep->addChild(pV2c); + + // plane + Eigen::Vector3f posZero(0,0,0); + Eigen::Vector3f posZeroN(0,0,1.0f); + SoSeparator* pV3 = CoinVisualizationFactory::CreatePlaneVisualization(posZero,posZeroN,1000.0f,0.5f); + gws1Sep->addChild(pV3); + + + // wrench + Eigen::Vector3f com = 0.333f * (pointM.p + pointM2.p + pointM3.p); + std::vector<VirtualRobot::MathTools::ContactPoint> wrenchP = GraspStudio::GraspQualityMeasureWrenchSpace::createWrenchPoints(resultsM,com,lengthMM); + MathTools::print(wrenchP); + // convex hull + VirtualRobot::MathTools::ConvexHull6DPtr ch1 = GraspStudio::ConvexHullGenerator::CreateConvexHull(wrenchP); + float minO = GraspStudio::GraspQualityMeasureWrenchSpace::minOffset(ch1); + cout << "minOffset:" << minO << endl; + std::vector<MathTools::TriangleFace6D>::iterator faceIter; + cout << "Distances to Origin:" << endl; + for (faceIter = ch1->faces.begin(); faceIter != ch1->faces.end(); faceIter++) + { + cout << faceIter->distPlaneZero << ", "; + if (faceIter->distPlaneZero > 1e-4 ) + cout << "<-- not force closure " << endl; + + } + cout << endl; + // ch visu + GraspStudio::CoinConvexHullVisualizationPtr visu(new GraspStudio::CoinConvexHullVisualization(ch1,false)); + SoSeparator *chV = visu->getCoinVisualization(); + SoSeparator *scaledCH = new SoSeparator; + SoScale *sc1 = new SoScale; + float sf1 = 1.0f/4.0f; + sc1->scaleFactor.setValue(sf1,sf1,sf1); + scaledCH->addChild(sc1); + scaledCH->addChild(chV); + gws1Sep->addChild(scaledCH); + + return; +#endif + + + if (!qualityMeasure) + return; + VirtualRobot::MathTools::ConvexHull6DPtr ch = qualityMeasure->getConvexHullGWS(); + VirtualRobot::MathTools::ConvexHull6DPtr chOWS = qualityMeasure->getConvexHullOWS(); + if (!ch) + return; + if (UI.checkBoxGWS1->isChecked()) + { + GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(ch,true)); + SoSeparator* s = v->getCoinVisualization(); + if (s) + { + gws1Sep->addChild(mt); + gws1Sep->addChild(s); + } + } + if (UI.checkBoxGWS2->isChecked()) + { + GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS,false)); + SoSeparator* s = v->getCoinVisualization(); + if (s) + { + gws2Sep->addChild(mt); + gws2Sep->addChild(s); + } + + } + if (UI.checkBoxOWS1->isChecked()) + { + GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS,true)); + SoSeparator* s = v->getCoinVisualization(); + if (s) + { + ows1Sep->addChild(mt); + ows1Sep->addChild(s); + } + } + if (UI.checkBoxOWS2->isChecked()) + { + GraspStudio::CoinConvexHullVisualizationPtr v(new GraspStudio::CoinConvexHullVisualization(chOWS,false)); + SoSeparator* s = v->getCoinVisualization(); + if (s) + { + ows2Sep->addChild(mt); + ows2Sep->addChild(s); + } + } +} + +void GraspQualityWindow::showOWS() +{ + +} diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..0e62b4e385fbd89ac166e1b7197401be8ecdbb60 --- /dev/null +++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h @@ -0,0 +1,118 @@ + +#ifndef __GraspQuality_WINDOW_H_ +#define __GraspQuality_WINDOW_H_ + +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/XML/SceneIO.h> +#include <VirtualRobot/Visualization/VisualizationFactory.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <VirtualRobot/Obstacle.h> +#include <VirtualRobot/ManipulationObject.h> +#include <VirtualRobot/Obstacle.h> + +#include "GraspPlanning/GraspStudio.h" +#include "GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h" + +#include <string.h> +#include <QtCore/QtGlobal> +#include <QtGui/QtGui> +#include <QtCore/QtCore> + +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/Qt/SoQt.h> +#include <Inventor/nodes/SoSeparator.h> + + +#include <vector> + +#include "ui_GraspQuality.h" + +class GraspQualityWindow : public QMainWindow +{ + Q_OBJECT +public: + GraspQualityWindow(std::string &robotFile, std::string &objectFile, Qt::WFlags flags = 0); + ~GraspQualityWindow(); + + /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */ + int main(); + +public slots: + /*! Closes the window and exits SoQt runloop. */ + void quit(); + + /*!< Overriding the close event, so we know when the window was closed by the user. */ + void closeEvent(QCloseEvent *event); + + void resetSceneryAll(); + + + void closeEEF(); + void openEEF(); + void colModel(); + void frictionConeVisu(); + + void sliderReleased_ObjectX(); + void sliderReleased_ObjectY(); + void sliderReleased_ObjectZ(); + void sliderReleased_ObjectA(); + void sliderReleased_ObjectB(); + void sliderReleased_ObjectG(); + + void buildVisu(); + + void selectEEF(int nr); + void objectToTCP(); + void graspQuality(); + void showGWS(); + void showOWS(); + +protected: + + void loadRobot(); + void loadObject(); + + void setupUI(); + + void updateObject(float x[6]); + + static void timerCB(void * data, SoSensor * sensor); + void buildRrtVisu(); + void setEEFComboBox(); + Ui::MainWindowGraspQuality UI; + SoQtExaminerViewer *m_pExViewer; /*!< Viewer to display the 3D model of the robot and the environment. */ + + SoSeparator *sceneSep; + SoSeparator *robotSep; + SoSeparator *objectSep; + SoSeparator *frictionConeSep; + SoSeparator *gws1Sep; + SoSeparator *gws2Sep; + SoSeparator *ows1Sep; + SoSeparator *ows2Sep; + + VirtualRobot::RobotPtr robot; + VirtualRobot::ObstaclePtr object; + + VirtualRobot::EndEffectorPtr eef; + std::vector< VirtualRobot::EndEffectorPtr > eefs; + + std::vector< VirtualRobot::EndEffector::ContactInfo > contacts; + + + std::string robotFile; + std::string objectFile; + std::string eefName; + + GraspStudio::GraspQualityMeasureWrenchSpacePtr qualityMeasure; + + boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationRobot; + boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject; +}; + +#endif // __GraspQuality_WINDOW_H_ diff --git a/MotionPlanning/CMakeLists.txt b/MotionPlanning/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..3f8cd1e7b316c8d0d860903e30bc9067e0100c47 --- /dev/null +++ b/MotionPlanning/CMakeLists.txt @@ -0,0 +1,80 @@ +PROJECT ( Saba ) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.2) +CMAKE_POLICY(VERSION 2.6) + +MESSAGE (STATUS "\n ***** CONFIGURING Simox project Saba *****") +INCLUDE (config.cmake) + +MESSAGE(STATUS "Saba: using VirtualRobotDir: ${SABA_VirtualRobotDir}") +MESSAGE("SIMOX_LIB_DIR: ${SIMOX_LIB_DIR}") +MESSAGE("SIMOX_BIN_DIR: ${SIMOX_BIN_DIR}") + +SET(SOURCES +CSpace/CSpace.cpp +CSpace/CSpaceSampled.cpp +CSpace/CSpaceNode.cpp +CSpace/CSpacePath.cpp +CSpace/CSpaceTree.cpp +CSpace/Sampler.cpp +Planner/MotionPlanner.cpp +Planner/Rrt.cpp +Planner/BiRrt.cpp +Planner/GraspIkRrt.cpp +Visualization/RrtWorkspaceVisualization.cpp +PostProcessing/PathProcessor.cpp +PostProcessing/ShortcutProcessor.cpp +) + +SET(INCLUDES +Saba.h +CSpace/CSpace.h +CSpace/CSpaceSampled.h +CSpace/CSpaceNode.h +CSpace/CSpacePath.h +CSpace/CSpaceTree.h +CSpace/Sampler.h +Planner/MotionPlanner.h +Planner/Rrt.h +Planner/BiRrt.h +Planner/GraspIkRrt.h +Visualization/RrtWorkspaceVisualization.h +PostProcessing/PathProcessor.h +PostProcessing/ShortcutProcessor.h +${SABA_SimoxDir}/VirtualRobot/definesVR.h +) + + +if (SIMOX_USE_COIN_VISUALIZATION) + SET(SOURCES + ${SOURCES} + Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp + ) + + SET(INCLUDES + ${INCLUDES} + Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h + ) +endif (SIMOX_USE_COIN_VISUALIZATION) + + +INCLUDE_DIRECTORIES(${SABA_VirtualRobotDir}/..) +INCLUDE_DIRECTORIES(${SABA_DIR}) +LINK_DIRECTORIES (${SIMOX_LIB_DIR}) + +ADD_LIBRARY (Saba SHARED ${SOURCES} ${INCLUDES}) +#MESSAGE("VIRTUAL_ROBOT_ROBOT_LINK_LIBRARIES:" ${VIRTUAL_ROBOT_LINK_LIBRARIES}) +TARGET_LINK_LIBRARIES (Saba ${COLLISIONDETECTION_LIB} ${VIRTUAL_ROBOT_LINK_LIBRARIES} VirtualRobot) + +# .DLL path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${SABA_BIN_DIR}) +# .so path +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${SABA_LIB_DIR}) +# .lib path (this is needed for setting the DLL-import library path on windows) +SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${SABA_LIB_DIR}) + +# include examples +ADD_SUBDIRECTORY(examples/) + +# include unit tests +ADD_SUBDIRECTORY(tests/) diff --git a/MotionPlanning/CSpace/CSpace.cpp b/MotionPlanning/CSpace/CSpace.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2e7e8aa9d2414b9a6a772b2e6b2b1c627c8530a6 --- /dev/null +++ b/MotionPlanning/CSpace/CSpace.cpp @@ -0,0 +1,750 @@ +#include "CSpace.h" +#include "CSpaceNode.h" +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include "Sampler.h" +#include "CSpaceTree.h" +#include "CSpacePath.h" +#include <VirtualRobot/CollisionDetection/CDManager.h> +#include <VirtualRobot/CollisionDetection/CollisionChecker.h> +#include "float.h" +#include <cmath> +#include <fstream> +#include <iomanip> +#include <time.h> + +#define GET_RANDOM_DATA_FROM_64BIT_ADDRESS(a) (int)(0xFF & (long)a) | (0xFF00 & ((long)a >> 16)) + +using namespace std; + +namespace Saba { + +SABA_IMPORT_EXPORT boost::mutex CSpace::colCheckMutex; +SABA_IMPORT_EXPORT int CSpace::cloneCounter = 0; + +//#define DO_THE_TESTS +CSpace::CSpace(VirtualRobot::RobotPtr robot, VirtualRobot::CDManagerPtr collisionManager, VirtualRobot::RobotNodeSetPtr robotNodes, unsigned int maxConfigs, unsigned int randomSeed) +{ + if (maxConfigs<=0) + THROW_SABA_EXCEPTION ("CSpaceTree: Initialization fails: INVALID MAX_CONFIGS"); + + performaceVars_collisionCheck = 0; + performaceVars_distanceCheck = 0; + checkForBorderlessDims = true; + robo = robot; + cdm = collisionManager; + + if (randomSeed == 0) + { + #ifdef __LP64__ + // This is for machines with 64Bit addresses and 32Bit int datatype + randomSeed = (unsigned int)(time(NULL) + GET_RANDOM_DATA_FROM_64BIT_ADDRESS(this)) % 10000; + #else + randomSeed = (unsigned int)(time(NULL) +(int)this) % 10000; + #endif + } + this->randomSeed = randomSeed; + + randMult = (float)(1.0/(double)(RAND_MAX)); + + useMetricWeights = false; + multiThreaded = false; + stopPathCheck = false; + + sampleAlgorithm.reset(); + + if (!robo) + { + THROW_SABA_EXCEPTION ("CSpace: Initialization fails: NO ROBOT"); + } + + srand(randomSeed); + //std::cout << "Using random seed: " << m_randomSeed << std::endl; + + dimension = 0; + + if (!robotNodes || robotNodes->getAllRobotNodes().size()==0) + { + THROW_SABA_EXCEPTION ("CSpace: Initialization fails: NO ROBOT JOINTS"); + } + this->robotNodes = robotNodes; + robotJoints = robotNodes->getAllRobotNodes(); + + dimension = robotJoints.size(); + if (dimension < 1) + { + THROW_SABA_EXCEPTION ("CSpace: Initialization fails: INVALID DIMENSION") + } + + boundaryMin.setZero(dimension); + boundaryMax.setZero(dimension); + boundaryDist.setZero(dimension); + for (unsigned int i=0;i<dimension;i++) + { + boundaryMin[i] = robotJoints[i]->getJointLimitLo(); + boundaryMax[i] = robotJoints[i]->getJointLimitHi(); + boundaryDist[i] = fabs(boundaryMax[i]-boundaryMin[i]); + if (boundaryDist[i]==0) + { + SABA_WARNING << "Limits for joint " << robotJoints[i]->getName() << " not set correctly. Degenerated dimension (" << i << ") in C-Space." << endl; + } + } + + metricWeights.setZero(dimension); + for (unsigned int i = 0; i < dimension; i++) + metricWeights[i] = 1.0; + + checkForBorderlessDimensions(checkForBorderlessDims); + + SABA_INFO << " dimension: " << dimension << ", random Seed: " << randomSeed << endl; + + // allocate configs + maxNodes = maxConfigs; + for (int i=0;i<maxNodes;i++) + { + CSpaceNodePtr node(new CSpaceNode()); + node->ID = nodes.size(); + node->parentID = -666; + node->dynDomRadius = 0; + node->obstacleDistance = -1.0f; + node->allocated = false; + freeNodes.push_back(node); + nodes.push_back(node); + } +} + + +CSpace::~CSpace() +{ + freeNodes.clear(); + nodes.clear(); +} + + +void CSpace::requestStop() +{ + stopPathCheck = true; +} + +void CSpace::reset() +{ + resetPerformanceVars(); + unsigned int i; + for (i=0;i<nodes.size();i++) + { + nodes[i]->allocated = false; + } + freeNodes = nodes; +} + +CSpaceNodePtr CSpace::getNode(unsigned int id) +{ + if (id>=nodes.size()) + { + SABA_ERROR << "ID " << id << " not known...."; + return CSpaceNodePtr(); + } + if (!nodes[id]->allocated) + SABA_WARNING << "ID "<< id << " not allocated...."; + return nodes[id]; +} + +bool CSpace::checkSolution(CSpacePathPtr solution, bool bVerbose) +{ + if (!solution) + { + std::cout << "NULL solution..." << std::endl; + return false; + } + unsigned int s = solution->getNrOfPathPoints(); + if (bVerbose) + SABA_INFO << "Checking " << s << " segments:" << endl; + for (unsigned int i = s-1; i > 0; i--) + { + clock_t t1 = clock(); + bool bColFree = isPathCollisionFree(solution->getPathEntry(i),solution->getPathEntry(i-1)); + clock_t t2 = clock(); + + if (bVerbose && t2-t1>0) + { + cout << "i:" << i << "->t:" << t2-t1 << " , "; + } + if (!bColFree) + { + return false; + } + } + if (bVerbose) + cout << endl; + return true; +} + +bool CSpace::checkTree(CSpaceTreePtr tree) +{ + if (!tree) + { + std::cout << "NULL tree..." << std::endl; + return false; + } + std::vector<CSpaceNodePtr> vNodes = tree->getNodes(); + + unsigned int s = (unsigned int)vNodes.size(); + for (unsigned int i = 0; i < s; i++) + { + CSpaceNodePtr pN1 = vNodes[i]; + if (pN1 && pN1->parentID>=0 && tree->getNode(pN1->parentID)) + { + CSpaceNodePtr pN2 = tree->getNode(pN1->parentID); + if (!isPathCollisionFree((pN1->configuration),(pN2->configuration))) + { + return false; + } + } + } + return true; +} + +void CSpace::setBoundaries(const Eigen::VectorXf &min, const Eigen::VectorXf &max) +{ + SABA_ASSERT (min.rows()==dimension) + + for (unsigned int i=0;i<dimension;i++) + { + setBoundary(i,min[i],max[i]); + } +} + + +void CSpace::setBoundary(unsigned int dim, float min, float max) +{ + SABA_ASSERT (dim<dimension) + + if (min>max) + { + float t = min; + min = max; + max = t; + } + boundaryMin[dim] = min; + boundaryMax[dim] = max; + boundaryDist[dim] = fabs(max-min); + checkForBorderlessDimensions(checkForBorderlessDims); +} + +void CSpace::setMetricWeights(const Eigen::VectorXf &weights) +{ + SABA_ASSERT (weights.rows()==dimension) + + for (unsigned int i = 0; i < dimension; i++) + { + metricWeights[i] = weights[i]; + //std::cout << "Dim: " << i << " Weight: " << metricWeights[i] << std::endl; + } + if (sampleAlgorithm) + sampleAlgorithm->enableMetricWeights(metricWeights); + enableWeights(true); +} + +void CSpace::checkForBorderlessDimensions(bool enable) +{ + checkForBorderlessDims = enable; + borderLessDimension.resize(dimension); + for (unsigned int i=0;i<dimension;i++) + { + borderLessDimension[i] = enable?isBorderlessDimension(i):false; + } +} + +inline +float CSpace::calcDist(const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, bool forceDisablingMetricWeights) +{ + return sqrtf(calcDist2(c1,c2,forceDisablingMetricWeights)); +} + +inline +float CSpace::calcDist2(const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, bool forceDisablingMetricWeights) +{ + SABA_ASSERT (c1.rows()==dimension) + SABA_ASSERT (c2.rows()==dimension) + + float res = 0.0f; + float dist; + + for (unsigned int i = 0 ; i < dimension; i++) + { + dist = c1[i]-c2[i]; + + if (borderLessDimension[i]) + { + // borderless + if (fabs(dist) > M_PI) + dist = 2.0f * (float)M_PI - fabs(dist); + } + + if (useMetricWeights && !forceDisablingMetricWeights) + { + res += metricWeights[i]*metricWeights[i]*dist*dist; + } else + res += dist*dist; + } + + + return res; +} + + +void CSpace::generateNewConfig(const Eigen::VectorXf &randomConfig, const Eigen::VectorXf &nearestConfig, Eigen::VectorXf &storeNewConfig, float stepSize, float preCalculatedDist /*=-1.0*/) +{ + SABA_ASSERT (randomConfig.rows()==dimension) + SABA_ASSERT (nearestConfig.rows()==dimension) + SABA_ASSERT (storeNewConfig.rows()==dimension) + + if (preCalculatedDist<0) + preCalculatedDist = calcDist(randomConfig,nearestConfig); + + // distance is smaller than extendStepSize: set randomly found configuration + if (preCalculatedDist <= stepSize) + { + storeNewConfig = randomConfig; + return; + } + + // distance is greater than stepSize: go stepSize in direction of the randomly found configuration + float factor = stepSize / preCalculatedDist; + + for (int i = 0; i < (int)dimension; i++) + { + storeNewConfig[i] = interpolate(nearestConfig,randomConfig,i,factor); + } +} + +void CSpace::getDirectionVector(const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, Eigen::VectorXf &storeDir, float length) +{ + SABA_ASSERT (c1.rows()==dimension) + SABA_ASSERT (c2.rows()==dimension) + SABA_ASSERT (storeDir.rows()==dimension) + float l = calcDist(c1,c2); + if (l==0.0) + { + std::cout << "getDirectionVector:Identical vectors..." << std::endl; + for (unsigned int i=0;i<dimension;i++) + storeDir[i] = 0.1f; + return; + } + + float factor = length / l; + for (unsigned int i = 0; i < dimension; i++) + { + storeDir[i] = c1[i] - interpolate(c1,c2,i,factor); + } +} + + +float CSpace::calculateObstacleDistance(const Eigen::VectorXf &config) +{ + SABA_ASSERT (config.rows()==dimension) + + performaceVars_distanceCheck++; + + if (multiThreaded) + colCheckMutex.lock(); + + // set configuration (joint values) + + robotNodes->setJointValues(config); + float d = cdm->getDistance(); + + if (multiThreaded) + colCheckMutex.unlock(); + + return d; +} + +float CSpace::getDirectedMaxMovement(const Eigen::VectorXf &config, const Eigen::VectorXf &nextConfig) +{ + SABA_ASSERT (config.rows()==dimension) + SABA_ASSERT (nextConfig.rows()==dimension) + + SABA_WARNING << "NYI..." << endl; + // todo : oobb updates in VirtualRobot + return 0.0f; + + /*if (multiThreaded) + colCheckMutex.lock(); + + robotNodes->setJointValues(config); + + float directedFreeSpaceStep = 0.0f; + for (unsigned int i=0;i<dimension;i++) + { + if (robotJoints[i]->isRotationalJoint()) + directedFreeSpaceStep += robotJoints[i]->getMaxDistanceOOBB() * fabs(nextConfig[i] - config[i]); + else if (robotJoints[i]->isTranslationalJoint()) + directedFreeSpaceStep += fabs(nextConfig[i] - config[i]); + } + + if (multiThreaded) + colCheckMutex.unlock(); + + return directedFreeSpaceStep;*/ +} + + +bool CSpace::isCollisionFree( const Eigen::VectorXf &config ) +{ + SABA_ASSERT (config.rows()==dimension) + if (multiThreaded) + colCheckMutex.lock(); + + // set configuration (joint values) + robotNodes->setJointValues(config); + bool res = cdm->isInCollision(); + + if (multiThreaded) + colCheckMutex.unlock(); + + performaceVars_collisionCheck++; + return !res; +} + +void CSpace::lock() +{ + //if (multiThreaded) + colCheckMutex.lock(); +} + +void CSpace::unlock() +{ + //if (multiThreaded) + colCheckMutex.unlock(); +} + +bool CSpace::isInBoundary( const Eigen::VectorXf &config ) +{ + SABA_ASSERT (config.rows()==dimension) + // check boundaries + for (unsigned int i = 0; i<dimension; i++) + { + if (config[i]<boundaryMin[i] || config[i]>boundaryMax[i]) + { + return false; + } + } + return true; +} + +bool CSpace::isConfigValid( const Eigen::VectorXf &config ) +{ + SABA_ASSERT (config.rows()==dimension) + // check boundaries + if (!isInBoundary(config)) + return false; + + return isCollisionFree(config); +} + + +float CSpace::getBoundaryMin(unsigned int d) +{ + if (d>=dimension) + { + SABA_ERROR << "Error: getBoundaryMin: could not get boundary dist.." << std::endl; + return 0.0f; + } + return boundaryMin[d]; +} + + +float CSpace::getBoundaryMax(unsigned int d) +{ + if (d>=dimension) + { + SABA_ERROR << "Error: getBoundaryMax: could not get boundary dist.." << std::endl; + return 0.0f; + } + return boundaryMax[d]; +} + + +float CSpace::getBoundaryDist(unsigned int d) +{ + if (d>=dimension) + { + SABA_ERROR << "Error: getBoundaryDist: could not get boundary dist.." << std::endl; + return 0.0f; + } + return boundaryDist[d]; +} + + + +void CSpace::respectBoundaries(Eigen::VectorXf &config) +{ + SABA_ASSERT (config.rows()==dimension) + for (unsigned int i=0;i<dimension;i++) + { + if (config[i]<boundaryMin[i]) + config[i] = boundaryMin[i]; + if (config[i]>boundaryMax[i]) + config[i] = boundaryMax[i]; + } +} + + +// config values are not set! (except id) +CSpaceNodePtr CSpace::createNewNode() +{ + if (freeNodes.size()==0) + { + SABA_ERROR << " Could not create new nodes... (maxNodes exceeded:" << maxNodes << ")" << std::endl; + return CSpaceNodePtr(); + } + + // get free node + CSpaceNodePtr result = freeNodes.back(); + freeNodes.pop_back(); + + result->children.clear(); + result->allocated = true; + + return result; +} + +Eigen::VectorXf CSpace::interpolate(const Eigen::VectorXf &q1, const Eigen::VectorXf &q2, float step) +{ + SABA_ASSERT (q1.rows()==dimension) + SABA_ASSERT (q2.rows()==dimension) + Eigen::VectorXf res(dimension); + for (unsigned int i=0;i<dimension;i++) + res[i] = interpolate(q1,q2,i,step); + return res; +} + +inline +float CSpace::interpolate(const Eigen::VectorXf &q1, const Eigen::VectorXf &q2, int dim, float step) +{ + SABA_ASSERT (q1.rows()==dimension) + SABA_ASSERT (q2.rows()==dimension) + SABA_ASSERT_MESSAGE ( (dim>=0 && dim<(int)robotJoints.size()), "Dim " << dim << " out of bounds..." << endl) + + // translational joint + if (robotJoints[dim]->isTranslationalJoint()) + return interpolateLinear(q1[dim],q2[dim],step); + + // rotational joint + if (!borderLessDimension[dim]) + { + return interpolateLinear(q1[dim],q2[dim],step); + } else + { + // borderless mode + + float start = q1[dim] - robotJoints[dim]->getJointLimitLo(); + float end = q2[dim] - robotJoints[dim]->getJointLimitLo(); + float res = interpolateRotational(start, end ,step); + res = (float)fmod((double)res,2.0*M_PI); + res = res + robotJoints[dim]->getJointLimitLo(); + return res; + } + +} + +inline +float CSpace::interpolateLinear(float a, float b, float step) +{ + return (a + step * (b - a)); +} + +inline +float CSpace::interpolateRotational(float a, float b, float step) +{ + //return (a + step * (b - a)); + + float angle; + + if (fabs(a - b) < M_PI) + { + //std::cout << "interpolateRotational: If 1" << std::endl; + angle = interpolateLinear(a,b,step); + } + else if (a < b) + { + //std::cout << "interpolateRotational: If 2" << std::endl; + angle = a - step * (a + 2.0f * (float)M_PI - b); + + } + else + { + //std::cout << "interpolateRotational: If 3" << std::endl; + angle = a + step * (b + 2.0f * (float)M_PI - a); + } + + if (angle < 0) angle += 2.0f * (float)M_PI; + if (angle >= 2.0f * (float)M_PI) angle -= 2.0f * (float)M_PI; + + return angle; +} + + +float CSpace::getRandomConfig_UniformSampling(unsigned int dim) +{ + SABA_ASSERT (dim <= dimension) + + float res = (float)rand() * randMult; // value from 0 to 1 + res = boundaryMin[dim] + (boundaryDist[dim]*res); + return res; +} + +void CSpace::getRandomConfig(Eigen::VectorXf &config, bool checkCollisionFree) +{ + SABA_ASSERT (config.rows()==dimension) + + do + { + if (sampleAlgorithm) + { + sampleAlgorithm->sample(config, shared_from_this()); + } else + { + float t; + for (unsigned int i=0;i<dimension; i++) + { + t = (float)rand() * randMult; // value from 0 to 1 + config[i] = boundaryMin[i] + (boundaryDist[i]*t); + } + } + } while (checkCollisionFree && !isCollisionFree(config)); +} + + +void CSpace::printConfig(const Eigen::VectorXf &config) const +{ +/* std::cout << std::fixed << std::setprecision(3); + std::cout << "("; + for (unsigned int i=0; i< dimension; i++) + { + std::cout << config[i]; + if (i!=dimension-1) + std::cout << ", "; + } + std::cout << ")";*/ + if (config.rows()!=dimension) + { + SABA_ERROR << "Wrong dimensions..." << endl; + } + streamsize pr = cout.precision(2); + cout << "<"; + for (int i=0; i<config.rows(); i++) + { + cout << config[i]; + if (i!=config.rows()-1) + cout << ","; + } + cout << ">" << endl; + cout.precision(pr); +} + +void CSpace::exclusiveRobotAccess(bool bGranted) +{ + multiThreaded = bGranted; +} + +bool CSpace::hasExclusiveRobotAccess() +{ + return multiThreaded; +} + +/*int CSpace::getMaxConfigs() const +{ + return maxConfigs; +}*/ + +void CSpace::setRandomSampler( SamplerPtr sampler ) +{ + sampleAlgorithm = sampler; + enableWeights(useMetricWeights); // update sample class +} + +void CSpace::enableWeights( bool enable ) +{ + useMetricWeights = enable; + if (sampleAlgorithm) + { + if (enable) + sampleAlgorithm->enableMetricWeights(metricWeights); + else + sampleAlgorithm->disableMetricWeights(); + } +} + +VirtualRobot::RobotNodeSetPtr CSpace::getRobotNodeSet() const +{ + return robotNodes; +} + +void CSpace::removeNode( CSpaceNodePtr node ) +{ + SABA_ASSERT (node) + if (node->ID>=nodes.size()) + { + SABA_ERROR << " Could not remove node... (corrupt ID:" << node->ID << ")" << std::endl; + return; + } + node->allocated = false; + freeNodes.push_back(node); +} + +CSpacePathPtr CSpace::createPath( const Eigen::VectorXf &start, const Eigen::VectorXf &goal ) +{ + SABA_ASSERT (start.rows() == dimension) + SABA_ASSERT (goal.rows() == dimension) + CSpacePathPtr p(new CSpacePath(shared_from_this())); + p->addPathPoint(start); + p->addPathPoint(goal); + return p; +} + +Saba::CSpacePathPtr CSpace::createPathUntilCollision( const Eigen::VectorXf &start, const Eigen::VectorXf &goal, float &storeAddedLength ) +{ + SABA_ASSERT (start.rows() == dimension); + SABA_ASSERT (goal.rows() == dimension); + CSpacePathPtr p(new CSpacePath(shared_from_this())); + storeAddedLength = 0.5f; + p->addPathPoint(start); + if (isCollisionFree(goal)) + { + p->addPathPoint(goal); + storeAddedLength = 1.0f; + } + return p; +} + +bool CSpace::isPathCollisionFree( const Eigen::VectorXf &q1, const Eigen::VectorXf &q2 ) +{ + return isCollisionFree(q2); +} + + +bool CSpace::isBorderlessDimension(unsigned int dim) const +{ + SABA_ASSERT(dim<dimension) + if (!(robotJoints[dim]->isTranslationalJoint())) + { + // rotational joint + if (abs((double)(robotJoints[dim]->getJointLimitHi() - robotJoints[dim]->getJointLimitLo())) > (1.9999999999999 * M_PI) ) + return true; + } + return false; +} + +unsigned int CSpace::getDimension() const +{ + return dimension; +} + +VirtualRobot::RobotPtr CSpace::getRobot() const +{ + return robo; +} + +} diff --git a/MotionPlanning/CSpace/CSpace.h b/MotionPlanning/CSpace/CSpace.h new file mode 100644 index 0000000000000000000000000000000000000000..ae75442d77ff79c202e44891b4fd54369e4a0fe7 --- /dev/null +++ b/MotionPlanning/CSpace/CSpace.h @@ -0,0 +1,353 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _saba_cspace_h_ +#define _saba_cspace_h_ + +#include "../Saba.h" +#include "VirtualRobot/Robot.h" +#include "VirtualRobot/VirtualRobot.h" +#include "VirtualRobot/CollisionDetection/CDManager.h" +#include <iostream> +#include <vector> +#include <boost/thread.hpp> + +namespace Saba { + + +/*! + + \brief A c-space interface for motion planning. + + A CSpace is defined by a set of robot nodes and a collision manager. + The RobotNodeSet specifies the dimension and the borders of the CSpace. + The collision manager is used to check configurations for collisions. Here, + multiple sets of objects can be defined which are internally mutually checked for + collisions. This allows to specify complex collision queries (@see VirtualRobot::CDManager). + When performing sampling-based motion planning, usually paths in c-space have to be verified + if constraints are violated or collisions occur. Therefore this class provides an interface, + in order to combine various planning approaches with different collision detection methods + (@see MotionPlanner, Rrt, BiRrt). + + Please note: + This implementation is not able to determine the collision status of path segments. + Use CSpaceSampled for discrete collision detection (@see CSpaceSampled). + * + */ +class SABA_IMPORT_EXPORT CSpace : public boost::enable_shared_from_this<CSpace> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + + /*! + Construct a c-space that represents the given set of joints. + The dimensionality of this c-space is set by the number of nodes in robotNodes. + The boundaries of this c-space are set according to definitions in robotNodes. + On startup, memory is allocate din order to allow fats processing on runtime. + */ + CSpace(VirtualRobot::RobotPtr robot, VirtualRobot::CDManagerPtr collisionManager, VirtualRobot::RobotNodeSetPtr robotNodes, unsigned int maxConfigs = 50000, unsigned int randomSeed = 0); + + //! destructor + virtual ~CSpace(); + + /*! + Create a path from start to goal without any checks. + Intermediate configurations are added according to the current implementation of the cspace. + In this implementation only the start and goal config are added to the path. + */ + virtual CSpacePathPtr createPath(const Eigen::VectorXf &start, const Eigen::VectorXf &goal); + + /*! + Create a path from start to the goal configuration. In case a collision + without any checks. Intermediate configurations are added according to the current implementation of the cspace + In this implementation only the start and goal config are added to the path. + \param start The start configuration. + \param goal The goal configuration. + \param storeAddedLength The length of the collision free path is stored here (1.0 means the complete path from start to goal was collision-free) + */ + virtual CSpacePathPtr createPathUntilCollision(const Eigen::VectorXf &start, const Eigen::VectorXf &goal, float &storeAddedLength); + + + /*! + Set boundary values for each dimension of the cspace. + Only needed when other values needed than those defined in the corresponding robotNodes. + \param min minimum values (array has to have same dimension as the cspace) + \param max maximum values (array has to have same dimension as the cspace) + */ + void setBoundaries(const Eigen::VectorXf &min, const Eigen::VectorXf &max); + void setBoundary(unsigned int dim, float min, float max); + + /*! + Set weights to uniformly handle differing dimensions in c-space. + Setting the weights with this methods enables weighting of distance calculations in c-space. + This affects path-creation and distance calculations. + */ + void setMetricWeights(const Eigen::VectorXf &weights); + + + //! get minimum boundary of an index + float getBoundaryMin(unsigned int d); + //! get maximum boundary of an index + float getBoundaryMax(unsigned int d); + //! get boundary distance of an index + float getBoundaryDist(unsigned int d); + + bool isBorderlessDimension(unsigned int dim) const; + + //! get cspace dimension + unsigned int getDimension() const; + + + //! get the robot of cspace + VirtualRobot::RobotPtr getRobot() const; + + //! get the sets of robotnodes representing of cspace + VirtualRobot::RobotNodeSetPtr getRobotNodeSet() const; + + VirtualRobot::CDManagerPtr getCDManager() const; + + /*! + Returns a uniformly sampled random value for dimension dim. + */ + virtual float getRandomConfig_UniformSampling(unsigned int dim); + + /*! + This method returns a random configuration. The value is generated uniformly (standard) + or, in case a sampler is defined a custom sampling strategy can be used. + \param storeValues Store the values here. + \param checkCollisionFree When set to true, it is guaranteed that the sample not in collision + */ + void getRandomConfig(Eigen::VectorXf &storeValues, bool checkCollisionFree = false); + + //! set values of configuration considering boundaries + virtual void respectBoundaries(Eigen::VectorXf &config); + + /*! + Check path between two configurations. + This cspace just checks q2! + \param q1 first configuration (from) + \param q2 second configuration (to) + \return true if path is valid + */ + virtual bool isPathCollisionFree(const Eigen::VectorXf &q1, const Eigen::VectorXf &q2); + //virtual bool CheckPath(float *c1, float *c2, float* storePathLengthA = NULL, float* storePathLengthB = NULL) = 0; + + /*! + Method for externally stopping the path checking routines. + Only useful in multi-threaded planners. + */ + void requestStop(); + + //! get random seed + + unsigned int getRandomSeed() const {return randomSeed;} + + void setRandomSeed(unsigned int random_seed){randomSeed=random_seed;} + + //! checks complete solution path (with the pathCheck methods provided by the CSpace implementation) + virtual bool checkSolution(CSpacePathPtr path, bool verbose = false); + + //! checks complete tree (with the pathCheck methods provided by the CSpace implementation) + virtual bool checkTree(CSpaceTreePtr tree); + + void resetPerformanceVars() + { + performaceVars_collisionCheck = 0; + performaceVars_distanceCheck = 0; + } + + virtual void reset(); + + /*! + Returns array of size dimension. + */ + Eigen::VectorXf getMetricWeights(){return metricWeights;} + + + /*! + Enable/Disable weighting of c-space dimensions. (standard: disabled) + By setting the weights with setMetricWeights() the weighting is enabled. + \param enable When set, the metric weights of this c-space are used to compute distances (when no metric weights have been specified for this c-space the option has no effect). + */ + void enableWeights(bool enable); + + + /*! + Method to allow concurrent/parallel access on robot. + Enabling the multiThreading support is only needed when sharing robot or environment models + between multiple CSpaces and the collision detection is done in parallel. + If each CSpace operates on it's own instances of robot and environment, the multithreading support must + not be enabled. Also rrtBiPlanners do not need to enable these option. + If enabled, the robot is locked for collision checking, so that only one instance operates on the model. + Planners operate much faster when doing "real" parallel collision checking, meaning that multiple instances + of the models are created and these instances have their own collision checker. + \param bGranted When set to true, no mutex protection is used to access the robot and to perform the collision detection. (standard) + If set to false, the Robot and CD calls are protected by a mutex. + */ + void exclusiveRobotAccess(bool bGranted = true); + bool hasExclusiveRobotAccess(); + + + //! if multithreading is enabled, the colChecking mutex can be locked/unlocked externally + static void lock(); + //! if multithreading is enabled, the colChecking mutex can be locked/unlocked externally + static void unlock(); + + /*! + Clone this CSpace structure + The new Robot and the new CCM are used, the robot and the ccm have to be linked to the new ColChecker! + */ + virtual CSpacePtr clone(VirtualRobot::CollisionCheckerPtr newCollisionChecker, VirtualRobot::RobotPtr newRobot, VirtualRobot::CDManagerPtr newCDM, unsigned int nNewRandomSeed = 0) = 0; + + + /*! + In the standard setup no sampler is used and the method getRandomConfig() performs a uniformly sampling of random configurations. + Since most planners use the getRandomConfig() method for generating their random configurations (@see Rrt), + this method offers the possibility to exchange the sampling routine by a custom sample algorithm (e.g. a brideover-sampler or a + Gaussian sampler can be realized). + In case NULL is passed, the sampling routine is set back to the standard uniform sample algorithm. + */ + void setRandomSampler(SamplerPtr sampler); + + int performaceVars_collisionCheck; + int performaceVars_distanceCheck; + + /*! + Compute the distance in c-space. + The modes useMetricWeights (standard:disabled) and checkForBorderlessDims (stanbdard:enabled) may affect the results. + \see setMetricWeights + \see checkForBorderlessDimensions + + */ + float calcDist (const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, bool forceDisablingMetricWeights = false); + float calcDist2 (const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, bool forceDisablingMetricWeights = false); + + + //! calculate distance to obstacles + /*! + \param config The config for which the obstacle distance should be calculated (according to the cdm of this cspace) + */ + virtual float calculateObstacleDistance(const Eigen::VectorXf &config); + + CSpaceNodePtr getNode(unsigned int id); + + virtual CSpaceNodePtr createNewNode(); + virtual void removeNode(CSpaceNodePtr node); + + //! returns the collision status (true for a valid config) + virtual bool isCollisionFree(const Eigen::VectorXf &config); + + //! returns the boundary violation status (true for a valid config) + virtual bool isInBoundary(const Eigen::VectorXf &config); + + void printConfig(const Eigen::VectorXf &c) const; + + + /*! + When the size of a c-space dimension is > 2PI and the corresponding joint is rotaional, it is assumed that there are no borders + \param enable When set, for each c-space dimension the (rotational) corresponding joints are checked: + Borderless dimension, if the distance between Lo and Hi limit is >2PI + This is useful e.g. for free flying or holonomic moving robots. + Translational joints are not considered. + */ + void checkForBorderlessDimensions(bool enable); + + /*! + Interpolates between two values in dimension dim. + Checks weather the corresponding joint moves translational or a rotational and performs the interpolation accordingly. + When joint boundaries of a rotational joint are >= 2PI (and checkForBorderlessDimensions was not disabled), the correct direction for interpolating is determined automatically. + */ + float interpolate(const Eigen::VectorXf &q1, const Eigen::VectorXf &q2, int dim, float step); + Eigen::VectorXf interpolate(const Eigen::VectorXf &q1, const Eigen::VectorXf &q2, float step); + + + //! check whether a configuration is valid (collision check and boundary check) + virtual bool isConfigValid(const Eigen::VectorXf &pConfig); + +protected: + + + /*! + calculates the squared distance between two samples + depending on the useMetricWeights flag the euclidean dist or the weighted dist is returned + */ + //virtual float getDist2(const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, bool useMetricWeights); + + // gets direction vector from c1 to c2, with (weighted) length + virtual void getDirectionVector(const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, Eigen::VectorXf &storeDir, float length); + + virtual void generateNewConfig(const Eigen::VectorXf &randomConfig, const Eigen::VectorXf &nearestConfig, Eigen::VectorXf &storeNewConfig, float stepSize, float preCalculatedDist = -1.0); + + + // returns pointer to a not-used config array + //Eigen::VectorXf* createNewConfiguration(); + + //! return upper limit for movement of any point on joints if moving from config to nextConfig + virtual float getDirectedMaxMovement(const Eigen::VectorXf &config, const Eigen::VectorXf &nextConfig); + + static int cloneCounter; + + //! interpolates linear between a and b using step as step size + float interpolateLinear(float a, float b, float step); + + //! interpolates rotational between a and b using step as step size + float interpolateRotational(float a, float b, float step); + + unsigned int dimension; //!< dimension of this c-space + + Eigen::VectorXf boundaryMax, boundaryMin, boundaryDist; //!< boundaries of this c-space + + Eigen::VectorXf metricWeights; //!< weights for distance computation + + bool stopPathCheck; + + VirtualRobot::RobotPtr robo; //!< the robot for collision checking + VirtualRobot::RobotNodeSetPtr robotNodes; //!< the robot nodes defining the c-space + + VirtualRobot::CDManagerPtr cdm; //!< handling of collision detections + + + int maxNodes; + std::vector< CSpaceNodePtr > nodes; //! vector with pointers to really used nodes + std::vector< CSpaceNodePtr > freeNodes; //! vector with pointers to free (not used) nodes + + std::vector<VirtualRobot::RobotNodePtr> robotJoints; //!< joints of the robot that we are manipulating + + unsigned int randomSeed; + + float randMult; + bool useMetricWeights; + bool checkForBorderlessDims; + std::vector< bool > borderLessDimension; // store borderless state + + bool multiThreaded; // indicates that more than one CSpace is used by some threads + static boost::mutex colCheckMutex; // only needed when multithreading support is enabled + // -> setting the configurations and checking against collisions is protected by this mutex + + + SamplerPtr sampleAlgorithm; // standard is NULL (uniformly sampling), is used in getRandomConfig() +}; + +} // namespace + +#endif // _saba_cspace_h diff --git a/MotionPlanning/CSpace/CSpaceNode.cpp b/MotionPlanning/CSpace/CSpaceNode.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7a21d840c452b07ebd2a72787faa26aacc2beed1 --- /dev/null +++ b/MotionPlanning/CSpace/CSpaceNode.cpp @@ -0,0 +1,15 @@ + +#include "CSpaceNode.h" + +namespace Saba +{ + +CSpaceNode::CSpaceNode() +{ +} + +CSpaceNode::~CSpaceNode() +{ +} + +} diff --git a/MotionPlanning/CSpace/CSpaceNode.h b/MotionPlanning/CSpace/CSpaceNode.h new file mode 100644 index 0000000000000000000000000000000000000000..67788769ca4c1bf4e1732876064137707f173766 --- /dev/null +++ b/MotionPlanning/CSpace/CSpaceNode.h @@ -0,0 +1,63 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_CSpaceNode_h +#define _Saba_CSpaceNode_h + +#include "../Saba.h" +#include <vector> +#include <Eigen/Core> +#include <Eigen/Geometry> + +namespace Saba { + +/*! + * + * \class CSpaceNode + * + * A CSpaceNode is used to store a configuration in cspace. + * + * @see CSpaceTree + * + */ +class SABA_IMPORT_EXPORT CSpaceNode +{ +public: + + CSpaceNode(); + virtual ~CSpaceNode(); + + Eigen::VectorXf configuration; //!< the configuration vector + int parentID; //!< id of parent (root node if < 0) + unsigned int ID; //!< id of CSpaceNode + + bool allocated; + + // optional + float obstacleDistance; //!< work space distance to obstacles (-1 if the dist was not calculated) + float dynDomRadius; //!< radius for this node (used by dynamic domain RRTs) + std::vector<CSpaceNodePtr> children; //!< children of this node +}; + +} // nameaspace + +#endif // _Saba_CSpaceNode_h diff --git a/MotionPlanning/CSpace/CSpacePath.cpp b/MotionPlanning/CSpace/CSpacePath.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8b2028e11ee647a3f3f9f6f4109d506854cd9a4d --- /dev/null +++ b/MotionPlanning/CSpace/CSpacePath.cpp @@ -0,0 +1,531 @@ + +#include "CSpacePath.h" +#include <algorithm> +#include <iostream> +#include <fstream> +using namespace std; + +namespace Saba +{ + +CSpacePath::CSpacePath(CSpacePtr cspace) +{ + this->cspace = cspace; + if (!cspace) + THROW_SABA_EXCEPTION ("No cpsace..."); + + dimension = cspace->getDimension(); +} + +CSpacePath::~CSpacePath() +{ + reset(); +} + +void CSpacePath::reset() +{ + path.clear(); +} + +void CSpacePath::addPathPoint(const Eigen::VectorXf &c) +{ + SABA_ASSERT (c.rows() == dimension) + path.push_back(c); +} + +unsigned int CSpacePath::getNrOfPathPoints() const +{ + return (unsigned int)path.size(); +} + + +Eigen::VectorXf CSpacePath::getPathEntry( unsigned int nr ) const +{ + if (nr>=path.size()) + { + SABA_ERROR << "CSpacePath::getPathEntry: " << nr << " >= " << (unsigned int)path.size() << std::endl; + if (path.size()>0) + return path[path.size()-1]; + else + { + Eigen::VectorXf x(dimension); + x.setZero(dimension); + return x; + } + } + return path[nr]; +} + +CSpacePathPtr CSpacePath::clone() const +{ + CSpacePathPtr res(new CSpacePath(cspace)); + for (unsigned int i=0;i<getNrOfPathPoints();i++) + { + res->addPathPoint(getPathEntry(i)); + } + return res; +} + +CSpacePathPtr CSpacePath::createSubPath(unsigned int startIndex, unsigned int endIndex) const +{ + if (startIndex>=getNrOfPathPoints() || endIndex>=getNrOfPathPoints()) + { + SABA_ERROR << "CSpacePath::createSubPath: wrong start or end pos" << std::endl; + return CSpacePathPtr(); + } + CSpacePathPtr res(new CSpacePath(cspace)); + for (unsigned int i=startIndex;i<=endIndex;i++) + { + res->addPathPoint(getPathEntry(i)); + } + return res; +} + +/* +bool CSpacePath::movePosition(unsigned int pos, const Eigen::VectorXf &moveVector, int sideSteps) +{ + if (pos>=getNrOfPathPoints()) + { + SABA_ERROR << "CSpacePath::movePosition: wrong pos" << std::endl; + return false; + } + if (moveVector==NULL) + { + SABA_ERROR << "CSpacePath::movePosition: NULL moveVec" << std::endl; + return false; + } + + Eigen::VectorXf c = getPathEntry(pos); + + for (unsigned int i=0;i<dimension;i++) + { + c[i] += moveVector[i]; + } +/* + // apply part of moveVector to sideSteps positions next to pos + if (sideSteps>0 && dimension>0) + { + float *tmp = new float[dimension]; + memcpy (tmp,moveVector,sizeof(float)*dimension); + int posA,posB; + float *cA,*cB; + + for (int j=1; j<=sideSteps; j++) + { + posA = pos - j; + posB = pos + j; + + // half moveVector + for (unsigned int i=0;i<dimension;i++) + { + tmp[i] *= 0.5f; + } + + // left + if (posA>=0) + { + cA = GetPathEntry(posA); + for (unsigned int i=0;i<dimension;i++) + { + cA[i] += tmp[i]; + } + } + + // right + if (posB<(int)getNrOfPathPoints()) + { + cB = GetPathEntry(posB); + for (unsigned int i=0;i<dimension;i++) + { + cB[i] += tmp[i]; + } + } + + + } // j + delete[] tmp; + } // sidesteps +*/ + + + +float CSpacePath::getPathLength(bool useMetricWeights) const +{ + return getPathLength(0,(int)path.size()-1, useMetricWeights); +} + +// be careful, this method calculates the c-space length of the path, not the workspace length! +float CSpacePath::getPathLength(unsigned int startIndex, unsigned int endIndex, bool forceDisablingMetricWeights) const +{ + if (endIndex<startIndex || endIndex>=path.size()) + { + SABA_ERROR << "CSpacePath::getPathLength: wrong index..." << std::endl; + return 0.0f; + } + float pathLength = 0.0f; + Eigen::VectorXf c1,c2; + float l; + for (unsigned int i=startIndex;i<endIndex;i++) + { + c1 = path[i]; + c2 = path[i+1]; + + l = cspace->calcDist(c1, c2, forceDisablingMetricWeights); + pathLength += l; + } + return pathLength; +} + +bool CSpacePath::getPathEntries(unsigned int start, unsigned int end , std::vector<Eigen::VectorXf> &storePosList) const +{ + if (start > end || end>=path.size()) + { + SABA_ERROR << "CSpacePath::getPathEntries: wrong start or end.." << std::endl; + return false; + } + unsigned int i = start; + Eigen::VectorXf data; + while (i<=end) + { + data = getPathEntry(i); + storePosList.push_back(data); + i++; + } + return true; +} + + +void CSpacePath::erasePosition(unsigned int pos) +{ + if (pos>=path.size()) + { + SABA_ERROR << "CSpacePath::erasePosition: pos not valid ?!" << std::endl; + return; + } + + std::vector<Eigen::VectorXf>::iterator iter = path.begin(); + iter += pos; + + path.erase( iter ); +} + + +unsigned int CSpacePath::removePositions(unsigned int startPos, unsigned int endPos) +{ + if (startPos>=path.size() || endPos>=path.size() ) + { + SABA_ERROR << "CSpacePath::removePositions: pos not valid ?!" << std::endl; + return 0; + } + if (startPos>endPos) + return 0; + + std::vector<Eigen::VectorXf>::iterator iter = path.begin(); + iter += startPos; + unsigned int result = 0; + for (unsigned int i=startPos; i<=endPos; i++) + { + if (iter==path.end()) + { + SABA_ERROR << "Internal error in CSpacePath::removePositions..." << std::endl; + return result; + } + //delete *iter; + iter = path.erase( iter ); + result++; + } + return result; +} + +// inserts at position before pos +void CSpacePath::insertPosition(unsigned int pos, const Eigen::VectorXf &c) +{ + if (pos > path.size()) + { + std::cout << "CSpacePath::insertPosition: pos not valid ?!" << std::endl; + return; + } + + // copy config and insert it + std::vector<Eigen::VectorXf>::iterator iter = path.begin(); + iter += pos; + + path.insert( iter, c ); +} + +void CSpacePath::insertPosition(unsigned int pos, std::vector<Eigen::VectorXf> &newConfigurations) +{ + std::vector<Eigen::VectorXf>::iterator iter = newConfigurations.begin(); + while (iter!=newConfigurations.end()) + { + insertPosition(pos,*iter); + pos++; + iter++; + } +} + +void CSpacePath::insertPath(unsigned int pos, CSpacePathPtr pathToInsert) +{ + if (!pathToInsert) + { + SABA_ERROR << "null data" << endl; + return; + } + int i = pathToInsert->getNrOfPathPoints()-1; + while (i >= 0) + { + insertPosition(pos,pathToInsert->getPathEntry(i)); + i--; + } +} +/* +int CSpacePath::checkIntermediatePositions(unsigned int startPos, const float *samplingDist) +{ + + unsigned int i = 0; + if (startPos>= path.size()-1) + { + std::cout << "CSpacePath::checkIntermediatePositions: wrong position" << std::endl; + return 0; + } + int nrOfNewPos = 0; + + // compute difference of each dimension + float *diff = new float[dimension]; + MathHelpers::calcDiff(path[startPos],path[startPos+1],diff,dimension); + + // compute intermediate step for each dimension, search for maximum step in all dimensions + int *step = new int[dimension]; + int maxStep = -1; + bool intermediateStepsNecessary = false; + for (i=0;i<dimension;i++) + { + if (fabs(diff[i])>samplingDist[i]) intermediateStepsNecessary = true; // intermediate steps necessary! + step[i] =(int)(floor(fabs(diff[i] / samplingDist[i]))); + if (maxStep<step[i]) maxStep = step[i]; + } + + float *actPos = new float[dimension]; + float *goalPos = new float[dimension]; + memcpy(actPos,path[startPos],sizeof(float)*dimension); // copy start pos + memcpy(goalPos,path[startPos+1],sizeof(float)*dimension); // copy goal pos + + if (intermediateStepsNecessary) + { + // apply these maxSteps and create new path configurations except for path[startPos+1] + for (i=1;(int)i<maxStep;i++) + { + float factor = (float)i / (float)maxStep; + for (unsigned int j=0;j<dimension;j++) + { + actPos[j] = path[startPos][j] + (goalPos[j] - path[startPos][j]) * factor;// go on one step + } + insertPosition(startPos+i,actPos); + nrOfNewPos++; + } + } + + // finally create a new path configuration to hold path[startPos+1] + for (unsigned int j=0;j<dimension;j++) + { + actPos[j] = goalPos[j]; // go on one step + } + insertPosition(startPos+i,actPos); + nrOfNewPos++; + + delete diff; + delete step; + delete actPos; + delete goalPos; + + return nrOfNewPos; +} + +// returns number of added nodes +int CSpacePath::checkIntermediatePositions(unsigned int startPos, float samplingDist, CSpaceSampled *cspace) +{ + if (cspace==NULL) + return 0; + + float dist = 0.0f; + unsigned int i; + int newConfigs = 0; + dist = cspace->getDist(path[startPos],path[startPos+1]);//MathHelpers::calcWeightedDist(startNode->configuration,config,m_metricWeights,m_nTransDim,m_nRotDim,dimension); + if (dist==0.0f) + { + // nothing to do + //std::cout << "append path: zero dist to new config!" << std::endl; + return 0; + } + + float *lastConfig = path[startPos]; + float *config = path[startPos+1]; + float *actPos = new float[dimension]; + + while (dist>samplingDist) + { + float factor = samplingDist / dist; + // create a new node with config, store it in nodeList and set parentID + + // copy values + for (i=0; i<dimension;i++) + { + actPos[i] = cspace->interpolate(lastConfig,config,i,factor);// lastConfig[i] + (config[i] - lastConfig[i])*factor; + } + + newConfigs++; + insertPosition(startPos+newConfigs,actPos); + lastConfig = actPos; + dist = cspace->getDist(actPos,config);//MathHelpers::calcWeightedDist(newNode->configuration,config,m_metricWeights,m_nTransDim,m_nRotDim,dimension); + } + delete[] actPos; + return newConfigs; +} +*/ + +float CSpacePath::getTime(unsigned int nr) +{ + if (getNrOfPathPoints()==0) + { + SABA_ERROR << " no Path.." << std::endl; + } + if(nr < 0 || nr > getNrOfPathPoints()-1) + { + SABA_ERROR << " path entry " << nr << " doesnt exist" << std::endl; + if(nr < 0) nr = 0; + if(nr > getNrOfPathPoints()-1) nr = getNrOfPathPoints() - 1; + } + + float t = 0.0f; + + float l = getPathLength(); + + Eigen::VectorXf c1 = getPathEntry(0); + + for(unsigned int i = 0; i < nr; i++) + { + Eigen::VectorXf c2 = getPathEntry(i+1); + t += cspace->calcDist(c1,c2)/l; + //t += MathHelpers::calcDistRotational(c1, c2, dimension, m_rotationalDimension)/l; + c1 = c2; + } + + return t; +} + +// returns position on path for time t (0<=t<=1) +void CSpacePath::interpolatePath( float t, Eigen::VectorXf &storePathPos, int *storeIndex /*= NULL*/ ) const +{ + storePathPos.resize(dimension); + if (t<0 || t>1.0f) + { + // check for rounding errors + if (t<-0.000000001 || t>1.000001f) + { + std::cout << "CSpacePath::interpolatePath: need t value between 0 and 1... (" << t << ")" << std::endl; + } + if (t<0) + t = 0.0f; + if (t>1.0f) + t = 1.0f; + } + + if (getNrOfPathPoints()==0) + { + SABA_WARNING << "CSpacePath::interpolatePath: no Path.." << std::endl; + } + if (t==0.0f) + { + storePathPos = getPathEntry(0); + if (storeIndex!=NULL) + *storeIndex = 0; + return; + } + else if (t==1.0f) + { + storePathPos = getPathEntry(getNrOfPathPoints()-1); + if (storeIndex!=NULL) + *storeIndex = (int)path.size(); + return; + } + + + float l = getPathLength(); + float wantedLength = l * t; + float actLength = 0.0f; + unsigned int startIndex = 0; + Eigen::VectorXf c1 = getPathEntry(startIndex); + Eigen::VectorXf c2 = c1; + float lastLength = 0.0f; + // search path segment for wantedLength + while (actLength < wantedLength && startIndex<getNrOfPathPoints()-1) + { + c1 = c2; + startIndex++; + c2 = getPathEntry(startIndex); + lastLength = cspace->calcDist(c1,c2); + //lastLength = MathHelpers::calcDistRotational(c1, c2, dimension, m_rotationalDimension); + actLength += lastLength; + } + startIndex--; + actLength-=lastLength; + + // segment starts with startIndex + float restLength = wantedLength - actLength; + + float factor = 0.0f; + if (lastLength>0) + factor = restLength / lastLength; + if (factor>1.0f) + { + // ignore rounding errors + factor = 1.0f; + } + for (unsigned int j=0;j<dimension;j++) + { + if (cspace->isBorderlessDimension(j)) + { + float diff = (c2[j] - c1[j]); + if (diff>M_PI) + diff -= (float)M_PI*2.0f; + if (diff<-M_PI) + diff += (float)M_PI*2.0f; + storePathPos[j] = c1[j] + diff * factor; // storePos = startPos + factor*segment + } else + { + storePathPos[j] = c1[j] + (c2[j] - c1[j]) * factor; // storePos = startPos + factor*segment + } + } + if (storeIndex!=NULL) + *storeIndex = startIndex; + +} + +void CSpacePath::reverse() +{ + std::reverse(path.begin(), path.end()); +} + +void CSpacePath::print() const +{ + std::cout << "<CSpacePath size='" << path.size() << "' dim='" << dimension << "'>" << std::endl << std::endl; + for (unsigned int i = 0; i < path.size(); i++) + { + std::cout << "\t<Node id='" << i << "'>" << std::endl; + Eigen::VectorXf c = path[i]; + for (unsigned int k = 0; k < dimension; k++) + { + std::cout << "\t\t<c value='" << c[k] << "'/>" << std::endl; + } + std::cout << "\t</Node>" << std::endl << std::endl; + } + + std::cout << "</CSpacePath>" << std::endl; +} + +const std::vector <Eigen::VectorXf>& CSpacePath::getPathData() const +{ + return path; +} + +} // namespace Saba diff --git a/MotionPlanning/CSpace/CSpacePath.h b/MotionPlanning/CSpace/CSpacePath.h new file mode 100644 index 0000000000000000000000000000000000000000..6d161511b2fe6faf47af8082075265512c984e27 --- /dev/null +++ b/MotionPlanning/CSpace/CSpacePath.h @@ -0,0 +1,151 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _saba_CSpacePath_h +#define _saba_CSpacePath_h + +#include "../Saba.h" +#include <vector> +#include <string> + +#include "CSpace.h" + +namespace Saba { + +/*! + * + * \brief A path in c-space. + * + */ +class SABA_IMPORT_EXPORT CSpacePath +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + Constructor. Dimension of this path is retrieved from cspace. + \param cspace Is used to retrieve the dimension of the corresponding c-space and the corresponding joints + */ + CSpacePath(CSpacePtr cspace); + + //! Destructor + virtual ~CSpacePath(); + + /*! + Insert a configuration as one point to the solution path. + \param c configuration vector + */ + void addPathPoint(const Eigen::VectorXf &c); + + /*! + Get a point in solution path with a index. + \param nr index to point in path + */ + Eigen::VectorXf getPathEntry(unsigned int nr) const; + + //! return total number of path points + unsigned int getNrOfPathPoints() const; + + //! to retrieve entries of path + bool getPathEntries(unsigned int start, unsigned int end , std::vector<Eigen::VectorXf> &storePosList) const; + + /*! + Creates a copy of the path instance. + \return pointer to new instance (copy) + */ + CSpacePathPtr clone() const; + + CSpacePathPtr createSubPath(unsigned int startIndex, unsigned int endIndex) const; + + //! reset all data + virtual void reset(); + + //! reverse position order: end becomes start + virtual void reverse(); + + /*! + Return euclidean c-space length of complete path + \param forceDisablingMetricWeights When disabled, the metricWeights that might have been specified in the corresponding c-space + are not used for computing the path length. + */ + float getPathLength(bool forceDisablingMetricWeights = false) const; + + /*! + Return length of part of the path (in CSpace!) + \param startIndex The start point + \param endIndex The end point + \param useMetricWeights When set, the metricWeights that have been specified in the corresponding c-space are used for computing the path length. + */ + float getPathLength(unsigned int startIndex, unsigned int endIndex, bool useMetricWeights = false) const; + + /*! + Erase a point in path. + \param pos position of point in path array + */ + virtual void erasePosition(unsigned int pos); + + /*! + Erases all points from start to end in path (including start&end). + \param startPos start position of point in path array + \param endPos end position of point in path array + */ + virtual unsigned int removePositions(unsigned int startPos, unsigned int endPos); + + /*! + Insert a point into path. + \param pos position of point being inserted + \param c configuration / valid joint values to insert as a point in solution path + */ + virtual void insertPosition(unsigned int pos, const Eigen::VectorXf &c); + virtual void insertPosition(unsigned int pos, std::vector<Eigen::VectorXf> &newConfigurations); + virtual void insertPath(unsigned int pos, CSpacePathPtr pathToInsert); + + + /*! + return position on path for time t (0<=t<=1) + If storeIndex!=NULL the index of the last path point is stored + */ + virtual void interpolatePath(float t, Eigen::VectorXf &storePos, int *storeIndex = NULL) const; + + //! return time t (0<=t<=1) for path entry with number nr + virtual float getTime(unsigned int nr); + + unsigned int getDimension() const {return dimension;} + + + //! prints path contents to console + virtual void print() const; + + //! For quick access to data. + const std::vector <Eigen::VectorXf>& getPathData() const; + +protected: + + std::vector <Eigen::VectorXf> path; //!< vector with configurations which represent the path + unsigned int dimension; //!< dimension of rrt space + CSpacePtr cspace; +}; + +} // namespace Saba + + +#endif // _saba_CSpacePath_h diff --git a/MotionPlanning/CSpace/CSpaceSampled.cpp b/MotionPlanning/CSpace/CSpaceSampled.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4e4fca60f8468d02fa439fe39aaea46d692ad70e --- /dev/null +++ b/MotionPlanning/CSpace/CSpaceSampled.cpp @@ -0,0 +1,239 @@ + +#include "CSpaceSampled.h" +#include "CSpaceNode.h" +#include "CSpacePath.h" +#include "CSpaceTree.h" +#include "VirtualRobot/Robot.h" +//#include "MathHelpers.h" +#include <cmath> +#include "float.h" +#include <iostream> +#include <string> + +using namespace std; + +//#define LOCAL_DEBUG(a) {SABA_INFO << a;}; +#define LOCAL_DEBUG(a) + +//#define DO_THE_TESTS + +// if changing this value, be sure to update CSpaceVariableDim +//#define RECURSIVE_DEPTH 1000 +namespace Saba +{ + +CSpaceSampled::CSpaceSampled(VirtualRobot::RobotPtr robot, VirtualRobot::CDManagerPtr collisionManager, VirtualRobot::RobotNodeSetPtr robotNodes, unsigned int maxConfigs, unsigned int randomSeed) +:CSpace(robot, collisionManager, robotNodes, maxConfigs, randomSeed), +recursionMaxDepth(1000) +{ + recursiveTmpValuesIndex = 0; + samplingSizePaths = 0.1f; + samplingSizeDCD = 0.1f; + SABA_ASSERT (dimension!=0); + + checkPathConfig.setZero(dimension); + tmpConfig.setZero(dimension); + for (int i=0;i<recursionMaxDepth;i++) + recursiveTmpValues.push_back(tmpConfig); +} + + +CSpaceSampled::~CSpaceSampled() +{ +} + +CSpacePtr CSpaceSampled::clone(VirtualRobot::CollisionCheckerPtr newColChecker, VirtualRobot::RobotPtr newRobot, VirtualRobot::CDManagerPtr newCDM, unsigned int newRandomSeed) +{ + cloneCounter++; + SABA_ASSERT (newColChecker && newRobot && newCDM && newRobot->getRootNode()); + + if (newRobot->getRootNode()->getCollisionChecker() != newColChecker) + { + SABA_ERROR << ": Collision Checker instances do not match..." << endl; + return CSpaceSampledPtr(); + } + + CSpaceSampledPtr result(new CSpaceSampled(newRobot,newCDM,robotNodes,newRandomSeed)); + result->setBoundaries(boundaryMin,boundaryMax); + if (useMetricWeights) + result->setMetricWeights(metricWeights); + + // sampled cspace + result->setSamplingSize(getSamplingSize()); + result->setSamplingSizeDCD(getSamplingSizeDCD()); + + // todo + return (CSpacePtr)result; +} + +void CSpaceSampled::setSamplingSize( float fSize ) +{ + //std::cout << "Setting sampling step size to " << samplingSize << std::endl; + samplingSizePaths = fSize; +} + +void CSpaceSampled::setSamplingSizeDCD( float fSize ) +{ + //std::cout << "Setting col check sampling step size to " << samplingSize << std::endl; + samplingSizeDCD = fSize; +} + + +CSpacePathPtr CSpaceSampled::createPath( const Eigen::VectorXf &start, const Eigen::VectorXf &goal ) +{ + SABA_ASSERT (start.rows() == dimension); + SABA_ASSERT (goal.rows() == dimension); + CSpacePathPtr p(new CSpacePath(shared_from_this())); + p->addPathPoint(start); + + float dist = 0.0f; + dist = calcDist (start,goal); + + if (dist==0.0f) + { + // nothing to do + return p; + } + + Eigen::VectorXf lastConfig = start; + float samplingSize = getSamplingSize(); + bool endLoop = false; + bool lastNode = false; + float factor; + + if (dist<samplingSize) + lastNode = true; + do + { + if (lastNode) + { + lastConfig = goal; + } else + { + factor = samplingSize / dist; + lastConfig = interpolate(lastConfig,goal,factor); + } + p->addPathPoint(lastConfig); + dist = calcDist (lastConfig,goal); + if (dist<samplingSize) + { + if (lastNode || dist<1e-10) + endLoop = true; + lastNode = true; + } + + } while (!endLoop); + return p; +} + +Saba::CSpacePathPtr CSpaceSampled::createPathUntilCollision( const Eigen::VectorXf &start, const Eigen::VectorXf &goal, float &storeAddedLength) +{ + SABA_ASSERT (start.rows() == dimension); + SABA_ASSERT (goal.rows() == dimension); + CSpacePathPtr p(new CSpacePath(shared_from_this())); + p->addPathPoint(start); + LOCAL_DEBUG ("Start:" << endl << start << endl); + LOCAL_DEBUG ("Goal:" << endl << goal << endl); + storeAddedLength = 0.0f; + + + float dist = 0.0f; + dist = calcDist (start,goal); + float origDist = dist; + + if (dist==0.0f) + { + // nothing to do + storeAddedLength = 1.0f; + return p; + } + + Eigen::VectorXf lastConfig = start; + + // init tmp values + tmpConfig = start; + + float nodeDist = 0.0f; + float colCheckDist = getSamplingSizeDCD(); + float samplingSize = getSamplingSize(); + bool endLoop = false; + bool lastNode = false; + + do + { + if (dist<=colCheckDist) + { + LOCAL_DEBUG ("END" << endl); + if (!(isCollisionFree(goal))) + { + LOCAL_DEBUG ("not col free" << endl); + storeAddedLength = (origDist - dist) / origDist; + LOCAL_DEBUG ("not col free, length: " << storeAddedLength << endl); + return p; + } + LOCAL_DEBUG ("col free" << endl); + storeAddedLength = 1.0f; + p->addPathPoint(goal); + endLoop = true; + } else + { + // check node + LOCAL_DEBUG ("step from " << endl << tmpConfig << endl); + generateNewConfig(goal,tmpConfig,tmpConfig,colCheckDist,dist); + LOCAL_DEBUG ("--> " << endl << tmpConfig << endl); + if (!(isCollisionFree(tmpConfig))) + return p; + nodeDist += colCheckDist; + if (nodeDist>=samplingSizePaths) + { + // create a new node with config, store it in nodeList and set parentID + p->addPathPoint(tmpConfig); + storeAddedLength = (origDist - dist) / origDist; + LOCAL_DEBUG ("AddPathPoint (length: " << storeAddedLength << ") " <<endl << tmpConfig << endl); + nodeDist -= samplingSizePaths; + LOCAL_DEBUG ("AddPathPoint (nodeDist: " << nodeDist << ")" << endl); + } + float distT = calcDist (tmpConfig,goal); + SABA_ASSERT (distT < dist); + dist = distT; + } + + } while (!endLoop); + + return p; +} + + +bool CSpaceSampled::isPathCollisionFree( const Eigen::VectorXf &q1, const Eigen::VectorXf &q2 ) +{ + if (stopPathCheck) + return false; + // actual weighted distance for collision checking + float actualWeightedDistance = calcDist(q1,q2); + + if(actualWeightedDistance <= samplingSizeDCD*1.001f) + { + // just checking q2, to avoid double checks + return isCollisionFree(q2); // assuming that q1 and q2 are within the limits of the CSpace + } + else + { + recursiveTmpValuesIndex++; + if (recursiveTmpValuesIndex>=recursionMaxDepth) + { + SABA_WARNING << "Recursion depth too high: " << recursiveTmpValuesIndex << std::endl; + recursiveTmpValuesIndex--; + return false; + } + Eigen::VectorXf &middleConfiguration = recursiveTmpValues[recursiveTmpValuesIndex]; + + // generate a configuration exactly in the middle of q1 and q2 using weighted distances + generateNewConfig(q2,q1,middleConfiguration,actualWeightedDistance*0.5f,actualWeightedDistance); + + bool r = (isPathCollisionFree(q1,middleConfiguration) && isPathCollisionFree(middleConfiguration,q2)); + recursiveTmpValuesIndex--; + return r; + } +} + +} // Saba diff --git a/MotionPlanning/CSpace/CSpaceSampled.h b/MotionPlanning/CSpace/CSpaceSampled.h new file mode 100644 index 0000000000000000000000000000000000000000..ef36e289ebd64cfe77bf2c26d4cd4a379cc1b4ac --- /dev/null +++ b/MotionPlanning/CSpace/CSpaceSampled.h @@ -0,0 +1,123 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_CSpaceSampled_h +#define _Saba_CSpaceSampled_h + +#include "../Saba.h" +#include "CSpace.h" +#include <string> + + + +namespace Saba { + +/*! + + \brief This class represents a sampled-based configuration space. This is the main class for RRT-related planning. + + A CSpace is defined by a set of robot nodes and a collision manager. + The RobotNodeSet specifies the dimension and the borders of the CSpace. + The collision manager is used to check configurations for collisions. Here, + multiple sets of objects can be defined which are internally mutually checked for + collisions. This allows to specify complex collision queries (@see VirtualRobot::CDManager). + The sampling-based c-space relies on two parameters: The sampling size, which specifies + the (c-space) distance between two succeeding configurations on a path segment. This + parameter affects how many intermediate configurations are added when creating new paths. + The second parameter is called DCD sampling size, where DCD stands for discrete collision detection. + This parameter is needed for the check if a path segment is valid or not. Therefore intermediate + configurations on the path segment are checked for collisions with the given maximum distance between two + neighboring intermediate configuration. This parameter affects both, the performance (the Rrt approach spends + most of the time for collision detection) and the reliability of the results. Since there is no guarantee that + all potential configurations are detected with sampling-based approaches, a large DCD sampling parameter + will increase the chance of missing a collision. + + @see Rrt + @see BiRrt + + */ +class SABA_IMPORT_EXPORT CSpaceSampled : public CSpace +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + Construct a c-space that represents the given set of joints. The dimension of this + c-space is related to the number of joints in robotNodes. + The boundaries of this c-space are set according to definitions in robotNodes. + */ + CSpaceSampled(VirtualRobot::RobotPtr robot, VirtualRobot::CDManagerPtr collisionManager, VirtualRobot::RobotNodeSetPtr robotNodes, unsigned int maxConfigs = 50000, unsigned int randomSeed = 0); + + virtual ~CSpaceSampled(); + + //! sets sampling step size (used when adding new paths in CSpace) + void setSamplingSize(float fSize); + //! sets sampling step size used for discrete collision checking + void setSamplingSizeDCD(float fSize); + + + float getSamplingSize(){ return samplingSizePaths; }; + float getSamplingSizeDCD(){ return samplingSizeDCD; }; + + + + virtual CSpacePtr clone(VirtualRobot::CollisionCheckerPtr newColChecker, VirtualRobot::RobotPtr newRobot, VirtualRobot::CDManagerPtr newCDM, unsigned int newRandomSeed = 0); + + /*! + Checks the middle configuration and when it is not in collision the path is split + and both parts are recursively checked until the distance is smalled than the DCD sampling size. + Recursion is performed maximal recursionMaxDepth times, since an array of temporary variables is used, + in order to avoid slow allocating/deallocating of memory. + */ + bool isPathCollisionFree( const Eigen::VectorXf &q1, const Eigen::VectorXf &q2 ); + + /*! + Create a path from start to goal without any checks. + Intermediate configurations are added according to the current implementation of the cspace. + */ + virtual CSpacePathPtr createPath(const Eigen::VectorXf &start, const Eigen::VectorXf &goal); + + /*! + Create a path from start to the goal configuration. In case a collision is detected the appending s stopped and the collision-free path is returned. + \param start The start + \param goal The goal + \param storeAddedLength The length of the collision free path is stored here (1.0 means the complete path from start to goal was collision-free) + */ + virtual CSpacePathPtr createPathUntilCollision(const Eigen::VectorXf &start, const Eigen::VectorXf &goal, float &storeAddedLength); + + +protected: + + float samplingSizePaths; //!< euclidean sample size + float samplingSizeDCD; //!< euclidean sample size for collision check + Eigen::VectorXf checkPathConfig; + + int recursiveTmpValuesIndex; + std::vector<Eigen::VectorXf> recursiveTmpValues; + Eigen::VectorXf tmpConfig; + + const int recursionMaxDepth; +}; + +} + +#endif // _Saba_CSpaceSampled_h diff --git a/MotionPlanning/CSpace/CSpaceTree.cpp b/MotionPlanning/CSpace/CSpaceTree.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f75e46d54e57f9c7f6fd8d2a4ff6988a676334ac --- /dev/null +++ b/MotionPlanning/CSpace/CSpaceTree.cpp @@ -0,0 +1,427 @@ + +#include "VirtualRobot/CollisionDetection/CDManager.h" +#include "CSpaceTree.h" +#include "CSpaceNode.h" +#include "CSpacePath.h" +#include "CSpace.h" +#include "VirtualRobot/Robot.h" +#include "VirtualRobot/RobotNodeSet.h" +#include "float.h" +#include <cmath> +#include <fstream> +#include <iomanip> +#include <time.h> + +using namespace std; + +//#define DO_THE_TESTS + +namespace Saba +{ + +CSpaceTree::CSpaceTree(CSpacePtr cspace) +{ + if (!cspace) + { + THROW_SABA_EXCEPTION ("NULL data, aborting..."); + } + + this->cspace = cspace; + randMult = (float)(1.0/(double)(RAND_MAX)); + updateChildren = false; + + dimension = cspace->getDimension(); + + if (dimension < 1) + { + std::cout << "CSpaceTree: Initialization fails: INVALID DIMENSION" << std::endl; + return; + } + tmpConfig.setZero(dimension); +} + +CSpaceTree::~CSpaceTree() +{ + +} + +void CSpaceTree::lock() +{ + mutex.lock(); +} + +void CSpaceTree::unlock() +{ + mutex.unlock(); +} + + + +void CSpaceTree::reset() +{ + idNodeMapping.clear(); + nodes.clear(); +} + +CSpaceNodePtr CSpaceTree::getNode(unsigned int id) +{ + if (idNodeMapping.find(id)==idNodeMapping.end()) + { + SABA_WARNING << " wrong ID: " << id << std::endl; + return CSpaceNodePtr(); + } + return idNodeMapping[id]; +} + +bool CSpaceTree::hasNode(CSpaceNodePtr n) +{ + if (!n) + return false; + if (idNodeMapping.find(n->ID)==idNodeMapping.end()) + return false; + return true; +} + +float CSpaceTree::getPathDist(unsigned int idStart, unsigned int idEnd, bool useMetricWeights) +{ + float result = 0.0f; + if (!getNode(idStart) || !getNode(idStart)) + { + SABA_ERROR << "CSpaceTree::getPathDist: start or goal id not correct..." << endl; + return -1.0f; + } + unsigned int actID = idEnd; + unsigned int actID2 = getNode(idEnd)->parentID; + CSpaceNodePtr actNode; + CSpaceNodePtr actNode2; + while (actID2 != idStart) + { + actNode = getNode(actID); + actNode2 = getNode(actID2); + if (actID2<0 || actNode->parentID<0 || !actNode || !actNode2) + { + std::cout << "CSpaceTree::getPathDist: error, no path from start to end ?!" << std::endl; + return result; + } + result += cspace->calcDist(actNode->configuration, actNode2->configuration); + + actID = actID2; + actID2 = actNode2->parentID; + } + + // add last part + result += cspace->calcDist(getNode(actID)->configuration, getNode(actID2)->configuration); + + return result; +} + +// creates a copy of configuration +CSpaceNodePtr CSpaceTree::appendNode(const Eigen::VectorXf &config, int parentID, bool calcDistance) +{ + SABA_ASSERT (config.rows()==dimension) + + // create a new node with config, store it in nodeList and set parentID + CSpaceNodePtr newNode = cspace->createNewNode(); + if (!newNode) + return newNode; + nodes.push_back(newNode); + idNodeMapping[newNode->ID] = newNode; + + // parent of new CSpaceNode + newNode->parentID = parentID; + + + + if (updateChildren && parentID>=0) + { + CSpaceNodePtr n = getNode(parentID); + if (!n) + { + SABA_ERROR << "CSpaceTree::appendNode: No parent node with id : " << parentID << std::endl; + } else + { + n->children.push_back(newNode); + } + } + // copy values + newNode->configuration = config; + // no distance information + newNode->obstacleDistance = -1.0f; + // calculate distances + if (calcDistance) + { + float d = cspace->calculateObstacleDistance(newNode->configuration); + newNode->obstacleDistance = d; + } +#ifdef DO_THE_TESTS + if (!cspace->IsConfigValid(config)) + { + std::cout << "Appending not valid node!" << std::endl; + } +#endif + return newNode; +} + + +// returns true if full path was added +bool CSpaceTree::appendPathUntilCollision(CSpaceNodePtr startNode, const Eigen::VectorXf &config, int *storeLastAddedID) +{ + SABA_ASSERT (hasNode(startNode)) + SABA_ASSERT (config.rows()==dimension) + + float dist; + CSpacePathPtr res = cspace->createPathUntilCollision(startNode->configuration, config, dist); + if (res->getNrOfPathPoints()<=1) + return false; + res->erasePosition(0); + bool appOK = appendPath(startNode, res, storeLastAddedID); + if (!appOK) + return false; + return (dist==1.0f); +} + +bool CSpaceTree::appendPath(CSpaceNodePtr startNode, CSpacePathPtr path, int *storeLastAddedID) +{ + SABA_ASSERT (hasNode(startNode)) + SABA_ASSERT (path) + CSpaceNodePtr n = startNode; + const std::vector <Eigen::VectorXf> data = path->getPathData(); + std::vector <Eigen::VectorXf>::const_iterator it; + for (it=data.begin(); it!=data.end(); it++) + { + n = appendNode(*it, n->ID); + } + if (storeLastAddedID) + *storeLastAddedID = n->ID; + return true; +} + +// appends path from startNode to config, creates in-between nodes according to cspace if needed +// no checks (for valid configs) +// creates a copy of configuration +bool CSpaceTree::appendPath(CSpaceNodePtr startNode, const Eigen::VectorXf &config, int *storeLastAddedID) +{ + SABA_ASSERT (hasNode(startNode)) + SABA_ASSERT (config.rows()==dimension) + + CSpacePathPtr res = cspace->createPath(startNode->configuration, config); + if (res->getNrOfPathPoints()<=1) + return false; + res->erasePosition(0); + return appendPath(startNode, res, storeLastAddedID); +} + + +void CSpaceTree::removeNode(CSpaceNodePtr n) +{ + if (!n) + { + SABA_ERROR << ": NULL node" << std::endl; + return; + } + if (n->ID<0) + { + SABA_ERROR << ": wrong ID" << std::endl; + return; + } + if (nodes.size()==0) + { + SABA_ERROR << ": no nodes in tree" << std::endl; + return; + } + + vector<CSpaceNodePtr>::iterator it = nodes.begin(); + while (*it!=n) + { + it++; + if (it==nodes.end()) + { + cout << "CSpaceTree::removeNode: node not in node vector ??? " << endl; + return; + } + } + nodes.erase(it); + idNodeMapping.erase(n->ID); + cspace->removeNode(n); +} + + +CSpaceNodePtr CSpaceTree::getNearestNeighbor(const Eigen::VectorXf &config, float *storeDist) +{ + return getNode(getNearestNeighborID(config,storeDist)); +} + + +unsigned int CSpaceTree::getNearestNeighborID(const Eigen::VectorXf &config, float *storeDist) +{ + // goes through complete list of nodes, toDO: everything is better than this: eg octrees,something hierarchical... + // update: only if we have more than 1000 nodes in the tree! + + if (nodes.size()==0 || !cspace) + { + SABA_WARNING << "no nodes in tree..." << endl; + return 0; + } + unsigned int bestID = nodes[0]->ID; + float dist2 = cspace->calcDist2(config,nodes[0]->configuration,true); + float test; + for (unsigned int i = 1; i<nodes.size();i++) + { + test = cspace->calcDist2(config,nodes[i]->configuration,true); + if (test<dist2) + { + dist2 = test; + bestID = nodes[i]->ID; + } + } + + if (storeDist!=NULL) + *storeDist = sqrtf(dist2); + return bestID; +} + + + + + +bool CSpaceTree::saveAllNodes(char const* filename) +{ + std::ofstream file(filename, std::ios::out); + if (!file.is_open()) + { + std::cout << "ERROR: rrtCSpace::saveAllNodes: Could not open file to write." << std::endl; + return false; + } + + file << "# DO NOT EDIT LIST OF NODES #" << std::endl; + file << "# FIRST ROW: KINEMATIC CHAIN NAME" << std::endl; + file << "# SECOND ROW: DIMENSION" << std::endl; + file << "# THIRD ROW: NUMBER OF ALL NODES" << std::endl; + file << "# THEN ACTUAL DATA IN FOLLOWING FORMAT: ID CONFIG PARENTID" << std::endl; + + // save kinematic chain name used + file << cspace->getRobotNodeSet()->getName() << std::endl; + + // save rrt dimension used for planning + file << dimension << std::endl; + + // save number of nodes + file << nodes.size() << std::endl; + + CSpaceNodePtr actualNode; + + for (unsigned int i = 0; i < nodes.size(); i++) + { + // save ID, configuration and parentID of each node in one row + actualNode = nodes[i]; + file << actualNode->ID << " "; + for (unsigned int j = 0; j < dimension; j++) + { + file << actualNode->configuration[j] << " "; + } + file << actualNode->parentID << std::endl; + } + + file.close(); + return true; +} + +CSpaceNodePtr CSpaceTree::getLastAddedNode() +{ + if (nodes.size()==0) + return CSpaceNodePtr(); + return nodes[nodes.size()-1]; +} + + +bool CSpaceTree::createPath (CSpaceNodePtr startNode, CSpaceNodePtr goalNode, CSpacePathPtr fillPath) +{ + // creates a path from goal to startNode + + if (!goalNode || !startNode || !fillPath) + return false; + std::vector<int> tmpSol; + fillPath->reset(); + int actID = goalNode->ID; + CSpaceNodePtr actNode = goalNode; + bool found = false; + while (actID>=0 && actNode && !found) + { + actNode = getNode(actID); + if (actNode) + { + tmpSol.push_back(actID); + //fillPath->addPathPoint(actNode->configuration); + if (actID == startNode->ID) + { + found = true; + } else + { + actID = actNode->parentID; + } + } + } + if (!found) + { + std::cout << "CSpaceTree::createPath: Start node not a parent of goalNode... goalID:" << goalNode->ID << ", startID:" << startNode->ID << std::endl; + } else + { + // revert path + for ( int i=( int)tmpSol.size()-1;i>=0;i--) + { + fillPath->addPathPoint(getNode(tmpSol[i])->configuration); + } + } + + return found; +} + +/* +void CSpaceTree::printNode(CSpaceNodePtr n) +{ + if (!n) + return; + cspace->printConfig(n->configuration); +}*/ + +/* +void CSpaceTree::printNode(unsigned int id) +{ + CSpaceNodePtr n = getNode(id); + cspace->printConfig(n->configuration); +} +*/ + +float CSpaceTree::getTreeLength(bool useMetricWeights) +{ + unsigned int nMaxNodes = (int)nodes.size(); + float res = 0; + for (unsigned int i=0; i<nMaxNodes;i++) + { + if (nodes[i] && nodes[i]->parentID>=0 && nodes[i]->parentID<(int)nMaxNodes) + res += cspace->calcDist(nodes[i]->configuration,nodes[nodes[i]->parentID]->configuration, !useMetricWeights); + } + return res; +} + +unsigned int CSpaceTree::getNrOfNodes() const +{ + return nodes.size(); +} + +std::vector<CSpaceNodePtr> CSpaceTree::getNodes() +{ + return nodes; +} + +unsigned int CSpaceTree::getDimension() const +{ + return dimension; +} + +Saba::CSpacePtr CSpaceTree::getCSpace() +{ + return cspace; +} + +} // namespace Saba diff --git a/MotionPlanning/CSpace/CSpaceTree.h b/MotionPlanning/CSpace/CSpaceTree.h new file mode 100644 index 0000000000000000000000000000000000000000..e67cafbead264c7ccf99caaaa740a1ea0862c688 --- /dev/null +++ b/MotionPlanning/CSpace/CSpaceTree.h @@ -0,0 +1,184 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _saba_cspacetree_h +#define _saba_cspacetree_h + +#include "../Saba.h" +#include <iostream> +#include <vector> +#include <map> +#include <boost/thread.hpp> + +namespace Saba{ + +/*! + * \brief Contains CSpaceTree which uses a CSpace for managing configurations + * + * + * A connected tree in c-space which represents a RRT. + * + */ +class SABA_IMPORT_EXPORT CSpaceTree +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + //! constructor + CSpaceTree(CSpacePtr cspace); + + //! destructor + virtual ~CSpaceTree(); + + + /*! + Return a cspace node. + \param id the id of the node + \return pointer to the cspace node + */ + CSpaceNodePtr getNode(unsigned int id); + + CSpaceNodePtr getLastAddedNode(); + + CSpacePtr getCSpace(); + + //! get number of nodes + virtual unsigned int getNrOfNodes() const; + + unsigned int getDimension() const; + + /*! + creates path from startNode to goalNode by traversing the parents of goalNode + \param startNode The start. + \param goalNode The goal. + \param fillPath Stores all configs. + \return true on success. + */ + virtual bool createPath (CSpaceNodePtr startNode, CSpaceNodePtr goalNode, CSpacePathPtr fillPath); + + //! set the number of nodes to zero + virtual void reset(); + + //! saves all nodes with ID, configuration and parentID to a file + virtual bool saveAllNodes(char const* filename); + + /*! + consider child vector list (@see CSpaceNode) when adding nodes to the tree + (standard: false) + */ + void setUpdateChildren(bool enable){updateChildren = enable;}; + bool getUpdateChildren(){return updateChildren;} + + + //! creates new CSpaceNode with configuration config and parentID + /*! + \param config the configuration + \param parentID ID of the parent + \param calcDistance if set, the distance to obstacles is calculated and stored in the node (be careful: expensive), otherwise the dist is set to -1.0 + \return pointer to the new node on success, otherwise NULL + */ + virtual CSpaceNodePtr appendNode(const Eigen::VectorXf &config, int parentID, bool calcDistance = false); + + //! remove tree node + virtual void removeNode(CSpaceNodePtr n); + + /*! + get ID of nearest neighbor + \param config configuration + \param storeDist pointer to float for storing the distance (if not NULL) + \return ID of nearest neighbor + */ + virtual unsigned int getNearestNeighborID(const Eigen::VectorXf &config, float *storeDist = NULL); + virtual CSpaceNodePtr getNearestNeighbor(const Eigen::VectorXf &config, float *storeDist = NULL); + + /*! + append a path to the tree (without any checks) + Intermediate configs are generated according to the current cspace + \param startNode start from this node + \param config + \param storeLastAddedID if given, the id of the last added node is stored here + */ + virtual bool appendPath (CSpaceNodePtr startNode, const Eigen::VectorXf& config, int *storeLastAddedID = NULL); + + /*! + Append the given path to this tree. No checks are performed. + */ + virtual bool appendPath (CSpaceNodePtr startNode, CSpacePathPtr path, int *storeLastAddedID = NULL); + + /*! + Append a path to the tree until a collision is detected + Intermediate configs are generated according to the current cspace + \param startNode start from this node + \param config + \param storeLastAddedID if given, the id of the last added node is stored here + */ + virtual bool appendPathUntilCollision(CSpaceNodePtr startNode, const Eigen::VectorXf &config, int *storeLastAddedID); + + virtual std::vector<CSpaceNodePtr> getNodes(); + + + /*! + Create a new cspace node (does !not! set the config values to zero). + \return pointer to new created node in cspace + */ + //virtual CSpaceNodePtr createNewNode(); + + void lock(); + void unlock(); + + + /*! + Computes the complete length of all edges. + \param useMetricWeights When set, the metric weights of the corresponding cspace are used for length calculation. When the cspace is not initialized with any metric weights, this option has no effect. + */ + float getTreeLength(bool useMetricWeights = true); + + bool hasNode(CSpaceNodePtr n); + +protected: + + //! get distance of path start to end + virtual float getPathDist(unsigned int idStart, unsigned int idEnd, bool useMetricWeights); + + //! shutdown tree, reset all data + //virtual void shutdown(); + unsigned int dimension; //!< dimension of the cspace + + CSpacePtr cspace; + + + Eigen::VectorXf tmpConfig; + + std::vector< CSpaceNodePtr > nodes; //! vector with pointers to all used nodes + + + bool updateChildren; // CSpaceNode child management + float randMult; + + std::map<unsigned int, CSpaceNodePtr > idNodeMapping; // mapping id<->node + + boost::mutex mutex; +}; + +} // namespace Saba + +#endif // _saba_cspacetree_h diff --git a/MotionPlanning/CSpace/Sampler.cpp b/MotionPlanning/CSpace/Sampler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..185270934f5b5b5036807bc468ac92a73ccca0b2 --- /dev/null +++ b/MotionPlanning/CSpace/Sampler.cpp @@ -0,0 +1,56 @@ +#include "Sampler.h" +#include "CSpace.h" + +namespace Saba +{ + +Sampler::Sampler(unsigned int dimension) +{ + dimension = dimension; + metricWeights.Ones(dimension); +} + +Sampler::~Sampler() +{ +} + +void Sampler::getUniformlyRandomConfig(Eigen::VectorXf &stroreConfig, CSpacePtr space) +{ + for(unsigned int i = 0; i < dimension; i++) + stroreConfig[i] = space->getRandomConfig_UniformSampling(i); +} + +void Sampler::getNormalRandomConfig(Eigen::VectorXf &stroreConfig, const Eigen::VectorXf &mean, const Eigen::MatrixXf &variance) +{ + //for(unsigned int i = 0; i < dimension; i++) + // pStoreConfig[i] = MathHelpers::getNormalRandom(mean[i], variance[i]); + cout << "todo" << endl; + +} + +void Sampler::getNormalRandomConfig(Eigen::VectorXf &stroreConfig, const Eigen::VectorXf &mean, float variance) +{ + //for(unsigned int i = 0; i < dimension; i++) + // pStoreConfig[i] = MathHelpers::getNormalRandom(mean[i], variance); + cout << "todo" << endl; + +} + +void Sampler::enableMetricWeights( const Eigen::VectorXf &weights ) +{ + /*delete [] m_pMetricWeights; + m_pMetricWeights = NULL; + if (pWeights && dimension>0) + { + m_pMetricWeights = new float[dimension]; + memcpy(m_pMetricWeights,pWeights,sizeof(float)*dimension); + }*/ + cout << "todo" << endl; +} + +void Sampler::disableMetricWeights() +{ + cout << "todo" << endl; +} + +} diff --git a/MotionPlanning/CSpace/Sampler.h b/MotionPlanning/CSpace/Sampler.h new file mode 100644 index 0000000000000000000000000000000000000000..f792c1d9676015e335e0e1d81bfe2914d8104603 --- /dev/null +++ b/MotionPlanning/CSpace/Sampler.h @@ -0,0 +1,76 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_sampler_h +#define _Saba_sampler_h + +#include "../Saba.h" +#include "../CSpace/CSpace.h" +#include <vector> + +namespace Saba { + +/*! +* +* \bief An interface class for custom sample algorithms +* +*/ +class SABA_IMPORT_EXPORT Sampler +{ +public: + Sampler(unsigned int dimension); + virtual ~Sampler(); + + virtual void sample(Eigen::VectorXf &stroreConfig, CSpacePtr space) = 0; + //virtual void sample(std::vector<float> &storeConfig, CSpacePtr cspace) = 0; + + /*! + Enable metric weighting. This can be useful for different variance in each dimension. + Standard: disabled + */ + virtual void enableMetricWeights(const Eigen::VectorXf &weights); + virtual void disableMetricWeights(); + +protected: + //! Returns a uniformly generated random configuration with nDimension components + void getUniformlyRandomConfig(Eigen::VectorXf &stroreConfig, CSpacePtr space); + + /*! Returns a normal distributed random configuration with nDimension components. + Note that we assume the covariance-matrix to be a diagonal matrix. This means, + that the components of the configuration are uncorrelated. + The result value is not checked against any boundaries of the configuration space. + */ + void getNormalRandomConfig(Eigen::VectorXf &stroreConfig, const Eigen::VectorXf &mean, const Eigen::MatrixXf &variance); + + /*! Returns a normal distributed random configuration with nDimension components. + This is a convenience function in case you want to apply the same variance in + every dimension. + */ + void getNormalRandomConfig(Eigen::VectorXf &stroreConfig, const Eigen::VectorXf &mean, float variance); + + unsigned int dimension; + Eigen::VectorXf metricWeights; +}; + +} + +#endif // _Saba_sampler_h diff --git a/MotionPlanning/Planner/BiRrt.cpp b/MotionPlanning/Planner/BiRrt.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eeff4309af258037ba6a7c88976b8533ae3b8cfe --- /dev/null +++ b/MotionPlanning/Planner/BiRrt.cpp @@ -0,0 +1,371 @@ + +#include "BiRrt.h" + +#include "../CSpace/CSpaceNode.h" +#include "../CSpace/CSpaceTree.h" +#include "../CSpace/CSpacePath.h" +#include "VirtualRobot/Robot.h" +#include <time.h> +#include <boost/pointer_cast.hpp> + +//#define LOCAL_DEBUG(a) {SABA_INFO << a;}; +#define LOCAL_DEBUG(a) + +using namespace std; +using namespace VirtualRobot; + +namespace Saba { + +BiRrt::BiRrt(CSpaceSampledPtr cspace, RrtMethod modeA, RrtMethod modeB) + : Rrt(cspace, modeA) +{ + rrtMode2 = modeB; + tree2.reset(new CSpaceTree(cspace)); + lastAddedID2 = -1; +} + +BiRrt::~BiRrt() +{ +} + + +bool BiRrt::plan(bool bQuiet) +{ + + if (!bQuiet) + { + SABA_INFO << "Starting BiRrt planner" << std::endl; + switch (rrtMode) + { + case eExtend: + cout << "-- ModeA: RRT-EXTEND" << endl; + break; + case eConnect: + cout << "-- ModeA: RRT-CONNECT" << endl; + break; + case eConnectCompletePath: + cout << "-- ModeA: RRT-CONNECT (only complete paths)" << endl; + break; + default: + break; + } + switch (rrtMode2) + { + case eExtend: + cout << "-- ModeB: RRT-EXTEND" << endl; + break; + case eConnect: + cout << "-- ModeB: RRT-CONNECT" << endl; + break; + case eConnectCompletePath: + cout << "-- ModeB: RRT-CONNECT (only complete paths)" << endl; + break; + default: + break; + } + } + + if (!isInitialized()) + { + SABA_ERROR << " planner: not initialized..." << std::endl; + return false; + } + + cycles = 0; + + int distChecksStart = cspace->performaceVars_distanceCheck; + int colChecksStart = cspace->performaceVars_collisionCheck; + + bool found = false; + stopSearch = false; + + clock_t startClock = clock(); + + solution.reset(); + + RobotPtr robot = cspace->getRobot(); + bool bVisStatus = true; + if (robot) + { + bVisStatus = robot->getUpdateVisualizationStatus(); + robot->setUpdateVisualization(false); + } + + ExtensionResult extResultA; + ExtensionResult extResultB; + + bool switched = false; + int *lastIDA = &lastAddedID; + int *lastIDB = &lastAddedID2; + CSpaceTreePtr treeA = tree; + CSpaceTreePtr treeB = tree2; + RrtMethod rrtModeA = rrtMode; + RrtMethod rrtModeB = rrtMode2; + + // PLANNING LOOP + do + { + // CHOOSE A RANDOM CONFIGURATION (NOT GOAL DIRECTED, ONLY RANDOMLY) + cspace->getRandomConfig(tmpConfig, true); + LOCAL_DEBUG("----------------------------------------------------" << endl); + LOCAL_DEBUG("Random Conf:" << endl << tmpConfig << endl); + if (switched) + { + LOCAL_DEBUG ("SWITCHED" << endl); + lastIDA = &lastAddedID2; + lastIDB = &lastAddedID; + treeA = tree2; + treeB = tree; + rrtModeA = rrtMode2; + rrtModeB = rrtMode; + } else + { + lastIDA = &lastAddedID; + lastIDB = &lastAddedID2; + treeA = tree; + treeB = tree2; + rrtModeA = rrtMode; + rrtModeB = rrtMode2; + } + + switch (rrtModeA) + { + case eExtend: + extResultA = extend(tmpConfig, treeA, *lastIDA); + break; + case eConnect: + extResultA = connectUntilCollision(tmpConfig, treeA, *lastIDA); + break; + case eConnectCompletePath: + extResultA = connectComplete(tmpConfig, treeA, *lastIDA); + break; + default: + break; + } + LOCAL_DEBUG ("ExtResultA:" << extResultA << endl); + if (extResultA==eError) + stopSearch = true; + + if (extResultA==ePartial || extResultA==eSuccess) + { + // update config + LOCAL_DEBUG ("Last ID A:" << *lastIDA << endl); + CSpaceNodePtr n = treeA->getNode(*lastIDA); + tmpConfig = n->configuration; + LOCAL_DEBUG ("Tmp goal B:" << endl << tmpConfig << endl); + switch (rrtModeB) + { + case eExtend: + extResultB = extend(tmpConfig, treeB, *lastIDB); + break; + case eConnect: + extResultB = connectUntilCollision(tmpConfig, treeB, *lastIDB); + break; + case eConnectCompletePath: + extResultB = connectComplete(tmpConfig, treeB, *lastIDB); + break; + default: + break; + } + LOCAL_DEBUG ("Last ID B:" << *lastIDB << endl); + LOCAL_DEBUG ("ExtResultB:" << extResultB << endl); + if (extResultB==eError) + stopSearch = true; + if (extResultB==eSuccess) + { + goalNode = treeB->getNode(*lastIDB); + if (!goalNode) + { + SABA_ERROR << " no node for ID: " << lastIDB << endl; + stopSearch = true; + } else + { + found = true; + } + } + } + + + cycles++; + switched = !switched; + + } while (!stopSearch && cycles<maxCycles && !found); + + clock_t endClock = clock(); + + long diffClock = (long)(((float)(endClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0); + planningTime = (float)diffClock; + + if (!bQuiet) + { + SABA_INFO << "Needed " << diffClock << " ms of processor time." << std::endl; + + SABA_INFO << "Created " << tree->getNrOfNodes() << " + " << tree2->getNrOfNodes() << " = " << tree->getNrOfNodes()+tree2->getNrOfNodes() << " nodes." << std::endl; + SABA_INFO << "Collision Checks: " << (cspace->performaceVars_collisionCheck-colChecksStart) << std::endl; + SABA_INFO << "Distance Calculations: " << (cspace->performaceVars_distanceCheck-distChecksStart) << std::endl; + + int nColChecks = (cspace->performaceVars_collisionCheck-colChecksStart); + if (diffClock>0) + { + float fPerf = (float)nColChecks / (float)diffClock * 1000.0f; + std::cout << "Performance: " << fPerf << " cps (collision-checks per second)." << std::endl; + } + } + + if (robot && bVisStatus) + { + robot->setUpdateVisualization(bVisStatus); + } + + if (found) + { + if (!bQuiet) + SABA_INFO << "Found RRT solution with " << cycles << " cycles."<< std::endl; + createSolution(bQuiet); + + return true; + } + + // something went wrong... + if (cycles>=maxCycles) + { + SABA_WARNING << " maxCycles exceeded..." << std::endl; + } + if (stopSearch) + { + SABA_WARNING << " search was stopped..." << std::endl; + } + + return false; + +} + + +// uses lastAddedID and lastAddedID2 +bool BiRrt::createSolution(bool bQuiet) +{ + // delete an existing solution if necessary + solution.reset(); + + // vectors to save the node ids for solution path + std::vector<int> tmpSol; // tree + std::vector<int> tmpSol2; // tree2 + + + // bridgeover node!!! + CSpaceNodePtr actNode = tree->getNode(lastAddedID); + CSpaceNodePtr actNode2 = tree2->getNode(lastAddedID2); + if (!bQuiet) + SABA_INFO << "node1:" << lastAddedID << ", node2: " << lastAddedID2 << std::endl; + if (!actNode || !actNode2) + { + SABA_ERROR << "No nodes for Solution ?!" << std::endl; + return false; + } + + // bridgeover node only once in solution vector + tmpSol.push_back(actNode->ID); + + int nextID; + int nextID2; + + int count1 = 0; + int count2 = 0; + // go through all solution nodes backwards in tree till m_pStartNode (parentID = -1) + nextID = actNode->parentID; + while (nextID >= 0 ) + { + count1++; + actNode = tree->getNode(nextID); + tmpSol.push_back(nextID); + nextID = actNode->parentID; + } + + // go through all solution nodes backwards in tree2 till m_pGoalNode (parentID = -1) + nextID2 = actNode2->parentID; + while (nextID2 >= 0) + { + count2++; + actNode2 = tree2->getNode(nextID2); + tmpSol2.push_back(nextID2); + nextID2 = actNode2->parentID; + } + if (!bQuiet) + SABA_INFO << "Count1: " << count1 << " - count2: " << count2 << std::endl; + //reverse the solution vector from bridgeover node to start node (correct order) + reverse(tmpSol.begin(),tmpSol.end()); + + // Create the CSpacePath, first start node till bridgeover node, then bridgeover node till goal node + solution.reset(new CSpacePath(cspace)); + for (unsigned int i=0;i<tmpSol.size();i++) + solution->addPathPoint(tree->getNode(tmpSol[i])->configuration); + + for (unsigned int i=0;i<tmpSol2.size();i++) + solution->addPathPoint(tree2->getNode(tmpSol2[i])->configuration); + + if (!bQuiet) + SABA_INFO << "Created solution with " << solution->getNrOfPathPoints() << " nodes." << std::endl; + //solution->checkDistance(tree->getColCheckSamplingSize()); + return true; +} + +bool BiRrt::setStart(const Eigen::VectorXf &c) +{ + return Rrt::setStart(c); +} + +bool BiRrt::setGoal(const Eigen::VectorXf &c) +{ + if (!Rrt::setGoal(c)) + return false; + + // copy goal values + if (goalNode) + tree2->removeNode(goalNode); + + goalNode = tree2->appendNode(c,-1); + + return true; +} + +void BiRrt::reset() +{ + Rrt::reset(); + if (tree2) + tree2->reset(); +} + +void BiRrt::printConfig(bool printOnlyParams) +{ + if (!printOnlyParams) + { + std::cout << "-- BiRrt config --" << std::endl; + std::cout << "------------------------------" << std::endl; + } + Rrt::printConfig (true); + switch (rrtMode2) + { + case eExtend: + cout << "-- Mode2: RRT-EXTEND" << endl; + break; + case eConnect: + cout << "-- Mode2: RRT-CONNECT" << endl; + break; + case eConnectCompletePath: + cout << "-- Mode2: RRT-CONNECT (only complete paths)" << endl; + break; + default: + break; + } + if (!printOnlyParams) + { + std::cout << "------------------------------" << std::endl; + } +} + +CSpaceTreePtr BiRrt::getTree2() +{ + return tree2; +} + +} // namespace diff --git a/MotionPlanning/Planner/BiRrt.h b/MotionPlanning/Planner/BiRrt.h new file mode 100644 index 0000000000000000000000000000000000000000..44f6325d3f8b81969bd1aa3f218a18acd75a87ca --- /dev/null +++ b/MotionPlanning/Planner/BiRrt.h @@ -0,0 +1,86 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_BiRrt_h +#define _Saba_BiRrt_h + +#include "../Saba.h" +#include "../CSpace/CSpaceSampled.h" +#include "../CSpace/CSpacePath.h" +#include "Rrt.h" + +namespace Saba { + + +/*! + * The standard implementation of the bidirectional RRT planner. + * Two search trees are started, one from the start and one from the goal node. + * + */ +class SABA_IMPORT_EXPORT BiRrt : public Rrt +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + Constructor + \param cspace An initialized cspace object. + \param modeA Specify the RRT method that should be used to build the first tree + \param modeB Specify the RRT method that should be used to build the second tree + */ + BiRrt(CSpaceSampledPtr cspace, RrtMethod modeA = eConnect, RrtMethod modeB = eConnect ); + virtual ~BiRrt(); + + /*! + do the planning (blocking method) + \return true if solution was found, otherwise false + */ + virtual bool plan(bool bQuiet = false); + + + virtual void printConfig(bool printOnlyParams = false); + + virtual void reset(); + + //! set start configuration + virtual bool setStart(const Eigen::VectorXf &c); + + //! set goal configuration + virtual bool setGoal(const Eigen::VectorXf &c); + + CSpaceTreePtr getTree2(); + +protected: + + virtual bool createSolution(bool bQuiet = false); + + CSpaceTreePtr tree2; //!< the second tree + + int lastAddedID2; //!< ID of last added node + RrtMethod rrtMode2; +}; + +} // namespace + +#endif // _Saba_RRT_h + + diff --git a/MotionPlanning/Planner/GraspIkRrt.cpp b/MotionPlanning/Planner/GraspIkRrt.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b0f04232579a97d57bd63a2894e94f202ef37fcd --- /dev/null +++ b/MotionPlanning/Planner/GraspIkRrt.cpp @@ -0,0 +1,286 @@ + +#include "GraspIkRrt.h" + +#include "../CSpace/CSpaceNode.h" +#include "../CSpace/CSpaceTree.h" +#include "../CSpace/CSpacePath.h" +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/RobotNodeSet.h> +#include <VirtualRobot/GraspSet.h> +#include <time.h> +#include <boost/pointer_cast.hpp> + +//#define LOCAL_DEBUG(a) {SABA_INFO << a;}; +#define LOCAL_DEBUG(a) + +using namespace std; +using namespace VirtualRobot; + +namespace Saba { + +GraspIkRrt::GraspIkRrt(CSpaceSampledPtr cspace, VirtualRobot::ManipulationObjectPtr object, VirtualRobot::IKSolverPtr ikSolver, VirtualRobot::GraspSetPtr graspSet, float probabSampleGoal) + : BiRrt(cspace, Rrt::eConnect, Rrt::eConnect), object(object), ikSolver(ikSolver), graspSet(graspSet), sampleGoalProbab(probabSampleGoal) +{ + THROW_VR_EXCEPTION_IF(!object,"NULL object"); + THROW_VR_EXCEPTION_IF(!ikSolver,"NULL ikSolver"); + THROW_VR_EXCEPTION_IF(!graspSet,"NULL graspSet"); + THROW_VR_EXCEPTION_IF(!cspace,"NULL cspace"); + THROW_VR_EXCEPTION_IF(!cspace->getRobot(),"NULL robot"); + if (sampleGoalProbab<0) + { + SABA_WARNING << "Low value for sample goal probability, setting probability to 0" << endl; + sampleGoalProbab = 0.0f; + } + if (sampleGoalProbab>=1.0f) + { + SABA_WARNING << "High value for sample goal probability, setting probability to 0.99" << endl; + sampleGoalProbab = 0.99f; + } + goalValid = true; + name = "IK-RRT Planner"; + rns = ikSolver->getRobotNodeSet(); + THROW_VR_EXCEPTION_IF(!rns,"NULL robotNodeSet in ikSolver?!"); + graspSetWorking = graspSet->clone(); +} + +GraspIkRrt::~GraspIkRrt() +{ +} + +bool GraspIkRrt::searchNewGoal() +{ + if (graspSetWorking->getSize()==0) + return true; + VirtualRobot::GraspPtr grasp = ikSolver->sampleSolution(object,graspSet); + + if (grasp) + { + Eigen::VectorXf config; + rns->getJointValues(config); + if (graspNodeMapping.find(grasp) != graspNodeMapping.end()) + { + LOCAL_DEBUG ("Grasp " << grasp->getName() << " already added..." << endl); + } else + { + // remove from working set + graspSetWorking->removeGrasp(grasp); + SABA_INFO << endl << "Found new IK Solution: " << grasp->getName() << endl; + SABA_INFO << config << endl; + if (checkGoalConfig(config)) + { + return addIKSolution(config,grasp); + } else + { + SABA_INFO << "IK solution in collision " << endl; + } + + } + } + + // no error + return true; +} + +bool GraspIkRrt::addIKSolution(Eigen::VectorXf &config, VirtualRobot::GraspPtr grasp) +{ + ikSolutions.push_back(config); + if (tree2->getNrOfNodes()>0) + { + if (connectComplete(config, tree2, lastAddedID2) == eSuccess) + { + LOCAL_DEBUG ("ADDIK success" << endl); + CSpaceNodePtr lastNode = tree2->getNode(lastAddedID2); + THROW_VR_EXCEPTION_IF(!lastNode,"Internal error, expecting node here?!"); + graspNodeMapping[grasp] = lastNode; + return true; + } else + { + LOCAL_DEBUG ("ADDIK failed" << endl); + } + } + + CSpaceNodePtr lastNode = tree2->appendNode(config,-(int)(ikSolutions.size())); + THROW_VR_EXCEPTION_IF(!lastNode,"Internal error, expecting node here?!"); + graspNodeMapping[grasp] = lastNode; + + return true; +} + +bool GraspIkRrt::checkGoalConfig(Eigen::VectorXf &config) +{ + return cspace->isConfigValid(config); +} + +bool GraspIkRrt::doPlanningCycle() +{ + static const float rMult = (float)(1.0/(double)(RAND_MAX)); + ExtensionResult resA,resB; + float r = (float)rand() * rMult; + if (r <= sampleGoalProbab || tree2->getNrOfNodes()==0) + { + return searchNewGoal(); + } else + { + // do a normal extend/connect + cspace->getRandomConfig(tmpConfig, true); + LOCAL_DEBUG("----------------------------------------------------" << endl); + LOCAL_DEBUG("Random Conf:" << endl << tmpConfig << endl); + resA = connectUntilCollision(tmpConfig, tree, lastAddedID); + if (resA==eError) + stopSearch = true; + + if (resA==ePartial || resA==eSuccess) + { + // update config + LOCAL_DEBUG ("Last ID A:" << lastAddedID << endl); + CSpaceNodePtr n = tree->getNode(lastAddedID); + tmpConfig = n->configuration; + LOCAL_DEBUG ("Tmp goal B:" << endl << tmpConfig << endl); + + resB = connectUntilCollision(tmpConfig, tree2, lastAddedID2); + if (resB==eError) + stopSearch = true; + LOCAL_DEBUG ("ExtResultB:" << resB << endl); + LOCAL_DEBUG ("Last ID B:" << lastAddedID2 << endl); + if (resB==eSuccess) + { + goalNode = tree2->getNode(lastAddedID2); + if (!goalNode) + { + SABA_ERROR << " no node for ID: " << lastAddedID2 << endl; + stopSearch = true; + } else + { + found = true; + } + } + } + } + + return !stopSearch; +} + + +bool GraspIkRrt::plan(bool bQuiet) +{ + + if (!bQuiet) + { + SABA_INFO << "Starting GraspIkRrt planner" << std::endl; + } + + if (!isInitialized()) + { + SABA_ERROR << " planner: not initialized..." << std::endl; + return false; + } + + cycles = 0; + + int distChecksStart = cspace->performaceVars_distanceCheck; + int colChecksStart = cspace->performaceVars_collisionCheck; + + found = false; + stopSearch = false; + + clock_t startClock = clock(); + + solution.reset(); + + RobotPtr robot = cspace->getRobot(); + bool bVisStatus = true; + bVisStatus = robot->getUpdateVisualizationStatus(); + robot->setUpdateVisualization(false); + + + + // PLANNING LOOP + do + { + if (!doPlanningCycle()) + stopSearch = true; + cycles++; + } while (!stopSearch && cycles<maxCycles && !found); + + clock_t endClock = clock(); + + long diffClock = (long)(((float)(endClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0); + planningTime = (float)diffClock; + + if (!bQuiet) + { + SABA_INFO << "Needed " << diffClock << " ms of processor time." << std::endl; + + SABA_INFO << "Created " << tree->getNrOfNodes() << " + " << tree2->getNrOfNodes() << " = " << tree->getNrOfNodes()+tree2->getNrOfNodes() << " nodes." << std::endl; + SABA_INFO << "Collision Checks: " << (cspace->performaceVars_collisionCheck-colChecksStart) << std::endl; + SABA_INFO << "Distance Calculations: " << (cspace->performaceVars_distanceCheck-distChecksStart) << std::endl; + SABA_INFO << "IK solutions: " << ikSolutions.size() << std::endl; + + int nColChecks = (cspace->performaceVars_collisionCheck-colChecksStart); + if (diffClock>0) + { + float fPerf = (float)nColChecks / (float)diffClock * 1000.0f; + std::cout << "Performance: " << fPerf << " cps (collision-checks per second)." << std::endl; + } + } + + if (robot && bVisStatus) + { + robot->setUpdateVisualization(bVisStatus); + } + + if (found) + { + if (!bQuiet) + SABA_INFO << "Found RRT solution with " << cycles << " cycles."<< std::endl; + createSolution(bQuiet); + + return true; + } + + // something went wrong... + if (cycles>=maxCycles) + { + SABA_WARNING << " maxCycles exceeded..." << std::endl; + } + if (stopSearch) + { + SABA_WARNING << " search was stopped..." << std::endl; + } + + return false; + +} + + +bool GraspIkRrt::setGoal(const Eigen::VectorXf &c) +{ + THROW_VR_EXCEPTION ("Not allowed here, goal configurations are sampled during planning.."); + return false; +} + +void GraspIkRrt::reset() +{ + BiRrt::reset(); + goalValid = true; + graspSetWorking = graspSet->clone(); + ikSolutions.clear(); + graspNodeMapping.clear(); +} + +void GraspIkRrt::printConfig(bool printOnlyParams) +{ + if (!printOnlyParams) + { + std::cout << "-- GraspIkRrt config --" << std::endl; + std::cout << "------------------------------" << std::endl; + } + BiRrt::printConfig (true); + if (!printOnlyParams) + { + std::cout << "------------------------------" << std::endl; + } +} + + +} // namespace diff --git a/MotionPlanning/Planner/GraspIkRrt.h b/MotionPlanning/Planner/GraspIkRrt.h new file mode 100644 index 0000000000000000000000000000000000000000..1ea093f167d40bafda9ba65979a34ff078ac9e6f --- /dev/null +++ b/MotionPlanning/Planner/GraspIkRrt.h @@ -0,0 +1,98 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_GraspIkRrt_h +#define _Saba_GraspIkRrt_h + +#include "../Saba.h" +#include "../CSpace/CSpaceSampled.h" +#include "../CSpace/CSpacePath.h" +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/IK/IKSolver.h> +#include "BiRrt.h" + +namespace Saba { + +/*! + * + * The GraspIkRrt planner combines the search for a feasible grasp and an IK solution with the search for a collision-free motion. + * + */ +class SABA_IMPORT_EXPORT GraspIkRrt : public BiRrt +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + Constructor + \param cspace The C-Space that should be used for collision detection + \param object The object to be grasped + \param ikSolver The ikSolver that should be used + \param graspSet The grasps, defining potential goal configurations. + \param probabSampleGoal Probability with which a goal config is created during planning loop. + */ + GraspIkRrt(CSpaceSampledPtr cspace, VirtualRobot::ManipulationObjectPtr object, VirtualRobot::IKSolverPtr ikSolver, VirtualRobot::GraspSetPtr graspSet, float probabSampleGoal = 0.1f); + virtual ~GraspIkRrt(); + + /*! + do the planning (blocking method) + \return true if solution was found, otherwise false + */ + virtual bool plan(bool bQuiet = false); + + + virtual void printConfig(bool printOnlyParams = false); + + //! This is not allowed here, since we sample goal configurations during planning: If called an exception is thrown + virtual bool setGoal(const Eigen::VectorXf &c); + + //! reset the planner + virtual void reset(); + +protected: + + //virtual bool createSolution(bool bQuiet = false); + bool doPlanningCycle(); + bool searchNewGoal(); + bool checkGoalConfig(Eigen::VectorXf &config); + bool addIKSolution(Eigen::VectorXf &config, VirtualRobot::GraspPtr grasp); + float sampleGoalProbab; + + VirtualRobot::ManipulationObjectPtr object; + VirtualRobot::IKSolverPtr ikSolver; + VirtualRobot::GraspSetPtr graspSet; + VirtualRobot::RobotNodeSetPtr rns; + + VirtualRobot::GraspSetPtr graspSetWorking; + + bool found; //!< Indicates if a solution was found + + std::map < VirtualRobot::GraspPtr, Saba::CSpaceNodePtr > graspNodeMapping; + std::vector< Eigen::VectorXf > ikSolutions; + +}; + +} // namespace + +#endif // _Saba_RRT_h + + diff --git a/MotionPlanning/Planner/MotionPlanner.cpp b/MotionPlanning/Planner/MotionPlanner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1a5b69b330811ec3d76ba50ac7a9aa02cef71765 --- /dev/null +++ b/MotionPlanning/Planner/MotionPlanner.cpp @@ -0,0 +1,174 @@ + +#include "MotionPlanner.h" + +namespace Saba { + +MotionPlanner::MotionPlanner( CSpacePtr cspace) +{ + this->cspace = cspace; + if (!cspace) + { + THROW_SABA_EXCEPTION("NULL data in MotionPlanner constructor"); + } + dimension = cspace->getDimension(); + if (dimension==0) + { + THROW_SABA_EXCEPTION("Need at least one dimensional cspace"); + } + + + planningTime = 0.0f; + + stopSearch = false; + maxCycles = 50000; // stop if cycles are exceeded + cycles = 0; + + startValid = false; + goalValid = false; + + name = "Motion Planner"; +} + +MotionPlanner::~MotionPlanner() +{ + reset(); +} + +void MotionPlanner::setName(std::string sName) +{ + this->name = sName; +} + +std::string MotionPlanner::getName() +{ + return name; +} + + +void MotionPlanner::printConfig(bool printOnlyParams) +{ + if (!printOnlyParams) + { + std::cout << "-- MotionPlanner config --" << std::endl; + std::cout << "-------------------------------" << std::endl; + } + std::cout << "-- dimension: " << dimension << std::endl; + std::cout << "-- maxCycles: " << maxCycles << std::endl; + + std::cout << "-- Start node: "; + if (cspace) + cspace->printConfig(startConfig); + else + std::cout << " NOT SET"; + std::cout << std::endl; + std::cout << "-- Goal node: "; + if (cspace) + cspace->printConfig(goalConfig); + else + std::cout << " NOT SET"; + std::cout << std::endl; + + if (!printOnlyParams) + { + std::cout << "-------------------------------" << std::endl; + } +} + + + + +bool MotionPlanner::setStart( const Eigen::VectorXf &c ) +{ + this->startConfig = c; + startValid = false; + if (cspace && !cspace->isCollisionFree(startConfig)) + { + //if (startNode->obstacleDistance<=0) + SABA_WARNING << " start pos is not collision free..." << std::endl; + std::cout << "config:" << std::endl; + cspace->printConfig(startConfig); + std::cout << std::endl; + return false; + } + if (cspace && !cspace->isInBoundary(startConfig)) + { + SABA_WARNING << " start node does violate CSpace boundaries! " << std::endl; + std::cout << "config:" << std::endl; + cspace->printConfig(startConfig); + std::cout << std::endl; + return false; + } + startValid = true; + return true; +} + + +bool MotionPlanner::setGoal( const Eigen::VectorXf &c ) +{ + this->goalConfig = c; + goalValid = false; + + //tree->calculateObstacleDistance(goalNode); + if (cspace && !cspace->isCollisionFree(goalConfig)) + { + SABA_WARNING << " goal pos is not collision free..." << std::endl; + std::cout << "config:" << std::endl; + cspace->printConfig(goalConfig); + std::cout << std::endl; + return false; + } + if (cspace && !cspace->isInBoundary(goalConfig)) + { + SABA_WARNING << " goal pos does violate CSpace boundaries!" << std::endl; + std::cout << "config:" << std::endl; + cspace->printConfig(goalConfig); + std::cout << std::endl; + return false; + } + goalValid = true; + return true; +} + + + +bool MotionPlanner::isInitialized() +{ + if (!startValid) + { + return false; + } + if (!goalValid) + { + return false; + } + + return true; +} + +void MotionPlanner::setMaxCycles(unsigned int mc) +{ + maxCycles = mc; +} + + +CSpacePathPtr MotionPlanner::getSolution() +{ + if (!solution) + createSolution(); + + return solution; +} + +void MotionPlanner::reset() +{ + startConfig.setZero(1); + goalConfig.setZero(1); + startValid = false; + goalValid = false; + cspace.reset(); + + planningTime = 0; + cycles = 0; +} + +} // namespace diff --git a/MotionPlanning/Planner/MotionPlanner.h b/MotionPlanning/Planner/MotionPlanner.h new file mode 100644 index 0000000000000000000000000000000000000000..981fc4382fab9eb9b64bcde1f0d92b89bbc1bf14 --- /dev/null +++ b/MotionPlanning/Planner/MotionPlanner.h @@ -0,0 +1,145 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_MotionPlanner_h +#define _Saba_MotionPlanner_h + +#include "../Saba.h" +#include "../CSpace/CSpace.h" +#include "../CSpace/CSpaceNode.h" +#include "../CSpace/CSpaceTree.h" + +namespace Saba +{ +/*! + * + * \brief An abstract base class of a motion planner. + * + */ +class SABA_IMPORT_EXPORT MotionPlanner +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + constructor + */ + MotionPlanner(CSpacePtr cspace); + + //! destructor + virtual ~MotionPlanner(); + + /*! + do the planning (blocking method) + \return true if solution was found, otherwise false + */ + virtual bool plan(bool bQuiet = false) = 0; + + /*! + Returns the solution path. + */ + CSpacePathPtr getSolution(); + + /*! + Set maximal cycles. Initially set to 50000. + */ + void setMaxCycles(unsigned int mc); + + //! return start configuration + Eigen::VectorXf getStartConfig(){return startConfig;} + + //! return goal configuration + Eigen::VectorXf getGoalConfig(){return goalConfig;} + + //! reset the planner + virtual void reset(); + + //! set start configuration + virtual bool setStart(const Eigen::VectorXf &c); + + //! set goal configuration + virtual bool setGoal(const Eigen::VectorXf &c); + + //! check that planner is initialized + //virtual bool isInitialized(); + + /*! + Return number of cycles that were needed for motion planning + */ + unsigned int getNrOfCycles(){return cycles;} + + /*! + Print setup of planner. + \param printOnlyParams If set the decorating start and end is skipped (can be used to print derived classes). + */ + virtual void printConfig(bool printOnlyParams = false); + + //! The CSpace + CSpacePtr getCSpace(){return cspace;} + + /*! + Sets stop flag, so that this planner can be notified to abort the search. + Only useful for threaded planners. + */ + virtual void stopExecution(){stopSearch = true;} + + //! Give the planner a name + void setName(std::string sName); + + //! The name of the planner. + std::string getName(); + + /*! + Returns the time needed for planning (in milliseconds). + */ + float getPlanningTimeMS(){return planningTime;} + + //! returns true, when start and goal config have been set + virtual bool isInitialized(); + +protected: + + //! create the solution + virtual bool createSolution(bool bQuiet = false) = 0; + CSpacePtr cspace; //!< the cspace on which are operating + CSpacePathPtr solution; //!< the solution + + //bool init; //!< indication the status of initialization + bool stopSearch; //!< indicates that the search should be interrupted + + unsigned int dimension; //!< dimension of c-space + + Eigen::VectorXf startConfig; //!< start config + bool startValid; + Eigen::VectorXf goalConfig; //!< goal config + bool goalValid; + + unsigned int maxCycles; //!< maximum cycles for searching + unsigned int cycles; //!< current cycles done in the run method + + std::string name; //!< Name of this planner (standard: "Motion Planner") + + float planningTime; //! Planning time in milliseconds +}; +} + +#endif // _Saba_MotionPlanner_ diff --git a/MotionPlanning/Planner/Rrt.cpp b/MotionPlanning/Planner/Rrt.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fb783d6fd34fe9f97c1260c2c6c3be48f89d843a --- /dev/null +++ b/MotionPlanning/Planner/Rrt.cpp @@ -0,0 +1,456 @@ + +#include "Rrt.h" +#include "../CSpace/CSpaceNode.h" +#include "../CSpace/CSpaceTree.h" +#include "../CSpace/CSpacePath.h" +#include "VirtualRobot/Robot.h" +#include <time.h> +#include <boost/pointer_cast.hpp> + +using namespace std; +using namespace VirtualRobot; + +namespace Saba { + +Rrt::Rrt(CSpaceSampledPtr cspace, RrtMethod mode, float probabilityExtendToGoal) + : MotionPlanner( boost::dynamic_pointer_cast<CSpace>(cspace)) +{ + rrtMode = mode; + tree.reset(new CSpaceTree(cspace)); + if (probabilityExtendToGoal<=0) + { + SABA_ERROR << " probabilityExtendToGoal is wrong: " << probabilityExtendToGoal << endl; + probabilityExtendToGoal=0.1f; + } + if (probabilityExtendToGoal>=1.0f) + { + SABA_ERROR << " probabilityExtendToGoal is wrong: " << probabilityExtendToGoal << endl; + probabilityExtendToGoal=0.9f; + } + extendGoToGoal = probabilityExtendToGoal; + + this->extendStepSize = cspace->getSamplingSize(); + tmpConfig.setZero(dimension); + lastAddedID = -1; +} + +Rrt::~Rrt() +{ +} + + +bool Rrt::plan(bool bQuiet) +{ + if (!bQuiet) + { + SABA_INFO << "Starting Rrt planner" << std::endl; + switch (rrtMode) + { + case eExtend: + cout << "-- Mode: RRT-EXTEND" << endl; + break; + case eConnect: + cout << "-- Mode: RRT-CONNECT" << endl; + break; + case eConnectCompletePath: + cout << "-- Mode: RRT-CONNECT (only complete paths)" << endl; + break; + default: + break; + } + } + + float rand_mult = (float)(1.0/(double)(RAND_MAX)); + float r; + + if (!isInitialized()) + { + SABA_ERROR << " planner: not initialized..." << std::endl; + return false; + } + + cycles = 0; + int distChecksStart = cspace->performaceVars_distanceCheck; + int colChecksStart = cspace->performaceVars_collisionCheck; + + bool found = false; + stopSearch = false; + + clock_t startClock = clock(); + + solution.reset(); + + RobotPtr robot = cspace->getRobot(); + bool bVisStatus = true; + if (robot) + { + bVisStatus = robot->getUpdateVisualizationStatus(); + robot->setUpdateVisualization(false); + } + + ExtensionResult extResult; + + // the extension loop + do + { + // CHOOSE A RANDOM CONFIGURATION + // check if we want to go to the goal directly or extend randomly + r = (float)rand() * rand_mult; + if (r <= extendGoToGoal) + { + switch (rrtMode) + { + case eExtend: + extResult = extend(goalConfig, tree, lastAddedID); + break; + case eConnect: + extResult = connectUntilCollision(goalConfig, tree, lastAddedID); + break; + case eConnectCompletePath: + extResult = connectComplete(goalConfig, tree, lastAddedID); + break; + default: + break; + } + if (extResult==eSuccess) + { + + goalNode = cspace->getNode(lastAddedID); + if (!goalNode) + { + SABA_ERROR << " no node for ID: " << lastAddedID << endl; + stopSearch = true; + } else + { + if (!goalConfig.isApprox(goalNode->configuration)) + { + SABA_WARNING << "GoalConfig: (" << goalConfig << ") != goalNode (" << goalNode->configuration << ")" << endl; + } + //SABA_ASSERT(goalConfig.isApprox(goalNode->configuration)); + found = true; + } + } + else if (extResult==eError) + stopSearch = true; + } + else + { + // extend randomly, create a random position in config space + cspace->getRandomConfig(tmpConfig); + + switch (rrtMode) + { + case eExtend: + extResult = extend(tmpConfig, tree, lastAddedID); + break; + case eConnect: + extResult = connectUntilCollision(tmpConfig, tree, lastAddedID); + break; + case eConnectCompletePath: + extResult = connectComplete(tmpConfig, tree, lastAddedID); + break; + default: + break; + } + if (extResult==eError) + stopSearch = true; + } + + cycles++; + + } while (!stopSearch && cycles<maxCycles && !found); + + clock_t endClock = clock(); + + long diffClock = (long)(((float)(endClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0); + planningTime = (float)diffClock; + + if (!bQuiet) + { + SABA_INFO << "Needed " << diffClock << " ms of processor time." << std::endl; + + SABA_INFO << "Created " << tree->getNrOfNodes() << " nodes." << std::endl; + SABA_INFO << "Collision Checks: " << (cspace->performaceVars_collisionCheck-colChecksStart) << std::endl; + SABA_INFO << "Distance Calculations: " << (cspace->performaceVars_distanceCheck-distChecksStart) << std::endl; + + int nColChecks = (cspace->performaceVars_collisionCheck-colChecksStart); + if (diffClock>0) + { + float fPerf = (float)nColChecks / (float)diffClock * 1000.0f; + std::cout << "Performance: " << fPerf << " cps (collision-checks per second)." << std::endl; + } + } + + if (robot && bVisStatus) + { + robot->setUpdateVisualization(bVisStatus); + } + + if (found) + { + if (!bQuiet) + SABA_INFO << "Found RRT solution with " << cycles << " cycles."<< std::endl; + createSolution(bQuiet); + + return true; + } + + // something went wrong... + if (cycles>=maxCycles) + { + SABA_WARNING << " maxCycles exceeded..." << std::endl; + } + if (stopSearch) + { + SABA_WARNING << " search was stopped..." << std::endl; + } + + return false; +} + +Rrt::ExtensionResult Rrt::extend(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID) +{ + // NEAREST NEIGHBOR OF RANDOM CONFIGURATION + CSpaceNodePtr nn = tree->getNearestNeighbor(c); + + SABA_ASSERT (nn); + + CSpacePtr cspace = tree->getCSpace(); + + // length of the new extension step + float totalLength = cspace->calcDist(nn->configuration,c); + + bool reached = false; + + // set randomly found configuration if distance is smaller than specific length + if (totalLength <= extendStepSize) + { + tmpConfig = c; + reached = true; + } + else + { + float factor = extendStepSize / totalLength; + // go a specific length in the direction of randomly found configuration + tmpConfig = nn->configuration + ((c - nn->configuration) * factor); + //for (unsigned int i = 0; i < m_nDimension; i++) + // m_pExtendRrtNewValue[i] = m_nnNode->configuration[i] + ((extGoal[i] - m_nnNode->configuration[i]) * factor); + } + + // CHECK PATH FOR COLLISIONS AND VALID NODES + if (cspace->isPathCollisionFree(nn->configuration,tmpConfig)) + { + // ADD IT TO RRT TREE + if (!tree->appendPath(nn,tmpConfig,&storeLastAddedID)) + return eError; + if (reached) + { + return eSuccess; + } + else + { + return ePartial; // ADVANCED + } + }//PATH NOT COLLISION FREE + + return eFailed; // EXTEND FAILS +} + + +Rrt::ExtensionResult Rrt::connectComplete(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID) +{ + // NEAREST NEIGHBOR OF RANDOM CONFIGURATION + CSpaceNodePtr nn = tree->getNearestNeighbor(c); + + SABA_ASSERT (nn); + + // CHECK PATH FOR COLLISIONS AND VALID NODES + if (cspace->isPathCollisionFree(nn->configuration,c)) + { + // ADD IT TO RRT TREE + if (!tree->appendPath(nn,c,&storeLastAddedID)) + return eError; + + return eSuccess; // REACHED + }//PATH NOT COLLISION FREE + + return eFailed; // CONNECT FAILS +} + +Rrt::ExtensionResult Rrt::connectUntilCollision(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID) +{ + // NEAREST NEIGHBOR OF RANDOM CONFIGURATION + CSpaceNodePtr nn = tree->getNearestNeighbor(c); + + int oldLastID = storeLastAddedID; + // CHECK PATH FOR COLLISIONS AND VALID NODES + if (tree->appendPathUntilCollision(nn,c,&storeLastAddedID)) + { + return eSuccess; // REACHED (add m_pExtendRrtRandValue to rrt tree) + }//PATH NOT COLLISION FREE + + if (oldLastID == storeLastAddedID) + return eFailed; // CONNECT FAILS + else + return ePartial; // added some new nodes +} + +void Rrt::printConfig(bool printOnlyParams) +{ + if (!printOnlyParams) + { + std::cout << "-- Rrt config --" << std::endl; + std::cout << "------------------------------" << std::endl; + } + + float DCDsize = 0.0f; + float Pathsize = 0.0f; + CSpaceSampledPtr cs = boost::dynamic_pointer_cast<CSpaceSampled>(cspace); + if (cs) + { + DCDsize = cs->getSamplingSizeDCD(); + Pathsize = cs->getSamplingSize(); + } + std::cout << "-- RRT-Extend step size: " << extendStepSize << std::endl; + std::cout << "-- C-Space: Add new Paths sampling size: " << Pathsize << std::endl; + std::cout << "-- C-Space: DCD sampling size: " << DCDsize << std::endl; + std::cout << "-- Probability: extend to goal: " << extendGoToGoal << std::endl; + + MotionPlanner::printConfig (true); + switch (rrtMode) + { + case eExtend: + cout << "-- Mode: RRT-EXTEND" << endl; + break; + case eConnect: + cout << "-- Mode: RRT-CONNECT" << endl; + break; + case eConnectCompletePath: + cout << "-- Mode: RRT-CONNECT (only complete paths)" << endl; + break; + default: + break; + } + if (!printOnlyParams) + { + std::cout << "------------------------------" << std::endl; + } +} + + +bool Rrt::setStart( const Eigen::VectorXf &c ) +{ + if (!MotionPlanner::setStart(c)) + return false; + + if (tree->getNrOfNodes()>0) + { + SABA_WARNING << "Removing all nodes from tree..." << endl; + tree->reset(); + } + + // init root node of RRT + startNode = tree->appendNode(c,-1); + + return true; +} + + +bool Rrt::setGoal( const Eigen::VectorXf &c ) +{ + if (!MotionPlanner::setGoal(c)) + return false; + + return true; +} + +// uses last added CSpaceNode as start +bool Rrt::createSolution(bool bQuiet) +{ + + // check if we have a path + + if (!goalNode || goalNode->parentID<0) + { + SABA_WARNING << " no path to goal..." << std::endl; + return false; + } + + std::vector<int> tmpSol; + + int count = 0; + CSpaceNodePtr actNode; + int nextID; + bool failure = false; + + // check if last node has goal configuration + CSpaceNodePtr lastNode = tree->getNode(goalNode->ID);//parentID); + tmpSol.push_back(lastNode->ID); + actNode = lastNode; + + // go through all solution nodes backwards + do + { + nextID = actNode->parentID; + if (nextID<0) + { + if (actNode->ID!=startNode->ID) + { + SABA_WARNING << " oops something went wrong..." << std::endl; + failure = true; + } + } else + { + // got to parent CSpaceNode + actNode = tree->getNode(nextID); + tmpSol.push_back(nextID); + } + count++; + if (!actNode) + failure = true; + } while (!failure && actNode->ID!=startNode->ID); + + if (!failure) + { + solution.reset(new CSpacePath(cspace)); + // store solution in correct order + for ( int i=( int)tmpSol.size()-1;i>=0;i--) + { + solution->addPathPoint(tree->getNode(tmpSol[i])->configuration); + } + + if (!bQuiet) + SABA_INFO << "Created solution with " << solution->getNrOfPathPoints() << " nodes." << std::endl; + + return true; + } else + return false; +} + +void Rrt::reset() +{ + MotionPlanner::reset(); + startNode.reset(); + goalNode.reset(); + if (tree) + tree->reset(); + cspace->reset(); +} + + + +void Rrt::setProbabilityExtendToGoal( float p ) +{ + extendGoToGoal = p; + if (extendGoToGoal<=0) + extendGoToGoal = 0.1f; + if (extendGoToGoal>=1.0f) + extendGoToGoal = 0.9f; +} + +Saba::CSpaceTreePtr Rrt::getTree() +{ + return tree; +} + +} // namespace diff --git a/MotionPlanning/Planner/Rrt.h b/MotionPlanning/Planner/Rrt.h new file mode 100644 index 0000000000000000000000000000000000000000..1f1df16dfa3fe4e539fad186dbdd19bcc1d49e80 --- /dev/null +++ b/MotionPlanning/Planner/Rrt.h @@ -0,0 +1,124 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_Rrt_h +#define _Saba_Rrt_h + +#include "../Saba.h" +#include "../CSpace/CSpaceSampled.h" +#include "../CSpace/CSpacePath.h" +#include "MotionPlanner.h" + +namespace Saba { +/*! + * + * \brief A simple sampling based, single rrt planner using the extend or connect method. + * + * Rrt-related algorithms are known to allow efficient motion planning in high dimensional + * configuration spaces. Complex setups can be specified via the used c-space classes. + * The number of degrees of freedom (DoF/dimension) is defined via the c-space. Also + * the collision detection is handled by the c-space classes. + * + * @see CSpaceSampled + * @see CSpacePath + * @see CSpaceTree + * + */ +class SABA_IMPORT_EXPORT Rrt : public MotionPlanner +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum RrtMethod + { + eExtend, // extend + eConnect, // extend path until collision is determined + eConnectCompletePath // all or nothing: connect only when complete path is valid + }; + + enum ExtensionResult + { + eError, // an error occurred + eFailed, // the extend failed (e.g. a collision was detected) + eSuccess, // the extension succeeded completely + ePartial, // the extension was performed only partially + }; + + /*! + Constructor + \param cspace A cspace, defining the used joints (the dimension/DoF) and the collision detection setup. + \param mode Specify the RRT method that should be used + \param probabilityExtendToGoal Specify how often the goal node should be used instead of a random config (value must be between 0 and 1) + */ + Rrt(CSpaceSampledPtr cspace, RrtMethod mode = eConnect, float probabilityExtendToGoal = 0.1f); + virtual ~Rrt(); + + /*! + Do the planning (blocking method). On success the Rrt can be accessed with the getTree() method and the + found solution with getSolution(). + \param bQuiet Print some info or not. + \return true if solution was found, otherwise false + */ + virtual bool plan(bool bQuiet = false); + + virtual void printConfig(bool printOnlyParams = false); + + virtual void reset(); + + //! set start configuration + virtual bool setStart(const Eigen::VectorXf &c); + + //! set goal configuration + virtual bool setGoal(const Eigen::VectorXf &c); + + void setProbabilityExtendToGoal(float p); + + CSpaceTreePtr getTree(); + +protected: + + virtual bool createSolution(bool bQuiet = false); + + CSpaceTreePtr tree; //!< the rrt on which are operating + + + ExtensionResult extend(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID); + ExtensionResult connectComplete(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID); + ExtensionResult connectUntilCollision(Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID); + + CSpaceNodePtr startNode; //!< start node (root of RRT) + CSpaceNodePtr goalNode; //!< goal node (set when RRT weas successfully connected to goalConfig) + + Eigen::VectorXf tmpConfig; //!< tmp config + + float extendGoToGoal; //!< the probability that the goal config is used instead of a randomly created configuration + + float extendStepSize; //!< step size for one rrt extend (copied from cspace) + int lastAddedID; //!< ID of last added node + + RrtMethod rrtMode; +}; + +} // namespace + +#endif // _Saba_RRT_h + diff --git a/MotionPlanning/PostProcessing/PathProcessor.cpp b/MotionPlanning/PostProcessing/PathProcessor.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8b1362c9b293741cbeb369b463016e0564cdc6e2 --- /dev/null +++ b/MotionPlanning/PostProcessing/PathProcessor.cpp @@ -0,0 +1,28 @@ + +#include "PathProcessor.h" +#include "../CSpace/CSpaceSampled.h" +#include "../CSpace/CSpacePath.h" + +namespace Saba +{ +PathProcessor::PathProcessor(CSpacePathPtr path, bool verbose) + :path(path), verbose(verbose) +{ + THROW_VR_EXCEPTION_IF( (!path), "NULl path..."); + stopOptimization = false; +} + +PathProcessor::~PathProcessor() +{ +} + +CSpacePathPtr PathProcessor::getOptimizedPath() +{ + return optimizedPath; +} + +void PathProcessor::stopExecution() +{ + stopOptimization = true; +} +} diff --git a/MotionPlanning/PostProcessing/PathProcessor.h b/MotionPlanning/PostProcessing/PathProcessor.h new file mode 100644 index 0000000000000000000000000000000000000000..45c7f5b3c4a5813a2f89e7b00c1502eec5b7a6bf --- /dev/null +++ b/MotionPlanning/PostProcessing/PathProcessor.h @@ -0,0 +1,74 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __Saba_PathProcessor_h__ +#define __Saba_PathProcessor_h__ + +#include "Saba.h" + +namespace Saba +{ +/*! + * + * \brief An abstract interface for path processing classes. + * + */ +class SABA_IMPORT_EXPORT PathProcessor +{ +public: + + /*! + Constructor + Creates a local copy of p. + */ + PathProcessor(CSpacePathPtr p, bool verbose = false); + + /*! + Destructor + Deletes local optimized path. + */ + virtual ~PathProcessor(); + + + /*! + Here the path processing is executed, the number of optimizing steps can be specified. + */ + virtual CSpacePathPtr optimize(int optimizeSteps) = 0; + + //! Stop the execution from outside. + virtual void stopExecution(); + + CSpacePathPtr getOptimizedPath(); + +protected: + + CSpacePathPtr optimizedPath; // the optimized path + CSpacePathPtr path; // stores the original path + unsigned int dim; + bool verbose; + + bool stopOptimization; // allows to stop a running optimization process +}; + +} // namespace + +#endif // __Saba_CPathProcessor_h__ diff --git a/MotionPlanning/PostProcessing/ShortcutProcessor.cpp b/MotionPlanning/PostProcessing/ShortcutProcessor.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0594b06f41414a6588838c0d819b2b124ed30941 --- /dev/null +++ b/MotionPlanning/PostProcessing/ShortcutProcessor.cpp @@ -0,0 +1,247 @@ + +#include "ShortcutProcessor.h" +#include "CSpace/CSpaceSampled.h" +#include "CSpace/CSpacePath.h" +#include <vector> +#include <time.h> +#include <math.h> + +namespace Saba +{ + +ShortcutProcessor::ShortcutProcessor(CSpacePathPtr path, CSpaceSampledPtr cspace, bool verbose) : PathProcessor(path, verbose), cspace(cspace) +{ + stopOptimization = false; +} + +ShortcutProcessor::~ShortcutProcessor() +{ +} + + + +int ShortcutProcessor::tryRandomShortcut(int maxSolutionPathDist) +{ + if (!path || !cspace) + { + SABA_ERROR << "NULL data" << endl; + return 0; + } + + if (maxSolutionPathDist<2) + maxSolutionPathDist = 2; + + if (verbose) + { + SABA_INFO << "Path length: " << optimizedPath->getPathLength() << std::endl; + SABA_INFO << "Path nr of nodes: " << optimizedPath->getNrOfPathPoints() << std::endl; + } + int startNodeIndex = (int)(rand()%(optimizedPath->getNrOfPathPoints()-1)); + + int dist = rand()%(2*maxSolutionPathDist)-maxSolutionPathDist; + if (dist==0) + dist++; + if (dist==1) + dist++; + if (dist==-1) + dist--; + + int endNodeIndex = startNodeIndex+dist; + if (startNodeIndex>endNodeIndex) + { + int tmp = startNodeIndex; + startNodeIndex = endNodeIndex; + endNodeIndex = tmp; + } + + if (startNodeIndex<0) + startNodeIndex = 0; + if (endNodeIndex>(int)optimizedPath->getNrOfPathPoints()-2) // last node should remain unchanged + endNodeIndex = (int)optimizedPath->getNrOfPathPoints()-2; + if ((endNodeIndex-startNodeIndex)<=2) + return 0; + + if (verbose) + SABA_INFO << "-- start: " << startNodeIndex << ", end: " << endNodeIndex << std::endl; + + Eigen::VectorXf s = optimizedPath->getPathEntry(startNodeIndex); + Eigen::VectorXf e = optimizedPath->getPathEntry(endNodeIndex); + Eigen::VectorXf d = e - s; + + // test line between start and end + + float distShortcut = d.norm(); + float distPath = optimizedPath->getPathLength(startNodeIndex,endNodeIndex); + + // ------------------------------------------------------------------- + // DEBUG + if (verbose) + std::cout << "-- distShortcut: " << distShortcut << " distPath: " << distPath << std::endl; + // ------------------------------------------------------------------- + + if (distShortcut<distPath*0.95f) + { + // ------------------------------------------------------------------- + // DEBUG + if (verbose) + std::cout << "ShortcutProcessor: tryRandomShortcut: Shortcut Path shorter!" << std::endl; + // ------------------------------------------------------------------- + + //bool pathOK = m_pTree->checkPathSampled(startConfig,endConfig); + // first check sampled, the validate with freeBubbles + // (if sampled check failed we don't need to do the expensive freeBubble check) + //bool pathOK = m_pTree->checkPathSampled(startConfig,endConfig); + bool pathOK = cspace->isPathCollisionFree(s,e); + if (pathOK) + { + /*cout << "start:" << endl << s << endl; + cout << "end:" << endl << e << endl; + cout << "all:" << endl; + optimizedPath->print();*/ + // ------------------------------------------------------------------- + // DEBUG + if (verbose) + std::cout << "ShortcutProcessor::tryRandomShortcut: pathOK!" << std::endl; + // ------------------------------------------------------------------- + + // complete path valid and dist is shorter + if (verbose) + std::cout << "Creating direct shortcut from node " << startNodeIndex << " to node " << endNodeIndex << std::endl; + for (int i=endNodeIndex-1; i>=startNodeIndex+1;i--) + { + // erase solution positions + optimizedPath->erasePosition(i); + } + if (verbose) + { + float distPathtest = optimizedPath->getPathLength(startNodeIndex,startNodeIndex+1); + std::cout << "-- erased intermediate positions, distPath startIndex to (startIndex+1): " << distPathtest << std::endl; + } + /*cout << "all2:" << endl; + optimizedPath->print();*/ + // create intermediate path + + CSpacePathPtr intermediatePath = cspace->createPath(s, e); + int newP = 0; + if (intermediatePath->getNrOfPathPoints()>2) + { + newP = intermediatePath->getNrOfPathPoints()-2; + /*cout << "before:" << endl; + optimizedPath->print(); + cout << "interm path:" << endl; + intermediatePath->print();*/ + intermediatePath->erasePosition(intermediatePath->getNrOfPathPoints()-1); + intermediatePath->erasePosition(0); + /*cout << "interm path without start end:" << endl; + intermediatePath->print();*/ + optimizedPath->insertPath(startNodeIndex+1,intermediatePath); + /*cout << "after:" << endl; + optimizedPath->print(); */ + } + + if (verbose) + { + float sum = 0.0f; + for (int u=startNodeIndex; u<=startNodeIndex+newP; u++) + { + float distPathtest2 = optimizedPath->getPathLength(u,u+1); + sum += distPathtest2; + std::cout << "---- intermediate position: " << u << ", distPath to next pos: " << distPathtest2 << ", sum:" << sum << std::endl; + } + } + int nodes = endNodeIndex-startNodeIndex-1 + newP; + if (verbose) + { + std::cout << "-- end, nodes: " << nodes << std::endl; + } + + + return nodes; + } + } // dist reduced + + // path not valid + return 0; +} + + +CSpacePathPtr ShortcutProcessor::optimize(int optimizeSteps) +{ + return shortenSolutionRandom(optimizeSteps); +} + +CSpacePathPtr ShortcutProcessor::shortenSolutionRandom(int shortenLoops /*=300*/, int maxSolutionPathDist) +{ + stopOptimization = false; + THROW_VR_EXCEPTION_IF((!cspace || !path), "NULL data"); + int counter = 0; + optimizedPath = path->clone(); + if (!optimizedPath) + { + std::cout << "ShortcutProcessor::ShortenSolutionRandom: Wrong parameters or no path to smooth..." << std::endl; + return CSpacePathPtr(); + } + if (optimizedPath->getNrOfPathPoints()<=2) + return optimizedPath; + + int result = 0; + int beforeCount = (int)optimizedPath->getNrOfPathPoints(); + float beforeLength = optimizedPath->getPathLength(); + if (verbose) + { + SABA_INFO << ": solution size before shortenSolutionRandom:" << beforeCount << std::endl; + SABA_INFO << ": solution length before shortenSolutionRandom:" << beforeLength << std::endl; + } + clock_t startT = clock(); + + int red; + int loopsOverall = 0; + while (counter<shortenLoops && !stopOptimization) + { + loopsOverall++; + red = tryRandomShortcut(maxSolutionPathDist); + + counter++; + result += red; + } + if(stopOptimization) + { + SABA_INFO << "optimization was stopped" << std::endl; + } + int afterCount = (int)optimizedPath->getNrOfPathPoints(); + float afterLength = optimizedPath->getPathLength(); + clock_t endT = clock(); + float timems = (float)(endT - startT) / (float)CLOCKS_PER_SEC * 1000.0f; + if (verbose) + { + SABA_INFO << ": shorten loops: " << loopsOverall << std::endl; + SABA_INFO << ": shorten time: " << timems << " ms " << std::endl; + SABA_INFO << ": solution size after ShortenSolutionRandom (nr of positions) : " << afterCount << std::endl; + SABA_INFO << ": solution length after ShortenSolutionRandom : " << afterLength << std::endl; + } + return optimizedPath; +} + +void ShortcutProcessor::doPathPruning() +{ + optimizedPath = path->clone(); + + unsigned int i = 0; + while (i < optimizedPath->getNrOfPathPoints()-2) + { + Eigen::VectorXf startConfig = optimizedPath->getPathEntry(i); + Eigen::VectorXf endConfig = optimizedPath->getPathEntry(i+2); + if(cspace->isPathCollisionFree(startConfig,endConfig) ) + { + optimizedPath->erasePosition(i+1); + if (i > 0) i--; + } + else + { + i++; + } + } +} + + +} // namespace diff --git a/MotionPlanning/PostProcessing/ShortcutProcessor.h b/MotionPlanning/PostProcessing/ShortcutProcessor.h new file mode 100644 index 0000000000000000000000000000000000000000..0408b97621065eafd4fd25b36ebc35cdeeedfed8 --- /dev/null +++ b/MotionPlanning/PostProcessing/ShortcutProcessor.h @@ -0,0 +1,72 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef __Saba_ShortcutProcessor_h__ +#define __Saba_ShortcutProcessor_h__ + +#include "Saba.h" +#include "PathProcessor.h" + +namespace Saba +{ +/*! + * + * \brief The ShortcutProcessor searches shortcuts in C-Space to produce smooth trajectories. + * + */ +class SABA_IMPORT_EXPORT ShortcutProcessor : public PathProcessor +{ +public: + + ShortcutProcessor(CSpacePathPtr path, CSpaceSampledPtr cspace, bool verbose = false); + virtual ~ShortcutProcessor(); + + //! A wrapper to the standard interface. Calls shortenSolutionRandom(). + virtual CSpacePathPtr optimize(int optimizeSteps); + + /*! + Creates a shortened CSpace path. + \param shortenLoops Calls tryRandomShort() shortenLoops times + \param maxSolutionPathDist The max solution path dist. + \return The local instance of the optimized solution. + */ + CSpacePathPtr shortenSolutionRandom(int shortenLoops = 300, int maxSolutionPathDist = 30); + + + /*! + Goes through path and checks if direct shortcut between node before to node behind current + node is collision free. + Has to be called multiple times in order to get a good result. + Slow method, shortenSolutionRandom() gets similar results but is much faster. + */ + void doPathPruning(); + +protected: + + // returns number of kicked nodes + int tryRandomShortcut(int maxSolutionPathDist); + CSpaceSampledPtr cspace; +}; + +}// namespace + +#endif // __Saba_ShortcutProcessor_h__ diff --git a/MotionPlanning/Saba.h b/MotionPlanning/Saba.h new file mode 100644 index 0000000000000000000000000000000000000000..b33211f03b87dd17f02dcf174e5da38a3a86283c --- /dev/null +++ b/MotionPlanning/Saba.h @@ -0,0 +1,117 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_h_ +#define _Saba_h_ + + +/*! \defgroup Saba The Sampling-Based Motion Planning Library +The Sampling-based Motion Planning Library Saba offers state-of-the-art algorithms for planning collision-free motions +in high-dimensional configuration spaces. Standard approaches, as the Rapidly-exploring Random Trees (RRT), are efficiently implemented. +Furthermore motion planning algorithms for grasping and for bimanual tasks are provided, which can be used to solve plannign problems for +mobile manipulators or service and humanoid robots. +*/ + +#ifdef WIN32 + +// needed to have M_PI etc defined +#if !defined(_USE_MATH_DEFINES) +#define _USE_MATH_DEFINES +#endif + +// eigen wants this on windows +#if !defined(NOMINMAX) +#define NOMINMAX +#endif + +#endif + +#include "VirtualRobot/VirtualRobot.h" +#include "VirtualRobot/VirtualRobotException.h" +#include <boost/shared_ptr.hpp> +#include <boost/weak_ptr.hpp> +#include <iostream> +#include <sstream> +#include <cmath> +#include <Eigen/Core> +#include <Eigen/Geometry> + +#ifdef WIN32 +# include <winsock2.h> +# include <windows.h> +# pragma warning ( disable : 4251 ) +# if defined(Saba_EXPORTS) +# define SABA_IMPORT_EXPORT __declspec(dllexport) +# else +# define SABA_IMPORT_EXPORT __declspec(dllimport) +# endif +#else +# define SABA_IMPORT_EXPORT +#endif + + +namespace Saba +{ + // only valid within the Saba namespace + using std::cout; + using std::endl; + + class CSpace; + class CSpaceSampled; + class CSpacePath; + class CSpaceTree; + class CSpaceNode; + class Sampler; + class Rrt; + class BiRrt; + class GraspIkRrt; + class ShortcutProcessor; + + typedef boost::shared_ptr<CSpace> CSpacePtr; + typedef boost::shared_ptr<CSpaceSampled> CSpaceSampledPtr; + typedef boost::shared_ptr<CSpacePath> CSpacePathPtr; + typedef boost::shared_ptr<Sampler> SamplerPtr; + typedef boost::shared_ptr<CSpaceTree> CSpaceTreePtr; + typedef boost::shared_ptr<CSpaceNode> CSpaceNodePtr; + typedef boost::shared_ptr<Rrt> RrtPtr; + typedef boost::shared_ptr<BiRrt> BiRrtPtr; + typedef boost::shared_ptr<GraspIkRrt> GraspIkRrtPtr; + typedef boost::shared_ptr<ShortcutProcessor> ShortcutProcessorPtr; + +#define SABA_INFO VR_INFO +#define SABA_WARNING VR_WARNING +#define SABA_ERROR VR_ERROR + +#define THROW_SABA_EXCEPTION(a) THROW_VR_EXCEPTION(a) + +#ifdef _DEBUG +#define SABA_ASSERT(a) if (!(a)) {cout << "ASSERT failed (" << #a <<")"<<endl; THROW_SABA_EXCEPTION( "ASSERT failed (" << #a << ")" )}; +#define SABA_ASSERT_MESSAGE(a,b) if (!(a)) {cout << "ASSERT failed (" << #a <<"): "<<b<<endl; THROW_SABA_EXCEPTION( "ASSERT failed (" << #a << "): " << b )}; + +#else +#define SABA_ASSERT(a) +#define SABA_ASSERT_MESSAGE(a,b) +#endif + +} + +#endif // _Saba_h_ diff --git a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp new file mode 100644 index 0000000000000000000000000000000000000000..be50eae723c844a6831050debab471e6fec7c074 --- /dev/null +++ b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp @@ -0,0 +1,412 @@ + +#include "CoinRrtWorkspaceVisualization.h" +#include "CSpace/CSpacePath.h" +#include "CSpace/CSpaceTree.h" +#include "CSpace/CSpaceNode.h" + +#include <Inventor/SoPrimitiveVertex.h> +#include <Inventor/SbLinear.h> +#include <Inventor/nodes/SoShape.h> +#include <Inventor/actions/SoCallbackAction.h> +#include <Inventor/nodes/SoSeparator.h> +#include <Inventor/nodes/SoMatrixTransform.h> + + +#include <Inventor/nodes/SoComplexity.h> +#include <Inventor/nodes/SoMaterial.h> +#include <Inventor/nodes/SoDrawStyle.h> +#include <Inventor/nodes/SoLineSet.h> + +#include <Inventor/nodes/SoCoordinate3.h> +#include <Inventor/nodes/SoSphere.h> +#include <Inventor/nodes/SoBaseColor.h> +#include <Inventor/nodes/SoTranslation.h> +#include <Inventor/nodes/SoUnits.h> + + +namespace Saba { + +CoinRrtWorkspaceVisualization::CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string &TCPName) : +RrtWorkspaceVisualization(robot,cspace,TCPName) +{ + init(); +} + +CoinRrtWorkspaceVisualization::CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string &TCPName) : +RrtWorkspaceVisualization(robot,robotNodeSet,TCPName) +{ + init(); +} + +void CoinRrtWorkspaceVisualization::init() +{ + visualization = new SoSeparator(); + visualization->ref(); + setPathStyle(); + setTreeStyle(); + + RenderColors red; + red.nodeR = 1.0f; + red.nodeG = 0; + red.nodeB = 0; + red.lineR = 0.5f; + red.lineG = 0.5f; + red.lineB = 0.5f; + colors[CoinRrtWorkspaceVisualization::eRed] = red; + + RenderColors blue; + blue.nodeR = 0; + blue.nodeG = 0; + blue.nodeB = 1.0f; + blue.lineR = 0.5f; + blue.lineG = 0.5f; + blue.lineB = 0.5f; + colors[CoinRrtWorkspaceVisualization::eBlue] = blue; + + RenderColors green; + green.nodeR = 0; + green.nodeG = 1.0f; + green.nodeB = 0; + green.lineR = 0.5f; + green.lineG = 0.5f; + green.lineB = 0.5f; + colors[CoinRrtWorkspaceVisualization::eGreen] = green; + + RenderColors custom; + custom.nodeR = 1.0f; + custom.nodeG = 1.0f; + custom.nodeB = 0; + custom.lineR = 0.5f; + custom.lineG = 0.5f; + custom.lineB = 0.5f; + colors[CoinRrtWorkspaceVisualization::eCustom] = custom; + +} + +/** + * If CoinRrtWorkspaceVisualization::visualization is a valid object call SoNode::unref() + * on it. + */ +CoinRrtWorkspaceVisualization::~CoinRrtWorkspaceVisualization() +{ + if (visualization) + visualization->unref(); +} + + + +/** + * This mehtod returns the internal CoinRrtWorkspaceVisualization::visualization. + */ +SoSeparator* CoinRrtWorkspaceVisualization::getCoinVisualization() +{ + return visualization; +} + +void CoinRrtWorkspaceVisualization::setPathStyle(float lineSize, float nodeSize, float renderComplexity) +{ + pathLineSize = lineSize; + pathNodeSize = nodeSize; + pathRenderComplexity = renderComplexity; +} + +void CoinRrtWorkspaceVisualization::setTreeStyle(float lineSize, float nodeSize, float renderComplexity) +{ + treeLineSize = lineSize; + treeNodeSize = nodeSize; + treeRenderComplexity = renderComplexity; +} + +void CoinRrtWorkspaceVisualization::setCustomColor(float nodeR, float nodeG, float nodeB, float lineR, float lineG, float lineB ) +{ + RenderColors custom; + custom.nodeR = nodeR; + custom.nodeG = nodeG; + custom.nodeB = nodeB; + custom.lineR = lineR; + custom.lineG = lineG; + custom.lineB = lineB; + colors[CoinRrtWorkspaceVisualization::eCustom] = custom; +} + + +bool CoinRrtWorkspaceVisualization::addCSpacePath(CSpacePathPtr path, CoinRrtWorkspaceVisualization::ColorSet colorSet) +{ + if (!path) + return false; + if (path->getDimension() != robotNodeSet->getSize()) + { + VR_ERROR << " Dimensions do not match: " << path->getDimension() <<"!="<< robotNodeSet->getSize()<<endl; + return false; + } + if (!TCPNode) + return false; + + float nodeSolutionSize = pathNodeSize;//15.0;//1.0f + float lineSolutionSize = pathLineSize;//4.0; + SoMaterial *materialNodeSolution = new SoMaterial(); + SoMaterial *materialLineSolution = new SoMaterial(); + materialNodeSolution->ambientColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB); + materialNodeSolution->diffuseColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB); + materialLineSolution->ambientColor.setValue(colors[colorSet].lineR,colors[colorSet].lineG,colors[colorSet].lineB); + materialLineSolution->diffuseColor.setValue(colors[colorSet].lineR,colors[colorSet].lineG,colors[colorSet].lineB); + SoSphere *sphereNodeSolution = new SoSphere(); + sphereNodeSolution->radius.setValue(nodeSolutionSize); + SoDrawStyle *lineSolutionStyle = new SoDrawStyle(); + lineSolutionStyle->lineWidth.setValue(lineSolutionSize); + + Eigen::VectorXf actConfig; + Eigen::VectorXf parentConfig; + float x,y,z; + float x2 = 0.0f,y2 = 0.0f,z2 = 0.0f; + + SoSeparator *sep = new SoSeparator(); + + SoComplexity *comple; + comple = new SoComplexity(); + comple->value = pathRenderComplexity; + sep->addChild(comple); + + for (unsigned int i = 0; i < path->getNrOfPathPoints(); i++) + { + actConfig = path->getPathEntry(i); + + // create 3D model for nodes + SoSeparator *s = new SoSeparator(); + + s->addChild(materialNodeSolution); + SoTranslation *t = new SoTranslation(); + + if (cspace->hasExclusiveRobotAccess()) + CSpace::lock(); + + // get tcp coords: + robotNodeSet->setJointValues(actConfig); + Eigen::Matrix4f m; + m = TCPNode->getGlobalPose(); + x = m(0,3); + y = m(1,3); + z = m(2,3); + + if (cspace->hasExclusiveRobotAccess()) + CSpace::unlock(); + + t->translation.setValue(x,y,z); + s->addChild(t); + // display a solution node different + s->addChild(sphereNodeSolution); + sep->addChild(s); + + if (i>0) // lines for all configurations + { + // create line to parent + SoSeparator *s2 = new SoSeparator(); + + SbVec3f points[2]; + points[0].setValue(x2,y2,z2); + points[1].setValue(x,y,z); + + s2->addChild(lineSolutionStyle); + s2->addChild(materialLineSolution); + + SoCoordinate3* coordinate3 = new SoCoordinate3; + coordinate3->point.set1Value(0,points[0]); + coordinate3->point.set1Value(1,points[1]); + s2->addChild(coordinate3); + + SoLineSet* lineSet = new SoLineSet; + lineSet->numVertices.setValue(2); + lineSet->startIndex.setValue(0); + s2->addChild(lineSet); + + sep->addChild(s2); + } + x2 = x; + y2 = y; + z2 = z; + parentConfig = actConfig; + } // for + + visualization->addChild(sep); + return true; +} + +bool CoinRrtWorkspaceVisualization::addTree(CSpaceTreePtr tree, CoinRrtWorkspaceVisualization::ColorSet colorSet) +{ + if (!tree) + return false; + if (tree->getDimension() != robotNodeSet->getSize()) + { + VR_ERROR << " Dimensions do not match: " << tree->getDimension() <<"!="<< robotNodeSet->getSize()<<endl; + return false; + } + if (!TCPNode) + return false; + + Eigen::VectorXf actConfig; + Eigen::VectorXf parentConfig; + + CSpaceNodePtr actualNode; + CSpaceNodePtr parentNode; + int parentid; + + SoMaterial *materialNode = new SoMaterial(); + SoMaterial *materialLine = new SoMaterial(); + materialLine->ambientColor.setValue(colors[colorSet].lineR,colors[colorSet].lineG,colors[colorSet].lineB); + materialLine->diffuseColor.setValue(colors[colorSet].lineR,colors[colorSet].lineG,colors[colorSet].lineB); + materialNode->ambientColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB); + materialNode->diffuseColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB); + + SoSphere *sphereNode = new SoSphere(); + sphereNode->radius.setValue(treeNodeSize); + + SoDrawStyle *lineStyle = new SoDrawStyle(); + lineStyle->lineWidth.setValue(treeLineSize); + + SoSeparator *sep = new SoSeparator(); + + SoComplexity *comple; + comple = new SoComplexity(); + comple->value = treeRenderComplexity; + sep->addChild(comple); + + std::vector<CSpaceNodePtr> nodes = tree->getNodes(); + + // pre-compute tcp positions + bool updateVisMode = robot->getUpdateVisualizationStatus(); + robot->setUpdateVisualization(false); + + std::map<CSpaceNodePtr,Eigen::Vector3f> tcpCoords; + Eigen::Vector3f p; + Eigen::Vector3f p2; + for (unsigned int i = 0; i < nodes.size(); i++) + { + actualNode = nodes[i]; + + // get tcp coords: + robotNodeSet->setJointValues(actualNode->configuration); + Eigen::Matrix4f m; + m = TCPNode->getGlobalPose(); + p(0) = m(0,3); + p(1) = m(1,3); + p(2) = m(2,3); + + tcpCoords[actualNode] = p; + } + + + for (unsigned int i = 0; i < nodes.size(); i++) + { + + actualNode = nodes[i]; + parentid = actualNode->parentID; + + // create 3D model for nodes + SoSeparator *s = new SoSeparator(); + + + s->addChild(materialNode); + + // get tcp coords + p = tcpCoords[actualNode]; + + SoTranslation *t = new SoTranslation(); + t->translation.setValue(p(0),p(1),p(2)); + s->addChild(t); + s->addChild(sphereNode); + sep->addChild(s); + + // not for the start node! startNode->parentID < 0 + if (parentid >= 0) // lines for all configurations + { + // create line to parent + SoSeparator *s2 = new SoSeparator(); + parentNode = tree->getNode(parentid); + if (parentNode) + { + + // get tcp coords + p2 = tcpCoords[parentNode]; + + s2->addChild(lineStyle); + s2->addChild(materialLine); + + SbVec3f points[2]; + points[0].setValue(p(0),p(1),p(2)); + points[1].setValue(p2(0),p2(1),p2(2)); + + SoCoordinate3* coordinate3 = new SoCoordinate3; + coordinate3->point.set1Value(0,points[0]); + coordinate3->point.set1Value(1,points[1]); + s2->addChild(coordinate3); + + SoLineSet* lineSet = new SoLineSet; + lineSet->numVertices.setValue(2); + lineSet->startIndex.setValue(0); + s2->addChild(lineSet); + + sep->addChild(s2); + } + } + } + visualization->addChild(sep); + + robot->setUpdateVisualization(updateVisMode); + + return true; +} + +void CoinRrtWorkspaceVisualization::reset() +{ + if (visualization) + visualization->removeAllChildren(); +} + +bool CoinRrtWorkspaceVisualization::addConfiguration( const Eigen::VectorXf &c, CoinRrtWorkspaceVisualization::ColorSet colorSet, float nodeSizeFactor ) +{ + if (c.rows() != robotNodeSet->getSize()) + { + VR_ERROR << " Dimensions do not match: " << c.rows() <<"!="<< robotNodeSet->getSize() << endl; + return false; + } + if (!TCPNode) + return false; + + float nodeSolutionSize = pathNodeSize*nodeSizeFactor;//15.0;//1.0f + SoMaterial *materialNodeSolution = new SoMaterial(); + materialNodeSolution->ambientColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB); + materialNodeSolution->diffuseColor.setValue(colors[colorSet].nodeR,colors[colorSet].nodeG,colors[colorSet].nodeB); + SoSphere *sphereNodeSolution = new SoSphere(); + sphereNodeSolution->radius.setValue(nodeSolutionSize); + + Eigen::VectorXf actConfig; + float x,y,z; + float x2 = 0.0f,y2 = 0.0f,z2 = 0.0f; + + SoSeparator *sep = new SoSeparator(); + + // create 3D model for nodes + SoSeparator *s = new SoSeparator(); + + s->addChild(materialNodeSolution); + SoTranslation *t = new SoTranslation(); + + // get tcp coords: + robotNodeSet->setJointValues(c); + Eigen::Matrix4f m; + m = TCPNode->getGlobalPose(); + x = m(0,3);//[0][3]; + y = m(1,3);//m[1][3]; + z = m(2,3);//m[2][3]; + + t->translation.setValue(x,y,z); + s->addChild(t); + // display a solution node different + s->addChild(sphereNodeSolution); + sep->addChild(s); + + visualization->addChild(sep); + return true; +} + +} // namespace Saba diff --git a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h new file mode 100644 index 0000000000000000000000000000000000000000..f240af8fb9eff9819f7690d50cbdfb4c21d67862 --- /dev/null +++ b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h @@ -0,0 +1,107 @@ +//---------------------------------------------------------------------- +/*! + * \ingroup Saba + * + * + * \author Nikolaus Vahrenkamp + * \date 2011-04-04 + * + * \class CoinRrtWorkspaceVisualization + * + * A Coin3D related visualization of an RRT search tree. + * + * Copyright Nikolaus Vahrenkamp + * Karlsruhe Institute of Technology (KIT) - HIS + + */ +//---------------------------------------------------------------------- + + +#ifndef _Saba_CoinRrtWorkspaceVisualization_h_ +#define _Saba_CoinRrtWorkspaceVisualization_h_ + + +#include "../../Saba.h" +#include "../RrtWorkspaceVisualization.h" + +#include <boost/shared_ptr.hpp> + +class SoNode; +class SoSeparator; +class SoCallbackAction; +class SoPrimitiveVertex; + +namespace Saba +{ + +class SABA_IMPORT_EXPORT CoinRrtWorkspaceVisualization : virtual public RrtWorkspaceVisualization +{ +public: + /*! + Constructor + Robot must have a node with name TCPName. + The visualizations are build by determining the TCP's position in workspace according to the configurations of a path or tree . + */ + CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string &TCPName); + CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string &TCPName); + + ~CoinRrtWorkspaceVisualization(); + + enum ColorSet + { + eRed, + eGreen, + eBlue, + eCustom + }; + + /*! + Add visualization of a path in cspace. + */ + virtual bool addCSpacePath(CSpacePathPtr path, CoinRrtWorkspaceVisualization::ColorSet colorSet = eBlue); + void setPathStyle(float lineSize = 4.0f, float nodeSize= 15.0f, float renderComplexity = 1.0f); + + /*! + Add visualization of a path in cspace. + */ + virtual bool addTree(CSpaceTreePtr tree, CoinRrtWorkspaceVisualization::ColorSet colorSet = eRed); + void setTreeStyle(float lineSize = 1.0f, float nodeSize= 15.0f, float renderComplexity = 0.1f); + + /*! + Add visualization of a configuration in cspace. + */ + virtual bool addConfiguration(const Eigen::VectorXf &c, CoinRrtWorkspaceVisualization::ColorSet colorSet = eGreen, float nodeSizeFactor = 1.0f); + + /*! + Set the custom line and node color. Does not affect already added trees or paths. + */ + void setCustomColor(float nodeR, float nodeG, float nodeB, float lineR = 0.5f, float lineG = 0.5f, float lineB = 0.5f); + + /*! + Clears all visualizations. + */ + virtual void reset(); + + + SoSeparator* getCoinVisualization(); + +protected: + + void init(); + + SoSeparator* visualization; + + float pathLineSize, pathNodeSize, pathRenderComplexity; + float treeLineSize, treeNodeSize, treeRenderComplexity; + + struct RenderColors + { + float nodeR, nodeG, nodeB, lineR, lineG, lineB; + }; + + std::map<ColorSet, RenderColors> colors; +}; + +} // namespace Saba + +#endif // _Saba_CoinRrtWorkspaceVisualization_h_ diff --git a/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp b/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp new file mode 100644 index 0000000000000000000000000000000000000000..65ca4cb37163d7c36993e4829160cc2e7ebae675 --- /dev/null +++ b/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp @@ -0,0 +1,63 @@ + +#include "RrtWorkspaceVisualization.h" +#include "CSpace/CSpace.h" +#include "VirtualRobot/Nodes/RobotNode.h" + +namespace Saba { + +RrtWorkspaceVisualization::RrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string &TCPName) +{ + this->robot = robot; + this->cspace = cspace; + SABA_ASSERT(robot); + SABA_ASSERT(cspace); + robotNodeSet = cspace->getRobotNodeSet(); + SABA_ASSERT(robotNodeSet); + setTCPName(TCPName); +} + + +RrtWorkspaceVisualization::RrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string &TCPName) +{ + this->robot = robot; + cspace = CSpacePtr(); + SABA_ASSERT(robot); + SABA_ASSERT(robotNodeSet); + this->robotNodeSet = robotNodeSet; + setTCPName(TCPName); +} + +RrtWorkspaceVisualization::~RrtWorkspaceVisualization() +{ +} +/* +bool RrtWorkspaceVisualization::addCSpacePath(CSpacePathPtr path) +{ + return false; +} + +bool RrtWorkspaceVisualization::addTree(CSpaceTreePtr tree) +{ + return false; +}*/ + +void RrtWorkspaceVisualization::reset() +{ +} + +void RrtWorkspaceVisualization::setTCPName(const std::string TCPName) +{ + this->TCPName = TCPName; + TCPNode = robot->getRobotNode(TCPName); + if (!TCPNode) + { + VR_ERROR << "No node with name " << TCPName << " available in robot.." << endl; + } +} +/* +bool RrtWorkspaceVisualization::addConfig( const Eigen::VectorXf &c ) +{ + return false; +}*/ + +} // namespace Saba diff --git a/MotionPlanning/Visualization/RrtWorkspaceVisualization.h b/MotionPlanning/Visualization/RrtWorkspaceVisualization.h new file mode 100644 index 0000000000000000000000000000000000000000..c276a7b1a3df3f09f6fd1cbac54093574b5b8c4c --- /dev/null +++ b/MotionPlanning/Visualization/RrtWorkspaceVisualization.h @@ -0,0 +1,95 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package Saba +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#ifndef _Saba_RrtWorkspaceVisualization_h_ +#define _Saba_RrtWorkspaceVisualization_h_ + +#include "../Saba.h" +#include "VirtualRobot/VirtualRobot.h" +#include "VirtualRobot/Robot.h" +#include <Eigen/Core> +#include <Eigen/Geometry> + +namespace Saba +{ + +/*! + * + * A visualization of an RRT search tree. + * @see CoinRrtWorkspaceVisualization + * + */ +class SABA_IMPORT_EXPORT RrtWorkspaceVisualization +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /*! + Constructor + Robot must have a node with name TCPName. + The visualizations are build by determining the TCP's position in workspace according to the configurations of a path or tree . + */ + RrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string &TCPName); + RrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string &TCPName); + + /*! + */ + virtual ~RrtWorkspaceVisualization(); + + /*! + Add visualization of a path in cspace. + */ + //virtual bool addCSpacePath(CSpacePathPtr path); + + /*! + Add visualization of a path in cspace. + */ + //virtual bool addTree(CSpaceTreePtr tree); + + /*! + Add visualization of a path in cspace. + */ + //virtual bool addConfig(const Eigen::VectorXf &c); + + /*! + Clears all visualizations. + */ + virtual void reset(); + + /*! + Set name of TCP joint. Does not affect already added paths or trees. + */ + void setTCPName(const std::string TCPName); + +protected: + VirtualRobot::RobotPtr robot; + CSpacePtr cspace; + VirtualRobot::RobotNodeSetPtr robotNodeSet; + VirtualRobot::RobotNodePtr TCPNode; + + std::string TCPName; + +}; + +} // namespace Saba + +#endif // _Saba_RrtWorkspaceVisualization_h_ diff --git a/MotionPlanning/config.cmake b/MotionPlanning/config.cmake new file mode 100644 index 0000000000000000000000000000000000000000..75fa883227f3f511c2ac0f1d373201953a306203 --- /dev/null +++ b/MotionPlanning/config.cmake @@ -0,0 +1,48 @@ +GET_FILENAME_COMPONENT (CurrentSabaPath ${CMAKE_CURRENT_LIST_FILE} PATH) +SET(SABA_DIR ${CurrentSabaPath}) + +############################# SETUP PATHS TO Simox ############################# +SET(SIMOX_DIR_STANDARD "${CurrentSabaPath}/..") +# be sure to have the absolute path +get_filename_component(SIMOX_DIR_STANDARD ${SIMOX_DIR_STANDARD} ABSOLUTE) + +SET (SABA_SimoxDir ${SIMOX_DIR_STANDARD} CACHE STRING "Path to Simox used by SaBa") + +INCLUDE(${SABA_SimoxDir}/config.cmake) + +IF(NOT DEFINED SIMOX_BUILD_DIRECTORY) + get_filename_component(SIMOX_BUILD_DIRECTORY ${SIMOX_BUILD_DIRECTORY} ABSOLUTE) + SET(SIMOX_BUILD_DIRECTORY "${SABA_SimoxDir}/build" CACHE STRING "Simox build directory used by SaBa") + SET(SIMOX_LIB_DIR ${SIMOX_BUILD_DIRECTORY}/lib) + SET(SIMOX_BIN_DIR ${SIMOX_BUILD_DIRECTORY}/bin) +ENDIF() + +############################# SETUP PATHS ############################# +ADD_DEFINITIONS(-DSABA_BASE_DIR="${SABA_DIR}") + +# Define, where to put the binaries +SET(SABA_LIB_DIR ${SIMOX_LIB_DIR}) +SET(SABA_BIN_DIR ${SIMOX_BIN_DIR}) +MESSAGE(STATUS "** SABA_LIB_DIR: ${SABA_LIB_DIR}") +MESSAGE(STATUS "** SABA_BIN_DIR: ${SABA_BIN_DIR}") + +# Define, where to install the binaries +# Define, where to install the binaries +SET(SABA_INSTALL_LIB_DIR ${SIMOX_INSTALL_LIB_DIR}) +SET(SABA_INSTALL_BIN_DIR ${SIMOX_INSTALL_BIN_DIR}) +SET(SABA_INSTALL_HEADER_DIR ${SIMOX_INSTALL_HEADER_DIR}) + +####################################################################### +# Setup for testing +####################################################################### +ENABLE_TESTING() +INCLUDE(CTest) + +MACRO(ADD_SABA_TEST TEST_NAME) + ADD_EXECUTABLE(${TEST_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_NAME}.cpp) + TARGET_LINK_LIBRARIES(${TEST_NAME} VirtualRobot Saba ${VIRTUAL_ROBOT_LINK_LIBRARIES}) + IF(NOT UNIX) + SET_TARGET_PROPERTIES(${TEST_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${SIMOX_BIN_DIR}) + ENDIF(NOT UNIX) + ADD_TEST( Saba_${TEST_NAME} ${TEST_NAME} --output_format=XML --log_level=all --report_level=no) +ENDMACRO(ADD_SABA_TEST) diff --git a/MotionPlanning/examples/CMakeLists.txt b/MotionPlanning/examples/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..8f7c8c78a21670f2def7cb55d1c60ae3ccb66fbe --- /dev/null +++ b/MotionPlanning/examples/CMakeLists.txt @@ -0,0 +1,6 @@ + +if (SOQT_FOUND) + ADD_SUBDIRECTORY(RRT) + ADD_SUBDIRECTORY(RrtGui) + ADD_SUBDIRECTORY(IKRRT) +endif() diff --git a/MotionPlanning/examples/IKRRT/CMakeLists.txt b/MotionPlanning/examples/IKRRT/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..259e33ba8d7772504b7121a406d4af4316d7341f --- /dev/null +++ b/MotionPlanning/examples/IKRRT/CMakeLists.txt @@ -0,0 +1,46 @@ +PROJECT ( IKRRT ) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4) + +CMAKE_POLICY(VERSION 2.6) + +IF(NOT DEFINED VR_CONFIGURED) + INCLUDE(../../config.cmake) +ENDIF() + +IF(SIMOX_USE_COIN_VISUALIZATION) + LINK_DIRECTORIES(${SIMOX_LIB_DIR}) + + + # the variable "demo_SRCS" contains all .cpp files of this project + FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/IKRRTDemo.cpp ${PROJECT_SOURCE_DIR}/IKRRTWindow.cpp) + FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/IKRRTWindow.h) + + +################################## moc'ing ############################## + set(GUI_MOC_HDRS + ${PROJECT_SOURCE_DIR}/IKRRTWindow.h + ) + + set(GUI_UIS + ${PROJECT_SOURCE_DIR}/IKRRT.ui + ) + + qt4_wrap_cpp(demo_SRCS ${GUI_MOC_HDRS}) + qt4_wrap_ui(UI_HEADER ${GUI_UIS}) + get_filename_component(UI_HEADER_DIR ${UI_HEADER} PATH) + list(APPEND demo_INCS ${UI_HEADER}) + + include_directories(${UI_HEADER_DIR}) + + + ADD_EXECUTABLE(${PROJECT_NAME} ${demo_SRCS} ${demo_INCS}) + SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${SIMOX_BIN_DIR}) + + TARGET_LINK_LIBRARIES(${PROJECT_NAME} VirtualRobot Saba ${QT_LIBRARIES} ${COIN3D_LIBRARIES} ${SoQt_LIBRARIES}) + INCLUDE_DIRECTORIES(${SoQt_INCLUDE_DIRS}) + INCLUDE(${QT_USE_FILE}) + ADD_DEFINITIONS(-DSOQT_DLL) + + MESSAGE( STATUS ${PROJECT_NAME} " will be placed into " ${SIMOX_BIN_DIR}) +ENDIF(SIMOX_USE_COIN_VISUALIZATION) diff --git a/MotionPlanning/examples/IKRRT/IKRRT.ui b/MotionPlanning/examples/IKRRT/IKRRT.ui new file mode 100644 index 0000000000000000000000000000000000000000..cdc67a5637b373ca6d57d920fe54e3b5e571c551 --- /dev/null +++ b/MotionPlanning/examples/IKRRT/IKRRT.ui @@ -0,0 +1,476 @@ +<?xml version="1.0" encoding="UTF-8"?> +<ui version="4.0"> + <class>MainWindowIKRRT</class> + <widget class="QMainWindow" name="MainWindowIKRRT"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1079</width> + <height>819</height> + </rect> + </property> + <property name="windowTitle"> + <string> Saba - IK-RRT</string> + </property> + <widget class="QWidget" name="centralwidget"> + <layout class="QGridLayout" name="gridLayout"> + <item row="0" column="0"> + <widget class="QFrame" name="frameViewer"> + <property name="frameShape"> + <enum>QFrame::StyledPanel</enum> + </property> + <property name="frameShadow"> + <enum>QFrame::Raised</enum> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QGroupBox" name="groupBox"> + <property name="maximumSize"> + <size> + <width>200</width> + <height>16777215</height> + </size> + </property> + <property name="title"> + <string/> + </property> + <widget class="QPushButton" name="pushButtonReset"> + <property name="geometry"> + <rect> + <x>20</x> + <y>20</y> + <width>171</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Reset</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonClose"> + <property name="geometry"> + <rect> + <x>30</x> + <y>170</y> + <width>151</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Close EEF</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonOpen"> + <property name="geometry"> + <rect> + <x>30</x> + <y>210</y> + <width>151</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Open EEF</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderX"> + <property name="geometry"> + <rect> + <x>39</x> + <y>470</y> + <width>151</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_3"> + <property name="geometry"> + <rect> + <x>20</x> + <y>450</y> + <width>81</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Move Object</string> + </property> + </widget> + <widget class="QLabel" name="label_4"> + <property name="geometry"> + <rect> + <x>20</x> + <y>470</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>X</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderY"> + <property name="geometry"> + <rect> + <x>39</x> + <y>490</y> + <width>151</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_5"> + <property name="geometry"> + <rect> + <x>20</x> + <y>490</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Y</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderZ"> + <property name="geometry"> + <rect> + <x>39</x> + <y>510</y> + <width>151</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_6"> + <property name="geometry"> + <rect> + <x>20</x> + <y>510</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Z</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderRo"> + <property name="geometry"> + <rect> + <x>39</x> + <y>530</y> + <width>151</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_7"> + <property name="geometry"> + <rect> + <x>20</x> + <y>530</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Ro</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderPi"> + <property name="geometry"> + <rect> + <x>39</x> + <y>550</y> + <width>151</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_8"> + <property name="geometry"> + <rect> + <x>20</x> + <y>550</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Pi</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderYa"> + <property name="geometry"> + <rect> + <x>39</x> + <y>570</y> + <width>151</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>-50</number> + </property> + <property name="maximum"> + <number>50</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label_9"> + <property name="geometry"> + <rect> + <x>20</x> + <y>570</y> + <width>16</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Ya</string> + </property> + </widget> + <widget class="QLabel" name="label_10"> + <property name="geometry"> + <rect> + <x>10</x> + <y>600</y> + <width>81</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Visualization</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxColModel"> + <property name="geometry"> + <rect> + <x>20</x> + <y>620</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Collision Model</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxGraspSet"> + <property name="geometry"> + <rect> + <x>20</x> + <y>660</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show Grasp Set</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxTCP"> + <property name="geometry"> + <rect> + <x>20</x> + <y>700</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show TCP Coord System</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxReachableGrasps"> + <property name="geometry"> + <rect> + <x>20</x> + <y>680</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show Reachable Grasps</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxReachabilitySpace"> + <property name="geometry"> + <rect> + <x>20</x> + <y>640</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Reachability Space</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonIK"> + <property name="geometry"> + <rect> + <x>50</x> + <y>60</y> + <width>111</width> + <height>31</height> + </rect> + </property> + <property name="text"> + <string>search IK</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxReachabilitySpaceIK"> + <property name="geometry"> + <rect> + <x>30</x> + <y>100</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Use Reachability Space</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxColCheckIK"> + <property name="geometry"> + <rect> + <x>30</x> + <y>130</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Collision Detection</string> + </property> + </widget> + <widget class="QPushButton" name="pushButtonIKRRT"> + <property name="geometry"> + <rect> + <x>40</x> + <y>280</y> + <width>121</width> + <height>31</height> + </rect> + </property> + <property name="text"> + <string>Grasp IK-RRT</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderSolution"> + <property name="geometry"> + <rect> + <x>20</x> + <y>360</y> + <width>171</width> + <height>20</height> + </rect> + </property> + <property name="minimum"> + <number>0</number> + </property> + <property name="maximum"> + <number>1000</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLabel" name="label"> + <property name="geometry"> + <rect> + <x>20</x> + <y>340</y> + <width>101</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Solution:</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxSolution"> + <property name="geometry"> + <rect> + <x>20</x> + <y>720</y> + <width>141</width> + <height>17</height> + </rect> + </property> + <property name="text"> + <string>Show Solution</string> + </property> + </widget> + </widget> + </item> + </layout> + </widget> + <widget class="QMenuBar" name="menubar"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1079</width> + <height>21</height> + </rect> + </property> + </widget> + <widget class="QStatusBar" name="statusbar"/> + </widget> + <resources/> + <connections/> +</ui> diff --git a/MotionPlanning/examples/IKRRT/IKRRTDemo.cpp b/MotionPlanning/examples/IKRRT/IKRRTDemo.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6d9576b939565dfa0a5abee7a2dde6835f8a5d42 --- /dev/null +++ b/MotionPlanning/examples/IKRRT/IKRRTDemo.cpp @@ -0,0 +1,81 @@ +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/XML/RobotIO.h> +#include <VirtualRobot/Visualization/VisualizationFactory.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <VirtualRobot/RuntimeEnvironment.h> + + +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/nodes/SoSeparator.h> +#include <Inventor/Qt/SoQt.h> +#include <boost/shared_ptr.hpp> +#include <string> +#include <iostream> + +using std::cout; +using std::endl; +using namespace VirtualRobot; + +#include <Eigen/Core> +#include <Eigen/Geometry> + +#include "IKRRTWindow.h" + + +int main(int argc, char *argv[]) +{ + SoDB::init(); + SoQt::init(argc,argv,"IKRRT"); + cout << " --- START --- " << endl; + + std::string filenameScene(SIMOX_BASE_DIR "/VirtualRobot/data/scenes/IKRRT_scene_iCub.xml"); + std::string filenameReach(SIMOX_BASE_DIR "/VirtualRobot/data/reachability/iCub_HipLeftArm.bin"); + std::string kinChain("Hip Left Arm"); + std::string eef("Left Hand"); + std::string colModel("Left HandArm ColModel"); + std::string colModelRob("BodyHeadLegsColModel"); + + VirtualRobot::RuntimeEnvironment::considerKey("scene"); + VirtualRobot::RuntimeEnvironment::considerKey("reachability"); + VirtualRobot::RuntimeEnvironment::considerKey("kinematicChain"); + VirtualRobot::RuntimeEnvironment::considerKey("endEffector"); + VirtualRobot::RuntimeEnvironment::considerKey("collisionModelKinChain"); + VirtualRobot::RuntimeEnvironment::considerKey("collisionModelRobot"); + VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv); + VirtualRobot::RuntimeEnvironment::print(); + + std::string scFile = VirtualRobot::RuntimeEnvironment::getValue("scene"); + if (!scFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(scFile)) + filenameScene = scFile; + std::string reachFile = VirtualRobot::RuntimeEnvironment::getValue("reachability"); + if (!reachFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(reachFile)) + filenameReach = reachFile; + std::string kc = VirtualRobot::RuntimeEnvironment::getValue("kinematicChain"); + if (!kc.empty()) + kinChain = kc; + std::string EndEff = VirtualRobot::RuntimeEnvironment::getValue("endEffector"); + if (!EndEff.empty()) + eef = EndEff; + std::string col1 = VirtualRobot::RuntimeEnvironment::getValue("collisionModelKinChain"); + if (!col1.empty()) + colModel = col1; + std::string col2 = VirtualRobot::RuntimeEnvironment::getValue("collisionModelRobot"); + if (!col2.empty()) + colModelRob = col2; + + + cout << "Using scene at " << filenameScene << endl; + cout << "Using reachability at " << filenameReach << endl; + cout << "Using end effector " << eef << endl; + cout << "Using col model (kin chain) " << colModel << endl; + cout << "Using col model (static robot)" << colModelRob << endl; + + IKRRTWindow rw(filenameScene,filenameReach,kinChain,eef,colModel,colModelRob); + + rw.main(); + + return 0; + +} diff --git a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp new file mode 100644 index 0000000000000000000000000000000000000000..63eaebeb3358a888e784d9e7466a1ab88fa025b8 --- /dev/null +++ b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp @@ -0,0 +1,623 @@ + +#include "IKRRTWindow.h" +#include "VirtualRobot/EndEffector/EndEffector.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/ManipulationObject.h" +#include "VirtualRobot/Grasp.h" +#include "VirtualRobot/IK/GenericIKSolver.h" +#include "VirtualRobot/GraspSet.h" +#include "VirtualRobot/CollisionDetection/CDManager.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/XML/ObjectIO.h" +#include "VirtualRobot/XML/RobotIO.h" +#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h" +#include "MotionPlanning/Planner/GraspIkRrt.h" +#include "MotionPlanning/CSpace/CSpaceSampled.h" +#include "MotionPlanning/PostProcessing/ShortcutProcessor.h" +#include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h> +#include <QFileDialog> +#include <Eigen/Geometry> + +#include <time.h> +#include <vector> +#include <iostream> +#include <cmath> + +#include "Inventor/actions/SoLineHighlightRenderAction.h" +#include <Inventor/nodes/SoShapeHints.h> +#include <Inventor/nodes/SoLightModel.h> +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/nodes/SoMatrixTransform.h> + +#include <sstream> +using namespace std; +using namespace VirtualRobot; + +float TIMER_MS = 30.0f; + +IKRRTWindow::IKRRTWindow(std::string &sceneFile, std::string &reachFile, std::string &rns, std::string &eef, std::string &colModel, std::string &colModelRob, Qt::WFlags flags) +:QMainWindow(NULL) +{ + VR_INFO << " start " << endl; + + this->sceneFile = sceneFile; + this->reachFile = reachFile; + eefName = eef; + rnsName = rns; + this->colModelName = colModel; + this->colModelNameRob = colModelRob; + + sceneSep = new SoSeparator; + sceneSep->ref(); + robotSep = new SoSeparator; + objectSep = new SoSeparator; + graspsSep = new SoSeparator; + reachableGraspsSep = new SoSeparator; + reachabilitySep = new SoSeparator; + obstaclesSep = new SoSeparator; + rrtSep = new SoSeparator; + + //sceneSep->addChild(robotSep); + + sceneSep->addChild(robotSep); + sceneSep->addChild(objectSep); + sceneSep->addChild(graspsSep); + sceneSep->addChild(reachableGraspsSep); + sceneSep->addChild(reachabilitySep); + sceneSep->addChild(obstaclesSep); + sceneSep->addChild(rrtSep); + + setupUI(); + + loadScene(); + + loadReach(); + + m_pExViewer->viewAll(); + + SoSensorManager *sensor_mgr = SoDB::getSensorManager(); + SoTimerSensor *timer = new SoTimerSensor(timerCB, this); + timer->setInterval(SbTime(TIMER_MS/1000.0f)); + sensor_mgr->insertTimerSensor(timer); +} + + +IKRRTWindow::~IKRRTWindow() +{ + sceneSep->unref(); +} + + +void IKRRTWindow::timerCB(void * data, SoSensor * sensor) +{ + IKRRTWindow *ikWindow = static_cast<IKRRTWindow*>(data); + float x[6]; + x[0] = (float)ikWindow->UI.horizontalSliderX->value(); + x[1] = (float)ikWindow->UI.horizontalSliderY->value(); + x[2] = (float)ikWindow->UI.horizontalSliderZ->value(); + x[3]= (float)ikWindow->UI.horizontalSliderRo->value(); + x[4] = (float)ikWindow->UI.horizontalSliderPi->value(); + x[5] = (float)ikWindow->UI.horizontalSliderYa->value(); + x[0] /= 10.0f; + x[1] /= 10.0f; + x[2] /= 10.0f; + x[3] /= 300.0f; + x[4] /= 300.0f; + x[5] /= 300.0f; + + if (x[0]!=0 || x[1]!=0 || x[2]!=0 || x[3]!=0 || x[4]!=0 || x[5]!=0) + ikWindow->updateObject(x); + ikWindow->redraw(); + +} + + +void IKRRTWindow::setupUI() +{ + UI.setupUi(this); + m_pExViewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP); + + // setup + m_pExViewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f)); + m_pExViewer->setAccumulationBuffer(true); +#ifdef WIN32 +#ifndef _DEBUG + m_pExViewer->setAntialiasing(true, 4); +#endif +#endif + m_pExViewer->setGLRenderAction(new SoLineHighlightRenderAction); + m_pExViewer->setTransparencyType(SoGLRenderAction::BLEND); + m_pExViewer->setFeedbackVisibility(true); + m_pExViewer->setSceneGraph(sceneSep); + m_pExViewer->viewAll(); + + connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll())); + connect(UI.pushButtonClose, SIGNAL(clicked()), this, SLOT(closeEEF())); + connect(UI.pushButtonOpen, SIGNAL(clicked()), this, SLOT(openEEF())); + connect(UI.pushButtonIK, SIGNAL(clicked()), this, SLOT(searchIK())); + + connect(UI.checkBoxSolution, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxTCP, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(colModel())); + connect(UI.checkBoxGraspSet, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxReachableGrasps, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxReachabilitySpace, SIGNAL(clicked()), this, SLOT(reachVisu())); + connect(UI.pushButtonIKRRT, SIGNAL(clicked()), this, SLOT(planIKRRT())); + + connect(UI.horizontalSliderX, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectX())); + connect(UI.horizontalSliderY, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectY())); + connect(UI.horizontalSliderZ, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectZ())); + connect(UI.horizontalSliderRo, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectA())); + connect(UI.horizontalSliderPi, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectB())); + connect(UI.horizontalSliderYa, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectG())); + connect(UI.horizontalSliderSolution, SIGNAL(sliderMoved(int)), this, SLOT(sliderSolution(int))); + + UI.checkBoxColCheckIK->setChecked(true); + UI.checkBoxReachabilitySpaceIK->setChecked(true); + +} + +QString IKRRTWindow::formatString(const char *s, float f) +{ + QString str1(s); + if (f>=0) + str1 += " "; + if (fabs(f)<1000) + str1 += " "; + if (fabs(f)<100) + str1 += " "; + if (fabs(f)<10) + str1 += " "; + QString str1n; + str1n.setNum(f,'f',3); + str1 = str1 + str1n; + return str1; +} + + +void IKRRTWindow::resetSceneryAll() +{ + if (rns) + rns->setJointValues(startConfig); +} + + +void IKRRTWindow::closeEvent(QCloseEvent *event) +{ + quit(); + QMainWindow::closeEvent(event); +} + + +void IKRRTWindow::buildVisu() +{ + showCoordSystem(); + + robotSep->removeAllChildren(); + //bool colModel = (UI.checkBoxColModel->isChecked()); + SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked())?SceneObject::Collision:SceneObject::Full; + + if (robot) + { + visualizationRobot = robot->getVisualization<CoinVisualization>(colModel); + SoNode* visualisationNode = visualizationRobot->getCoinVisualization(); + if (visualisationNode) + { + robotSep->addChild(visualisationNode); + //visualizationRobot->highlight(true); + } + } + + objectSep->removeAllChildren(); + if (object) + { + SceneObject::VisualizationType colModel2 = (UI.checkBoxColModel->isChecked())?SceneObject::Collision:SceneObject::Full; + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(object,colModel2); + if (visualisationNode) + objectSep->addChild(visualisationNode); + } + + obstaclesSep->removeAllChildren(); + if (obstacles.size()>0) + { + for (size_t i=0;i<obstacles.size();i++) + { + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(obstacles[i],colModel); + if (visualisationNode) + obstaclesSep->addChild(visualisationNode); + } + } + buildGraspSetVisu(); + + buildRRTVisu(); + + + m_pExViewer->scheduleRedraw(); + m_pExViewer->scheduleRedraw(); + +} + +int IKRRTWindow::main() +{ + SoQt::show(this); + SoQt::mainLoop(); + return 0; +} + + +void IKRRTWindow::quit() +{ + std::cout << "IKRRTWindow: Closing" << std::endl; + this->close(); + SoQt::exitMainLoop(); +} + +void IKRRTWindow::loadScene() +{ + graspSet.reset(); + robot.reset(); + object.reset(); + obstacles.clear(); + eef.reset(); + ScenePtr scene = SceneIO::loadScene(sceneFile); + if (!scene) + { + VR_ERROR << " no scene ..." << endl; + return; + } + std::vector< RobotPtr > robots = scene->getRobots(); + if (robots.size()!=1) + { + VR_ERROR << "Need exactly 1 robot" << endl; + return; + } + robot = robots[0]; + + + std::vector< ManipulationObjectPtr > objects = scene->getManipulationObjects(); + if (objects.size()!=1) + { + VR_ERROR << "Need exactly 1 object" << endl; + return; + } + object = objects[0]; + + + obstacles = scene->getObstacles(); + + if (robot && object) + { + eef = robot->getEndEffector(eefName); + if (!eef) + { + VR_ERROR << "Need a correct EEF in robot" << endl; + return; + } + graspSet = object->getGraspSet(eef); + + rns = robot->getRobotNodeSet(rnsName); + + if (!rns) + { + VR_ERROR << "Need a correct RNS in robot" << endl; + } + + } + if (rns) + rns->getJointValues(startConfig); + buildVisu(); + +} + + +void IKRRTWindow::closeEEF() +{ + if (eef) + { + eef->closeActors(object); + } + m_pExViewer->scheduleRedraw(); +} + +void IKRRTWindow::openEEF() +{ + if (eef) + { + eef->openActors(); + } + m_pExViewer->scheduleRedraw(); +} + + + +void IKRRTWindow::updateObject( float x[6] ) +{ + if (object) + { + //cout << "getGlobalPose robot:" << endl << robotEEF->getGlobalPose() << endl; + //cout << "getGlobalPose TCP:" << endl << robotEEF_EEF->getTcp()->getGlobalPose() << endl; + Eigen::Matrix4f m; + MathTools::posrpy2eigen4f(x,m); + + m = object->getGlobalPose() * m; + object->setGlobalPose(m); + cout << "object " << endl; + cout << m << endl; + + } + m_pExViewer->scheduleRedraw(); +} + +void IKRRTWindow::sliderReleased_ObjectX() +{ + UI.horizontalSliderX->setValue(0); + buildVisu(); +} + +void IKRRTWindow::sliderReleased_ObjectY() +{ + UI.horizontalSliderY->setValue(0); + buildVisu(); +} + +void IKRRTWindow::sliderReleased_ObjectZ() +{ + UI.horizontalSliderZ->setValue(0); + buildVisu(); +} + +void IKRRTWindow::sliderReleased_ObjectA() +{ + UI.horizontalSliderRo->setValue(0); + buildVisu(); +} + +void IKRRTWindow::sliderReleased_ObjectB() +{ + UI.horizontalSliderPi->setValue(0); + buildVisu(); +} + +void IKRRTWindow::sliderReleased_ObjectG() +{ + UI.horizontalSliderYa->setValue(0); + buildVisu(); +} + +void IKRRTWindow::showCoordSystem() +{ + if (eef) + { + RobotNodePtr tcp = eef->getTcp(); + if (!tcp) + return; + tcp->showCoordinateSystem(UI.checkBoxTCP->isChecked()); + } + if (object) + object->showCoordinateSystem(UI.checkBoxTCP->isChecked()); +} + +void IKRRTWindow::buildRRTVisu() +{ + rrtSep->removeAllChildren(); + if (!UI.checkBoxSolution->isChecked()) + return; + if (!solution) + return; + + boost::shared_ptr<Saba::CoinRrtWorkspaceVisualization> w(new Saba::CoinRrtWorkspaceVisualization(robot,cspace,eef->getTcpName())); + if (tree) + w->addTree(tree); + if (tree2) + w->addTree(tree2); + w->addCSpacePath(solution); + if (solutionOptimized) + w->addCSpacePath(solutionOptimized,Saba::CoinRrtWorkspaceVisualization::eGreen); + w->addConfiguration(startConfig,Saba::CoinRrtWorkspaceVisualization::eGreen,3.0f); + SoSeparator *sol = w->getCoinVisualization(); + rrtSep->addChild(sol); +} + +void IKRRTWindow::buildGraspSetVisu() +{ + graspsSep->removeAllChildren(); + if (UI.checkBoxGraspSet->isChecked() && eef && graspSet && object) + { + SoSeparator* visu = CoinVisualizationFactory::CreateGraspSetVisualization(graspSet, eef, object->getGlobalPose()); + if (visu) + graspsSep->addChild(visu); + } + + // show reachable graps + reachableGraspsSep->removeAllChildren(); + if (UI.checkBoxReachableGrasps->isChecked() && eef && graspSet && object && reachSpace) + { + GraspSetPtr rg = reachSpace->getReachableGrasps(graspSet,object); + if (rg->getSize()>0) + { + SoSeparator* visu = CoinVisualizationFactory::CreateGraspSetVisualization(rg, eef, object->getGlobalPose()); + if (visu) + reachableGraspsSep->addChild(visu); + } + } +} + + +void IKRRTWindow::reachVisu() +{ + if (!robot || !reachSpace) + return; + + reachabilitySep->removeAllChildren(); + if (UI.checkBoxReachabilitySpace->checkState() == Qt::Checked) + { + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(reachSpace,VirtualRobot::ColorMap::eRed,true); + if (visualisationNode) + reachabilitySep->addChild(visualisationNode); + /* + boost::shared_ptr<VirtualRobot::CoinVisualization> visualization = reachSpace->getVisualization<CoinVisualization>(); + SoNode* visualisationNode = NULL; + if (visualization) + visualisationNode = visualization->getCoinVisualization(); + + if (visualisationNode) + reachabilitySep->addChild(visualisationNode); + */ + } +} + +void IKRRTWindow::loadReach() +{ + reachabilitySep->removeAllChildren(); + if (!robot) + return; + cout << "Loading ReachabilitySpace from " << reachFile << endl; + reachSpace.reset(new ReachabilitySpace(robot)); + + try + { + reachSpace->load(reachFile); + } + catch (VirtualRobotException &e) + { + cout << " ERROR while loading reach space" << endl; + cout << e.what(); + reachSpace.reset(); + return; + } + + reachSpace->print(); + + buildVisu(); +} +void IKRRTWindow::planIKRRT() +{ + + GenericIKSolverPtr ikSolver(new GenericIKSolver(rns)); + if (UI.checkBoxReachabilitySpaceIK->checkState() == Qt::Checked) + ikSolver->setReachabilityCheck(reachSpace); + // setup collision detection + CDManagerPtr cdm; + if (UI.checkBoxColCheckIK->checkState() == Qt::Checked) + { + SceneObjectSetPtr colModelSet = robot->getSceneObjectSet(colModelName); + SceneObjectSetPtr colModelSet2; + if (!colModelNameRob.empty()) + colModelSet2 = robot->getSceneObjectSet(colModelNameRob); + if (colModelSet) + { + cdm.reset(new CDManager()); + cdm->addCollisionModel(object); + cdm->addCollisionModel(colModelSet); + if (colModelSet2) + cdm->addCollisionModel(colModelSet2); + ikSolver->collisionDetection(cdm); + } + } else + cdm.reset(new CDManager()); + + ikSolver->setMaximumError(10.0f,0.08f); + ikSolver->setupJacobian(0.9f,20); + + cspace.reset(new Saba::CSpaceSampled(robot,cdm,rns)); + + GraspSetPtr graspSet = object->getGraspSet(robot->getType(),eefName); + Saba::GraspIkRrtPtr ikRrt(new Saba::GraspIkRrt(cspace,object,ikSolver,graspSet)); + + + ikRrt->setStart(startConfig); + bool planOK = ikRrt->plan(); + if (planOK) + { + VR_INFO << " Planning succeeded " << endl; + solution = ikRrt->getSolution(); + Saba::ShortcutProcessorPtr postProcessing(new Saba::ShortcutProcessor(solution,cspace,false)); + solutionOptimized = postProcessing->optimize(100); + tree = ikRrt->getTree(); + tree2 = ikRrt->getTree2(); + + } else + VR_INFO << " Planning failed" << endl; + + sliderSolution(1000); + + buildVisu(); +} + +void IKRRTWindow::colModel() +{ +#if 0 + if (reachSpace && graspSet && object) + { + Eigen::Matrix4f m = reachSpace->sampleReachablePose(); + cout << "getEntry: " << (int)reachSpace->getEntry(m) << endl; + /*SoSeparator* sep1 = new SoSeparator; + SoSeparator *cs = CoinVisualizationFactory::CreateCoordSystemVisualization(); + SoMatrixTransform *mt = new SoMatrixTransform; + SbMatrix ma(reinterpret_cast<SbMat*>(m.data())); + mt->matrix.setValue(ma); + sep1->addChild(mt); + sep1->addChild(cs); + sceneSep->addChild(sep1);*/ + + + GraspPtr g = graspSet->getGrasp(0); + m = g->getObjectTargetPoseGlobal(m); + object->setGlobalPose(m); + } +#endif + buildVisu(); +} + +void IKRRTWindow::searchIK() +{ + GenericIKSolverPtr ikSolver(new GenericIKSolver(rns)); + if (UI.checkBoxReachabilitySpaceIK->checkState() == Qt::Checked) + ikSolver->setReachabilityCheck(reachSpace); + // setup collision detection + if (UI.checkBoxColCheckIK->checkState() == Qt::Checked) + { + SceneObjectSetPtr colModelSet = robot->getSceneObjectSet(colModelName); + if (colModelSet) + { + CDManagerPtr cdm(new CDManager()); + cdm->addCollisionModel(object); + cdm->addCollisionModel(colModelSet); + ikSolver->collisionDetection(cdm); + } + } + + ikSolver->setMaximumError(5.0f,0.05f); + ikSolver->setupJacobian(0.9f,20); + GraspPtr grasp = ikSolver->solve(object,IKSolver::All,10); + if (grasp) + { + VR_INFO << "IK successful..." << endl; + } else + { + VR_INFO << "IK failed..." << endl; + } + m_pExViewer->scheduleRedraw(); +} + +void IKRRTWindow::sliderSolution( int pos ) +{ + if (!solution) + return; + Saba::CSpacePathPtr s = solution; + if (solutionOptimized) + s = solutionOptimized; + float p = (float)pos/1000.0f; + Eigen::VectorXf iPos; + s->interpolatePath(p,iPos); + rns->setJointValues(iPos); + m_pExViewer->scheduleRedraw(); +} + +void IKRRTWindow::redraw() +{ + m_pExViewer->scheduleRedraw(); +} + + diff --git a/MotionPlanning/examples/IKRRT/IKRRTWindow.h b/MotionPlanning/examples/IKRRT/IKRRTWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..4d2fa6a5182eab0470feabcd8d136271f486d25c --- /dev/null +++ b/MotionPlanning/examples/IKRRT/IKRRTWindow.h @@ -0,0 +1,132 @@ + +#ifndef __IKRRT_WINDOW_H_ +#define __IKRRT_WINDOW_H_ + +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/XML/SceneIO.h> +#include <VirtualRobot/Visualization/VisualizationFactory.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <VirtualRobot/Obstacle.h> +#include <VirtualRobot/ManipulationObject.h> + +#include "MotionPlanning/Saba.h" +#include "MotionPlanning/CSpace/CSpacePath.h" + +#include <string.h> +#include <QtCore/QtGlobal> +#include <QtGui/QtGui> +#include <QtCore/QtCore> + +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/Qt/SoQt.h> +#include <Inventor/nodes/SoSeparator.h> + + +#include <vector> + +#include "ui_IKRRT.h" + +class IKRRTWindow : public QMainWindow +{ + Q_OBJECT +public: + IKRRTWindow(std::string &sceneFile, std::string &reachFile, std::string &rns, std::string &eef, std::string &colModel, std::string &colModelRob, Qt::WFlags flags = 0); + ~IKRRTWindow(); + + /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */ + int main(); + + void redraw(); +public slots: + /*! Closes the window and exits SoQt runloop. */ + void quit(); + + /*!< Overriding the close event, so we know when the window was closed by the user. */ + void closeEvent(QCloseEvent *event); + + void resetSceneryAll(); + + + void closeEEF(); + void openEEF(); + void searchIK(); + + void colModel(); + + void sliderReleased_ObjectX(); + void sliderReleased_ObjectY(); + void sliderReleased_ObjectZ(); + void sliderReleased_ObjectA(); + void sliderReleased_ObjectB(); + void sliderReleased_ObjectG(); + void sliderSolution(int pos); + + void buildVisu(); + + void showCoordSystem(); + void reachVisu(); + + void planIKRRT(); + +protected: + + void loadScene(); + void loadReach(); + + void setupUI(); + QString formatString(const char *s, float f); + + void buildGraspSetVisu(); + + void buildRRTVisu(); + + void updateObject(float x[6]); + + static void timerCB(void * data, SoSensor * sensor); + void buildRrtVisu(); + Ui::MainWindowIKRRT UI; + SoQtExaminerViewer *m_pExViewer; /*!< Viewer to display the 3D model of the robot and the environment. */ + + SoSeparator *sceneSep; + SoSeparator *robotSep; + SoSeparator *objectSep; + SoSeparator *graspsSep; + SoSeparator *reachableGraspsSep; + SoSeparator *reachabilitySep; + SoSeparator *obstaclesSep; + SoSeparator *rrtSep; + + VirtualRobot::RobotPtr robot; + std::vector< VirtualRobot::ObstaclePtr > obstacles; + VirtualRobot::ManipulationObjectPtr object; + VirtualRobot::ReachabilitySpacePtr reachSpace; + + VirtualRobot::EndEffectorPtr eef; + Saba::CSpaceSampledPtr cspace; + Eigen::VectorXf startConfig; + + VirtualRobot::GraspSetPtr graspSet; + VirtualRobot::RobotNodeSetPtr rns; + + std::string sceneFile; + std::string reachFile; + std::string eefName; + std::string rnsName; + std::string colModelName; + std::string colModelNameRob; + + Saba::CSpacePathPtr solution; + Saba::CSpacePathPtr solutionOptimized; + Saba::CSpaceTreePtr tree; + Saba::CSpaceTreePtr tree2; + + boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationRobot; + boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject; +}; + +#endif // __IKRRT_WINDOW_H_ diff --git a/MotionPlanning/examples/RRT/CMakeLists.txt b/MotionPlanning/examples/RRT/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..e9c28926de47767f891e0eaee50b8bf53b02f675 --- /dev/null +++ b/MotionPlanning/examples/RRT/CMakeLists.txt @@ -0,0 +1,51 @@ +PROJECT ( RRTdemo ) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4) + +CMAKE_POLICY(VERSION 2.6) + +INCLUDE(${SABA_DIR}/config.cmake) + +INCLUDE_DIRECTORIES(${SABA_DIR}) + +IF(SIMOX_USE_COIN_VISUALIZATION) + LINK_DIRECTORIES(${SIMOX_LIB_DIR}) + + # the variable "demo_SRCS" contains all .cpp files of this project + FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/RRTdemo.cpp) + #FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/RRTdemoWindow.h) + + +################################## moc'ing ############################## +# set(GUI_MOC_HDRS +# ${PROJECT_SOURCE_DIR}/RRTdemoWindow.h +# ) + +# set(GUI_UIS +# ${PROJECT_SOURCE_DIR}/RRTDemo.ui +# ) + +#MESSAGE("GUI_MOC_HDRS: ${GUI_MOC_HDRS}") +#MESSAGE("GUI_UIS: ${GUI_UIS}") +# qt4_wrap_cpp(demo_SRCS ${GUI_MOC_HDRS}) +# qt4_wrap_ui(UI_HEADER ${GUI_UIS}) +# get_filename_component(UI_HEADER_DIR ${UI_HEADER} PATH) +#MESSAGE("UI_HEADER: ${UI_HEADER}") +# list(APPEND demo_INCS ${UI_HEADER}) + +# include_directories(${UI_HEADER_DIR}) + + + ADD_EXECUTABLE(${PROJECT_NAME} ${demo_SRCS} ${demo_INCS}) + SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${SIMOX_BIN_DIR}) + #MESSAGE( STATUS "SIMOX_LIB_DIR " ${SIMOX_LIB_DIR}) + + TARGET_LINK_LIBRARIES(${PROJECT_NAME} VirtualRobot Saba ${QT_LIBRARIES} ${COIN3D_LIBRARIES} ${SoQt_LIBRARIES}) + INCLUDE_DIRECTORIES(${SoQt_INCLUDE_DIRS}) + INCLUDE(${QT_USE_FILE}) + ADD_DEFINITIONS(-DSOQT_DLL) + # ADD_DEFINITIONS(-DQT_DLL) + # ADD_DEFINITIONS(-DQT3_SUPPORT) + + MESSAGE( STATUS ${PROJECT_NAME} " will be placed into " ${SIMOX_BIN_DIR}) +ENDIF(SIMOX_USE_COIN_VISUALIZATION) diff --git a/MotionPlanning/examples/RRT/Joint3.xml b/MotionPlanning/examples/RRT/Joint3.xml new file mode 100644 index 0000000000000000000000000000000000000000..345a7d79b78c6aa4768f67bc64b85f2779267786 --- /dev/null +++ b/MotionPlanning/examples/RRT/Joint3.xml @@ -0,0 +1,85 @@ +<?xml version="1.0" encoding="UTF-8" ?> + +<Robot Type="Simple3DoFRobot" StandardName="Simple3DoFRobot" RootNode="Start"> + + <RobotNode name="Start"> + <Visualization enable="false"> + <CoordinateAxis enable="true" scaling="1" text="Root"/> + </Visualization> + <Child name="Joint1"/> + </RobotNode> + + <RobotNode name="Joint1"> + <Joint type="revolute"> + <Limits unit="degree" lo="-180" hi="180"/> + <Axis x="0" y="0" z="1"/> + <PostJointTransform> + <Transform> + <Translation x="0" y="0" z="300"/> + </Transform> + </PostJointTransform> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Joint2"/> + </RobotNode> + + <RobotNode name="Joint2"> + <Joint type="revolute"> + <Limits unit="degree" lo="-180" hi="180"/> + <Axis x="1" y="0" z="0"/> + <PostJointTransform> + <Transform> + <Translation x="0" y="0" z="300"/> + </Transform> + </PostJointTransform> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Joint3"/> + </RobotNode> + + <RobotNode name="Joint3"> + <Joint type="revolute"> + <Limits unit="degree" lo="-180" hi="180"/> + <Axis x="1" y="0" z="0"/> + <PostJointTransform> + <Transform> + <Translation x="0" y="0" z="300"/> + </Transform> + </PostJointTransform> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="EndPoint"/> + </RobotNode> + + <RobotNode name="EndPoint"> + <Joint type="fixed"> + </Joint> + <Visualization enable="true"> + <File type="Inventor">sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + </RobotNode> + + <RobotNodeSet name="AllJoints" kinematicRoot="Joint1"> + <Node name="Joint1"/> + <Node name="Joint2"/> + <Node name="Joint3"/> + </RobotNodeSet> + + <RobotNodeSet name="CollisionModel"> + <Node name="Joint1"/> + <Node name="Joint2"/> + <Node name="Joint3"/> + <Node name="EndPoint"/> + </RobotNodeSet> + +</Robot> diff --git a/MotionPlanning/examples/RRT/Joint3DH.xml b/MotionPlanning/examples/RRT/Joint3DH.xml new file mode 100644 index 0000000000000000000000000000000000000000..107ca361db3b2d4515e6c5b7916bcab7e07e9cca --- /dev/null +++ b/MotionPlanning/examples/RRT/Joint3DH.xml @@ -0,0 +1,70 @@ +<?xml version="1.0" encoding="UTF-8" ?> + +<Robot Type="Simple3DoFRobot" StandardName="Simple3DoFRobot" RootNode="Start"> + + <RobotNode name="Start"> + <Visualization enable="false"> + <CoordinateAxis enable="true" scaling="1" text="Root"/> + </Visualization> + <Child name="Joint1"/> + </RobotNode> + + <RobotNode name="Joint1"> + <Joint type="revolute"> + <Limits unit="degree" lo="-90" hi="90"/> + <DH a="300" d="0" theta="0" alpha="90" units="degree"/> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_rot_sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Joint2"/> + </RobotNode> + + <RobotNode name="Joint2"> + <Joint type="revolute"> + <Limits unit="degree" lo="0" hi="360"/> + <DH a="300" d="0" theta="0" alpha="0" units="degree"/> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_rot_sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="Joint3"/> + </RobotNode> + + <RobotNode name="Joint3"> + <Joint type="revolute"> + <Limits unit="degree" lo="-90" hi="90"/> + <DH a="300" d="0" theta="0" alpha="0" units="degree"/> + </Joint> + <Visualization enable="true"> + <File type="Inventor">joint_rot_sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + <Child name="EndPoint"/> + </RobotNode> + + <RobotNode name="EndPoint"> + <Joint type="fixed"> + </Joint> + <Visualization enable="true"> + <File type="Inventor">sphere.iv</File> + <UseAsCollisionModel/> + </Visualization> + </RobotNode> + + <RobotNodeSet name="AllJoints" kinematicRoot="Joint1"> + <Node name="Joint1"/> + <Node name="Joint2"/> + <Node name="Joint3"/> + </RobotNodeSet> + + <RobotNodeSet name="CollisionModel"> + <Node name="Joint1"/> + <Node name="Joint2"/> + <Node name="Joint3"/> + <Node name="EndPoint"/> + </RobotNodeSet> + +</Robot> diff --git a/MotionPlanning/examples/RRT/RRTdemo.cpp b/MotionPlanning/examples/RRT/RRTdemo.cpp new file mode 100644 index 0000000000000000000000000000000000000000..37453002f53e439c7bfc216807d8d0e67ca492d1 --- /dev/null +++ b/MotionPlanning/examples/RRT/RRTdemo.cpp @@ -0,0 +1,212 @@ + +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/Obstacle.h> +#include <VirtualRobot/SceneObjectSet.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/XML/RobotIO.h> +#include <VirtualRobot/Visualization/VisualizationFactory.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <Saba.h> +#include <Planner/Rrt.h> +#include <Planner/BiRrt.h> +#include <Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h> +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/nodes/SoSeparator.h> +#include <Inventor/Qt/SoQt.h> +#include <boost/shared_ptr.hpp> +#include <string> +#include <iostream> + +using std::cout; +using std::endl; +using namespace VirtualRobot; +using namespace Saba; + +#define USE_BIRRT + +bool useColModel = false; +QWidget* win; + +void show(SoNode *n) +{ + if (win==NULL) + { + printf ("Could not create window.\n"); + exit(-3); + } + SoQtExaminerViewer* viewer = new SoQtExaminerViewer(win); + + // set the robot + if (n) + viewer->setSceneGraph(n); + + // register timer callback for animation and draw state updates + /* + SoSensorManager *sensor_mgr = SoDB::getSensorManager(); + SoTimerSensor *timer = new SoTimerSensor(TimerCallback, NULL); + timer->setInterval(SbTime(0.02)); + sensor_mgr->insertTimerSensor(timer); + */ + viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f)); + viewer->setAccumulationBuffer(true); + viewer->setAntialiasing(true, 4); + viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND); + viewer->setFeedbackVisibility(true); + + // show everything + viewer->show(); + + // start the mainloop + SoQt::show(win); + SoQt::mainLoop(); + + // clean up + delete viewer; +} + +void startRRTVisualization() +{ + + // create robot + std::string filename(SABA_BASE_DIR "/examples/RRT/Joint3.xml"); + cout << "Loading 3DOF robot from " << filename << endl; + RobotPtr robot = RobotIO::loadRobot(filename); + if (!robot) + return; + + + float sampling_extend_stepsize = 0.04f; + + // create environment + float posX=0.0f, posY=-400.0f, posZ=400.0f, sizeX=100.0f, sizeY=300.0f, sizeZ=1000.0f; + ObstaclePtr o = Obstacle::createBox( sizeX, sizeY, sizeZ ); + + Eigen::Affine3f tmpT(Eigen::Translation3f(posX,posY,posZ)); + o->setGlobalPose(tmpT.matrix()); + + + // setup collision detection + std::string colModelName("CollisionModel"); + SceneObjectSetPtr cms = robot->getSceneObjectSet(colModelName); + CDManagerPtr cdm(new CDManager()); + cdm->addCollisionModel(cms); + cdm->addCollisionModel(o); + + float planningTime = 0; + int failed = 0; + int loops = 1; + bool ok; + CSpaceSampledPtr cspace; + std::string planningJoints("AllJoints"); + RobotNodeSetPtr planningNodes = robot->getRobotNodeSet(planningJoints); +#ifdef USE_BIRRT + BiRrtPtr rrt; +#else + RrtPtr rrt; +#endif + Eigen::VectorXf start(3); + start(0) = (float)M_PI / 4.0f; + start(1) = (float)M_PI / 4.0f * 1.5f; + start(2) = (float)-M_PI / 4.0f; + + Eigen::VectorXf goal(3); + goal(0) = (float)-M_PI / 4.0f; + goal(1) = (float)M_PI / 4.0f * 1.5f; + goal(2) = (float)-M_PI / 4.0f; + for (int i=0;i<loops;i++) + { + // setup C-Space + + cspace.reset(new CSpaceSampled(robot,cdm,planningNodes)); + cspace->setSamplingSize(sampling_extend_stepsize); + cspace->setSamplingSizeDCD(sampling_extend_stepsize); + // setup planner + + +#ifdef USE_BIRRT + rrt.reset(new BiRrt(cspace)); +#else + rrt.reset(new Rrt(cspace)); +#endif + rrt->setStart(start); + rrt->setGoal(goal); + + clock_t startT = clock(); + ok = rrt->plan(true); + clock_t endT = clock(); + + float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f); + planningTime += diffClock; + if (!ok) + failed++; + } + planningTime /= (float)loops; + cout << "Avg planning time: " << planningTime << endl; + cout << "failed:" << failed << endl; + + if (!ok) + { + cout << "planning failed..." << endl; + return; + } + + CSpacePathPtr solution = rrt->getSolution(); + CSpaceTreePtr tree = rrt->getTree(); + + + + planningNodes->setJointValues(start); + + // display robot + SoSeparator *sep = new SoSeparator(); + SceneObject::VisualizationType colModel = SceneObject::Full; + + boost::shared_ptr<CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(colModel); + SoNode* visualisationNode = NULL; + if (visualization) + visualisationNode = visualization->getCoinVisualization(); + sep->addChild(visualisationNode); + + // display obstacle + VisualizationNodePtr visuObstacle = o->getVisualization(); + std::vector<VisualizationNodePtr> visus; + visus.push_back(visuObstacle); + boost::shared_ptr<CoinVisualization> visualizationO(new CoinVisualization(visus)); + SoNode* obstacleSoNode = visualizationO->getCoinVisualization(); + sep->addChild(obstacleSoNode); + + // show rrt visu + boost::shared_ptr<CoinRrtWorkspaceVisualization> w(new CoinRrtWorkspaceVisualization(robot,cspace,"EndPoint")); + w->addTree(tree); +#ifdef USE_BIRRT + CSpaceTreePtr tree2 = rrt->getTree2(); + w->addTree(tree2); +#endif + w->addCSpacePath(solution); + w->addConfiguration(start,CoinRrtWorkspaceVisualization::eGreen,3.0f); + w->addConfiguration(goal,CoinRrtWorkspaceVisualization::eGreen,3.0f); + SoSeparator *sol = w->getCoinVisualization(); + sep->addChild(sol); + + show (sep); +} + +int main (int argc, char** argv) +{ + SoDB::init(); + win = SoQt::init("RoboViewer","RoboViewer"); + cout << " --- START --- " << endl; + try { + startRRTVisualization(); + } + catch( VirtualRobot::VirtualRobotException v){ + std::cout << "VirtualRobot Exception: " << v.what() << std::endl ; + } + catch( std::exception e){ + std::cout << "Exception: " << e.what() << std::endl ; + } + catch( ... ){;} + cout << " --- END --- " << endl; + + return 0; +} diff --git a/MotionPlanning/examples/RRT/RRTdemoWindow.cpp b/MotionPlanning/examples/RRT/RRTdemoWindow.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4379b234b795c7d72cd4ad5f3bc0025975a2aa60 --- /dev/null +++ b/MotionPlanning/examples/RRT/RRTdemoWindow.cpp @@ -0,0 +1,168 @@ + +#include "RRTdemoWindow.h" + +#include "VirtualRobot/XML/RobotIO.h" +#include "VirtualRobot/CollisionDetection/CollisionChecker.h" +#include "VirtualRobot/Robot.h" + +#include <time.h> +#include <vector> +#include <iostream> +#include <QtGui/QtGui> +#include <QtCore/QtCore> +#include <Qt3Support/Qt3Support> + +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/Qt/SoQt.h> + + +#include <sstream> +using namespace std; + +float TIMER_MS = 30.0f; + +CRRTdemoWindow::CRRTdemoWindow(Qt::WFlags flags) +:QMainWindow(NULL) +{ + cout << __PRETTY_FUNCTION__ << " start " << endl; + //this->setCaption(QString("RRTdemo - KIT - Humanoids Group")); + //resize(1100, 768); + + + m_pRobotSep = NULL; + m_pGraspScenery = new CRRTdemoScenery(); + m_pSceneSep = m_pGraspScenery->getScene(); + + setupUI(); + + loadRobot(); // load robot before loading object so they are correctly linked + //setEnvironment(0); + //loadObject(0); + + updateGraspObjectPose(0,0,0,0,0,0); + m_pExViewer->viewAll(); +} + + +CRRTdemoWindow::~CRRTdemoWindow() +{ +} + +/* +void CRRTdemoWindow::saveScreenshot() +{ + static int counter = 0; + SbString framefile; + + framefile.sprintf("MPL_Render_Frame%06d.png", counter); + counter++; + + m_pExViewer->getSceneManager()->render(); + m_pExViewer->getSceneManager()->scheduleRedraw(); + QGLWidget* w = (QGLWidget*)m_pExViewer->getGLWidget(); + + QImage i = w->grabFrameBuffer(); + bool bRes = i.save(framefile.getString(), "PNG"); + if (bRes) + cout << "wrote image " << counter << endl; + else + cout << "failed writing image " << counter << endl; + +}*/ + +void CRRTdemoWindow::setupUI() +{ + UI.setupUi(this); + //centralWidget()->setLayout(UI.gridLayoutViewer); + m_pExViewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP); + + // setup + m_pExViewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f)); + m_pExViewer->setAccumulationBuffer(true); +#ifdef WIN32 +#ifndef _DEBUG + m_pExViewer->setAntialiasing(true, 4); +#endif +#endif + m_pExViewer->setTransparencyType(SoGLRenderAction::BLEND); + m_pExViewer->setFeedbackVisibility(true); + m_pExViewer->setSceneGraph(m_pSceneSep); + m_pExViewer->viewAll(); + + connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll())); + connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(collisionModel())); + //connect(UI.comboBoxJoint, SIGNAL(activated(int)), this, SLOT(selectJoint(int))); + //connect(UI.horizontalSliderJointValue, SIGNAL(valueChanged(int)), this, SLOT(jointValueChanged(int))); + +} + +QString CRRTdemoWindow::formatString(const char *s, float f) +{ + QString str1(s); + if (f>=0) + str1 += " "; + if (fabs(f)<1000) + str1 += " "; + if (fabs(f)<100) + str1 += " "; + if (fabs(f)<10) + str1 += " "; + QString str1n; + str1n.setNum(f,'f',3); + str1 = str1 + str1n; + return str1; +} + + +void CRRTdemoWindow::resetSceneryAll() +{ + loadRobot(); +} + + + +void CRRTdemoWindow::collisionModel() +{ + //setRobotModelShape(UI.checkBoxColModel->state() == QCheckBox::On); +} + + +void CRRTdemoWindow::showRobot() +{ + //m_pGraspScenery->showRobot(m_pShowRobot->state() == QCheckBox::On); +} + +void CRRTdemoWindow::closeEvent(QCloseEvent *event) +{ + quit(); + QMainWindow::closeEvent(event); +} + + + + +int CRRTdemoWindow::main() +{ + SoQt::show(this); + SoQt::mainLoop(); + return 0; +} + + +void CRRTdemoWindow::quit() +{ + std::cout << "CRRTdemoWindow: Closing" << std::endl; + this->close(); + SoQt::exitMainLoop(); +} + + +void CRRTdemoWindow::loadRobot() +{ + std::cout << "CRRTdemoWindow: Loading robot" << std::endl; + //m_pGraspScenery->loadRobot(); + m_pExViewer->viewAll(); +} + diff --git a/MotionPlanning/examples/RRT/RRTdemoWindow.h b/MotionPlanning/examples/RRT/RRTdemoWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..58a918cbe3bdd67f53b17c44bf56b721ee315169 --- /dev/null +++ b/MotionPlanning/examples/RRT/RRTdemoWindow.h @@ -0,0 +1,69 @@ + +#ifndef __RRTdemo_WINDOW_H_ +#define __RRTdemo_WINDOW_H_ + +#include <VirtualRobot/VirtualRobot.h> +#include <string.h> + +#include <qobject.h> +#include <qmainwindow.h> +#include <qpushbutton.h> +#include <qtextedit.h> +#include <qcombobox.h> +#include <qprogressbar.h> +#include <qlabel.h> +#include <qslider.h> +#include <qcheckbox.h> +#include <qspinbox.h> +#include <qlineedit.h> + +#include <vector> + +#include "ui_RRTdemo.h" + +class CRobot; +class SoQtExaminerViewer; +class SoSeparator; +class SoSensor; +class CRRTdemoScenery; + +class CRRTdemoWindow : public QMainWindow +{ + Q_OBJECT +public: + CRRTdemoWindow(Qt::WFlags flags = 0); + ~CRRTdemoWindow(); + + /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */ + int main(); + +public slots: + /*! Closes the window and exits SoQt runloop. */ + void quit(); + + /*!< Overriding the close event, so we know when the window was closed by the user. */ + void closeEvent(QCloseEvent *event); + + void resetSceneryAll(); + void collisionModel(); + void showRobot(); + + SoQtExaminerViewer* getExaminerViewer(){return m_pExViewer;}; + +protected: + void setupUI(); + QString formatString(const char *s, float f); + + void loadRobot(); + + Ui_MainWindowRRTDemo UI; + SoQtExaminerViewer* m_pExViewer; /*!< Viewer to display the 3D model of the robot and the environment. */ + + SoSeparator* m_pSceneSep; + SoSeparator* m_pRobotSep; + + + +}; + +#endif // __RRTdemo_WINDOW_H_ diff --git a/MotionPlanning/examples/RRT/joint.iv b/MotionPlanning/examples/RRT/joint.iv new file mode 100644 index 0000000000000000000000000000000000000000..86d4dea31266f7b9d9224df559d00c33b10fd9df --- /dev/null +++ b/MotionPlanning/examples/RRT/joint.iv @@ -0,0 +1,22 @@ +#Inventor V2.0 ascii + +Separator { + + #Units { + # units MILLIMETERS + #} + Material { + diffuseColor 1 1 1 + } + Rotation { + rotation 1 0 0 1.57 + } + Translation { + translation 0 150 0 + } + Cylinder { + radius 50 + height 300 + parts (SIDES | TOP | BOTTOM) + } +} \ No newline at end of file diff --git a/MotionPlanning/examples/RRT/joint_rot.iv b/MotionPlanning/examples/RRT/joint_rot.iv new file mode 100644 index 0000000000000000000000000000000000000000..6b1e0c5c0e58c7388912b77e9ca9386a0db4cb4f --- /dev/null +++ b/MotionPlanning/examples/RRT/joint_rot.iv @@ -0,0 +1,22 @@ +#Inventor V2.0 ascii + +Separator { + + #Units { + # units MILLIMETERS + #} + Material { + diffuseColor 1 1 1 + } + Rotation { + rotation 0 0 1 -1.57 + } + Translation { + translation 0 150 0 + } + Cylinder { + radius 50 + height 300 + parts (SIDES | TOP | BOTTOM) + } +} \ No newline at end of file diff --git a/MotionPlanning/examples/RRT/joint_rot_sphere.iv b/MotionPlanning/examples/RRT/joint_rot_sphere.iv new file mode 100644 index 0000000000000000000000000000000000000000..520133ba86de69a0f2997dbb4fb95494ad5ca3c1 --- /dev/null +++ b/MotionPlanning/examples/RRT/joint_rot_sphere.iv @@ -0,0 +1,25 @@ +#Inventor V2.0 ascii + +Separator { + + #Units { + # units MILLIMETERS + #} + Material { + diffuseColor 1 1 1 + } + Rotation { + rotation 0 0 1 -1.57 + } + Sphere { + radius 70 + } + Translation { + translation 0 150 0 + } + Cylinder { + radius 50 + height 300 + parts (SIDES | TOP | BOTTOM) + } +} \ No newline at end of file diff --git a/MotionPlanning/examples/RRT/joint_sphere.iv b/MotionPlanning/examples/RRT/joint_sphere.iv new file mode 100644 index 0000000000000000000000000000000000000000..ac0b0895da75a8497e9f7dbe717b00907ccb588e --- /dev/null +++ b/MotionPlanning/examples/RRT/joint_sphere.iv @@ -0,0 +1,25 @@ +#Inventor V2.0 ascii + +Separator { + + #Units { + # units MILLIMETERS + #} + Material { + diffuseColor 1 1 1 + } + Rotation { + rotation 1 0 0 1.57 + } + Sphere { + radius 70 + } + Translation { + translation 0 150 0 + } + Cylinder { + radius 50 + height 300 + parts (SIDES | TOP | BOTTOM) + } +} \ No newline at end of file diff --git a/MotionPlanning/examples/RRT/sphere.iv b/MotionPlanning/examples/RRT/sphere.iv new file mode 100644 index 0000000000000000000000000000000000000000..8584430130f7ebaf65ab9e431b1301bd62236563 --- /dev/null +++ b/MotionPlanning/examples/RRT/sphere.iv @@ -0,0 +1,17 @@ +#Inventor V2.0 ascii + +Separator { + + #Units { + # units MILLIMETERS + #} + Material { + diffuseColor 1 1 1 + } + + + Sphere { + radius 70 + + } +} \ No newline at end of file diff --git a/MotionPlanning/examples/RrtGui/CMakeLists.txt b/MotionPlanning/examples/RrtGui/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..fdbfbab7994ea7a1289ea760785cce9a9d21d4ec --- /dev/null +++ b/MotionPlanning/examples/RrtGui/CMakeLists.txt @@ -0,0 +1,46 @@ +PROJECT ( RrtGui ) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6.4) + +CMAKE_POLICY(VERSION 2.6) + +IF(NOT DEFINED VR_CONFIGURED) + INCLUDE(../../config.cmake) +ENDIF() + +IF(SIMOX_USE_COIN_VISUALIZATION) + LINK_DIRECTORIES(${SIMOX_LIB_DIR}) + + + # the variable "demo_SRCS" contains all .cpp files of this project + FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/RrtGui.cpp ${PROJECT_SOURCE_DIR}/RrtGuiWindow.cpp) + FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/RrtGuiWindow.h) + + +################################## moc'ing ############################## + set(GUI_MOC_HDRS + ${PROJECT_SOURCE_DIR}/RrtGuiWindow.h + ) + + set(GUI_UIS + ${PROJECT_SOURCE_DIR}/RrtGui.ui + ) + + qt4_wrap_cpp(demo_SRCS ${GUI_MOC_HDRS}) + qt4_wrap_ui(UI_HEADER ${GUI_UIS}) + get_filename_component(UI_HEADER_DIR ${UI_HEADER} PATH) + list(APPEND demo_INCS ${UI_HEADER}) + + include_directories(${UI_HEADER_DIR}) + + + ADD_EXECUTABLE(${PROJECT_NAME} ${demo_SRCS} ${demo_INCS}) + SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${SIMOX_BIN_DIR}) + + TARGET_LINK_LIBRARIES(${PROJECT_NAME} VirtualRobot Saba ${QT_LIBRARIES} ${COIN3D_LIBRARIES} ${SoQt_LIBRARIES}) + INCLUDE_DIRECTORIES(${SoQt_INCLUDE_DIRS}) + INCLUDE(${QT_USE_FILE}) + ADD_DEFINITIONS(-DSOQT_DLL) + + MESSAGE( STATUS ${PROJECT_NAME} " will be placed into " ${SIMOX_BIN_DIR}) +ENDIF(SIMOX_USE_COIN_VISUALIZATION) diff --git a/MotionPlanning/examples/RrtGui/RrtGui.cpp b/MotionPlanning/examples/RrtGui/RrtGui.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5077ecc84897390be9550f241363994524805a78 --- /dev/null +++ b/MotionPlanning/examples/RrtGui/RrtGui.cpp @@ -0,0 +1,88 @@ +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/XML/RobotIO.h> +#include <VirtualRobot/Visualization/VisualizationFactory.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <VirtualRobot/RuntimeEnvironment.h> + + +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/nodes/SoSeparator.h> +#include <Inventor/Qt/SoQt.h> +#include <boost/shared_ptr.hpp> +#include <string> +#include <iostream> + +using std::cout; +using std::endl; +using namespace VirtualRobot; + +#include <Eigen/Core> +#include <Eigen/Geometry> + +#include "RrtGuiWindow.h" + + +int main(int argc, char *argv[]) +{ + SoDB::init(); + SoQt::init(argc,argv,"RrtGui"); + cout << " --- START --- " << endl; + + std::string filenameScene(SABA_BASE_DIR "/examples/RrtGui/scenes/planning.xml"); + std::string startConfig("start"); + std::string goalConfig("goal"); + std::string rnsName("Planning"); + std::string colModel1("ColModel Robot Moving"); + std::string colModel2("ColModel Robot Body"); + std::string colModel3("ColModel Obstacles"); + + VirtualRobot::RuntimeEnvironment::considerKey("scene"); + VirtualRobot::RuntimeEnvironment::considerKey("startConfig"); + VirtualRobot::RuntimeEnvironment::considerKey("goalConfig"); + VirtualRobot::RuntimeEnvironment::considerKey("robotNodeSet"); + VirtualRobot::RuntimeEnvironment::considerKey("colModelRobot1"); + VirtualRobot::RuntimeEnvironment::considerKey("colModelRobot2"); + VirtualRobot::RuntimeEnvironment::considerKey("colModelEnv"); + VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv); + VirtualRobot::RuntimeEnvironment::print(); + + std::string scFile = VirtualRobot::RuntimeEnvironment::getValue("scene"); + if (!scFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(scFile)) + filenameScene = scFile; + std::string sConf = VirtualRobot::RuntimeEnvironment::getValue("startConfig"); + if (!sConf.empty()) + startConfig = sConf; + std::string gConf = VirtualRobot::RuntimeEnvironment::getValue("goalConfig"); + if (!gConf.empty()) + goalConfig = gConf; + std::string rns = VirtualRobot::RuntimeEnvironment::getValue("robotNodeSet"); + if (!rns.empty()) + rnsName = rns; + std::string c1 = VirtualRobot::RuntimeEnvironment::getValue("colModelRobot1"); + if (!c1.empty()) + colModel1 = c1; + std::string c2 = VirtualRobot::RuntimeEnvironment::getValue("colModelRobot2"); + if (!c2.empty()) + colModel2 = c2; + std::string c3 = VirtualRobot::RuntimeEnvironment::getValue("colModelEnv"); + if (!c3.empty()) + colModel3 = c3; + + + cout << "Using scene: " << filenameScene << endl; + cout << "Using start config: " << startConfig << endl; + cout << "Using goal config: " << goalConfig << endl; + cout << "Using RobotNodeSet for planning: " << rnsName << endl; + cout << "Using robot collision model sets: " << colModel1 << " and " << colModel2 << endl; + cout << "Using environment collision model set: " << colModel3 << endl; + + + RrtGuiWindow rw(filenameScene, startConfig, goalConfig,rnsName,colModel1,colModel2,colModel3); + + rw.main(); + + return 0; + +} diff --git a/MotionPlanning/examples/RrtGui/RrtGui.ui b/MotionPlanning/examples/RrtGui/RrtGui.ui new file mode 100644 index 0000000000000000000000000000000000000000..a4b6201b460925d8ab2f418d21b9594c06a29073 --- /dev/null +++ b/MotionPlanning/examples/RrtGui/RrtGui.ui @@ -0,0 +1,544 @@ +<?xml version="1.0" encoding="UTF-8"?> +<ui version="4.0"> + <class>MainWindowRRTDemo</class> + <widget class="QMainWindow" name="MainWindowRRTDemo"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1055</width> + <height>769</height> + </rect> + </property> + <property name="windowTitle"> + <string>RRTDemo</string> + </property> + <widget class="QWidget" name="centralwidget"> + <layout class="QGridLayout" name="gridLayout"> + <item row="0" column="0"> + <widget class="QFrame" name="frameViewer"> + <property name="frameShape"> + <enum>QFrame::StyledPanel</enum> + </property> + <property name="frameShadow"> + <enum>QFrame::Raised</enum> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QGroupBox" name="groupBox"> + <property name="maximumSize"> + <size> + <width>250</width> + <height>16777215</height> + </size> + </property> + <property name="title"> + <string/> + </property> + <widget class="QGroupBox" name="groupBox_2"> + <property name="geometry"> + <rect> + <x>10</x> + <y>490</y> + <width>231</width> + <height>91</height> + </rect> + </property> + <property name="title"> + <string>Display options</string> + </property> + <widget class="QCheckBox" name="checkBoxColModel"> + <property name="geometry"> + <rect> + <x>20</x> + <y>20</y> + <width>101</width> + <height>21</height> + </rect> + </property> + <property name="text"> + <string>Collision model</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxShowRRT"> + <property name="geometry"> + <rect> + <x>130</x> + <y>40</y> + <width>51</width> + <height>20</height> + </rect> + </property> + <property name="text"> + <string>RRT</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxShowSolution"> + <property name="geometry"> + <rect> + <x>20</x> + <y>40</y> + <width>71</width> + <height>20</height> + </rect> + </property> + <property name="text"> + <string>Solution</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxShowSolutionOpti"> + <property name="geometry"> + <rect> + <x>20</x> + <y>60</y> + <width>151</width> + <height>20</height> + </rect> + </property> + <property name="text"> + <string>Postprocessed Solution</string> + </property> + </widget> + <widget class="QCheckBox" name="checkBoxStartGoal"> + <property name="geometry"> + <rect> + <x>130</x> + <y>20</y> + <width>71</width> + <height>20</height> + </rect> + </property> + <property name="text"> + <string>Start/Goal</string> + </property> + </widget> + </widget> + <widget class="QGroupBox" name="groupBox_3"> + <property name="geometry"> + <rect> + <x>10</x> + <y>580</y> + <width>231</width> + <height>121</height> + </rect> + </property> + <property name="title"> + <string>Execute</string> + </property> + <widget class="QLabel" name="label_2"> + <property name="geometry"> + <rect> + <x>10</x> + <y>70</y> + <width>141</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Position on Solution</string> + </property> + </widget> + <widget class="QSlider" name="horizontalSliderPos"> + <property name="geometry"> + <rect> + <x>10</x> + <y>90</y> + <width>211</width> + <height>20</height> + </rect> + </property> + <property name="maximum"> + <number>1000</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> + </property> + </widget> + <widget class="QLCDNumber" name="lcdNumber"> + <property name="geometry"> + <rect> + <x>150</x> + <y>60</y> + <width>64</width> + <height>23</height> + </rect> + </property> + <property name="smallDecimalPoint"> + <bool>true</bool> + </property> + <property name="numDigits"> + <number>5</number> + </property> + <property name="digitCount"> + <number>5</number> + </property> + <property name="value" stdset="0"> + <double>0.000000000000000</double> + </property> + </widget> + <widget class="QRadioButton" name="radioButtonSolution"> + <property name="geometry"> + <rect> + <x>20</x> + <y>20</y> + <width>95</width> + <height>20</height> + </rect> + </property> + <property name="text"> + <string>Solution</string> + </property> + </widget> + <widget class="QRadioButton" name="radioButtonSolutionOpti"> + <property name="geometry"> + <rect> + <x>20</x> + <y>40</y> + <width>161</width> + <height>20</height> + </rect> + </property> + <property name="text"> + <string>Postprocessed Sol.</string> + </property> + </widget> + </widget> + <widget class="QGroupBox" name="groupBoxStart"> + <property name="geometry"> + <rect> + <x>10</x> + <y>10</y> + <width>231</width> + <height>401</height> + </rect> + </property> + <property name="title"> + <string>Setup</string> + </property> + <widget class="QPushButton" name="pushButtonLoad"> + <property name="geometry"> + <rect> + <x>10</x> + <y>20</y> + <width>81</width> + <height>28</height> + </rect> + </property> + <property name="text"> + <string>Load Scene</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxGoal"> + <property name="geometry"> + <rect> + <x>70</x> + <y>90</y> + <width>151</width> + <height>22</height> + </rect> + </property> + </widget> + <widget class="QLabel" name="label_5"> + <property name="geometry"> + <rect> + <x>10</x> + <y>90</y> + <width>61</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Goal Conf.</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxColModelEnv"> + <property name="geometry"> + <rect> + <x>10</x> + <y>290</y> + <width>211</width> + <height>22</height> + </rect> + </property> + </widget> + <widget class="QLabel" name="label_3"> + <property name="geometry"> + <rect> + <x>10</x> + <y>270</y> + <width>161</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Environment Collision Model Set</string> + </property> + </widget> + <widget class="QLabel" name="label_4"> + <property name="geometry"> + <rect> + <x>10</x> + <y>60</y> + <width>71</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Start Conf.</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxStart"> + <property name="geometry"> + <rect> + <x>70</x> + <y>60</y> + <width>151</width> + <height>22</height> + </rect> + </property> + </widget> + <widget class="QLabel" name="label_6"> + <property name="geometry"> + <rect> + <x>10</x> + <y>170</y> + <width>201</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Robot Collision Model Set A (e.g arm)</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxColModelRobot"> + <property name="geometry"> + <rect> + <x>10</x> + <y>190</y> + <width>211</width> + <height>22</height> + </rect> + </property> + </widget> + <widget class="QDoubleSpinBox" name="doubleSpinBoxColChecking"> + <property name="geometry"> + <rect> + <x>110</x> + <y>340</y> + <width>62</width> + <height>22</height> + </rect> + </property> + <property name="decimals"> + <number>3</number> + </property> + <property name="minimum"> + <double>0.001000000000000</double> + </property> + <property name="maximum"> + <double>10.000000000000000</double> + </property> + <property name="singleStep"> + <double>0.010000000000000</double> + </property> + <property name="value"> + <double>0.040000000000000</double> + </property> + </widget> + <widget class="QDoubleSpinBox" name="doubleSpinBoxCSpaceSampling"> + <property name="geometry"> + <rect> + <x>110</x> + <y>370</y> + <width>62</width> + <height>22</height> + </rect> + </property> + <property name="decimals"> + <number>3</number> + </property> + <property name="minimum"> + <double>0.001000000000000</double> + </property> + <property name="maximum"> + <double>10.000000000000000</double> + </property> + <property name="singleStep"> + <double>0.010000000000000</double> + </property> + <property name="value"> + <double>0.080000000000000</double> + </property> + </widget> + <widget class="QLabel" name="label_7"> + <property name="geometry"> + <rect> + <x>20</x> + <y>350</y> + <width>91</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Collision Checking</string> + </property> + </widget> + <widget class="QLabel" name="label_8"> + <property name="geometry"> + <rect> + <x>20</x> + <y>370</y> + <width>71</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>CSpace Paths</string> + </property> + </widget> + <widget class="QLabel" name="label_9"> + <property name="geometry"> + <rect> + <x>10</x> + <y>330</y> + <width>91</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Sampling</string> + </property> + </widget> + <widget class="QLabel" name="label_10"> + <property name="geometry"> + <rect> + <x>10</x> + <y>220</y> + <width>211</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Robot Collision Model Set B (e.g. body)</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxColModelRobotStatic"> + <property name="geometry"> + <rect> + <x>10</x> + <y>240</y> + <width>211</width> + <height>22</height> + </rect> + </property> + </widget> + <widget class="QLabel" name="label_11"> + <property name="geometry"> + <rect> + <x>10</x> + <y>120</y> + <width>71</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>RobotNodeSet</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxRNS"> + <property name="geometry"> + <rect> + <x>10</x> + <y>140</y> + <width>211</width> + <height>22</height> + </rect> + </property> + </widget> + </widget> + <widget class="QGroupBox" name="groupBox_5"> + <property name="geometry"> + <rect> + <x>10</x> + <y>420</y> + <width>231</width> + <height>71</height> + </rect> + </property> + <property name="title"> + <string>Planning</string> + </property> + <widget class="QPushButton" name="pushButtonPlan"> + <property name="geometry"> + <rect> + <x>170</x> + <y>20</y> + <width>51</width> + <height>41</height> + </rect> + </property> + <property name="text"> + <string>Plan</string> + </property> + </widget> + <widget class="QComboBox" name="comboBoxRRT"> + <property name="geometry"> + <rect> + <x>10</x> + <y>40</y> + <width>151</width> + <height>22</height> + </rect> + </property> + </widget> + <widget class="QLabel" name="label"> + <property name="geometry"> + <rect> + <x>10</x> + <y>20</y> + <width>141</width> + <height>16</height> + </rect> + </property> + <property name="text"> + <string>Select RRT variant</string> + </property> + </widget> + </widget> + </widget> + </item> + </layout> + </widget> + <widget class="QMenuBar" name="menubar"> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>1055</width> + <height>21</height> + </rect> + </property> + </widget> + <widget class="QStatusBar" name="statusbar"/> + </widget> + <resources/> + <connections> + <connection> + <sender>horizontalSliderPos</sender> + <signal>sliderMoved(int)</signal> + <receiver>lcdNumber</receiver> + <slot>display(int)</slot> + <hints> + <hint type="sourcelabel"> + <x>700</x> + <y>375</y> + </hint> + <hint type="destinationlabel"> + <x>751</x> + <y>397</y> + </hint> + </hints> + </connection> + </connections> +</ui> diff --git a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp new file mode 100644 index 0000000000000000000000000000000000000000..633f90351a630a5b347f49503c8e6be952cfdbc8 --- /dev/null +++ b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp @@ -0,0 +1,591 @@ + +#include "RrtGuiWindow.h" +#include "VirtualRobot/EndEffector/EndEffector.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/ManipulationObject.h" +#include "VirtualRobot/Grasp.h" +#include "VirtualRobot/IK/GenericIKSolver.h" +#include "VirtualRobot/GraspSet.h" +#include "VirtualRobot/CollisionDetection/CDManager.h" +#include "VirtualRobot/ReachabilitySpace.h" +#include "VirtualRobot/XML/ObjectIO.h" +#include "VirtualRobot/XML/RobotIO.h" +#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h" +#include "MotionPlanning/CSpace/CSpaceSampled.h" +#include "MotionPlanning/Planner/Rrt.h" +#include "MotionPlanning/Planner/BiRrt.h" +#include "MotionPlanning/PostProcessing/ShortcutProcessor.h" +#include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h> +#include <QFileDialog> +#include <Eigen/Geometry> +#include <time.h> +#include <vector> +#include <iostream> +#include <cmath> + +#include "Inventor/actions/SoLineHighlightRenderAction.h" +#include <Inventor/nodes/SoShapeHints.h> +#include <Inventor/nodes/SoLightModel.h> +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/nodes/SoMatrixTransform.h> + +#include <sstream> +using namespace std; +using namespace VirtualRobot; + +float TIMER_MS = 200.0f; + +RrtGuiWindow::RrtGuiWindow(const std::string &sceneFile, const std::string &sConf, const std::string &gConf, + const std::string &rns, const std::string &colModelRob1, const std::string &colModelRob2, const std::string &colModelEnv, Qt::WFlags flags) +:QMainWindow(NULL) +{ + VR_INFO << " start " << endl; + + this->sceneFile = sceneFile; + + allSep = new SoSeparator; + allSep->ref(); + sceneFileSep = new SoSeparator; + startGoalSep = new SoSeparator; + rrtSep = new SoSeparator; + + allSep->addChild(sceneFileSep); + allSep->addChild(startGoalSep); + allSep->addChild(rrtSep); + + setupUI(); + + loadScene(); + + selectRNS(rns); + selectStart(sConf); + selectGoal(gConf); + + selectColModelRobA(colModelRob1); + selectColModelRobB(colModelRob2); + selectColModelEnv(colModelEnv); + + if (sConf!="") + UI.comboBoxStart->setEnabled(false); + if (gConf!="") + UI.comboBoxGoal->setEnabled(false); + if (rns!="") + UI.comboBoxRNS->setEnabled(false); + if (colModelRob1!="") + UI.comboBoxColModelRobot->setEnabled(false); + if (colModelRob2!="") + UI.comboBoxColModelRobotStatic->setEnabled(false); + //if (colModelEnv!="") + // UI.comboBoxColModelEnv->setEnabled(false); + + viewer->viewAll(); + + SoSensorManager *sensor_mgr = SoDB::getSensorManager(); + SoTimerSensor *timer = new SoTimerSensor(timerCB, this); + timer->setInterval(SbTime(TIMER_MS/1000.0f)); + sensor_mgr->insertTimerSensor(timer); +} + + +RrtGuiWindow::~RrtGuiWindow() +{ + allSep->unref(); +} + + +void RrtGuiWindow::timerCB(void * data, SoSensor * sensor) +{ + RrtGuiWindow *ikWindow = static_cast<RrtGuiWindow*>(data); + ikWindow->redraw(); +} + + +void RrtGuiWindow::setupUI() +{ + UI.setupUi(this); + viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP); + + // setup + viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f)); + viewer->setAccumulationBuffer(true); +#ifdef WIN32 +#ifndef _DEBUG + viewer->setAntialiasing(true, 4); +#endif +#endif + viewer->setGLRenderAction(new SoLineHighlightRenderAction); + viewer->setTransparencyType(SoGLRenderAction::BLEND); + viewer->setFeedbackVisibility(true); + viewer->setSceneGraph(allSep); + viewer->viewAll(); + + QString q1("Rrt Extend"); + QString q2("Rrt Connect"); + QString q3("BiRrt Ext/Ext"); + QString q4("BiRrt Ext/Con"); + QString q5("BiRrt Con/Ext"); + QString q6("BiRrt Con/Con"); + UI.comboBoxRRT->addItem(q1); + UI.comboBoxRRT->addItem(q2); + UI.comboBoxRRT->addItem(q3); + UI.comboBoxRRT->addItem(q4); + UI.comboBoxRRT->addItem(q5); + UI.comboBoxRRT->addItem(q6); + UI.comboBoxRRT->setCurrentIndex(3); + + UI.radioButtonSolution->setChecked(true); + + connect(UI.pushButtonLoad, SIGNAL(clicked()), this, SLOT(loadSceneWindow())); + connect(UI.checkBoxShowSolution, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxShowSolutionOpti, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxShowRRT, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxStartGoal, SIGNAL(clicked()), this, SLOT(buildVisu())); + connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(colModel())); + connect(UI.pushButtonPlan, SIGNAL(clicked()), this, SLOT(plan())); + connect(UI.horizontalSliderPos, SIGNAL(sliderMoved(int)), this, SLOT(sliderSolution(int))); + connect(UI.radioButtonSolution, SIGNAL(clicked()), this, SLOT(solutionSelected())); + connect(UI.radioButtonSolutionOpti, SIGNAL(clicked()), this, SLOT(solutionSelected())); + + connect(UI.comboBoxStart, SIGNAL(activated(int)), this, SLOT(selectStart(int))); + connect(UI.comboBoxGoal, SIGNAL(activated(int)), this, SLOT(selectGoal(int))); + connect(UI.comboBoxRNS, SIGNAL(activated(int)), this, SLOT(selectRNS(int))); + connect(UI.comboBoxColModelRobot, SIGNAL(activated(int)), this, SLOT(selectColModelRobA(int))); + connect(UI.comboBoxColModelRobotStatic, SIGNAL(activated(int)), this, SLOT(selectColModelRobB(int))); + connect(UI.comboBoxColModelEnv, SIGNAL(activated(int)), this, SLOT(selectColModelEnv(int))); + + +} + + + + +void RrtGuiWindow::closeEvent(QCloseEvent *event) +{ + quit(); + QMainWindow::closeEvent(event); +} + + +void RrtGuiWindow::buildVisu() +{ + sceneFileSep->removeAllChildren(); + + SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked())?SceneObject::Collision:SceneObject::Full; + + if (scene) + { + visualization = scene->getVisualization<CoinVisualization>(colModel); + SoNode* visualisationNode = NULL; + if (visualization) + visualisationNode = visualization->getCoinVisualization(); + + if (visualisationNode) + sceneFileSep->addChild(visualisationNode); + } + + startGoalSep->removeAllChildren(); + if (UI.checkBoxStartGoal->isChecked()) + { + if (robotStart) + { + SoNode *st = CoinVisualizationFactory::getCoinVisualization(robotStart,colModel); + if (st) + startGoalSep->addChild(st); + } + if (robotGoal) + { + SoNode *go = CoinVisualizationFactory::getCoinVisualization(robotGoal,colModel); + if (go) + startGoalSep->addChild(go); + } + } + buildRRTVisu(); + + redraw(); +} + +int RrtGuiWindow::main() +{ + SoQt::show(this); + SoQt::mainLoop(); + return 0; +} + + +void RrtGuiWindow::quit() +{ + std::cout << "RrtGuiWindow: Closing" << std::endl; + this->close(); + SoQt::exitMainLoop(); +} + +void RrtGuiWindow::loadSceneWindow() +{ + QString fi = QFileDialog::getOpenFileName(this, tr("Open Scene File"), QString(), tr("XML Files (*.xml)")); + if (fi=="") + return; + sceneFile = std::string(fi.toAscii()); + loadScene(); +} + +void RrtGuiWindow::loadScene() +{ + robot.reset(); + scene = SceneIO::loadScene(sceneFile); + if (!scene) + { + VR_ERROR << " no scene ..." << endl; + return; + } + std::vector< RobotPtr > robots = scene->getRobots(); + if (robots.size()!=1) + { + VR_ERROR << "Need exactly 1 robot" << endl; + return; + } + robot = robots[0]; + robotStart = robot->clone("StartConfig"); + robotGoal = robot->clone("GoalConfig"); + configs = scene->getRobotConfigs(robot); + if (configs.size()<2) + { + VR_ERROR << "Need at least 2 Robot Configurations" << endl; + return; + } + UI.comboBoxGoal->clear(); + UI.comboBoxStart->clear(); + for (size_t i=0; i < configs.size(); i++) + { + QString qtext = configs[i]->getName().c_str(); + UI.comboBoxStart->addItem(qtext); + UI.comboBoxGoal->addItem(qtext); + } + UI.comboBoxStart->setCurrentIndex(0); + selectStart(0); + UI.comboBoxGoal->setCurrentIndex(1); + selectGoal(1); + + std::vector<SceneObjectSetPtr> soss = scene->getSceneObjectSets(); + UI.comboBoxColModelEnv->clear(); + QString qtext; + for (size_t i=0; i < soss.size(); i++) + { + qtext = soss[i]->getName().c_str(); + UI.comboBoxColModelEnv->addItem(qtext); + } + qtext = "<none>"; + UI.comboBoxColModelEnv->addItem(qtext); + + std::vector<RobotNodeSetPtr> rnss = robot->getRobotNodeSets(); + UI.comboBoxColModelRobot->clear(); + UI.comboBoxColModelRobotStatic->clear(); + UI.comboBoxRNS->clear(); + + for (size_t i=0; i < rnss.size(); i++) + { + qtext = rnss[i]->getName().c_str(); + UI.comboBoxColModelRobot->addItem(qtext); + UI.comboBoxColModelRobotStatic->addItem(qtext); + UI.comboBoxRNS->addItem(qtext); + } + qtext = "<none>"; + UI.comboBoxColModelRobot->addItem(qtext); + UI.comboBoxColModelRobotStatic->addItem(qtext); + + buildVisu(); +} + +void RrtGuiWindow::selectStart(const std::string &conf) +{ + for (size_t i=0;i<configs.size();i++) + { + if (configs[i]->getName()==conf) + { + selectStart(i); + UI.comboBoxStart->setCurrentIndex(i); + return; + } + } + VR_ERROR << "No configuration with name <" << conf << "> found..." << endl; +} +void RrtGuiWindow::selectGoal(const std::string &conf) +{ + for (size_t i=0;i<configs.size();i++) + { + if (configs[i]->getName()==conf) + { + selectGoal(i); + UI.comboBoxGoal->setCurrentIndex(i); + return; + } + } + VR_ERROR << "No configuration with name <" << conf << "> found..." << endl; +} + +void RrtGuiWindow::selectRNS(const std::string &rns) +{ + if (!robot) + return; + std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets(); + for (size_t i=0;i<rnss.size();i++) + { + if (rnss[i]->getName()==rns) + { + selectRNS(i); + UI.comboBoxRNS->setCurrentIndex(i); + return; + } + } + VR_ERROR << "No rns with name <" << rns << "> found..." << endl; +} + +void RrtGuiWindow::selectColModelRobA(const std::string &colModel) +{ + if (!robot) + return; + std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets(); + for (size_t i=0;i<rnss.size();i++) + { + if (rnss[i]->getName()==colModel) + { + selectColModelRobA(i); + UI.comboBoxColModelRobot->setCurrentIndex(i); + return; + } + } + VR_ERROR << "No col model set with name <" << colModel << "> found..." << endl; +} +void RrtGuiWindow::selectColModelRobB(const std::string &colModel) +{ + if (!robot) + return; + std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets(); + for (size_t i=0;i<rnss.size();i++) + { + if (rnss[i]->getName()==colModel) + { + selectColModelRobB(i); + UI.comboBoxColModelRobotStatic->setCurrentIndex(i); + return; + } + } + VR_ERROR << "No col model set with name <" << colModel << "> found..." << endl; +} +void RrtGuiWindow::selectColModelEnv(const std::string &colModel) +{ + if (!scene) + return; + std::vector< SceneObjectSetPtr > rnss = scene->getSceneObjectSets(); + for (size_t i=0;i<rnss.size();i++) + { + if (rnss[i]->getName()==colModel) + { + selectColModelEnv(i); + UI.comboBoxColModelEnv->setCurrentIndex(i); + return; + } + } + VR_ERROR << "No scene object set with name <" << colModel << "> found..." << endl; +} + +void RrtGuiWindow::selectStart(int nr) +{ + if (nr<0 || nr>=(int)configs.size()) + return; + if (robotStart) + configs[nr]->applyToRobot(robotStart); + if (robot) + configs[nr]->applyToRobot(robot); + configs[nr]->setJointValues(); + if (rns) + rns->getJointValues(startConfig); +} + +void RrtGuiWindow::selectGoal(int nr) +{ + if (nr<0 || nr>=(int)configs.size()) + return; + if (robotGoal) + configs[nr]->applyToRobot(robotGoal); + configs[nr]->setJointValues(); + if (rns) + rns->getJointValues(goalConfig); +} +void RrtGuiWindow::selectRNS(int nr) +{ + this->rns.reset(); + if (!robot) + return; + std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets(); + if (nr<0 || nr>=(int)rnss.size()) + return; + this->rns = rnss[nr]; +} +void RrtGuiWindow::selectColModelRobA(int nr) +{ + colModelRobA.reset(); + if (!robot) + return; + std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets(); + if (nr<0 || nr>=(int)rnss.size()) + return; + this->colModelRobA = robot->getSceneObjectSet(rnss[nr]->getName()); +} +void RrtGuiWindow::selectColModelRobB(int nr) +{ + colModelRobB.reset(); + if (!robot) + return; + std::vector< RobotNodeSetPtr > rnss = robot->getRobotNodeSets(); + if (nr<0 || nr>=(int)rnss.size()) + return; + this->colModelRobB = robot->getSceneObjectSet(rnss[nr]->getName()); +} +void RrtGuiWindow::selectColModelEnv(int nr) +{ + colModelEnv.reset(); + if (!scene) + return; + std::vector< SceneObjectSetPtr > rnss = scene->getSceneObjectSets(); + if (nr<0 || nr>=(int)rnss.size()) + return; + this->colModelEnv = scene->getSceneObjectSet(rnss[nr]->getName()); +} +void RrtGuiWindow::buildRRTVisu() +{ + rrtSep->removeAllChildren(); + if (!cspace || !robot) + return; + + boost::shared_ptr<Saba::CoinRrtWorkspaceVisualization> w(new Saba::CoinRrtWorkspaceVisualization(robot,cspace,rns->getTCP()->getName())); + if (UI.checkBoxShowRRT->isChecked()) + { + if (tree) + w->addTree(tree); + if (tree2) + w->addTree(tree2); + } + if (UI.checkBoxShowSolution->isChecked() && solution) + w->addCSpacePath(solution); + if (UI.checkBoxShowSolutionOpti->isChecked() && solutionOptimized) + w->addCSpacePath(solutionOptimized,Saba::CoinRrtWorkspaceVisualization::eGreen); + w->addConfiguration(startConfig,Saba::CoinRrtWorkspaceVisualization::eGreen,3.0f); + w->addConfiguration(goalConfig,Saba::CoinRrtWorkspaceVisualization::eRed,3.0f); + SoSeparator *sol = w->getCoinVisualization(); + rrtSep->addChild(sol); +} + +void RrtGuiWindow::plan() +{ + if (!robot || !rns) + return; + + // setup collision detection + CDManagerPtr cdm(new CDManager()); + if (colModelRobA) + cdm->addCollisionModel(colModelRobA); + if (colModelRobB) + cdm->addCollisionModel(colModelRobB); + if (colModelEnv) + cdm->addCollisionModel(colModelEnv); + + cspace.reset(new Saba::CSpaceSampled(robot,cdm,rns)); + float sampl = (float)UI.doubleSpinBoxCSpaceSampling->value(); + float samplDCD = (float)UI.doubleSpinBoxColChecking->value(); + cspace->setSamplingSize(sampl); + cspace->setSamplingSizeDCD(samplDCD); + Saba::Rrt::RrtMethod mode; + Saba::Rrt::RrtMethod mode2; + bool planOk = false; + Saba::RrtPtr mp; + Saba::BiRrtPtr mpBi; + bool biRRT = false; + if (UI.comboBoxRRT->currentIndex()==0 || UI.comboBoxRRT->currentIndex()==1) + { + if (UI.comboBoxRRT->currentIndex()==0) + mode = Saba::Rrt::eExtend; + else + mode = Saba::Rrt::eConnect; + Saba::RrtPtr rrt(new Saba::Rrt(cspace,mode)); + mp = rrt; + } else + { + biRRT = true; + if (UI.comboBoxRRT->currentIndex()==2) + { + mode = Saba::Rrt::eExtend; + mode2 = Saba::Rrt::eExtend; + } else if (UI.comboBoxRRT->currentIndex()==3) + { + mode = Saba::Rrt::eExtend; + mode2 = Saba::Rrt::eConnect; + } else if (UI.comboBoxRRT->currentIndex()==4) + { + mode = Saba::Rrt::eConnect; + mode2 = Saba::Rrt::eExtend; + } else + { + mode = Saba::Rrt::eConnect; + mode2 = Saba::Rrt::eConnect; + } + Saba::BiRrtPtr rrt(new Saba::BiRrt(cspace,mode,mode2)); + mp = rrt; + mpBi = rrt; + } + mp->setStart(startConfig); + mp->setGoal(goalConfig); + + bool planOK = mp->plan(); + if (planOK) + { + VR_INFO << " Planning succeeded " << endl; + solution = mp->getSolution(); + Saba::ShortcutProcessorPtr postProcessing(new Saba::ShortcutProcessor(solution,cspace,false)); + solutionOptimized = postProcessing->optimize(100); + tree = mp->getTree(); + if (biRRT) + tree2 = mpBi->getTree2(); + else + tree2.reset(); + + } else + VR_INFO << " Planning failed" << endl; + + sliderSolution(1000); + + buildVisu(); +} + +void RrtGuiWindow::colModel() +{ + buildVisu(); +} +void RrtGuiWindow::solutionSelected() +{ + sliderSolution(UI.horizontalSliderPos->sliderPosition()); +} +void RrtGuiWindow::sliderSolution( int pos ) +{ + if (!solution) + return; + Saba::CSpacePathPtr s = solution; + if (UI.radioButtonSolutionOpti->isChecked() && solutionOptimized) + s = solutionOptimized; + float p = (float)pos/1000.0f; + Eigen::VectorXf iPos; + s->interpolatePath(p,iPos); + rns->setJointValues(iPos); + redraw(); +} + +void RrtGuiWindow::redraw() +{ + viewer->scheduleRedraw(); + UI.frameViewer->update(); + viewer->scheduleRedraw(); + this->update(); + viewer->scheduleRedraw(); +} + + diff --git a/MotionPlanning/examples/RrtGui/RrtGuiWindow.h b/MotionPlanning/examples/RrtGui/RrtGuiWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..66e4124ed440f5e21d858d2a0d01e370c20c7575 --- /dev/null +++ b/MotionPlanning/examples/RrtGui/RrtGuiWindow.h @@ -0,0 +1,122 @@ + +#ifndef __RrtGui_WINDOW_H_ +#define __RrtGui_WINDOW_H_ + +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/Nodes/RobotNode.h> +#include <VirtualRobot/XML/SceneIO.h> +#include <VirtualRobot/Visualization/VisualizationFactory.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <VirtualRobot/Obstacle.h> +#include <VirtualRobot/ManipulationObject.h> + +#include "MotionPlanning/Saba.h" +#include "MotionPlanning/CSpace/CSpacePath.h" + +#include <string.h> +#include <QtCore/QtGlobal> +#include <QtGui/QtGui> +#include <QtCore/QtCore> + +#include <Inventor/sensors/SoTimerSensor.h> +#include <Inventor/nodes/SoEventCallback.h> +#include <Inventor/Qt/viewers/SoQtExaminerViewer.h> +#include <Inventor/Qt/SoQt.h> +#include <Inventor/nodes/SoSeparator.h> + + +#include <vector> + +#include "ui_RrtGui.h" + +class RrtGuiWindow : public QMainWindow +{ + Q_OBJECT +public: + RrtGuiWindow( const std::string &sceneFile, const std::string &sConf, const std::string &gConf, const std::string &rns, + const std::string &colModelRob1, const std::string &colModelRob2, const std::string &colModelEnv, Qt::WFlags flags = 0); + ~RrtGuiWindow(); + + /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */ + int main(); + + void redraw(); +public slots: + /*! Closes the window and exits SoQt runloop. */ + void quit(); + + /*!< Overriding the close event, so we know when the window was closed by the user. */ + void closeEvent(QCloseEvent *event); + + void loadSceneWindow(); + + void selectStart(int nr); + void selectGoal(int nr); + void selectRNS(int nr); + + void selectColModelRobA(int nr); + void selectColModelRobB(int nr); + void selectColModelEnv(int nr); + + void colModel(); + void solutionSelected(); + + void sliderSolution(int pos); + + void buildVisu(); + + void plan(); + +protected: + + void loadScene(); + + void setupUI(); + QString formatString(const char *s, float f); + void buildRRTVisu(); + void selectStart(const std::string &conf); + void selectGoal(const std::string &conf); + void selectRNS(const std::string &rns); + + static void timerCB(void * data, SoSensor * sensor); + void buildRrtVisu(); + void selectColModelRobA(const std::string &colModel); + void selectColModelRobB(const std::string &colModel); + void selectColModelEnv(const std::string &colModel); + Ui::MainWindowRRTDemo UI; + SoQtExaminerViewer *viewer; /*!< Viewer to display the 3D model of the robot and the environment. */ + + SoSeparator *allSep; + SoSeparator *sceneFileSep; + SoSeparator *startGoalSep; + SoSeparator *rrtSep; + + VirtualRobot::RobotPtr robot; + VirtualRobot::RobotPtr robotStart; + VirtualRobot::RobotPtr robotGoal; + + Saba::CSpaceSampledPtr cspace; + Eigen::VectorXf startConfig; + Eigen::VectorXf goalConfig; + + VirtualRobot::RobotNodeSetPtr rns; + VirtualRobot::SceneObjectSetPtr colModelRobA; + VirtualRobot::SceneObjectSetPtr colModelRobB; + VirtualRobot::SceneObjectSetPtr colModelEnv; + + std::vector< VirtualRobot::RobotConfigPtr > configs; + + std::string sceneFile; + VirtualRobot::ScenePtr scene; + + Saba::CSpacePathPtr solution; + Saba::CSpacePathPtr solutionOptimized; + Saba::CSpaceTreePtr tree; + Saba::CSpaceTreePtr tree2; + + boost::shared_ptr<VirtualRobot::CoinVisualization> visualization; +}; + +#endif // __RrtGui_WINDOW_H_ diff --git a/MotionPlanning/examples/RrtGui/scenes/planning.xml b/MotionPlanning/examples/RrtGui/scenes/planning.xml new file mode 100644 index 0000000000000000000000000000000000000000..e464331866ac49be6e36ece3f9fef77d84accfae --- /dev/null +++ b/MotionPlanning/examples/RrtGui/scenes/planning.xml @@ -0,0 +1,120 @@ +<?xml version="1.0" encoding="UTF-8" ?> + +<Scene name="iCubScene"> + + <Robot name="iCub" initConfig="init"> + <File>/robots/iCub/iCub.xml</File> + <Configuration name="init"> + <Node name="Left Arm Shoulder2" unit="radian" value="0.6"/> + <Node name="Left Arm Elbow1" unit="radian" value="1.8"/> + <Node name="Left Arm Elbow2" unit="radian" value="-0.5"/> + <Node name="Left Hand Thumb Joint1" unit="radian" value="0.8"/> + <Node name="Left Hand Thumb Joint2" unit="radian" value="-0.2"/> + <Node name="Right Arm Shoulder2" unit="radian" value="0.6"/> + <Node name="Right Arm Elbow1" unit="radian" value="1.8"/> + <Node name="Right Arm Elbow2" unit="radian" value="-0.5"/> + <Node name="Right Hand Thumb Joint1" unit="radian" value="0.8"/> + <Node name="Right Hand Thumb Joint2" unit="radian" value="-0.2"/> + </Configuration> + + <Configuration name="start"> + <Node name="Torso1" unit="radian" value="0.0"/> + <Node name="Torso2" unit="radian" value="0.0"/> + <Node name="Torso3" unit="radian" value="-0.75"/> + <Node name="Left Arm Shoulder1" unit="radian" value="-1.6"/> + <Node name="Left Arm Shoulder2" unit="radian" value="1.8"/> + <Node name="Left Arm Shoulder3" unit="radian" value="0"/> + <Node name="Left Arm Elbow1" unit="radian" value="0"/> + <Node name="Left Arm Elbow2" unit="radian" value="0"/> + <Node name="Left Arm Wrist1" unit="radian" value="0"/> + <Node name="Left Arm Wrist2" unit="radian" value="0"/> + </Configuration> + + <Configuration name="goal"> + <Node name="Torso1" unit="radian" value="0.6"/> + <Node name="Torso2" unit="radian" value="-0.4"/> + <Node name="Torso3" unit="radian" value="0.45"/> + <Node name="Left Arm Shoulder1" unit="radian" value="-0.7"/> + <Node name="Left Arm Shoulder2" unit="radian" value="0.15"/> + <Node name="Left Arm Shoulder3" unit="radian" value="0"/> + <Node name="Left Arm Elbow1" unit="radian" value="1"/> + <Node name="Left Arm Elbow2" unit="radian" value="0"/> + <Node name="Left Arm Wrist1" unit="radian" value="0"/> + <Node name="Left Arm Wrist2" unit="radian" value="0"/> + </Configuration> + + <!-- These joints are considered for motion planning--> + <RobotNodeSet name="Planning" kinematicRoot="iCubRoot" tcp="Left Arm TCP"> + <Node name="Torso1"/> + <Node name="Torso2"/> + <Node name="Torso3"/> + <Node name="Left Arm Shoulder1"/> + <Node name="Left Arm Shoulder2"/> + <Node name="Left Arm Shoulder3"/> + <Node name="Left Arm Elbow1"/> + <Node name="Left Arm Elbow2"/> + <Node name="Left Arm Wrist1"/> + <Node name="Left Arm Wrist2"/> + </RobotNodeSet> + + <!-- The first collision model (arm and hand) --> + <RobotNodeSet name="ColModel Robot Moving"> + <Node name="Left Arm Shoulder3"/> + <Node name="Left Arm Elbow2"/> + <Node name="Left Hand Palm"/> + <Node name="Left Hand Thumb Joint1"/> + <Node name="Left Hand Thumb Joint2"/> + <Node name="Left Hand Thumb Joint3"/> + <Node name="Left Hand Thumb Joint4"/> + + <Node name="Left Hand Index Joint1"/> + <Node name="Left Hand Index Joint2"/> + <Node name="Left Hand Index Joint3"/> + <Node name="Left Hand Index Joint4"/> + + <Node name="Left Hand Middle Joint1"/> + <Node name="Left Hand Middle Joint2"/> + <Node name="Left Hand Middle Joint3"/> + <Node name="Left Hand Middle Joint4"/> + + <Node name="Left Hand Ring Joint1"/> + <Node name="Left Hand Ring Joint2"/> + <Node name="Left Hand Ring Joint3"/> + <Node name="Left Hand Ring Joint4"/> + + <Node name="Left Hand Pinky Joint1"/> + <Node name="Left Hand Pinky Joint2"/> + <Node name="Left Hand Pinky Joint3"/> + <Node name="Left Hand Pinky Joint4"/> + </RobotNodeSet> + + <!-- The second collision model (torso, head and legs)--> + <RobotNodeSet name="ColModel Robot Body"> + <!--Node name="UpperBodyModel"/--><!-- reports self collisions with shoulder3--> + <Node name="LowerBody"/> + <Node name="Neck3"/> + <Node name="Left Leg Waist3"/> + <Node name="Left Leg Knee"/> + <Node name="Right Leg Waist3"/> + <Node name="Right Leg Knee"/> + </RobotNodeSet> + + </Robot> + + + <ManipulationObject name="RiceBox"> + <File type="Inventor">objects/riceBox_iCub_gr0.25.xml</File> + + <GlobalPose> + <Transform> + <Translation x="-200" y="-200" z="650"/> + <rollpitchyaw units="degree" roll="90" pitch="0" yaw="90"/> + </Transform> + </GlobalPose> + </ManipulationObject> + + <SceneObjectSet name="ColModel Obstacles"> + <SceneObject name="RiceBox"/> + </SceneObjectSet> + +</Scene> \ No newline at end of file diff --git a/MotionPlanning/tests/CMakeLists.txt b/MotionPlanning/tests/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..9d8eb5673bfce1e01eb83e9da6ae7038ae8594dd --- /dev/null +++ b/MotionPlanning/tests/CMakeLists.txt @@ -0,0 +1,3 @@ + +ADD_SABA_TEST( SabaCSpaceTest ) + diff --git a/MotionPlanning/tests/SabaCSpaceTest.cpp b/MotionPlanning/tests/SabaCSpaceTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2a5bde047cddfe579ca23c167c4a091227fe6b56 --- /dev/null +++ b/MotionPlanning/tests/SabaCSpaceTest.cpp @@ -0,0 +1,138 @@ +/** +* @package VirtualRobot +* @author Nikolaus Vahrenkamp +* @copyright 2010 Nikolaus Vahrenkamp +*/ + +#define BOOST_TEST_MODULE Saba_SabaCSpaceTest + +#include <VirtualRobot/VirtualRobotTest.h> +#include <VirtualRobot/XML/RobotIO.h> +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/RobotNodeSet.h> +#include <VirtualRobot/CollisionDetection/CollisionChecker.h> +#include <VirtualRobot/CollisionDetection/CollisionModel.h> +#include <VirtualRobot/Obstacle.h> +#include <CSpace/CSpaceSampled.h> +#include <VirtualRobot/CollisionDetection/CDManager.h> +#include <string> + +#include <Eigen/Core> +#include <Eigen/Geometry> + + +BOOST_AUTO_TEST_SUITE(CSpace) + + +BOOST_AUTO_TEST_CASE(testCollisionModel) +{ + const std::string robotString = + "<Robot Type='MyDemoRobotType' StandardName='ExampleRobo' RootNode='Joint1'>" + " <RobotNode name='Joint1'>" + " <Limits unit='degree' lo='-180' hi='180'/>" + " <Joint type='revolute'>" + " <Axis x='1' y='0' z='0'/>" + " </Joint>" + " </RobotNode>" + "</Robot>"; + VirtualRobot::RobotPtr rob = VirtualRobot::RobotIO::createRobotFromString(robotString); + BOOST_REQUIRE(rob); + std::vector< std::string > nodes; + nodes.push_back(std::string("Joint1")); + VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(rob,"nodeSet",nodes); + VirtualRobot::CDManagerPtr cdm( new VirtualRobot::CDManager() ); + Saba::CSpaceSampledPtr cspace(new Saba::CSpaceSampled(rob,cdm,rns)); + BOOST_REQUIRE(cspace); + + Eigen::VectorXf p1(1); + Eigen::VectorXf p2(1); + float d; + + // ---distance tests--- + p1(0) = 0; + p2(0) = 0.5f; + d = cspace->calcDist(p1,p2); + BOOST_CHECK_EQUAL(d,0.5f); + + p1(0) = 0; + p2(0) = -0.5f; + d = cspace->calcDist(p1,p2); + BOOST_CHECK_EQUAL(d,0.5f); + + p1(0) = 0.5; + p2(0) = -0.5f; + d = cspace->calcDist(p1,p2); + BOOST_CHECK_EQUAL(d,1.0f); + + p1(0) = -0.5; + p2(0) = 0.5f; + d = cspace->calcDist(p1,p2); + BOOST_CHECK_EQUAL(d,1.0f); + + // traverse border + p1(0) = -0.75f * (float)M_PI; + p2(0) = 0.75f * (float)M_PI; + d = cspace->calcDist(p1,p2); + BOOST_CHECK_EQUAL(d,(float)M_PI*0.5f); + + p1(0) = 0.75f * (float)M_PI; + p2(0) = -0.75f * (float)M_PI; + d = cspace->calcDist(p1,p2); + BOOST_CHECK_EQUAL(d,(float)M_PI*0.5f); + + // ---interpolation tests--- + p1(0) = 0; + p2(0) = 0.5f; + d = cspace->interpolate(p1,p2,0,0); + BOOST_CHECK_EQUAL(d,0); + d = cspace->interpolate(p1,p2,0,1); + BOOST_CHECK_EQUAL(d,0.5f); + d = cspace->interpolate(p1,p2,0,0.5f); + BOOST_CHECK_EQUAL(d,0.25f); + + p1(0) = 0; + p2(0) = -0.5f; + d = cspace->interpolate(p1,p2,0,0); + BOOST_CHECK_EQUAL(d,0); + d = cspace->interpolate(p1,p2,0,1); + BOOST_CHECK_EQUAL(d,-0.5f); + d = cspace->interpolate(p1,p2,0,0.5f); + BOOST_CHECK_EQUAL(d,-0.25f); + + p1(0) = 0.5; + p2(0) = -0.5f; + d = cspace->interpolate(p1,p2,0,0); + BOOST_CHECK_EQUAL(d,0.5f); + d = cspace->interpolate(p1,p2,0,1); + BOOST_CHECK_EQUAL(d,-0.5f); + d = cspace->interpolate(p1,p2,0,0.5f); + BOOST_CHECK_EQUAL(d,0); + + p1(0) = -0.5; + p2(0) = 0.5f; + d = cspace->interpolate(p1,p2,0,0); + BOOST_CHECK_EQUAL(d,-0.5f); + d = cspace->interpolate(p1,p2,0,1); + BOOST_CHECK_EQUAL(d,0.5f); + d = cspace->interpolate(p1,p2,0,0.5f); + BOOST_CHECK_EQUAL(d,0); + + // traverse border + p1(0) = -0.75f * (float)M_PI; + p2(0) = 0.75f * (float)M_PI; + d = cspace->interpolate(p1,p2,0,0); + BOOST_CHECK_EQUAL(d,-0.75f * (float)M_PI); + d = cspace->interpolate(p1,p2,0,1); + BOOST_CHECK_EQUAL(d,0.75f * (float)M_PI); + d = cspace->interpolate(p1,p2,0,0.1f); // dist is 0.5PI -> result (-0.75 - 0.1*0.5) * PI + BOOST_CHECK_EQUAL(d,-0.75f * (float)M_PI - 0.1f*0.5f*(float)M_PI); + d = cspace->interpolate(p1,p2,0,0.4f); + BOOST_CHECK_EQUAL(d,-0.75f * (float)M_PI - 0.4f*0.5f*(float)M_PI); + d = cspace->interpolate(p1,p2,0,0.6f); + BOOST_CHECK_EQUAL(d,0.75f * (float)M_PI + 0.4f*0.5f*(float)M_PI); + d = cspace->interpolate(p1,p2,0,0.9f); + BOOST_CHECK_EQUAL(d,0.75f * (float)M_PI + 0.1f*0.5f*(float)M_PI); + +} + +BOOST_AUTO_TEST_SUITE_END()