diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt new file mode 100644 index 0000000..3bb00c6 --- /dev/null +++ b/apps/CMakeLists.txt @@ -0,0 +1,14 @@ +# Add applications +ADD_SUBDIRECTORY(InterfaceCOLMAP) +ADD_SUBDIRECTORY(InterfaceMetashape) +ADD_SUBDIRECTORY(InterfaceMVSNet) +ADD_SUBDIRECTORY(InterfacePolycam) +ADD_SUBDIRECTORY(DensifyPointCloud) +ADD_SUBDIRECTORY(ReconstructMesh) +ADD_SUBDIRECTORY(RefineMesh) +ADD_SUBDIRECTORY(TextureMesh) +ADD_SUBDIRECTORY(TransformScene) +ADD_SUBDIRECTORY(Viewer) +if(OpenMVS_ENABLE_TESTS) + ADD_SUBDIRECTORY(Tests) +endif() diff --git a/apps/DensifyPointCloud/CMakeLists.txt b/apps/DensifyPointCloud/CMakeLists.txt new file mode 100644 index 0000000..38e2731 --- /dev/null +++ b/apps/DensifyPointCloud/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(DensifyPointCloud "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS DensifyPointCloud + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/DensifyPointCloud/DensifyPointCloud.cpp b/apps/DensifyPointCloud/DensifyPointCloud.cpp new file mode 100644 index 0000000..f314c77 --- /dev/null +++ b/apps/DensifyPointCloud/DensifyPointCloud.cpp @@ -0,0 +1,433 @@ +/* + * DensifyPointCloud.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("DensifyPointCloud") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strPointCloudFileName; +String strOutputFileName; +String strViewNeighborsFileName; +String strOutputViewNeighborsFileName; +String strMeshFileName; +String strExportROIFileName; +String strImportROIFileName; +String strDenseConfigFileName; +String strExportDepthMapsName; +String strMaskPath; +float fMaxSubsceneArea; +float fSampleMesh; +float fBorderROI; +bool bCrop2ROI; +int nEstimateROI; +int nTowerMode; +int nFusionMode; +float fEstimateScale; +int thFilterPointCloud; +int nExportNumViews; +int nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + #ifdef _USE_CUDA + ("cuda-device", boost::program_options::value(&CUDA::desiredDeviceID)->default_value(-1), "CUDA device number to be used for depth-map estimation (-2 - CPU processing, -1 - best GPU, >=0 - device index)") + #endif + ; + + // group of options allowed both on command line and in config file + #ifdef _USE_CUDA + const unsigned nNumViewsDefault(8); + const unsigned numIters(4); + #else + const unsigned nNumViewsDefault(5); + const unsigned numIters(3); + #endif + unsigned nResolutionLevel; + unsigned nMaxResolution; + unsigned nMinResolution; + unsigned nNumViews; + unsigned nMinViewsFuse; + unsigned nSubResolutionLevels; + unsigned nEstimationIters; + unsigned nEstimationGeometricIters; + unsigned nEstimateColors; + unsigned nEstimateNormals; + unsigned nOptimize; + int nIgnoreMaskLabel; + bool bRemoveDmaps; + boost::program_options::options_description config("Densify options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses and image list") + ("pointcloud-file,p", boost::program_options::value(&OPT::strPointCloudFileName), "sparse point-cloud with views file name to densify (overwrite existing point-cloud)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the dense point-cloud (optional)") + ("view-neighbors-file", boost::program_options::value(&OPT::strViewNeighborsFileName), "input filename containing the list of views and their neighbors (optional)") + ("output-view-neighbors-file", boost::program_options::value(&OPT::strOutputViewNeighborsFileName), "output filename containing the generated list of views and their neighbors") + ("resolution-level", boost::program_options::value(&nResolutionLevel)->default_value(1), "how many times to scale down the images before point cloud computation") + ("max-resolution", boost::program_options::value(&nMaxResolution)->default_value(2560), "do not scale images higher than this resolution") + ("min-resolution", boost::program_options::value(&nMinResolution)->default_value(640), "do not scale images lower than this resolution") + ("sub-resolution-levels", boost::program_options::value(&nSubResolutionLevels)->default_value(2), "number of patch-match sub-resolution iterations (0 - disabled)") + ("number-views", boost::program_options::value(&nNumViews)->default_value(nNumViewsDefault), "number of views used for depth-map estimation (0 - all neighbor views available)") + ("number-views-fuse", boost::program_options::value(&nMinViewsFuse)->default_value(3), "minimum number of images that agrees with an estimate during fusion in order to consider it inlier (<2 - only merge depth-maps)") + ("ignore-mask-label", boost::program_options::value(&nIgnoreMaskLabel)->default_value(-1), "label value to ignore in the image mask, stored in the MVS scene or next to each image with '.mask.png' extension (<0 - disabled)") + ("mask-path", boost::program_options::value(&OPT::strMaskPath), "path to folder containing mask images with '.mask.png' extension") + ("iters", boost::program_options::value(&nEstimationIters)->default_value(numIters), "number of patch-match iterations") + ("geometric-iters", boost::program_options::value(&nEstimationGeometricIters)->default_value(2), "number of geometric consistent patch-match iterations (0 - disabled)") + ("estimate-colors", boost::program_options::value(&nEstimateColors)->default_value(2), "estimate the colors for the dense point-cloud (0 - disabled, 1 - final, 2 - estimate)") + ("estimate-normals", boost::program_options::value(&nEstimateNormals)->default_value(2), "estimate the normals for the dense point-cloud (0 - disabled, 1 - final, 2 - estimate)") + ("estimate-scale", boost::program_options::value(&OPT::fEstimateScale)->default_value(0.f), "estimate the point-scale for the dense point-cloud (scale multiplier, 0 - disabled)") + ("sub-scene-area", boost::program_options::value(&OPT::fMaxSubsceneArea)->default_value(0.f), "split the scene in sub-scenes such that each sub-scene surface does not exceed the given maximum sampling area (0 - disabled)") + ("sample-mesh", boost::program_options::value(&OPT::fSampleMesh)->default_value(0.f), "uniformly samples points on a mesh (0 - disabled, <0 - number of points, >0 - sample density per square unit)") + ("fusion-mode", boost::program_options::value(&OPT::nFusionMode)->default_value(0), "depth-maps fusion mode (-2 - fuse disparity-maps, -1 - export disparity-maps only, 0 - depth-maps & fusion, 1 - export depth-maps only)") + ("postprocess-dmaps", boost::program_options::value(&nOptimize)->default_value(7), "flags used to filter the depth-maps after estimation (0 - disabled, 1 - remove-speckles, 2 - fill-gaps, 4 - adjust-filter)") + ("filter-point-cloud", boost::program_options::value(&OPT::thFilterPointCloud)->default_value(0), "filter dense point-cloud based on visibility (0 - disabled)") + ("export-number-views", boost::program_options::value(&OPT::nExportNumViews)->default_value(0), "export points with >= number of views (0 - disabled, <0 - save MVS project too)") + ("roi-border", boost::program_options::value(&OPT::fBorderROI)->default_value(0), "add a border to the region-of-interest when cropping the scene (0 - disabled, >0 - percentage, <0 - absolute)") + ("estimate-roi", boost::program_options::value(&OPT::nEstimateROI)->default_value(2), "estimate and set region-of-interest (0 - disabled, 1 - enabled, 2 - adaptive)") + ("crop-to-roi", boost::program_options::value(&OPT::bCrop2ROI)->default_value(true), "crop scene using the region-of-interest") + ("remove-dmaps", boost::program_options::value(&bRemoveDmaps)->default_value(false), "remove depth-maps after fusion") + ("tower-mode", boost::program_options::value(&OPT::nTowerMode)->default_value(4), "add a cylinder of points in the center of ROI; scene assume to be Z-up oriented (0 - disabled, 1 - replace, 2 - append, 3 - select neighbors, 4 - select neighbors & append, <0 - force tower mode)") + ; + + // hidden options, allowed both on command line and + // in config file, but will not be shown to the user + boost::program_options::options_description hidden("Hidden options"); + hidden.add_options() + ("mesh-file", boost::program_options::value(&OPT::strMeshFileName), "mesh file name used for image pair overlap estimation") + ("export-roi-file", boost::program_options::value(&OPT::strExportROIFileName), "ROI file name to be exported form the scene") + ("import-roi-file", boost::program_options::value(&OPT::strImportROIFileName), "ROI file name to be imported into the scene") + ("dense-config-file", boost::program_options::value(&OPT::strDenseConfigFileName), "optional configuration file for the densifier (overwritten by the command line options)") + ("export-depth-maps-name", boost::program_options::value(&OPT::strExportDepthMapsName), "render given mesh and save the depth-map for every image to this file name base (empty - disabled)") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config).add(hidden); + + boost::program_options::options_description config_file_options; + config_file_options.add(config).add(hidden); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + if (OPT::vm.count("help") || OPT::strInputFileName.empty()) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (OPT::strInputFileName.empty()) + return false; + + // initialize optional options + Util::ensureValidPath(OPT::strPointCloudFileName); + Util::ensureValidPath(OPT::strOutputFileName); + Util::ensureValidPath(OPT::strViewNeighborsFileName); + Util::ensureValidPath(OPT::strOutputViewNeighborsFileName); + Util::ensureValidPath(OPT::strMeshFileName); + Util::ensureValidPath(OPT::strExportROIFileName); + Util::ensureValidPath(OPT::strImportROIFileName); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFileFullName(OPT::strInputFileName) + _T("_dense.mvs"); + + // init dense options + if (!OPT::strDenseConfigFileName.empty()) + OPT::strDenseConfigFileName = MAKE_PATH_SAFE(OPT::strDenseConfigFileName); + OPTDENSE::init(); + const bool bValidConfig(OPTDENSE::oConfig.Load(OPT::strDenseConfigFileName)); + OPTDENSE::update(); + OPTDENSE::nResolutionLevel = nResolutionLevel; + OPTDENSE::nMaxResolution = nMaxResolution; + OPTDENSE::nMinResolution = nMinResolution; + OPTDENSE::nSubResolutionLevels = nSubResolutionLevels; + OPTDENSE::nNumViews = nNumViews; + OPTDENSE::nMinViewsFuse = nMinViewsFuse; + OPTDENSE::nEstimationIters = nEstimationIters; + OPTDENSE::nEstimationGeometricIters = nEstimationGeometricIters; + OPTDENSE::nEstimateColors = nEstimateColors; + OPTDENSE::nEstimateNormals = nEstimateNormals; + OPTDENSE::nOptimize = nOptimize; + OPTDENSE::nIgnoreMaskLabel = nIgnoreMaskLabel; + OPTDENSE::bRemoveDmaps = bRemoveDmaps; + if (!bValidConfig && !OPT::strDenseConfigFileName.empty()) + OPTDENSE::oConfig.Save(OPT::strDenseConfigFileName); + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + Scene scene(OPT::nMaxThreads); + if (OPT::fSampleMesh != 0) { + // sample input mesh and export the obtained point-cloud + if (!scene.Load(MAKE_PATH_SAFE(OPT::strInputFileName), true) || scene.mesh.IsEmpty()) + return EXIT_FAILURE; + TD_TIMER_START(); + PointCloud pointcloud; + if (OPT::fSampleMesh > 0) + scene.mesh.SamplePoints(OPT::fSampleMesh, 0, pointcloud); + else + scene.mesh.SamplePoints(ROUND2INT(-OPT::fSampleMesh), pointcloud); + VERBOSE("Sample mesh completed: %u points (%s)", pointcloud.GetSize(), TD_TIMER_GET_FMT().c_str()); + pointcloud.Save(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))+_T(".ply")); + return EXIT_SUCCESS; + } + // load and estimate a dense point-cloud + const Scene::SCENE_TYPE sceneType(scene.Load(MAKE_PATH_SAFE(OPT::strInputFileName))); + if (sceneType == Scene::SCENE_NA) + return EXIT_FAILURE; + if (!OPT::strPointCloudFileName.empty() && !scene.pointcloud.Load(MAKE_PATH_SAFE(OPT::strPointCloudFileName))) { + VERBOSE("error: cannot load point-cloud file"); + return EXIT_FAILURE; + } + if (!OPT::strMaskPath.empty()) { + Util::ensureValidFolderPath(OPT::strMaskPath); + for (Image& image : scene.images) { + if (!image.maskName.empty()) { + VERBOSE("error: Image %s has non-empty maskName %s", image.name.c_str(), image.maskName.c_str()); + return EXIT_FAILURE; + } + image.maskName = OPT::strMaskPath + Util::getFileName(image.name) + ".mask.png"; + if (!File::access(image.maskName)) { + VERBOSE("error: Mask image %s not found", image.maskName.c_str()); + return EXIT_FAILURE; + } + } + } + if (!OPT::strImportROIFileName.empty()) { + std::ifstream fs(MAKE_PATH_SAFE(OPT::strImportROIFileName)); + if (!fs) + return EXIT_FAILURE; + fs >> scene.obb; + scene.Save(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))+_T(".mvs"), (ARCHIVE_TYPE)OPT::nArchiveType); + return EXIT_SUCCESS; + } + if (!scene.IsBounded()) + scene.EstimateROI(OPT::nEstimateROI, 1.1f); + if (!OPT::strExportROIFileName.empty() && scene.IsBounded()) { + std::ofstream fs(MAKE_PATH_SAFE(OPT::strExportROIFileName)); + if (!fs) + return EXIT_FAILURE; + fs << scene.obb; + return EXIT_SUCCESS; + } + if (OPT::nTowerMode!=0) + scene.InitTowerScene(OPT::nTowerMode); + if (!OPT::strMeshFileName.empty()) + scene.mesh.Load(MAKE_PATH_SAFE(OPT::strMeshFileName)); + if (!OPT::strViewNeighborsFileName.empty()) + scene.LoadViewNeighbors(MAKE_PATH_SAFE(OPT::strViewNeighborsFileName)); + if (!OPT::strOutputViewNeighborsFileName.empty()) { + if (!scene.ImagesHaveNeighbors()) { + VERBOSE("error: neighbor views not computed yet"); + return EXIT_FAILURE; + } + scene.SaveViewNeighbors(MAKE_PATH_SAFE(OPT::strOutputViewNeighborsFileName)); + return EXIT_SUCCESS; + } + if (!OPT::strExportDepthMapsName.empty() && !scene.mesh.IsEmpty()) { + // project mesh onto each image and save the resulted depth-maps + TD_TIMER_START(); + if (!scene.ExportMeshToDepthMaps(MAKE_PATH_SAFE(OPT::strExportDepthMapsName))) + return EXIT_FAILURE; + VERBOSE("Mesh projection completed: %u depth-maps (%s)", scene.images.size(), TD_TIMER_GET_FMT().c_str()); + return EXIT_SUCCESS; + } + if (OPT::fMaxSubsceneArea > 0) { + // split the scene in sub-scenes by maximum sampling area + Scene::ImagesChunkArr chunks; + scene.Split(chunks, OPT::fMaxSubsceneArea); + scene.ExportChunks(chunks, GET_PATH_FULL(OPT::strOutputFileName), (ARCHIVE_TYPE)OPT::nArchiveType); + return EXIT_SUCCESS; + } + if (OPT::thFilterPointCloud < 0) { + // filter point-cloud based on camera-point visibility intersections + scene.PointCloudFilter(OPT::thFilterPointCloud); + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))+_T("_filtered")); + scene.Save(baseFileName+_T(".mvs"), (ARCHIVE_TYPE)OPT::nArchiveType); + scene.pointcloud.Save(baseFileName+_T(".ply")); + return EXIT_SUCCESS; + } + if (OPT::nExportNumViews && scene.pointcloud.IsValid()) { + // export point-cloud containing only points with N+ views + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))+ + String::FormatString(_T("_%dviews"), ABS(OPT::nExportNumViews))); + if (OPT::nExportNumViews > 0) { + // export point-cloud containing only points with N+ views + scene.pointcloud.SaveNViews(baseFileName+_T(".ply"), (IIndex)OPT::nExportNumViews); + } else { + // save scene and export point-cloud containing only points with N+ views + scene.pointcloud.RemoveMinViews((IIndex)-OPT::nExportNumViews); + scene.Save(baseFileName+_T(".mvs"), (ARCHIVE_TYPE)OPT::nArchiveType); + scene.pointcloud.Save(baseFileName+_T(".ply")); + } + return EXIT_SUCCESS; + } + if (OPT::fEstimateScale > 0 && !scene.pointcloud.IsEmpty() && !scene.images.empty()) { + // simply export existing point-cloud with scale + if (scene.pointcloud.normals.empty()) { + if (!scene.pointcloud.IsValid()) { + VERBOSE("error: can not estimate normals as the point-cloud is not valid"); + return EXIT_FAILURE; + } + EstimatePointNormals(scene.images, scene.pointcloud); + } + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))); + scene.pointcloud.SaveWithScale(baseFileName+_T("_scale.ply"), scene.images, OPT::fEstimateScale); + return EXIT_SUCCESS; + } + PointCloud sparsePointCloud; + if ((ARCHIVE_TYPE)OPT::nArchiveType != ARCHIVE_MVS || sceneType == Scene::SCENE_INTERFACE) { + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 1 && !scene.pointcloud.IsEmpty()) + scene.pointcloud.PrintStatistics(scene.images.data(), &scene.obb); + #endif + if ((ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS) + sparsePointCloud = scene.pointcloud; + TD_TIMER_START(); + if (!scene.DenseReconstruction(OPT::nFusionMode, OPT::bCrop2ROI, OPT::fBorderROI)) { + if (ABS(OPT::nFusionMode) != 1) + return EXIT_FAILURE; + VERBOSE("Depth-maps estimated (%s)", TD_TIMER_GET_FMT().c_str()); + return EXIT_SUCCESS; + } + VERBOSE("Densifying point-cloud completed: %u points (%s)", scene.pointcloud.GetSize(), TD_TIMER_GET_FMT().c_str()); + } + + // save the final point-cloud + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))); + scene.pointcloud.Save(baseFileName+_T(".ply"), (ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS); + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 2) + scene.ExportCamerasMLP(baseFileName+_T(".mlp"), baseFileName+_T(".ply")); + #endif + if ((ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS) + scene.pointcloud.Swap(sparsePointCloud); + scene.Save(baseFileName+_T(".mvs"), (ARCHIVE_TYPE)OPT::nArchiveType); + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfaceCOLMAP/CMakeLists.txt b/apps/InterfaceCOLMAP/CMakeLists.txt new file mode 100644 index 0000000..5d48c79 --- /dev/null +++ b/apps/InterfaceCOLMAP/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(InterfaceCOLMAP "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS InterfaceCOLMAP + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/InterfaceCOLMAP/InterfaceCOLMAP.cpp b/apps/InterfaceCOLMAP/InterfaceCOLMAP.cpp new file mode 100644 index 0000000..2c31092 --- /dev/null +++ b/apps/InterfaceCOLMAP/InterfaceCOLMAP.cpp @@ -0,0 +1,1493 @@ +/* + * InterfaceCOLMAP.cpp + * + * Copyright (c) 2014-2018 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include +#include "endian.h" + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("InterfaceCOLMAP") +#define MVS_FILE_EXTENSION _T(".mvs") +#define COLMAP_IMAGES_FOLDER _T("images/") +#define COLMAP_SPARSE_FOLDER _T("sparse/") +#define COLMAP_CAMERAS_TXT COLMAP_SPARSE_FOLDER _T("cameras.txt") +#define COLMAP_IMAGES_TXT COLMAP_SPARSE_FOLDER _T("images.txt") +#define COLMAP_POINTS_TXT COLMAP_SPARSE_FOLDER _T("points3D.txt") +#define COLMAP_CAMERAS_BIN COLMAP_SPARSE_FOLDER _T("cameras.bin") +#define COLMAP_IMAGES_BIN COLMAP_SPARSE_FOLDER _T("images.bin") +#define COLMAP_POINTS_BIN COLMAP_SPARSE_FOLDER _T("points3D.bin") +#define COLMAP_DENSE_POINTS _T("fused.ply") +#define COLMAP_DENSE_POINTS_VISIBILITY _T("fused.ply.vis") +#define COLMAP_STEREO_FOLDER _T("stereo/") +#define COLMAP_FUSION COLMAP_STEREO_FOLDER _T("fusion.cfg") +#define COLMAP_PATCHMATCH COLMAP_STEREO_FOLDER _T("patch-match.cfg") +#define COLMAP_STEREO_CONSISTENCYGRAPHS_FOLDER COLMAP_STEREO_FOLDER _T("consistency_graphs/") +#define COLMAP_STEREO_DEPTHMAPS_FOLDER COLMAP_STEREO_FOLDER _T("depth_maps/") +#define COLMAP_STEREO_NORMALMAPS_FOLDER COLMAP_STEREO_FOLDER _T("normal_maps/") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +bool IsFromOpenMVS; // conversion direction +bool bNormalizeIntrinsics; +bool bForceSparsePointCloud; +String strInputFileName; +String strPointCloudFileName; +String strOutputFileName; +String strImageFolder; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { CleanUp(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void CleanUp(); +}; // Application + +// Initializes the application and parses command-line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // Open the log file and console for output + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "imports SfM or MVS scene stored in COLMAP undistoreted format OR exports MVS scene to COLMAP format") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description configOptions("Main options"); + configOptions.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input COLMAP folder containing cameras, images and points files OR input MVS project file") + ("pointcloud-file,p", boost::program_options::value(&OPT::strPointCloudFileName), "point-cloud with views file name (overwrite existing point-cloud)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the MVS project") + ("image-folder", boost::program_options::value(&OPT::strImageFolder)->default_value(COLMAP_IMAGES_FOLDER), "folder to the undistorted images") + ("normalize,f", boost::program_options::value(&OPT::bNormalizeIntrinsics)->default_value(false), "normalize intrinsics while exporting to MVS format") + ("force-points,e", boost::program_options::value(&OPT::bForceSparsePointCloud)->default_value(false), "force exporting point-cloud as sparse points also even if dense point-cloud detected") + ; + + // Combine all command-line options + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(configOptions); + + // Only config file options + boost::program_options::options_description config_file_options; + config_file_options.add(configOptions); + + boost::program_options::positional_options_description positionalOptions; + positionalOptions.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(positionalOptions).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + + // Initialize working folder + INIT_WORKING_FOLDER; + + // Parse configuration file if exists + std::ifstream configFile(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (configFile) { + boost::program_options::store(parse_config_file(configFile, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // Open and configure the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log"))); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // Check and validate paths + Util::ensureValidPath(OPT::strInputFileName); + Util::ensureValidPath(OPT::strPointCloudFileName); + + // Handle help option or invalid command + const bool bInvalidCommand(OPT::strInputFileName.empty()); + if (OPT::vm.count("help") || bInvalidCommand) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(configOptions); + GET_LOG() << _T("\n" + "Import/export 3D reconstruction from COLMAP (TXT/BIN format) and to COLMAP (TXT format). \n" + "In order to import a scene, run COLMAP SfM and next undistort the images (only PINHOLE\n" + "camera model supported for the moment)." + "\n") + << visible; + } + + if (bInvalidCommand) + return false; + + // initialize optional options + Util::ensureValidFolderPath(OPT::strImageFolder); + Util::ensureValidPath(OPT::strOutputFileName); + OPT::strImageFolder = MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strImageFolder); + + // Determine if the source file is an OpenMVS project by checking the file extension + const String inputFileExtension(Util::getFileExt(OPT::strInputFileName).ToLower()); + OPT::IsFromOpenMVS = (inputFileExtension == MVS_FILE_EXTENSION); + + // Set default output filename based on input filename if not explicitly specified + if (OPT::IsFromOpenMVS) { + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFilePath(OPT::strInputFileName); + } else { + // Ensure the input filename ends with a directory slash if needed + Util::ensureFolderSlash(OPT::strInputFileName); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = OPT::strInputFileName + _T("scene") MVS_FILE_EXTENSION; + } + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::CleanUp() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +namespace COLMAP { + +using namespace colmap; + +// See colmap/src/util/types.h +typedef uint32_t camera_t; +typedef uint32_t image_t; +typedef uint64_t image_pair_t; +typedef uint32_t point2D_t; +typedef uint64_t point3D_t; + +const std::vector mapCameraModel = { + "SIMPLE_PINHOLE", + "PINHOLE", + "SIMPLE_RADIAL", + "RADIAL", + "OPENCV", + "OPENCV_FISHEYE", + "FULL_OPENCV", + "FOV", + "SIMPLE_RADIAL_FISHEYE", + "RADIAL_FISHEYE", + "THIN_PRISM_FISHEYE" +}; + +// tools +bool NextLine(std::istream& stream, std::istringstream& in, bool bIgnoreEmpty=true) { + String line; + do { + std::getline(stream, line); + Util::strTrim(line, _T(" ")); + if (stream.fail()) + return false; + } while (((bIgnoreEmpty && line.empty()) || line[0u] == '#') && stream.good()); + in.clear(); + in.str(line); + return true; +} +// structure describing a camera +struct Camera { + uint32_t ID; // ID of the camera + String model; // camera model name + uint32_t width, height; // camera resolution + std::vector params; // camera parameters + uint64_t numCameras{0}; // only for binary format + + Camera() {} + Camera(uint32_t _ID) : ID(_ID) {} + bool operator < (const Camera& rhs) const { return ID < rhs.ID; } + + struct CameraHash { + size_t operator()(const Camera& camera) const { + const size_t h1(std::hash()(camera.model)); + const size_t h2(std::hash()(camera.width)); + const size_t h3(std::hash()(camera.height)); + size_t h(h1 ^ ((h2 ^ (h3 << 1)) << 1)); + for (REAL p: camera.params) + h = std::hash()(p) ^ (h << 1); + return h; + } + }; + struct CameraEqualTo { + bool operator()(const Camera& _Left, const Camera& _Right) const { + return _Left.model == _Right.model && + _Left.width == _Right.width && _Left.height == _Right.height && + _Left.params == _Right.params; + } + }; + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // Camera list with one line of data per camera: + // CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[] + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + in >> ID >> model >> width >> height; + if (in.fail()) + return false; + --ID; + if (model != _T("PINHOLE")) + return false; + params.resize(4); + in >> params[0] >> params[1] >> params[2] >> params[3]; + return !in.fail(); + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadCamerasBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (numCameras == 0) { + // Read the first entry in the binary file + numCameras = ReadBinaryLittleEndian(&stream); + } + + ID = ReadBinaryLittleEndian(&stream)-1; + model = mapCameraModel[ReadBinaryLittleEndian(&stream)]; + width = (uint32_t)ReadBinaryLittleEndian(&stream); + height = (uint32_t)ReadBinaryLittleEndian(&stream); + if (model != _T("PINHOLE")) + return false; + params.resize(4); + ReadBinaryLittleEndian(&stream, ¶ms); + return true; + } + + bool WriteTXT(std::ostream& out) const { + out << ID+1 << _T(" ") << model << _T(" ") << width << _T(" ") << height; + if (out.fail()) + return false; + for (REAL param: params) { + out << _T(" ") << param; + if (out.fail()) + return false; + } + out << std::endl; + return true; + } + + bool WriteBIN(std::ostream& stream) { + if (numCameras != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numCameras); + numCameras = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + const int64 modelId(std::distance(mapCameraModel.begin(), std::find(mapCameraModel.begin(), mapCameraModel.end(), model))); + WriteBinaryLittleEndian(&stream, (int)modelId); + WriteBinaryLittleEndian(&stream, width); + WriteBinaryLittleEndian(&stream, height); + for (REAL param: params) + WriteBinaryLittleEndian(&stream, param); + return !stream.fail(); + } +}; +typedef std::vector Cameras; +// structure describing an image +struct Image { + struct Proj { + Eigen::Vector2f p; + uint32_t idPoint; + }; + uint32_t ID; // ID of the image + Eigen::Quaterniond q; // rotation + Eigen::Vector3d t; // translation + uint32_t idCamera; // ID of the associated camera + String name; // image file name + std::vector projs; // known image projections + uint64_t numRegImages{0}; // only for binary format + + Image() {} + Image(uint32_t _ID) : ID(_ID) {} + bool operator < (const Image& rhs) const { return ID < rhs.ID; } + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // Image list with two lines of data per image: + // IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME + // POINTS2D[] as (X, Y, POINT3D_ID) + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + in >> ID + >> q.w() >> q.x() >> q.y() >> q.z() + >> t(0) >> t(1) >> t(2) + >> idCamera >> name; + if (in.fail()) + return false; + --ID; --idCamera; + Util::ensureValidPath(name); + if (!NextLine(stream, in, false)) + return false; + projs.clear(); + while (true) { + Proj proj; + in >> proj.p(0) >> proj.p(1) >> (int&)proj.idPoint; + if (in.fail()) + break; + --proj.idPoint; + projs.emplace_back(proj); + } + return true; + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadImagesBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (!numRegImages) { + // Read the first entry in the binary file + numRegImages = ReadBinaryLittleEndian(&stream); + } + + ID = ReadBinaryLittleEndian(&stream)-1; + q.w() = ReadBinaryLittleEndian(&stream); + q.x() = ReadBinaryLittleEndian(&stream); + q.y() = ReadBinaryLittleEndian(&stream); + q.z() = ReadBinaryLittleEndian(&stream); + t(0) = ReadBinaryLittleEndian(&stream); + t(1) = ReadBinaryLittleEndian(&stream); + t(2) = ReadBinaryLittleEndian(&stream); + idCamera = ReadBinaryLittleEndian(&stream)-1; + + name = ""; + while (true) { + char nameChar; + stream.read(&nameChar, 1); + if (nameChar == '\0') + break; + name += nameChar; + } + Util::ensureValidPath(name); + + const size_t numPoints2D = ReadBinaryLittleEndian(&stream); + projs.clear(); + for (size_t j = 0; j < numPoints2D; ++j) { + Proj proj; + proj.p(0) = (float)ReadBinaryLittleEndian(&stream); + proj.p(1) = (float)ReadBinaryLittleEndian(&stream); + proj.idPoint = (uint32_t)ReadBinaryLittleEndian(&stream)-1; + projs.emplace_back(proj); + } + return true; + } + + bool WriteTXT(std::ostream& out) const { + out << ID+1 << _T(" ") + << q.w() << _T(" ") << q.x() << _T(" ") << q.y() << _T(" ") << q.z() << _T(" ") + << t(0) << _T(" ") << t(1) << _T(" ") << t(2) << _T(" ") + << idCamera+1 << _T(" ") << name + << std::endl; + for (const Proj& proj: projs) { + out << proj.p(0) << _T(" ") << proj.p(1) << _T(" ") << (int)proj.idPoint+1 << _T(" "); + if (out.fail()) + return false; + } + out << std::endl; + return !out.fail(); + } + + bool WriteBIN(std::ostream& stream) { + if (numRegImages != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numRegImages); + numRegImages = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + + WriteBinaryLittleEndian(&stream, q.w()); + WriteBinaryLittleEndian(&stream, q.x()); + WriteBinaryLittleEndian(&stream, q.y()); + WriteBinaryLittleEndian(&stream, q.z()); + + WriteBinaryLittleEndian(&stream, t(0)); + WriteBinaryLittleEndian(&stream, t(1)); + WriteBinaryLittleEndian(&stream, t(2)); + + WriteBinaryLittleEndian(&stream, idCamera+1); + + stream.write(name.c_str(), name.size()+1); + + WriteBinaryLittleEndian(&stream, projs.size()); + for (const Proj& proj: projs) { + WriteBinaryLittleEndian(&stream, proj.p(0)); + WriteBinaryLittleEndian(&stream, proj.p(1)); + WriteBinaryLittleEndian(&stream, proj.idPoint+1); + } + return !stream.fail(); + } +}; +typedef std::vector Images; +// structure describing a 3D point +struct Point { + struct Track { + uint32_t idImage; + uint32_t idProj; + }; + uint32_t ID; // ID of the point + Interface::Pos3f p; // position + Interface::Col3 c; // BGR color + float e; // error + std::vector tracks; // point track + uint64_t numPoints3D{0}; // only for binary format + + Point() {} + Point(uint32_t _ID) : ID(_ID) {} + bool operator < (const Image& rhs) const { return ID < rhs.ID; } + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // 3D point list with one line of data per point: + // POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX) + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + int r,g,b; + in >> ID + >> p.x >> p.y >> p.z + >> r >> g >> b + >> e; + c.x = CLAMP(b,0,255); + c.y = CLAMP(g,0,255); + c.z = CLAMP(r,0,255); + if (in.fail()) + return false; + --ID; + tracks.clear(); + while (true) { + Track track; + in >> track.idImage >> track.idProj; + if (in.fail()) + break; + --track.idImage; --track.idProj; + tracks.emplace_back(track); + } + return !tracks.empty(); + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadPoints3DBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (!numPoints3D) { + // Read the first entry in the binary file + numPoints3D = ReadBinaryLittleEndian(&stream); + } + + int r,g,b; + ID = (uint32_t)ReadBinaryLittleEndian(&stream)-1; + p.x = (float)ReadBinaryLittleEndian(&stream); + p.y = (float)ReadBinaryLittleEndian(&stream); + p.z = (float)ReadBinaryLittleEndian(&stream); + r = ReadBinaryLittleEndian(&stream); + g = ReadBinaryLittleEndian(&stream); + b = ReadBinaryLittleEndian(&stream); + e = (float)ReadBinaryLittleEndian(&stream); + c.x = CLAMP(b,0,255); + c.y = CLAMP(g,0,255); + c.z = CLAMP(r,0,255); + + const size_t trackLength = ReadBinaryLittleEndian(&stream); + tracks.clear(); + for (size_t j = 0; j < trackLength; ++j) { + Track track; + track.idImage = ReadBinaryLittleEndian(&stream)-1; + track.idProj = ReadBinaryLittleEndian(&stream)-1; + tracks.emplace_back(track); + } + return !tracks.empty(); + } + + bool WriteTXT(std::ostream& out) const { + ASSERT(!tracks.empty()); + const int r(c.z),g(c.y),b(c.x); + out << ID+1 << _T(" ") + << p.x << _T(" ") << p.y << _T(" ") << p.z << _T(" ") + << r << _T(" ") << g << _T(" ") << b << _T(" ") + << e << _T(" "); + for (const Track& track: tracks) { + out << track.idImage+1 << _T(" ") << track.idProj+1 << _T(" "); + if (out.fail()) + return false; + } + out << std::endl; + return !out.fail(); + } + + bool WriteBIN(std::ostream& stream) { + ASSERT(!tracks.empty()); + if (numPoints3D != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numPoints3D); + numPoints3D = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + WriteBinaryLittleEndian(&stream, p.x); + WriteBinaryLittleEndian(&stream, p.y); + WriteBinaryLittleEndian(&stream, p.z); + WriteBinaryLittleEndian(&stream, c.z); + WriteBinaryLittleEndian(&stream, c.y); + WriteBinaryLittleEndian(&stream, c.x); + WriteBinaryLittleEndian(&stream, e); + + WriteBinaryLittleEndian(&stream, tracks.size()); + for (const Track& track: tracks) { + WriteBinaryLittleEndian(&stream, track.idImage+1); + WriteBinaryLittleEndian(&stream, track.idProj+1); + } + return !stream.fail(); + } +}; +typedef std::vector Points; +// structure describing an 2D dynamic matrix +template +struct Mat { + size_t width_ = 0; + size_t height_ = 0; + size_t depth_ = 0; + std::vector data_; + + size_t GetNumBytes() const { + return data_.size() * sizeof(T); + } + const T* GetChannelPtr(size_t c) const { + return data_.data()+width_*height_*c; + } + + // See: colmap/src/mvs/mat.h + void Read(const std::string& path) { + std::streampos pos; { + std::fstream text_file(path, std::ios::in | std::ios::binary); + char unused_char; + text_file >> width_ >> unused_char >> height_ >> unused_char >> depth_ >> + unused_char; + pos = text_file.tellg(); + } + data_.resize(width_ * height_ * depth_); + std::fstream binary_file(path, std::ios::in | std::ios::binary); + binary_file.seekg(pos); + ReadBinaryLittleEndian(&binary_file, &data_); + } + void Write(const std::string& path) const { + { + std::fstream text_file(path, std::ios::out); + text_file << width_ << "&" << height_ << "&" << depth_ << "&"; + } + std::fstream binary_file(path, std::ios::out | std::ios::binary | std::ios::app); + WriteBinaryLittleEndian(&binary_file, data_); + } +}; +} // namespace COLMAP + +typedef Eigen::Matrix EMat33d; +typedef Eigen::Matrix EVec3d; + +/** + * Determines whether to use the TXT or BIN version of a file based on availability. + * It tries to open the TXT file first; if unsuccessful, it attempts to open the BIN file. + * + * @param filenameTXT The path to the TXT file. + * @param filenameBIN The path to the BIN file. + * @param fileStream Reference to the ifstream object that will be used for file operations. + * @param chosenFilename Reference to a string that will store the path of the chosen file. + * @param isBinaryFormat Reference to a boolean that indicates whether the chosen file is binary. + * @return true if either file was successfully opened, false otherwise. + */ +bool DetermineInputSource(const String& filenameTXT, const String& filenameBIN, std::ifstream& fileStream, String& filenameCamera, bool& isBinaryFormat) +{ + // Try to open the TXT file first. + fileStream.open(filenameTXT); + if (fileStream.good()) { + filenameCamera = filenameTXT; + isBinaryFormat = false; + return true; + } + fileStream.open(filenameBIN, std::ios::binary); + if (fileStream.good()) { + filenameCamera = filenameBIN; + isBinaryFormat = true; + return true; + } + VERBOSE("error: unable to open file '%s'", filenameTXT.c_str()); + VERBOSE("error: unable to open file '%s'", filenameBIN.c_str()); + return false; +} + + +bool ImportScene(const String& inputFolder, const String& outputFolder, Interface& scene, PointCloud& pointcloud) +{ + // Define a map to store camera IDs and associated platform index. + typedef std::unordered_map CamerasMap; + CamerasMap mapCameras; + { + // Determine the camera file source between TXT and BIN formats. + const String filenameCamerasTXT(inputFolder+COLMAP_CAMERAS_TXT); + const String filenameCamerasBIN(inputFolder+COLMAP_CAMERAS_BIN); + std::ifstream cameraFile; + bool isBinaryFormat; + String filenameCamera; + if (!DetermineInputSource(filenameCamerasTXT, filenameCamerasBIN, cameraFile, filenameCamera, isBinaryFormat)) { + return false; // Exit if file source determination fails. + } + LOG_OUT() << "Reading cameras: " << filenameCamera << std::endl; + + typedef std::unordered_map CamerasSet; + CamerasSet setCameras; + COLMAP::Camera colmapCamera; + while (cameraFile.good() && colmapCamera.Read(cameraFile, isBinaryFormat)) { + const auto setIt(setCameras.emplace(colmapCamera, (uint32_t)scene.platforms.size())); + mapCameras.emplace(colmapCamera.ID, setIt.first->second); + if (!setIt.second) { + // reuse existing platform + continue; + } + // create new platform + Interface::Platform platform; + platform.name = String::FormatString(_T("platform%03u"), colmapCamera.ID); // only one camera per platform supported + Interface::Platform::Camera camera; + camera.name = colmapCamera.model; + camera.K = Interface::Mat33d::eye(); + // account for different pixel center conventions as COLMAP uses pixel center at (0.5,0.5) + camera.K(0,0) = colmapCamera.params[0]; + camera.K(1,1) = colmapCamera.params[1]; + camera.K(0,2) = colmapCamera.params[2]-REAL(0.5); + camera.K(1,2) = colmapCamera.params[3]-REAL(0.5); + camera.R = Interface::Mat33d::eye(); + camera.C = Interface::Pos3d(0,0,0); + if (OPT::bNormalizeIntrinsics) { + // normalize camera intrinsics + camera.K = Camera::ScaleK(camera.K, 1.0/Camera::GetNormalizationScale(colmapCamera.width, colmapCamera.height)); + } else { + camera.width = colmapCamera.width; + camera.height = colmapCamera.height; + } + platform.cameras.emplace_back(camera); + scene.platforms.emplace_back(platform); + } + } + if (mapCameras.empty()) { + VERBOSE("error: no valid cameras (make sure they are in PINHOLE model)"); + return false; + } + + // Function to read and process image data from input files + typedef std::map ImagesMap; + ImagesMap mapImages; + { + // Define a map to associate COLMAP image structures with scene image indices + const String filenameImagesTXT(inputFolder+COLMAP_IMAGES_TXT); + const String filenameImagesBIN(inputFolder+COLMAP_IMAGES_BIN); + std::ifstream file; + bool binary; + String filenameImages; + if (!DetermineInputSource(filenameImagesTXT, filenameImagesBIN, file, filenameImages, binary)) { + return false; + } + LOG_OUT() << "Reading images: " << filenameImages << std::endl; + + // Read image data from the file and store in the scene structure + COLMAP::Image imageColmap; + while (file.good() && imageColmap.Read(file, binary)) { + mapImages.emplace(imageColmap, (uint32_t)scene.images.size()); + Interface::Platform::Pose pose; + Eigen::Map(pose.R.val) = imageColmap.q.toRotationMatrix(); + EnsureRotationMatrix((Matrix3x3d&)pose.R); + Eigen::Map(&pose.C.x) = -(imageColmap.q.inverse() * imageColmap.t); + + Interface::Image image; + image.name = MAKE_PATH_REL(outputFolder,OPT::strImageFolder+imageColmap.name); + image.platformID = mapCameras.at(imageColmap.idCamera); + image.cameraID = 0; + image.ID = imageColmap.ID; + + Interface::Platform& platform = scene.platforms[image.platformID]; + image.poseID = (uint32_t)platform.poses.size(); + platform.poses.emplace_back(pose); + scene.images.emplace_back(image); + } + } + + // read points list + const String filenameDensePoints(inputFolder+COLMAP_DENSE_POINTS); + const String filenameDenseVisPoints(inputFolder+COLMAP_DENSE_POINTS_VISIBILITY); + { + // parse sparse point-cloud + const String filenamePointsTXT(inputFolder+COLMAP_POINTS_TXT); + const String filenamePointsBIN(inputFolder+COLMAP_POINTS_BIN); + std::ifstream file; + bool binary; + String filenamePoints; + if (!DetermineInputSource(filenamePointsTXT, filenamePointsBIN, file, filenamePoints, binary)) { + return false; + } + LOG_OUT() << "Reading points: " << filenamePoints << std::endl; + COLMAP::Point point; + while (file.good() && point.Read(file, binary)) { + Interface::Vertex vertex; + vertex.X = point.p; + for (const COLMAP::Point::Track& track: point.tracks) { + Interface::Vertex::View view; + view.imageID = mapImages.at(COLMAP::Image(track.idImage)); + view.confidence = 0; + vertex.views.emplace_back(view); + } + std::sort(vertex.views.begin(), vertex.views.end(), + [](const Interface::Vertex::View& view0, const Interface::Vertex::View& view1) { return view0.imageID < view1.imageID; }); + scene.vertices.emplace_back(std::move(vertex)); + scene.verticesColor.emplace_back(Interface::Color{point.c}); + } + } + pointcloud.Release(); + if (File::access(filenameDensePoints) && File::access(filenameDenseVisPoints)) { + // parse dense point-cloud + LOG_OUT() << "Reading points: " << filenameDensePoints << " and " << filenameDenseVisPoints << std::endl; + if (!pointcloud.Load(filenameDensePoints)) { + VERBOSE("error: unable to open file '%s'", filenameDensePoints.c_str()); + return false; + } + File file(filenameDenseVisPoints, File::READ, File::OPEN); + if (!file.isOpen()) { + VERBOSE("error: unable to open file '%s'", filenameDenseVisPoints.c_str()); + return false; + } + uint64_t numPoints(0); + file.read(&numPoints, sizeof(uint64_t)); + if (pointcloud.GetSize() != numPoints) { + VERBOSE("error: point-cloud and visibility have different size"); + return false; + } + pointcloud.pointViews.resize(numPoints); + for (size_t i=0; isecond]; + CLISTDEF2(String) neighborNames; + Util::strSplit(neighbors, _T(','), neighborNames); + FOREACH(i, neighborNames) { + String& neighborName = neighborNames[i]; + Util::strTrim(neighborName, _T(" ")); + const ImagesMap::const_iterator it_neighbor = std::find_if(mapImages.begin(), mapImages.end(), + [&neighborName](const ImagesMap::value_type& image) { + return image.first.name == neighborName; + }); + if (it_neighbor == mapImages.end()) { + if (i == 0) + break; + continue; + } + imageNeighbors.emplace_back(scene.images[it_neighbor->second].ID); + } + } + } + LOG_OUT() << "Reading depth-maps/normal-maps: " << pathDepthMaps << " and " << pathNormalMaps << std::endl; + Util::ensureFolder(outputFolder); + const String strType[] = {".geometric.bin", ".photometric.bin"}; + FOREACH(idx, scene.images) { + const Interface::Image& image = scene.images[idx]; + COLMAP::Mat colDepthMap, colNormalMap; + const String filenameImage(Util::getFileNameExt(image.name)); + for (int i=0; i<2; ++i) { + const String filenameDepthMaps(pathDepthMaps+filenameImage+strType[i]); + if (File::isFile(filenameDepthMaps)) { + colDepthMap.Read(filenameDepthMaps); + const String filenameNormalMaps(pathNormalMaps+filenameImage+strType[i]); + if (File::isFile(filenameNormalMaps)) { + colNormalMap.Read(filenameNormalMaps); + } + break; + } + } + if (!colDepthMap.data_.empty()) { + IIndexArr IDs = {image.ID}; + IDs.Join(imagesNeighbors[(IIndex)idx]); + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Pose pose(platform.GetPose(image.cameraID, image.poseID)); + const Interface::Mat33d K(platform.GetFullK(image.cameraID, (uint32_t)colDepthMap.width_, (uint32_t)colDepthMap.height_)); + MVS::DepthMap depthMap((int)colDepthMap.height_, (int)colDepthMap.width_); + memcpy(depthMap.getData(), colDepthMap.data_.data(), colDepthMap.GetNumBytes()); + MVS::NormalMap normalMap; + if (!colNormalMap.data_.empty()) { + normalMap.create((int)colNormalMap.height_, (int)colNormalMap.width_); + cv::merge(std::vector{ + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(0)), + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(1)), + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(2)) + }, normalMap); + } + MVS::ConfidenceMap confMap; + MVS::ViewsMap viewsMap; + const auto depthMM(std::minmax_element(colDepthMap.data_.cbegin(), colDepthMap.data_.cend())); + const MVS::Depth dMin(*depthMM.first), dMax(*depthMM.second); + if (!ExportDepthDataRaw(outputFolder+String::FormatString("depth%04u.dmap", image.ID), MAKE_PATH_FULL(outputFolder, image.name), IDs, depthMap.size(), K, pose.R, pose.C, dMin, dMax, depthMap, normalMap, confMap, viewsMap)) + return false; + } + } + } + return true; +} + + +bool ImportPointCloud(const String& strPointCloudFileName, Interface& scene) +{ + PointCloud pointcloud; + if (!pointcloud.Load(strPointCloudFileName)) { + VERBOSE("error: cannot load point-cloud file"); + return false; + } + if (!pointcloud.IsValid()) { + VERBOSE("error: loaded point-cloud does not have visibility information"); + return false; + } + // replace scene point-cloud with the loaded one + scene.vertices.clear(); + scene.verticesColor.clear(); + scene.verticesNormal.clear(); + scene.vertices.reserve(pointcloud.points.size()); + if (!pointcloud.colors.empty()) + scene.verticesColor.reserve(pointcloud.points.size()); + if (!pointcloud.normals.empty()) + scene.verticesNormal.reserve(pointcloud.points.size()); + FOREACH(i, pointcloud.points) { + Interface::Vertex vertex; + vertex.X = pointcloud.points[i]; + vertex.views.reserve(pointcloud.pointViews[i].size()); + FOREACH(j, pointcloud.pointViews[i]) { + Interface::Vertex::View& view = vertex.views.emplace_back(); + view.imageID = pointcloud.pointViews[i][j]; + view.confidence = (pointcloud.pointWeights.empty() ? 0.f : pointcloud.pointWeights[i][j]); + } + scene.vertices.emplace_back(std::move(vertex)); + if (!pointcloud.colors.empty()) { + const Pixel8U& c = pointcloud.colors[i]; + scene.verticesColor.emplace_back(Interface::Color{Interface::Col3{c.b, c.g, c.r}}); + } + if (!pointcloud.normals.empty()) + scene.verticesNormal.emplace_back(Interface::Normal{pointcloud.normals[i]}); + } + return true; +} + +bool ExportScene(const String& strFolder, const Interface& scene, bool bForceSparsePointCloud = false, bool binary = true) +{ + Util::ensureFolder(strFolder+COLMAP_SPARSE_FOLDER); + + // write camera list + CLISTDEF0IDX(KMatrix,uint32_t) Ks; + CLISTDEF0IDX(COLMAP::Camera,uint32_t) cams; + { + // Construct the filename for camera data based on the format preference + const String filenameCameras(strFolder+(binary?COLMAP_CAMERAS_BIN:COLMAP_CAMERAS_TXT)); + LOG_OUT() << "Writing cameras: " << filenameCameras << std::endl; + std::ofstream file(filenameCameras, binary ? std::ios::trunc|std::ios::binary : std::ios::trunc); + + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenameCameras.c_str()); + return false; + } + + COLMAP::Camera cam; + if (binary) { + cam.numCameras = 0; + for (const Interface::Platform& platform: scene.platforms) + cam.numCameras += (uint32_t)platform.cameras.size(); + } else { + file << _T("# Camera list with one line of data per camera:") << std::endl; + file << _T("# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]") << std::endl; + } + cam.model = _T("PINHOLE"); + cam.params.resize(4); + for (uint32_t ID=0; ID<(uint32_t)scene.platforms.size(); ++ID) { + const Interface::Platform& platform = scene.platforms[ID]; + ASSERT(platform.cameras.size() == 1); // only one camera per platform supported + const Interface::Platform::Camera& camera = platform.cameras[0]; + cam.ID = ID; + KMatrix K; + if (camera.width == 0 || camera.height == 0) { + // find one image using this camera + const Interface::Image* pImage(NULL); + for (uint32_t i=0; i<(uint32_t)scene.images.size(); ++i) { + const Interface::Image& image = scene.images[i]; + if (image.platformID == ID && image.cameraID == 0 && image.poseID != NO_ID) { + pImage = ℑ + break; + } + } + if (pImage == NULL) { + LOG("error: no image using camera %u of platform %u", 0, ID); + continue; + } + IMAGEPTR ptrImage(Image::ReadImageHeader(MAKE_PATH_SAFE(pImage->name))); + if (ptrImage == NULL) + return false; + cam.width = ptrImage->GetWidth(); + cam.height = ptrImage->GetHeight(); + // unnormalize camera intrinsics + K = platform.GetFullK(0, cam.width, cam.height); + } else { + cam.width = camera.width; + cam.height = camera.height; + K = camera.K; + } + // account for different pixel center conventions as COLMAP uses pixel center at (0.5,0.5) + cam.params[0] = K(0,0); + cam.params[1] = K(1,1); + cam.params[2] = K(0,2)+REAL(0.5); + cam.params[3] = K(1,2)+REAL(0.5); + if (!cam.Write(file, binary)) + return false; + Ks.emplace_back(K); + cams.emplace_back(cam); + } + } + + // create images list + COLMAP::Images images; + CameraArr cameras; + float maxNumPointsSparse(0); + const float avgViewsPerPoint(3.f); + const uint32_t avgResolutionSmallView(640*480), avgResolutionLargeView(6000*4000); + const uint32_t avgPointsPerSmallView(3000), avgPointsPerLargeView(12000); + { + images.resize(scene.images.size()); + cameras.resize((unsigned)scene.images.size()); + for (uint32_t ID=0; ID<(uint32_t)scene.images.size(); ++ID) { + const Interface::Image& image = scene.images[ID]; + if (image.poseID == NO_ID) + continue; + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Pose& pose = platform.poses[image.poseID]; + ASSERT(image.cameraID == 0); + COLMAP::Image& img = images[ID]; + img.ID = ID; + img.q = Eigen::Quaterniond(Eigen::Map(pose.R.val)); + img.t = -(img.q * Eigen::Map(&pose.C.x)); + img.idCamera = image.platformID; + img.name = MAKE_PATH_REL(OPT::strImageFolder, MAKE_PATH_FULL(WORKING_FOLDER_FULL, image.name)); + Camera& camera = cameras[ID]; + camera.K = Ks[image.platformID]; + camera.R = pose.R; + camera.C = pose.C; + camera.ComposeP(); + const COLMAP::Camera& cam = cams[image.platformID]; + const uint32_t resolutionView(cam.width*cam.height); + const float linearFactor(float(avgResolutionLargeView-resolutionView)/(avgResolutionLargeView-avgResolutionSmallView)); + maxNumPointsSparse += (avgPointsPerSmallView+(avgPointsPerLargeView-avgPointsPerSmallView)*linearFactor)/avgViewsPerPoint; + } + } + + // auto-select dense or sparse mode based on number of points + const bool bSparsePointCloud(scene.vertices.size() < (size_t)maxNumPointsSparse); + if (bSparsePointCloud || bForceSparsePointCloud) { + // write points list + { + const String filenamePoints(strFolder+(binary?COLMAP_POINTS_BIN:COLMAP_POINTS_TXT)); + LOG_OUT() << "Writing points: " << filenamePoints << std::endl; + std::ofstream file(filenamePoints, binary ? std::ios::trunc|std::ios::binary : std::ios::trunc); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenamePoints.c_str()); + return false; + } + uint64_t numPoints3D = 0; + if (binary) { + numPoints3D = scene.vertices.size(); + } else { + file << _T("# 3D point list with one line of data per point:") << std::endl; + file << _T("# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)") << std::endl; + } + for (uint32_t ID=0; ID<(uint32_t)scene.vertices.size(); ++ID) { + const Interface::Vertex& vertex = scene.vertices[ID]; + COLMAP::Point point; + point.ID = ID; + point.p = vertex.X; + for (const Interface::Vertex::View& view: vertex.views) { + COLMAP::Image& img = images[view.imageID]; + point.tracks.emplace_back(COLMAP::Point::Track{view.imageID, (uint32_t)img.projs.size()}); + COLMAP::Image::Proj proj; + proj.idPoint = ID; + const Point3 X(vertex.X); + ProjectVertex_3x4_3_2(cameras[view.imageID].P.val, X.ptr(), proj.p.data()); + // account for different pixel center conventions as COLMAP uses pixel center at (0.5,0.5) + proj.p[0] += REAL(0.5); + proj.p[1] += REAL(0.5); + img.projs.emplace_back(proj); + } + point.c = scene.verticesColor.empty() ? Interface::Col3(255,255,255) : scene.verticesColor[ID].c; + point.e = 0; + if (numPoints3D != 0) { + point.numPoints3D = numPoints3D; + numPoints3D = 0; + } + if (!point.Write(file, binary)) + return false; + } + } + + Util::ensureFolder(strFolder+COLMAP_STEREO_FOLDER); + + // write fusion list + { + const String filenameFusion(strFolder+COLMAP_FUSION); + LOG_OUT() << "Writing fusion configuration: " << filenameFusion << std::endl; + std::ofstream file(filenameFusion); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenameFusion.c_str()); + return false; + } + for (const COLMAP::Image& img: images) { + if (img.projs.empty()) + continue; + file << img.name << std::endl; + if (file.fail()) + return false; + } + } + + // write patch-match list + { + const String filenameFusion(strFolder+COLMAP_PATCHMATCH); + LOG_OUT() << "Writing patch-match configuration: " << filenameFusion << std::endl; + std::ofstream file(filenameFusion); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenameFusion.c_str()); + return false; + } + for (const COLMAP::Image& img: images) { + if (img.projs.empty()) + continue; + file << img.name << std::endl; + if (file.fail()) + return false; + file << _T("__auto__, 20") << std::endl; + if (file.fail()) + return false; + } + } + + Util::ensureFolder(strFolder+COLMAP_STEREO_CONSISTENCYGRAPHS_FOLDER); + Util::ensureFolder(strFolder+COLMAP_STEREO_DEPTHMAPS_FOLDER); + Util::ensureFolder(strFolder+COLMAP_STEREO_NORMALMAPS_FOLDER); + } + if (!bSparsePointCloud) { + // export dense point-cloud + const String filenameDensePoints(strFolder+COLMAP_DENSE_POINTS); + const String filenameDenseVisPoints(strFolder+COLMAP_DENSE_POINTS_VISIBILITY); + LOG_OUT() << "Writing points: " << filenameDensePoints << " and " << filenameDenseVisPoints << std::endl; + File file(filenameDenseVisPoints, File::WRITE, File::CREATE | File::TRUNCATE); + if (!file.isOpen()) { + VERBOSE("error: unable to write file '%s'", filenameDenseVisPoints.c_str()); + return false; + } + const uint64_t numPoints(scene.vertices.size()); + file.write(&numPoints, sizeof(uint64_t)); + PointCloud pointcloud; + for (size_t i=0; iGetWidth(), pImage->GetHeight()); + Eigen::Matrix4d K4(Eigen::Matrix4d::Identity()); + K4.topLeftCorner<3,3>() = Eigen::Map(K.val); + Util::ensureFolder(fileName); + std::ofstream out(fileName); + if (!out.good()) { + VERBOSE("error: unable to open file '%s'", fileName.c_str()); + return false; + } + out << std::setprecision(12); + out << K4(0,0) << _T(" ") << K4(0,1) << _T(" ") << K4(0,2) << _T(" ") << K4(0,3) << _T("\n"); + out << K4(1,0) << _T(" ") << K4(1,1) << _T(" ") << K4(1,2) << _T(" ") << K4(1,3) << _T("\n"); + out << K4(2,0) << _T(" ") << K4(2,1) << _T(" ") << K4(2,2) << _T(" ") << K4(2,3) << _T("\n"); + out << K4(3,0) << _T(" ") << K4(3,1) << _T(" ") << K4(3,2) << _T(" ") << K4(3,3) << _T("\n"); + return !out.fail(); +} + + +// export image poses in log format +// see: http://redwood-data.org/indoor/fileformat.html +// to support: https://www.tanksandtemples.org/tutorial +bool ExportImagesLog(const String& fileName, const Interface& scene) +{ + LOG_OUT() << "Writing poses: " << fileName << std::endl; + Util::ensureFolder(fileName); + std::ofstream out(fileName); + if (!out.good()) { + VERBOSE("error: unable to open file '%s'", fileName.c_str()); + return false; + } + out << std::setprecision(12); + IIndexArr orderedImages((uint32_t)scene.images.size()); + std::iota(orderedImages.begin(), orderedImages.end(), 0u); + orderedImages.Sort([&scene](IIndex i, IIndex j) { + return scene.images[i].ID < scene.images[j].ID; + }); + for (IIndex ID: orderedImages) { + const Interface::Image& image = scene.images[ID]; + Eigen::Matrix3d R(Eigen::Matrix3d::Identity()); + Eigen::Vector3d t(Eigen::Vector3d::Zero()); + if (image.poseID != NO_ID) { + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Pose& pose = platform.poses[image.poseID]; + R = Eigen::Map(pose.R.val).transpose(); + t = Eigen::Map(&pose.C.x); + } + Eigen::Matrix4d T(Eigen::Matrix4d::Identity()); + T.topLeftCorner<3,3>() = R; + T.topRightCorner<3,1>() = t; + out << ID << _T(" ") << ID << _T(" ") << 0 << _T("\n"); + out << T(0,0) << _T(" ") << T(0,1) << _T(" ") << T(0,2) << _T(" ") << T(0,3) << _T("\n"); + out << T(1,0) << _T(" ") << T(1,1) << _T(" ") << T(1,2) << _T(" ") << T(1,3) << _T("\n"); + out << T(2,0) << _T(" ") << T(2,1) << _T(" ") << T(2,2) << _T(" ") << T(2,3) << _T("\n"); + out << T(3,0) << _T(" ") << T(3,1) << _T(" ") << T(3,2) << _T(" ") << T(3,3) << _T("\n"); + } + return !out.fail(); +} + + +// export poses in Strecha camera format: +// Strecha model is P = K[R^T|-R^T t] +// our model is P = K[R|t], t = -RC +bool ExportImagesCamera(const String& pathName, const Interface& scene) +{ + LOG_OUT() << "Writing poses: " << pathName << std::endl; + Util::ensureFolder(pathName); + for (uint32_t ID=0; ID<(uint32_t)scene.images.size(); ++ID) { + const Interface::Image& image = scene.images[ID]; + String imageFileName(image.name); + Util::ensureValidPath(imageFileName); + const String fileName(pathName+Util::getFileNameExt(imageFileName)+".camera"); + std::ofstream out(fileName); + if (!out.good()) { + VERBOSE("error: unable to open file '%s'", fileName.c_str()); + return false; + } + out << std::setprecision(12); + KMatrix K(KMatrix::IDENTITY); + RMatrix R(RMatrix::IDENTITY); + CMatrix t(CMatrix::ZERO); + unsigned width(0), height(0); + if (image.platformID != NO_ID && image.cameraID != NO_ID) { + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Camera& camera = platform.cameras[image.cameraID]; + if (camera.HasResolution()) { + width = camera.width; + height = camera.height; + K = camera.K; + } else { + IMAGEPTR pImage = Image::ReadImageHeader(image.name); + width = pImage->GetWidth(); + height = pImage->GetHeight(); + K = platform.GetFullK(image.cameraID, width, height); + } + if (image.poseID != NO_ID) { + const Interface::Platform::Pose& pose = platform.poses[image.poseID]; + R = pose.R.t(); + t = pose.C; + } + } + out << K(0,0) << _T(" ") << K(0,1) << _T(" ") << K(0,2) << _T("\n"); + out << K(1,0) << _T(" ") << K(1,1) << _T(" ") << K(1,2) << _T("\n"); + out << K(2,0) << _T(" ") << K(2,1) << _T(" ") << K(2,2) << _T("\n"); + out << _T("0 0 0") << _T("\n"); + out << R(0,0) << _T(" ") << R(0,1) << _T(" ") << R(0,2) << _T("\n"); + out << R(1,0) << _T(" ") << R(1,1) << _T(" ") << R(1,2) << _T("\n"); + out << R(2,0) << _T(" ") << R(2,1) << _T(" ") << R(2,2) << _T("\n"); + out << t.x << _T(" ") << t.y << _T(" ") << t.z << _T("\n"); + out << width << _T(" ") << height << _T("\n"); + if (out.fail()) { + VERBOSE("error: unable to write file '%s'", fileName.c_str()); + return false; + } + } + return true; +} + +} // unnamed namespace + +// Main function with command-line arguments +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application app; // Instance of the application + if (!app.Initialize(argc, argv)) // Initialize app with command line arguments + return EXIT_FAILURE; // Exit if initialization fails + + TD_TIMER_START(); + + if (OPT::IsFromOpenMVS) { + // read MVS input data + Interface scene; // Scene data structure for MVS input + if (!ARCHIVE::SerializeLoad(scene, MAKE_PATH_SAFE(OPT::strInputFileName))) + return EXIT_FAILURE; // Exit if loading fails + + // Check output file type and export accordingly + if (Util::getFileExt(OPT::strOutputFileName) == _T(".log")) { + // write poses in log format + ExportIntrinsicsTxt(MAKE_PATH_FULL(WORKING_FOLDER_FULL, String("intrinsics.txt")), scene); + ExportImagesLog(MAKE_PATH_SAFE(OPT::strOutputFileName), scene); + } else + if (Util::getFileExt(OPT::strOutputFileName) == _T(".camera")) { + // write poses in Strecha camera format + ExportImagesCamera((OPT::strOutputFileName=Util::getFileFullName(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strOutputFileName)))+PATH_SEPARATOR, scene); + } else { + // write COLMAP input data + if (!OPT::strPointCloudFileName.empty() && !ImportPointCloud(MAKE_PATH_SAFE(OPT::strPointCloudFileName), scene)) + return EXIT_FAILURE; + Util::ensureFolderSlash(OPT::strOutputFileName); + ExportScene(MAKE_PATH_SAFE(OPT::strOutputFileName), scene, OPT::bForceSparsePointCloud); + } + VERBOSE("Input data exported: %u images & %u vertices (%s)", scene.images.size(), scene.vertices.size(), TD_TIMER_GET_FMT().c_str()); + } else { + // read COLMAP input data + Interface scene; + const String strOutFolder(Util::getFilePath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strOutputFileName))); + PointCloud pointcloud; + if (!ImportScene(MAKE_PATH_SAFE(OPT::strInputFileName), strOutFolder, scene, pointcloud)) + return EXIT_FAILURE; + // write MVS input data + Util::ensureFolder(strOutFolder); + if (!ARCHIVE::SerializeSave(scene, MAKE_PATH_SAFE(OPT::strOutputFileName))) + return EXIT_FAILURE; + if (!pointcloud.IsEmpty() && !pointcloud.Save(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName)) + _T(".ply"), true)) + return EXIT_FAILURE; + VERBOSE("Exported data: %u images, %u points%s (%s)", + scene.images.size(), scene.vertices.size(), pointcloud.IsEmpty()?"":String::FormatString(", %d dense points", pointcloud.GetSize()).c_str(), + TD_TIMER_GET_FMT().c_str()); + } + + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfaceCOLMAP/endian.h b/apps/InterfaceCOLMAP/endian.h new file mode 100644 index 0000000..12a22ca --- /dev/null +++ b/apps/InterfaceCOLMAP/endian.h @@ -0,0 +1,167 @@ +// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_ENDIAN_H_ +#define COLMAP_SRC_UTIL_ENDIAN_H_ + +#include +#include +#include + +namespace colmap { + +// Reverse the order of each byte. +template +T ReverseBytes(const T& data); + +// Check the order in which bytes are stored in computer memory. +bool IsLittleEndian(); +bool IsBigEndian(); + +// Convert data between endianness and the native format. Note that, for float +// and double types, these functions are only valid if the format is IEEE-754. +// This is the case for pretty much most processors. +template +T LittleEndianToNative(const T x); +template +T BigEndianToNative(const T x); +template +T NativeToLittleEndian(const T x); +template +T NativeToBigEndian(const T x); + +// Read data in little endian format for cross-platform support. +template +T ReadBinaryLittleEndian(std::istream* stream); +template +void ReadBinaryLittleEndian(std::istream* stream, std::vector* data); + +// Write data in little endian format for cross-platform support. +template +void WriteBinaryLittleEndian(std::ostream* stream, const T& data); +template +void WriteBinaryLittleEndian(std::ostream* stream, const std::vector& data); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +T ReverseBytes(const T& data) { + T data_reversed = data; + std::reverse(reinterpret_cast(&data_reversed), + reinterpret_cast(&data_reversed) + sizeof(T)); + return data_reversed; +} + +inline bool IsLittleEndian() { +#ifdef BOOST_BIG_ENDIAN + return false; +#else + return true; +#endif +} + +inline bool IsBigEndian() { +#ifdef BOOST_BIG_ENDIAN + return true; +#else + return false; +#endif +} + +template +T LittleEndianToNative(const T x) { + if (IsLittleEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T BigEndianToNative(const T x) { + if (IsBigEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T NativeToLittleEndian(const T x) { + if (IsLittleEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T NativeToBigEndian(const T x) { + if (IsBigEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T ReadBinaryLittleEndian(std::istream* stream) { + T data_little_endian; + stream->read(reinterpret_cast(&data_little_endian), sizeof(T)); + return LittleEndianToNative(data_little_endian); +} + +template +void ReadBinaryLittleEndian(std::istream* stream, std::vector* data) { + for (size_t i = 0; i < data->size(); ++i) { + (*data)[i] = ReadBinaryLittleEndian(stream); + } +} + +template +void WriteBinaryLittleEndian(std::ostream* stream, const T& data) { + const T data_little_endian = NativeToLittleEndian(data); + stream->write(reinterpret_cast(&data_little_endian), sizeof(T)); +} + +template +void WriteBinaryLittleEndian(std::ostream* stream, const std::vector& data) { + for (const auto& elem : data) { + WriteBinaryLittleEndian(stream, elem); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_ENDIAN_H_ + diff --git a/apps/InterfaceCOLMAPTest/CMakeLists.txt b/apps/InterfaceCOLMAPTest/CMakeLists.txt new file mode 100644 index 0000000..5d48c79 --- /dev/null +++ b/apps/InterfaceCOLMAPTest/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(InterfaceCOLMAP "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS InterfaceCOLMAP + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/InterfaceCOLMAPTest/InterfaceCOLMAPTest.cpp b/apps/InterfaceCOLMAPTest/InterfaceCOLMAPTest.cpp new file mode 100644 index 0000000..0c333b3 --- /dev/null +++ b/apps/InterfaceCOLMAPTest/InterfaceCOLMAPTest.cpp @@ -0,0 +1,1493 @@ +/* + * InterfaceCOLMAP.cpp + * + * Copyright (c) 2014-2018 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include +#include "endian.h" + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("InterfaceCOLMAP") +#define MVS_EXT _T(".mvs") +#define COLMAP_IMAGES_FOLDER _T("images/") +#define COLMAP_SPARSE_FOLDER _T("sparse/") +#define COLMAP_CAMERAS_TXT COLMAP_SPARSE_FOLDER _T("cameras.txt") +#define COLMAP_IMAGES_TXT COLMAP_SPARSE_FOLDER _T("images.txt") +#define COLMAP_POINTS_TXT COLMAP_SPARSE_FOLDER _T("points3D.txt") +#define COLMAP_CAMERAS_BIN COLMAP_SPARSE_FOLDER _T("cameras.bin") +#define COLMAP_IMAGES_BIN COLMAP_SPARSE_FOLDER _T("images.bin") +#define COLMAP_POINTS_BIN COLMAP_SPARSE_FOLDER _T("points3D.bin") +#define COLMAP_DENSE_POINTS _T("fused.ply") +#define COLMAP_DENSE_POINTS_VISIBILITY _T("fused.ply.vis") +#define COLMAP_STEREO_FOLDER _T("stereo/") +#define COLMAP_FUSION COLMAP_STEREO_FOLDER _T("fusion.cfg") +#define COLMAP_PATCHMATCH COLMAP_STEREO_FOLDER _T("patch-match.cfg") +#define COLMAP_STEREO_CONSISTENCYGRAPHS_FOLDER COLMAP_STEREO_FOLDER _T("consistency_graphs/") +#define COLMAP_STEREO_DEPTHMAPS_FOLDER COLMAP_STEREO_FOLDER _T("depth_maps/") +#define COLMAP_STEREO_NORMALMAPS_FOLDER COLMAP_STEREO_FOLDER _T("normal_maps/") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +bool IsFromOpenMVS; // conversion direction +bool bNormalizeIntrinsics; +bool bForceSparsePointCloud; +String strInputFileName; +String strPointCloudFileName; +String strOutputFileName; +String strImageFolder; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { CleanUp(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void CleanUp(); +}; // Application + +// Initializes the application and parses command-line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // Open the log file and console for output + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "imports SfM or MVS scene stored in COLMAP undistoreted format OR exports MVS scene to COLMAP format") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description configOptions("Main options"); + configOptions.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input COLMAP folder containing cameras, images and points files OR input MVS project file") + ("pointcloud-file,p", boost::program_options::value(&OPT::strPointCloudFileName), "point-cloud with views file name (overwrite existing point-cloud)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the MVS project") + ("image-folder", boost::program_options::value(&OPT::strImageFolder)->default_value(COLMAP_IMAGES_FOLDER), "folder to the undistorted images") + ("normalize,f", boost::program_options::value(&OPT::bNormalizeIntrinsics)->default_value(false), "normalize intrinsics while exporting to MVS format") + ("force-points,e", boost::program_options::value(&OPT::bForceSparsePointCloud)->default_value(false), "force exporting point-cloud as sparse points also even if dense point-cloud detected") + ; + + // Combine all command-line options + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(configOptions); + + // Only config file options + boost::program_options::options_description config_file_options; + config_file_options.add(configOptions); + + boost::program_options::positional_options_description positionalOptions; + positionalOptions.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(positionalOptions).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + + // Initialize working folder + INIT_WORKING_FOLDER; + + // Parse configuration file if exists + std::ifstream configFile(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (configFile) { + boost::program_options::store(parse_config_file(configFile, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // Open and configure the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log"))); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // Check and validate paths + Util::ensureValidPath(OPT::strInputFileName); + Util::ensureValidPath(OPT::strPointCloudFileName); + + // Handle help option or invalid command + const bool bInvalidCommand(OPT::strInputFileName.empty()); + if (OPT::vm.count("help") || bInvalidCommand) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(configOptions); + GET_LOG() << _T("\n" + "Import/export 3D reconstruction from COLMAP (TXT/BIN format) and to COLMAP (TXT format). \n" + "In order to import a scene, run COLMAP SfM and next undistort the images (only PINHOLE\n" + "camera model supported for the moment)." + "\n") + << visible; + } + + if (bInvalidCommand) + return false; + + // initialize optional options + Util::ensureValidFolderPath(OPT::strImageFolder); + Util::ensureValidPath(OPT::strOutputFileName); + OPT::strImageFolder = MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strImageFolder); + + // Determine if the source file is an OpenMVS project by checking the file extension + const String inputFileExtension(Util::getFileExt(OPT::strInputFileName).ToLower()); + OPT::IsFromOpenMVS = (inputFileExtension == MVS_FILE_EXTENSION); + + // Set default output filename based on input filename if not explicitly specified + if (OPT::IsFromOpenMVS) { + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFilePath(OPT::strInputFileName); + } else { + // Ensure the input filename ends with a directory slash if needed + Util::ensureFolderSlash(OPT::strInputFileName); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = OPT::strInputFileName + _T("scene") MVS_FILE_EXTENSION; + } + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::CleanUp() +{ + MVS::CleanUp(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +namespace COLMAP { + +using namespace colmap; + +// See colmap/src/util/types.h +typedef uint32_t camera_t; +typedef uint32_t image_t; +typedef uint64_t image_pair_t; +typedef uint32_t point2D_t; +typedef uint64_t point3D_t; + +const std::vector mapCameraModel = { + "SIMPLE_PINHOLE", + "PINHOLE", + "SIMPLE_RADIAL", + "RADIAL", + "OPENCV", + "OPENCV_FISHEYE", + "FULL_OPENCV", + "FOV", + "SIMPLE_RADIAL_FISHEYE", + "RADIAL_FISHEYE", + "THIN_PRISM_FISHEYE" +}; + +// tools +bool NextLine(std::istream& stream, std::istringstream& in, bool bIgnoreEmpty=true) { + String line; + do { + std::getline(stream, line); + Util::strTrim(line, _T(" ")); + if (stream.fail()) + return false; + } while (((bIgnoreEmpty && line.empty()) || line[0u] == '#') && stream.good()); + in.clear(); + in.str(line); + return true; +} +// structure describing a camera +struct Camera { + uint32_t ID; // ID of the camera + String model; // camera model name + uint32_t width, height; // camera resolution + std::vector params; // camera parameters + uint64_t numCameras{0}; // only for binary format + + Camera() {} + Camera(uint32_t _ID) : ID(_ID) {} + bool operator < (const Camera& rhs) const { return ID < rhs.ID; } + + struct CameraHash { + size_t operator()(const Camera& camera) const { + const size_t h1(std::hash()(camera.model)); + const size_t h2(std::hash()(camera.width)); + const size_t h3(std::hash()(camera.height)); + size_t h(h1 ^ ((h2 ^ (h3 << 1)) << 1)); + for (REAL p: camera.params) + h = std::hash()(p) ^ (h << 1); + return h; + } + }; + struct CameraEqualTo { + bool operator()(const Camera& _Left, const Camera& _Right) const { + return _Left.model == _Right.model && + _Left.width == _Right.width && _Left.height == _Right.height && + _Left.params == _Right.params; + } + }; + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // Camera list with one line of data per camera: + // CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[] + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + in >> ID >> model >> width >> height; + if (in.fail()) + return false; + --ID; + if (model != _T("PINHOLE")) + return false; + params.resize(4); + in >> params[0] >> params[1] >> params[2] >> params[3]; + return !in.fail(); + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadCamerasBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (numCameras == 0) { + // Read the first entry in the binary file + numCameras = ReadBinaryLittleEndian(&stream); + } + + ID = ReadBinaryLittleEndian(&stream)-1; + model = mapCameraModel[ReadBinaryLittleEndian(&stream)]; + width = (uint32_t)ReadBinaryLittleEndian(&stream); + height = (uint32_t)ReadBinaryLittleEndian(&stream); + if (model != _T("PINHOLE")) + return false; + params.resize(4); + ReadBinaryLittleEndian(&stream, ¶ms); + return true; + } + + bool WriteTXT(std::ostream& out) const { + out << ID+1 << _T(" ") << model << _T(" ") << width << _T(" ") << height; + if (out.fail()) + return false; + for (REAL param: params) { + out << _T(" ") << param; + if (out.fail()) + return false; + } + out << std::endl; + return true; + } + + bool WriteBIN(std::ostream& stream) { + if (numCameras != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numCameras); + numCameras = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + const int64 modelId(std::distance(mapCameraModel.begin(), std::find(mapCameraModel.begin(), mapCameraModel.end(), model))); + WriteBinaryLittleEndian(&stream, (int)modelId); + WriteBinaryLittleEndian(&stream, width); + WriteBinaryLittleEndian(&stream, height); + for (REAL param: params) + WriteBinaryLittleEndian(&stream, param); + return !stream.fail(); + } +}; +typedef std::vector Cameras; +// structure describing an image +struct Image { + struct Proj { + Eigen::Vector2f p; + uint32_t idPoint; + }; + uint32_t ID; // ID of the image + Eigen::Quaterniond q; // rotation + Eigen::Vector3d t; // translation + uint32_t idCamera; // ID of the associated camera + String name; // image file name + std::vector projs; // known image projections + uint64_t numRegImages{0}; // only for binary format + + Image() {} + Image(uint32_t _ID) : ID(_ID) {} + bool operator < (const Image& rhs) const { return ID < rhs.ID; } + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // Image list with two lines of data per image: + // IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME + // POINTS2D[] as (X, Y, POINT3D_ID) + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + in >> ID + >> q.w() >> q.x() >> q.y() >> q.z() + >> t(0) >> t(1) >> t(2) + >> idCamera >> name; + if (in.fail()) + return false; + --ID; --idCamera; + Util::ensureValidPath(name); + if (!NextLine(stream, in, false)) + return false; + projs.clear(); + while (true) { + Proj proj; + in >> proj.p(0) >> proj.p(1) >> (int&)proj.idPoint; + if (in.fail()) + break; + --proj.idPoint; + projs.emplace_back(proj); + } + return true; + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadImagesBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (!numRegImages) { + // Read the first entry in the binary file + numRegImages = ReadBinaryLittleEndian(&stream); + } + + ID = ReadBinaryLittleEndian(&stream)-1; + q.w() = ReadBinaryLittleEndian(&stream); + q.x() = ReadBinaryLittleEndian(&stream); + q.y() = ReadBinaryLittleEndian(&stream); + q.z() = ReadBinaryLittleEndian(&stream); + t(0) = ReadBinaryLittleEndian(&stream); + t(1) = ReadBinaryLittleEndian(&stream); + t(2) = ReadBinaryLittleEndian(&stream); + idCamera = ReadBinaryLittleEndian(&stream)-1; + + name = ""; + while (true) { + char nameChar; + stream.read(&nameChar, 1); + if (nameChar == '\0') + break; + name += nameChar; + } + Util::ensureValidPath(name); + + const size_t numPoints2D = ReadBinaryLittleEndian(&stream); + projs.clear(); + for (size_t j = 0; j < numPoints2D; ++j) { + Proj proj; + proj.p(0) = (float)ReadBinaryLittleEndian(&stream); + proj.p(1) = (float)ReadBinaryLittleEndian(&stream); + proj.idPoint = (uint32_t)ReadBinaryLittleEndian(&stream)-1; + projs.emplace_back(proj); + } + return true; + } + + bool WriteTXT(std::ostream& out) const { + out << ID+1 << _T(" ") + << q.w() << _T(" ") << q.x() << _T(" ") << q.y() << _T(" ") << q.z() << _T(" ") + << t(0) << _T(" ") << t(1) << _T(" ") << t(2) << _T(" ") + << idCamera+1 << _T(" ") << name + << std::endl; + for (const Proj& proj: projs) { + out << proj.p(0) << _T(" ") << proj.p(1) << _T(" ") << (int)proj.idPoint+1 << _T(" "); + if (out.fail()) + return false; + } + out << std::endl; + return !out.fail(); + } + + bool WriteBIN(std::ostream& stream) { + if (numRegImages != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numRegImages); + numRegImages = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + + WriteBinaryLittleEndian(&stream, q.w()); + WriteBinaryLittleEndian(&stream, q.x()); + WriteBinaryLittleEndian(&stream, q.y()); + WriteBinaryLittleEndian(&stream, q.z()); + + WriteBinaryLittleEndian(&stream, t(0)); + WriteBinaryLittleEndian(&stream, t(1)); + WriteBinaryLittleEndian(&stream, t(2)); + + WriteBinaryLittleEndian(&stream, idCamera+1); + + stream.write(name.c_str(), name.size()+1); + + WriteBinaryLittleEndian(&stream, projs.size()); + for (const Proj& proj: projs) { + WriteBinaryLittleEndian(&stream, proj.p(0)); + WriteBinaryLittleEndian(&stream, proj.p(1)); + WriteBinaryLittleEndian(&stream, proj.idPoint+1); + } + return !stream.fail(); + } +}; +typedef std::vector Images; +// structure describing a 3D point +struct Point { + struct Track { + uint32_t idImage; + uint32_t idProj; + }; + uint32_t ID; // ID of the point + Interface::Pos3f p; // position + Interface::Col3 c; // BGR color + float e; // error + std::vector tracks; // point track + uint64_t numPoints3D{0}; // only for binary format + + Point() {} + Point(uint32_t _ID) : ID(_ID) {} + bool operator < (const Image& rhs) const { return ID < rhs.ID; } + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // 3D point list with one line of data per point: + // POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX) + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + int r,g,b; + in >> ID + >> p.x >> p.y >> p.z + >> r >> g >> b + >> e; + c.x = CLAMP(b,0,255); + c.y = CLAMP(g,0,255); + c.z = CLAMP(r,0,255); + if (in.fail()) + return false; + --ID; + tracks.clear(); + while (true) { + Track track; + in >> track.idImage >> track.idProj; + if (in.fail()) + break; + --track.idImage; --track.idProj; + tracks.emplace_back(track); + } + return !tracks.empty(); + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadPoints3DBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (!numPoints3D) { + // Read the first entry in the binary file + numPoints3D = ReadBinaryLittleEndian(&stream); + } + + int r,g,b; + ID = (uint32_t)ReadBinaryLittleEndian(&stream)-1; + p.x = (float)ReadBinaryLittleEndian(&stream); + p.y = (float)ReadBinaryLittleEndian(&stream); + p.z = (float)ReadBinaryLittleEndian(&stream); + r = ReadBinaryLittleEndian(&stream); + g = ReadBinaryLittleEndian(&stream); + b = ReadBinaryLittleEndian(&stream); + e = (float)ReadBinaryLittleEndian(&stream); + c.x = CLAMP(b,0,255); + c.y = CLAMP(g,0,255); + c.z = CLAMP(r,0,255); + + const size_t trackLength = ReadBinaryLittleEndian(&stream); + tracks.clear(); + for (size_t j = 0; j < trackLength; ++j) { + Track track; + track.idImage = ReadBinaryLittleEndian(&stream)-1; + track.idProj = ReadBinaryLittleEndian(&stream)-1; + tracks.emplace_back(track); + } + return !tracks.empty(); + } + + bool WriteTXT(std::ostream& out) const { + ASSERT(!tracks.empty()); + const int r(c.z),g(c.y),b(c.x); + out << ID+1 << _T(" ") + << p.x << _T(" ") << p.y << _T(" ") << p.z << _T(" ") + << r << _T(" ") << g << _T(" ") << b << _T(" ") + << e << _T(" "); + for (const Track& track: tracks) { + out << track.idImage+1 << _T(" ") << track.idProj+1 << _T(" "); + if (out.fail()) + return false; + } + out << std::endl; + return !out.fail(); + } + + bool WriteBIN(std::ostream& stream) { + ASSERT(!tracks.empty()); + if (numPoints3D != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numPoints3D); + numPoints3D = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + WriteBinaryLittleEndian(&stream, p.x); + WriteBinaryLittleEndian(&stream, p.y); + WriteBinaryLittleEndian(&stream, p.z); + WriteBinaryLittleEndian(&stream, c.z); + WriteBinaryLittleEndian(&stream, c.y); + WriteBinaryLittleEndian(&stream, c.x); + WriteBinaryLittleEndian(&stream, e); + + WriteBinaryLittleEndian(&stream, tracks.size()); + for (const Track& track: tracks) { + WriteBinaryLittleEndian(&stream, track.idImage+1); + WriteBinaryLittleEndian(&stream, track.idProj+1); + } + return !stream.fail(); + } +}; +typedef std::vector Points; +// structure describing an 2D dynamic matrix +template +struct Mat { + size_t width_ = 0; + size_t height_ = 0; + size_t depth_ = 0; + std::vector data_; + + size_t GetNumBytes() const { + return data_.size() * sizeof(T); + } + const T* GetChannelPtr(size_t c) const { + return data_.data()+width_*height_*c; + } + + // See: colmap/src/mvs/mat.h + void Read(const std::string& path) { + std::streampos pos; { + std::fstream text_file(path, std::ios::in | std::ios::binary); + char unused_char; + text_file >> width_ >> unused_char >> height_ >> unused_char >> depth_ >> + unused_char; + pos = text_file.tellg(); + } + data_.resize(width_ * height_ * depth_); + std::fstream binary_file(path, std::ios::in | std::ios::binary); + binary_file.seekg(pos); + ReadBinaryLittleEndian(&binary_file, &data_); + } + void Write(const std::string& path) const { + { + std::fstream text_file(path, std::ios::out); + text_file << width_ << "&" << height_ << "&" << depth_ << "&"; + } + std::fstream binary_file(path, std::ios::out | std::ios::binary | std::ios::app); + WriteBinaryLittleEndian(&binary_file, data_); + } +}; +} // namespace COLMAP + +typedef Eigen::Matrix EMat33d; +typedef Eigen::Matrix EVec3d; + +/** + * Determines whether to use the TXT or BIN version of a file based on availability. + * It tries to open the TXT file first; if unsuccessful, it attempts to open the BIN file. + * + * @param filenameTXT The path to the TXT file. + * @param filenameBIN The path to the BIN file. + * @param fileStream Reference to the ifstream object that will be used for file operations. + * @param chosenFilename Reference to a string that will store the path of the chosen file. + * @param isBinaryFormat Reference to a boolean that indicates whether the chosen file is binary. + * @return true if either file was successfully opened, false otherwise. + */ +bool DetermineInputSource(const String& filenameTXT, const String& filenameBIN, std::ifstream& fileStream, String& filenameCamera, bool& isBinaryFormat) +{ + // Try to open the TXT file first. + fileStream.open(filenameTXT); + if (fileStream.good()) { + filenameCamera = filenameTXT; + isBinaryFormat = false; + return true; + } + fileStream.open(filenameBIN, std::ios::binary); + if (fileStream.good()) { + filenameCamera = filenameBIN; + isBinaryFormat = true; + return true; + } + VERBOSE("error: unable to open file '%s'", filenameTXT.c_str()); + VERBOSE("error: unable to open file '%s'", filenameBIN.c_str()); + return false; +} + + +bool ImportScene(const String& inputFolder, const String& outputFolder, Interface& scene, PointCloud& pointcloud) +{ + // Define a map to store camera IDs and associated platform index. + typedef std::unordered_map CamerasMap; + CamerasMap mapCameras; + { + // Determine the camera file source between TXT and BIN formats. + const String filenameCamerasTXT(inputFolder+COLMAP_CAMERAS_TXT); + const String filenameCamerasBIN(inputFolder+COLMAP_CAMERAS_BIN); + std::ifstream cameraFile; + bool isBinaryFormat; + String filenameCamera; + if (!DetermineInputSource(filenameCamerasTXT, filenameCamerasBIN, cameraFile, filenameCamera, isBinaryFormat)) { + return false; // Exit if file source determination fails. + } + LOG_OUT() << "Reading cameras: " << filenameCamera << std::endl; + + typedef std::unordered_map CamerasSet; + CamerasSet setCameras; + COLMAP::Camera colmapCamera; + while (cameraFile.good() && colmapCamera.Read(cameraFile, isBinaryFormat)) { + const auto setIt(setCameras.emplace(colmapCamera, (uint32_t)scene.platforms.size())); + mapCameras.emplace(colmapCamera.ID, setIt.first->second); + if (!setIt.second) { + // reuse existing platform + continue; + } + // create new platform + Interface::Platform platform; + platform.name = String::FormatString(_T("platform%03u"), colmapCamera.ID); // only one camera per platform supported + Interface::Platform::Camera camera; + camera.name = colmapCamera.model; + camera.K = Interface::Mat33d::eye(); + // account for different pixel center conventions as COLMAP uses pixel center at (0.5,0.5) + camera.K(0,0) = colmapCamera.params[0]; + camera.K(1,1) = colmapCamera.params[1]; + camera.K(0,2) = colmapCamera.params[2]-REAL(0.5); + camera.K(1,2) = colmapCamera.params[3]-REAL(0.5); + camera.R = Interface::Mat33d::eye(); + camera.C = Interface::Pos3d(0,0,0); + if (OPT::bNormalizeIntrinsics) { + // normalize camera intrinsics + camera.K = Camera::ScaleK(camera.K, 1.0/Camera::GetNormalizationScale(colmapCamera.width, colmapCamera.height)); + } else { + camera.width = colmapCamera.width; + camera.height = colmapCamera.height; + } + platform.cameras.emplace_back(camera); + scene.platforms.emplace_back(platform); + } + } + if (mapCameras.empty()) { + VERBOSE("error: no valid cameras (make sure they are in PINHOLE model)"); + return false; + } + + // Function to read and process image data from input files + typedef std::map ImagesMap; + ImagesMap mapImages; + { + // Define a map to associate COLMAP image structures with scene image indices + const String filenameImagesTXT(inputFolder+COLMAP_IMAGES_TXT); + const String filenameImagesBIN(inputFolder+COLMAP_IMAGES_BIN); + std::ifstream file; + bool binary; + String filenameImages; + if (!DetermineInputSource(filenameImagesTXT, filenameImagesBIN, file, filenameImages, binary)) { + return false; + } + LOG_OUT() << "Reading images: " << filenameImages << std::endl; + + // Read image data from the file and store in the scene structure + COLMAP::Image imageColmap; + while (file.good() && imageColmap.Read(file, binary)) { + mapImages.emplace(imageColmap, (uint32_t)scene.images.size()); + Interface::Platform::Pose pose; + Eigen::Map(pose.R.val) = imageColmap.q.toRotationMatrix(); + EnsureRotationMatrix((Matrix3x3d&)pose.R); + Eigen::Map(&pose.C.x) = -(imageColmap.q.inverse() * imageColmap.t); + + Interface::Image image; + image.name = MAKE_PATH_REL(outputFolder,OPT::strImageFolder+imageColmap.name); + image.platformID = mapCameras.at(imageColmap.idCamera); + image.cameraID = 0; + image.ID = imageColmap.ID; + + Interface::Platform& platform = scene.platforms[image.platformID]; + image.poseID = (uint32_t)platform.poses.size(); + platform.poses.emplace_back(pose); + scene.images.emplace_back(image); + } + } + + // read points list + const String filenameDensePoints(inputFolder+COLMAP_DENSE_POINTS); + const String filenameDenseVisPoints(inputFolder+COLMAP_DENSE_POINTS_VISIBILITY); + { + // parse sparse point-cloud + const String filenamePointsTXT(inputFolder+COLMAP_POINTS_TXT); + const String filenamePointsBIN(inputFolder+COLMAP_POINTS_BIN); + std::ifstream file; + bool binary; + String filenamePoints; + if (!DetermineInputSource(filenamePointsTXT, filenamePointsBIN, file, filenamePoints, binary)) { + return false; + } + LOG_OUT() << "Reading points: " << filenamePoints << std::endl; + COLMAP::Point point; + while (file.good() && point.Read(file, binary)) { + Interface::Vertex vertex; + vertex.X = point.p; + for (const COLMAP::Point::Track& track: point.tracks) { + Interface::Vertex::View view; + view.imageID = mapImages.at(COLMAP::Image(track.idImage)); + view.confidence = 0; + vertex.views.emplace_back(view); + } + std::sort(vertex.views.begin(), vertex.views.end(), + [](const Interface::Vertex::View& view0, const Interface::Vertex::View& view1) { return view0.imageID < view1.imageID; }); + scene.vertices.emplace_back(std::move(vertex)); + scene.verticesColor.emplace_back(Interface::Color{point.c}); + } + } + pointcloud.Release(); + if (File::access(filenameDensePoints) && File::access(filenameDenseVisPoints)) { + // parse dense point-cloud + LOG_OUT() << "Reading points: " << filenameDensePoints << " and " << filenameDenseVisPoints << std::endl; + if (!pointcloud.Load(filenameDensePoints)) { + VERBOSE("error: unable to open file '%s'", filenameDensePoints.c_str()); + return false; + } + File file(filenameDenseVisPoints, File::READ, File::OPEN); + if (!file.isOpen()) { + VERBOSE("error: unable to open file '%s'", filenameDenseVisPoints.c_str()); + return false; + } + uint64_t numPoints(0); + file.read(&numPoints, sizeof(uint64_t)); + if (pointcloud.GetSize() != numPoints) { + VERBOSE("error: point-cloud and visibility have different size"); + return false; + } + pointcloud.pointViews.resize(numPoints); + for (size_t i=0; isecond]; + CLISTDEF2(String) neighborNames; + Util::strSplit(neighbors, _T(','), neighborNames); + FOREACH(i, neighborNames) { + String& neighborName = neighborNames[i]; + Util::strTrim(neighborName, _T(" ")); + const ImagesMap::const_iterator it_neighbor = std::find_if(mapImages.begin(), mapImages.end(), + [&neighborName](const ImagesMap::value_type& image) { + return image.first.name == neighborName; + }); + if (it_neighbor == mapImages.end()) { + if (i == 0) + break; + continue; + } + imageNeighbors.emplace_back(scene.images[it_neighbor->second].ID); + } + } + } + LOG_OUT() << "Reading depth-maps/normal-maps: " << pathDepthMaps << " and " << pathNormalMaps << std::endl; + Util::ensureFolder(outputFolder); + const String strType[] = {".geometric.bin", ".photometric.bin"}; + FOREACH(idx, scene.images) { + const Interface::Image& image = scene.images[idx]; + COLMAP::Mat colDepthMap, colNormalMap; + const String filenameImage(Util::getFileNameExt(image.name)); + for (int i=0; i<2; ++i) { + const String filenameDepthMaps(pathDepthMaps+filenameImage+strType[i]); + if (File::isFile(filenameDepthMaps)) { + colDepthMap.Read(filenameDepthMaps); + const String filenameNormalMaps(pathNormalMaps+filenameImage+strType[i]); + if (File::isFile(filenameNormalMaps)) { + colNormalMap.Read(filenameNormalMaps); + } + break; + } + } + if (!colDepthMap.data_.empty()) { + IIndexArr IDs = {image.ID}; + IDs.Join(imagesNeighbors[(IIndex)idx]); + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Pose pose(platform.GetPose(image.cameraID, image.poseID)); + const Interface::Mat33d K(platform.GetFullK(image.cameraID, (uint32_t)colDepthMap.width_, (uint32_t)colDepthMap.height_)); + MVS::DepthMap depthMap((int)colDepthMap.height_, (int)colDepthMap.width_); + memcpy(depthMap.getData(), colDepthMap.data_.data(), colDepthMap.GetNumBytes()); + MVS::NormalMap normalMap; + if (!colNormalMap.data_.empty()) { + normalMap.create((int)colNormalMap.height_, (int)colNormalMap.width_); + cv::merge(std::vector{ + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(0)), + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(1)), + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(2)) + }, normalMap); + } + MVS::ConfidenceMap confMap; + MVS::ViewsMap viewsMap; + const auto depthMM(std::minmax_element(colDepthMap.data_.cbegin(), colDepthMap.data_.cend())); + const MVS::Depth dMin(*depthMM.first), dMax(*depthMM.second); + if (!ExportDepthDataRaw(outputFolder+String::FormatString("depth%04u.dmap", image.ID), MAKE_PATH_FULL(outputFolder, image.name), IDs, depthMap.size(), K, pose.R, pose.C, dMin, dMax, depthMap, normalMap, confMap, viewsMap)) + return false; + } + } + } + return true; +} + + +bool ImportPointCloud(const String& strPointCloudFileName, Interface& scene) +{ + PointCloud pointcloud; + if (!pointcloud.Load(strPointCloudFileName)) { + VERBOSE("error: cannot load point-cloud file"); + return false; + } + if (!pointcloud.IsValid()) { + VERBOSE("error: loaded point-cloud does not have visibility information"); + return false; + } + // replace scene point-cloud with the loaded one + scene.vertices.clear(); + scene.verticesColor.clear(); + scene.verticesNormal.clear(); + scene.vertices.reserve(pointcloud.points.size()); + if (!pointcloud.colors.empty()) + scene.verticesColor.reserve(pointcloud.points.size()); + if (!pointcloud.normals.empty()) + scene.verticesNormal.reserve(pointcloud.points.size()); + FOREACH(i, pointcloud.points) { + Interface::Vertex vertex; + vertex.X = pointcloud.points[i]; + vertex.views.reserve(pointcloud.pointViews[i].size()); + FOREACH(j, pointcloud.pointViews[i]) { + Interface::Vertex::View& view = vertex.views.emplace_back(); + view.imageID = pointcloud.pointViews[i][j]; + view.confidence = (pointcloud.pointWeights.empty() ? 0.f : pointcloud.pointWeights[i][j]); + } + scene.vertices.emplace_back(std::move(vertex)); + if (!pointcloud.colors.empty()) { + const Pixel8U& c = pointcloud.colors[i]; + scene.verticesColor.emplace_back(Interface::Color{Interface::Col3{c.b, c.g, c.r}}); + } + if (!pointcloud.normals.empty()) + scene.verticesNormal.emplace_back(Interface::Normal{pointcloud.normals[i]}); + } + return true; +} + +bool ExportScene(const String& strFolder, const Interface& scene, bool bForceSparsePointCloud = false, bool binary = true) +{ + Util::ensureFolder(strFolder+COLMAP_SPARSE_FOLDER); + + // write camera list + CLISTDEF0IDX(KMatrix,uint32_t) Ks; + CLISTDEF0IDX(COLMAP::Camera,uint32_t) cams; + { + // Construct the filename for camera data based on the format preference + const String filenameCameras(strFolder+(binary?COLMAP_CAMERAS_BIN:COLMAP_CAMERAS_TXT)); + LOG_OUT() << "Writing cameras: " << filenameCameras << std::endl; + std::ofstream file(filenameCameras, binary ? std::ios::trunc|std::ios::binary : std::ios::trunc); + + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenameCameras.c_str()); + return false; + } + + COLMAP::Camera cam; + if (binary) { + cam.numCameras = 0; + for (const Interface::Platform& platform: scene.platforms) + cam.numCameras += (uint32_t)platform.cameras.size(); + } else { + file << _T("# Camera list with one line of data per camera:") << std::endl; + file << _T("# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]") << std::endl; + } + cam.model = _T("PINHOLE"); + cam.params.resize(4); + for (uint32_t ID=0; ID<(uint32_t)scene.platforms.size(); ++ID) { + const Interface::Platform& platform = scene.platforms[ID]; + ASSERT(platform.cameras.size() == 1); // only one camera per platform supported + const Interface::Platform::Camera& camera = platform.cameras[0]; + cam.ID = ID; + KMatrix K; + if (camera.width == 0 || camera.height == 0) { + // find one image using this camera + const Interface::Image* pImage(NULL); + for (uint32_t i=0; i<(uint32_t)scene.images.size(); ++i) { + const Interface::Image& image = scene.images[i]; + if (image.platformID == ID && image.cameraID == 0 && image.poseID != NO_ID) { + pImage = ℑ + break; + } + } + if (pImage == NULL) { + LOG("error: no image using camera %u of platform %u", 0, ID); + continue; + } + IMAGEPTR ptrImage(Image::ReadImageHeader(MAKE_PATH_SAFE(pImage->name))); + if (ptrImage == NULL) + return false; + cam.width = ptrImage->GetWidth(); + cam.height = ptrImage->GetHeight(); + // unnormalize camera intrinsics + K = platform.GetFullK(0, cam.width, cam.height); + } else { + cam.width = camera.width; + cam.height = camera.height; + K = camera.K; + } + // account for different pixel center conventions as COLMAP uses pixel center at (0.5,0.5) + cam.params[0] = K(0,0); + cam.params[1] = K(1,1); + cam.params[2] = K(0,2)+REAL(0.5); + cam.params[3] = K(1,2)+REAL(0.5); + if (!cam.Write(file, binary)) + return false; + Ks.emplace_back(K); + cams.emplace_back(cam); + } + } + + // create images list + COLMAP::Images images; + CameraArr cameras; + float maxNumPointsSparse(0); + const float avgViewsPerPoint(3.f); + const uint32_t avgResolutionSmallView(640*480), avgResolutionLargeView(6000*4000); + const uint32_t avgPointsPerSmallView(3000), avgPointsPerLargeView(12000); + { + images.resize(scene.images.size()); + cameras.resize((unsigned)scene.images.size()); + for (uint32_t ID=0; ID<(uint32_t)scene.images.size(); ++ID) { + const Interface::Image& image = scene.images[ID]; + if (image.poseID == NO_ID) + continue; + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Pose& pose = platform.poses[image.poseID]; + ASSERT(image.cameraID == 0); + COLMAP::Image& img = images[ID]; + img.ID = ID; + img.q = Eigen::Quaterniond(Eigen::Map(pose.R.val)); + img.t = -(img.q * Eigen::Map(&pose.C.x)); + img.idCamera = image.platformID; + img.name = MAKE_PATH_REL(OPT::strImageFolder, MAKE_PATH_FULL(WORKING_FOLDER_FULL, image.name)); + Camera& camera = cameras[ID]; + camera.K = Ks[image.platformID]; + camera.R = pose.R; + camera.C = pose.C; + camera.ComposeP(); + const COLMAP::Camera& cam = cams[image.platformID]; + const uint32_t resolutionView(cam.width*cam.height); + const float linearFactor(float(avgResolutionLargeView-resolutionView)/(avgResolutionLargeView-avgResolutionSmallView)); + maxNumPointsSparse += (avgPointsPerSmallView+(avgPointsPerLargeView-avgPointsPerSmallView)*linearFactor)/avgViewsPerPoint; + } + } + + // auto-select dense or sparse mode based on number of points + const bool bSparsePointCloud(scene.vertices.size() < (size_t)maxNumPointsSparse); + if (bSparsePointCloud || bForceSparsePointCloud) { + // write points list + { + const String filenamePoints(strFolder+(binary?COLMAP_POINTS_BIN:COLMAP_POINTS_TXT)); + LOG_OUT() << "Writing points: " << filenamePoints << std::endl; + std::ofstream file(filenamePoints, binary ? std::ios::trunc|std::ios::binary : std::ios::trunc); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenamePoints.c_str()); + return false; + } + uint64_t numPoints3D = 0; + if (binary) { + numPoints3D = scene.vertices.size(); + } else { + file << _T("# 3D point list with one line of data per point:") << std::endl; + file << _T("# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)") << std::endl; + } + for (uint32_t ID=0; ID<(uint32_t)scene.vertices.size(); ++ID) { + const Interface::Vertex& vertex = scene.vertices[ID]; + COLMAP::Point point; + point.ID = ID; + point.p = vertex.X; + for (const Interface::Vertex::View& view: vertex.views) { + COLMAP::Image& img = images[view.imageID]; + point.tracks.emplace_back(COLMAP::Point::Track{view.imageID, (uint32_t)img.projs.size()}); + COLMAP::Image::Proj proj; + proj.idPoint = ID; + const Point3 X(vertex.X); + ProjectVertex_3x4_3_2(cameras[view.imageID].P.val, X.ptr(), proj.p.data()); + // account for different pixel center conventions as COLMAP uses pixel center at (0.5,0.5) + proj.p[0] += REAL(0.5); + proj.p[1] += REAL(0.5); + img.projs.emplace_back(proj); + } + point.c = scene.verticesColor.empty() ? Interface::Col3(255,255,255) : scene.verticesColor[ID].c; + point.e = 0; + if (numPoints3D != 0) { + point.numPoints3D = numPoints3D; + numPoints3D = 0; + } + if (!point.Write(file, binary)) + return false; + } + } + + Util::ensureFolder(strFolder+COLMAP_STEREO_FOLDER); + + // write fusion list + { + const String filenameFusion(strFolder+COLMAP_FUSION); + LOG_OUT() << "Writing fusion configuration: " << filenameFusion << std::endl; + std::ofstream file(filenameFusion); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenameFusion.c_str()); + return false; + } + for (const COLMAP::Image& img: images) { + if (img.projs.empty()) + continue; + file << img.name << std::endl; + if (file.fail()) + return false; + } + } + + // write patch-match list + { + const String filenameFusion(strFolder+COLMAP_PATCHMATCH); + LOG_OUT() << "Writing patch-match configuration: " << filenameFusion << std::endl; + std::ofstream file(filenameFusion); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", filenameFusion.c_str()); + return false; + } + for (const COLMAP::Image& img: images) { + if (img.projs.empty()) + continue; + file << img.name << std::endl; + if (file.fail()) + return false; + file << _T("__auto__, 20") << std::endl; + if (file.fail()) + return false; + } + } + + Util::ensureFolder(strFolder+COLMAP_STEREO_CONSISTENCYGRAPHS_FOLDER); + Util::ensureFolder(strFolder+COLMAP_STEREO_DEPTHMAPS_FOLDER); + Util::ensureFolder(strFolder+COLMAP_STEREO_NORMALMAPS_FOLDER); + } + if (!bSparsePointCloud) { + // export dense point-cloud + const String filenameDensePoints(strFolder+COLMAP_DENSE_POINTS); + const String filenameDenseVisPoints(strFolder+COLMAP_DENSE_POINTS_VISIBILITY); + LOG_OUT() << "Writing points: " << filenameDensePoints << " and " << filenameDenseVisPoints << std::endl; + File file(filenameDenseVisPoints, File::WRITE, File::CREATE | File::TRUNCATE); + if (!file.isOpen()) { + VERBOSE("error: unable to write file '%s'", filenameDenseVisPoints.c_str()); + return false; + } + const uint64_t numPoints(scene.vertices.size()); + file.write(&numPoints, sizeof(uint64_t)); + PointCloud pointcloud; + for (size_t i=0; iGetWidth(), pImage->GetHeight()); + Eigen::Matrix4d K4(Eigen::Matrix4d::Identity()); + K4.topLeftCorner<3,3>() = Eigen::Map(K.val); + Util::ensureFolder(fileName); + std::ofstream out(fileName); + if (!out.good()) { + VERBOSE("error: unable to open file '%s'", fileName.c_str()); + return false; + } + out << std::setprecision(12); + out << K4(0,0) << _T(" ") << K4(0,1) << _T(" ") << K4(0,2) << _T(" ") << K4(0,3) << _T("\n"); + out << K4(1,0) << _T(" ") << K4(1,1) << _T(" ") << K4(1,2) << _T(" ") << K4(1,3) << _T("\n"); + out << K4(2,0) << _T(" ") << K4(2,1) << _T(" ") << K4(2,2) << _T(" ") << K4(2,3) << _T("\n"); + out << K4(3,0) << _T(" ") << K4(3,1) << _T(" ") << K4(3,2) << _T(" ") << K4(3,3) << _T("\n"); + return !out.fail(); +} + + +// export image poses in log format +// see: http://redwood-data.org/indoor/fileformat.html +// to support: https://www.tanksandtemples.org/tutorial +bool ExportImagesLog(const String& fileName, const Interface& scene) +{ + LOG_OUT() << "Writing poses: " << fileName << std::endl; + Util::ensureFolder(fileName); + std::ofstream out(fileName); + if (!out.good()) { + VERBOSE("error: unable to open file '%s'", fileName.c_str()); + return false; + } + out << std::setprecision(12); + IIndexArr orderedImages((uint32_t)scene.images.size()); + std::iota(orderedImages.begin(), orderedImages.end(), 0u); + orderedImages.Sort([&scene](IIndex i, IIndex j) { + return scene.images[i].ID < scene.images[j].ID; + }); + for (IIndex ID: orderedImages) { + const Interface::Image& image = scene.images[ID]; + Eigen::Matrix3d R(Eigen::Matrix3d::Identity()); + Eigen::Vector3d t(Eigen::Vector3d::Zero()); + if (image.poseID != NO_ID) { + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Pose& pose = platform.poses[image.poseID]; + R = Eigen::Map(pose.R.val).transpose(); + t = Eigen::Map(&pose.C.x); + } + Eigen::Matrix4d T(Eigen::Matrix4d::Identity()); + T.topLeftCorner<3,3>() = R; + T.topRightCorner<3,1>() = t; + out << ID << _T(" ") << ID << _T(" ") << 0 << _T("\n"); + out << T(0,0) << _T(" ") << T(0,1) << _T(" ") << T(0,2) << _T(" ") << T(0,3) << _T("\n"); + out << T(1,0) << _T(" ") << T(1,1) << _T(" ") << T(1,2) << _T(" ") << T(1,3) << _T("\n"); + out << T(2,0) << _T(" ") << T(2,1) << _T(" ") << T(2,2) << _T(" ") << T(2,3) << _T("\n"); + out << T(3,0) << _T(" ") << T(3,1) << _T(" ") << T(3,2) << _T(" ") << T(3,3) << _T("\n"); + } + return !out.fail(); +} + + +// export poses in Strecha camera format: +// Strecha model is P = K[R^T|-R^T t] +// our model is P = K[R|t], t = -RC +bool ExportImagesCamera(const String& pathName, const Interface& scene) +{ + LOG_OUT() << "Writing poses: " << pathName << std::endl; + Util::ensureFolder(pathName); + for (uint32_t ID=0; ID<(uint32_t)scene.images.size(); ++ID) { + const Interface::Image& image = scene.images[ID]; + String imageFileName(image.name); + Util::ensureValidPath(imageFileName); + const String fileName(pathName+Util::getFileNameExt(imageFileName)+".camera"); + std::ofstream out(fileName); + if (!out.good()) { + VERBOSE("error: unable to open file '%s'", fileName.c_str()); + return false; + } + out << std::setprecision(12); + KMatrix K(KMatrix::IDENTITY); + RMatrix R(RMatrix::IDENTITY); + CMatrix t(CMatrix::ZERO); + unsigned width(0), height(0); + if (image.platformID != NO_ID && image.cameraID != NO_ID) { + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Camera& camera = platform.cameras[image.cameraID]; + if (camera.HasResolution()) { + width = camera.width; + height = camera.height; + K = camera.K; + } else { + IMAGEPTR pImage = Image::ReadImageHeader(image.name); + width = pImage->GetWidth(); + height = pImage->GetHeight(); + K = platform.GetFullK(image.cameraID, width, height); + } + if (image.poseID != NO_ID) { + const Interface::Platform::Pose& pose = platform.poses[image.poseID]; + R = pose.R.t(); + t = pose.C; + } + } + out << K(0,0) << _T(" ") << K(0,1) << _T(" ") << K(0,2) << _T("\n"); + out << K(1,0) << _T(" ") << K(1,1) << _T(" ") << K(1,2) << _T("\n"); + out << K(2,0) << _T(" ") << K(2,1) << _T(" ") << K(2,2) << _T("\n"); + out << _T("0 0 0") << _T("\n"); + out << R(0,0) << _T(" ") << R(0,1) << _T(" ") << R(0,2) << _T("\n"); + out << R(1,0) << _T(" ") << R(1,1) << _T(" ") << R(1,2) << _T("\n"); + out << R(2,0) << _T(" ") << R(2,1) << _T(" ") << R(2,2) << _T("\n"); + out << t.x << _T(" ") << t.y << _T(" ") << t.z << _T("\n"); + out << width << _T(" ") << height << _T("\n"); + if (out.fail()) { + VERBOSE("error: unable to write file '%s'", fileName.c_str()); + return false; + } + } + return true; +} + +} // unnamed namespace + +// Main function with command-line arguments +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application app; // Instance of the application + if (!app.Initialize(argc, argv)) // Initialize app with command line arguments + return EXIT_FAILURE; // Exit if initialization fails + + TD_TIMER_START(); + + if (OPT::IsFromOpenMVS) { + // read MVS input data + Interface scene; // Scene data structure for MVS input + if (!ARCHIVE::SerializeLoad(scene, MAKE_PATH_SAFE(OPT::strInputFileName))) + return EXIT_FAILURE; // Exit if loading fails + + // Check output file type and export accordingly + if (Util::getFileExt(OPT::strOutputFileName) == _T(".log")) { + // write poses in log format + ExportIntrinsicsTxt(MAKE_PATH_FULL(WORKING_FOLDER_FULL, String("intrinsics.txt")), scene); + ExportImagesLog(MAKE_PATH_SAFE(OPT::strOutputFileName), scene); + } else + if (Util::getFileExt(OPT::strOutputFileName) == _T(".camera")) { + // write poses in Strecha camera format + ExportImagesCamera((OPT::strOutputFileName=Util::getFileFullName(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strOutputFileName)))+PATH_SEPARATOR, scene); + } else { + // write COLMAP input data + if (!OPT::strPointCloudFileName.empty() && !ImportPointCloud(MAKE_PATH_SAFE(OPT::strPointCloudFileName), scene)) + return EXIT_FAILURE; + Util::ensureFolderSlash(OPT::strOutputFileName); + ExportScene(MAKE_PATH_SAFE(OPT::strOutputFileName), scene, OPT::bForceSparsePointCloud); + } + VERBOSE("Input data exported: %u images & %u vertices (%s)", scene.images.size(), scene.vertices.size(), TD_TIMER_GET_FMT().c_str()); + } else { + // read COLMAP input data + Interface scene; + const String strOutFolder(Util::getFilePath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strOutputFileName))); + PointCloud pointcloud; + if (!ImportScene(MAKE_PATH_SAFE(OPT::strInputFileName), strOutFolder, scene, pointcloud)) + return EXIT_FAILURE; + // write MVS input data + Util::ensureFolder(strOutFolder); + if (!ARCHIVE::ofstream(scene, MAKE_PATH_SAFE(OPT::strOutputFileName))) + return EXIT_FAILURE; + if (!pointcloud.IsEmpty() && !pointcloud.Save(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName)) + _T(".ply"), true)) + return EXIT_FAILURE; + VERBOSE("Exported data: %u images, %u points%s (%s)", + scene.images.size(), scene.vertices.size(), pointcloud.IsEmpty()?"":String::FormatString(", %d dense points", pointcloud.GetSize()).c_str(), + TD_TIMER_GET_FMT().c_str()); + } + + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfaceCOLMAPTest/endian.h b/apps/InterfaceCOLMAPTest/endian.h new file mode 100644 index 0000000..12a22ca --- /dev/null +++ b/apps/InterfaceCOLMAPTest/endian.h @@ -0,0 +1,167 @@ +// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_ENDIAN_H_ +#define COLMAP_SRC_UTIL_ENDIAN_H_ + +#include +#include +#include + +namespace colmap { + +// Reverse the order of each byte. +template +T ReverseBytes(const T& data); + +// Check the order in which bytes are stored in computer memory. +bool IsLittleEndian(); +bool IsBigEndian(); + +// Convert data between endianness and the native format. Note that, for float +// and double types, these functions are only valid if the format is IEEE-754. +// This is the case for pretty much most processors. +template +T LittleEndianToNative(const T x); +template +T BigEndianToNative(const T x); +template +T NativeToLittleEndian(const T x); +template +T NativeToBigEndian(const T x); + +// Read data in little endian format for cross-platform support. +template +T ReadBinaryLittleEndian(std::istream* stream); +template +void ReadBinaryLittleEndian(std::istream* stream, std::vector* data); + +// Write data in little endian format for cross-platform support. +template +void WriteBinaryLittleEndian(std::ostream* stream, const T& data); +template +void WriteBinaryLittleEndian(std::ostream* stream, const std::vector& data); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +T ReverseBytes(const T& data) { + T data_reversed = data; + std::reverse(reinterpret_cast(&data_reversed), + reinterpret_cast(&data_reversed) + sizeof(T)); + return data_reversed; +} + +inline bool IsLittleEndian() { +#ifdef BOOST_BIG_ENDIAN + return false; +#else + return true; +#endif +} + +inline bool IsBigEndian() { +#ifdef BOOST_BIG_ENDIAN + return true; +#else + return false; +#endif +} + +template +T LittleEndianToNative(const T x) { + if (IsLittleEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T BigEndianToNative(const T x) { + if (IsBigEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T NativeToLittleEndian(const T x) { + if (IsLittleEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T NativeToBigEndian(const T x) { + if (IsBigEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T ReadBinaryLittleEndian(std::istream* stream) { + T data_little_endian; + stream->read(reinterpret_cast(&data_little_endian), sizeof(T)); + return LittleEndianToNative(data_little_endian); +} + +template +void ReadBinaryLittleEndian(std::istream* stream, std::vector* data) { + for (size_t i = 0; i < data->size(); ++i) { + (*data)[i] = ReadBinaryLittleEndian(stream); + } +} + +template +void WriteBinaryLittleEndian(std::ostream* stream, const T& data) { + const T data_little_endian = NativeToLittleEndian(data); + stream->write(reinterpret_cast(&data_little_endian), sizeof(T)); +} + +template +void WriteBinaryLittleEndian(std::ostream* stream, const std::vector& data) { + for (const auto& elem : data) { + WriteBinaryLittleEndian(stream, elem); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_ENDIAN_H_ + diff --git a/apps/InterfaceMVSNet/CMakeLists.txt b/apps/InterfaceMVSNet/CMakeLists.txt new file mode 100644 index 0000000..e38a82c --- /dev/null +++ b/apps/InterfaceMVSNet/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(InterfaceMVSNet "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS InterfaceMVSNet + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/InterfaceMVSNet/InterfaceMVSNet.cpp b/apps/InterfaceMVSNet/InterfaceMVSNet.cpp new file mode 100644 index 0000000..7081fb2 --- /dev/null +++ b/apps/InterfaceMVSNet/InterfaceMVSNet.cpp @@ -0,0 +1,706 @@ +/* + * InterfaceMVSNet.cpp + * + * Copyright (c) 2014-2021 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#define JSON_NOEXCEPTION +#include "../../libs/IO/json.hpp" +#include + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +// uncomment to enable multi-threading based on OpenMP +#ifdef _USE_OPENMP +#define MVSNET_USE_OPENMP +#endif + +#define APPNAME _T("InterfaceMVSNet") +#define MVS_FILE_EXTENSION _T(".mvs") +#define MVSNET_IMAGES_FOLDER _T("images") +#define MVSNET_CAMERAS_FOLDER _T("cams") +#define MVSNET_IMAGES_EXT _T(".jpg") +#define MVSNET_CAMERAS_NAME _T("_cam.txt") +#define RTMV_CAMERAS_EXT _T(".json") +#define NERFSTUDIO_TRANSFORMS _T("transforms.json") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { + String strInputFileName; + String strOutputFileName; + unsigned nArchiveType; + int nProcessPriority; + unsigned nMaxThreads; + String strConfigFileName; + boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Main options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses and image list") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the mesh") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-") + Util::getUniqueName(0) + _T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + const String strInputFileNameExt(Util::getFileExt(OPT::strInputFileName).ToLower()); + const bool bInvalidCommand(OPT::strInputFileName.empty()); + if (OPT::vm.count("help") || bInvalidCommand) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (bInvalidCommand) + return false; + + // initialize optional options + Util::ensureValidPath(OPT::strOutputFileName); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFileName(OPT::strInputFileName) + "scene" MVS_FILE_EXTENSION; + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +void ImageListParse(const LPSTR* argv, Point3& C) +{ + // read position vector + C.x = String::FromString(argv[0]); + C.y = String::FromString(argv[1]); + C.z = String::FromString(argv[2]); +} + +void ImageListParse(const LPSTR* argv, Matrix3x3& R) +{ + // read rotation matrix + R(0, 0) = String::FromString(argv[0]); + R(0, 1) = String::FromString(argv[1]); + R(0, 2) = String::FromString(argv[2]); + R(1, 0) = String::FromString(argv[3]); + R(1, 1) = String::FromString(argv[4]); + R(1, 2) = String::FromString(argv[5]); + R(2, 0) = String::FromString(argv[6]); + R(2, 1) = String::FromString(argv[7]); + R(2, 2) = String::FromString(argv[8]); +} + +void ImageListParse(const LPSTR* argv, Matrix3x4& P) +{ + // read projection matrix + P(0, 0) = String::FromString(argv[0]); + P(0, 1) = String::FromString(argv[1]); + P(0, 2) = String::FromString(argv[2]); + P(0, 3) = String::FromString(argv[3]); + P(1, 0) = String::FromString(argv[4]); + P(1, 1) = String::FromString(argv[5]); + P(1, 2) = String::FromString(argv[6]); + P(1, 3) = String::FromString(argv[7]); + P(2, 0) = String::FromString(argv[8]); + P(2, 1) = String::FromString(argv[9]); + P(2, 2) = String::FromString(argv[10]); + P(2, 3) = String::FromString(argv[11]); +} + +// convert a range-map to depth-map +void RangeToDepthMap(const Image32F& rangeMap, const Camera& camera, DepthMap& depthMap) +{ + depthMap.create(rangeMap.size()); + for (int y = 0; y < depthMap.rows; ++y) { + const float* const rangeRow = rangeMap.ptr(y); + float* const depthRow = depthMap.ptr(y); + for (int x = 0; x < depthMap.cols; ++x) { + const float range = rangeRow[x]; + depthRow[x] = (Depth)(range <= 0 ? 0 : normalized(camera.TransformPointI2C(Point2(x,y))).z*range); + } + } +} + +// parse scene stored in MVSNet format composed of undistorted images and camera poses +// for example see GigaVision benchmark (http://gigamvs.net): +// |--sceneX +// |--images +// |--xxx.jpg +// |--xxx.jpg +// .... +// |--cams +// |--xxx_cam.txt +// |--xxx_cam.txt +// .... +// |--render_cams +// |--xxx_cam.txt +// |--xxx_cam.txt +// .... +// +// where the camera parameter of one image stored in a cam.txt file contains the camera +// extrinsic E = [R|t], intrinsic K and the depth range: +// extrinsic +// E00 E01 E02 E03 +// E10 E11 E12 E13 +// E20 E21 E22 E23 +// E30 E31 E32 E33 +// +// intrinsic +// K00 K01 K02 +// K10 K11 K12 +// K20 K21 K22 +// +// DEPTH_MIN DEPTH_INTERVAL (DEPTH_NUM DEPTH_MAX) +bool ParseSceneMVSNet(Scene& scene, const String& strPath) +{ + #if defined(_SUPPORT_CPP17) && (!defined(__GNUC__) || (__GNUC__ > 7)) + IIndex prevPlatformID = NO_ID; + for (const std::filesystem::directory_entry& entry : std::filesystem::directory_iterator((strPath + MVSNET_IMAGES_FOLDER).c_str())) { + if (entry.path().extension() != MVSNET_IMAGES_EXT) + continue; + // parse camera + const std::string strCamFileName(strPath + MVSNET_CAMERAS_FOLDER PATH_SEPARATOR_STR + entry.path().stem().string().c_str() + MVSNET_CAMERAS_NAME); + std::ifstream fcam(strCamFileName); + if (!fcam) + continue; + String line; + do { + std::getline(fcam, line); + } while (line != "extrinsic" && fcam.good()); + String strP; + for (int i = 0; i < 3; ++i) { + std::getline(fcam, line); + strP += ' ' + line; + } + if (!fcam.good()) + continue; + size_t argc; + CAutoPtrArr argv; + argv = Util::CommandLineToArgvA(strP, argc); + if (argc != 12) + continue; + Matrix3x4 P; + ImageListParse(argv, P); + do { + std::getline(fcam, line); + } while (line != "intrinsic" && fcam.good()); + strP.clear(); + for (int i = 0; i < 3; ++i) { + std::getline(fcam, line); + strP += ' ' + line; + } + if (!fcam.good()) + continue; + argv = Util::CommandLineToArgvA(strP, argc); + if (argc != 9) + continue; + Matrix3x3 K; + ImageListParse(argv, K); + // setup camera + IIndex platformID; + if (prevPlatformID == NO_ID || !K.IsEqual(scene.platforms[prevPlatformID].cameras[0].K, 1e-3)) { + prevPlatformID = platformID = scene.platforms.size(); + Platform& platform = scene.platforms.emplace_back(); + Platform::Camera& camera = platform.cameras.emplace_back(); + camera.K = K; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + } else { + platformID = prevPlatformID; + } + Platform& platform = scene.platforms[platformID]; + // setup image + const IIndex ID = scene.images.size(); + Image& imageData = scene.images.emplace_back(); + imageData.platformID = platformID; + imageData.cameraID = 0; // only one camera per platform supported by this format + imageData.poseID = NO_ID; + imageData.ID = ID; + imageData.name = entry.path().string(); + Util::ensureUnifySlash(imageData.name); + imageData.name = MAKE_PATH_FULL(strPath, imageData.name); + // set image resolution + IMAGEPTR pimage = Image::ReadImageHeader(imageData.name); + imageData.width = pimage->GetWidth(); + imageData.height = pimage->GetHeight(); + imageData.scale = 1; + // set camera pose + imageData.poseID = platform.poses.size(); + Platform::Pose& pose = platform.poses.emplace_back(); + DecomposeProjectionMatrix(P, pose.R, pose.C); + imageData.camera = platform.GetCamera(imageData.cameraID, imageData.poseID); + } + if (scene.images.size() < 2) + return false; + scene.nCalibratedImages = (unsigned)scene.images.size(); + return true; + #else + VERBOSE("error: C++17 is required to parse MVSNet format"); + return false; + #endif // _SUPPORT_CPP17 +} + +// RTMV scene format: http://www.cs.umd.edu/~mmeshry/projects/rtmv +// |--sceneX +// |--images +// |--xxx.jpg +// |--xxx.jpg +// .... +// |--outputs (optional) +// |--depthxxxx.exr +// |--normalxxxx.exr +// .... +// |--transforms.json +bool ParseSceneNerfstudio(Scene& scene, const String& strPath) +{ + const nlohmann::json data = nlohmann::json::parse(std::ifstream(strPath + NERFSTUDIO_TRANSFORMS)); + if (data.empty()) + return false; + // parse camera + const cv::Size resolution(data["w"].get(), data["h"].get()); + const IIndex platformID = scene.platforms.size(); + Platform& platform = scene.platforms.emplace_back(); + Platform::Camera& camera = platform.cameras.emplace_back(); + camera.K = KMatrix::IDENTITY; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + camera.K(0,0) = data["fl_x"].get(); + camera.K(1,1) = data["fl_y"].get(); + camera.K(0,2) = data["cx"].get(); + camera.K(1,2) = data["cy"].get(); + const String cameraModel = data["camera_model"].get(); + if (cameraModel == "SIMPLE_PINHOLE") { + } else + // check ZERO radial distortion for all "PERSPECTIVE" type cameras + if (cameraModel == "PINHOLE" || cameraModel == "SIMPLE_RADIAL" || cameraModel == "RADIAL" || cameraModel == "OPENCV") { + const REAL k1 = data["k1"].get(); + const REAL k2 = data["k2"].get(); + const REAL p1 = data["p1"].get(); + const REAL p2 = data["p2"].get(); + if (k1 != 0 || k2 != 0 || p1 != 0 || p2 != 0) { + VERBOSE("error: radial distortion not supported"); + return false; + } + } else { + VERBOSE("error: camera model not supported"); + return false; + } + // parse images + const nlohmann::json& frames = data["frames"]; + for (const nlohmann::json& frame: frames) { + // set image + // frames expected to be ordered in JSON + const IIndex imageID = scene.images.size(); + const String strFileName(strPath + frame["file_path"].get().c_str()); + Image& imageData = scene.images.emplace_back(); + imageData.platformID = platformID; + imageData.cameraID = 0; // only one camera per platform supported by this format + imageData.poseID = NO_ID; + imageData.ID = imageID; + imageData.name = strFileName; + ASSERT(Util::isFullPath(imageData.name)); + // set image resolution + imageData.width = resolution.width; + imageData.height = resolution.height; + imageData.scale = 1; + // load camera pose + imageData.poseID = platform.poses.size(); + Platform::Pose& pose = platform.poses.emplace_back(); + const auto Ps = frame["transform_matrix"].get>>(); + Eigen::Matrix4d P{ + {Ps[0][0], Ps[0][1], Ps[0][2], Ps[0][3]}, + {Ps[1][0], Ps[1][1], Ps[1][2], Ps[1][3]}, + {Ps[2][0], Ps[2][1], Ps[2][2], Ps[2][3]}, + {Ps[3][0], Ps[3][1], Ps[3][2], Ps[3][3]} + }; + // revert nerfstudio conversion: + // convert from COLMAP's camera coordinate system (OpenCV) to ours (OpenGL) + // c2w[0:3, 1:3] *= -1 + // c2w = c2w[np.array([1, 0, 2, 3]), :] + // c2w[2, :] *= -1 + P.row(2) *= -1; + P.row(0).swap(P.row(1)); + P.col(2) *= -1; + P.col(1) *= -1; + // set camera pose + pose.R = P.topLeftCorner<3, 3>().transpose().eval(); + pose.R.EnforceOrthogonality(); + pose.C = P.topRightCorner<3, 1>().eval(); + imageData.camera = platform.GetCamera(imageData.cameraID, imageData.poseID); + // try reading depth-map and normal-map + DepthMap depthMap; { + const String depthPath(strPath + String::FormatString("outputs/depth%04u.exr", imageID)); + const Image32F rangeMap = cv::imread(depthPath, cv::IMREAD_UNCHANGED); + if (rangeMap.empty()) { + VERBOSE("Unable to load depthmap %s.", depthPath.c_str()); + continue; + } + RangeToDepthMap(rangeMap, imageData.camera, depthMap); + } + NormalMap normalMap; { + const String normalPath(strPath + String::FormatString("outputs/normal%04u.exr", imageID)); + normalMap = cv::imread(normalPath, cv::IMREAD_UNCHANGED); + if (normalMap.empty()) { + VERBOSE("Unable to load normalMap %s.", normalPath.c_str()); + continue; + } + } + const ConfidenceMap confMap; + const ViewsMap viewsMap; + const IIndexArr IDs = {imageID}; + double dMin, dMax; + cv::minMaxIdx(depthMap, &dMin, &dMax, NULL, NULL, depthMap > 0); + const String dmapPath(strPath + String::FormatString("depth%04u.dmap", imageID)); + if (!ExportDepthDataRaw(dmapPath, + imageData.name, IDs, resolution, + camera.K, pose.R, pose.C, + (float)dMin, (float)dMax, + depthMap, normalMap, confMap, viewsMap)) + { + VERBOSE("Unable to save dmap: %s", dmapPath.c_str()); + continue; + } + } + if (scene.images.size() < 2) + return false; + scene.nCalibratedImages = (unsigned)scene.images.size(); + return true; +} + +// RTMV scene format: http://www.cs.umd.edu/~mmeshry/projects/rtmv +// |--sceneX +// |--xxx.exr +// |--xxx.seg.exr +// |--xxx.depth.exr +// |--xxx.json +// .... +bool ParseSceneRTMV(Scene& scene, const String& strPath) +{ + const String strImagePath(strPath + "images/"); + Util::ensureFolder(strImagePath); + std::vector strImageNames; + #if defined(_SUPPORT_CPP17) && (!defined(__GNUC__) || (__GNUC__ > 7)) + for (const std::filesystem::directory_entry& entry : std::filesystem::directory_iterator(strPath.c_str())) { + if (entry.path().extension() != RTMV_CAMERAS_EXT) + continue; + strImageNames.emplace_back(entry.path().stem().string()); + } + #else + VERBOSE("error: C++17 is required to parse RTMV format"); + return false; + #endif // _SUPPORT_CPP17 + IIndex prevPlatformID = NO_ID; + scene.images.resize((IIndex)strImageNames.size()); + scene.platforms.reserve((IIndex)strImageNames.size()); + #ifdef MVSNET_USE_OPENMP + #pragma omp parallel for schedule(dynamic) + for (int_t i=0; i<(int_t)strImageNames.size(); ++i) { + #else + FOREACH(i, strImageNames) { + #endif + const IIndex imageID((IIndex)i); + const String& strImageName(strImageNames[imageID]); + // parse camera + const String strFileName(strPath + strImageName); + const nlohmann::json dataCamera = nlohmann::json::parse(std::ifstream(strFileName+RTMV_CAMERAS_EXT)); + if (dataCamera.empty()) + continue; + const nlohmann::json& data = dataCamera["camera_data"]; + const cv::Size resolution(data["width"].get(), data["height"].get()); + // set platform + Matrix3x3 K = Matrix3x3::IDENTITY; + K(0,0) = data["intrinsics"]["fx"].get(); + K(1,1) = data["intrinsics"]["fy"].get(); + K(0,2) = data["intrinsics"]["cx"].get(); + K(1,2) = data["intrinsics"]["cy"].get(); + IIndex platformID; + if (prevPlatformID == NO_ID || !K.IsEqual(scene.platforms[prevPlatformID].cameras[0].K, 1e-3)) { + #ifdef MVSNET_USE_OPENMP + #pragma omp critical + #endif + { + prevPlatformID = platformID = scene.platforms.size(); + Platform& platform = scene.platforms.emplace_back(); + Platform::Camera& camera = platform.cameras.emplace_back(); + platform.poses.reserve((IIndex)strImageNames.size()); + camera.K = K; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + } + } else { + platformID = prevPlatformID; + } + Platform& platform = scene.platforms[platformID]; + // set image + Image& imageData = scene.images[imageID]; + imageData.platformID = platformID; + imageData.cameraID = 0; // only one camera per platform supported by this format + imageData.poseID = NO_ID; + imageData.ID = imageID; + imageData.name = strImagePath+strImageName+".jpg"; + ASSERT(Util::isFullPath(imageData.name)); + { + cv::Mat image = cv::imread(strFileName+".exr", cv::IMREAD_UNCHANGED); + ASSERT(image.type() == CV_32FC4); + std::vector channels; + cv::split(image, channels); + cv::merge(std::vector{channels[0], channels[1], channels[2]}, image); + image.convertTo(imageData.image, CV_8UC3, 255); + } + ASSERT(resolution == imageData.image.size()); + if (imageData.image.empty()) { + VERBOSE("Unable to load image %s.", (strFileName+".exr").c_str()); + continue; + } + cv::imwrite(imageData.name, imageData.image); + imageData.ReleaseImage(); + // set image resolution + imageData.width = resolution.width; + imageData.height = resolution.height; + imageData.scale = 1; + // load camera pose + #ifdef MVSNET_USE_OPENMP + #pragma omp critical + #endif + { + imageData.poseID = platform.poses.size(); + platform.poses.emplace_back(); + } + Platform::Pose& pose = platform.poses[imageData.poseID]; + const auto Ps = data["cam2world"].get>>(); + Eigen::Matrix4d P{ + {Ps[0][0], Ps[1][0], Ps[2][0], Ps[3][0]}, + {Ps[0][1], Ps[1][1], Ps[2][1], Ps[3][1]}, + {Ps[0][2], Ps[1][2], Ps[2][2], Ps[3][2]}, + {Ps[0][3], Ps[1][3], Ps[2][3], Ps[3][3]} + }; + // apply the same transforms as nerfstudio converter + P.row(2) *= -1; + P.row(0).swap(P.row(1)); + P.col(2) *= -1; + P.col(1) *= -1; + // set camera pose + pose.R = P.topLeftCorner<3, 3>().transpose().eval(); + pose.R.EnforceOrthogonality(); + pose.C = P.topRightCorner<3, 1>().eval(); + imageData.camera = platform.GetCamera(imageData.cameraID, imageData.poseID); + // try reading the segmentation mask + { + cv::Mat imgMask = cv::imread(strFileName+".seg.exr", cv::IMREAD_UNCHANGED); + if (imgMask.empty()) { + VERBOSE("Unable to load segmentation mask %s.", (strFileName+".seg.exr").c_str()); + continue; + } + ASSERT(imgMask.type() == CV_32FC4); + ASSERT(resolution == imgMask.size()); + std::vector channels; + cv::split(imgMask, channels); + channels[0].convertTo(imgMask, CV_16U); + imageData.maskName = strImagePath+strImageName+".mask.png"; + cv::imwrite(imageData.maskName, imgMask); + } + // try reading the depth-map + DepthMap depthMap; { + const cv::Mat imgDepthMap = cv::imread(strFileName+".depth.exr", cv::IMREAD_UNCHANGED); + if (imgDepthMap.empty()) { + VERBOSE("Unable to load depthmap %s.", (strFileName+".depth.exr").c_str()); + continue; + } + ASSERT(imgDepthMap.type() == CV_32FC4); + ASSERT(resolution == imgDepthMap.size()); + std::vector channels; + cv::split(imgDepthMap, channels); + RangeToDepthMap(channels[0], imageData.camera, depthMap); + } + const NormalMap normalMap; + const ConfidenceMap confMap; + const ViewsMap viewsMap; + const IIndexArr IDs = {imageID}; + double dMin, dMax; + cv::minMaxIdx(depthMap, &dMin, &dMax, NULL, NULL, depthMap > 0); + const String dmapPath(strPath + String::FormatString("depth%04u.dmap", imageID)); + if (!ExportDepthDataRaw(dmapPath, + imageData.name, IDs, resolution, + K, pose.R, pose.C, + (float)dMin, (float)dMax, + depthMap, normalMap, confMap, viewsMap)) + { + VERBOSE("Unable to save dmap: %s", dmapPath.c_str()); + continue; + } + } + if (scene.images.size() < 2) + return false; + scene.nCalibratedImages = (unsigned)scene.images.size(); + return true; +} + +bool ParseScene(Scene& scene) +{ + #if defined(_SUPPORT_CPP17) && (!defined(__GNUC__) || (__GNUC__ > 7)) + String strPath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strInputFileName)); + Util::ensureValidFolderPath(strPath); + const std::filesystem::path path(static_cast(strPath)); + enum Type { + MVSNet = 0, + NERFSTUDIO, + RTMV, + } sceneType = MVSNet; + for (const std::filesystem::directory_entry& entry : std::filesystem::directory_iterator(path)) { + if (entry.path().extension() == RTMV_CAMERAS_EXT) { + if (entry.path().filename() == NERFSTUDIO_TRANSFORMS) { + sceneType = NERFSTUDIO; + break; + } + sceneType = RTMV; + } + } + switch (sceneType) { + case NERFSTUDIO: return ParseSceneNerfstudio(scene, strPath); + case RTMV: return ParseSceneRTMV(scene, strPath); + default: return ParseSceneMVSNet(scene, strPath); + } + #else + VERBOSE("error: C++17 is required to parse MVSNet format"); + return false; + #endif // _SUPPORT_CPP17 +} + +} // unnamed namespace + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + TD_TIMER_START(); + + Scene scene(OPT::nMaxThreads); + + // convert data from MVSNet format to OpenMVS + if (!ParseScene(scene)) + return EXIT_FAILURE; + + // write OpenMVS input data + scene.Save(MAKE_PATH_SAFE(OPT::strOutputFileName), (ARCHIVE_TYPE)OPT::nArchiveType); + + VERBOSE("Imported data: %u platforms, %u images, %u vertices (%s)", + scene.platforms.size(), scene.images.size(), scene.pointcloud.GetSize(), + TD_TIMER_GET_FMT().c_str()); + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfaceMetashape/CMakeLists.txt b/apps/InterfaceMetashape/CMakeLists.txt new file mode 100644 index 0000000..100e829 --- /dev/null +++ b/apps/InterfaceMetashape/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(InterfaceMetashape "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS InterfaceMetashape + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/InterfaceMetashape/InterfaceMetashape.cpp b/apps/InterfaceMetashape/InterfaceMetashape.cpp new file mode 100644 index 0000000..d618a65 --- /dev/null +++ b/apps/InterfaceMetashape/InterfaceMetashape.cpp @@ -0,0 +1,887 @@ +/* + * InterfaceMetashape.cpp + * + * Copyright (c) 2014-2021 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include "../../libs/IO/TinyXML2.h" +#include + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("InterfaceMetashape") // previously PhotoScan +#define MVS_FILE_EXTENSION _T(".mvs") +#define XML_EXT _T(".xml") +#define PLY_EXT _T(".ply") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strPointsFileName; +String strOutputFileName; +String strOutputImageFolder; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "imports SfM scene stored either in Metashape Agisoft/BlocksExchange or ContextCapture BlocksExchange XML format") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Main options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses and image list") + ("points-file,p", boost::program_options::value(&OPT::strPointsFileName), "input filename containing the 3D points") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the scene") + ("output-image-folder", boost::program_options::value(&OPT::strOutputImageFolder)->default_value("undistorted_images"), "output folder to store undistorted images") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strPointsFileName); + Util::ensureValidPath(OPT::strInputFileName); + Util::ensureValidFolderPath(OPT::strOutputImageFolder); + const bool bInvalidCommand(OPT::strInputFileName.empty()); + if (OPT::vm.count("help") || bInvalidCommand) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (bInvalidCommand) + return false; + + // initialize optional options + Util::ensureValidPath(OPT::strOutputFileName); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFileName(OPT::strInputFileName) + MVS_FILE_EXTENSION; + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +struct DistCoeff { + union { + REAL coeff[8]; + struct { + REAL k1, k2, p1, p2, k3, k4, k5, k6; + }; + }; + DistCoeff() : k1(0), k2(0), p1(0), p2(0), k3(0), k4(0), k5(0), k6(0) {} + bool HasDistortion() const { return k1 != 0 || k2 != 0 || k3 != 0 || k4 != 0 || k5 != 0 || k6 != 0; } +}; +typedef cList DistCoeffs; +typedef cList PlatformDistCoeffs; + +void ImageListParseC(const LPSTR* argv, Point3& C) +{ + // read position vector + C.x = String::FromString(argv[0]); + C.y = String::FromString(argv[1]); + C.z = String::FromString(argv[2]); +} + +void ImageListParseR(const LPSTR* argv, Matrix3x3& R) +{ + // read rotation matrix + R(0, 0) = String::FromString(argv[0]); + R(0, 1) = String::FromString(argv[1]); + R(0, 2) = String::FromString(argv[2]); + R(1, 0) = String::FromString(argv[3]); + R(1, 1) = String::FromString(argv[4]); + R(1, 2) = String::FromString(argv[5]); + R(2, 0) = String::FromString(argv[6]); + R(2, 1) = String::FromString(argv[7]); + R(2, 2) = String::FromString(argv[8]); +} + +void ImageListParseP(const LPSTR* argv, Matrix3x4& P) +{ + // read projection matrix + P(0, 0) = String::FromString(argv[0]); + P(0, 1) = String::FromString(argv[1]); + P(0, 2) = String::FromString(argv[2]); + P(0, 3) = String::FromString(argv[3]); + P(1, 0) = String::FromString(argv[4]); + P(1, 1) = String::FromString(argv[5]); + P(1, 2) = String::FromString(argv[6]); + P(1, 3) = String::FromString(argv[7]); + P(2, 0) = String::FromString(argv[8]); + P(2, 1) = String::FromString(argv[9]); + P(2, 2) = String::FromString(argv[10]); + P(2, 3) = String::FromString(argv[11]); +} + +// parse images list containing calibration and pose information +// and load the corresponding 3D point-cloud +bool ParseImageListXML(tinyxml2::XMLDocument& doc, Scene& scene, PlatformDistCoeffs& pltDistCoeffs, size_t& nCameras, size_t& nPoses) +{ + String strInputFileName(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strInputFileName)); + Util::ensureValidPath(strInputFileName); + tinyxml2::XMLElement* elem; + if (doc.ErrorID() != tinyxml2::XML_SUCCESS) + goto InvalidDocument; + { + tinyxml2::XMLElement* document = doc.FirstChildElement(_T("document"))->FirstChildElement(_T("chunk")); + if (document == NULL) + goto InvalidDocument; + { + bool bMetashapeFile(false); + CLISTDEF0(cv::Size) resolutions; + std::unordered_map mapPlatformID; + std::unordered_map mapImageID; + + // parse platform and camera models + { + tinyxml2::XMLElement* sensors = document->FirstChildElement(_T("sensors")); + if (sensors == NULL) + goto InvalidDocument; + { + for (tinyxml2::XMLElement* sensor=sensors->FirstChildElement(); sensor!=NULL; sensor=sensor->NextSiblingElement()) { + unsigned ID; + if (0 != _tcsicmp(sensor->Value(), _T("sensor")) || sensor->QueryUnsignedAttribute(_T("id"), &ID) != tinyxml2::XML_SUCCESS) + goto InvalidDocument; + { + // add new camera + enum CameraModel {METASHAPE=0, VSFM}; + int model(METASHAPE); + sensor->QueryIntAttribute(_T("model"), &model); + mapPlatformID.emplace(ID, scene.platforms.size()); + Platform& platform = scene.platforms.AddEmpty(); + LPCTSTR name; + if ((name=sensor->Attribute(_T("label"))) != NULL) + platform.name = name; + // parse intrinsics + tinyxml2::XMLElement* calibration = sensor->FirstChildElement(_T("calibration")); + if (calibration == NULL) + goto InvalidDocument; + { + if ((elem=calibration->FirstChildElement(_T("resolution"))) != NULL) { + resolutions.emplace_back( + elem->UnsignedAttribute(_T("width")), + elem->UnsignedAttribute(_T("height")) + ); + ASSERT(model == METASHAPE); + bMetashapeFile = true; + } + Platform::Camera& camera = platform.cameras.AddEmpty(); + camera.K = KMatrix::IDENTITY; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + DistCoeff& dc = pltDistCoeffs.AddEmpty().AddEmpty(); + for (elem=calibration->FirstChildElement(); elem!=NULL; elem=elem->NextSiblingElement()) { + if (0 == _tcsicmp(elem->Value(), _T("f"))) { + camera.K(0,0) = camera.K(1,1) = String::FromString(elem->GetText()); + } else + if (0 == _tcsicmp(elem->Value(), _T("fx"))) { + elem->QueryDoubleText(&camera.K(0,0)); + } else + if (0 == _tcsicmp(elem->Value(), _T("fy"))) { + elem->QueryDoubleText(&camera.K(1,1)); + } else + if (0 == _tcsicmp(elem->Value(), _T("cx"))) { + elem->QueryDoubleText(&camera.K(0,2)); + } else + if (0 == _tcsicmp(elem->Value(), _T("cy"))) { + elem->QueryDoubleText(&camera.K(1,2)); + } else + if (0 == _tcsicmp(elem->Value(), _T("k1"))) { + elem->QueryDoubleText(&dc.k1); + } else + if (0 == _tcsicmp(elem->Value(), _T("k2"))) { + elem->QueryDoubleText(&dc.k2); + } else + if (0 == _tcsicmp(elem->Value(), _T("k3"))) { + elem->QueryDoubleText(&dc.k3); + } else + if (0 == _tcsicmp(elem->Value(), _T("p1"))) { + elem->QueryDoubleText(&dc.p1); + } else + if (0 == _tcsicmp(elem->Value(), _T("p2"))) { + elem->QueryDoubleText(&dc.p2); + } else + if (0 == _tcsicmp(elem->Value(), _T("k4"))) { + elem->QueryDoubleText(&dc.k4); + } else + if (0 == _tcsicmp(elem->Value(), _T("k5"))) { + elem->QueryDoubleText(&dc.k5); + } else + if (0 == _tcsicmp(elem->Value(), _T("k6"))) { + elem->QueryDoubleText(&dc.k6); + } + } + if (bMetashapeFile) { + const cv::Size& resolution = resolutions.back(); + camera.K(0,2) += resolution.width*REAL(0.5); + camera.K(1,2) += resolution.height*REAL(0.5); + camera.K = camera.GetScaledK(REAL(1)/Camera::GetNormalizationScale(resolution.width, resolution.height)); + std::swap(dc.p1, dc.p2); + } + ++nCameras; + } + } + } + } + } + + // parse poses + { + tinyxml2::XMLElement* cameras = document->FirstChildElement(_T("cameras")); + if (cameras == NULL) + goto InvalidDocument; + { + PMatrix P; + size_t argc; + const String strPath(Util::getFilePath(strInputFileName)); + for (tinyxml2::XMLElement* camera=cameras->FirstChildElement(); camera!=NULL; camera=camera->NextSiblingElement()) { + unsigned ID; + if (0 != _tcsicmp(camera->Value(), _T("camera")) || camera->QueryUnsignedAttribute(_T("id"), &ID) != tinyxml2::XML_SUCCESS) + goto InvalidDocument; + { + // add new image + mapImageID.emplace(ID, scene.images.size()); + Image& imageData = scene.images.AddEmpty(); + LPCTSTR name; + if ((name=camera->Attribute(_T("type"))) != NULL && _tcsicmp(name, _T("frame")) != 0) { + DEBUG_EXTRA("warning: unsupported camera calibration '%s'", name); + continue; + } + if ((name=camera->Attribute(_T("label"))) != NULL) + imageData.name = name; + Util::ensureUnifySlash(imageData.name); + if (Util::getFileExt(imageData.name).empty()) + imageData.name += _T(".jpg"); + imageData.name = MAKE_PATH_FULL(strPath, imageData.name); + imageData.platformID = mapPlatformID.at(camera->UnsignedAttribute(_T("sensor_id"))); + imageData.cameraID = 0; // only one camera per platform supported by this format + imageData.ID = mapImageID.at(ID); + const cv::Size& resolution = resolutions[imageData.platformID]; + imageData.width = resolution.width; + imageData.height = resolution.height; + imageData.scale = 1; + if (!bMetashapeFile && !camera->BoolAttribute(_T("enabled"))) { + imageData.poseID = NO_ID; + DEBUG_EXTRA("warning: uncalibrated image '%s'", name); + continue; + } + // set pose + CAutoPtrArr argv; + if ((elem=camera->FirstChildElement(_T("transform"))) == NULL || + (argv=Util::CommandLineToArgvA(elem->GetText(), argc)) == NULL || + (argc != (bMetashapeFile ? 16 : 12))) + { + VERBOSE("Invalid image list camera: %u", ID); + continue; + } + Platform& platform = scene.platforms[imageData.platformID]; + imageData.poseID = platform.poses.size(); + Platform::Pose& pose = platform.poses.AddEmpty(); + ImageListParseP(argv, P); + DecomposeProjectionMatrix(P, pose.R, pose.C); + if (bMetashapeFile) { + pose.C = pose.R*(-pose.C); + pose.R = pose.R.t(); + } + imageData.camera = platform.GetCamera(imageData.cameraID, imageData.poseID); + ++nPoses; + } + } + scene.nCalibratedImages = (unsigned)nPoses; + } + + // parse bounding-box + { + tinyxml2::XMLElement* region = document->FirstChildElement(_T("region")); + if (region == NULL) + goto InvalidDocument; + { + size_t argc; + CAutoPtrArr argv; + Point3 C, E; Matrix3x3 R; + if ((elem=region->FirstChildElement(_T("center"))) == NULL || + (argv=Util::CommandLineToArgvA(elem->GetText(), argc)) == NULL || + argc != 3) + { + VERBOSE("Invalid image list region: %s", elem->GetText()); + goto InvalidDocument; + } + ImageListParseC(argv, C); + if ((elem=region->FirstChildElement(_T("size"))) == NULL || + (argv=Util::CommandLineToArgvA(elem->GetText(), argc)) == NULL || + argc != 3) + { + VERBOSE("Invalid image list region: %s", elem->GetText()); + goto InvalidDocument; + } + ImageListParseC(argv, E); + E *= REAL(0.5); + if ((elem=region->FirstChildElement(_T("R"))) == NULL || + (argv=Util::CommandLineToArgvA(elem->GetText(), argc)) == NULL || + argc != 9) + { + VERBOSE("Invalid image list region: %s", elem->GetText()); + goto InvalidDocument; + } + ImageListParseR(argv, R); + scene.obb.m_rot = Cast(R); + scene.obb.m_pos = Cast(C); + scene.obb.m_ext = Cast(E); + } + } + } + } + } + + return true; + InvalidDocument: + VERBOSE("Invalid camera list"); + return false; +} + +// parse scene stored in ContextCapture BlocksExchange format containing cameras, images and sparse point-cloud +bool ParseBlocksExchangeXML(tinyxml2::XMLDocument& doc, Scene& scene, PlatformDistCoeffs& pltDistCoeffs, size_t& nCameras, size_t& nPoses) { + String strInputFileName(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strInputFileName)); + Util::ensureValidPath(strInputFileName); + const tinyxml2::XMLElement* blocksExchange; + const tinyxml2::XMLElement* document; + const tinyxml2::XMLElement* photogroups; + if (doc.ErrorID() != tinyxml2::XML_SUCCESS || + (blocksExchange=doc.FirstChildElement("BlocksExchange")) == NULL || + (document=blocksExchange->FirstChildElement("Block")) == NULL || + (photogroups=document->FirstChildElement("Photogroups")) == NULL) { + VERBOSE("error: invalid scene file"); + return false; + } + CLISTDEF0(cv::Size) resolutions; + std::unordered_map mapImageID; + const String strPath(Util::getFilePath(strInputFileName)); + const tinyxml2::XMLElement* elem; + for (const tinyxml2::XMLElement* photogroup=photogroups->FirstChildElement(); photogroup!=NULL; photogroup=photogroup->NextSiblingElement()) { + if ((elem=photogroup->FirstChildElement("CameraModelType")) == NULL || + std::strcmp(elem->GetText(), "Perspective") != 0) + continue; + if ((elem=photogroup->FirstChildElement("ImageDimensions")) == NULL) + continue; + const IIndex platformID = scene.platforms.size(); + Platform& platform = scene.platforms.AddEmpty(); + platform.name = photogroup->FirstChildElement("Name")->GetText(); + resolutions.emplace_back( + elem->FirstChildElement("Width")->UnsignedText(), + elem->FirstChildElement("Height")->UnsignedText() + ); + // parse camera + Platform::Camera& camera = platform.cameras.AddEmpty(); + camera.K = KMatrix::IDENTITY; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + const float resolutionScale = Camera::GetNormalizationScale(resolutions.back().width, resolutions.back().height); + if ((elem=photogroup->FirstChildElement("FocalLengthPixels")) != NULL) { + camera.K(0,0) = camera.K(1,1) = photogroup->FirstChildElement("FocalLengthPixels")->DoubleText(); + } else { + camera.K(0,0) = camera.K(1,1) = photogroup->FirstChildElement("FocalLength")->DoubleText() * resolutionScale / photogroup->FirstChildElement("SensorSize")->DoubleText(); + } + if ((elem=photogroup->FirstChildElement("PrincipalPoint")) != NULL) { + camera.K(0,2) = elem->FirstChildElement("x")->DoubleText(); + camera.K(1,2) = elem->FirstChildElement("y")->DoubleText(); + } else { + camera.K(0,2) = resolutions.back().width*REAL(0.5); + camera.K(1,2) = resolutions.back().height*REAL(0.5); + } + if ((elem=photogroup->FirstChildElement("AspectRatio")) != NULL) + camera.K(1,1) *= elem->DoubleText(); + if ((elem=photogroup->FirstChildElement("Skew")) != NULL) + camera.K(0,1) = elem->DoubleText(); + camera.K = camera.GetScaledK(REAL(1)/resolutionScale); + // parse distortion parameters + DistCoeff& dc = pltDistCoeffs.AddEmpty().AddEmpty(); { + const tinyxml2::XMLElement* distortion=photogroup->FirstChildElement("Distortion"); + if (distortion) { + if ((elem=distortion->FirstChildElement("K1")) != NULL) + dc.k1 = elem->DoubleText(); + if ((elem=distortion->FirstChildElement("K2")) != NULL) + dc.k2 = elem->DoubleText(); + if ((elem=distortion->FirstChildElement("K3")) != NULL) + dc.k3 = elem->DoubleText(); + if ((elem=distortion->FirstChildElement("P1")) != NULL) + dc.p2 = elem->DoubleText(); + if ((elem=distortion->FirstChildElement("P2")) != NULL) + dc.p1 = elem->DoubleText(); + } + } + ++nCameras; + for (const tinyxml2::XMLElement* photo=photogroup->FirstChildElement("Photo"); photo!=NULL; photo=photo->NextSiblingElement()) { + const IIndex idxImage = scene.images.size(); + Image& imageData = scene.images.AddEmpty(); + imageData.platformID = platformID; + imageData.cameraID = 0; // only one camera per platform supported by this format + imageData.poseID = NO_ID; + imageData.ID = photo->FirstChildElement("Id")->UnsignedText(); + imageData.name = photo->FirstChildElement("ImagePath")->GetText(); + Util::ensureUnifySlash(imageData.name); + imageData.name = MAKE_PATH_FULL(strPath, imageData.name); + mapImageID.emplace(imageData.ID, idxImage); + // set image resolution + const cv::Size& resolution = resolutions[imageData.platformID]; + imageData.width = resolution.width; + imageData.height = resolution.height; + imageData.scale = 1; + // set camera pose + const tinyxml2::XMLElement* photoPose = photo->FirstChildElement("Pose"); + if (photoPose == NULL) + continue; + if ((elem=photoPose->FirstChildElement("Rotation")) == NULL) + continue; + imageData.poseID = platform.poses.size(); + Platform::Pose& pose = platform.poses.AddEmpty(); + pose.R = Matrix3x3( + elem->FirstChildElement("M_00")->DoubleText(), + elem->FirstChildElement("M_01")->DoubleText(), + elem->FirstChildElement("M_02")->DoubleText(), + elem->FirstChildElement("M_10")->DoubleText(), + elem->FirstChildElement("M_11")->DoubleText(), + elem->FirstChildElement("M_12")->DoubleText(), + elem->FirstChildElement("M_20")->DoubleText(), + elem->FirstChildElement("M_21")->DoubleText(), + elem->FirstChildElement("M_22")->DoubleText()); + if ((elem=photoPose->FirstChildElement("Center")) == NULL) + continue; + pose.C = Point3( + elem->FirstChildElement("x")->DoubleText(), + elem->FirstChildElement("y")->DoubleText(), + elem->FirstChildElement("z")->DoubleText()); + imageData.camera = platform.GetCamera(imageData.cameraID, imageData.poseID); + // set depth stats + if ((elem=photo->FirstChildElement("MedianDepth")) != NULL) + imageData.avgDepth = (float)elem->DoubleText(); + else if (photo->FirstChildElement("NearDepth") != NULL && photo->FirstChildElement("FarDepth") != NULL) + imageData.avgDepth = (float)((photo->FirstChildElement("NearDepth")->DoubleText() + photo->FirstChildElement("FarDepth")->DoubleText())/2); + else + imageData.avgDepth = 0; + ++nPoses; + } + } + if (scene.images.size() < 2) + return false; + scene.nCalibratedImages = (unsigned)nPoses; + // transform poses to a local coordinate system + const bool bLocalCoords(document->FirstChildElement("SRSId") == NULL || + ((elem=blocksExchange->FirstChildElement("SpatialReferenceSystems")) != NULL && (elem=elem->FirstChildElement("SRS")) != NULL && (elem=elem->FirstChildElement("Name")) != NULL && _tcsncmp(elem->GetText(), "Local Coordinates", 17) == 0)); + Point3 center = Point3::ZERO; + if (!bLocalCoords) { + for (const Image& imageData : scene.images) + center += imageData.camera.C; + center /= scene.images.size(); + for (Platform& platform : scene.platforms) + for (Platform::Pose& pose : platform.poses) + pose.C -= center; + } + // try to read also the sparse point-cloud + const tinyxml2::XMLElement* tiepoints = document->FirstChildElement("TiePoints"); + if (tiepoints == NULL) + return true; + for (const tinyxml2::XMLElement* tiepoint=tiepoints->FirstChildElement(); tiepoint!=NULL; tiepoint=tiepoint->NextSiblingElement()) { + if ((elem=tiepoint->FirstChildElement("Position")) == NULL) + continue; + scene.pointcloud.points.emplace_back( + (float)elem->FirstChildElement("x")->DoubleText(), + (float)elem->FirstChildElement("y")->DoubleText(), + (float)elem->FirstChildElement("z")->DoubleText()); + if (!bLocalCoords) + scene.pointcloud.points.back() -= Cast(center); + if ((elem=tiepoint->FirstChildElement("Color")) != NULL) + scene.pointcloud.colors.emplace_back( + (uint8_t)CLAMP(elem->FirstChildElement("Red")->DoubleText()*255, 0.0, 255.0), + (uint8_t)CLAMP(elem->FirstChildElement("Green")->DoubleText()*255, 0.0, 255.0), + (uint8_t)CLAMP(elem->FirstChildElement("Blue")->DoubleText()*255, 0.0, 255.0)); + PointCloud::ViewArr views; + for (const tinyxml2::XMLElement* view=tiepoint->FirstChildElement("Measurement"); view!=NULL; view=view->NextSiblingElement()) + views.emplace_back(mapImageID.at(view->FirstChildElement("PhotoId")->UnsignedText())); + scene.pointcloud.pointViews.emplace_back(std::move(views)); + } + return true; +} + +// parse scene stored either in Metashape images list format or ContextCapture BlocksExchange format +bool ParseSceneXML(Scene& scene, PlatformDistCoeffs& pltDistCoeffs, size_t& nCameras, size_t& nPoses) +{ + // parse XML file + const String strInputFileName(MAKE_PATH_SAFE(OPT::strInputFileName)); + tinyxml2::XMLDocument doc; { + ISTREAMPTR pStream(new File(strInputFileName, File::READ, File::OPEN)); + if (!((File*)(ISTREAM*)pStream)->isOpen()) { + VERBOSE("error: failed opening the input scene file"); + return false; + } + const size_t nLen(pStream->getSize()); + String str; str.resize(nLen); + pStream->read(&str[0], nLen); + doc.Parse(str.c_str(), nLen); + } + if (doc.ErrorID() != tinyxml2::XML_SUCCESS) { + VERBOSE("error: invalid XML file"); + return false; + } + // parse scene + if (doc.FirstChildElement("BlocksExchange") == NULL) + return ParseImageListXML(doc, scene, pltDistCoeffs, nCameras, nPoses); + return ParseBlocksExchangeXML(doc, scene, pltDistCoeffs, nCameras, nPoses); +} + +// undistort image using Brown's model +bool UndistortBrown(Image& imageData, uint32_t ID, const DistCoeff& dc, const String& pathData) +{ + // do we need to undistort? + if (!dc.HasDistortion()) + return true; + + // load image pixels + if (!imageData.ReloadImage()) + return false; + + // initialize intrinsics + const cv::Vec& distCoeffs = *reinterpret_cast*>(dc.coeff); + const KMatrix prevK(imageData.camera.GetK(imageData.width, imageData.height)); + #if 1 + const KMatrix& K(prevK); + #else + const KMatrix K(cv::getOptimalNewCameraMatrix(prevK, distCoeffs, imageData.size(), 0.0, cv::Size(), NULL, true)); + ASSERT(K(0,2) == Camera::ComposeK(prevK(0,0), prevK(1,1), imageData.width(), imageData.height())(0,2)); + ASSERT(K(1,2) == Camera::ComposeK(prevK(0,0), prevK(1,1), imageData.width(), imageData.height())(1,2)); + if (K.IsEqual(prevK)) { + int i(0); + while (distCoeffs(i++) == 0.0) { + if (i == 8) + return true; // nothing to do + } + } + #endif + + // undistort image + Image8U3 imgUndist; + cv::undistort(imageData.image, imgUndist, prevK, distCoeffs, K); + imageData.ReleaseImage(); + + // save undistorted image + imageData.image = imgUndist; + imageData.name = pathData + String::FormatString(_T("%05u.jpg"), ID); + Util::ensureFolder(imageData.name); + return imageData.image.Save(imageData.name); +} + +// project all points in this image and keep those looking at the camera and are most in front +void AssignPoints(const Image& imageData, uint32_t ID, PointCloud& pointcloud) +{ + ASSERT(pointcloud.IsValid()); + const int CHalfSize(1); + const int FHalfSize(5); + const Depth thCloseDepth(0.1f); + + // sort points by depth + IndexScoreArr points(0, pointcloud.points.size()); + FOREACH(p, pointcloud.points) { + const PointCloud::Point& X(pointcloud.points[p]); + const float d((float)imageData.camera.PointDepth(X)); + if (d <= 0) + continue; + points.emplace_back((uint32_t)p, d); + } + points.Sort(); + + // project all points to this view + DepthMap depthMap(imageData.GetSize()); + TImage pointMap(imageData.GetSize()); + depthMap.fill(FLT_MAX); + pointMap.memset((uint8_t)NO_ID); + RFOREACHPTR(pPD, points) { + const Point3 X(pointcloud.points[pPD->idx]); + const Point3f Xc(imageData.camera.TransformPointW2C(X)); + // (also the view to point vector cause the face is in camera view space) + // point skip already in the previous step if the (cos) angle between + // the view to point vector and the view direction is negative + ASSERT(Xc.z > 0); + // skip point if the (cos) angle between + // its normal and the point to view vector is negative + if (!pointcloud.normals.empty() && Xc.dot(pointcloud.normals[pPD->idx]) > 0) + continue; + const Point2f x(imageData.camera.TransformPointC2I(Xc)); + const ImageRef ir(ROUND2INT(x)); + if (!depthMap.isInside(ir)) + continue; + // skip point if the there is a very near by point closer + for (int i=-CHalfSize; i<=CHalfSize; ++i) { + const int rw(ir.y+i); + for (int j=-CHalfSize; j<=CHalfSize; ++j) { + const int cw(ir.x+j); + if (!depthMap.isInside(ImageRef(cw,rw))) + continue; + if (depthMap(rw,cw) < Xc.z) + goto NEXT_POINT; + } + } + // skip the point if there is a near by point much closer + for (int i=-FHalfSize; i<=FHalfSize; ++i) { + const int rw(ir.y+i); + for (int j=-FHalfSize; j<=FHalfSize; ++j) { + const int cw(ir.x+j); + if (!depthMap.isInside(ImageRef(cw,rw))) + continue; + const Depth depth(depthMap(rw,cw)); + if (depth < Xc.z && !IsDepthSimilar(depth, Xc.z, thCloseDepth)) + goto NEXT_POINT; + } + } + // store this point + depthMap(ir) = Xc.z; + pointMap(ir) = pPD->idx; + NEXT_POINT:; + } + + // add all points viewed by this camera + const int HalfSize(1); + const int RowsEnd(pointMap.rows-HalfSize); + const int ColsEnd(pointMap.cols-HalfSize); + unsigned nNumPoints(0); + #ifdef _USE_OPENMP + #pragma omp critical + #endif + for (int r=HalfSize; r at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + TD_TIMER_START(); + + Scene scene(OPT::nMaxThreads); + + // convert data from Metashape format to OpenMVS + PlatformDistCoeffs pltDistCoeffs; + size_t nCameras(0), nPoses(0); + if (!ParseSceneXML(scene, pltDistCoeffs, nCameras, nPoses)) + return EXIT_FAILURE; + + // read the 3D point-cloud if available + if (!OPT::strPointsFileName.empty() && !scene.pointcloud.Load(MAKE_PATH_SAFE(OPT::strPointsFileName))) + return EXIT_FAILURE; + const bool bAssignPoints(!scene.pointcloud.IsEmpty() && !scene.pointcloud.IsValid()); + if (bAssignPoints) + scene.pointcloud.pointViews.resize(scene.pointcloud.GetSize()); + + // undistort images + const String pathData(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strOutputImageFolder)); + Util::Progress progress(_T("Processed images"), scene.images.size()); + GET_LOGCONSOLE().Pause(); + #ifdef _USE_OPENMP + bool bAbort(false); + #pragma omp parallel for shared(bAbort) schedule(dynamic) + for (int ID=0; ID<(int)scene.images.size(); ++ID) { + #pragma omp flush (bAbort) + if (bAbort) + continue; + #else + FOREACH(ID, scene.images) { + #endif + ++progress; + Image& imageData = scene.images[ID]; + if (!imageData.IsValid()) + continue; + if (!UndistortBrown(imageData, ID, pltDistCoeffs[imageData.platformID][imageData.cameraID], pathData)) { + #ifdef _USE_OPENMP + bAbort = true; + #pragma omp flush (bAbort) + continue; + #else + return EXIT_FAILURE; + #endif + } + imageData.UpdateCamera(scene.platforms); + if (bAssignPoints) + AssignPoints(imageData, ID, scene.pointcloud); + } + GET_LOGCONSOLE().Play(); + #ifdef _USE_OPENMP + if (bAbort) + return EXIT_FAILURE; + #endif + progress.close(); + + if (scene.pointcloud.IsValid()) { + // filter invalid points + RFOREACH(i, scene.pointcloud.points) + if (scene.pointcloud.pointViews[i].size() < 2) + scene.pointcloud.RemovePoint(i); + // compute average scene depth per image + if (!std::any_of(scene.images.begin(), scene.images.end(), [](const Image& imageData) { return imageData.avgDepth > 0; })) { + std::vector avgDepths(scene.images.size(), 0.f); + std::vector numDepths(scene.images.size(), 0u); + FOREACH(idxPoint, scene.pointcloud.points) { + const Point3 X(scene.pointcloud.points[idxPoint]); + for (const PointCloud::View& idxImage: scene.pointcloud.pointViews[idxPoint]) { + const Image& imageData = scene.images[idxImage]; + const float depth((float)imageData.camera.PointDepth(X)); + if (depth > 0) { + avgDepths[idxImage] += depth; + ++numDepths[idxImage]; + } + } + } + FOREACH(idxImage, scene.images) { + Image& imageData = scene.images[idxImage]; + if (numDepths[idxImage] > 0) + imageData.avgDepth = avgDepths[idxImage] / numDepths[idxImage]; + } + } + } + + // print average scene depth per image stats + MeanStdMinMax acc; + for (const Image& imageData: scene.images) + if (imageData.avgDepth > 0) + acc.Update(imageData.avgDepth); + + // write OpenMVS input data + scene.Save(MAKE_PATH_SAFE(OPT::strOutputFileName), (ARCHIVE_TYPE)OPT::nArchiveType); + + VERBOSE("Exported data: %u platforms, %u cameras, %u poses, %u images, %u vertices, %g min / %g mean (%g std) / %g max average scene depth per image (%s)", + scene.platforms.size(), nCameras, nPoses, scene.images.size(), scene.pointcloud.GetSize(), + acc.minVal, acc.GetMean(), acc.GetStdDev(), acc.maxVal, + TD_TIMER_GET_FMT().c_str()); + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfaceOpenMVG/CMakeLists.txt b/apps/InterfaceOpenMVG/CMakeLists.txt new file mode 100644 index 0000000..6377497 --- /dev/null +++ b/apps/InterfaceOpenMVG/CMakeLists.txt @@ -0,0 +1,23 @@ +FIND_PACKAGE(OpenMVG QUIET) +IF (OPENMVG_FOUND) + INCLUDE_DIRECTORIES(${OPENMVG_INCLUDE_DIRS}) + add_definitions(-D_USE_OPENMVG) + set(LIBS_DEPEND "MVS;${OPENMVG_LIBRARIES}") +ELSE() + set(LIBS_DEPEND "MVS") + MESSAGE("OPENMVG_NOT FOUND : OpenMVG importer with JSON support will not be build") +ENDIF() + +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(InterfaceOpenMVG "Apps" "${cxx_default}" "${LIBS_DEPEND};${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS InterfaceOpenMVG + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/InterfaceOpenMVG/InterfaceOpenMVG.cpp b/apps/InterfaceOpenMVG/InterfaceOpenMVG.cpp new file mode 100644 index 0000000..9361af0 --- /dev/null +++ b/apps/InterfaceOpenMVG/InterfaceOpenMVG.cpp @@ -0,0 +1,755 @@ +/* + * InterfaceOpenMVG.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * Pierre MOULON + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include +#ifdef _USE_OPENMVG +#undef D2R +#undef R2D +#include +#include +#include +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("InterfaceOpenMVG") +#define MVS_EXT _T(".mvs") +#define MVG_EXT _T(".baf") +#define MVG2_EXT _T(".json") +#define MVG3_EXT _T(".bin") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace openMVS { +namespace MVS_IO { + +typedef REAL RealT; +typedef Eigen::Matrix Mat33; +typedef Eigen::Matrix Vec3; + +// Structure to model the pinhole camera projection model +struct Camera +{ + Mat33 K; // camera's normalized intrinsics matrix +}; +typedef std::vector vec_Camera; + +// structure describing a pose along the trajectory of a platform +struct Pose { + Mat33 R; // pose's rotation matrix + Vec3 C; // pose's translation vector +}; +typedef std::vector vec_Pose; + +// structure describing an image +struct Image { + uint32_t id_camera; // ID of the associated camera on the associated platform + uint32_t id_pose; // ID of the pose of the associated platform + std::string name; // image file name +}; +typedef std::vector vec_Image; + +// structure describing a 3D point +struct Vertex { + + typedef std::vector vec_View; + + Vec3 X; // 3D point position + vec_View views; // view visibility for this 3D feature +}; +typedef std::vector vec_Vertex; + +struct SfM_Scene +{ + vec_Pose poses; // array of poses + vec_Camera cameras; // array of cameras + vec_Image images; // array of images + vec_Vertex vertices; // array of reconstructed 3D points +}; + + +bool ImportScene(const std::string& sList_filename, const std::string& sBaf_filename, SfM_Scene& sceneBAF) +{ + LOG_OUT() << "Reading:\n" + << sList_filename << "\n" + << sBaf_filename << std::endl; + + // Read view list file (view filename, id_intrinsic, id_pose) + // Must be read first, since it allow to establish the link between the ViewId and the camera/poses ids. + std::map< std::pair, uint32_t > map_cam_pose_toViewId; + { + std::ifstream file(sList_filename.c_str()); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", sList_filename.c_str()); + return false; + } + Image image; + uint32_t count = 0; + while (file >> image.name >> image.id_camera >> image.id_pose) { + sceneBAF.images.push_back(image); + map_cam_pose_toViewId[std::make_pair(image.id_camera, image.id_pose)] = count++; + LOG_OUT() << image.name << ' ' << image.id_camera << ' ' << image.id_pose << std::endl; + } + } + + // Read BAF file + { + std::ifstream file(sBaf_filename.c_str()); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", sBaf_filename.c_str()); + return false; + } + + uint32_t num_intrinsics, num_poses, num_points; + + // Read header + file >> num_intrinsics; + file >> num_poses; + file >> num_points; + + LOG_OUT() << "Reading BAF file with:\n" + << " num_intrinsics: " << num_intrinsics << "\n" + << " num_poses: " << num_poses << "\n" + << " num_points: " << num_points << "\n"; + + // Read the intrinsics (only support reading Pinhole Radial 3). + { + for (uint32_t i = 0; i < num_intrinsics; ++i) { + double focal, ppx, ppy, k1, k2, k3; + file >> focal >> ppx >> ppy >> k1 >> k2 >> k3; + Camera cam; + cam.K << + focal, 0, ppx, + 0, focal, ppy, + 0, 0, 1; + LOG_OUT() << "\n" << cam.K << std::endl; + sceneBAF.cameras.push_back(cam); + } + } + + // Read poses + { + for (uint32_t i = 0; i < num_poses; ++i) { + Pose pose; + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + file >> pose.R(r,c); + } + } + file >> pose.C[0] >> pose.C[1] >> pose.C[2]; + #ifndef _RELEASE + LOG_OUT() << "\n" << pose.R << "\n\n" << pose.C.transpose() << std::endl; + #endif + sceneBAF.poses.push_back(pose); + } + } + + // Read structure and visibility + { + #ifdef _RELEASE + Util::Progress progress(_T("Processed points"), num_points); + #endif + for (uint32_t i = 0; i < num_points; ++i) { + Vertex vertex; + file >> vertex.X[0] >> vertex.X[1] >> vertex.X[2]; + uint32_t num_observations_for_point = 0; + file >> num_observations_for_point; + for (uint32_t j = 0; j < num_observations_for_point; ++j) { + uint32_t id_intrinsics, id_pose; + double x, y; + file >> id_intrinsics >> id_pose >> x >> y; + #ifndef _RELEASE + LOG_OUT() << "observation:" + << " " << id_intrinsics + << " " << id_pose + << " " << x << " " << y << std::endl; + #endif + const auto itIntrPose(map_cam_pose_toViewId.find(std::make_pair(id_intrinsics, id_pose))); + if (itIntrPose == map_cam_pose_toViewId.end()) { + LOG_OUT() << "error: intrinsics-pose pair not existing" << std::endl; + continue; + } + const uint32_t id_view(itIntrPose->second); + vertex.views.push_back(id_view); + } + sceneBAF.vertices.push_back(vertex); + #ifdef _RELEASE + progress.display(i); + #endif + } + #ifdef _RELEASE + progress.close(); + #endif + } + } + return true; +} + +bool ExportScene(const std::string& sList_filename, const std::string& sBaf_filename, const SfM_Scene& sceneBAF) +{ + LOG_OUT() << "Writing:\n" + << sList_filename << "\n" + << sBaf_filename << std::endl; + + // Write view list file (view filename, id_intrinsic, id_pose) + { + std::ofstream file(sList_filename.c_str()); + if (!file.good()) { + VERBOSE("error: unable to open file '%s'", sList_filename.c_str()); + return false; + } + for (uint32_t i=0; i(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_DEFAULT), "project archive type: 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Main options"); + config.add_options() + ("images-list-file,l", boost::program_options::value(&OPT::strListFileName), "input filename containing image list") + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses and image list") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the mesh") + ("output-image-folder", boost::program_options::value(&OPT::strOutputImageFolder)->default_value("undistorted_images"), "output folder to store undistorted images") + ("normalize,f", boost::program_options::value(&OPT::bNormalizeIntrinsics)->default_value(true), "normalize intrinsics while exporting to OpenMVS format") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strListFileName); + Util::ensureUnifySlash(OPT::strListFileName); + Util::ensureValidPath(OPT::strInputFileName); + Util::ensureUnifySlash(OPT::strInputFileName); + Util::ensureUnifySlash(OPT::strOutputImageFolder); + Util::ensureFolderSlash(OPT::strOutputImageFolder); + const String strInputFileNameExt(Util::getFileExt(OPT::strInputFileName).ToLower()); + OPT::bOpenMVS2OpenMVG = (strInputFileNameExt == MVS_FILE_EXTENSION); + #ifdef _USE_OPENMVG + OPT::bOpenMVGjson = (strInputFileNameExt == MVG2_EXT || strInputFileNameExt == MVG3_EXT); + const bool bInvalidCommand(OPT::strInputFileName.IsEmpty() || (OPT::strListFileName.IsEmpty() && !OPT::bOpenMVGjson && !OPT::bOpenMVS2OpenMVG)); + #else + const bool bInvalidCommand(OPT::strInputFileName.IsEmpty() || (OPT::strListFileName.IsEmpty() && !OPT::bOpenMVS2OpenMVG)); + #endif + if (OPT::vm.count("help") || bInvalidCommand) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + #ifndef _USE_OPENMVG + GET_LOG() << "\nWARNING: Only " MVG_EXT " files supported! In order to import " MVG2_EXT " or " MVG3_EXT " files use the converter included in OpenMVG, or link OpenMVS to the latest version of OpenMVG during compile time.\n"; + #endif + } + if (bInvalidCommand) + return false; + + // initialize optional options + Util::ensureValidPath(OPT::strOutputFileName); + Util::ensureUnifySlash(OPT::strOutputFileName); + if (OPT::bOpenMVS2OpenMVG) { + if (OPT::strOutputFileName.IsEmpty()) + OPT::strOutputFileName = Util::getFileFullName(OPT::strInputFileName); + } else { + if (OPT::strOutputFileName.IsEmpty()) + OPT::strOutputFileName = Util::getFileFullName(OPT::strInputFileName) + MVS_FILE_EXTENSION; + } + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + TD_TIMER_START(); + + if (OPT::bOpenMVS2OpenMVG) { + // read OpenMVS input data + MVS::Scene scene(OPT::nMaxThreads); + if (!scene.Load(MAKE_PATH_SAFE(OPT::strInputFileName))) + return EXIT_FAILURE; + + // convert data from OpenMVS to OpenMVG + openMVS::MVS_IO::SfM_Scene sceneBAF; + FOREACH(p, scene.platforms) { + const MVS::Platform& platform = scene.platforms[p]; + if (platform.cameras.GetSize() != 1) { + LOG("error: unsupported scene structure"); + return EXIT_FAILURE; + } + const MVS::Platform::Camera& camera = platform.cameras[0]; + openMVS::MVS_IO::Camera cameraBAF; + cameraBAF.K = camera.K; + sceneBAF.cameras.push_back(cameraBAF); + } + FOREACH(i, scene.images) { + const MVS::Image& image = scene.images[i]; + const MVS::Platform& platform = scene.platforms[image.platformID]; + const MVS::Platform::Pose& pose = platform.poses[image.poseID]; + openMVS::MVS_IO::Image imageBAF; + imageBAF.name = image.name; + imageBAF.name = MAKE_PATH_REL(WORKING_FOLDER_FULL, imageBAF.name); + imageBAF.id_camera = image.platformID; + imageBAF.id_pose = (uint32_t)sceneBAF.poses.size(); + sceneBAF.images.push_back(imageBAF); + openMVS::MVS_IO::Pose poseBAF; + poseBAF.R = pose.R; + poseBAF.C = pose.C; + sceneBAF.poses.push_back(poseBAF); + } + sceneBAF.vertices.reserve(scene.pointcloud.points.GetSize()); + FOREACH(p, scene.pointcloud.points) { + const MVS::PointCloud::Point& point = scene.pointcloud.points[p]; + openMVS::MVS_IO::Vertex vertexBAF; + vertexBAF.X = ((const MVS::PointCloud::Point::EVec)point).cast(); + const MVS::PointCloud::ViewArr& views = scene.pointcloud.pointViews[p]; + FOREACH(v, views) { + unsigned viewBAF = views[(uint32_t)v]; + vertexBAF.views.push_back(viewBAF); + } + sceneBAF.vertices.push_back(vertexBAF); + } + + // write OpenMVG input data + const String strOutputFileNameMVG(OPT::strOutputFileName + MVG_EXT); + openMVS::MVS_IO::ExportScene(MAKE_PATH_SAFE(OPT::strListFileName), MAKE_PATH_SAFE(strOutputFileNameMVG), sceneBAF); + + VERBOSE("Input data exported: %u cameras & %u poses & %u images & %u vertices (%s)", sceneBAF.cameras.size(), sceneBAF.poses.size(), sceneBAF.images.size(), sceneBAF.vertices.size(), TD_TIMER_GET_FMT().c_str()); + } else { + // convert data from OpenMVG to OpenMVS + MVS::Scene scene(OPT::nMaxThreads); + size_t nCameras(0), nPoses(0); + + #ifdef _USE_OPENMVG + if (OPT::bOpenMVGjson) { + // read OpenMVG input data from a JSON file + using namespace openMVG::sfm; + using namespace openMVG::cameras; + SfM_Data sfm_data; + const String strSfM_Data_Filename(MAKE_PATH_SAFE(OPT::strInputFileName)); + if (!Load(sfm_data, strSfM_Data_Filename, ESfM_Data(ALL))) { + VERBOSE("error: the input SfM_Data file '%s' cannot be read", strSfM_Data_Filename.c_str()); + return EXIT_FAILURE; + } + VERBOSE("Imported data: %u cameras, %u poses, %u images, %u vertices", + sfm_data.GetIntrinsics().size(), + sfm_data.GetPoses().size(), + sfm_data.GetViews().size(), + sfm_data.GetLandmarks().size()); + + // OpenMVG can have not contiguous index, use a map to create the required OpenMVS contiguous ID index + std::map map_intrinsic, map_view; + + // define a platform with all the intrinsic group + nCameras = sfm_data.GetIntrinsics().size(); + for (const auto& intrinsic: sfm_data.GetIntrinsics()) { + if (isPinhole(intrinsic.second.get()->getType())) { + const Pinhole_Intrinsic * cam = dynamic_cast(intrinsic.second.get()); + if (map_intrinsic.count(intrinsic.first) == 0) + map_intrinsic.insert(std::make_pair(intrinsic.first, scene.platforms.GetSize())); + MVS::Platform& platform = scene.platforms.AddEmpty(); + // add the camera + MVS::Platform::Camera& camera = platform.cameras.AddEmpty(); + camera.K = cam->K(); + // sub-pose + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + } + } + + // define images & poses + const uint32_t nViews((uint32_t)sfm_data.GetViews().size()); + scene.images.Reserve(nViews); + scene.nCalibratedImages = 0; + Util::Progress progress(_T("Processed images"), nViews); + GET_LOGCONSOLE().Pause(); + #ifdef _USE_OPENMP + const std::vector views(sfm_data.GetViews().cbegin(), sfm_data.GetViews().cend()); + #pragma omp parallel for schedule(dynamic) + for (int i=0; i<(int)nViews; ++i) { + const Views::value_type& view = views[i]; + #pragma omp critical + map_view[view.first] = scene.images.GetSize(); + #else + for (const auto& view : sfm_data.GetViews()) { + map_view[view.first] = scene.images.GetSize(); + #endif + MVS::Image& image = scene.images.AddEmpty(); + image.name = view.second->s_Img_path; + Util::ensureUnifySlash(image.name); + Util::strTrim(image.name, PATH_SEPARATOR_STR); + String pathRoot(sfm_data.s_root_path); Util::ensureFolderSlash(pathRoot); + const String srcImage(MAKE_PATH_FULL(WORKING_FOLDER_FULL, pathRoot+image.name)); + image.name = MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strOutputImageFolder+image.name); + Util::ensureDirectory(image.name); + image.ID = static_cast(view.first); + image.platformID = map_intrinsic.at(view.second->id_intrinsic); + MVS::Platform& platform = scene.platforms[image.platformID]; + image.cameraID = 0; + if (sfm_data.IsPoseAndIntrinsicDefined(view.second.get()) && + File::access(srcImage)) { + MVS::Platform::Pose* pPose; + #ifdef _USE_OPENMP + #pragma omp critical + #endif + { + image.poseID = platform.poses.GetSize(); + pPose = &platform.poses.AddEmpty(); + ++scene.nCalibratedImages; + } + const openMVG::geometry::Pose3 poseMVG(sfm_data.GetPoseOrDie(view.second.get())); + pPose->R = poseMVG.rotation(); + pPose->C = poseMVG.center(); + // export undistorted images + const openMVG::cameras::IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view.second->id_intrinsic).get(); + if (cam->have_disto()) { + // undistort and save the image + openMVG::image::Image imageRGB, imageRGB_ud; + openMVG::image::ReadImage(srcImage, &imageRGB); + openMVG::cameras::UndistortImage(imageRGB, cam, imageRGB_ud, openMVG::image::BLACK); + openMVG::image::WriteImage(image.name, imageRGB_ud); + } else { + // no distortion, copy the image + File::copyFile(srcImage, image.name); + } + ++nPoses; + } else { + // image have not valid pose, so set an undefined pose + image.poseID = NO_ID; + // just copy the image + File::copyFile(srcImage, image.name); + DEBUG_EXTRA("warning: uncalibrated image '%s'", view.second->s_Img_path.c_str()); + } + ++progress; + } + GET_LOGCONSOLE().Play(); + progress.close(); + + // define structure + scene.pointcloud.points.Reserve(sfm_data.GetLandmarks().size()); + scene.pointcloud.pointViews.Reserve(sfm_data.GetLandmarks().size()); + for (const auto& vertex: sfm_data.GetLandmarks()) { + const Landmark & landmark = vertex.second; + MVS::PointCloud::ViewArr& views = scene.pointcloud.pointViews.AddEmpty(); + for (const auto& observation: landmark.obs) { + const auto it(map_view.find(observation.first)); + if (it != map_view.end()) + views.InsertSort(it->second); + } + if (views.GetSize() < 2) { + scene.pointcloud.pointViews.RemoveLast(); + continue; + } + MVS::PointCloud::Point& point = scene.pointcloud.points.AddEmpty(); + point = landmark.X.cast(); + } + } else + #endif + { + // read OpenMVG input data from BAF file + openMVS::MVS_IO::SfM_Scene sceneBAF; + if (!openMVS::MVS_IO::ImportScene(MAKE_PATH_SAFE(OPT::strListFileName), MAKE_PATH_SAFE(OPT::strInputFileName), sceneBAF)) + return EXIT_FAILURE; + + // convert data from OpenMVG to OpenMVS + nCameras = sceneBAF.cameras.size(); + scene.platforms.Reserve((uint32_t)nCameras); + for (const auto& cameraBAF: sceneBAF.cameras) { + MVS::Platform& platform = scene.platforms.AddEmpty(); + MVS::Platform::Camera& camera = platform.cameras.AddEmpty(); + camera.K = cameraBAF.K; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + } + nPoses = sceneBAF.images.size(); + scene.images.Reserve((uint32_t)nPoses); + for (const auto& imageBAF: sceneBAF.images) { + openMVS::MVS_IO::Pose& poseBAF = sceneBAF.poses[imageBAF.id_pose]; + MVS::Image& image = scene.images.AddEmpty(); + image.name = imageBAF.name; + Util::ensureUnifySlash(image.name); + image.name = MAKE_PATH_FULL(WORKING_FOLDER_FULL, image.name); + image.ID = imageBAF.id_camera; + image.platformID = imageBAF.id_camera; + MVS::Platform& platform = scene.platforms[image.platformID]; + image.cameraID = 0; + image.poseID = platform.poses.GetSize(); + MVS::Platform::Pose& pose = platform.poses.AddEmpty(); + pose.R = poseBAF.R; + pose.C = poseBAF.C; + } + scene.pointcloud.points.Reserve(sceneBAF.vertices.size()); + scene.pointcloud.pointViews.Reserve(sceneBAF.vertices.size()); + for (const auto& vertexBAF: sceneBAF.vertices) { + MVS::PointCloud::Point& point = scene.pointcloud.points.AddEmpty(); + point = vertexBAF.X.cast(); + MVS::PointCloud::ViewArr& views = scene.pointcloud.pointViews.AddEmpty(); + for (const auto& viewBAF: vertexBAF.views) + views.InsertSort(viewBAF); + } + } + + // read images meta-data + FOREACHPTR(pImage, scene.images) { + if (!pImage->ReloadImage(0, false)) + LOG("error: can not read image %s", pImage->name.c_str()); + } + if (OPT::bNormalizeIntrinsics) { + // normalize camera intrinsics + FOREACH(p, scene.platforms) { + MVS::Platform& platform = scene.platforms[p]; + FOREACH(c, platform.cameras) { + MVS::Platform::Camera& camera = platform.cameras[c]; + // find one image using this camera + MVS::Image* pImage(NULL); + FOREACHPTR(pImg, scene.images) { + if (pImg->platformID == p && pImg->cameraID == c && pImg->poseID != NO_ID) { + pImage = pImg; + break; + } + } + if (pImage == NULL) { + LOG("error: no image using camera %u of platform %u", c, p); + continue; + } + const REAL fScale(REAL(1)/MVS::Camera::GetNormalizationScale(pImage->width, pImage->height)); + camera.K(0,0) *= fScale; + camera.K(1,1) *= fScale; + camera.K(0,2) *= fScale; + camera.K(1,2) *= fScale; + } + } + } + + // write OpenMVS input data + scene.Save(MAKE_PATH_SAFE(OPT::strOutputFileName), (ARCHIVE_TYPE)OPT::nArchiveType); + + VERBOSE("Exported data: %u platforms, %u cameras, %u poses, %u images, %u vertices (%s)", + scene.platforms.GetSize(), nCameras, nPoses, scene.images.GetSize(), scene.pointcloud.GetSize(), + TD_TIMER_GET_FMT().c_str()); + } + + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfacePolycam/CMakeLists.txt b/apps/InterfacePolycam/CMakeLists.txt new file mode 100644 index 0000000..de8fda1 --- /dev/null +++ b/apps/InterfacePolycam/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(InterfacePolycam "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS InterfacePolycam + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/InterfacePolycam/InterfacePolycam.cpp b/apps/InterfacePolycam/InterfacePolycam.cpp new file mode 100644 index 0000000..ccd16c5 --- /dev/null +++ b/apps/InterfacePolycam/InterfacePolycam.cpp @@ -0,0 +1,362 @@ +/* + * InterfacePolycam.cpp + * + * Copyright (c) 2014-2023 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#define JSON_NOEXCEPTION +#include "../../libs/IO/json.hpp" +#include + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("InterfacePolycam") +#define MVS_FILE_EXTENSION _T(".mvs") +#define JSON_EXT _T(".json") +#define DEPTH_EXT _T(".png") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strOutputFileName; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "imports SfM scene stored Polycam format") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Main options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input folder containing Polycam camera poses, images and depth-maps") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the scene") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidFolderPath(OPT::strInputFileName); + const bool bInvalidCommand(OPT::strInputFileName.empty()); + if (OPT::vm.count("help") || bInvalidCommand) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (bInvalidCommand) + return false; + + // initialize optional options + Util::ensureValidFolderPath(OPT::strOutputFileName); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = "scene" MVS_FILE_EXTENSION; + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +// parse image containing calibration, pose, and depth-map information +bool ParseImage(Scene& scene, const String& imagePath, const String& cameraPath, const String& depthPath, + const std::unordered_map& mapImageName) +{ + nlohmann::json data = nlohmann::json::parse(std::ifstream(cameraPath)); + if (data.empty()) + return false; + const cv::Size resolution(data["width"].get(), data["height"].get()); + // set platform + const IIndex platformID = scene.platforms.size(); + Platform& platform = scene.platforms.AddEmpty(); + Platform::Camera& camera = platform.cameras.AddEmpty(); + camera.K = KMatrix::IDENTITY; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + camera.K(0,0) = data["fx"].get(); + camera.K(1,1) = data["fy"].get(); + camera.K(0,2) = data["cx"].get(); + camera.K(1,2) = data["cy"].get(); + // set image + const IIndex imageID = scene.images.size(); + Image& imageData = scene.images.AddEmpty(); + imageData.platformID = platformID; + imageData.cameraID = 0; // only one camera per platform supported by this format + imageData.poseID = NO_ID; + imageData.ID = imageID; + imageData.name = imagePath; + ASSERT(Util::isFullPath(imageData.name)); + // set image resolution + imageData.width = resolution.width; + imageData.height = resolution.height; + imageData.scale = 1; + // set camera pose + imageData.poseID = platform.poses.size(); + Platform::Pose& pose = platform.poses.AddEmpty(); + const Eigen::Matrix3d R_session_arkitcam{ + {data["t_00"].get(), data["t_01"].get(), data["t_02"].get()}, + {data["t_10"].get(), data["t_11"].get(), data["t_12"].get()}, + {data["t_20"].get(), data["t_21"].get(), data["t_22"].get()} + }; + const Eigen::Vector3d t_session_arkitcam{ + data["t_03"].get(), + data["t_13"].get(), + data["t_23"].get() + }; + const Eigen::Affine3d T_session_arkitcam{ + Eigen::Affine3d(Eigen::Translation3d(t_session_arkitcam)) * Eigen::Affine3d(Eigen::AngleAxisd(R_session_arkitcam)) + }; + const Eigen::Affine3d T_cam_arkitcam{ + Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()) + }; + const Eigen::Matrix4d P{ + (T_cam_arkitcam * T_session_arkitcam.inverse()).matrix() + }; + pose.R = P.topLeftCorner<3, 3>().eval(); + pose.R.EnforceOrthogonality(); + const Point3d t = P.topRightCorner<3, 1>().eval(); + pose.C = pose.R.t() * (-t); + imageData.camera = platform.GetCamera(imageData.cameraID, imageData.poseID); + // set image neighbors if available + nlohmann::json::const_iterator itNeighbors = data.find("neighbors"); + if (itNeighbors != data.end()) { + const std::vector neighborTimestamps = itNeighbors->get>(); + for (uint64_t timestamp: neighborTimestamps) { + const String neighborName = std::to_string(timestamp); + const IIndex neighborID = mapImageName.at(neighborName); + if (neighborID != imageData.ID) + imageData.neighbors.emplace_back(ViewScore{neighborID, 0, 1.f, FD2R(15.f), 0.5f, 3.f}); + } + } + // load and convert depth-map + if (!depthPath.empty()) { + DepthMap depthMap; { + constexpr double depthScale{1000.0}; + const cv::Mat imgDepthMap = cv::imread(depthPath, cv::IMREAD_ANYDEPTH); + if (imgDepthMap.empty()) + return false; + imgDepthMap.convertTo(depthMap, CV_32FC1, 1.0/depthScale); + } + IIndexArr IDs = {imageData.ID}; + IDs.JoinFunctor(imageData.neighbors.size(), [&imageData](IIndex i) { + return imageData.neighbors[i].ID; + }); + double dMin, dMax; + cv::minMaxIdx(depthMap, &dMin, &dMax, NULL, NULL, depthMap > 0); + const NormalMap normalMap; + const ConfidenceMap confMap; + const ViewsMap viewsMap; + if (!ExportDepthDataRaw(MAKE_PATH(String::FormatString("depth%04u.dmap", imageData.ID)), + imageData.name, IDs, resolution, + camera.K, pose.R, pose.C, + (float)dMin, (float)dMax, + depthMap, normalMap, confMap, viewsMap)) + return false; + } + return true; +} + +// parse scene stored in Polycam format +bool ParseScene(Scene& scene, const String& scenePath) +{ + #if defined(_SUPPORT_CPP17) && (!defined(__GNUC__) || (__GNUC__ > 7)) + size_t numCorrectedFolders(0), numCorrectedDepthFolders(0), numFolders(0), numDepthFolders(0); + for (const auto& file: std::filesystem::directory_iterator(scenePath.c_str())) { + if (file.path().stem() == "corrected_cameras" || + file.path().stem() == "corrected_images") + ++numCorrectedFolders; + else if (file.path().stem() == "corrected_depth") + ++numCorrectedDepthFolders; + else if (file.path().stem() == "cameras" || + file.path().stem() == "images") + ++numFolders; + else if (file.path().stem() == "depth") + ++numDepthFolders; + } + if (numFolders != 2) { + VERBOSE("Invalid scene folder"); + return false; + } + if (numCorrectedFolders == 2) { + // corrected data + CLISTDEFIDX(String, IIndex) imagePaths; + for (const auto& file: std::filesystem::directory_iterator((scenePath + "corrected_images").c_str())) + imagePaths.emplace_back(file.path().string()); + VERBOSE("Parsing corrected data: %u...", imagePaths.size()); + std::unordered_map mapImageName; + mapImageName.reserve(imagePaths.size()); + for (String& imagePath: imagePaths) { + Util::ensureValidPath(imagePath); + mapImageName.emplace(Util::getFileName(imagePath), static_cast(mapImageName.size())); + } + for (const String& imagePath: imagePaths) { + const String imageName = Util::getFileName(imagePath); + const String cameraPath(scenePath + "corrected_cameras" + PATH_SEPARATOR_STR + imageName + JSON_EXT); + const String depthPath(numCorrectedDepthFolders ? scenePath + "corrected_depth" + PATH_SEPARATOR_STR + imageName + DEPTH_EXT : String()); + if (!ParseImage(scene, imagePath, cameraPath, depthPath, mapImageName)) + return false; + } + } else { + // raw data + CLISTDEFIDX(String, IIndex) imagePaths; + for (const auto& file: std::filesystem::directory_iterator((scenePath + "images").c_str())) + imagePaths.emplace_back(file.path().string()); + VERBOSE("Parsing raw data: %u...", imagePaths.size()); + std::unordered_map mapImageName; + mapImageName.reserve(imagePaths.size()); + for (String& imagePath: imagePaths) { + Util::ensureValidPath(imagePath); + mapImageName.emplace(Util::getFileName(imagePath), static_cast(mapImageName.size())); + } + for (const String& imagePath: imagePaths) { + const String imageName = Util::getFileName(imagePath); + const String cameraPath(scenePath + "cameras" + PATH_SEPARATOR_STR + imageName + JSON_EXT); + const String depthPath(numDepthFolders ? scenePath + "depth" + PATH_SEPARATOR_STR + imageName + DEPTH_EXT : String()); + if (!ParseImage(scene, imagePath, cameraPath, depthPath, mapImageName)) + return false; + } + } + return true; + #else + return false; + #endif +} + +} // unnamed namespace + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + TD_TIMER_START(); + + Scene scene(OPT::nMaxThreads); + + // convert data from Polycam format to OpenMVS + if (!ParseScene(scene, MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strInputFileName))) + return EXIT_FAILURE; + + // write OpenMVS input data + scene.Save(MAKE_PATH_SAFE(OPT::strOutputFileName), (ARCHIVE_TYPE)OPT::nArchiveType); + + VERBOSE("Exported data: %u platforms, %u cameras, %u poses, %u images (%s)", + scene.platforms.size(), scene.images.size(), scene.images.size(), scene.images.size(), + TD_TIMER_GET_FMT().c_str()); + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfaceVisualSFM/CMakeLists.txt b/apps/InterfaceVisualSFM/CMakeLists.txt new file mode 100644 index 0000000..a1b16af --- /dev/null +++ b/apps/InterfaceVisualSFM/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(InterfaceVisualSFM "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS InterfaceVisualSFM + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/InterfaceVisualSFM/DataInterface.h b/apps/InterfaceVisualSFM/DataInterface.h new file mode 100644 index 0000000..4ef630c --- /dev/null +++ b/apps/InterfaceVisualSFM/DataInterface.h @@ -0,0 +1,383 @@ +//////////////////////////////////////////////////////////////////////////// +// File: DataInterface.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : data interface, the data format been uploaded to GPU +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DATA_INTERFACE_GPU_H +#define DATA_INTERFACE_GPU_H + +namespace PBA { + +// ----------------------------WARNING------------------------------ +// ----------------------------------------------------------------- +// ROTATION CONVERSION: +// The internal rotation representation is 3x3 float matrix. Reading +// back the rotations as quaternion or Rodrigues's representation will +// cause inaccuracy, IF you have wrongly reconstructed cameras with +// a very very large focal length (typically also very far away). +// In this case, any small change in the rotation matrix, will cause +// a large reprojection error. +// +// --------------------------------------------------------------------- +// RADIAL distortion is NOT enabled by default, use parameter "-md", -pd" +// or set ConfigBA::__use_radial_distortion to 1 or -1 to enable it. +// --------------------------------------------------------------------------- + +//transfer data type with 4-float alignment +#define CameraT CameraT_ +#define Point3D Point3D_ +template + +struct CameraT_ +{ + typedef FT float_t; + ////////////////////////////////////////////////////// + float_t f; // single focal length, K = [f, 0, 0; 0 f 0; 0 0 1] + float_t t[3]; // T in P = K[R T], T = - RC + float_t m[3][3]; // R in P = K[R T]. + float_t radial; // WARNING: BE careful with the radial distortion model. + float_t distortion_type; + float_t constant_camera; + + ////////////////////////////////////////////////////////// + CameraT_() { radial = 0; distortion_type = 0; constant_camera = 0; } + + ////////////////////////////////////////////// + template void SetCameraT(const CameraX & cam) + { + f = (float_t)cam.f; + t[0] = (float_t)cam.t[0]; t[1] = (float_t)cam.t[1]; t[2] = (float_t)cam.t[2]; + for(int i = 0; i < 3; ++i) for(int j = 0; j < 3; ++j) m[i][j] = (float_t)cam.m[i][j]; + radial = (float_t)cam.radial; + distortion_type = (float_t)cam.distortion_type; + constant_camera = (float_t)cam.constant_camera; + } + + ////////////////////////////////////////// + enum { + CAMERA_VARIABLE = 0, + CAMERA_FIXEDINTRINSIC = (1<<0), + CAMERA_FIXEDEXTRINSIC = (1<<1), + }; + void SetVariableCamera() {(int&)constant_camera = CAMERA_VARIABLE;} + void SetFixedIntrinsic() {(int&)constant_camera = CAMERA_FIXEDINTRINSIC;} + void SetFixedExtrinsic() {(int&)constant_camera = CAMERA_FIXEDEXTRINSIC;} + void SetConstantCamera() {(int&)constant_camera = CAMERA_FIXEDINTRINSIC|CAMERA_FIXEDEXTRINSIC;} + + ////////////////////////////////////// + template void SetFocalLength(Float F){ f = (float_t) F; } + float_t GetFocalLength() const{return f;} + + template void SetMeasurementDistortion(Float r) {radial = (float_t) r; distortion_type = -1;} + float_t GetMeasurementDistortion() const {return distortion_type == -1 ? radial : 0; } + + //normalize radial distortion that applies to angle will be (radial * f * f); + template void SetNormalizedMeasurementDistortion(Float r) {SetMeasurementDistortion(r / (f * f)); } + float_t GetNormalizedMeasurementDistortion() const{return GetMeasurementDistortion() * (f * f); } + + //use projection distortion + template void SetProjectionDistortion(Float r) {radial = float_t(r); distortion_type = 1; } + template void SetProjectionDistortion(const Float* r) {SetProjectionDistortion(r[0]); } + float_t GetProjectionDistortion() {return distortion_type == 1 ? radial : 0; } + + template void SetRodriguesRotation(const Float r[3]) + { + double a = sqrt(r[0]*r[0]+r[1]*r[1]+r[2]*r[2]); + double ct = a==0.0?0.5:(1.0-cos(a))/a/a; + double st = a==0.0?1:sin(a)/a; + m[0][0]=float_t(1.0 - (r[1]*r[1] + r[2]*r[2])*ct); + m[0][1]=float_t(r[0]*r[1]*ct - r[2]*st); + m[0][2]=float_t(r[2]*r[0]*ct + r[1]*st); + m[1][0]=float_t(r[0]*r[1]*ct + r[2]*st); + m[1][1]=float_t(1.0 - (r[2]*r[2] + r[0]*r[0])*ct); + m[1][2]=float_t(r[1]*r[2]*ct - r[0]*st); + m[2][0]=float_t(r[2]*r[0]*ct - r[1]*st); + m[2][1]=float_t(r[1]*r[2]*ct + r[0]*st); + m[2][2]=float_t(1.0 - (r[0]*r[0] + r[1]*r[1])*ct ); + } + template void GetRodriguesRotation(Float r[3]) const + { + double a = (m[0][0]+m[1][1]+m[2][2]-1.0)/2.0; + const double epsilon = 0.01; + if( fabs(m[0][1] - m[1][0]) < epsilon && + fabs(m[1][2] - m[2][1]) < epsilon && + fabs(m[0][2] - m[2][0]) < epsilon ) + { + if( fabs(m[0][1] + m[1][0]) < 0.1 && + fabs(m[1][2] + m[2][1]) < 0.1 && + fabs(m[0][2] + m[2][0]) < 0.1 && a > 0.9) + { + r[0] = 0; + r[1] = 0; + r[2] = 0; + } + else + { + const Float ha = Float(sqrt(0.5) * 3.14159265358979323846); + double xx = (m[0][0]+1.0)/2.0; + double yy = (m[1][1]+1.0)/2.0; + double zz = (m[2][2]+1.0)/2.0; + double xy = (m[0][1]+m[1][0])/4.0; + double xz = (m[0][2]+m[2][0])/4.0; + double yz = (m[1][2]+m[2][1])/4.0; + + if ((xx > yy) && (xx > zz)) + { + if (xx< epsilon) + { + r[0] = 0; r[1] = r[2] = ha; + } else + { + double t = sqrt(xx) ; + r[0] = Float(t * 3.14159265358979323846); + r[1] = Float(xy/t * 3.14159265358979323846); + r[2] = Float(xz/t * 3.14159265358979323846); + } + } else if (yy > zz) + { + if (yy< epsilon) + { + r[0] = r[2] = ha; r[1] = 0; + } else + { + double t = sqrt(yy); + r[0] = Float(xy/t* 3.14159265358979323846); + r[1] = Float( t * 3.14159265358979323846); + r[2] = Float(yz/t* 3.14159265358979323846); + } + } else + { + if (zz< epsilon) + { + r[0] = r[1] = ha; r[2] = 0; + } else + { + double t = sqrt(zz); + r[0] = Float(xz/ t* 3.14159265358979323846); + r[1] = Float(yz/ t* 3.14159265358979323846); + r[2] = Float( t * 3.14159265358979323846); + } + } + } + } + else + { + a = acos(a); + double b = 0.5*a/sin(a); + r[0] = Float(b*(m[2][1]-m[1][2])); + r[1] = Float(b*(m[0][2]-m[2][0])); + r[2] = Float(b*(m[1][0]-m[0][1])); + } + } + //////////////////////// + template void SetQuaternionRotation(const Float q[4]) + { + double qq = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]); + double qw, qx, qy, qz; + if(qq>0) + { + qw=q[0]/qq; + qx=q[1]/qq; + qy=q[2]/qq; + qz=q[3]/qq; + }else + { + qw = 1; + qx = qy = qz = 0; + } + m[0][0]=float_t(qw*qw + qx*qx- qz*qz- qy*qy ); + m[0][1]=float_t(2*qx*qy -2*qz*qw ); + m[0][2]=float_t(2*qy*qw + 2*qz*qx); + m[1][0]=float_t(2*qx*qy+ 2*qw*qz); + m[1][1]=float_t(qy*qy+ qw*qw - qz*qz- qx*qx); + m[1][2]=float_t(2*qz*qy- 2*qx*qw); + m[2][0]=float_t(2*qx*qz- 2*qy*qw); + m[2][1]=float_t(2*qy*qz + 2*qw*qx ); + m[2][2]=float_t(qz*qz+ qw*qw- qy*qy- qx*qx); + } + template void GetQuaternionRotation(Float q[4]) const + { + q[0]= 1 + m[0][0] + m[1][1] + m[2][2]; + if(q[0]>0.000000001) + { + q[0] = sqrt(q[0])/2.0; + q[1]= (m[2][1] - m[1][2])/( 4.0 *q[0]); + q[2]= (m[0][2] - m[2][0])/( 4.0 *q[0]); + q[3]= (m[1][0] - m[0][1])/( 4.0 *q[0]); + }else + { + double s; + if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) + { + s = 2.0 * sqrt( 1.0 + m[0][0] - m[1][1] - m[2][2]); + q[1] = 0.25 * s; + q[2] = (m[0][1] + m[1][0] ) / s; + q[3] = (m[0][2] + m[2][0] ) / s; + q[0] = (m[1][2] - m[2][1] ) / s; + } else if (m[1][1] > m[2][2]) + { + s = 2.0 * sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2]); + q[1] = (m[0][1] + m[1][0] ) / s; + q[2] = 0.25 * s; + q[3] = (m[1][2] + m[2][1] ) / s; + q[0] = (m[0][2] - m[2][0] ) / s; + } else + { + s = 2.0 * sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1]); + q[1] = (m[0][2] + m[2][0] ) / s; + q[2] = (m[1][2] + m[2][1] ) / s; + q[3] = 0.25f * s; + q[0] = (m[0][1] - m[1][0] ) / s; + } + } + } + //////////////////////////////////////////////// + template void SetMatrixRotation(const Float * r) + { + for(int i = 0; i < 9; ++i) m[0][i] = float_t(r[i]); + } + template void GetMatrixRotation(Float * r) const + { + for(int i = 0; i < 9; ++i) r[i] = Float(m[0][i]); + } + float GetRotationMatrixDeterminant()const + { + return m[0][0]*m[1][1]*m[2][2] + + m[0][1]*m[1][2]*m[2][0] + + m[0][2]*m[1][0]*m[2][1] - + m[0][2]*m[1][1]*m[2][0] - + m[0][1]*m[1][0]*m[2][2] - + m[0][0]*m[1][2]*m[2][1]; + } + /////////////////////////////////////// + template void SetTranslation(const Float T[3]) + { + t[0] = (float_t)T[0]; + t[1] = (float_t)T[1]; + t[2] = (float_t)T[2]; + } + template void GetTranslation(Float T[3]) const + { + T[0] = (Float)t[0]; + T[1] = (Float)t[1]; + T[2] = (Float)t[2]; + } + ///////////////////////////////////////////// + template void SetCameraCenterAfterRotation(const Float c[3]) + { + //t = - R * C + for(int j = 0; j < 3; ++j) t[j] = -float_t(double(m[j][0])*double(c[0]) + double(m[j][1])*double(c[1]) + double(m[j][2])*double(c[2])); + } + template void GetCameraCenter(Float c[3]) const + { + //C = - R' * t + for(int j = 0; j < 3; ++j) c[j] = -Float(double(m[0][j])*double(t[0]) + double(m[1][j])*double(t[1]) + double(m[2][j])*double(t[2])); + } + //////////////////////////////////////////// + template void SetInvertedRT(const Float e[3], const Float T[3]) + { + SetRodriguesRotation(e); + for(int i = 3; i < 9; ++i) m[0][i] = - m[0][i]; + SetTranslation(T); t[1] = - t[1]; t[2] = -t[2]; + } + + template void GetInvertedRT (Float e[3], Float T[3]) const + { + CameraT ci; ci.SetMatrixRotation(m[0]); + for(int i = 3; i < 9; ++i) ci.m[0][i] = - ci.m[0][i]; + //for(int i = 1; i < 3; ++i) for(int j = 0; j < 3; ++j) ci.m[i][j] = - ci.m[i][j]; + ci.GetRodriguesRotation(e); + GetTranslation(T); T[1] = - T[1]; T[2] = -T[2]; + } + template void SetInvertedR9T(const Float e[9], const Float T[3]) + { + //for(int i = 0; i < 9; ++i) m[0][i] = (i < 3 ? e[i] : - e[i]); + //SetTranslation(T); t[1] = - t[1]; t[2] = -t[2]; + m[0][0] = e[0]; m[0][1] = e[1]; m[0][2] = e[2]; + m[1][0] = -e[3]; m[1][1] = -e[4]; m[1][2] = -e[5]; + m[2][0] = -e[6]; m[2][1] = -e[7]; m[2][2] = -e[8]; + t[0] = T[0]; t[1] = -T[1]; t[2] = -T[2]; + } + template void GetInvertedR9T(Float e[9], Float T[3]) const + { + e[0] = m[0][0]; e[1] = m[0][1]; e[2] = m[0][2]; + e[3] = - m[1][0]; e[4] = -m[1][1]; e[5] = -m[1][2]; + e[6] = -m[2][0]; e[7] = -m[2][1]; e[8] = -m[2][2] ; + T[0] = t[0]; T[1] = -t[1]; T[2] = -t[2]; + } +}; + + + +template +struct Point3D +{ + typedef FT float_t; + float_t xyz[3]; //3D point location + float_t reserved; //alignment + //////////////////////////////// + template void SetPoint(Float x, Float y, Float z) + { + xyz[0] = (float_t) x; + xyz[1] = (float_t) y; + xyz[2] = (float_t) z; + reserved = 0; + } + template void SetPoint(const Float * p) + { + xyz[0] = (float_t) p[0]; + xyz[1] = (float_t) p[1]; + xyz[2] = (float_t) p[2]; + reserved = 0; + } + template void GetPoint(Float* p) const + { + p[0] = (Float) xyz[0]; + p[1] = (Float) xyz[1]; + p[2] = (Float) xyz[2]; + } + template void GetPoint(Float&x, Float&y, Float&z) const + { + x = (Float) xyz[0]; + y = (Float) xyz[1]; + z = (Float) xyz[2]; + } +}; + +#undef CameraT +#undef Point3D + +typedef CameraT_ CameraT; +typedef CameraT_ CameraD; +typedef Point3D_ Point3D; +typedef Point3D_ Point3B; + +struct Point2D +{ + float x, y; + //////////////////////////////////////////////////////// + Point2D(){} + template Point2D(Float X, Float Y) {SetPoint2D(X, Y); } + template void SetPoint2D(Float X, Float Y) { x = (float) X; y = (float) Y; } + template void GetPoint2D(Float&X, Float&Y) const { X = (Float) x; Y = (Float) y; } +}; + +} // namespace PBA + +#endif + diff --git a/apps/InterfaceVisualSFM/InterfaceVisualSFM.cpp b/apps/InterfaceVisualSFM/InterfaceVisualSFM.cpp new file mode 100644 index 0000000..2cc8330 --- /dev/null +++ b/apps/InterfaceVisualSFM/InterfaceVisualSFM.cpp @@ -0,0 +1,607 @@ +/* + * InterfaceVisualSFM.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#define LOG_OUT() GET_LOG() +#define LOG_ERR() GET_LOG() +#include "Util.h" +#include + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("InterfaceVisualSFM") +#define MVS_EXT _T(".mvs") +#define VSFM_EXT _T(".nvm") +#define BUNDLE_EXT _T(".out") +#define CMPMVS_EXT _T(".lst") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strOutputFileName; +String strOutputImageFolder; +bool IsFromOpenMVS; // conversion direction +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_DEFAULT), "project archive type: 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Main options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses and image list (NVM, undistorted OUT + image_list.TXT, LST)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the mesh") + ("output-image-folder", boost::program_options::value(&OPT::strOutputImageFolder)->default_value("undistorted_images"), "output folder to store undistorted images") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + Util::ensureUnifySlash(OPT::strInputFileName); + if (OPT::vm.count("help") || OPT::strInputFileName.IsEmpty()) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (OPT::strInputFileName.IsEmpty()) + return false; + + // initialize optional options + if (OPT::strInputFileName.IsEmpty()) + return false; + Util::ensureValidPath(OPT::strOutputFileName); + Util::ensureUnifySlash(OPT::strOutputFileName); + Util::ensureUnifySlash(OPT::strOutputImageFolder); + Util::ensureFolderSlash(OPT::strOutputImageFolder); + const String strInputFileNameExt(Util::getFileExt(OPT::strInputFileName).ToLower()); + OPT::IsFromOpenMVS = (strInputFileNameExt == MVS_FILE_EXTENSION); + if (OPT::IsFromOpenMVS) { + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFilePath(OPT::strInputFileName); + } else { + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFilePath(OPT::strInputFileName) + _T("scene") MVS_FILE_EXTENSION; + else + OPT::strOutputImageFolder = Util::getRelativePath(Util::getFilePath(OPT::strOutputFileName), Util::getFilePath(OPT::strInputFileName)+OPT::strOutputImageFolder); + } + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + +#define PBA_PRECISION float + +namespace PBA { +template struct CameraT_; +typedef CameraT_ Camera; +template struct Point3D_; +typedef Point3D_ Point3D; +} // namespace PBA + +namespace MVS { +// given an undistorted pixel coordinate and one radial-undistortion parameter, +// compute the corresponding distorted coordinate +template +inline TPoint2 DistortPointR1(const TPoint2& pt, const REAL& k1) { + if (k1 == 0) + return pt; + const REAL y(pt.y == 0 ? REAL(1.e-12) : REAL(pt.y)); + const REAL t2(y*y); + const REAL t3(t2*t2*t2); + const REAL t4(pt.x*pt.x); + const REAL t7(k1*(t2+t4)); + const REAL t9(1.0/t7); + const REAL t10(t2*t9*y*0.5); + const REAL t11(t3*t9*t9*(0.25+t9/27.0)); + #ifndef _RELEASE + TPoint2 upt; + #endif + if (k1 > 0) { + const REAL t17(CBRT(t10+SQRT(t11))); + const REAL t18(t17-t2*t9/(t17*3)); + #ifndef _RELEASE + upt = + #else + return + #endif + TPoint2(TYPE(t18*pt.x/y), TYPE(t18)); + } else { + ASSERT(t11 <= 0); + const std::complex t16(t10, SQRT(-t11)); + const std::complex t17(pow(t16, 1.0/3.0)); + const std::complex t14((t2*t9)/(t17*3.0)); + const std::complex t18((t17+t14)*std::complex(0.0,SQRT_3)); + const std::complex t19(0.5*(t14-t17-t18)); + #ifndef _RELEASE + upt = + #else + return + #endif + TPoint2(TYPE(t19.real()*pt.x/y), TYPE(t19.real())); + } + #ifndef _RELEASE + ASSERT(ABS(TYPE((1.0+k1*(upt.x*upt.x+upt.y*upt.y))*upt.x) - pt.x) < TYPE(0.001)); + ASSERT(ABS(TYPE((1.0+k1*(upt.x*upt.x+upt.y*upt.y))*upt.y) - pt.y) < TYPE(0.001)); + return upt; + #endif +} + +void UndistortImage(const Camera& camera, const REAL& k1, const Image8U3 imgIn, Image8U3& imgOut) +{ + // allocate the undistorted image + if (imgOut.data == imgIn.data || + imgOut.cols != imgIn.cols || + imgOut.rows != imgIn.rows || + imgOut.type() != imgIn.type()) + imgOut = Image8U3(imgIn.rows, imgIn.cols); + + // compute each pixel + const int w = imgIn.cols; + const int h = imgIn.rows; + const Matrix3x3f K(camera.K); + const Matrix3x3f invK(camera.GetInvK()); + ASSERT(ISEQUAL(K(0,2),0.5f*(w-1)) && ISEQUAL(K(1,2),0.5f*(h-1))); + typedef Sampler::Cubic Sampler; + const Sampler sampler; + Point2f pt; + for (int v=0; v(sampler, pt).cast(); + } else { + // set to black + col = Pixel8U::BLACK; + } + } + } +} +} // namespace MVS + + +bool ExportSceneVSFM() +{ + TD_TIMER_START(); + + // read MVS input data + MVS::Scene scene(OPT::nMaxThreads); + if (!scene.Load(MAKE_PATH_SAFE(OPT::strInputFileName))) + return false; + + // convert and write data from OpenMVS to VisualSFM + std::vector cameras; + std::vector vertices; + std::vector measurements; // the array of 2D projections (only inliers) + std::vector correspondingPoint; // 3D point index corresponding to each 2D projection + std::vector correspondingView; // and camera index + std::vector names; + std::vector ptc; + cameras.reserve(scene.images.size()); + names.reserve(scene.images.size()); + MVS::IIndexArr mapIdx(scene.images.size()); + bool bFocalWarning(false), bPrincipalpointWarning(false); + FOREACH(idx, scene.images) { + const MVS::Image& image = scene.images[idx]; + if (!image.IsValid()) { + mapIdx[idx] = NO_ID; + continue; + } + if (!bFocalWarning && !ISEQUAL(image.camera.K(0, 0), image.camera.K(1, 1))) { + DEBUG("warning: fx != fy and NVM format does not support it"); + bFocalWarning = true; + } + if (!bPrincipalpointWarning && (!ISEQUAL(REAL(image.width-1)*0.5, image.camera.K(0, 2)) || !ISEQUAL(REAL(image.height-1)*0.5, image.camera.K(1, 2)))) { + DEBUG("warning: cx, cy are not the image center and NVM format does not support it"); + bPrincipalpointWarning = true; + } + PBA::Camera cameraNVM; + cameraNVM.SetFocalLength((image.camera.K(0, 0) + image.camera.K(1, 1)) * 0.5); + cameraNVM.SetMatrixRotation(image.camera.R.val); + cameraNVM.SetCameraCenterAfterRotation(image.camera.C.ptr()); + mapIdx[idx] = static_cast(cameras.size()); + cameras.emplace_back(cameraNVM); + names.emplace_back(MAKE_PATH_REL(WORKING_FOLDER_FULL, image.name)); + } + vertices.reserve(scene.pointcloud.points.size()); + measurements.reserve(scene.pointcloud.pointViews.size()); + correspondingPoint.reserve(scene.pointcloud.pointViews.size()); + correspondingView.reserve(scene.pointcloud.pointViews.size()); + FOREACH(idx, scene.pointcloud.points) { + const MVS::PointCloud::Point& X = scene.pointcloud.points[idx]; + const MVS::PointCloud::ViewArr& views = scene.pointcloud.pointViews[idx]; + const size_t prevMeasurements(measurements.size()); + for (MVS::IIndex idxView: views) { + const MVS::Image& image = scene.images[idxView]; + const Point2f pt(image.camera.TransformPointW2I(Cast(X))); + if (pt.x < 0 || pt.y < 0 || pt.x > image.width-1 || pt.y > image.height-1) + continue; + measurements.emplace_back(pt.x, pt.y); + correspondingView.emplace_back(static_cast(mapIdx[idxView])); + correspondingPoint.emplace_back(static_cast(vertices.size())); + } + if (prevMeasurements < measurements.size()) + vertices.emplace_back(PBA::Point3D{X.x, X.y, X.z}); + } + if (!scene.pointcloud.colors.empty()) { + ptc.reserve(scene.pointcloud.colors.size()*3); + FOREACH(idx, scene.pointcloud.points) { + const MVS::PointCloud::Color& c = scene.pointcloud.colors[idx]; + ptc.emplace_back(c.r); + ptc.emplace_back(c.g); + ptc.emplace_back(c.b); + } + } + PBA::SaveModelFile(MAKE_PATH_SAFE(OPT::strOutputFileName), cameras, vertices, measurements, correspondingPoint, correspondingView, names, ptc); + + VERBOSE("Input data exported: %u images & %u points (%s)", scene.images.size(), scene.pointcloud.GetSize(), TD_TIMER_GET_FMT().c_str()); + return true; +} + + +bool ImportSceneVSFM() +{ + TD_TIMER_START(); + + // read VisualSFM input data + std::vector cameras; + std::vector vertices; + std::vector measurements; // the array of 2D projections (only inliers) + std::vector correspondingPoint; // 3D point index corresponding to each 2D projection + std::vector correspondingView; // and camera index + std::vector names; + std::vector ptc; + if (!PBA::LoadModelFile(MAKE_PATH_SAFE(OPT::strInputFileName), cameras, vertices, measurements, correspondingPoint, correspondingView, names, ptc)) + return false; + + // convert data from VisualSFM to OpenMVS + MVS::Scene scene(OPT::nMaxThreads); + scene.platforms.Reserve((uint32_t)cameras.size()); + scene.images.Reserve((MVS::IIndex)cameras.size()); + scene.nCalibratedImages = 0; + for (size_t idx=0; idx(idx); + const PBA::Camera& cameraNVM = cameras[idx]; + camera.K = MVS::Platform::Camera::ComposeK(cameraNVM.GetFocalLength(), cameraNVM.GetFocalLength(), image.width, image.height); + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + // normalize camera intrinsics + camera.K = camera.GetScaledK(REAL(1)/MVS::Camera::GetNormalizationScale(image.width, image.height)); + // set pose + image.poseID = platform.poses.GetSize(); + MVS::Platform::Pose& pose = platform.poses.AddEmpty(); + cameraNVM.GetMatrixRotation(pose.R.val); + cameraNVM.GetCameraCenter(pose.C.ptr()); + image.UpdateCamera(scene.platforms); + ++scene.nCalibratedImages; + } + scene.pointcloud.points.Reserve(vertices.size()); + for (size_t idx=0; idx +void _ImageListParseP(const LPSTR* argv, TMatrix& P) +{ + // read projection matrix + P(0,0) = String::FromString(argv[0]); + P(0,1) = String::FromString(argv[1]); + P(0,2) = String::FromString(argv[2]); + P(0,3) = String::FromString(argv[3]); + P(1,0) = String::FromString(argv[4]); + P(1,1) = String::FromString(argv[5]); + P(1,2) = String::FromString(argv[6]); + P(1,3) = String::FromString(argv[7]); + P(2,0) = String::FromString(argv[8]); + P(2,1) = String::FromString(argv[9]); + P(2,2) = String::FromString(argv[10]); + P(2,3) = String::FromString(argv[11]); +} + +int ImportSceneCMPMVS() +{ + TD_TIMER_START(); + + MVS::Scene scene(OPT::nMaxThreads); + + // read CmpMVS input data as a list of images and their projection matrices + std::ifstream iFilein(MAKE_PATH_SAFE(OPT::strInputFileName)); + if (!iFilein.is_open()) + return false; + while (iFilein.good()) { + String strImageName; + std::getline(iFilein, strImageName); + if (strImageName.empty()) + continue; + if (!File::access(MAKE_PATH_SAFE(strImageName))) + return false; + const String strImageNameP(Util::getFileFullName(strImageName)+"_P.txt"); + std::ifstream iFileP(MAKE_PATH_SAFE(strImageNameP)); + if (!iFileP.is_open()) + return false; + String strP; int numLines(0); + while (iFileP.good()) { + String line; + std::getline(iFileP, line); + if (strImageName.empty()) + break; + if (strP.empty()) + strP = line; + else + strP += _T(' ') + line; + ++numLines; + } + if (numLines != 3) + return false; + PMatrix P; + size_t argc; + CAutoPtrArr argv(Util::CommandLineToArgvA(strP, argc)); + if (argc != 12) + return false; + _ImageListParseP(argv, P); + KMatrix K; RMatrix R; CMatrix C; + MVS::DecomposeProjectionMatrix(P, K, R, C); + // set image + MVS::Image& image = scene.images.AddEmpty(); + image.name = strImageName; + Util::ensureUnifySlash(image.name); + image.name = MAKE_PATH_FULL(WORKING_FOLDER_FULL, image.name); + if (!image.ReloadImage(0, false)) { + LOG("error: can not read image %s", image.name.c_str()); + return false; + } + // set camera + image.platformID = scene.platforms.GetSize(); + MVS::Platform& platform = scene.platforms.AddEmpty(); + MVS::Platform::Camera& camera = platform.cameras.AddEmpty(); + image.cameraID = 0; + camera.K = K; + camera.R = RMatrix::IDENTITY; + camera.C = CMatrix::ZERO; + // normalize camera intrinsics + camera.K = camera.GetScaledK(REAL(1)/MVS::Camera::GetNormalizationScale(image.width, image.height)); + // set pose + image.poseID = platform.poses.GetSize(); + MVS::Platform::Pose& pose = platform.poses.AddEmpty(); + pose.R = R; + pose.C = C; + image.UpdateCamera(scene.platforms); + ++scene.nCalibratedImages; + } + + VERBOSE("Input data imported: %u images (%s)", scene.images.size(), TD_TIMER_GET_FMT().c_str()); + + // write OpenMVS input data + return scene.SaveInterface(MAKE_PATH_SAFE(OPT::strOutputFileName)); +} + + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + if (OPT::IsFromOpenMVS) { + ExportSceneVSFM(); + } else { + const String strInputFileNameExt(Util::getFileExt(OPT::strInputFileName).ToLower()); + if (strInputFileNameExt == VSFM_EXT || strInputFileNameExt == BUNDLE_EXT) { + if (!ImportSceneVSFM()) + return EXIT_FAILURE; + } else + if (strInputFileNameExt == CMPMVS_EXT) { + if (!ImportSceneCMPMVS()) + return EXIT_FAILURE; + } + } + + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/InterfaceVisualSFM/Util.h b/apps/InterfaceVisualSFM/Util.h new file mode 100644 index 0000000..abf7e1d --- /dev/null +++ b/apps/InterfaceVisualSFM/Util.h @@ -0,0 +1,756 @@ +//////////////////////////////////////////////////////////////////////////// +// File: util.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : some utility functions for reading/writing SfM data +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include +#include +#include +#include +#include "DataInterface.h" + +namespace PBA { + +//File loader supports .nvm format and bundler format +bool LoadModelFile(const char* name, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc); +void SaveNVM(const char* filename, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc); +void SaveBundlerModel(const char* filename, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx); + +////////////////////////////////////////////////////////////////// +void AddNoise(std::vector& camera_data, std::vector& point_data, float percent); +void AddStableNoise(std::vector& camera_data, std::vector& point_data, + const std::vector& ptidx, const std::vector& camidx, float percent); +bool RemoveInvisiblePoints( std::vector& camera_data, std::vector& point_data, + std::vector& ptidx, std::vector& camidx, + std::vector& measurements, std::vector& names, std::vector& ptc); + +///////////////////////////////////////////////////////////////////////////// +bool LoadNVM(std::ifstream& in, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc) +{ + int rotation_parameter_num = 4; + bool format_r9t = false; + std::string token; + if(in.peek() == 'N') + { + in >> token; //file header + if(strstr(token.c_str(), "R9T")) + { + rotation_parameter_num = 9; //rotation as 3x3 matrix + format_r9t = true; + } + } + + double fxFixed, fyFixed, cxFixed, cyFixed, k1(0); + int ncam = 0, npoint = 0, nproj = 0; + in >> token; + if (token == "FixedK") { + // read fixed intrinsics + std::getline(in, token); + sscanf(token.c_str(), "%lf %lf %lf %lf %lf", &fxFixed, &cxFixed, &fyFixed, &cyFixed, &k1); + // read # of cameras + in >> ncam; + } else { + // read # of cameras + ncam = atoi(token.c_str()); + } + if(ncam <= 1) return false; + + //read the camera parameters + camera_data.resize(ncam); // allocate the camera data + names.resize(ncam); + for(int i = 0; i < ncam; ++i) + { + double f, q[9], c[3], d[2]; + in >> token >> f ; + for(int j = 0; j < rotation_parameter_num; ++j) in >> q[j]; + in >> c[0] >> c[1] >> c[2] >> d[0] >> d[1]; + + camera_data[i].SetFocalLength(f); + if(format_r9t) + { + camera_data[i].SetMatrixRotation(q); + camera_data[i].SetTranslation(c); + } + else + { + //older format for compatibility + camera_data[i].SetQuaternionRotation(q); //quaternion from the file + camera_data[i].SetCameraCenterAfterRotation(c); //camera center from the file + } + camera_data[i].SetNormalizedMeasurementDistortion(k1!=0 ? k1 : d[0]); + names[i] = token; + } + + ////////////////////////////////////// + in >> npoint; if(npoint <= 0) return false; + + //read image projections and 3D points. + point_data.resize(npoint); + for(int i = 0; i < npoint; ++i) + { + float pt[3]; int cc[3], npj; + in >> pt[0] >> pt[1] >> pt[2] + >> cc[0] >> cc[1] >> cc[2] >> npj; + for(int j = 0; j < npj; ++j) + { + int cidx, fidx; float imx, imy; + in >> cidx >> fidx >> imx >> imy; + + camidx.push_back(cidx); //camera index + ptidx.push_back(i); //point index + + //add a measurement to the vector + measurements.push_back(Point2D(imx, imy)); + nproj ++; + } + point_data[i].SetPoint(pt); + ptc.insert(ptc.end(), cc, cc + 3); + } + /////////////////////////////////////////////////////////////////////////////// + LOG_OUT() << ncam << " cameras; " << npoint << " 3D points; " << nproj << " projections\n"; + + return true; +} + + +void SaveNVM(const char* filename, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc) +{ + LOG_OUT() << "Saving model to " << filename << "...\n"; + std::ofstream out(filename); + + out << "NVM_V3_R9T\n" << camera_data.size() << '\n' << std::setprecision(12); + if(names.size() < camera_data.size()) names.resize(camera_data.size(),std::string("unknown")); + if(ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0); + + //////////////////////////////////// + for(size_t i = 0; i < camera_data.size(); ++i) + { + CameraT& cam = camera_data[i]; + out << names[i] << ' ' << cam.GetFocalLength() << ' '; + for(int j = 0; j < 9; ++j) out << cam.m[0][j] << ' '; + out << cam.t[0] << ' ' << cam.t[1] << ' ' << cam.t[2] << ' ' + << cam.GetNormalizedMeasurementDistortion() << " 0\n"; + } + + out << point_data.size() << '\n'; + + for(size_t i = 0, j = 0; i < point_data.size(); ++i) + { + Point3D& pt = point_data[i]; + int * pc = &ptc[i * 3]; + out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << ' ' + << pc[0] << ' ' << pc[1] << ' ' << pc[2] << ' '; + + size_t je = j; + while(je < ptidx.size() && ptidx[je] == (int) i) je++; + + out << (je - j) << ' '; + + for(; j < je; ++j) out << camidx[j] << ' ' << " 0 " << measurements[j].x << ' ' << measurements[j].y << ' '; + + out << '\n'; + } +} + + +bool LoadBundlerOut(const char* name, std::ifstream& in, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc) +{ + int rotation_parameter_num = 9; + std::string token; + while(in.peek() == '#') std::getline(in, token); + + char listpath[1024], filepath[1024]; + strcpy(listpath, name); + char* ext = strstr(listpath, ".out"); + strcpy(ext, "-list.txt\0"); + + /////////////////////////////////// + std::ifstream listin(listpath); + if(!listin.is_open()) + { + listin.close(); listin.clear(); + strcpy(ext, ".txt\0"); + listin.open(listpath); + } + if(!listin.is_open()) + { + listin.close(); listin.clear(); + char * slash = strrchr(listpath, '/'); + if(slash == NULL) slash = strrchr(listpath, '\\'); + slash = slash ? slash + 1 : listpath; + strcpy(slash, "image_list.txt"); + listin.open(listpath); + } + if(listin) LOG_OUT() << "Using image list: " << listpath << '\n'; + + // read # of cameras + int ncam = 0, npoint = 0, nproj = 0; + in >> ncam >> npoint; + if(ncam <= 1 || npoint <= 1) return false; + LOG_OUT() << ncam << " cameras; " << npoint << " 3D points;\n"; + + //read the camera parameters + camera_data.resize(ncam); // allocate the camera data + names.resize(ncam); + + bool det_checked = false; + for(int i = 0; i < ncam; ++i) + { + float f, q[9], c[3], d[2]; + in >> f >> d[0] >> d[1]; + for(int j = 0; j < rotation_parameter_num; ++j) in >> q[j]; + in >> c[0] >> c[1] >> c[2]; + + camera_data[i].SetFocalLength(f); + camera_data[i].SetInvertedR9T(q, c); + camera_data[i].SetProjectionDistortion(d[0]); + + if(listin >> filepath && f != 0) + { + names[i] = filepath; + std::getline(listin, token); + + if(!det_checked) + { + float det = camera_data[i].GetRotationMatrixDeterminant(); + LOG_OUT() << "Check rotation matrix: " << det << '\n'; + det_checked = true; + } + }else + { + names[i] = "unknown"; + } + } + + + //read image projections and 3D points. + point_data.resize(npoint); + for(int i = 0; i < npoint; ++i) + { + float pt[3]; int cc[3], npj; + in >> pt[0] >> pt[1] >> pt[2] + >> cc[0] >> cc[1] >> cc[2] >> npj; + for(int j = 0; j < npj; ++j) + { + int cidx, fidx; float imx, imy; + in >> cidx >> fidx >> imx >> imy; + + camidx.push_back(cidx); //camera index + ptidx.push_back(i); //point index + + //add a measurement to the vector + measurements.push_back(Point2D(imx, -imy)); + nproj ++; + } + point_data[i].SetPoint(pt[0], pt[1], pt[2]); + ptc.insert(ptc.end(), cc, cc + 3); + } + /////////////////////////////////////////////////////////////////////////////// + LOG_OUT() << ncam << " cameras; " << npoint << " 3D points; " << nproj << " projections\n"; + return true; +} + +void SaveBundlerOut(const char* filename, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc) +{ + char listpath[1024]; strcpy(listpath, filename); + char* ext = strstr(listpath, ".out"); if(ext == NULL) return; + strcpy(ext, "-list.txt\0"); + + std::ofstream out(filename); + out << "# Bundle file v0.3\n"; + out << std::setprecision(12); //need enough precision + out << camera_data.size() << " " << point_data.size() << '\n'; + + //save camera data + for(size_t i = 0; i < camera_data.size(); ++i) + { + float q[9], c[3]; + CameraT& ci = camera_data[i]; + out << ci.GetFocalLength() << ' ' << ci.GetProjectionDistortion() << " 0\n"; + ci.GetInvertedR9T(q, c); + for(int j = 0; j < 9; ++j) out << q[j] << (((j % 3) == 2)? '\n' : ' '); + out << c[0] << ' ' << c[1] << ' ' << c[2] << '\n'; + } + /// + for(size_t i = 0, j = 0; i < point_data.size(); ++i) + { + int npj = 0, *ci = &ptc[i * 3]; Point3D& pt = point_data[i]; + while(j + npj < point_data.size() && ptidx[j + npj] == ptidx[j]) npj++; + /////////////////////////// + out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n'; + out << ci[0] << ' ' << ci[1] << ' ' << ci[2] << '\n'; + out << npj << ' '; + for(int k = 0; k < npj; ++k) out << camidx[j + k] << " 0 " + << measurements[j + k].x << ' ' << -measurements[j + k].y << '\n'; + out << '\n'; j += npj; + } + + std::ofstream listout(listpath); + for(size_t i = 0; i < names.size(); ++i) listout << names[i] << '\n'; +} + +template +bool LoadBundlerModel(std::ifstream& in, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx) +{ + // read bundle data from a file + size_t ncam = 0, npt = 0, nproj = 0; + if(!(in >> ncam >> npt >> nproj)) return false; + /////////////////////////////////////////////////////////////////////////////// + LOG_OUT() << ncam << " cameras; " << npt << " 3D points; " << nproj << " projections\n"; + + camera_data.resize(ncam); + point_data.resize(npt); + measurements.resize(nproj); + camidx.resize(nproj); + ptidx.resize(nproj); + + for(size_t i = 0; i < nproj; ++i) + { + double x, y; int cidx, pidx; + in >> cidx >> pidx >> x >> y; + if(((size_t) pidx) == npt && camidx.size() > i) + { + camidx.resize(i); + ptidx.resize(i); + measurements.resize(i); + LOG_OUT() << "Truncate measurements to " << i << '\n'; + }else if(((size_t) pidx) >= npt) + { + continue; + }else + { + camidx[i] = cidx; ptidx[i] = pidx; + measurements[i].SetPoint2D(x, -y); + } + } + + for(size_t i = 0; i < ncam; ++i) + { + double p[9]; + for(int j = 0; j < 9; ++j) in >> p[j]; + CameraT& cam = camera_data[i]; + cam.SetFocalLength(p[6]); + cam.SetInvertedRT(p, p + 3); + cam.SetProjectionDistortion(p[7]); + } + + for(size_t i = 0; i < npt; ++i) + { + double pt[3]; + in >> pt[0] >> pt[1] >> pt[2]; + point_data[i].SetPoint(pt); + } + return true; +} + +void SaveBundlerModel(const char* filename, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx) +{ + LOG_OUT() << "Saving model to " << filename << "...\n"; + std::ofstream out(filename); + out << std::setprecision(12); //need enough precision + out << camera_data.size() << ' ' << point_data.size() << ' ' << measurements.size() << '\n'; + for(size_t i = 0; i < measurements.size(); ++i) + { + out << camidx[i] << ' ' << ptidx[i] << ' ' << measurements[i].x << ' ' << -measurements[i].y << '\n'; + } + + for(size_t i = 0; i < camera_data.size(); ++i) + { + CameraT& cam = camera_data[i]; + double r[3], t[3]; cam.GetInvertedRT(r, t); + out << r[0] << ' ' << r[1] << ' ' << r[2] << ' ' + << t[0] << ' ' << t[1] << ' ' << t[2] << ' ' << cam.f + << ' ' << cam.GetProjectionDistortion() << " 0\n"; + } + + for(size_t i = 0; i < point_data.size(); ++i) + { + Point3D& pt = point_data[i]; + out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n'; + } +} + +bool LoadModelFile(const char* name, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc) +{ + if(name == NULL)return false; + std::ifstream in(name); + + LOG_OUT() << "Loading cameras/points: " << name <<"\n" ; + if(!in.is_open()) return false; + + if(strstr(name, ".nvm"))return LoadNVM(in, camera_data, point_data, measurements, ptidx, camidx, names, ptc); + else if(strstr(name, ".out")) return LoadBundlerOut(name, in, camera_data, point_data, measurements, ptidx, camidx, names, ptc); + else return LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, camidx); +} + + +float random_ratio(float percent) +{ + return (rand() % 101 - 50) * 0.02f * percent + 1.0f; +} + +void AddNoise(std::vector& camera_data, std::vector& point_data, float percent) +{ + std::srand((unsigned int) time(NULL)); + for(size_t i = 0; i < camera_data.size(); ++i) + { + camera_data[i].f *= random_ratio(percent); + camera_data[i].t[0] *= random_ratio(percent); + camera_data[i].t[1] *= random_ratio(percent); + camera_data[i].t[2] *= random_ratio(percent); + double e[3]; + camera_data[i].GetRodriguesRotation(e); + e[0] *= random_ratio(percent); + e[1] *= random_ratio(percent); + e[2] *= random_ratio(percent); + camera_data[i].SetRodriguesRotation(e); + } + + for(size_t i = 0; i < point_data.size(); ++i) + { + point_data[i].xyz[0] *= random_ratio(percent); + point_data[i].xyz[1] *= random_ratio(percent); + point_data[i].xyz[2] *= random_ratio(percent); + } +} + +void AddStableNoise(std::vector& camera_data, std::vector& point_data, + const std::vector& ptidx, const std::vector& camidx, float percent) +{ + /// + std::srand((unsigned int) time(NULL)); + //do not modify the visibility status.. + std::vector zz0(ptidx.size()); + std::vector backup = camera_data; + std::vector vx(point_data.size()), vy(point_data.size()), vz(point_data.size()); + for(size_t i = 0; i < point_data.size(); ++i) + { + Point3D& pt = point_data[i]; + vx[i] = pt.xyz[0]; + vy[i] = pt.xyz[1]; + vz[i] = pt.xyz[2]; + } + + //find out the median location of all the 3D points. + size_t median_idx = point_data.size() / 2; + + std::nth_element(vx.begin(), vx.begin() + median_idx, vx.end()); + std::nth_element(vy.begin(), vy.begin() + median_idx, vy.end()); + std::nth_element(vz.begin(), vz.begin() + median_idx, vz.end()); + float cx = vx[median_idx], cy = vy[median_idx], cz = vz[median_idx]; + + for(size_t i = 0; i < ptidx.size(); ++i) + { + CameraT& cam = camera_data[camidx[i]]; + Point3D& pt = point_data[ptidx[i]]; + zz0[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + } + + std::vector z2 = zz0; median_idx = ptidx.size() / 2; + std::nth_element(z2.begin(), z2.begin() + median_idx, z2.end()); + float mz = z2[median_idx]; // median depth + float dist_noise_base = mz * 0.2f; + + ///////////////////////////////////////////////// + //modify points first.. + for(size_t i = 0; i < point_data.size(); ++i) + { + Point3D& pt = point_data[i]; + pt.xyz[0] = pt.xyz[0] - cx + dist_noise_base * random_ratio(percent); + pt.xyz[1] = pt.xyz[1] - cy + dist_noise_base * random_ratio(percent); + pt.xyz[2] = pt.xyz[2] - cz + dist_noise_base * random_ratio(percent); + } + + std::vector need_modification(camera_data.size(), true); + int invalid_count = 0, modify_iteration = 1; + + do + { + if(invalid_count) LOG_OUT() << "NOTE" << std::setw(2) << modify_iteration + << ": modify " << invalid_count << " camera to fix visibility\n"; + + ////////////////////////////////////////////////////// + for(size_t i = 0; i < camera_data.size(); ++i) + { + if(!need_modification[i])continue; + CameraT & cam = camera_data[i]; + double e[3], c[3]; cam = backup[i]; + cam.f *= random_ratio(percent); + + /////////////////////////////////////////////////////////// + cam.GetCameraCenter(c); + c[0] = c[0] - cx + dist_noise_base * random_ratio(percent); + c[1] = c[1] - cy + dist_noise_base * random_ratio(percent); + c[2] = c[2] - cz + dist_noise_base * random_ratio(percent); + + /////////////////////////////////////////////////////////// + cam.GetRodriguesRotation(e); + e[0] *= random_ratio(percent); + e[1] *= random_ratio(percent); + e[2] *= random_ratio(percent); + + /////////////////////////////////////////////////////////// + cam.SetRodriguesRotation(e); + cam.SetCameraCenterAfterRotation(c); + } + std::vector invalidc(camera_data.size(), false); + + invalid_count = 0; + for(size_t i = 0; i < ptidx.size(); ++i) + { + int cid = camidx[i]; + if(need_modification[cid] ==false) continue; + if(invalidc[cid])continue; + CameraT& cam = camera_data[cid]; + Point3D& pt = point_data[ptidx[i]]; + float z = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + if (z * zz0[i] > 0)continue; + if (zz0[i] == 0 && z > 0) continue; + invalid_count++; + invalidc[cid] = true; + } + + need_modification = invalidc; + modify_iteration++; + + }while(invalid_count && modify_iteration < 20); + +} + +void ExamineVisiblity(const char* input_filename ) +{ + + ////////////// + std::vector camera_data; + std::vector point_data; + std::vector ptidx, camidx; + std::vector measurements; + std::ifstream in (input_filename); + LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, camidx); + + //////////////// + int count = 0; double d1 = 100, d2 = 100; + LOG_OUT() << "checking visibility...\n"; + std::vector zz(ptidx.size()); + for(size_t i = 0; i < ptidx.size(); ++i) + { + CameraD& cam = camera_data[camidx[i]]; + Point3B& pt = point_data[ptidx[i]]; + double dz = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + //double dx = cam.m[0][0] * pt.xyz[0] + cam.m[0][1] * pt.xyz[1] + cam.m[0][2] * pt.xyz[2] + cam.t[0]; + //double dy = cam.m[1][0] * pt.xyz[0] + cam.m[1][1] * pt.xyz[1] + cam.m[1][2] * pt.xyz[2] + cam.t[1]; + + //////////////////////////////////////// + float c[3]; cam.GetCameraCenter(c); + + CameraT camt; camt.SetCameraT(cam); + Point3D ptt; ptt.SetPoint(pt.xyz); + double fz = camt.m[2][0] * ptt.xyz[0] + camt.m[2][1] * ptt.xyz[1] + camt.m[2][2] * ptt.xyz[2] + camt.t[2]; + double fz2 = camt.m[2][0] * (ptt.xyz[0] - c[0]) + camt.m[2][1] * (ptt.xyz[1] - c[1]) + + camt.m[2][2] * (ptt.xyz[2] - c[2]); + + + //if(dz == 0 && fz == 0) continue; + + if(dz * fz <= 0 || fz == 0) + { + LOG_OUT() << "cam " << camidx[i] //<& camera_data, std::vector& point_data, + std::vector& ptidx, std::vector& camidx, + std::vector& measurements, std::vector& names, std::vector& ptc) +{ + std::vector zz(ptidx.size()); + for(size_t i = 0; i < ptidx.size(); ++i) + { + CameraT& cam = camera_data[camidx[i]]; + Point3D& pt = point_data[ptidx[i]]; + zz[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + } + size_t median_idx = ptidx.size() / 2; + std::nth_element(zz.begin(), zz.begin() + median_idx, zz.end()); + float dist_threshold = zz[median_idx] * 0.001f; + + //keep removing 3D points. until all of them are infront of the cameras.. + std::vector pmask(point_data.size(), true); + int points_removed = 0; + for(size_t i = 0; i < ptidx.size(); ++i) + { + int cid = camidx[i], pid = ptidx[i]; + if(!pmask[pid])continue; + CameraT& cam = camera_data[cid]; + Point3D& pt = point_data[pid]; + bool visible = (cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2] > dist_threshold); + pmask[pid] = visible; //this point should be removed + if(!visible) points_removed++; + } + if(points_removed == 0) return false; + std::vector cv(camera_data.size(), 0); + //should any cameras be removed ? + int min_observation = 20; //cameras should see at least 20 points + + do + { + //count visible points for each camera + std::fill(cv.begin(), cv.end(), 0); + for(size_t i = 0; i < ptidx.size(); ++i) + { + int cid = camidx[i], pid = ptidx[i]; + if(pmask[pid]) cv[cid]++; + } + + //check if any more points should be removed + std::vector pv(point_data.size(), 0); + for(size_t i = 0; i < ptidx.size(); ++i) + { + int cid = camidx[i], pid = ptidx[i]; + if(!pmask[pid]) continue; //point already removed + if(cv[cid] < min_observation) //this camera shall be removed. + { + /// + }else + { + pv[pid]++; + } + } + + points_removed = 0; + for(size_t i = 0; i < point_data.size(); ++i) + { + if(pmask[i] == false) continue; + if(pv[i] >= 2) continue; + pmask[i] = false; + points_removed++; + } + }while(points_removed > 0); + + //////////////////////////////////// + std::vector cmask(camera_data.size(), true); + for(size_t i = 0; i < camera_data.size(); ++i) cmask[i] = cv[i] >= min_observation; + //////////////////////////////////////////////////////// + + std::vector cidx(camera_data.size()); + std::vector pidx(point_data.size()); + + + + + ///modified model. + std::vector camera_data2; + std::vector point_data2; + std::vector ptidx2; + std::vector camidx2; + std::vector measurements2; + std::vector names2; + std::vector ptc2; + + + // + if(names.size() < camera_data.size()) names.resize(camera_data.size(),std::string("unknown")); + if(ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0); + + ////////////////////////////// + int new_camera_count = 0, new_point_count = 0; + for(size_t i = 0; i < camera_data.size(); ++i) + { + if(!cmask[i])continue; + camera_data2.push_back(camera_data[i]); + names2.push_back(names[i]); + cidx[i] = new_camera_count++; + } + + for(size_t i = 0; i < point_data.size(); ++i) + { + if(!pmask[i])continue; + point_data2.push_back(point_data[i]); + ptc.push_back(ptc[i]); + pidx[i] = new_point_count++; + } + + int new_observation_count = 0; + for(size_t i = 0; i < ptidx.size(); ++i) + { + int pid = ptidx[i], cid = camidx[i]; + if(!pmask[pid] || ! cmask[cid]) continue; + ptidx2.push_back(pidx[pid]); + camidx2.push_back(cidx[cid]); + measurements2.push_back(measurements[i]); + new_observation_count++; + } + + LOG_OUT() << "NOTE: removing " << (camera_data.size() - new_camera_count) << " cameras; "<< (point_data.size() - new_point_count) + << " 3D Points; " << (measurements.size() - new_observation_count) << " Observations;\n"; + + camera_data2.swap(camera_data); names2.swap(names); + point_data2.swap(point_data); ptc2.swap(ptc); + ptidx2.swap(ptidx); camidx2.swap(camidx); + measurements2.swap(measurements); + + return true; +} + +void SaveModelFile(const char* outpath, std::vector& camera_data, std::vector& point_data, + std::vector& measurements, std::vector& ptidx, std::vector& camidx, + std::vector& names, std::vector& ptc) +{ + if(outpath == NULL) return; + if(strstr(outpath, ".nvm")) + SaveNVM(outpath, camera_data, point_data, measurements, ptidx, camidx, names, ptc); + else if(strstr(outpath, ".out")) + SaveBundlerOut(outpath, camera_data, point_data, measurements, ptidx, camidx, names, ptc); + else + SaveBundlerModel(outpath, camera_data, point_data, measurements, ptidx, camidx); +} + +} // namespace PBA diff --git a/apps/ReconstructMesh/CMakeLists.txt b/apps/ReconstructMesh/CMakeLists.txt new file mode 100644 index 0000000..4b6aac7 --- /dev/null +++ b/apps/ReconstructMesh/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(ReconstructMesh "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS ReconstructMesh + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/ReconstructMesh/ReconstructMesh.cpp b/apps/ReconstructMesh/ReconstructMesh.cpp new file mode 100644 index 0000000..f7c0f6d --- /dev/null +++ b/apps/ReconstructMesh/ReconstructMesh.cpp @@ -0,0 +1,498 @@ +/* + * ReconstructMesh.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("ReconstructMesh") + +// uncomment to enable multi-threading based on OpenMP +#ifdef _USE_OPENMP +#define RECMESH_USE_OPENMP +#endif + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strPointCloudFileName; +String strOutputFileName; +String strMeshFileName; +String strImportROIFileName; +String strImagePointsFileName; +bool bMeshExport; +float fDistInsert; +bool bUseOnlyROI; +bool bUseConstantWeight; +bool bUseFreeSpaceSupport; +float fThicknessFactor; +float fQualityFactor; +float fDecimateMesh; +unsigned nTargetFaceNum; +float fRemoveSpurious; +bool bRemoveSpikes; +unsigned nCloseHoles; +unsigned nSmoothMesh; +float fEdgeLength; +bool bCrop2ROI; +float fBorderROI; +float fSplitMaxArea; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strExportType; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("export-type", boost::program_options::value(&OPT::strExportType)->default_value(_T("ply")), "file type used to export the 3D scene (ply or obj)") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + #ifdef _USE_CUDA + ("cuda-device", boost::program_options::value(&CUDA::desiredDeviceID)->default_value(-1), "CUDA device number to be used to reconstruct the mesh (-2 - CPU processing, -1 - best GPU, >=0 - device index)") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config_main("Reconstruct options"); + config_main.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses and image list") + ("pointcloud-file,p", boost::program_options::value(&OPT::strPointCloudFileName), "dense point-cloud with views file name to reconstruct (overwrite existing point-cloud)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the mesh") + ("min-point-distance,d", boost::program_options::value(&OPT::fDistInsert)->default_value(2.5f), "minimum distance in pixels between the projection of two 3D points to consider them different while triangulating (0 - disabled)") + ("integrate-only-roi", boost::program_options::value(&OPT::bUseOnlyROI)->default_value(false), "use only the points inside the ROI") + ("constant-weight", boost::program_options::value(&OPT::bUseConstantWeight)->default_value(true), "considers all view weights 1 instead of the available weight") + ("free-space-support,f", boost::program_options::value(&OPT::bUseFreeSpaceSupport)->default_value(false), "exploits the free-space support in order to reconstruct weakly-represented surfaces") + ("thickness-factor", boost::program_options::value(&OPT::fThicknessFactor)->default_value(1.f), "multiplier adjusting the minimum thickness considered during visibility weighting") + ("quality-factor", boost::program_options::value(&OPT::fQualityFactor)->default_value(1.f), "multiplier adjusting the quality weight considered during graph-cut") + ; + boost::program_options::options_description config_clean("Clean options"); + config_clean.add_options() + ("decimate", boost::program_options::value(&OPT::fDecimateMesh)->default_value(1.f), "decimation factor in range (0..1] to be applied to the reconstructed surface (1 - disabled)") + ("target-face-num", boost::program_options::value(&OPT::nTargetFaceNum)->default_value(0), "target number of faces to be applied to the reconstructed surface. (0 - disabled)") + ("remove-spurious", boost::program_options::value(&OPT::fRemoveSpurious)->default_value(20.f), "spurious factor for removing faces with too long edges or isolated components (0 - disabled)") + ("remove-spikes", boost::program_options::value(&OPT::bRemoveSpikes)->default_value(true), "flag controlling the removal of spike faces") + ("close-holes", boost::program_options::value(&OPT::nCloseHoles)->default_value(30), "try to close small holes in the reconstructed surface (0 - disabled)") + ("smooth", boost::program_options::value(&OPT::nSmoothMesh)->default_value(2), "number of iterations to smooth the reconstructed surface (0 - disabled)") + ("edge-length", boost::program_options::value(&OPT::fEdgeLength)->default_value(0.f), "remesh such that the average edge length is this size (0 - disabled)") + ("roi-border", boost::program_options::value(&OPT::fBorderROI)->default_value(0), "add a border to the region-of-interest when cropping the scene (0 - disabled, >0 - percentage, <0 - absolute)") + ("crop-to-roi", boost::program_options::value(&OPT::bCrop2ROI)->default_value(true), "crop scene using the region-of-interest") + ; + + // hidden options, allowed both on command line and + // in config file, but will not be shown to the user + boost::program_options::options_description hidden("Hidden options"); + hidden.add_options() + ("mesh-file", boost::program_options::value(&OPT::strMeshFileName), "mesh file name to clean (skips the reconstruction step)") + ("mesh-export", boost::program_options::value(&OPT::bMeshExport)->default_value(false), "just export the mesh contained in loaded project") + ("split-max-area", boost::program_options::value(&OPT::fSplitMaxArea)->default_value(0.f), "maximum surface area that a sub-mesh can contain (0 - disabled)") + ("import-roi-file", boost::program_options::value(&OPT::strImportROIFileName), "ROI file name to be imported into the scene") + ("image-points-file", boost::program_options::value(&OPT::strImagePointsFileName), "input filename containing the list of points from an image to project on the mesh (optional)") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config_main).add(config_clean).add(hidden); + + boost::program_options::options_description config_file_options; + config_file_options.add(config_main).add(config_clean).add(hidden); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + if (OPT::vm.count("help") || OPT::strInputFileName.empty()) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config_main).add(config_clean); + GET_LOG() << visible; + } + if (OPT::strInputFileName.empty()) + return false; + OPT::strExportType = OPT::strExportType.ToLower() == _T("obj") ? _T(".obj") : _T(".ply"); + + // initialize optional options + Util::ensureValidPath(OPT::strPointCloudFileName); + Util::ensureValidPath(OPT::strOutputFileName); + Util::ensureValidPath(OPT::strImportROIFileName); + Util::ensureValidPath(OPT::strImagePointsFileName); + Util::ensureValidPath(OPT::strMeshFileName); + if (OPT::strPointCloudFileName.empty() && (ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS) + OPT::strPointCloudFileName = Util::getFileFullName(OPT::strInputFileName) + _T(".ply"); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFileFullName(OPT::strInputFileName) + _T("_mesh.mvs"); + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + + +// export 3D coordinates corresponding to 2D coordinates provided by inputFileName: +// parse image point list; first line is the name of the image to project, +// each consequent line store the xy coordinates to project: +// +// +// +// ... +// +// for example: +// N01.JPG 3 +// 3090 2680 +// 3600 2100 +// 3640 2190 +bool Export3DProjections(Scene& scene, const String& inputFileName) { + SML smlPointList(_T("ImagePoints")); + smlPointList.Load(inputFileName); + ASSERT(smlPointList.GetArrChildren().size() <= 1); + IDX idx(0); + + // read image name + size_t argc; + CAutoPtrArr argv; + while (true) { + argv = Util::CommandLineToArgvA(smlPointList.GetValue(idx).val, argc); + if (argc > 0 && argv[0][0] != _T('#')) + break; + if (++idx == smlPointList.size()) + return false; + } + if (argc < 2) + return false; + String imgName(argv[0]); + IIndex imgID(NO_ID); + for (const Image& imageData : scene.images) { + if (!imageData.IsValid()) + continue; + if (imageData.name.substr(imageData.name.size() - imgName.size()) == imgName) { + imgID = imageData.ID; + break; + } + } + if (imgID == NO_ID) { + VERBOSE("Unable to find image named: %s", imgName.c_str()); + return false; + } + + // read image points + std::vector imagePoints; + while (++idx != smlPointList.size()) { + // parse image element + const String& line(smlPointList.GetValue(idx).val); + argv = Util::CommandLineToArgvA(line, argc); + if (argc > 0 && argv[0][0] == _T('#')) + continue; + if (argc < 2) { + VERBOSE("Invalid image coordinates: %s", line.c_str()); + continue; + } + const Point2f pt( + String::FromString(argv[0], -1), + String::FromString(argv[1], -1)); + if (pt.x > 0 && pt.y > 0) + imagePoints.emplace_back(pt); + } + if (imagePoints.empty()) { + VERBOSE("Unable to read image points from: %s", imgName.c_str()); + return false; + } + + // prepare output file + String outFileName(Util::insertBeforeFileExt(inputFileName, "_3D")); + File oStream(outFileName, File::WRITE, File::CREATE | File::TRUNCATE); + if (!oStream.isOpen()) { + VERBOSE("Unable to open output file: %s", outFileName.c_str()); + return false; + } + + // print image name + oStream.print("%s %u\n", imgName.c_str(), imagePoints.size()); + + // init mesh octree + const Mesh::Octree octree(scene.mesh.vertices, [](Mesh::Octree::IDX_TYPE size, Mesh::Octree::Type /*radius*/) { + return size > 256; + }); + scene.mesh.ListIncidenteFaces(); + + // save 3D coord in the output file + const Image& imgToExport = scene.images[imgID]; + for (const Point2f& pt : imagePoints) { + // define ray from camera center to each x,y image coord + const Ray3 ray(imgToExport.camera.C, normalized(imgToExport.camera.RayPoint(pt))); + // find ray intersection with the mesh + const IntersectRayMesh intRay(octree, ray, scene.mesh); + if (intRay.pick.IsValid()) { + const Point3d ptHit(ray.GetPoint(intRay.pick.dist)); + oStream.print("%.7f %.7f %.7f\n", ptHit.x, ptHit.y, ptHit.z); + } else + oStream.print("NA\n"); + } + return true; +} + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + Scene scene(OPT::nMaxThreads); + // load project + const Scene::SCENE_TYPE sceneType(scene.Load(MAKE_PATH_SAFE(OPT::strInputFileName), + OPT::fSplitMaxArea > 0 || OPT::fDecimateMesh < 1 || OPT::nTargetFaceNum > 0 || !OPT::strImportROIFileName.empty())); + if (sceneType == Scene::SCENE_NA) + return EXIT_FAILURE; + if (!OPT::strPointCloudFileName.empty() && (File::isFile(MAKE_PATH_SAFE(OPT::strPointCloudFileName)) ? + !scene.pointcloud.Load(MAKE_PATH_SAFE(OPT::strPointCloudFileName)) : + !scene.pointcloud.IsValid())) { + VERBOSE("error: cannot load point-cloud file"); + return EXIT_FAILURE; + } + if (!OPT::strMeshFileName.empty() && !scene.mesh.Load(MAKE_PATH_SAFE(OPT::strMeshFileName))) { + VERBOSE("error: cannot load mesh file"); + return EXIT_FAILURE; + } + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))); + if (OPT::fSplitMaxArea > 0) { + // split mesh using max-area constraint + Mesh::FacesChunkArr chunks; + if (scene.mesh.Split(chunks, OPT::fSplitMaxArea)) + scene.mesh.Save(chunks, baseFileName); + return EXIT_SUCCESS; + } + + if (!OPT::strImportROIFileName.empty()) { + std::ifstream fs(MAKE_PATH_SAFE(OPT::strImportROIFileName)); + if (!fs) + return EXIT_FAILURE; + fs >> scene.obb; + if (OPT::bCrop2ROI && !scene.mesh.IsEmpty() && !scene.IsValid()) { + TD_TIMER_START(); + const size_t numVertices = scene.mesh.vertices.size(); + const size_t numFaces = scene.mesh.faces.size(); + scene.mesh.RemoveFacesOutside(scene.obb); + VERBOSE("Mesh trimmed to ROI: %u vertices and %u faces removed (%s)", + numVertices-scene.mesh.vertices.size(), numFaces-scene.mesh.faces.size(), TD_TIMER_GET_FMT().c_str()); + scene.mesh.Save(baseFileName+OPT::strExportType); + return EXIT_SUCCESS; + } + } + + if (!OPT::strImagePointsFileName.empty() && !scene.mesh.IsEmpty()) { + Export3DProjections(scene, MAKE_PATH_SAFE(OPT::strImagePointsFileName)); + return EXIT_SUCCESS; + } + + if (OPT::bMeshExport) { + // check there is a mesh to export + if (scene.mesh.IsEmpty()) + return EXIT_FAILURE; + // save mesh + const String fileName(MAKE_PATH_SAFE(OPT::strOutputFileName)); + scene.mesh.Save(fileName); + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 2) + scene.ExportCamerasMLP(baseFileName+_T(".mlp"), fileName); + #endif + } else { + const OBB3f initialOBB(scene.obb); + if (OPT::fBorderROI > 0) + scene.obb.EnlargePercent(OPT::fBorderROI); + else if (OPT::fBorderROI < 0) + scene.obb.Enlarge(-OPT::fBorderROI); + if (OPT::strMeshFileName.empty() && scene.mesh.IsEmpty()) { + // reset image resolution to the original size and + // make sure the image neighbors are initialized before deleting the point-cloud + #ifdef RECMESH_USE_OPENMP + bool bAbort(false); + #pragma omp parallel for + for (int_t idx=0; idx<(int_t)scene.images.size(); ++idx) { + #pragma omp flush (bAbort) + if (bAbort) + continue; + const uint32_t idxImage((uint32_t)idx); + #else + FOREACH(idxImage, scene.images) { + #endif + Image& imageData = scene.images[idxImage]; + if (!imageData.IsValid()) + continue; + // reset image resolution + if (!imageData.ReloadImage(0, false)) { + #ifdef RECMESH_USE_OPENMP + bAbort = true; + #pragma omp flush (bAbort) + continue; + #else + return EXIT_FAILURE; + #endif + } + imageData.UpdateCamera(scene.platforms); + // select neighbor views + if (imageData.neighbors.empty()) { + IndexArr points; + scene.SelectNeighborViews(idxImage, points); + } + } + #ifdef RECMESH_USE_OPENMP + if (bAbort) + return EXIT_FAILURE; + #endif + // reconstruct a coarse mesh from the given point-cloud + TD_TIMER_START(); + if (OPT::bUseConstantWeight) + scene.pointcloud.pointWeights.Release(); + if (!scene.ReconstructMesh(OPT::fDistInsert, OPT::bUseFreeSpaceSupport, OPT::bUseOnlyROI, 4, OPT::fThicknessFactor, OPT::fQualityFactor)) + return EXIT_FAILURE; + VERBOSE("Mesh reconstruction completed: %u vertices, %u faces (%s)", scene.mesh.vertices.GetSize(), scene.mesh.faces.GetSize(), TD_TIMER_GET_FMT().c_str()); + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 2) { + // dump raw mesh + scene.mesh.Save(baseFileName+_T("_raw")+OPT::strExportType); + } + #endif + } else if (!OPT::strMeshFileName.empty()) { + // load existing mesh to clean + scene.mesh.Load(MAKE_PATH_SAFE(OPT::strMeshFileName)); + } + + // clean the mesh + if (OPT::bCrop2ROI && scene.IsBounded()) { + TD_TIMER_START(); + const size_t numVertices = scene.mesh.vertices.size(); + const size_t numFaces = scene.mesh.faces.size(); + scene.mesh.RemoveFacesOutside(scene.obb); + VERBOSE("Mesh trimmed to ROI: %u vertices and %u faces removed (%s)", + numVertices-scene.mesh.vertices.size(), numFaces-scene.mesh.faces.size(), TD_TIMER_GET_FMT().c_str()); + } + const float fDecimate(OPT::nTargetFaceNum ? static_cast(OPT::nTargetFaceNum) / scene.mesh.faces.size() : OPT::fDecimateMesh); + scene.mesh.Clean(fDecimate, OPT::fRemoveSpurious, OPT::bRemoveSpikes, OPT::nCloseHoles, OPT::nSmoothMesh, OPT::fEdgeLength, false); + scene.mesh.Clean(1.f, 0.f, OPT::bRemoveSpikes, OPT::nCloseHoles, 0u, 0.f, false); // extra cleaning trying to close more holes + scene.mesh.Clean(1.f, 0.f, false, 0u, 0u, 0.f, true); // extra cleaning to remove non-manifold problems created by closing holes + scene.obb = initialOBB; + + // save the final mesh + scene.mesh.Save(baseFileName+OPT::strExportType); + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 2) + scene.ExportCamerasMLP(baseFileName+_T(".mlp"), baseFileName+OPT::strExportType); + #endif + if ((ARCHIVE_TYPE)OPT::nArchiveType != ARCHIVE_MVS || sceneType != Scene::SCENE_INTERFACE) + scene.Save(baseFileName+_T(".mvs"), (ARCHIVE_TYPE)OPT::nArchiveType); + } + + if (!OPT::strImagePointsFileName.empty()) { + Export3DProjections(scene, MAKE_PATH_SAFE(OPT::strImagePointsFileName)); + return EXIT_SUCCESS; + } + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/RefineMesh/CMakeLists.txt b/apps/RefineMesh/CMakeLists.txt new file mode 100644 index 0000000..26a6f84 --- /dev/null +++ b/apps/RefineMesh/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(RefineMesh "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS RefineMesh + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/RefineMesh/RefineMesh.cpp b/apps/RefineMesh/RefineMesh.cpp new file mode 100644 index 0000000..3aa6230 --- /dev/null +++ b/apps/RefineMesh/RefineMesh.cpp @@ -0,0 +1,266 @@ +/* + * RefineMesh.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("RefineMesh") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strMeshFileName; +String strOutputFileName; +unsigned nResolutionLevel; +unsigned nMinResolution; +unsigned nMaxViews; +float fDecimateMesh; +unsigned nCloseHoles; +unsigned nEnsureEdgeSize; +unsigned nScales; +float fScaleStep; +unsigned nReduceMemory; +unsigned nAlternatePair; +float fRegularityWeight; +float fRatioRigidityElasticity; +unsigned nMaxFaceArea; +float fPlanarVertexRatio; +float fGradientStep; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +String strExportType; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("export-type", boost::program_options::value(&OPT::strExportType)->default_value(_T("ply")), "file type used to export the 3D scene (ply or obj)") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + #ifdef _USE_CUDA + ("cuda-device", boost::program_options::value(&CUDA::desiredDeviceID)->default_value(-2), "CUDA device number to be used for mesh refinement (-2 - CPU processing, -1 - best GPU, >=0 - device index)") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Refine options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses and image list") + ("mesh-file,m", boost::program_options::value(&OPT::strMeshFileName), "mesh file name to refine (overwrite existing mesh)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the mesh") + ("resolution-level", boost::program_options::value(&OPT::nResolutionLevel)->default_value(0), "how many times to scale down the images before mesh refinement") + ("min-resolution", boost::program_options::value(&OPT::nMinResolution)->default_value(640), "do not scale images lower than this resolution") + ("max-views", boost::program_options::value(&OPT::nMaxViews)->default_value(8), "maximum number of neighbor images used to refine the mesh") + ("decimate", boost::program_options::value(&OPT::fDecimateMesh)->default_value(0.f), "decimation factor in range [0..1] to be applied to the input surface before refinement (0 - auto, 1 - disabled)") + ("close-holes", boost::program_options::value(&OPT::nCloseHoles)->default_value(30), "try to close small holes in the input surface (0 - disabled)") + ("ensure-edge-size", boost::program_options::value(&OPT::nEnsureEdgeSize)->default_value(1), "ensure edge size and improve vertex valence of the input surface (0 - disabled, 1 - auto, 2 - force)") + ("max-face-area", boost::program_options::value(&OPT::nMaxFaceArea)->default_value(32), "maximum face area projected in any pair of images that is not subdivided (0 - disabled)") + ("scales", boost::program_options::value(&OPT::nScales)->default_value(2), "how many iterations to run mesh optimization on multi-scale images") + ("scale-step", boost::program_options::value(&OPT::fScaleStep)->default_value(0.5f), "image scale factor used at each mesh optimization step") + ("alternate-pair", boost::program_options::value(&OPT::nAlternatePair)->default_value(0), "refine mesh using an image pair alternatively as reference (0 - both, 1 - alternate, 2 - only left, 3 - only right)") + ("regularity-weight", boost::program_options::value(&OPT::fRegularityWeight)->default_value(0.2f), "scalar regularity weight to balance between photo-consistency and regularization terms during mesh optimization") + ("rigidity-elasticity-ratio", boost::program_options::value(&OPT::fRatioRigidityElasticity)->default_value(0.9f), "scalar ratio used to compute the regularity gradient as a combination of rigidity and elasticity") + ("gradient-step", boost::program_options::value(&OPT::fGradientStep)->default_value(45.05f), "gradient step to be used instead (0 - auto)") + ("planar-vertex-ratio", boost::program_options::value(&OPT::fPlanarVertexRatio)->default_value(0.f), "threshold used to remove vertices on planar patches (0 - disabled)") + ("reduce-memory", boost::program_options::value(&OPT::nReduceMemory)->default_value(1), "recompute some data in order to reduce memory requirements") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + if (OPT::vm.count("help") || OPT::strInputFileName.empty()) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (OPT::strInputFileName.empty()) + return false; + OPT::strExportType = OPT::strExportType.ToLower() == _T("obj") ? _T(".obj") : _T(".ply"); + + // initialize optional options + Util::ensureValidPath(OPT::strMeshFileName); + Util::ensureValidPath(OPT::strOutputFileName); + if (OPT::strMeshFileName.empty() && (ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS) + OPT::strMeshFileName = Util::getFileFullName(OPT::strInputFileName) + _T(".ply"); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFileFullName(OPT::strInputFileName) + _T("_refine.mvs"); + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + Scene scene(OPT::nMaxThreads); + // load and refine the coarse mesh + const Scene::SCENE_TYPE sceneType(scene.Load(MAKE_PATH_SAFE(OPT::strInputFileName))); + if (sceneType == Scene::SCENE_NA) + return EXIT_FAILURE; + if (!OPT::strMeshFileName.empty() && !scene.mesh.Load(MAKE_PATH_SAFE(OPT::strMeshFileName))) { + VERBOSE("error: cannot load mesh file"); + return EXIT_FAILURE; + } + if (scene.mesh.IsEmpty()) { + VERBOSE("error: empty initial mesh"); + return EXIT_FAILURE; + } + TD_TIMER_START(); + #ifdef _USE_CUDA + if (CUDA::desiredDeviceID < -1 || + !scene.RefineMeshCUDA(OPT::nResolutionLevel, OPT::nMinResolution, OPT::nMaxViews, + OPT::fDecimateMesh, OPT::nCloseHoles, OPT::nEnsureEdgeSize, + OPT::nMaxFaceArea, + OPT::nScales, OPT::fScaleStep, + OPT::nAlternatePair>10 ? OPT::nAlternatePair%10 : 0, + OPT::fRegularityWeight, + OPT::fRatioRigidityElasticity, + OPT::fGradientStep)) + #endif + if (!scene.RefineMesh(OPT::nResolutionLevel, OPT::nMinResolution, OPT::nMaxViews, + OPT::fDecimateMesh, OPT::nCloseHoles, OPT::nEnsureEdgeSize, + OPT::nMaxFaceArea, + OPT::nScales, OPT::fScaleStep, + OPT::nAlternatePair, + OPT::fRegularityWeight, + OPT::fRatioRigidityElasticity, + OPT::fGradientStep, + OPT::fPlanarVertexRatio, + OPT::nReduceMemory)) + return EXIT_FAILURE; + VERBOSE("Mesh refinement completed: %u vertices, %u faces (%s)", scene.mesh.vertices.GetSize(), scene.mesh.faces.GetSize(), TD_TIMER_GET_FMT().c_str()); + + // save the final mesh + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))); + scene.mesh.Save(baseFileName+OPT::strExportType); + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 2) + scene.ExportCamerasMLP(baseFileName+_T(".mlp"), baseFileName+OPT::strExportType); + #endif + if ((ARCHIVE_TYPE)OPT::nArchiveType != ARCHIVE_MVS || sceneType != Scene::SCENE_INTERFACE) + scene.Save(baseFileName+_T(".mvs"), (ARCHIVE_TYPE)OPT::nArchiveType); + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/Tests/CMakeLists.txt b/apps/Tests/CMakeLists.txt new file mode 100644 index 0000000..192178b --- /dev/null +++ b/apps/Tests/CMakeLists.txt @@ -0,0 +1,15 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +ADD_DEFINITIONS(-D_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/data/") + +cxx_executable_with_flags(Tests "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS Tests + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/Tests/Tests.cpp b/apps/Tests/Tests.cpp new file mode 100644 index 0000000..731c04d --- /dev/null +++ b/apps/Tests/Tests.cpp @@ -0,0 +1,136 @@ +/* + * Tests.cpp + * + * Copyright (c) 2014-2021 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("Tests") + + +// S T R U C T S /////////////////////////////////////////////////// + +// test various algorithms independently +bool UnitTests() +{ + TD_TIMER_START(); + if (!SEACAVE::cListTest(100)) { + VERBOSE("ERROR: cListTest failed!"); + return false; + } + if (!SEACAVE::OctreeTest(100)) { + VERBOSE("ERROR: OctreeTest failed!"); + return false; + } + if (!SEACAVE::OctreeTest(100)) { + VERBOSE("ERROR: OctreeTest failed!"); + return false; + } + if (!SEACAVE::TestRayTriangleIntersection(1000)) { + VERBOSE("ERROR: TestRayTriangleIntersection failed!"); + return false; + } + if (!SEACAVE::TestRayTriangleIntersection(1000)) { + VERBOSE("ERROR: TestRayTriangleIntersection failed!"); + return false; + } + VERBOSE("All unit tests passed (%s)", TD_TIMER_GET_FMT().c_str()); + return true; +} + + +// test MVS stages on a small sample dataset +bool PipelineTest(bool verbose=false) +{ + TD_TIMER_START(); + Scene scene; + if (!scene.Load(MAKE_PATH("scene.mvs"))) { + VERBOSE("ERROR: TestDataset failed loading the scene!"); + return false; + } + OPTDENSE::init(); + OPTDENSE::bRemoveDmaps = true; + if (!scene.DenseReconstruction() || scene.pointcloud.GetSize() < 200000u) { + VERBOSE("ERROR: TestDataset failed estimating dense point cloud!"); + return false; + } + if (verbose) + scene.pointcloud.Save(MAKE_PATH("scene_dense.ply")); + if (!scene.ReconstructMesh() || scene.mesh.faces.size() < 75000u) { + VERBOSE("ERROR: TestDataset failed reconstructing the mesh!"); + return false; + } + if (verbose) + scene.mesh.Save(MAKE_PATH("scene_dense_mesh.ply")); + constexpr float decimate = 0.5f; + scene.mesh.Clean(decimate); + if (!ISINSIDE(scene.mesh.faces.size(), 35000u, 45000u)) { + VERBOSE("ERROR: TestDataset failed cleaning the mesh!"); + return false; + } + #ifdef _USE_OPENMP + TestMeshProjectionMT(scene.mesh, scene.images[1]); + #endif + if (!scene.TextureMesh(0, 0) || !scene.mesh.HasTexture()) { + VERBOSE("ERROR: TestDataset failed texturing the mesh!"); + return false; + } + if (verbose) + scene.mesh.Save(MAKE_PATH("scene_dense_mesh_texture.ply")); + VERBOSE("All pipeline stages passed (%s)", TD_TIMER_GET_FMT().c_str()); + return true; +} + +// test OpenMVS functionality +int main(int argc, LPCTSTR* argv) +{ + OPEN_LOG(); + OPEN_LOGCONSOLE(); + MVS::Initialize(APPNAME); + WORKING_FOLDER = _DATA_PATH; + INIT_WORKING_FOLDER; + if (argc < 2 || std::atoi(argv[1]) == 0) { + if (!UnitTests()) + return EXIT_FAILURE; + } else { + if (!PipelineTest()) + return EXIT_FAILURE; + } + MVS::Finalize(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/Tests/data/images/00000.jpg b/apps/Tests/data/images/00000.jpg new file mode 100644 index 0000000..2ec67cb Binary files /dev/null and b/apps/Tests/data/images/00000.jpg differ diff --git a/apps/Tests/data/images/00001.jpg b/apps/Tests/data/images/00001.jpg new file mode 100644 index 0000000..a5b6f95 Binary files /dev/null and b/apps/Tests/data/images/00001.jpg differ diff --git a/apps/Tests/data/images/00002.jpg b/apps/Tests/data/images/00002.jpg new file mode 100644 index 0000000..d4caa0b Binary files /dev/null and b/apps/Tests/data/images/00002.jpg differ diff --git a/apps/Tests/data/images/00003.jpg b/apps/Tests/data/images/00003.jpg new file mode 100644 index 0000000..5041550 Binary files /dev/null and b/apps/Tests/data/images/00003.jpg differ diff --git a/apps/Tests/data/scene.mvs b/apps/Tests/data/scene.mvs new file mode 100644 index 0000000..b6bd8f9 Binary files /dev/null and b/apps/Tests/data/scene.mvs differ diff --git a/apps/TextureMesh/CMakeLists.txt b/apps/TextureMesh/CMakeLists.txt new file mode 100644 index 0000000..bef488e --- /dev/null +++ b/apps/TextureMesh/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(TextureMesh "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS TextureMesh + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/TextureMesh/TextureMesh.cpp b/apps/TextureMesh/TextureMesh.cpp new file mode 100644 index 0000000..9764849 --- /dev/null +++ b/apps/TextureMesh/TextureMesh.cpp @@ -0,0 +1,1168 @@ +/* + * TextureMesh.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include + +#include "../InterfaceCOLMAP/endian.h" + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("TextureMesh") + + +#define COLMAP_IMAGES_FOLDER _T("/images/") +// #define COLMAP_SPARSE_FOLDER _T("/sparse/0/") +#define COLMAP_CAMERAS_TXT _T("/cameras.txt") +#define COLMAP_IMAGES_TXT _T("/images.txt") +#define COLMAP_POINTS_TXT _T("/points3D.txt") +#define COLMAP_CAMERAS_BIN _T("/cameras.bin") +#define COLMAP_IMAGES_BIN _T("/images.bin") +#define COLMAP_POINTS_BIN _T("/points3D.bin") +#define COLMAP_DENSE_POINTS _T("fused.ply") +#define COLMAP_DENSE_POINTS_VISIBILITY _T("fused.ply.vis") + +#define COLMAP_STEREO_FOLDER _T("stereo/") +#define COLMAP_PATCHMATCH COLMAP_STEREO_FOLDER _T("patch-match.cfg") +#define COLMAP_STEREO_DEPTHMAPS_FOLDER COLMAP_STEREO_FOLDER _T("depth_maps/") +#define COLMAP_STEREO_NORMALMAPS_FOLDER COLMAP_STEREO_FOLDER _T("normal_maps/") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strMeshFileName; +String strOutputFileName; +String strViewsFileName; +String strImageFolder; +float fDecimateMesh; +unsigned nCloseHoles; +unsigned nResolutionLevel; +unsigned nMinResolution; +unsigned minCommonCameras; +float fOutlierThreshold; +float fRatioDataSmoothness; +bool bGlobalSeamLeveling; +bool bLocalSeamLeveling; +bool bNormalizeIntrinsics; +bool bOriginFaceview; +unsigned nID; +unsigned nTextureSizeMultiple; +unsigned nRectPackingHeuristic; +uint32_t nColEmpty; +float fSharpnessWeight; +int nIgnoreMaskLabel; +unsigned nOrthoMapResolution; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +int nMaxTextureSize; +String strExportType; +String strConfigFileName; +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("export-type", boost::program_options::value(&OPT::strExportType)->default_value(_T("obj")), "file type used to export the 3D scene (ply, obj, glb or gltf)") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + #ifdef _USE_CUDA + ("cuda-device", boost::program_options::value(&CUDA::desiredDeviceID)->default_value(-1), "CUDA device number to be used to texture the mesh (-2 - CPU processing, -1 - best GPU, >=0 - device index)") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Texture options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input filename containing camera poses") + ("mesh-file,m", boost::program_options::value(&OPT::strMeshFileName), "mesh file name to texture (overwrite existing mesh)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the mesh") + ("decimate", boost::program_options::value(&OPT::fDecimateMesh)->default_value(1.f), "decimation factor in range [0..1] to be applied to the input surface before refinement (0 - auto, 1 - disabled)") + ("close-holes", boost::program_options::value(&OPT::nCloseHoles)->default_value(30), "try to close small holes in the input surface (0 - disabled)") + ("resolution-level", boost::program_options::value(&OPT::nResolutionLevel)->default_value(0), "how many times to scale down the images before mesh refinement") + ("min-resolution", boost::program_options::value(&OPT::nMinResolution)->default_value(640), "do not scale images lower than this resolution") + ("outlier-threshold", boost::program_options::value(&OPT::fOutlierThreshold)->default_value(6e-2f), "threshold used to find and remove outlier face textures (0 - disabled)") + ("cost-smoothness-ratio", boost::program_options::value(&OPT::fRatioDataSmoothness)->default_value(0.1f), "ratio used to adjust the preference for more compact patches (1 - best quality/worst compactness, ~0 - worst quality/best compactness)") + ("virtual-face-images", boost::program_options::value(&OPT::minCommonCameras)->default_value(0), "generate texture patches using virtual faces composed of coplanar triangles sharing at least this number of views (0 - disabled, 3 - good value)") + ("global-seam-leveling", boost::program_options::value(&OPT::bGlobalSeamLeveling)->default_value(true), "generate uniform texture patches using global seam leveling") + ("local-seam-leveling", boost::program_options::value(&OPT::bLocalSeamLeveling)->default_value(true), "generate uniform texture patch borders using local seam leveling") + ("texture-size-multiple", boost::program_options::value(&OPT::nTextureSizeMultiple)->default_value(0), "texture size should be a multiple of this value (0 - power of two)") + ("patch-packing-heuristic", boost::program_options::value(&OPT::nRectPackingHeuristic)->default_value(3), "specify the heuristic used when deciding where to place a new patch (0 - best fit, 3 - good speed, 100 - best speed)") + ("empty-color", boost::program_options::value(&OPT::nColEmpty)->default_value(0x00FF7F27), "color used for faces not covered by any image") + ("sharpness-weight", boost::program_options::value(&OPT::fSharpnessWeight)->default_value(0.9f), "amount of sharpness to be applied on the texture (0 - disabled)") + ("orthographic-image-resolution", boost::program_options::value(&OPT::nOrthoMapResolution)->default_value(0), "orthographic image resolution to be generated from the textured mesh - the mesh is expected to be already geo-referenced or at least properly oriented (0 - disabled)") + ("ignore-mask-label", boost::program_options::value(&OPT::nIgnoreMaskLabel)->default_value(-1), "label value to ignore in the image mask, stored in the MVS scene or next to each image with '.mask.png' extension (-1 - auto estimate mask for lens distortion, -2 - disabled)") + ("max-texture-size", boost::program_options::value(&OPT::nMaxTextureSize)->default_value(8192), "maximum texture size, split it in multiple textures of this size if needed (0 - unbounded)") + ("normalize,f", boost::program_options::value(&OPT::bNormalizeIntrinsics)->default_value(false), "normalize intrinsics while exporting to MVS format") + ("image-folder", boost::program_options::value(&OPT::strImageFolder)->default_value(COLMAP_IMAGES_FOLDER), "folder to the undistorted images") + ("origin-faceview", boost::program_options::value(&OPT::bOriginFaceview)->default_value(false), "use origin faceview selection") + ("id", boost::program_options::value(&OPT::nID)->default_value(0), "id") + ; + + // hidden options, allowed both on command line and + // in config file, but will not be shown to the user + boost::program_options::options_description hidden("Hidden options"); + hidden.add_options() + ("views-file", boost::program_options::value(&OPT::strViewsFileName), "file name containing the list of views to be used for texturing (optional)") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config).add(hidden); + + boost::program_options::options_description config_file_options; + config_file_options.add(config).add(hidden); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + if (OPT::vm.count("help") || OPT::strInputFileName.empty()) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (OPT::strInputFileName.empty()) + return false; + OPT::strExportType = OPT::strExportType.ToLower(); + if (OPT::strExportType == _T("obj")) + OPT::strExportType = _T(".obj"); + else + if (OPT::strExportType == _T("glb")) + OPT::strExportType = _T(".glb"); + else + if (OPT::strExportType == _T("gltf")) + OPT::strExportType = _T(".gltf"); + else + OPT::strExportType = _T(".ply"); + + // initialize optional options + Util::ensureValidPath(OPT::strMeshFileName); + Util::ensureValidPath(OPT::strOutputFileName); + Util::ensureValidPath(OPT::strViewsFileName); + if (OPT::strMeshFileName.empty() && (ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS) + OPT::strMeshFileName = Util::getFileFullName(OPT::strInputFileName) + _T(".ply"); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFileFullName(OPT::strInputFileName) + _T("_texture.mvs"); + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + +IIndexArr ParseViewsFile(const String& filename, const Scene& scene) { + IIndexArr views; + std::ifstream file(filename); + printf("filename: %s\n", filename.c_str()); + if (!file.good()) { + VERBOSE("error: unable to open views file '%s'", filename.c_str()); + return views; + } + while (true) { + String imageName; + std::getline(file, imageName); + if (file.fail() || imageName.empty()) + break; + LPTSTR endIdx; + const IDX idx(strtoul(imageName, &endIdx, 10)); + const size_t szIndex(*endIdx == '\0' ? size_t(0) : imageName.size()); + FOREACH(idxImage, scene.images) { + const Image& image = scene.images[idxImage]; + if (szIndex == 0) { + // try to match by index + if (image.ID != idx) + continue; + } else { + // try to match by file name + const String name(Util::getFileNameExt(image.name)); + if (name.size() < szIndex || _tcsnicmp(name, imageName, szIndex) != 0) + continue; + } + views.emplace_back(idxImage); + break; + } + } + return views; +} + +bool DetermineInputSource(const String& filenameTXT, const String& filenameBIN, std::ifstream& fileStream, String& filenameCamera, bool& isBinaryFormat) +{ + // Try to open the TXT file first. + fileStream.open(filenameTXT); + if (fileStream.good()) { + filenameCamera = filenameTXT; + isBinaryFormat = false; + return true; + } + fileStream.open(filenameBIN, std::ios::binary); + if (fileStream.good()) { + filenameCamera = filenameBIN; + isBinaryFormat = true; + return true; + } + VERBOSE("error: unable to open file '%s'", filenameTXT.c_str()); + VERBOSE("error: unable to open file '%s'", filenameBIN.c_str()); + return false; +} + +namespace COLMAP { + +using namespace colmap; + +// See colmap/src/util/types.h +typedef uint32_t camera_t; +typedef uint32_t image_t; +typedef uint64_t image_pair_t; +typedef uint32_t point2D_t; +typedef uint64_t point3D_t; + +const std::vector mapCameraModel = { + "SIMPLE_PINHOLE", + "PINHOLE", + "SIMPLE_RADIAL", + "RADIAL", + "OPENCV", + "OPENCV_FISHEYE", + "FULL_OPENCV", + "FOV", + "SIMPLE_RADIAL_FISHEYE", + "RADIAL_FISHEYE", + "THIN_PRISM_FISHEYE" +}; + +// tools +bool NextLine(std::istream& stream, std::istringstream& in, bool bIgnoreEmpty=true) { + String line; + do { + std::getline(stream, line); + Util::strTrim(line, _T(" ")); + if (stream.fail()) + return false; + } while (((bIgnoreEmpty && line.empty()) || line[0u] == '#') && stream.good()); + in.clear(); + in.str(line); + return true; +} +// structure describing a camera +struct Camera { + uint32_t ID; // ID of the camera + String model; // camera model name + uint32_t width, height; // camera resolution + std::vector params; // camera parameters + uint64_t numCameras{0}; // only for binary format + + Camera() {} + Camera(uint32_t _ID) : ID(_ID) {} + bool operator < (const Camera& rhs) const { return ID < rhs.ID; } + + struct CameraHash { + size_t operator()(const Camera& camera) const { + const size_t h1(std::hash()(camera.model)); + const size_t h2(std::hash()(camera.width)); + const size_t h3(std::hash()(camera.height)); + size_t h(h1 ^ ((h2 ^ (h3 << 1)) << 1)); + for (REAL p: camera.params) + h = std::hash()(p) ^ (h << 1); + return h; + } + }; + struct CameraEqualTo { + bool operator()(const Camera& _Left, const Camera& _Right) const { + return _Left.model == _Right.model && + _Left.width == _Right.width && _Left.height == _Right.height && + _Left.params == _Right.params; + } + }; + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // Camera list with one line of data per camera: + // CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[] + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + in >> ID >> model >> width >> height; + if (in.fail()) + return false; + --ID; + if (model != _T("PINHOLE")) + return false; + params.resize(4); + in >> params[0] >> params[1] >> params[2] >> params[3]; + return !in.fail(); + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadCamerasBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (numCameras == 0) { + // Read the first entry in the binary file + numCameras = ReadBinaryLittleEndian(&stream); + } + + ID = ReadBinaryLittleEndian(&stream)-1; + model = mapCameraModel[ReadBinaryLittleEndian(&stream)]; + width = (uint32_t)ReadBinaryLittleEndian(&stream); + height = (uint32_t)ReadBinaryLittleEndian(&stream); + if (model != _T("PINHOLE")) + return false; + params.resize(4); + ReadBinaryLittleEndian(&stream, ¶ms); + return true; + } + + bool WriteTXT(std::ostream& out) const { + out << ID+1 << _T(" ") << model << _T(" ") << width << _T(" ") << height; + if (out.fail()) + return false; + for (REAL param: params) { + out << _T(" ") << param; + if (out.fail()) + return false; + } + out << std::endl; + return true; + } + + bool WriteBIN(std::ostream& stream) { + if (numCameras != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numCameras); + numCameras = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + const int64 modelId(std::distance(mapCameraModel.begin(), std::find(mapCameraModel.begin(), mapCameraModel.end(), model))); + WriteBinaryLittleEndian(&stream, (int)modelId); + WriteBinaryLittleEndian(&stream, width); + WriteBinaryLittleEndian(&stream, height); + for (REAL param: params) + WriteBinaryLittleEndian(&stream, param); + return !stream.fail(); + } +}; +typedef std::vector Cameras; +// structure describing an image +struct Image { + struct Proj { + Eigen::Vector2f p; + uint32_t idPoint; + }; + uint32_t ID; // ID of the image + Eigen::Quaterniond q; // rotation + Eigen::Vector3d t; // translation + uint32_t idCamera; // ID of the associated camera + String name; // image file name + std::vector projs; // known image projections + uint64_t numRegImages{0}; // only for binary format + + Image() {} + Image(uint32_t _ID) : ID(_ID) {} + bool operator < (const Image& rhs) const { return ID < rhs.ID; } + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // Image list with two lines of data per image: + // IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME + // POINTS2D[] as (X, Y, POINT3D_ID) + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + in >> ID + >> q.w() >> q.x() >> q.y() >> q.z() + >> t(0) >> t(1) >> t(2) + >> idCamera >> name; + if (in.fail()) + return false; + --ID; --idCamera; + Util::ensureValidPath(name); + if (!NextLine(stream, in, false)) + return false; + projs.clear(); + while (true) { + Proj proj; + in >> proj.p(0) >> proj.p(1) >> (int&)proj.idPoint; + if (in.fail()) + break; + --proj.idPoint; + projs.emplace_back(proj); + } + return true; + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadImagesBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (!numRegImages) { + // Read the first entry in the binary file + numRegImages = ReadBinaryLittleEndian(&stream); + } + + ID = ReadBinaryLittleEndian(&stream)-1; + q.w() = ReadBinaryLittleEndian(&stream); + q.x() = ReadBinaryLittleEndian(&stream); + q.y() = ReadBinaryLittleEndian(&stream); + q.z() = ReadBinaryLittleEndian(&stream); + t(0) = ReadBinaryLittleEndian(&stream); + t(1) = ReadBinaryLittleEndian(&stream); + t(2) = ReadBinaryLittleEndian(&stream); + idCamera = ReadBinaryLittleEndian(&stream)-1; + + name = ""; + while (true) { + char nameChar; + stream.read(&nameChar, 1); + if (nameChar == '\0') + break; + name += nameChar; + } + Util::ensureValidPath(name); + + const size_t numPoints2D = ReadBinaryLittleEndian(&stream); + projs.clear(); + for (size_t j = 0; j < numPoints2D; ++j) { + Proj proj; + proj.p(0) = (float)ReadBinaryLittleEndian(&stream); + proj.p(1) = (float)ReadBinaryLittleEndian(&stream); + proj.idPoint = (uint32_t)ReadBinaryLittleEndian(&stream)-1; + projs.emplace_back(proj); + } + return true; + } + + bool WriteTXT(std::ostream& out) const { + out << ID+1 << _T(" ") + << q.w() << _T(" ") << q.x() << _T(" ") << q.y() << _T(" ") << q.z() << _T(" ") + << t(0) << _T(" ") << t(1) << _T(" ") << t(2) << _T(" ") + << idCamera+1 << _T(" ") << name + << std::endl; + for (const Proj& proj: projs) { + out << proj.p(0) << _T(" ") << proj.p(1) << _T(" ") << (int)proj.idPoint+1 << _T(" "); + if (out.fail()) + return false; + } + out << std::endl; + return !out.fail(); + } + + bool WriteBIN(std::ostream& stream) { + if (numRegImages != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numRegImages); + numRegImages = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + + WriteBinaryLittleEndian(&stream, q.w()); + WriteBinaryLittleEndian(&stream, q.x()); + WriteBinaryLittleEndian(&stream, q.y()); + WriteBinaryLittleEndian(&stream, q.z()); + + WriteBinaryLittleEndian(&stream, t(0)); + WriteBinaryLittleEndian(&stream, t(1)); + WriteBinaryLittleEndian(&stream, t(2)); + + WriteBinaryLittleEndian(&stream, idCamera+1); + + stream.write(name.c_str(), name.size()+1); + + WriteBinaryLittleEndian(&stream, projs.size()); + for (const Proj& proj: projs) { + WriteBinaryLittleEndian(&stream, proj.p(0)); + WriteBinaryLittleEndian(&stream, proj.p(1)); + WriteBinaryLittleEndian(&stream, proj.idPoint+1); + } + return !stream.fail(); + } +}; +typedef std::vector Images; +// structure describing a 3D point +struct Point { + struct Track { + uint32_t idImage; + uint32_t idProj; + }; + uint32_t ID; // ID of the point + Interface::Pos3f p; // position + Interface::Col3 c; // BGR color + float e; // error + std::vector tracks; // point track + uint64_t numPoints3D{0}; // only for binary format + + Point() {} + Point(uint32_t _ID) : ID(_ID) {} + bool operator < (const Image& rhs) const { return ID < rhs.ID; } + + bool Read(std::istream& stream, bool binary) { + if (binary) + return ReadBIN(stream); + return ReadTXT(stream); + } + + bool Write(std::ostream& stream, bool binary) { + if (binary) + return WriteBIN(stream); + return WriteTXT(stream); + } + + // 3D point list with one line of data per point: + // POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX) + bool ReadTXT(std::istream& stream) { + std::istringstream in; + if (!NextLine(stream, in)) + return false; + int r,g,b; + in >> ID + >> p.x >> p.y >> p.z + >> r >> g >> b + >> e; + c.x = CLAMP(b,0,255); + c.y = CLAMP(g,0,255); + c.z = CLAMP(r,0,255); + if (in.fail()) + return false; + --ID; + tracks.clear(); + while (true) { + Track track; + in >> track.idImage >> track.idProj; + if (in.fail()) + break; + --track.idImage; --track.idProj; + tracks.emplace_back(track); + } + return !tracks.empty(); + } + + // See: colmap/src/base/reconstruction.cc + // void Reconstruction::ReadPoints3DBinary(const std::string& path) + bool ReadBIN(std::istream& stream) { + if (stream.peek() == EOF) + return false; + + if (!numPoints3D) { + // Read the first entry in the binary file + numPoints3D = ReadBinaryLittleEndian(&stream); + } + + int r,g,b; + ID = (uint32_t)ReadBinaryLittleEndian(&stream)-1; + p.x = (float)ReadBinaryLittleEndian(&stream); + p.y = (float)ReadBinaryLittleEndian(&stream); + p.z = (float)ReadBinaryLittleEndian(&stream); + r = ReadBinaryLittleEndian(&stream); + g = ReadBinaryLittleEndian(&stream); + b = ReadBinaryLittleEndian(&stream); + e = (float)ReadBinaryLittleEndian(&stream); + c.x = CLAMP(b,0,255); + c.y = CLAMP(g,0,255); + c.z = CLAMP(r,0,255); + + const size_t trackLength = ReadBinaryLittleEndian(&stream); + tracks.clear(); + for (size_t j = 0; j < trackLength; ++j) { + Track track; + track.idImage = ReadBinaryLittleEndian(&stream)-1; + track.idProj = ReadBinaryLittleEndian(&stream)-1; + tracks.emplace_back(track); + } + return !tracks.empty(); + } + + bool WriteTXT(std::ostream& out) const { + ASSERT(!tracks.empty()); + const int r(c.z),g(c.y),b(c.x); + out << ID+1 << _T(" ") + << p.x << _T(" ") << p.y << _T(" ") << p.z << _T(" ") + << r << _T(" ") << g << _T(" ") << b << _T(" ") + << e << _T(" "); + for (const Track& track: tracks) { + out << track.idImage+1 << _T(" ") << track.idProj+1 << _T(" "); + if (out.fail()) + return false; + } + out << std::endl; + return !out.fail(); + } + + bool WriteBIN(std::ostream& stream) { + ASSERT(!tracks.empty()); + if (numPoints3D != 0) { + // Write the first entry in the binary file + WriteBinaryLittleEndian(&stream, numPoints3D); + numPoints3D = 0; + } + + WriteBinaryLittleEndian(&stream, ID+1); + WriteBinaryLittleEndian(&stream, p.x); + WriteBinaryLittleEndian(&stream, p.y); + WriteBinaryLittleEndian(&stream, p.z); + WriteBinaryLittleEndian(&stream, c.z); + WriteBinaryLittleEndian(&stream, c.y); + WriteBinaryLittleEndian(&stream, c.x); + WriteBinaryLittleEndian(&stream, e); + + WriteBinaryLittleEndian(&stream, tracks.size()); + for (const Track& track: tracks) { + WriteBinaryLittleEndian(&stream, track.idImage+1); + WriteBinaryLittleEndian(&stream, track.idProj+1); + } + return !stream.fail(); + } +}; +typedef std::vector Points; +// structure describing an 2D dynamic matrix +template +struct Mat { + size_t width_ = 0; + size_t height_ = 0; + size_t depth_ = 0; + std::vector data_; + + size_t GetNumBytes() const { + return data_.size() * sizeof(T); + } + const T* GetChannelPtr(size_t c) const { + return data_.data()+width_*height_*c; + } + + // See: colmap/src/mvs/mat.h + void Read(const std::string& path) { + std::streampos pos; { + std::fstream text_file(path, std::ios::in | std::ios::binary); + char unused_char; + text_file >> width_ >> unused_char >> height_ >> unused_char >> depth_ >> + unused_char; + pos = text_file.tellg(); + } + data_.resize(width_ * height_ * depth_); + std::fstream binary_file(path, std::ios::in | std::ios::binary); + binary_file.seekg(pos); + ReadBinaryLittleEndian(&binary_file, &data_); + } + void Write(const std::string& path) const { + { + std::fstream text_file(path, std::ios::out); + text_file << width_ << "&" << height_ << "&" << depth_ << "&"; + } + std::fstream binary_file(path, std::ios::out | std::ios::binary | std::ios::app); + WriteBinaryLittleEndian(&binary_file, data_); + } +}; +} // namespace COLMAP + +typedef Eigen::Matrix EMat33d; +typedef Eigen::Matrix EVec3d; + +bool ImportScene(const String& inputFolder, const String& outputFolder, Interface& scene, PointCloud& pointcloud) +{ + // Define a map to store camera IDs and associated platform index. + typedef std::unordered_map CamerasMap; + CamerasMap mapCameras; + { + // Determine the camera file source between TXT and BIN formats. + const String filenameCamerasTXT(inputFolder+COLMAP_CAMERAS_TXT); + const String filenameCamerasBIN(inputFolder+COLMAP_CAMERAS_BIN); + std::ifstream cameraFile; + bool isBinaryFormat; + String filenameCamera; + if (!DetermineInputSource(filenameCamerasTXT, filenameCamerasBIN, cameraFile, filenameCamera, isBinaryFormat)) { + return false; // Exit if file source determination fails. + } + LOG_OUT() << "Reading cameras: " << filenameCamera << std::endl; + + typedef std::unordered_map CamerasSet; + CamerasSet setCameras; + COLMAP::Camera colmapCamera; + while (cameraFile.good() && colmapCamera.Read(cameraFile, isBinaryFormat)) { + const auto setIt(setCameras.emplace(colmapCamera, (uint32_t)scene.platforms.size())); + mapCameras.emplace(colmapCamera.ID, setIt.first->second); + if (!setIt.second) { + // reuse existing platform + continue; + } + // create new platform + Interface::Platform platform; + platform.name = String::FormatString(_T("platform%03u"), colmapCamera.ID); // only one camera per platform supported + Interface::Platform::Camera camera; + camera.name = colmapCamera.model; + camera.K = Interface::Mat33d::eye(); + // account for different pixel center conventions as COLMAP uses pixel center at (0.5,0.5) + camera.K(0,0) = colmapCamera.params[0]; + camera.K(1,1) = colmapCamera.params[1]; + camera.K(0,2) = colmapCamera.params[2]-REAL(0.5); + camera.K(1,2) = colmapCamera.params[3]-REAL(0.5); + camera.R = Interface::Mat33d::eye(); + camera.C = Interface::Pos3d(0,0,0); + if (OPT::bNormalizeIntrinsics) { + // normalize camera intrinsics + camera.K = Camera::ScaleK(camera.K, 1.0/Camera::GetNormalizationScale(colmapCamera.width, colmapCamera.height)); + } else { + camera.width = colmapCamera.width; + camera.height = colmapCamera.height; + } + platform.cameras.emplace_back(camera); + scene.platforms.emplace_back(platform); + } + } + if (mapCameras.empty()) { + VERBOSE("error: no valid cameras (make sure they are in PINHOLE model)"); + return false; + } + + // Function to read and process image data from input files + typedef std::map ImagesMap; + ImagesMap mapImages; + { + // Define a map to associate COLMAP image structures with scene image indices + const String filenameImagesTXT(inputFolder+COLMAP_IMAGES_TXT); + const String filenameImagesBIN(inputFolder+COLMAP_IMAGES_BIN); + std::ifstream file; + bool binary; + String filenameImages; + if (!DetermineInputSource(filenameImagesTXT, filenameImagesBIN, file, filenameImages, binary)) { + return false; + } + LOG_OUT() << "Reading images: " << filenameImages << std::endl; + + // Read image data from the file and store in the scene structure + COLMAP::Image imageColmap; + while (file.good() && imageColmap.Read(file, binary)) { + mapImages.emplace(imageColmap, (uint32_t)scene.images.size()); + Interface::Platform::Pose pose; + Eigen::Map(pose.R.val) = imageColmap.q.toRotationMatrix(); + EnsureRotationMatrix((Matrix3x3d&)pose.R); + Eigen::Map(&pose.C.x) = -(imageColmap.q.inverse() * imageColmap.t); + + Interface::Image image; + // Start Generation Here + if (!OPT::strImageFolder.empty() && OPT::strImageFolder.back() != '/') + OPT::strImageFolder += '/'; + image.name = MAKE_PATH_REL(outputFolder,OPT::strImageFolder+imageColmap.name); + image.platformID = mapCameras.at(imageColmap.idCamera); + image.cameraID = 0; + image.ID = imageColmap.ID; + + Interface::Platform& platform = scene.platforms[image.platformID]; + image.poseID = (uint32_t)platform.poses.size(); + platform.poses.emplace_back(pose); + scene.images.emplace_back(image); + } + } + + // read points list + const String filenameDensePoints(inputFolder+COLMAP_DENSE_POINTS); + const String filenameDenseVisPoints(inputFolder+COLMAP_DENSE_POINTS_VISIBILITY); + printf("Don't read any cloud points\n"); + // { + // // parse sparse point-cloud + // const String filenamePointsTXT(inputFolder+COLMAP_POINTS_TXT); + // const String filenamePointsBIN(inputFolder+COLMAP_POINTS_BIN); + // std::ifstream file; + // bool binary; + // String filenamePoints; + // if (!DetermineInputSource(filenamePointsTXT, filenamePointsBIN, file, filenamePoints, binary)) { + // return false; + // } + // LOG_OUT() << "Reading points: " << filenamePoints << std::endl; + + // // Process each point in the file + // COLMAP::Point point; + // while (file.good() && point.Read(file, binary)) { + // Interface::Vertex vertex; + // vertex.X = point.p; // Assign 3D coordinates to vertex + + // // Process each track associated with the point + // for (const COLMAP::Point::Track& track: point.tracks) { + // Interface::Vertex::View view; + // view.imageID = mapImages.at(COLMAP::Image(track.0)); // Map COLMAP image ID to internal image ID + // view.confidence = 0; + // vertex.views.emplace_back(view); + // } + + // // Ensure views are sorted by image ID to maintain consistency + // std::sort(vertex.views.begin(), vertex.views.end(), + // [](const Interface::Vertex::View& view0, const Interface::Vertex::View& view1) { return view0.imageID < view1.imageID; }); + // scene.vertices.emplace_back(std::move(vertex)); + // scene.verticesColor.emplace_back(Interface::Color{point.c}); + // } + // } + pointcloud.Release(); + if (File::access(filenameDensePoints) && File::access(filenameDenseVisPoints)) { + // parse dense point-cloud + LOG_OUT() << "Reading points: " << filenameDensePoints << " and " << filenameDenseVisPoints << std::endl; + if (!pointcloud.Load(filenameDensePoints)) { + VERBOSE("error: unable to open file '%s'", filenameDensePoints.c_str()); + return false; + } + File file(filenameDenseVisPoints, File::READ, File::OPEN); + if (!file.isOpen()) { + VERBOSE("error: unable to open file '%s'", filenameDenseVisPoints.c_str()); + return false; + } + uint64_t numPoints(0); + file.read(&numPoints, sizeof(uint64_t)); + if (pointcloud.GetSize() != numPoints) { + VERBOSE("error: point-cloud and visibility have different size"); + return false; + } + pointcloud.pointViews.resize(numPoints); + for (size_t i=0; isecond]; + CLISTDEF2(String) neighborNames; + Util::strSplit(neighbors, _T(','), neighborNames); + FOREACH(i, neighborNames) { + String& neighborName = neighborNames[i]; + Util::strTrim(neighborName, _T(" ")); + const ImagesMap::const_iterator it_neighbor = std::find_if(mapImages.begin(), mapImages.end(), + [&neighborName](const ImagesMap::value_type& image) { + return image.first.name == neighborName; + }); + if (it_neighbor == mapImages.end()) { + if (i == 0) + break; + continue; + } + imageNeighbors.emplace_back(scene.images[it_neighbor->second].ID); + } + } + } + LOG_OUT() << "Reading depth-maps/normal-maps: " << pathDepthMaps << " and " << pathNormalMaps << std::endl; + Util::ensureFolder(outputFolder); + const String strType[] = {".geometric.bin", ".photometric.bin"}; + FOREACH(idx, scene.images) { + const Interface::Image& image = scene.images[idx]; + COLMAP::Mat colDepthMap, colNormalMap; + const String filenameImage(Util::getFileNameExt(image.name)); + for (int i=0; i<2; ++i) { + const String filenameDepthMaps(pathDepthMaps+filenameImage+strType[i]); + if (File::isFile(filenameDepthMaps)) { + colDepthMap.Read(filenameDepthMaps); + const String filenameNormalMaps(pathNormalMaps+filenameImage+strType[i]); + if (File::isFile(filenameNormalMaps)) { + colNormalMap.Read(filenameNormalMaps); + } + break; + } + } + if (!colDepthMap.data_.empty()) { + IIndexArr IDs = {image.ID}; + IDs.Join(imagesNeighbors[(IIndex)idx]); + const Interface::Platform& platform = scene.platforms[image.platformID]; + const Interface::Platform::Pose pose(platform.GetPose(image.cameraID, image.poseID)); + const Interface::Mat33d K(platform.GetFullK(image.cameraID, (uint32_t)colDepthMap.width_, (uint32_t)colDepthMap.height_)); + MVS::DepthMap depthMap((int)colDepthMap.height_, (int)colDepthMap.width_); + memcpy(depthMap.getData(), colDepthMap.data_.data(), colDepthMap.GetNumBytes()); + MVS::NormalMap normalMap; + if (!colNormalMap.data_.empty()) { + normalMap.create((int)colNormalMap.height_, (int)colNormalMap.width_); + cv::merge(std::vector{ + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(0)), + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(1)), + cv::Mat((int)colNormalMap.height_, (int)colNormalMap.width_, CV_32F, (void*)colNormalMap.GetChannelPtr(2)) + }, normalMap); + } + MVS::ConfidenceMap confMap; + MVS::ViewsMap viewsMap; + const auto depthMM(std::minmax_element(colDepthMap.data_.cbegin(), colDepthMap.data_.cend())); + const MVS::Depth dMin(*depthMM.first), dMax(*depthMM.second); + if (!ExportDepthDataRaw(outputFolder+String::FormatString("depth%04u.dmap", image.ID), MAKE_PATH_FULL(outputFolder, image.name), IDs, depthMap.size(), K, pose.R, pose.C, dMin, dMax, depthMap, normalMap, confMap, viewsMap)) + return false; + } + } + } + return true; +} + +bool checkFileExistence(const std::string& filename) { + std::ifstream file(filename); + return file.good(); +} + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + if (1) { + if (checkFileExistence(MAKE_PATH_SAFE(OPT::strOutputFileName).c_str())) { + std::cout << "The file exists." << std::endl; + } else { + std::cout << "The file does not exist." << std::endl; + } + + // read COLMAP input data + Interface scene; + const String strOutFolder(Util::getFilePath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strOutputFileName))); + printf("Util::getFilePath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strInputFileName)): %s\n", Util::getFilePath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strInputFileName)).c_str()); + PointCloud pointcloud; + if (!ImportScene(MAKE_PATH_SAFE(OPT::strInputFileName), strOutFolder, scene, pointcloud)) + return EXIT_FAILURE; + printf("###########\n"); + // write MVS input data + Util::ensureFolder(strOutFolder); + if (!ARCHIVE::SerializeSave(scene, MAKE_PATH_SAFE(OPT::strOutputFileName))) + return EXIT_FAILURE; + printf("MAKE_PATH_SAFE(OPT::strOutputFileName): %s\n", MAKE_PATH_SAFE(OPT::strOutputFileName).c_str()); + + + if (checkFileExistence(MAKE_PATH_SAFE(OPT::strOutputFileName).c_str())) { + std::cout << "The file exists." << std::endl; + } else { + std::cout << "The file does not exist." << std::endl; + } + } + + Scene scene(OPT::nMaxThreads); + printf("OPT::strViewsFileName: %s\n", OPT::strViewsFileName.c_str()); + printf("MAKE_PATH_SAFE(OPT::strViewsFileName): %s\n", MAKE_PATH_SAFE(OPT::strViewsFileName).c_str()); + // load and texture the mesh + printf("MAKE_PATH_SAFE(OPT::strOutputFileName): %s\n", MAKE_PATH_SAFE(OPT::strOutputFileName).c_str()); + const Scene::SCENE_TYPE sceneType(scene.Load(MAKE_PATH_SAFE(OPT::strOutputFileName))); + if (sceneType == Scene::SCENE_NA) + return EXIT_FAILURE; + printf("Now\n"); + VERBOSE(MAKE_PATH_SAFE(OPT::strMeshFileName)); + if (!OPT::strMeshFileName.empty() && !scene.mesh.Load(MAKE_PATH_SAFE(OPT::strMeshFileName))) { + VERBOSE("error: cannot load mesh file"); + return EXIT_FAILURE; + } + if (scene.mesh.IsEmpty()) { + VERBOSE("error: empty initial mesh"); + return EXIT_FAILURE; + } + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))); + if (OPT::nOrthoMapResolution && !scene.mesh.HasTexture()) { + // the input mesh is already textured and an orthographic projection was requested + goto ProjectOrtho; + } + + { + // decimate to the desired resolution + if (OPT::fDecimateMesh < 1.f) { + ASSERT(OPT::fDecimateMesh > 0.f); + scene.mesh.Clean(OPT::fDecimateMesh, 0.f, false, OPT::nCloseHoles, 0u, 0.f, false); + scene.mesh.Clean(1.f, 0.f, false, 0u, 0u, 0.f, true); // extra cleaning to remove non-manifold problems created by closing holes + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 3) + scene.mesh.Save(baseFileName +_T("_decim")+OPT::strExportType); + #endif + } + // fetch list of views to be used for texturing + IIndexArr views; + // VERBOSE(MAKE_PATH_SAFE(OPT::strViewsFileName)); + printf("MAKE_PATH_SAFE(OPT::strViewsFileName): %s\n", MAKE_PATH_SAFE(OPT::strViewsFileName).c_str()); + if (!OPT::strViewsFileName.empty()) + views = ParseViewsFile(MAKE_PATH_SAFE(OPT::strViewsFileName), scene); + printf("Second\n"); + // compute mesh texture + TD_TIMER_START(); + if (!scene.TextureMesh(OPT::nResolutionLevel, OPT::nMinResolution, OPT::minCommonCameras, OPT::fOutlierThreshold, OPT::fRatioDataSmoothness, + OPT::bGlobalSeamLeveling, OPT::bLocalSeamLeveling, OPT::nTextureSizeMultiple, OPT::nRectPackingHeuristic, Pixel8U(OPT::nColEmpty), + OPT::fSharpnessWeight, OPT::nIgnoreMaskLabel, OPT::nMaxTextureSize, views, baseFileName, OPT::bOriginFaceview)) + return EXIT_FAILURE; + VERBOSE("Mesh texturing completed: %u vertices, %u faces (%s)", scene.mesh.vertices.GetSize(), scene.mesh.faces.GetSize(), TD_TIMER_GET_FMT().c_str()); + + // save the final mesh + scene.mesh.Save(baseFileName+OPT::strExportType); + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 2) + scene.ExportCamerasMLP(baseFileName+_T(".mlp"), baseFileName+OPT::strExportType); + #endif + if ((ARCHIVE_TYPE)OPT::nArchiveType != ARCHIVE_MVS || sceneType != Scene::SCENE_INTERFACE) + scene.Save(baseFileName+_T(".mvs"), (ARCHIVE_TYPE)OPT::nArchiveType); + } + + if (OPT::nOrthoMapResolution) { + // project mesh as an orthographic image + ProjectOrtho: + Image8U3 imageRGB; + Image8U imageRGBA[4]; + Point3 center; + scene.mesh.ProjectOrthoTopDown(OPT::nOrthoMapResolution, imageRGB, imageRGBA[3], center); + Image8U4 image; + cv::split(imageRGB, imageRGBA); + cv::merge(imageRGBA, 4, image); + image.Save(baseFileName+_T("_orthomap.png")); + SML sml(_T("OrthoMap")); + sml[_T("Center")].val = String::FormatString(_T("%g %g %g"), center.x, center.y, center.z); + sml.Save(baseFileName+_T("_orthomap.txt")); + } + + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/TransformScene/CMakeLists.txt b/apps/TransformScene/CMakeLists.txt new file mode 100644 index 0000000..2ea8a7c --- /dev/null +++ b/apps/TransformScene/CMakeLists.txt @@ -0,0 +1,13 @@ +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(TransformScene "Apps" "${cxx_default}" "MVS;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Install +INSTALL(TARGETS TransformScene + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/TransformScene/TransformScene.cpp b/apps/TransformScene/TransformScene.cpp new file mode 100644 index 0000000..95f06f2 --- /dev/null +++ b/apps/TransformScene/TransformScene.cpp @@ -0,0 +1,329 @@ +/* + * TransformScene.cpp + * + * Copyright (c) 2014-2021 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" +#include + +using namespace MVS; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("TransformScene") +#define MVS_FILE_EXTENSION _T(".mvs") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { + String strInputFileName; + String strPointCloudFileName; + String strMeshFileName; + String strOutputFileName; + String strAlignFileName; + String strTransformFileName; + String strTransferTextureFileName; + String strIndicesFileName; + bool bComputeVolume; + float fPlaneThreshold; + float fSampleMesh; + unsigned nMaxResolution; + unsigned nUpAxis; + unsigned nArchiveType; + int nProcessPriority; + unsigned nMaxThreads; + String strExportType; + String strConfigFileName; + boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("export-type", boost::program_options::value(&OPT::strExportType)->default_value(_T("ply")), "file type used to export the 3D scene (ply, obj, glb or gltf)") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(-1), "process priority (below normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads (0 for using all available cores)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Main options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input scene filename") + ("pointcloud-file,p", boost::program_options::value(&OPT::strPointCloudFileName), "dense point-cloud with views file name to transform (overwrite existing point-cloud)") + ("mesh-file,m", boost::program_options::value(&OPT::strMeshFileName), "mesh file name to transform (overwrite existing mesh)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the scene") + ("align-file,a", boost::program_options::value(&OPT::strAlignFileName), "input scene filename to which the scene will be cameras aligned") + ("transform-file,t", boost::program_options::value(&OPT::strTransformFileName), "input transform filename by which the scene will transformed") + ("transfer-texture-file", boost::program_options::value(&OPT::strTransferTextureFileName), "input mesh filename to which the texture of the scene's mesh will be transfered to (the two meshes should be aligned and the new mesh to have UV-map)") + ("indices-file", boost::program_options::value(&OPT::strIndicesFileName), "input indices filename to be used with ex. texture transfer to select a subset of the scene's mesh") + ("compute-volume", boost::program_options::value(&OPT::bComputeVolume)->default_value(false), "compute the volume of the given watertight mesh, or else try to estimate the ground plane and assume the mesh is bounded by it") + ("plane-threshold", boost::program_options::value(&OPT::fPlaneThreshold)->default_value(0.f), "threshold used to estimate the ground plane (<0 - disabled, 0 - auto, >0 - desired threshold)") + ("sample-mesh", boost::program_options::value(&OPT::fSampleMesh)->default_value(-300000.f), "uniformly samples points on a mesh (0 - disabled, <0 - number of points, >0 - sample density per square unit)") + ("max-resolution", boost::program_options::value(&OPT::nMaxResolution)->default_value(0), "make sure image resolution are not not larger than this (0 - disabled)") + ("up-axis", boost::program_options::value(&OPT::nUpAxis)->default_value(2), "scene axis considered to point upwards (0 - x, 1 - y, 2 - z)") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + // initialize the log file + OPEN_LOGFILE(MAKE_PATH(APPNAME _T("-") + Util::getUniqueName(0) + _T(".log")).c_str()); + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + Util::ensureValidPath(OPT::strAlignFileName); + Util::ensureValidPath(OPT::strTransformFileName); + Util::ensureValidPath(OPT::strTransferTextureFileName); + Util::ensureValidPath(OPT::strIndicesFileName); + const String strInputFileNameExt(Util::getFileExt(OPT::strInputFileName).ToLower()); + const bool bInvalidCommand(OPT::strInputFileName.empty() || + (OPT::strAlignFileName.empty() && OPT::strTransformFileName.empty() && OPT::strTransferTextureFileName.empty() && !OPT::bComputeVolume)); + if (OPT::vm.count("help") || bInvalidCommand) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << visible; + } + if (bInvalidCommand) + return false; + OPT::strExportType = OPT::strExportType.ToLower(); + if (OPT::strExportType == _T("obj")) + OPT::strExportType = _T(".obj"); + else + if (OPT::strExportType == _T("glb")) + OPT::strExportType = _T(".glb"); + else + if (OPT::strExportType == _T("gltf")) + OPT::strExportType = _T(".gltf"); + else + OPT::strExportType = _T(".ply"); + + // initialize optional options + Util::ensureValidPath(OPT::strPointCloudFileName); + Util::ensureValidPath(OPT::strMeshFileName); + Util::ensureValidPath(OPT::strOutputFileName); + if (OPT::strMeshFileName.empty() && (ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS && strInputFileNameExt == MVS_FILE_EXTENSION) + OPT::strMeshFileName = Util::getFileFullName(OPT::strInputFileName) + _T(".ply"); + if (OPT::strOutputFileName.empty()) + OPT::strOutputFileName = Util::getFileName(OPT::strInputFileName) + _T("_transformed") MVS_FILE_EXTENSION; + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + TD_TIMER_START(); + + Scene scene(OPT::nMaxThreads); + + // load given scene + const Scene::SCENE_TYPE sceneType(scene.Load(MAKE_PATH_SAFE(OPT::strInputFileName), + !OPT::strTransformFileName.empty() || !OPT::strTransferTextureFileName.empty() || OPT::bComputeVolume)); + if (sceneType == Scene::SCENE_NA) + return EXIT_FAILURE; + if (!OPT::strPointCloudFileName.empty() && !scene.pointcloud.Load(MAKE_PATH_SAFE(OPT::strPointCloudFileName))) { + VERBOSE("error: cannot load point-cloud file"); + return EXIT_FAILURE; + } + if (!OPT::strMeshFileName.empty() && !scene.mesh.Load(MAKE_PATH_SAFE(OPT::strMeshFileName))) { + VERBOSE("error: cannot load mesh file"); + return EXIT_FAILURE; + } + const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName))); + + if (!OPT::strAlignFileName.empty()) { + // transform this scene such that it best aligns with the given scene based on the camera positions + Scene sceneRef(OPT::nMaxThreads); + if (!sceneRef.Load(MAKE_PATH_SAFE(OPT::strAlignFileName))) + return EXIT_FAILURE; + if (!scene.AlignTo(sceneRef)) + return EXIT_FAILURE; + VERBOSE("Scene aligned to the given reference scene (%s)", TD_TIMER_GET_FMT().c_str()); + } + + if (!OPT::strTransformFileName.empty()) { + // transform this scene by the given transform matrix + std::ifstream file(MAKE_PATH_SAFE(OPT::strTransformFileName)); + std::string value; + std::vector transformValues; + while (file >> value) { + double v; + try { + v = std::stod(value); + } + catch (...) { + continue; + } + transformValues.push_back(v); + } + if (transformValues.size() != 12 && + (transformValues.size() != 16 || transformValues[12] != 0 || transformValues[13] != 0 || transformValues[14] != 0 || transformValues[15] != 1)) { + VERBOSE("error: invalid transform"); + return EXIT_FAILURE; + } + Matrix3x4 transform; + for (unsigned i=0; i<12; ++i) + transform[i] = transformValues[i]; + scene.Transform(transform); + VERBOSE("Scene transformed by the given transformation matrix (%s)", TD_TIMER_GET_FMT().c_str()); + } + + if (!OPT::strTransferTextureFileName.empty()) { + // transfer the texture of the scene's mesh to the new mesh; + // the two meshes should be aligned and the new mesh to have UV-coordinates + Mesh newMesh; + if (!newMesh.Load(MAKE_PATH_SAFE(OPT::strTransferTextureFileName))) + return EXIT_FAILURE; + Mesh::FaceIdxArr faceSubsetIndices; + if (!OPT::strIndicesFileName.empty()) { + std::ifstream in(OPT::strIndicesFileName.c_str()); + while (true) { + String index; + in >> index; + if (!in.good()) + break; + faceSubsetIndices.emplace_back(index.From()); + } + } + if (!scene.mesh.TransferTexture(newMesh, faceSubsetIndices)) + return EXIT_FAILURE; + newMesh.Save(baseFileName + OPT::strExportType); + VERBOSE("Texture transfered (%s)", TD_TIMER_GET_FMT().c_str()); + return EXIT_SUCCESS; + } + + if (OPT::nMaxResolution > 0) { + // scale scene images + const String folderName(Util::getFilePath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, OPT::strInputFileName)) + String::FormatString("images%u" PATH_SEPARATOR_STR, OPT::nMaxResolution)); + if (!scene.ScaleImages(OPT::nMaxResolution, 0, folderName)) { + DEBUG("error: can not scale scene images to '%s'", folderName.c_str()); + return EXIT_FAILURE; + } + } + + if (OPT::bComputeVolume && !scene.mesh.IsEmpty()) { + // compute the mesh volume + const REAL volume(scene.ComputeLeveledVolume(OPT::fPlaneThreshold, OPT::fSampleMesh, OPT::nUpAxis)); + VERBOSE("Mesh volume: %g (%s)", volume, TD_TIMER_GET_FMT().c_str()); + } + + // write transformed scene + if (scene.IsValid()) + scene.Save(MAKE_PATH_SAFE(OPT::strOutputFileName), (ARCHIVE_TYPE)OPT::nArchiveType); + if (!scene.IsValid() || (ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS) { + if (!scene.pointcloud.IsEmpty()) + scene.pointcloud.Save(baseFileName + _T(".ply"), (ARCHIVE_TYPE)OPT::nArchiveType == ARCHIVE_MVS); + if (!scene.mesh.IsEmpty()) + scene.mesh.Save(baseFileName + OPT::strExportType); + } + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/Viewer/CMakeLists.txt b/apps/Viewer/CMakeLists.txt new file mode 100644 index 0000000..fe09202 --- /dev/null +++ b/apps/Viewer/CMakeLists.txt @@ -0,0 +1,47 @@ +if((NOT OpenMVS_USE_OPENGL) OR (NOT _USE_OPENGL)) + RETURN() +endif() + +if(NOT VIEWER_NAME) + set(VIEWER_NAME "Viewer") +endif() + +# Find required packages +FIND_PACKAGE(GLEW QUIET) +if(GLEW_FOUND) + INCLUDE_DIRECTORIES(${GLEW_INCLUDE_DIRS}) + ADD_DEFINITIONS(${GLEW_DEFINITIONS}) + MESSAGE(STATUS "GLEW ${GLEW_VERSION} found (include: ${GLEW_INCLUDE_DIRS})") +else() + MESSAGE("-- Can't find GLEW. Continuing without it.") + RETURN() +endif() +FIND_PACKAGE(glfw3 QUIET) +if(glfw3_FOUND) + INCLUDE_DIRECTORIES(${glfw3_INCLUDE_DIRS}) + ADD_DEFINITIONS(${glfw3_DEFINITIONS}) + MESSAGE(STATUS "GLFW3 ${glfw3_VERSION} found (include: ${glfw3_INCLUDE_DIRS})") +else() + MESSAGE("-- Can't find GLFW3. Continuing without it.") + RETURN() +endif() + +# List sources files +if(MSVC) + FILE(GLOB LIBRARY_FILES_C "*.cpp" "*.rc") +else() + FILE(GLOB LIBRARY_FILES_C "*.cpp") +endif() +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_executable_with_flags(${VIEWER_NAME} "Apps" "${cxx_default}" "MVS;${OPENGL_LIBRARIES};${GLEW_LIBRARY};${GLFW_STATIC_LIBRARIES};GLEW::GLEW;${glfw3_LIBRARY};${GLFW3_LIBRARY};glfw;${OpenMVS_EXTRA_LIBS}" ${LIBRARY_FILES_C} ${LIBRARY_FILES_H}) + +# Manually set Common.h as the precompiled header +IF(CMAKE_VERSION VERSION_GREATER_EQUAL 3.16.0) + TARGET_PRECOMPILE_HEADERS(${VIEWER_NAME} PRIVATE "Common.h") +endif() + +# Install +INSTALL(TARGETS ${VIEWER_NAME} + EXPORT OpenMVSTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin) diff --git a/apps/Viewer/Camera.cpp b/apps/Viewer/Camera.cpp new file mode 100644 index 0000000..e6cb750 --- /dev/null +++ b/apps/Viewer/Camera.cpp @@ -0,0 +1,180 @@ +/* + * Camera.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "Common.h" +#include "Camera.h" + +using namespace VIEWER; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +Camera::Camera(const AABB3d& _box, const Point3d& _center, float _scaleF, float _fov) + : + boxScene(_box), + centerScene(_center), + rotation(Eigen::Quaterniond::Identity()), + center(Eigen::Vector3d::Zero()), + dist(0), radius(100), + fovDef(_fov), scaleFDef(_scaleF), + prevCamID(NO_ID), currentCamID(NO_ID), maxCamID(0) +{ + Reset(); +} + +void Camera::Reset() +{ + if (boxScene.IsEmpty()) { + center = Point3d::ZERO; + radius = 1; + } else { + center = centerScene; + radius = boxScene.GetSize().norm()*0.5; + } + rotation = Eigen::Quaterniond::Identity(); + scaleF = scaleFDef; + prevCamID = currentCamID = NO_ID; + fov = fovDef; + dist = radius*0.5 / SIN(D2R((double)fov)); + if (size.area()) + Resize(size); +} + +void Camera::Resize(const cv::Size& _size) +{ + ASSERT(MINF(_size.width, _size.height) > 0); + size = _size; + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + const GLfloat zNear = 1e-3f; + const GLfloat zFar = (float)boxScene.GetSize().norm()*10; + const GLfloat aspect = float(size.width)/float(size.height); + if (fov == 5.f) { + // orthographic projection + const GLfloat fH = (float)boxScene.GetSize().norm()*0.5f; + const GLfloat fW = fH * aspect; + glOrtho(-fW, fW, -fH, fH, zNear, zFar); + } else { + // perspective projection + const GLfloat fH = TAN(FD2R(fov)) * zNear; + const GLfloat fW = fH * aspect; + glFrustum(-fW, fW, -fH, fH, zNear, zFar); + } +} + +void Camera::SetFOV(float _fov) +{ + fov = MAXF(_fov, 5.f); + Resize(size); +} + + +Eigen::Vector3d Camera::GetPosition() const +{ + const Eigen::Matrix3d R(GetRotation()); + return center + R.col(2) * dist; +} + +Eigen::Matrix3d Camera::GetRotation() const +{ + return rotation.toRotationMatrix(); +} + +Eigen::Matrix4d Camera::GetLookAt() const +{ + const Eigen::Matrix3d R(GetRotation()); + const Eigen::Vector3d eye(center + R.col(2) * dist); + const Eigen::Vector3d up(R.col(1)); + + const Eigen::Vector3d n((center-eye).normalized()); + const Eigen::Vector3d s(n.cross(up)); + const Eigen::Vector3d v(s.cross(n)); + + Eigen::Matrix4d m; m << + s(0), s(1), s(2), -eye.dot(s), + v(0), v(1), v(2), -eye.dot(v), + -n(0), -n(1), -n(2), eye.dot(n), + 0.0, 0.0, 0.0, 1.0; + return m; +} +void Camera::GetLookAt(Eigen::Vector3d& _eye, Eigen::Vector3d& _center, Eigen::Vector3d& _up) const +{ + const Eigen::Matrix3d R(GetRotation()); + _eye = center + R.col(2) * dist; + _center = center; + _up = R.col(1); +} + + +void Camera::Rotate(const Eigen::Vector2d& pos, const Eigen::Vector2d& prevPos) +{ + if (pos.isApprox(prevPos, ZEROTOLERANCE())) + return; + + Eigen::Vector3d oldp(prevPos.x(), prevPos.y(), 0); + Eigen::Vector3d newp(pos.x(), pos.y(), 0); + const double radiusSphere(0.9); + ProjectOnSphere(radiusSphere, oldp); + ProjectOnSphere(radiusSphere, newp); + rotation *= Eigen::Quaterniond().setFromTwoVectors(newp, oldp); + + // disable camera view mode + prevCamID = currentCamID; +} + +void Camera::Translate(const Eigen::Vector2d& pos, const Eigen::Vector2d& prevPos) +{ + if (pos.isApprox(prevPos, ZEROTOLERANCE())) + return; + + Eigen::Matrix P, V; + glGetDoublev(GL_MODELVIEW_MATRIX, V.data()); + glGetDoublev(GL_PROJECTION_MATRIX, P.data()); + Eigen::Vector3d centerScreen((P*V*center.homogeneous().eval()).hnormalized()); + centerScreen.head<2>() += prevPos - pos; + center = (V.inverse()*P.inverse()*centerScreen.homogeneous().eval()).hnormalized(); + + // disable camera view mode + prevCamID = currentCamID; +} + +void Camera::ProjectOnSphere(double radius, Eigen::Vector3d& p) const +{ + p.z() = 0; + const double d = p.x()* p.x()+ p.y() * p.y(); + const double r = radius * radius; + if (d < r) p.z() = SQRT(r - d); + else p *= radius / p.norm(); +} +/*----------------------------------------------------------------*/ diff --git a/apps/Viewer/Camera.h b/apps/Viewer/Camera.h new file mode 100644 index 0000000..d83880a --- /dev/null +++ b/apps/Viewer/Camera.h @@ -0,0 +1,87 @@ +/* + * Camera.h + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#ifndef _VIEWER_CAMERA_H_ +#define _VIEWER_CAMERA_H_ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace VIEWER { + +class Camera +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + cv::Size size; + AABB3d boxScene; + Eigen::Vector3d centerScene; + Eigen::Quaterniond rotation; + Eigen::Vector3d center; + double dist, radius; + float fov, fovDef; + float scaleF, scaleFDef; + MVS::IIndex prevCamID, currentCamID, maxCamID; + +public: + Camera(const AABB3d& _box=AABB3d(true), const Point3d& _center=Point3d::ZERO, float _scaleF=1, float _fov=40); + + void Reset(); + void Resize(const cv::Size&); + void SetFOV(float _fov); + + const cv::Size& GetSize() const { return size; } + + Eigen::Vector3d GetPosition() const; + Eigen::Matrix3d GetRotation() const; + Eigen::Matrix4d GetLookAt() const; + + void GetLookAt(Eigen::Vector3d& eye, Eigen::Vector3d& center, Eigen::Vector3d& up) const; + void Rotate(const Eigen::Vector2d& pos, const Eigen::Vector2d& prevPos); + void Translate(const Eigen::Vector2d& pos, const Eigen::Vector2d& prevPos); + + bool IsCameraViewMode() const { return prevCamID != currentCamID && currentCamID != NO_ID; } + +protected: + void ProjectOnSphere(double radius, Eigen::Vector3d& p) const; +}; +/*----------------------------------------------------------------*/ + +} // namespace VIEWER + +#endif // _VIEWER_CAMERA_H_ diff --git a/apps/Viewer/Common.cpp b/apps/Viewer/Common.cpp new file mode 100644 index 0000000..ec1dc0a --- /dev/null +++ b/apps/Viewer/Common.cpp @@ -0,0 +1,36 @@ +/* + * Common.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +// Source file that includes just the standard includes +// Common.pch will be the pre-compiled header +// Common.obj will contain the pre-compiled type information + +#include "Common.h" diff --git a/apps/Viewer/Common.h b/apps/Viewer/Common.h new file mode 100644 index 0000000..f338783 --- /dev/null +++ b/apps/Viewer/Common.h @@ -0,0 +1,104 @@ +/* + * Common.h + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#ifndef _VIEWER_COMMON_H_ +#define _VIEWER_COMMON_H_ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include +#include "../../libs/MVS/Common.h" +#include "../../libs/MVS/Scene.h" + +#if defined(_MSC_VER) +#include +#elif defined(__APPLE__) +#include +#else +#include +#endif +#include + + +// D E F I N E S /////////////////////////////////////////////////// + + +// P R O T O T Y P E S ///////////////////////////////////////////// + +using namespace SEACAVE; + +namespace VIEWER { + +// the conversion matrix from OpenGL default coordinate system +// to the camera coordinate system (NADIR orientation): +// [ 1 0 0 0] * [ x ] = [ x ] +// 0 -1 0 0 y -y +// 0 0 -1 0 z -z +// 0 0 0 1 1 1 +static const Eigen::Matrix4d gs_convert = [] { + Eigen::Matrix4d tmp; tmp << + 1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, -1, 0, + 0, 0, 0, 1; + return tmp; +}(); + +/// given rotation matrix R and translation vector t, +/// column-major matrix m is equal to: +/// [ R11 R12 R13 t.x ] +/// | R21 R22 R23 t.y | +/// | R31 R32 R33 t.z | +/// [ 0.0 0.0 0.0 1.0 ] +// +// World to Local +inline Eigen::Matrix4d TransW2L(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) +{ + Eigen::Matrix4d m(Eigen::Matrix4d::Identity()); + m.block(0,0,3,3) = R; + m.block(0,3,3,1) = t; + return m; +} +// Local to World +// same as above, but with the inverse of the two +inline Eigen::Matrix4d TransL2W(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) +{ + Eigen::Matrix4d m(Eigen::Matrix4d::Identity()); + m.block(0,0,3,3) = R.transpose(); + m.block(0,3,3,1) = -t; + return m; +} +/*----------------------------------------------------------------*/ + +} // namespace MVS + +#endif // _VIEWER_COMMON_H_ diff --git a/apps/Viewer/Image.cpp b/apps/Viewer/Image.cpp new file mode 100644 index 0000000..4268196 --- /dev/null +++ b/apps/Viewer/Image.cpp @@ -0,0 +1,124 @@ +/* + * Image.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "Common.h" +#include "Image.h" + +using namespace VIEWER; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +Image::Image(MVS::IIndex _idx) + : + idx(_idx), + texture(0) +{ +} +Image::~Image() +{ + Release(); +} + +void Image::Release() +{ + if (IsValid()) { + glDeleteTextures(1, &texture); + texture = 0; + } + ReleaseImage(); +} +void Image::ReleaseImage() +{ + if (IsImageValid()) { + cv::Mat* const p(pImage); + Thread::safeExchange(pImage.ptr, (int_t)IMG_NULL); + delete p; + } +} + +void Image::SetImageLoading() +{ + ASSERT(IsImageEmpty()); + Thread::safeExchange(pImage.ptr, (int_t)IMG_LOADING); +} +void Image::AssignImage(cv::InputArray img) +{ + ASSERT(IsImageLoading()); + ImagePtrInt pImg(new cv::Mat(img.getMat())); + if (pImg.pImage->cols%4 != 0) { + // make sure the width is multiple of 4 (seems to be an OpenGL limitation) + cv::resize(*pImg.pImage, *pImg.pImage, cv::Size((pImg.pImage->cols/4)*4, pImg.pImage->rows), 0, 0, cv::INTER_AREA); + } + Thread::safeExchange(pImage.ptr, pImg.ptr); +} +bool Image::TransferImage() +{ + if (!IsImageValid()) + return false; + SetImage(*pImage); + glfwPostEmptyEvent(); + ReleaseImage(); + return true; +} + +void Image::SetImage(cv::InputArray img) +{ + cv::Mat image(img.getMat()); + glEnable(GL_TEXTURE_2D); + // create texture + glGenTextures(1, &texture); + // select our current texture + glBindTexture(GL_TEXTURE_2D, texture); + // load texture + width = image.cols; + height = image.rows; + ASSERT(image.channels() == 1 || image.channels() == 3); + ASSERT(image.isContinuous()); + glTexImage2D(GL_TEXTURE_2D, + 0, image.channels(), + width, height, + 0, (image.channels() == 1) ? GL_LUMINANCE : GL_BGR, + GL_UNSIGNED_BYTE, image.ptr()); + glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR); +} +void Image::GenerateMipmap() const { + glBindTexture(GL_TEXTURE_2D, texture); + glGenerateMipmap(GL_TEXTURE_2D); +} +void Image::Bind() const { + glBindTexture(GL_TEXTURE_2D, texture); +} +/*----------------------------------------------------------------*/ diff --git a/apps/Viewer/Image.h b/apps/Viewer/Image.h new file mode 100644 index 0000000..41c1851 --- /dev/null +++ b/apps/Viewer/Image.h @@ -0,0 +1,97 @@ +/* + * Image.h + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#ifndef _VIEWER_IMAGE_H_ +#define _VIEWER_IMAGE_H_ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace VIEWER { + +class Image +{ +public: + typedef CLISTDEFIDX(Image,uint32_t) ImageArr; + enum { + IMG_NULL = 0, + IMG_LOADING, + IMG_VALID + }; + union ImagePtrInt { + cv::Mat* pImage; + int_t ptr; + inline ImagePtrInt() : ptr(IMG_NULL) {} + inline ImagePtrInt(cv::Mat* p) : pImage(p) {} + inline operator cv::Mat* () const { return pImage; } + inline operator cv::Mat*& () { return pImage; } + }; + +public: + MVS::IIndex idx; // image index in the current scene + int width, height; + GLuint texture; + double opacity; + ImagePtrInt pImage; + +public: + Image(MVS::IIndex = NO_ID); + ~Image(); + + void Release(); + void ReleaseImage(); + inline bool IsValid() const { return texture > 0; } + inline bool IsImageEmpty() const { return pImage.ptr == IMG_NULL; } + inline bool IsImageLoading() const { return pImage.ptr == IMG_LOADING; } + inline bool IsImageValid() const { return pImage.ptr >= IMG_VALID; } + + void SetImageLoading(); + void AssignImage(cv::InputArray); + bool TransferImage(); + + void SetImage(cv::InputArray); + void GenerateMipmap() const; + void Bind() const; + +protected: +}; +typedef Image::ImageArr ImageArr; +/*----------------------------------------------------------------*/ + +} // namespace VIEWER + +#endif // _VIEWER_IMAGE_H_ diff --git a/apps/Viewer/Scene.cpp b/apps/Viewer/Scene.cpp new file mode 100644 index 0000000..161c33e --- /dev/null +++ b/apps/Viewer/Scene.cpp @@ -0,0 +1,823 @@ +/* + * Scene.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "Common.h" +#include "Scene.h" + +using namespace VIEWER; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define IMAGE_MAX_RESOLUTION 1024 + + +// S T R U C T S /////////////////////////////////////////////////// + +enum EVENT_TYPE { + EVT_JOB = 0, + EVT_CLOSE, +}; + +class EVTClose : public Event +{ +public: + EVTClose() : Event(EVT_CLOSE) {} +}; +class EVTLoadImage : public Event +{ +public: + Scene* pScene; + MVS::IIndex idx; + unsigned nMaxResolution; + bool Run(void*) { + Image& image = pScene->images[idx]; + ASSERT(image.idx != NO_ID); + MVS::Image& imageData = pScene->scene.images[image.idx]; + ASSERT(imageData.IsValid()); + if (imageData.image.empty() && !imageData.ReloadImage(nMaxResolution)) + return false; + imageData.UpdateCamera(pScene->scene.platforms); + image.AssignImage(imageData.image); + imageData.ReleaseImage(); + glfwPostEmptyEvent(); + return true; + } + EVTLoadImage(Scene* _pScene, MVS::IIndex _idx, unsigned _nMaxResolution=0) + : Event(EVT_JOB), pScene(_pScene), idx(_idx), nMaxResolution(_nMaxResolution) {} +}; +class EVTComputeOctree : public Event +{ +public: + Scene* pScene; + bool Run(void*) { + MVS::Scene& scene = pScene->scene; + if (!scene.mesh.IsEmpty()) { + Scene::OctreeMesh octMesh(scene.mesh.vertices, [](Scene::OctreeMesh::IDX_TYPE size, Scene::OctreeMesh::Type /*radius*/) { + return size > 256; + }); + scene.mesh.ListIncidenteFaces(); + pScene->octMesh.Swap(octMesh); + } else + if (!scene.pointcloud.IsEmpty()) { + Scene::OctreePoints octPoints(scene.pointcloud.points, [](Scene::OctreePoints::IDX_TYPE size, Scene::OctreePoints::Type /*radius*/) { + return size > 512; + }); + pScene->octPoints.Swap(octPoints); + } + return true; + } + EVTComputeOctree(Scene* _pScene) + : Event(EVT_JOB), pScene(_pScene) {} +}; + +void* Scene::ThreadWorker(void*) { + while (true) { + CAutoPtr evt(events.GetEvent()); + switch (evt->GetID()) { + case EVT_JOB: + evt->Run(); + break; + case EVT_CLOSE: + return NULL; + default: + ASSERT("Should not happen!" == NULL); + } + } + return NULL; +} +/*----------------------------------------------------------------*/ + + +// S T R U C T S /////////////////////////////////////////////////// + +SEACAVE::EventQueue Scene::events; +SEACAVE::Thread Scene::thread; + +Scene::Scene(ARCHIVE_TYPE _nArchiveType) + : + nArchiveType(_nArchiveType), + listPointCloud(0) +{ +} +Scene::~Scene() +{ + Release(); +} + +void Scene::Empty() +{ + ReleasePointCloud(); + ReleaseMesh(); + obbPoints.Release(); + if (window.IsValid()) { + window.ReleaseClbk(); + window.Reset(); + window.SetName(_T("(empty)")); + } + textures.Release(); + images.Release(); + scene.Release(); + sceneName.clear(); + geometryName.clear(); +} +void Scene::Release() +{ + if (window.IsValid()) + window.SetVisible(false); + if (!thread.isRunning()) { + events.AddEvent(new EVTClose()); + thread.join(); + } + Empty(); + window.Release(); + glfwTerminate(); +} +void Scene::ReleasePointCloud() +{ + if (listPointCloud) { + glDeleteLists(listPointCloud, 1); + listPointCloud = 0; + } +} +void Scene::ReleaseMesh() +{ + if (!listMeshes.empty()) { + for (GLuint listMesh: listMeshes) + glDeleteLists(listMesh, 1); + listMeshes.Release(); + } +} + +bool Scene::Init(const cv::Size& size, LPCTSTR windowName, LPCTSTR fileName, LPCTSTR geometryFileName) +{ + ASSERT(scene.IsEmpty()); + + // init window + if (glfwInit() == GL_FALSE) + return false; + if (!window.Init(size, windowName)) + return false; + if (glewInit() != GLEW_OK) + return false; + name = windowName; + window.clbkOpenScene = DELEGATEBINDCLASS(Window::ClbkOpenScene, &Scene::Open, this); + + // init OpenGL + glPolygonMode(GL_FRONT, GL_FILL); + glEnable(GL_DEPTH_TEST); + glClearColor(0.f, 0.5f, 0.9f, 1.f); + + static const float light0_ambient[] = {0.1f, 0.1f, 0.1f, 1.0f}; + static const float light0_diffuse[] = {1.0f, 1.0f, 1.0f, 1.0f}; + static const float light0_position[] = {0.0f, 0.0f, 1000.0f, 0.0f}; + static const float light0_specular[] = {0.4f, 0.4f, 0.4f, 1.0f}; + + glLightfv(GL_LIGHT0, GL_AMBIENT, light0_ambient); + glLightfv(GL_LIGHT0, GL_DIFFUSE, light0_diffuse); + glLightfv(GL_LIGHT0, GL_SPECULAR, light0_specular); + glLightfv(GL_LIGHT0, GL_POSITION, light0_position); + glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE); + + glEnable(GL_LIGHT0); + glDisable(GL_LIGHTING); + + // init working thread + thread.start(ThreadWorker); + + // open scene or init empty scene + window.SetCamera(Camera()); + if (fileName != NULL) + Open(fileName, geometryFileName); + window.SetVisible(true); + return true; +} +bool Scene::Open(LPCTSTR fileName, LPCTSTR geometryFileName) +{ + ASSERT(fileName); + DEBUG_EXTRA("Loading: '%s'", Util::getFileNameExt(fileName).c_str()); + Empty(); + sceneName = fileName; + + // load the scene + WORKING_FOLDER = Util::getFilePath(fileName); + INIT_WORKING_FOLDER; + if (!scene.Load(fileName, true)) + return false; + if (geometryFileName) { + // try to load given mesh + MVS::Mesh mesh; + MVS::PointCloud pointcloud; + if (mesh.Load(geometryFileName)) { + scene.mesh.Swap(mesh); + geometryName = geometryFileName; + geometryMesh = true; + } else + // try to load as a point-cloud + if (pointcloud.Load(geometryFileName)) { + scene.pointcloud.Swap(pointcloud); + geometryName = geometryFileName; + geometryMesh = false; + } + } + if (!scene.pointcloud.IsEmpty()) + scene.pointcloud.PrintStatistics(scene.images.data(), &scene.obb); + + #if 1 + // create octree structure used to accelerate selection functionality + if (!scene.IsEmpty()) + events.AddEvent(new EVTComputeOctree(this)); + #endif + + // init scene + AABB3d bounds(true); + Point3d center(Point3d::INF); + if (scene.IsBounded()) { + bounds = AABB3d(scene.obb.GetAABB()); + center = bounds.GetCenter(); + } else { + if (!scene.pointcloud.IsEmpty()) { + bounds = scene.pointcloud.GetAABB(MINF(3u,scene.nCalibratedImages)); + if (bounds.IsEmpty()) + bounds = scene.pointcloud.GetAABB(); + center = scene.pointcloud.GetCenter(); + } + if (!scene.mesh.IsEmpty()) { + scene.mesh.ComputeNormalFaces(); + bounds.Insert(scene.mesh.GetAABB()); + center = scene.mesh.GetCenter(); + } + } + + // init images + AABB3d imageBounds(true); + images.Reserve(scene.images.size()); + FOREACH(idxImage, scene.images) { + const MVS::Image& imageData = scene.images[idxImage]; + if (!imageData.IsValid()) + continue; + images.emplace_back(idxImage); + imageBounds.InsertFull(imageData.camera.C); + } + if (imageBounds.IsEmpty()) + imageBounds.Enlarge(0.5); + if (bounds.IsEmpty()) + bounds = imageBounds; + + // init and load texture + if (scene.mesh.HasTexture()) { + FOREACH(i, scene.mesh.texturesDiffuse) { + Image& image = textures.emplace_back(); + ASSERT(image.idx == NO_ID); + #if 0 + Image8U3& textureDiffuse = scene.mesh.texturesDiffuse[i]; + cv::flip(textureDiffuse, textureDiffuse, 0); + image.SetImage(textureDiffuse); + textureDiffuse.release(); + #else // preserve texture, used only to be able to export the mesh + Image8U3 textureDiffuse; + cv::flip(scene.mesh.texturesDiffuse[i], textureDiffuse, 0); + image.SetImage(textureDiffuse); + #endif + image.GenerateMipmap(); + } + } + + // init display lists + // compile point-cloud + CompilePointCloud(); + // compile mesh + CompileMesh(); + // compile bounding-box + CompileBounds(); + + // init camera + window.SetCamera(Camera(bounds, + center == Point3d::INF ? Point3d(bounds.GetCenter()) : center, + images.size()<2?1.f:(float)imageBounds.EnlargePercent(REAL(1)/images.size()).GetSize().norm())); + window.camera.maxCamID = images.size(); + window.SetName(String::FormatString((name + _T(": %s")).c_str(), Util::getFileName(fileName).c_str())); + window.clbkSaveScene = DELEGATEBINDCLASS(Window::ClbkSaveScene, &Scene::Save, this); + window.clbkExportScene = DELEGATEBINDCLASS(Window::ClbkExportScene, &Scene::Export, this); + window.clbkCenterScene = DELEGATEBINDCLASS(Window::ClbkCenterScene, &Scene::Center, this); + window.clbkCompilePointCloud = DELEGATEBINDCLASS(Window::ClbkCompilePointCloud, &Scene::CompilePointCloud, this); + window.clbkCompileMesh = DELEGATEBINDCLASS(Window::ClbkCompileMesh, &Scene::CompileMesh, this); + window.clbkTogleSceneBox = DELEGATEBINDCLASS(Window::ClbkTogleSceneBox, &Scene::TogleSceneBox, this); + window.clbkCropToBounds = DELEGATEBINDCLASS(Window::ClbkCropToBounds, &Scene::CropToBounds, this); + if (scene.IsBounded()) + window.clbkCompileBounds = DELEGATEBINDCLASS(Window::ClbkCompileBounds, &Scene::CompileBounds, this); + if (!scene.IsEmpty()) + window.clbkRayScene = DELEGATEBINDCLASS(Window::ClbkRayScene, &Scene::CastRay, this); + window.Reset(!scene.pointcloud.IsEmpty()&&!scene.mesh.IsEmpty()?Window::SPR_NONE:Window::SPR_ALL, + MINF(2u,images.size())); + return true; +} + +// export the scene +bool Scene::Save(LPCTSTR _fileName, bool bRescaleImages) +{ + if (!IsOpen()) + return false; + REAL imageScale = 0; + if (bRescaleImages) { + window.SetVisible(false); + std::cout << "Enter image resolution scale: "; + String strScale; + std::cin >> strScale; + window.SetVisible(true); + imageScale = strScale.From(0); + } + const String fileName(_fileName != NULL ? String(_fileName) : Util::insertBeforeFileExt(sceneName, _T("_new"))); + MVS::Mesh mesh; + if (!scene.mesh.IsEmpty() && !geometryName.empty() && geometryMesh) + mesh.Swap(scene.mesh); + MVS::PointCloud pointcloud; + if (!scene.pointcloud.IsEmpty() && !geometryName.empty() && !geometryMesh) + pointcloud.Swap(scene.pointcloud); + if (imageScale > 0 && imageScale < 1) { + // scale and save images + const String folderName(Util::getFilePath(MAKE_PATH_FULL(WORKING_FOLDER_FULL, fileName)) + String::FormatString("images%d" PATH_SEPARATOR_STR, ROUND2INT(imageScale*100))); + if (!scene.ScaleImages(0, imageScale, folderName)) { + DEBUG("error: can not scale scene images to '%s'", folderName.c_str()); + return false; + } + } + if (!scene.Save(fileName, nArchiveType)) { + DEBUG("error: can not save scene to '%s'", fileName.c_str()); + return false; + } + if (!mesh.IsEmpty()) + scene.mesh.Swap(mesh); + if (!pointcloud.IsEmpty()) + scene.pointcloud.Swap(pointcloud); + sceneName = fileName; + return true; +} + +// export the scene +bool Scene::Export(LPCTSTR _fileName, LPCTSTR exportType) const +{ + if (!IsOpen()) + return false; + ASSERT(!sceneName.IsEmpty()); + String lastFileName; + const String fileName(_fileName != NULL ? String(_fileName) : sceneName); + const String baseFileName(Util::getFileFullName(fileName)); + const bool bPoints(scene.pointcloud.Save(lastFileName=(baseFileName+_T("_pointcloud.ply")), nArchiveType==ARCHIVE_MVS)); + const bool bMesh(scene.mesh.Save(lastFileName=(baseFileName+_T("_mesh")+(exportType?exportType:(Util::getFileExt(fileName)==_T(".obj")?_T(".obj"):_T(".ply")))), cList(), true)); + #if TD_VERBOSE != TD_VERBOSE_OFF + if (VERBOSITY_LEVEL > 2 && (bPoints || bMesh)) + scene.ExportCamerasMLP(Util::getFileFullName(lastFileName)+_T(".mlp"), lastFileName); + #endif + AABB3f aabb(true); + if (scene.IsBounded()) { + std::ofstream fs(baseFileName+_T("_roi.txt")); + if (fs) + fs << scene.obb; + aabb = scene.obb.GetAABB(); + } else + if (!scene.pointcloud.IsEmpty()) { + aabb = scene.pointcloud.GetAABB(); + } else + if (!scene.mesh.IsEmpty()) { + aabb = scene.mesh.GetAABB(); + } + if (!aabb.IsEmpty()) { + std::ofstream fs(baseFileName+_T("_roi_box.txt")); + if (fs) + fs << aabb; + } + return bPoints || bMesh; +} + +void Scene::CompilePointCloud() +{ + if (scene.pointcloud.IsEmpty()) + return; + ReleasePointCloud(); + listPointCloud = glGenLists(1); + glNewList(listPointCloud, GL_COMPILE); + ASSERT((window.sparseType&(Window::SPR_POINTS|Window::SPR_LINES)) != 0); + // compile point-cloud + if ((window.sparseType&Window::SPR_POINTS) != 0) { + ASSERT_ARE_SAME_TYPE(float, MVS::PointCloud::Point::Type); + glBegin(GL_POINTS); + glColor3f(1.f,1.f,1.f); + FOREACH(i, scene.pointcloud.points) { + if (!scene.pointcloud.pointViews.empty() && + scene.pointcloud.pointViews[i].size() < window.minViews) + continue; + if (!scene.pointcloud.colors.empty()) { + const MVS::PointCloud::Color& c = scene.pointcloud.colors[i]; + glColor3ub(c.r,c.g,c.b); + } + const MVS::PointCloud::Point& X = scene.pointcloud.points[i]; + glVertex3fv(X.ptr()); + } + glEnd(); + } + glEndList(); +} + +void Scene::CompileMesh() +{ + if (scene.mesh.IsEmpty()) + return; + ReleaseMesh(); + if (scene.mesh.faceNormals.empty()) + scene.mesh.ComputeNormalFaces(); + // translate, normalize and flip Y axis of the texture coordinates + MVS::Mesh::TexCoordArr normFaceTexcoords; + if (scene.mesh.HasTexture() && window.bRenderTexture) + scene.mesh.FaceTexcoordsNormalize(normFaceTexcoords, true); + MVS::Mesh::TexIndex texIdx(0); + do { + GLuint& listMesh = listMeshes.emplace_back(glGenLists(1)); + listMesh = glGenLists(1); + glNewList(listMesh, GL_COMPILE); + // compile mesh + ASSERT_ARE_SAME_TYPE(float, MVS::Mesh::Vertex::Type); + ASSERT_ARE_SAME_TYPE(float, MVS::Mesh::Normal::Type); + ASSERT_ARE_SAME_TYPE(float, MVS::Mesh::TexCoord::Type); + glColor3f(1.f, 1.f, 1.f); + glBegin(GL_TRIANGLES); + FOREACH(idxFace, scene.mesh.faces) { + if (!scene.mesh.faceTexindices.empty() && scene.mesh.faceTexindices[idxFace] != texIdx) + continue; + const MVS::Mesh::Face& face = scene.mesh.faces[idxFace]; + const MVS::Mesh::Normal& n = scene.mesh.faceNormals[idxFace]; + glNormal3fv(n.ptr()); + for (int j = 0; j < 3; ++j) { + if (!normFaceTexcoords.empty()) { + const MVS::Mesh::TexCoord& t = normFaceTexcoords[idxFace*3 + j]; + glTexCoord2fv(t.ptr()); + } + const MVS::Mesh::Vertex& p = scene.mesh.vertices[face[j]]; + glVertex3fv(p.ptr()); + } + } + glEnd(); + glEndList(); + } while (++texIdx < scene.mesh.texturesDiffuse.size()); +} + +void Scene::CompileBounds() +{ + obbPoints.Release(); + if (!scene.IsBounded()) { + window.bRenderBounds = false; + return; + } + window.bRenderBounds = !window.bRenderBounds; + if (window.bRenderBounds) { + static const uint8_t indices[12*2] = { + 0,2, 2,3, 3,1, 1,0, + 0,6, 2,4, 3,5, 1,7, + 6,4, 4,5, 5,7, 7,6 + }; + OBB3f::POINT corners[OBB3f::numCorners]; + scene.obb.GetCorners(corners); + for (int i=0; i<12; ++i) { + obbPoints.emplace_back(corners[indices[i*2+0]]); + obbPoints.emplace_back(corners[indices[i*2+1]]); + } + } +} + +void Scene::CropToBounds() +{ + if (!IsOpen()) + return; + if (!scene.IsBounded()) + return; + scene.pointcloud.RemovePointsOutside(scene.obb); + scene.mesh.RemoveFacesOutside(scene.obb); + Center(); +} + +void Scene::Draw() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glPointSize(window.pointSize); + + // render point-cloud + if (listPointCloud) { + glDisable(GL_TEXTURE_2D); + glCallList(listPointCloud); + } + // render mesh + if (!listMeshes.empty()) { + glEnable(GL_DEPTH_TEST); + glEnable(GL_CULL_FACE); + if (!scene.mesh.faceTexcoords.empty() && window.bRenderTexture) { + glEnable(GL_TEXTURE_2D); + FOREACH(i, listMeshes) { + textures[i].Bind(); + glCallList(listMeshes[i]); + } + glDisable(GL_TEXTURE_2D); + } else { + glEnable(GL_LIGHTING); + for (GLuint listMesh: listMeshes) + glCallList(listMesh); + glDisable(GL_LIGHTING); + } + } + // render cameras + if (window.bRenderCameras) { + glDisable(GL_CULL_FACE); + const Point3* ptrPrevC(NULL); + FOREACH(idx, images) { + Image& image = images[idx]; + const MVS::Image& imageData = scene.images[image.idx]; + const MVS::Camera& camera = imageData.camera; + // cache image corner coordinates + const double scaleFocal(window.camera.scaleF); + const Point2d pp(camera.GetPrincipalPoint()); + const double focal(camera.GetFocalLength()/scaleFocal); + const double cx(-pp.x/focal); + const double cy(-pp.y/focal); + const double px((double)imageData.width/focal+cx); + const double py((double)imageData.height/focal+cy); + const Point3d ic1(cx, cy, scaleFocal); + const Point3d ic2(cx, py, scaleFocal); + const Point3d ic3(px, py, scaleFocal); + const Point3d ic4(px, cy, scaleFocal); + // change coordinates system to the camera space + glPushMatrix(); + glMultMatrixd((GLdouble*)TransL2W((const Matrix3x3::EMat)camera.R, -(const Point3::EVec)camera.C).data()); + glPointSize(window.pointSize+1.f); + glDisable(GL_TEXTURE_2D); + // draw camera position and image center + glBegin(GL_POINTS); + glColor3f(1,0,0); glVertex3f(0,0,0); // camera position + glColor3f(0,1,0); glVertex3f(0,0,(float)scaleFocal); // image center + glColor3f(0,0,1); glVertex3d((0.5*imageData.width-pp.x)/focal, cy, scaleFocal); // image up + glEnd(); + // draw image thumbnail + const bool bSelectedImage(idx == window.camera.currentCamID); + if (bSelectedImage) { + if (image.IsValid()) { + // render image + glEnable(GL_TEXTURE_2D); + image.Bind(); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glEnable(GL_BLEND); + glDisable(GL_DEPTH_TEST); + glColor4f(1,1,1,window.cameraBlend); + glBegin(GL_QUADS); + glTexCoord2d(0,0); glVertex3dv(ic1.ptr()); + glTexCoord2d(0,1); glVertex3dv(ic2.ptr()); + glTexCoord2d(1,1); glVertex3dv(ic3.ptr()); + glTexCoord2d(1,0); glVertex3dv(ic4.ptr()); + glEnd(); + glDisable(GL_TEXTURE_2D); + glDisable(GL_BLEND); + glEnable(GL_DEPTH_TEST); + } else { + // start and wait to load the image + if (image.IsImageEmpty()) { + // start loading + image.SetImageLoading(); + events.AddEvent(new EVTLoadImage(this, idx, IMAGE_MAX_RESOLUTION)); + } else { + // check if the image is available and set it + image.TransferImage(); + } + } + } + // draw camera frame + glColor3f(bSelectedImage ? 0.f : 1.f, 1.f, 0.f); + glBegin(GL_LINES); + glVertex3d(0,0,0); glVertex3dv(ic1.ptr()); + glVertex3d(0,0,0); glVertex3dv(ic2.ptr()); + glVertex3d(0,0,0); glVertex3dv(ic3.ptr()); + glVertex3d(0,0,0); glVertex3dv(ic4.ptr()); + glVertex3dv(ic1.ptr()); glVertex3dv(ic2.ptr()); + glVertex3dv(ic2.ptr()); glVertex3dv(ic3.ptr()); + glVertex3dv(ic3.ptr()); glVertex3dv(ic4.ptr()); + glVertex3dv(ic4.ptr()); glVertex3dv(ic1.ptr()); + glEnd(); + // restore coordinate system + glPopMatrix(); + // render image visibility info + if (window.bRenderImageVisibility && idx != NO_ID && idx==window.camera.currentCamID) { + if (scene.pointcloud.IsValid()) { + const Image& image = images[idx]; + glPointSize(window.pointSize*1.1f); + glDisable(GL_DEPTH_TEST); + glBegin(GL_POINTS); + glColor3f(1.f,0.f,0.f); + FOREACH(i, scene.pointcloud.points) { + ASSERT(!scene.pointcloud.pointViews[i].empty()); + if (scene.pointcloud.pointViews[i].size() < window.minViews) + continue; + if (scene.pointcloud.pointViews[i].FindFirst(image.idx) == MVS::PointCloud::ViewArr::NO_INDEX) + continue; + glVertex3fv(scene.pointcloud.points[i].ptr()); + } + glEnd(); + glEnable(GL_DEPTH_TEST); + glPointSize(window.pointSize); + } + } + // render camera trajectory + if (window.bRenderCameraTrajectory && ptrPrevC) { + glBegin(GL_LINES); + glColor3f(1.f,0.5f,0.f); + glVertex3dv(ptrPrevC->ptr()); + glVertex3dv(camera.C.ptr()); + glEnd(); + } + ptrPrevC = &camera.C; + } + } + // render selection + if (window.selectionType != Window::SEL_NA) { + glPointSize(window.pointSize+4); + glDisable(GL_DEPTH_TEST); + glBegin(GL_POINTS); + glColor3f(1,0,0); glVertex3fv(window.selectionPoints[0].ptr()); + if (window.selectionType == Window::SEL_TRIANGLE) { + glColor3f(0,1,0); glVertex3fv(window.selectionPoints[1].ptr()); + glColor3f(0,0,1); glVertex3fv(window.selectionPoints[2].ptr()); + } + glEnd(); + if (window.bRenderViews && window.selectionType == Window::SEL_POINT) { + if (!scene.pointcloud.pointViews.empty()) { + glBegin(GL_LINES); + const MVS::PointCloud::ViewArr& views = scene.pointcloud.pointViews[(MVS::PointCloud::Index)window.selectionIdx]; + ASSERT(!views.empty()); + for (MVS::PointCloud::View idxImage: views) { + const MVS::Image& imageData = scene.images[idxImage]; + glVertex3dv(imageData.camera.C.ptr()); + glVertex3fv(window.selectionPoints[0].ptr()); + } + glEnd(); + } + } + glEnable(GL_DEPTH_TEST); + glPointSize(window.pointSize); + } + // render oriented-bounding-box + if (!obbPoints.empty()) { + glDepthMask(GL_FALSE); + glBegin(GL_LINES); + glColor3f(0.5f,0.1f,0.8f); + for (IDX i=0; i timeClick) { + // this is a long click, ignore it + break; + } else + if (window.selectionType != Window::SEL_NA && + now-window.selectionTime < timeDblClick) { + // this is a double click, center scene at the selected point + window.CenterCamera(window.selectionPoints[3]); + window.selectionTime = now; + } else + if (!octMesh.IsEmpty()) { + // find ray intersection with the mesh + const MVS::IntersectRayMesh intRay(octMesh, ray, scene.mesh); + if (intRay.pick.IsValid()) { + const MVS::Mesh::Face& face = scene.mesh.faces[(MVS::Mesh::FIndex)intRay.pick.idx]; + window.selectionPoints[0] = scene.mesh.vertices[face[0]]; + window.selectionPoints[1] = scene.mesh.vertices[face[1]]; + window.selectionPoints[2] = scene.mesh.vertices[face[2]]; + window.selectionPoints[3] = ray.GetPoint(intRay.pick.dist).cast(); + window.selectionType = Window::SEL_TRIANGLE; + window.selectionTime = now; + window.selectionIdx = intRay.pick.idx; + DEBUG("Face selected:\n\tindex: %u\n\tvertex 1: %u (%g %g %g)\n\tvertex 2: %u (%g %g %g)\n\tvertex 3: %u (%g %g %g)", + intRay.pick.idx, + face[0], window.selectionPoints[0].x, window.selectionPoints[0].y, window.selectionPoints[0].z, + face[1], window.selectionPoints[1].x, window.selectionPoints[1].y, window.selectionPoints[1].z, + face[2], window.selectionPoints[2].x, window.selectionPoints[2].y, window.selectionPoints[2].z + ); + } else { + window.selectionType = Window::SEL_NA; + } + } else + if (!octPoints.IsEmpty()) { + // find ray intersection with the points + const MVS::IntersectRayPoints intRay(octPoints, ray, scene.pointcloud, window.minViews); + if (intRay.pick.IsValid()) { + window.selectionPoints[0] = window.selectionPoints[3] = scene.pointcloud.points[intRay.pick.idx]; + window.selectionType = Window::SEL_POINT; + window.selectionTime = now; + window.selectionIdx = intRay.pick.idx; + DEBUG("Point selected:\n\tindex: %u (%g %g %g)%s", + intRay.pick.idx, + window.selectionPoints[0].x, window.selectionPoints[0].y, window.selectionPoints[0].z, + [&]() { + if (scene.pointcloud.pointViews.empty()) + return String(); + const MVS::PointCloud::ViewArr& views = scene.pointcloud.pointViews[intRay.pick.idx]; + ASSERT(!views.empty()); + String strViews(String::FormatString("\n\tviews: %u", views.size())); + for (MVS::PointCloud::View idxImage: views) { + const MVS::Image& imageData = scene.images[idxImage]; + const Point2 x(imageData.camera.TransformPointW2I(Cast(window.selectionPoints[0]))); + strViews += String::FormatString("\n\t\t%s (%.2f %.2f)", Util::getFileNameExt(imageData.name).c_str(), x.x, x.y); + } + return strViews; + }().c_str() + ); + } else { + window.selectionType = Window::SEL_NA; + } + } + break; } + } +} +/*----------------------------------------------------------------*/ diff --git a/apps/Viewer/Scene.h b/apps/Viewer/Scene.h new file mode 100644 index 0000000..9315b35 --- /dev/null +++ b/apps/Viewer/Scene.h @@ -0,0 +1,111 @@ +/* + * Scene.h + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#ifndef _VIEWER_SCENE_H_ +#define _VIEWER_SCENE_H_ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Window.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace VIEWER { + +class Scene +{ +public: + typedef MVS::PointCloud::Octree OctreePoints; + typedef MVS::Mesh::Octree OctreeMesh; + +public: + ARCHIVE_TYPE nArchiveType; + String name; + + String sceneName; + String geometryName; + bool geometryMesh; + MVS::Scene scene; + Window window; + ImageArr images; // scene photos + ImageArr textures; // mesh textures + + OctreePoints octPoints; + OctreeMesh octMesh; + Point3fArr obbPoints; + + GLuint listPointCloud; + CLISTDEF0IDX(GLuint,MVS::Mesh::TexIndex) listMeshes; + + // multi-threading + static SEACAVE::EventQueue events; // internal events queue (processed by the working threads) + static SEACAVE::Thread thread; // worker thread + +public: + explicit Scene(ARCHIVE_TYPE _nArchiveType = ARCHIVE_MVS); + ~Scene(); + + void Empty(); + void Release(); + void ReleasePointCloud(); + void ReleaseMesh(); + inline bool IsValid() const { return window.IsValid(); } + inline bool IsOpen() const { return IsValid() && !scene.IsEmpty(); } + inline bool IsOctreeValid() const { return !octPoints.IsEmpty() || !octMesh.IsEmpty(); } + + bool Init(const cv::Size&, LPCTSTR windowName, LPCTSTR fileName=NULL, LPCTSTR geometryFileName=NULL); + bool Open(LPCTSTR fileName, LPCTSTR geometryFileName=NULL); + bool Save(LPCTSTR fileName=NULL, bool bRescaleImages=false); + bool Export(LPCTSTR fileName, LPCTSTR exportType=NULL) const; + void CompilePointCloud(); + void CompileMesh(); + void CompileBounds(); + void CropToBounds(); + + void Draw(); + void Loop(); + + void Center(); + void TogleSceneBox(); + void CastRay(const Ray3&, int); +protected: + static void* ThreadWorker(void*); +}; +/*----------------------------------------------------------------*/ + +} // namespace VIEWER + +#endif // _VIEWER_SCENE_H_ diff --git a/apps/Viewer/Viewer.cpp b/apps/Viewer/Viewer.cpp new file mode 100644 index 0000000..5bff3c2 --- /dev/null +++ b/apps/Viewer/Viewer.cpp @@ -0,0 +1,226 @@ +/* + * Viewer.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "Common.h" +#include + +#include "Scene.h" + +using namespace VIEWER; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define APPNAME _T("Viewer") + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace { + +namespace OPT { +String strInputFileName; +String strGeometryFileName; +String strOutputFileName; +unsigned nArchiveType; +int nProcessPriority; +unsigned nMaxThreads; +unsigned nMaxMemory; +String strExportType; +String strConfigFileName; +#if TD_VERBOSE != TD_VERBOSE_OFF +bool bLogFile; +#endif +boost::program_options::variables_map vm; +} // namespace OPT + +class Application { +public: + Application() {} + ~Application() { Finalize(); } + + bool Initialize(size_t argc, LPCTSTR* argv); + void Finalize(); +}; // Application + +// initialize and parse the command line parameters +bool Application::Initialize(size_t argc, LPCTSTR* argv) +{ + // initialize log and console + OPEN_LOG(); + OPEN_LOGCONSOLE(); + + // group of options allowed only on command line + boost::program_options::options_description generic("Generic options"); + generic.add_options() + ("help,h", "produce this help message") + ("working-folder,w", boost::program_options::value(&WORKING_FOLDER), "working directory (default current directory)") + ("config-file,c", boost::program_options::value(&OPT::strConfigFileName)->default_value(APPNAME _T(".cfg")), "file name containing program options") + ("export-type", boost::program_options::value(&OPT::strExportType), "file type used to export the 3D scene (ply or obj)") + ("archive-type", boost::program_options::value(&OPT::nArchiveType)->default_value(ARCHIVE_MVS), "project archive type: -1-interface, 0-text, 1-binary, 2-compressed binary") + ("process-priority", boost::program_options::value(&OPT::nProcessPriority)->default_value(0), "process priority (normal by default)") + ("max-threads", boost::program_options::value(&OPT::nMaxThreads)->default_value(0), "maximum number of threads that this process should use (0 - use all available cores)") + ("max-memory", boost::program_options::value(&OPT::nMaxMemory)->default_value(0), "maximum amount of memory in MB that this process should use (0 - use all available memory)") + #if TD_VERBOSE != TD_VERBOSE_OFF + ("log-file", boost::program_options::value(&OPT::bLogFile)->default_value(false), "dump log to a file") + ("verbosity,v", boost::program_options::value(&g_nVerbosityLevel)->default_value( + #if TD_VERBOSE == TD_VERBOSE_DEBUG + 3 + #else + 2 + #endif + ), "verbosity level") + #endif + ; + + // group of options allowed both on command line and in config file + boost::program_options::options_description config("Viewer options"); + config.add_options() + ("input-file,i", boost::program_options::value(&OPT::strInputFileName), "input project filename containing camera poses and scene (point-cloud/mesh)") + ("geometry-file,g", boost::program_options::value(&OPT::strGeometryFileName), "mesh or point-cloud with views file name (overwrite existing geometry)") + ("output-file,o", boost::program_options::value(&OPT::strOutputFileName), "output filename for storing the mesh") + ; + + boost::program_options::options_description cmdline_options; + cmdline_options.add(generic).add(config); + + boost::program_options::options_description config_file_options; + config_file_options.add(config); + + boost::program_options::positional_options_description p; + p.add("input-file", -1); + + try { + // parse command line options + boost::program_options::store(boost::program_options::command_line_parser((int)argc, argv).options(cmdline_options).positional(p).run(), OPT::vm); + boost::program_options::notify(OPT::vm); + INIT_WORKING_FOLDER; + // parse configuration file + std::ifstream ifs(MAKE_PATH_SAFE(OPT::strConfigFileName)); + if (ifs) { + boost::program_options::store(parse_config_file(ifs, config_file_options), OPT::vm); + boost::program_options::notify(OPT::vm); + } + } + catch (const std::exception& e) { + LOG(e.what()); + return false; + } + + #if TD_VERBOSE != TD_VERBOSE_OFF + // initialize the log file + if (OPT::bLogFile) + OPEN_LOGFILE((MAKE_PATH(APPNAME _T("-")+Util::getUniqueName(0)+_T(".log"))).c_str()); + #endif + + // print application details: version and command line + Util::LogBuild(); + LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str()); + + // validate input + Util::ensureValidPath(OPT::strInputFileName); + if (OPT::vm.count("help")) { + boost::program_options::options_description visible("Available options"); + visible.add(generic).add(config); + GET_LOG() << _T("\n" + "Visualize any know point-cloud/mesh formats or MVS projects. Supply files through command line or Drag&Drop.\n" + "Keys:\n" + "\tE: export scene\n" + "\tR: reset scene\n" + "\tB: render bounds\n" + "\tB + Shift: togle bounds\n" + "\tC: render cameras\n" + "\tC + Shift: render camera trajectory\n" + "\tC + Ctrl: center scene\n" + "\tLeft/Right: select next camera to view the scene\n" + "\tS: save scene\n" + "\tS + Shift: rescale images and save scene\n" + "\tT: render mesh texture\n" + "\tW: render wire-frame mesh\n" + "\tV: render view rays to the selected point\n" + "\tV + Shift: render points seen by the current view\n" + "\tUp/Down: adjust point size\n" + "\tUp/Down + Shift: adjust minimum number of views accepted when displaying a point or line\n" + "\t+/-: adjust camera thumbnail transparency\n" + "\t+/- + Shift: adjust camera cones' length\n" + "\n") + << visible; + } + if (!OPT::strExportType.empty()) + OPT::strExportType = OPT::strExportType.ToLower() == _T("obj") ? _T(".obj") : _T(".ply"); + + // initialize optional options + Util::ensureValidPath(OPT::strGeometryFileName); + Util::ensureValidPath(OPT::strOutputFileName); + + MVS::Initialize(APPNAME, OPT::nMaxThreads, OPT::nProcessPriority); + return true; +} + +// finalize application instance +void Application::Finalize() +{ + MVS::Finalize(); + + if (OPT::bLogFile) + CLOSE_LOGFILE(); + CLOSE_LOGCONSOLE(); + CLOSE_LOG(); +} + +} // unnamed namespace + +int main(int argc, LPCTSTR* argv) +{ + #ifdef _DEBUGINFO + // set _crtBreakAlloc index to stop in at allocation + _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);// | _CRTDBG_CHECK_ALWAYS_DF); + #endif + + Application application; + if (!application.Initialize(argc, argv)) + return EXIT_FAILURE; + + // create viewer + Scene viewer; + if (!viewer.Init(cv::Size(1280, 720), APPNAME, + OPT::strInputFileName.empty() ? NULL : MAKE_PATH_SAFE(OPT::strInputFileName).c_str(), + OPT::strGeometryFileName.empty() ? NULL : MAKE_PATH_SAFE(OPT::strGeometryFileName).c_str())) + return EXIT_FAILURE; + if (viewer.IsOpen() && !OPT::strOutputFileName.empty()) { + // export the scene + viewer.Export(MAKE_PATH_SAFE(OPT::strOutputFileName), OPT::strExportType.empty()?LPCTSTR(NULL):OPT::strExportType.c_str()); + } + // enter viewer loop + viewer.Loop(); + return EXIT_SUCCESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/Viewer/Window.cpp b/apps/Viewer/Window.cpp new file mode 100644 index 0000000..cfd2f12 --- /dev/null +++ b/apps/Viewer/Window.cpp @@ -0,0 +1,493 @@ +/* + * Window.cpp + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#include "Common.h" +#include "Window.h" + +using namespace VIEWER; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +Window::WindowsMap Window::g_mapWindows; + +Window::Window() + : + window(NULL), + pos(Eigen::Vector2d::Zero()), + prevPos(Eigen::Vector2d::Zero()) +{ +} +Window::~Window() +{ + Release(); +} + +void Window::Release() +{ + if (IsValid()) { + #ifdef _USE_NUKLEAR + nk_glfw3_shutdown(); + #endif + glfwDestroyWindow(window); + window = NULL; + } + clbkOpenScene.reset(); + ReleaseClbk(); +} + +void Window::ReleaseClbk() +{ + clbkSaveScene.reset(); + clbkExportScene.reset(); + clbkCenterScene.reset(); + clbkRayScene.reset(); + clbkCompilePointCloud.reset(); + clbkCompileMesh.reset(); + clbkCompileBounds.reset(); + clbkTogleSceneBox.reset(); + clbkCropToBounds.reset(); +} + +bool Window::Init(const cv::Size& _size, LPCTSTR name) +{ + sizeScale = 1; + size = _size; + + glfwDefaultWindowHints(); + glfwWindowHint(GLFW_VISIBLE, 0); + window = glfwCreateWindow(size.width, size.height, name, NULL, NULL); + if (!window) + return false; + glfwMakeContextCurrent(window); + glfwSetFramebufferSizeCallback(window, Window::Resize); + glfwSetKeyCallback(window, Window::Key); + glfwSetMouseButtonCallback(window, Window::MouseButton); + glfwSetCursorPosCallback(window, Window::MouseMove); + glfwSetScrollCallback(window, Window::Scroll); + glfwSetDropCallback(window, Window::Drop); + g_mapWindows[window] = this; + + Reset(); + return true; +} +void Window::SetCamera(const Camera& cam) +{ + camera = cam; + cv::Size _size; + glfwGetFramebufferSize(window, &_size.width, &_size.height); + Resize(_size); +} +void Window::SetName(LPCTSTR name) +{ + glfwSetWindowTitle(window, name); +} +void Window::SetVisible(bool v) +{ + if (v) + glfwShowWindow(window); + else + glfwHideWindow(window); +} +bool Window::IsVisible() const +{ + return glfwGetWindowAttrib(window, GLFW_VISIBLE) != 0; +} +void Window::Reset(SPARSE _sparseType, unsigned _minViews) +{ + camera.Reset(); + inputType = INP_NA; + sparseType = _sparseType; + minViews = _minViews; + pointSize = 2.f; + cameraBlend = 0.5f; + bRenderCameras = true; + bRenderCameraTrajectory = true; + bRenderImageVisibility = false; + bRenderViews = true; + bRenderSolid = true; + bRenderTexture = true; + bRenderBounds = false; + selectionType = SEL_NA; + selectionIdx = NO_IDX; + if (clbkCompilePointCloud != NULL) + clbkCompilePointCloud(); + if (clbkCompileMesh != NULL) + clbkCompileMesh(); + glfwPostEmptyEvent(); +} + + +void Window::CenterCamera(const Point3& pos) +{ + camera.center = pos; + camera.dist *= 0.7; +} + + +void Window::UpdateView(const ImageArr& images, const MVS::ImageArr& sceneImagesMVS) +{ + if (camera.IsCameraViewMode()) { + // enable camera view mode and apply current camera transform + const Image& image = images[camera.currentCamID]; + const MVS::Camera& camera = sceneImagesMVS[image.idx].camera; + UpdateView((const Matrix3x3::EMat)camera.R, camera.GetT()); + } else { + // apply view point transform + glMatrixMode(GL_MODELVIEW); + const Eigen::Matrix4d trans(camera.GetLookAt()); + glLoadMatrixd((GLdouble*)trans.data()); + } +} + +void Window::UpdateView(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) +{ + glMatrixMode(GL_MODELVIEW); + transform = gs_convert * TransW2L(R, t); + glLoadMatrixd((GLdouble*)transform.data()); +} + +void Window::UpdateMousePosition(double xpos, double ypos) +{ + prevPos = pos; + pos.x() = xpos; + pos.y() = ypos; + // normalize position to [-1:1] range + const int w(camera.size.width); + const int h(camera.size.height); + pos.x() = (2.0 * pos.x() - w) / w; + pos.y() = (h - 2.0 * pos.y()) / h; +} + + +void Window::GetFrame(Image8U3& image) const +{ + image.create(GetSize()); + glReadPixels(0, 0, image.width(), image.height(), GL_BGR_EXT, GL_UNSIGNED_BYTE, image.ptr()); + cv::flip(image, image, 0); +} + + +cv::Size Window::GetSize() const +{ + cv::Size _size; + glfwGetWindowSize(window, &_size.width, &_size.height); + return _size; +} +void Window::Resize(const cv::Size& _size) +{ + // detect scaled window + sizeScale = (double)GetSize().width/_size.width; + size = _size; + // update resolution + glfwMakeContextCurrent(window); + glViewport(0, 0, size.width, size.height); + camera.Resize(cv::Size(ROUND2INT(size.width*sizeScale), ROUND2INT(size.height*sizeScale))); +} +void Window::Resize(GLFWwindow* window, int width, int height) +{ + g_mapWindows[window]->Resize(cv::Size(width, height)); +} + +void Window::Key(int k, int /*scancode*/, int action, int mod) +{ + switch (k) { + case GLFW_KEY_ESCAPE: + if (action == GLFW_RELEASE) + glfwSetWindowShouldClose(window, 1); + break; + case GLFW_KEY_DOWN: + if (action == GLFW_RELEASE) { + if (mod & GLFW_MOD_SHIFT) { + if (minViews > 2) { + minViews--; + if (clbkCompilePointCloud != NULL) + clbkCompilePointCloud(); + } + } else { + pointSize = MAXF(pointSize-0.5f, 0.5f); + } + } + break; + case GLFW_KEY_UP: + if (action == GLFW_RELEASE) { + if (mod & GLFW_MOD_SHIFT) { + minViews++; + if (clbkCompilePointCloud != NULL) + clbkCompilePointCloud(); + } else { + pointSize += 0.5f; + } + } + break; + case GLFW_KEY_LEFT: + if (action != GLFW_RELEASE) { + camera.prevCamID = camera.currentCamID; + camera.currentCamID--; + if (camera.currentCamID < NO_ID && camera.currentCamID >= camera.maxCamID) + camera.currentCamID = camera.maxCamID-1; + } + break; + case GLFW_KEY_RIGHT: + if (action != GLFW_RELEASE) { + camera.prevCamID = camera.currentCamID; + camera.currentCamID++; + if (camera.currentCamID >= camera.maxCamID) + camera.currentCamID = NO_ID; + } + break; + case GLFW_KEY_B: + if (action == GLFW_RELEASE) { + if (mod & GLFW_MOD_CONTROL) { + if (clbkCropToBounds != NULL) + clbkCropToBounds(); + } else if (mod & GLFW_MOD_SHIFT) { + if (clbkTogleSceneBox != NULL) + clbkTogleSceneBox(); + } else { + if (clbkCompileBounds != NULL) + clbkCompileBounds(); + } + } + break; + case GLFW_KEY_C: + if (action == GLFW_RELEASE) { + if (mod & GLFW_MOD_SHIFT) { + bRenderCameraTrajectory = !bRenderCameraTrajectory; + } else if (mod & GLFW_MOD_CONTROL) { + if (clbkCenterScene != NULL) + clbkCenterScene(); + } else { + bRenderCameras = !bRenderCameras; + } + } + break; + case GLFW_KEY_E: + if (action == GLFW_RELEASE && clbkExportScene != NULL) + clbkExportScene(NULL, NULL); + break; + case GLFW_KEY_P: + switch (sparseType) { + case SPR_POINTS: sparseType = SPR_LINES; break; + case SPR_LINES: sparseType = SPR_ALL; break; + case SPR_ALL: sparseType = SPR_POINTS; break; + } + if (clbkCompilePointCloud != NULL) + clbkCompilePointCloud(); + break; + case GLFW_KEY_R: + if (action == GLFW_RELEASE) + Reset(); + break; + case GLFW_KEY_S: + if (action == GLFW_RELEASE) { + if (clbkSaveScene != NULL) + clbkSaveScene(NULL, (mod & GLFW_MOD_SHIFT) != 0); + } + break; + case GLFW_KEY_T: + if (action == GLFW_RELEASE) { + bRenderTexture = !bRenderTexture; + if (clbkCompileMesh != NULL) + clbkCompileMesh(); + } + break; + case GLFW_KEY_V: + if (action == GLFW_RELEASE) { + if (mod & GLFW_MOD_SHIFT) { + bRenderImageVisibility = !bRenderImageVisibility; + } else { + bRenderViews = !bRenderViews; + } + } + break; + case GLFW_KEY_W: + if (action == GLFW_RELEASE) { + if (bRenderSolid) { + bRenderSolid = false; + glPolygonMode(GL_FRONT, GL_LINE); + } else { + bRenderSolid = true; + glPolygonMode(GL_FRONT, GL_FILL); + } + } + break; + case GLFW_KEY_KP_SUBTRACT: + if (action == GLFW_RELEASE) { + if (mod & GLFW_MOD_CONTROL) + camera.SetFOV(camera.fov-5.f); + else if (mod & GLFW_MOD_SHIFT) + camera.scaleF *= 0.9f; + else + cameraBlend = MAXF(cameraBlend-0.1f, 0.f); + } + break; + case GLFW_KEY_KP_ADD: + if (action == GLFW_RELEASE) { + if (mod & GLFW_MOD_CONTROL) + camera.SetFOV(camera.fov+5.f); + else if (mod & GLFW_MOD_SHIFT) + camera.scaleF *= 1.1111f; + else + cameraBlend = MINF(cameraBlend+0.1f, 1.f); + } + break; + } +} +void Window::Key(GLFWwindow* window, int k, int scancode, int action, int mod) +{ + g_mapWindows[window]->Key(k, scancode, action, mod); +} + +void Window::MouseButton(int button, int action, int /*mods*/) +{ + switch (button) { + case GLFW_MOUSE_BUTTON_LEFT: { + if (action == GLFW_PRESS) { + inputType.set(INP_MOUSE_LEFT); + } else + if (action == GLFW_RELEASE) { + inputType.unset(INP_MOUSE_LEFT); + glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_NORMAL); + } + if (clbkRayScene != NULL) { + typedef Eigen::Matrix Mat4; + Mat4 P, V; + glGetDoublev(GL_MODELVIEW_MATRIX, V.data()); + glGetDoublev(GL_PROJECTION_MATRIX, P.data()); + // 4d Homogeneous Clip Coordinates + const Eigen::Vector4d ray_clip(pos.x(), pos.y(), -1.0, 1.0); + // 4d Eye (Camera) Coordinates + Eigen::Vector4d ray_eye(P.inverse()*ray_clip); + ray_eye.z() = -1.0; + ray_eye.w() = 0.0; + // 4d World Coordinates + const Mat4 invV(V.inverse()); + ASSERT(ISEQUAL(invV(3,3),1.0)); + const Eigen::Vector3d start(invV.topRightCorner<3,1>()); + const Eigen::Vector4d ray_wor(invV*ray_eye); + const Eigen::Vector3d dir(ray_wor.topRows<3>().normalized()); + clbkRayScene(Ray3d(start, dir), action); + } + } break; + case GLFW_MOUSE_BUTTON_MIDDLE: { + if (action == GLFW_PRESS) { + inputType.set(INP_MOUSE_MIDDLE); + glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED); + } else + if (action == GLFW_RELEASE) { + inputType.unset(INP_MOUSE_MIDDLE); + glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_NORMAL); + } + } break; + case GLFW_MOUSE_BUTTON_RIGHT: { + if (action == GLFW_PRESS) { + inputType.set(INP_MOUSE_RIGHT); + } else + if (action == GLFW_RELEASE) { + inputType.unset(INP_MOUSE_RIGHT); + } + } + } +} +void Window::MouseButton(GLFWwindow* window, int button, int action, int mods) +{ + g_mapWindows[window]->MouseButton(button, action, mods); +} + +void Window::MouseMove(double xpos, double ypos) +{ + UpdateMousePosition(xpos, ypos); + if (inputType.isSet(INP_MOUSE_LEFT)) { + glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED); + camera.Rotate(pos, prevPos); + } else + if (inputType.isSet(INP_MOUSE_MIDDLE)) { + camera.Translate(pos, prevPos); + } +} +void Window::MouseMove(GLFWwindow* window, double xpos, double ypos) +{ + g_mapWindows[window]->MouseMove(xpos, ypos); +} + +void Window::Scroll(double /*xoffset*/, double yoffset) +{ + camera.dist *= (yoffset>0 ? POW(1.11,yoffset) : POW(0.9,-yoffset)); +} +void Window::Scroll(GLFWwindow* window, double xoffset, double yoffset) +{ + g_mapWindows[window]->Scroll(xoffset, yoffset); +} + +void Window::Drop(int count, const char** paths) +{ + if (clbkOpenScene && count > 0) { + SetVisible(false); + String fileName(paths[0]); + Util::ensureUnifySlash(fileName); + if (count > 1) { + String geometryFileName(paths[1]); + Util::ensureUnifySlash(geometryFileName); + clbkOpenScene(fileName, geometryFileName); + } else { + clbkOpenScene(fileName, NULL); + } + SetVisible(true); + } +} +void Window::Drop(GLFWwindow* window, int count, const char** paths) +{ + g_mapWindows[window]->Drop(count, paths); +} + +bool Window::IsShiftKeyPressed() const +{ + return + glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS || + glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS; +} +bool Window::IsCtrlKeyPressed() const +{ + return + glfwGetKey(window, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS || + glfwGetKey(window, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS; +} +bool Window::IsAltKeyPressed() const +{ + return + glfwGetKey(window, GLFW_KEY_LEFT_ALT) == GLFW_PRESS || + glfwGetKey(window, GLFW_KEY_RIGHT_ALT) == GLFW_PRESS; +} +/*----------------------------------------------------------------*/ diff --git a/apps/Viewer/Window.h b/apps/Viewer/Window.h new file mode 100644 index 0000000..ad9a957 --- /dev/null +++ b/apps/Viewer/Window.h @@ -0,0 +1,167 @@ +/* + * Window.h + * + * Copyright (c) 2014-2015 SEACAVE + * + * Author(s): + * + * cDc + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + * + * Additional Terms: + * + * You are required to preserve legal notices and author attributions in + * that material or in the Appropriate Legal Notices displayed by works + * containing it. + */ + +#ifndef _VIEWER_WINDOW_H_ +#define _VIEWER_WINDOW_H_ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Camera.h" +#include "Image.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace VIEWER { + +class Window +{ +public: + GLFWwindow* window; // window handle + Camera camera; // current camera + Eigen::Vector2d pos, prevPos; // current and previous mouse position (normalized) + Eigen::Matrix4d transform; // view matrix corresponding to the currently selected image + cv::Size size; // resolution in pixels, sometimes not equal to window resolution, ex. on Retina display + double sizeScale; // window/screen resolution scale + + enum INPUT : unsigned { + INP_NA = 0, + INP_MOUSE_LEFT = (1 << 0), + INP_MOUSE_MIDDLE = (1 << 1), + INP_MOUSE_RIGHT = (1 << 2), + }; + Flags inputType; + + enum SPARSE { + SPR_NONE = 0, + SPR_POINTS = (1 << 0), + SPR_LINES = (1 << 1), + SPR_ALL = SPR_POINTS|SPR_LINES + }; + SPARSE sparseType; + unsigned minViews; + float pointSize; + float cameraBlend; + bool bRenderCameras; + bool bRenderCameraTrajectory; + bool bRenderImageVisibility; + bool bRenderViews; + bool bRenderSolid; + bool bRenderTexture; + bool bRenderBounds; + + enum SELECTION { + SEL_NA = 0, + SEL_POINT, + SEL_TRIANGLE + }; + SELECTION selectionType; + Point3f selectionPoints[4]; + double selectionTimeClick, selectionTime; + IDX selectionIdx; + + typedef DELEGATE ClbkOpenScene; + ClbkOpenScene clbkOpenScene; + typedef DELEGATE ClbkSaveScene; + ClbkSaveScene clbkSaveScene; + typedef DELEGATE ClbkExportScene; + ClbkExportScene clbkExportScene; + typedef DELEGATE ClbkCenterScene; + ClbkCenterScene clbkCenterScene; + typedef DELEGATE ClbkRayScene; + ClbkRayScene clbkRayScene; + typedef DELEGATE ClbkCompilePointCloud; + ClbkCompilePointCloud clbkCompilePointCloud; + typedef DELEGATE ClbkCompileMesh; + ClbkCompileMesh clbkCompileMesh; + typedef DELEGATE ClbkCompileBounds; + ClbkCompileBounds clbkCompileBounds; + typedef DELEGATE ClbkTogleSceneBox; + ClbkTogleSceneBox clbkTogleSceneBox; + typedef DELEGATE ClbkCropToBounds; + ClbkCropToBounds clbkCropToBounds; + + typedef std::unordered_map WindowsMap; + static WindowsMap g_mapWindows; + +public: + Window(); + ~Window(); + + void Release(); + void ReleaseClbk(); + inline bool IsValid() const { return window != NULL; } + + inline GLFWwindow* GetWindow() { return window; } + + bool Init(const cv::Size&, LPCTSTR name); + void SetCamera(const Camera&); + void SetName(LPCTSTR); + void SetVisible(bool); + bool IsVisible() const; + void Reset(SPARSE sparseType=SPR_ALL, unsigned minViews=2); + + void CenterCamera(const Point3&); + + void UpdateView(const ImageArr&, const MVS::ImageArr&); + void UpdateView(const Eigen::Matrix3d& R, const Eigen::Vector3d& t); + void UpdateMousePosition(double xpos, double ypos); + + void GetFrame(Image8U3&) const; + + cv::Size GetSize() const; + void Resize(const cv::Size&); + static void Resize(GLFWwindow* window, int width, int height); + void Key(int k, int scancode, int action, int mod); + static void Key(GLFWwindow* window, int k, int scancode, int action, int mod); + void MouseButton(int button, int action, int mods); + static void MouseButton(GLFWwindow* window, int button, int action, int mods); + void MouseMove(double xpos, double ypos); + static void MouseMove(GLFWwindow* window, double xpos, double ypos); + void Scroll(double xoffset, double yoffset); + static void Scroll(GLFWwindow* window, double xoffset, double yoffset); + void Drop(int count, const char** paths); + static void Drop(GLFWwindow* window, int count, const char** paths); + +protected: + bool IsShiftKeyPressed() const; + bool IsCtrlKeyPressed() const; + bool IsAltKeyPressed() const; +}; +/*----------------------------------------------------------------*/ + +} // namespace VIEWER + +#endif // _VIEWER_WINDOW_H_ diff --git a/libs/CMakeLists.txt b/libs/CMakeLists.txt new file mode 100644 index 0000000..3b6adb2 --- /dev/null +++ b/libs/CMakeLists.txt @@ -0,0 +1,8 @@ +# Add libraries +ADD_SUBDIRECTORY(Common) +ADD_SUBDIRECTORY(Math) +ADD_SUBDIRECTORY(IO) +ADD_SUBDIRECTORY(MVS) + +# Install +INSTALL(FILES "MVS.h" DESTINATION "${INSTALL_INCLUDE_DIR}") diff --git a/libs/Common/AABB.h b/libs/Common/AABB.h new file mode 100644 index 0000000..da291f3 --- /dev/null +++ b/libs/Common/AABB.h @@ -0,0 +1,121 @@ +//////////////////////////////////////////////////////////////////// +// AABB.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_AABB_H__ +#define __SEACAVE_AABB_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// Basic axis-aligned bounding-box class +template +class TAABB +{ + STATIC_ASSERT(DIMS > 0 && DIMS <= 3); + +public: + typedef TYPE Type; + typedef Eigen::Matrix POINT; + typedef Eigen::Matrix MATRIX; + enum { numChildren = (2<<(DIMS-1)) }; + enum { numCorners = (DIMS==1 ? 2 : (DIMS==2 ? 4 : 8)) }; // 2^DIMS + enum { numScalar = (2*DIMS) }; + + POINT ptMin, ptMax; // box extreme points + + //--------------------------------------- + + inline TAABB() {} + inline TAABB(bool); + inline TAABB(const POINT& _pt); + inline TAABB(const POINT& _ptMin, const POINT& _ptMax); + inline TAABB(const POINT& center, const TYPE& radius); + template + inline TAABB(const TPoint* pts, size_t n); + template + inline TAABB(const TAABB&); + + inline void Reset(); + inline void Set(const POINT& _pt); + inline void Set(const POINT& _ptMin, const POINT& _ptMax); + inline void Set(const POINT& center, const TYPE& radius); + template + inline void Set(const TPoint* pts, size_t n); + + inline bool IsEmpty() const; + + inline TAABB& Enlarge(TYPE); + inline TAABB& EnlargePercent(TYPE); + + void InsertFull(const POINT&); + void Insert(const POINT&); + void Insert(const TAABB&); + void BoundBy(const TAABB&); + + inline void Translate(const POINT&); + inline void Transform(const MATRIX&); + + inline POINT GetCenter() const; + inline void GetCenter(POINT&) const; + + inline POINT GetSize() const; + inline void GetSize(POINT&) const; + + inline void GetCorner(BYTE i, POINT&) const; + inline POINT GetCorner(BYTE i) const; + inline void GetCorners(POINT pts[numCorners]) const; + + bool Intersects(const TAABB&) const; + bool IntersectsComplete(const TAABB&, TYPE) const; + bool IntersectsComplete(const TAABB&) const; + bool Intersects(const POINT&) const; + bool IntersectsComplete(const POINT&, TYPE) const; + bool IntersectsComplete(const POINT&) const; + + unsigned SplitBy(TYPE, int, TAABB [2]) const; + unsigned SplitBy(const POINT&, TAABB [numChildren], unsigned&) const; + + inline TYPE& operator [] (BYTE i) { ASSERT(i> (std::istream& st, TAABB& obb) { + st >> obb.ptMin; + st >> obb.ptMax; + return st; + } + + #ifdef _USE_BOOST + // implement BOOST serialization + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & ptMin; + ar & ptMax; + } + #endif +}; // class TAABB +/*----------------------------------------------------------------*/ + + +#include "AABB.inl" +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_AABB_H__ diff --git a/libs/Common/AABB.inl b/libs/Common/AABB.inl new file mode 100644 index 0000000..b645ef1 --- /dev/null +++ b/libs/Common/AABB.inl @@ -0,0 +1,419 @@ +//////////////////////////////////////////////////////////////////// +// AABB.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +template +inline TAABB::TAABB(bool) + : + ptMin(POINT::Constant(std::numeric_limits::max())), + ptMax(POINT::Constant(std::numeric_limits::lowest())) +{ +} +template +inline TAABB::TAABB(const POINT& _pt) + : + ptMin(_pt), ptMax(_pt) +{ +} +template +inline TAABB::TAABB(const POINT& _ptMin, const POINT& _ptMax) + : + ptMin(_ptMin), ptMax(_ptMax) +{ +} +template +inline TAABB::TAABB(const POINT& center, const TYPE& radius) + : + ptMin(center-POINT::Constant(radius)), ptMax(center+POINT::Constant(radius)) +{ +} +template +template +inline TAABB::TAABB(const TPoint* pts, size_t n) +{ + Set(pts, n); +} // constructor +template +template +inline TAABB::TAABB(const TAABB& rhs) + : + ptMin(rhs.ptMin.template cast()), ptMax(rhs.ptMax.template cast()) +{ +} // copy constructor +/*----------------------------------------------------------------*/ + + +template +inline void TAABB::Reset() +{ + ptMin = POINT::Constant(std::numeric_limits::max()); + ptMax = POINT::Constant(std::numeric_limits::lowest()); +} +template +inline void TAABB::Set(const POINT& _pt) +{ + ptMin = ptMax = _pt; +} +template +inline void TAABB::Set(const POINT& _ptMin, const POINT& _ptMax) +{ + ptMin = _ptMin; + ptMax = _ptMax; +} +template +inline void TAABB::Set(const POINT& center, const TYPE& radius) +{ + ptMin = center-POINT::Constant(radius); + ptMax = center+POINT::Constant(radius); +} +template +template +inline void TAABB::Set(const TPoint* pts, size_t n) +{ + ASSERT(n > 0); + ptMin = ptMax = pts[0]; + for (size_t i=1; i +inline bool TAABB::IsEmpty() const +{ + for (int i=0; i= ptMax[i]) + return true; + return false; +} // IsEmpty +/*----------------------------------------------------------------*/ + + +template +inline TAABB& TAABB::Enlarge(TYPE x) +{ + ptMin.array() -= x; + ptMax.array() += x; + return *this; +} +template +inline TAABB& TAABB::EnlargePercent(TYPE x) +{ + const POINT ptSizeDelta(GetSize() * (x - TYPE(1)) / TYPE(2)); + ptMin -= ptSizeDelta; + ptMax += ptSizeDelta; + return *this; +} // Enlarge +/*----------------------------------------------------------------*/ + + +template +inline typename TAABB::POINT TAABB::GetCenter() const +{ + return (ptMax + ptMin) * TYPE(0.5); +} +template +inline void TAABB::GetCenter(POINT& ptCenter) const +{ + ptCenter = (ptMax + ptMin) * TYPE(0.5); +} // GetCenter +/*----------------------------------------------------------------*/ + + +template +inline typename TAABB::POINT TAABB::GetSize() const +{ + return ptMax - ptMin; +} +template +inline void TAABB::GetSize(POINT& ptSize) const +{ + ptSize = ptMax - ptMin; +} // GetSize +/*----------------------------------------------------------------*/ + + +template +inline void TAABB::GetCorner(BYTE i, POINT& ptCorner) const +{ + ASSERT(i +inline typename TAABB::POINT TAABB::GetCorner(BYTE i) const +{ + POINT ptCorner; + GetCorner(i, ptCorner); + return ptCorner; +} // GetCorner +template +inline void TAABB::GetCorners(POINT pts[numCorners]) const +{ + for (BYTE i=0; i +inline void TAABB::Translate(const POINT& d) +{ + ptMin += d; + ptMax += d; +} +/*----------------------------------------------------------------*/ + + +// Update the box by the given transform. +template +inline void TAABB::Transform(const MATRIX& m) +{ + ptMin = m * ptMin; + ptMax = m * ptMax; +} +/*----------------------------------------------------------------*/ + + +// Update the box such that it contains the given point. +template +void TAABB::InsertFull(const POINT& pt) +{ + if (ptMin[0] > pt[0]) + ptMin[0] = pt[0]; + if (ptMax[0] < pt[0]) + ptMax[0] = pt[0]; + + if (DIMS > 1) { + if (ptMin[1] > pt[1]) + ptMin[1] = pt[1]; + if (ptMax[1] < pt[1]) + ptMax[1] = pt[1]; + } + + if (DIMS > 2) { + if (ptMin[2] > pt[2]) + ptMin[2] = pt[2]; + if (ptMax[2] < pt[2]) + ptMax[2] = pt[2]; + } +} +// same as above, but for the initialized case +template +void TAABB::Insert(const POINT& pt) +{ + if (ptMin[0] > pt[0]) + ptMin[0] = pt[0]; + else if (ptMax[0] < pt[0]) + ptMax[0] = pt[0]; + + if (DIMS > 1) { + if (ptMin[1] > pt[1]) + ptMin[1] = pt[1]; + else if (ptMax[1] < pt[1]) + ptMax[1] = pt[1]; + } + + if (DIMS > 2) { + if (ptMin[2] > pt[2]) + ptMin[2] = pt[2]; + else if (ptMax[2] < pt[2]) + ptMax[2] = pt[2]; + } +} +/*----------------------------------------------------------------*/ + +// Update the box such that it contains the given bounding box. +template +void TAABB::Insert(const TAABB& aabb) +{ + if (ptMin[0] > aabb.ptMin[0]) + ptMin[0] = aabb.ptMin[0]; + if (ptMax[0] < aabb.ptMax[0]) + ptMax[0] = aabb.ptMax[0]; + + if (DIMS > 1) { + if (ptMin[1] > aabb.ptMin[1]) + ptMin[1] = aabb.ptMin[1]; + if (ptMax[1] < aabb.ptMax[1]) + ptMax[1] = aabb.ptMax[1]; + } + + if (DIMS > 2) { + if (ptMin[2] > aabb.ptMin[2]) + ptMin[2] = aabb.ptMin[2]; + if (ptMax[2] < aabb.ptMax[2]) + ptMax[2] = aabb.ptMax[2]; + } +} +/*----------------------------------------------------------------*/ + +// Update the box such that it does not exceed the given bounding box. +template +void TAABB::BoundBy(const TAABB& aabb) +{ + if (ptMin[0] < aabb.ptMin[0]) + ptMin[0] = aabb.ptMin[0]; + if (ptMax[0] > aabb.ptMax[0]) + ptMax[0] = aabb.ptMax[0]; + + if (DIMS > 1) { + if (ptMin[1] < aabb.ptMin[1]) + ptMin[1] = aabb.ptMin[1]; + if (ptMax[1] > aabb.ptMax[1]) + ptMax[1] = aabb.ptMax[1]; + } + + if (DIMS > 2) { + if (ptMin[2] < aabb.ptMin[2]) + ptMin[2] = aabb.ptMin[2]; + if (ptMax[2] > aabb.ptMax[2]) + ptMax[2] = aabb.ptMax[2]; + } +} +/*----------------------------------------------------------------*/ + + +// intersection between two AABBs +template +bool TAABB::Intersects(const TAABB& aabb) const +{ + if (aabb.ptMax[0] < ptMin[0]) return false; + if (aabb.ptMin[0] >= ptMax[0]) return false; + if (DIMS > 1) { + if (aabb.ptMax[1] < ptMin[1]) return false; + if (aabb.ptMin[1] >= ptMax[1]) return false; + } + if (DIMS > 2) { + if (aabb.ptMax[2] < ptMin[2]) return false; + if (aabb.ptMin[2] >= ptMax[2]) return false; + } + return true; +} +template +bool TAABB::IntersectsComplete(const TAABB& aabb, TYPE tolerance) const +{ + if (aabb.ptMax[0]+tolerance < ptMin[0]) return false; + if (aabb.ptMin[0] > ptMax[0]+tolerance) return false; + if (DIMS > 1) { + if (aabb.ptMax[1]+tolerance < ptMin[1]) return false; + if (aabb.ptMin[1] > ptMax[1]+tolerance) return false; + } + if (DIMS > 2) { + if (aabb.ptMax[2]+tolerance < ptMin[2]) return false; + if (aabb.ptMin[2] > ptMax[2]+tolerance) return false; + } + return true; +} +template +bool TAABB::IntersectsComplete(const TAABB& aabb) const +{ + return IntersectsComplete(aabb, ZEROTOLERANCE()); +} // Intersects(TAABB) +/*----------------------------------------------------------------*/ + +// does TAABB contain the given point +template +bool TAABB::Intersects(const POINT& pt) const +{ + if (pt[0] < ptMin[0]) return false; + if (pt[0] >= ptMax[0]) return false; + if (DIMS > 1) { + if (pt[1] < ptMin[1]) return false; + if (pt[1] >= ptMax[1]) return false; + } + if (DIMS > 2) { + if (pt[2] < ptMin[2]) return false; + if (pt[2] >= ptMax[2]) return false; + } + return true; +} +template +bool TAABB::IntersectsComplete(const POINT& pt, TYPE tolerance) const +{ + if (pt[0]+tolerance < ptMin[0]) return false; + if (pt[0] > ptMax[0]+tolerance) return false; + if (DIMS > 1) { + if (pt[1]+tolerance < ptMin[1]) return false; + if (pt[1] > ptMax[1]+tolerance) return false; + } + if (DIMS > 2) { + if (pt[2]+tolerance < ptMin[2]) return false; + if (pt[2] > ptMax[2]+tolerance) return false; + } + return true; +} +template +bool TAABB::IntersectsComplete(const POINT& pt) const +{ + return IntersectsComplete(pt, ZEROTOLERANCE()); +} // Intersects(point) +/*----------------------------------------------------------------*/ + + +// split this TAABB by the given axis +// returns number of children (0 or 2) +template +unsigned TAABB::SplitBy(TYPE d, int a, TAABB child[2]) const +{ + ASSERT(a < DIMS); + if (d >= ptMin[a] && d < ptMax[a]) { + TAABB& child0 = child[0]; + child0.Set(ptMin, ptMax); + TAABB& child1 = child[1]; + child1.Set(ptMin, ptMax); + child0.ptMax[a] = child1.ptMin[a] = d; + return 2; + } + return 0; +} // SplitBy(axis) +/*----------------------------------------------------------------*/ + +// split this TAABB by the given point +// returns number of children (0 - numScalar) +// idxFirst returns the index of the first aabb +template +unsigned TAABB::SplitBy(const POINT& pt, TAABB child[numChildren], unsigned& idxFirst) const +{ + idxFirst = 0; + unsigned size; + int a = 0; + do { + size = SplitBy(pt[a], a, child); + if (++a == DIMS) + return size; + } while (size == 0); + do { + unsigned n = 0; + for (unsigned b=0; b=n) return(0.0); + if (n-k +static inline void makelogcombi_n(size_t n, std::vector& l) +{ + l.resize(n+1); + for (size_t k = 0; k <= n; ++k) + l[k] = static_cast(logcombi(k,n)); +} + +/// tabulate logcombi(k,.) +template +static inline void makelogcombi_k(size_t nmax, std::vector& l) +{ + l.resize(nmax+1); + for (size_t n = 0; n <= nmax; ++n) + l[n] = static_cast(logcombi(k,n)); +} + +/// Distance and associated index +struct ErrorIndex { + double first; + size_t second; + inline ErrorIndex() {} + inline ErrorIndex(double _first, size_t _second) : first(_first), second(_second) {} + inline bool operator==(const ErrorIndex& r) const { return (first == r.first); } + inline bool operator <(const ErrorIndex& r) const { return (first < r.first); } +}; +#ifdef ACRANSAC_STD_VECTOR +typedef std::vector ErrorIndexArr; +#else +typedef CLISTDEF0(ErrorIndex) ErrorIndexArr; +#endif + +/// Find best NFA and its index wrt square error threshold in e. +template +static ErrorIndex bestNFA( + double logalpha0, + const ErrorIndexArr& e, + double loge0, + double maxThresholdSq, + const std::vector& logc_n, + const std::vector& logc_k, + double multError = 1.0) +{ + ErrorIndex bestIndex(DBL_MAX, NSAMPLES); + const size_t n = e.size(); + for (size_t k=NSAMPLES+1; k<=n && e[k-1].first<=maxThresholdSq; ++k) { + const double logalpha = logalpha0 + multError * log10(e[k-1].first+FLT_EPSILON); + const double NFA = loge0 + + logalpha * (double)(k-NSAMPLES) + + logc_n[k] + + logc_k[k]; + if (NFA < bestIndex.first) + bestIndex = ErrorIndex(NFA, k); + } + return bestIndex; +} + +/// Pick a random subset of the integers [0, total), in random order. +/// Note that this can behave badly if num_samples is close to total; runtime +/// could be unlimited! +/// +/// This uses a quadratic rejection strategy and should only be used for small +/// num_samples. +/// +/// \param NSAMPLES The number of samples to produce. +/// \param n The number of samples available. +/// \param samples num_samples of numbers in [0, total_samples) is placed +/// here on return. +struct UniformSampler { + template + inline void Sample( + size_t n, + size_t* samples) + { + ASSERT(NSAMPLES > 0); + size_t* const samplesBegin = samples; + size_t* const samplesEnd = samples + NSAMPLES; + *samples = size_t(RAND() % n); + while (++samples < samplesEnd) { + do { + *samples = size_t(RAND() % n); + } while (std::find(samplesBegin, samples, *samples) != samples); + } + } +}; +/// Same as above, but ensure that the first point is always included +struct UniformSamplerLockFirst { + template + inline void Sample( + size_t n, + size_t* samples) + { + ASSERT(NSAMPLES > 1); + size_t* const samplesBegin = samples; + size_t* const samplesEnd = samples + NSAMPLES; + *samples++ = 0; + do { + do { + *samples = size_t(RAND() % n); + } while (std::find(samplesBegin, samples, *samples) != samples); + } while (++samples < samplesEnd); + } +}; + +/// Pick a random sample: +/// get a (sorted) random sample of size NSAMPLES in [0:n-1] +/// \param NSAMPLES The number of samples to produce. +/// \param vec_index The possible data indices. +/// \param n The number of samples available. +/// \param samples The random sample of NSAMPLES indices (output). +struct UniformSamplerSorted { + template + inline void Sample( + #ifdef ACRANSAC_SAMPLE_INLIERS + const std::vector& vec_index, + #else + size_t n, + #endif + size_t* samples) + { + #ifdef ACRANSAC_SAMPLE_INLIERS + const size_t n = vec_index.size(); + #endif + for (size_t i=0; i>3)%(n-i), j; + for (j=0; j=samples[j]; ++j) + ++r; + size_t j0 = j; + for (j=i; j>j0; --j) + samples[j] = samples[j-1]; + samples[j0] = r; + } + #ifdef ACRANSAC_SAMPLE_INLIERS + for (size_t i=0; i + inline void Sample( + size_t n, + size_t* samples) + { + ASSERT(NSAMPLES > 1); + samples[0] = 0; + for (size_t i=1; i>3)%(n-i), j; + for (j=0; j=samples[j]; ++j) + ++r; + size_t j0 = j; + for (j=i; j>j0; --j) + samples[j] = samples[j-1]; + samples[j0] = r; + } + } +}; + +/// Returns the requested matrix rows +template +TMat ExtractRows(const TMat &A, const TRows &rows) { + TMat compressed((int)rows.size(), A.cols); + for (size_t i=0; i(rows.size()); ++i) + A.row(rows[i]).copyTo(compressed.row((int)i)); + return compressed; +} + +/// ACRANSAC routine (ErrorThreshold, NFA) +template +std::pair ACRANSAC( + Kernel& kernel, + Sampler& sampler, + std::vector& vec_inliers, + typename Kernel::Model& model, + double maxThreshold = DBL_MAX, + #ifndef ACRANSAC_SAMPLE_INLIERS + double confidence = 0.9999, + #endif + size_t nIter = 0) +{ + const size_t nData = kernel.NumSamples(); + if (nData < Kernel::MINIMUM_SAMPLES) { + vec_inliers.clear(); + return std::make_pair(0.0,0.0); + } + const double maxThresholdSq = SQUARE(maxThreshold); + std::vector vec_sample(Kernel::MINIMUM_SAMPLES); // sample indices + typename Kernel::Models vec_models; // up to max_models solutions + if (nData == Kernel::MINIMUM_SAMPLES) { + vec_inliers.resize(Kernel::MINIMUM_SAMPLES); + std::iota(vec_inliers.begin(), vec_inliers.end(), 0); + if (!kernel.Fit(vec_inliers, vec_models) || vec_models.size() != 1) { + vec_inliers.clear(); + return std::make_pair(0.0,0.0); + } + model = vec_models[0]; + return std::make_pair(maxThresholdSq,0.0); + } + + #ifdef ACRANSAC_SAMPLE_INLIERS + // Possible sampling indices (could change in the optimization phase) + std::vector vec_index(nData); + for (size_t i = 0; i < nData; ++i) + vec_index[i] = i; + #endif + + // Precompute log combi + const double loge0 = log10((double)Kernel::MAX_MODELS * (nData-Kernel::MINIMUM_SAMPLES)); + std::vector vec_logc_n, vec_logc_k; + makelogcombi_n(nData, vec_logc_n); + makelogcombi_k(nData, vec_logc_k); + + // Output parameters + double minNFA = DBL_MAX; + double errorSqMax = DBL_MAX; + + // Reserve 10% of iterations for focused sampling + if (nIter == 0) + nIter = MINF((size_t)4000, MAXF((size_t)300, nData*3/2)); + const size_t nMinIter = nIter*8/100; + + // Main estimation loop + ErrorIndexArr vec_residuals(nData); // [residual,index] + for (size_t iter=0; iter( + #ifdef ACRANSAC_SAMPLE_INLIERS + vec_index, + #else + nData, + #endif + &vec_sample[0] + ); + + if (!kernel.Fit(vec_sample, vec_models)) + continue; + + // Evaluate models + #ifdef ACRANSAC_SAMPLE_INLIERS + bool better = false; + #else + const size_t nTrialsRound = nIter; + #endif + for (typename Kernel::Models::const_iterator itModel=vec_models.cbegin(); itModel!=vec_models.cend(); ++itModel) { + // Residuals computation and ordering + kernel.EvaluateModel(*itModel); + for (size_t i = 0; i < nData; ++i) + vec_residuals[i] = ErrorIndex(kernel.Error(i), i); + std::sort(vec_residuals.begin(), vec_residuals.end()); + + // Most meaningful discrimination inliers/outliers + ErrorIndex best = bestNFA( + kernel.logalpha0(), + vec_residuals, + loge0, + maxThresholdSq, + vec_logc_n, + vec_logc_k, + kernel.multError()); + + if (best.first < minNFA /*&& vec_residuals[best.second-1].first < errorSqMax*/) { + // A better model was found + minNFA = best.first; + vec_inliers.resize(best.second); + for (size_t i=0; inData/3) { + // Optimization: draw samples among best set of inliers so far + vec_index = vec_inliers; + } + #endif + } + + if (minNFA >= 0) + vec_inliers.clear(); + + return std::make_pair(errorSqMax, minNFA); +} +/*----------------------------------------------------------------*/ + + +/// RANSAC routine (standard robust fitter, based on a known threshold) +template +void RANSAC( + Kernel& kernel, + Sampler& sampler, + std::vector& vec_inliers, + typename Kernel::Model& model, + double threshold, + #ifndef ACRANSAC_SAMPLE_INLIERS + double confidence = 0.9999, + #endif + size_t nIter = 0) +{ + vec_inliers.clear(); + const size_t nData = kernel.NumSamples(); + if (nData < Kernel::MINIMUM_SAMPLES) + return; + const double thresholdSq = SQUARE(threshold); + std::vector vec_sample(Kernel::MINIMUM_SAMPLES); // sample indices + typename Kernel::Models vec_models; // up to max_models solutions + + #ifdef ACRANSAC_SAMPLE_INLIERS + // Possible sampling indices (could change in the optimization phase) + std::vector vec_index(nData); + for (size_t i = 0; i < nData; ++i) + vec_index[i] = i; + #endif + + // Reserve 10% of iterations for focused sampling + if (nIter == 0) + nIter = MINF((size_t)4000, MAXF((size_t)300, nData*3/2)); + const size_t nMinIter = nIter*8/100; + + // Main estimation loop + #ifdef _USE_MSAC + double best_score = thresholdSq*nData; + #endif + std::vector vec_inliers_tmp; vec_inliers_tmp.reserve(nData); vec_inliers.reserve(nData); + for (size_t iter=0; iter( + #ifdef ACRANSAC_SAMPLE_INLIERS + vec_index, + #else + nData, + #endif + &vec_sample[0] + ); + + if (!kernel.Fit(vec_sample, vec_models)) + continue; + + // Evaluate models + #ifdef ACRANSAC_SAMPLE_INLIERS + bool better = false; + #else + const size_t nTrialsRound = nIter; + #endif + for (typename Kernel::Models::const_iterator itModel=vec_models.cbegin(); itModel!=vec_models.cend(); ++itModel) { + // Residuals computation and ordering + #ifdef _USE_MSAC + double score = 0; + #endif + vec_inliers_tmp.clear(); + kernel.EvaluateModel(*itModel); + for (size_t i = 0; i < nData; ++i) { + const double errSq = kernel.Error(i); + if (errSq <= thresholdSq) { + #ifdef _USE_MSAC + score += errSq; + #endif + vec_inliers_tmp.push_back(i); + } else { + #ifdef _USE_MSAC + score += thresholdSq; + #endif + } + #ifdef _USE_MSAC + if (score >= best_score) + break; + #endif + } + // Store best model + #ifdef _USE_MSAC + if (score < best_score) { + #else + if (vec_inliers_tmp.size() > vec_inliers.size()) { + #endif + // A better model was found + model = *itModel; + vec_inliers.swap(vec_inliers_tmp); + if (vec_inliers.size() == nData) { + nIter = 0; // force loop exit + break; + } + #ifdef _USE_MSAC + best_score = score; + DEBUG_LEVEL(4, "\titer=%3u inliers=%u score=%g", iter, vec_inliers.size(), best_score); + #else + DEBUG_LEVEL(4, "\titer=%3u inliers=%u", iter, vec_inliers.size()); + #endif + #ifdef ACRANSAC_SAMPLE_INLIERS + better = true; + #else + #ifdef _USE_MSAC + nIter = cvRANSACUpdateNumIters(confidence, (double)score/((double)nData*thresholdSq), unsigned(Kernel::MINIMUM_SAMPLES), (unsigned)nTrialsRound); + #else + nIter = cvRANSACUpdateNumIters(confidence, (double)(nData-vec_inliers.size())/nData, unsigned(Kernel::MINIMUM_SAMPLES), (unsigned)nTrialsRound); + #endif + #endif + } + } + vec_models.clear(); + + #ifdef ACRANSAC_SAMPLE_INLIERS + if (better && vec_inliers.size()>nData/3) { + // Optimization: draw samples among best set of inliers so far + vec_index = vec_inliers; + } + #endif + } + + if (vec_inliers.size() < Kernel::MINIMUM_SAMPLES) + vec_inliers.clear(); +} +/*----------------------------------------------------------------*/ + + +/// Two view Kernel adaptator for the A contrario model estimator +/// Handle data normalization and compute the corresponding logalpha 0 +/// that depends of the error model (point to line, or point to point) +/// This kernel adaptor is working for affine, homography, fundamental matrix +/// estimation. +template +class ACKernelAdaptor +{ +public: + typedef SOLVER Solver; + typedef typename SOLVER::Model Model; + typedef typename SOLVER::Models Models; + + enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; + enum { MAX_MODELS = Solver::MAX_MODELS }; + + ACKernelAdaptor( + const DMatrix32F &x1, float w1, float h1, + const DMatrix32F &x2, float w2, float h2, + bool bPointToLine = true) + : x1_(x1), x2_(x2), + bPointToLine_(bPointToLine) + { + ASSERT(2 == x1_.cols); + ASSERT(x1_.rows == x2_.rows); + ASSERT(x1_.cols == x2_.cols); + ASSERT(NumSamples() >= MINIMUM_SAMPLES); + + // LogAlpha0 is used to make error data scale invariant + const float s2 = 1.0f; + if (bPointToLine) { + // Ratio of containing diagonal image rectangle over image area + const float D = sqrt(w2*w2 + h2*h2); // diameter + const float A = w2*h2; // area + logalpha0_ = log10(2.0f*D/A/s2); + } + else { + // ratio of area : unit circle over image area + logalpha0_ = log10(M_PI/(w2*h2)/(s2*s2)); + } + } + + inline bool Fit(const std::vector& samples, Models& models) const { + const DMatrix32F x1(ExtractRows(x1_, samples)); + if (CheckCollinearity(x1.ptr(), x1.rows)) + return false; + const DMatrix32F x2(ExtractRows(x2_, samples)); + if (CheckCollinearity(x2.ptr(), x2.rows)) + return false; + Solver::Solve(x1, x2, models); + return true; + } + + inline void EvaluateModel(const Model& model) { + model2evaluate = model; + } + + inline double Error(size_t sample) const { + return Solver::Error(model2evaluate, *x1_.ptr((int)sample), *x2_.ptr((int)sample)); + } + + inline size_t NumSamples() const { return static_cast(x1_.rows); } + inline double logalpha0() const { return logalpha0_; } + inline double multError() const { return (bPointToLine_ ? 0.5 : 1.0); } + +protected: + DMatrix32F x1_, x2_; // Normalized input data + double logalpha0_; // Alpha0 is used to make the error adaptive to the image size + bool bPointToLine_;// Store if error model is pointToLine or point to point + Model model2evaluate; // current model to be evaluated +}; + +/// Pose/Resection Kernel adaptator for the A contrario model estimator +template +class ACKernelAdaptorResection +{ +public: + typedef SOLVER Solver; + typedef typename SOLVER::Model Model; + typedef typename SOLVER::Models Models; + + enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; + enum { MAX_MODELS = Solver::MAX_MODELS }; + + ACKernelAdaptorResection(const DMatrix32F &x2d, const DMatrix &x3D) + : x2d_(x2d), x3D_(x3D), + logalpha0_(log10(M_PI)) + { + ASSERT(2 == x2d_.cols); + ASSERT(3 == x3D_.cols); + ASSERT(x2d_.rows == x3D_.rows); + ASSERT(NumSamples() >= MINIMUM_SAMPLES); + } + + inline bool Fit(const std::vector& samples, Models& models) const { + const DMatrix32F x1(ExtractRows(x2d_, samples)); + if (CheckCollinearity(x1.ptr(), x1.rows)) + return false; + const DMatrix x2(ExtractRows(x3D_, samples)); + Solver::Solve(x1, x2, models); + return true; + } + + inline void EvaluateModel(const Model &model) { + model2evaluate = model; + } + + inline double Error(size_t sample) const { + return Solver::Error(model2evaluate, *x2d_.ptr((int)sample), *x3D_.ptr((int)sample)); + } + + inline size_t NumSamples() const { return x2d_.rows; } + inline double logalpha0() const { return logalpha0_; } + inline double multError() const { return 1.0; } // point to point error + +protected: + DMatrix32F x2d_; + DMatrix x3D_; + double logalpha0_; // Alpha0 is used to make the error adaptive to the image size + Model model2evaluate; // current model to be evaluated +}; + +/// Essential matrix Kernel adaptator for the A contrario model estimator +template +class ACKernelAdaptorEssential +{ +public: + typedef SOLVER Solver; + typedef typename SOLVER::Model Model; + typedef typename SOLVER::Models Models; + + enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; + enum { MAX_MODELS = Solver::MAX_MODELS }; + + ACKernelAdaptorEssential( + const DMatrix32F &x1, float w1, float h1, const Matrix3x3f& invK1, + const DMatrix32F &x2, float w2, float h2, const Matrix3x3f& invK2) + : solver(invK1, invK2), + x1_(x1), x2_(x2) + { + ASSERT(2 == x1_.cols); + ASSERT(x1_.rows == x2_.rows); + ASSERT(x1_.cols == x2_.cols); + ASSERT(NumSamples() >= MINIMUM_SAMPLES); + + //Point to line probability (line is the epipolar line) + const float D = sqrt(w2*w2 + h2*h2); // diameter + const float A = w2*h2+1.f; // area + logalpha0_ = log10(2.0f*D/A*0.5f); + } + + inline bool Fit(const std::vector& samples, Models& models) const { + const DMatrix32F x1(ExtractRows(x1_, samples)); + if (CheckCollinearity(x1.ptr(), x1.rows)) + return false; + const DMatrix32F x2(ExtractRows(x2_, samples)); + if (CheckCollinearity(x2.ptr(), x2.rows)) + return false; + solver.Solve(x1, x2, models); + return true; + } + + inline void EvaluateModel(const Model &model) { + solver.EvaluateModel(model); + } + + inline double Error(size_t sample) const { + return solver.Error(*x1_.ptr((int)sample), *x2_.ptr((int)sample)); + } + + inline size_t NumSamples() const { return x1_.rows; } + inline double logalpha0() const { return logalpha0_; } + inline double multError() const { return 0.5; } // point to line error + +protected: + Solver solver; + DMatrix32F x1_, x2_; // image point and camera plane point. + double logalpha0_; // Alpha0 is used to make the error adaptive to the image size +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_AUTO_ESTIMATOR_H__ diff --git a/libs/Common/AutoPtr.h b/libs/Common/AutoPtr.h new file mode 100644 index 0000000..375cca4 --- /dev/null +++ b/libs/Common/AutoPtr.h @@ -0,0 +1,275 @@ +//////////////////////////////////////////////////////////////////// +// AutoPtr.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_AUTOPTR_H__ +#define __SEACAVE_AUTOPTR_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +/************************************************************************************** + * CAutoPtr template + * --------------- + * simple smart pointer + **************************************************************************************/ + +struct AutoPtrMoveCopy { + template + static inline void Copy(TYPE*& ptrLeft, TYPE*& ptrRight) { + ptrLeft = ptrRight; + ptrRight = NULL; + } +}; + +struct AutoPtrDeepCopy { + template + static inline void Copy(TYPE*& ptrLeft, TYPE*& ptrRight) { + ptrLeft = (ptrRight != NULL ? new TYPE(*ptrRight) : (TYPE*)NULL); + } +}; + +template +class CAutoPtr +{ +private: + typedef TYPE Type; + typedef TYPE* TypePtr; + +public: + inline CAutoPtr() : m_pointer(NULL) + { // construct with NULL pointer + } + + inline explicit CAutoPtr(TypePtr _Ptr) : m_pointer(_Ptr) + { // construct from object pointer + } + + inline CAutoPtr(CAutoPtr& _Right) + { // copy-construct by assuming pointer from _Right CAutoPtr + CopyPolicy::Copy(m_pointer, _Right.m_pointer); + } + + inline ~CAutoPtr() + { // destroy the object + delete m_pointer; + } + + void Swap(CAutoPtr& _Right) + { // swap compatible _Right (assume pointer) + const TypePtr tmp(m_pointer); + m_pointer = _Right.m_pointer; + _Right.m_pointer = tmp; + } + + CAutoPtr& operator=(CAutoPtr& _Right) + { // assign compatible _Right (assume pointer) + if (this != &_Right) { + delete m_pointer; + CopyPolicy::Copy(m_pointer, _Right.m_pointer); + } + return (*this); + } + + CAutoPtr& operator=(TypePtr _Ptr) + { // assign compatible _Right (assume pointer) + if (m_pointer != _Ptr) { + delete m_pointer; + m_pointer = _Ptr; + } + return (*this); + } + + inline Type& operator*() const + { // return designated value + ASSERT(m_pointer); + return (*m_pointer); + } + + inline Type* operator->() const + { // return pointer to class object + ASSERT(m_pointer); + return m_pointer; + } + + inline operator TypePtr() const + { // return pointer to class object + return m_pointer; + } + + inline operator TypePtr&() + { // return reference to class object + return m_pointer; + } + + inline bool operator==(const TypePtr _Right) const + { // return pointer to class object + return (m_pointer == _Right); + } + + inline bool operator!=(const TypePtr _Right) const + { // return pointer to class object + return (m_pointer != _Right); + } + + inline void Release() + { // release pointer + delete m_pointer; + m_pointer = NULL; + } + + inline void Reset(TypePtr _Ptr = NULL) + { // reset pointer + m_pointer = _Ptr; + } + +protected: + TypePtr m_pointer; // the wrapped object pointer + + #ifdef _USE_BOOST + // implement BOOST serialization + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int /*version*/) const + { + ar & m_pointer; + } + template + void load(Archive& ar, const unsigned int /*version*/) + { + TypePtr newPointer; + ar & newPointer; + operator=(newPointer); + } + BOOST_SERIALIZATION_SPLIT_MEMBER() + #endif +}; + + +template +class CAutoPtrArr +{ +private: + typedef TYPE Type; + typedef TYPE* TypePtr; + +public: + inline CAutoPtrArr() : m_pointer(NULL) + { // construct with NULL pointer + } + + inline explicit CAutoPtrArr(TypePtr _Ptr) : m_pointer(_Ptr) + { // construct from object pointer + } + + inline CAutoPtrArr(CAutoPtrArr& _Right) : m_pointer(_Right.m_pointer) + { // construct by assuming pointer from _Right CAutoPtrArr + _Right.m_pointer = NULL; + } + + inline ~CAutoPtrArr() + { // destroy the object + delete[] m_pointer; + } + + CAutoPtrArr& operator=(CAutoPtrArr& _Right) + { // assign compatible _Right (assume pointer) + if (this != &_Right) + { + delete[] m_pointer; + m_pointer = _Right.m_pointer; + _Right.m_pointer = NULL; + } + return (*this); + } + + CAutoPtrArr& operator=(TypePtr _Ptr) + { // assign compatible _Right (assume pointer) + if (m_pointer != _Ptr) + { + delete[] m_pointer; + m_pointer = _Ptr; + } + return (*this); + } + + inline Type& operator*() const + { // return designated value + ASSERT(m_pointer); + return (*m_pointer); + } + + inline Type* operator->() const + { // return pointer to class object + ASSERT(m_pointer); + return m_pointer; + } + + inline operator TypePtr() const + { // return pointer to class object + return m_pointer; + } + + inline operator TypePtr&() + { // return reference to class object + return m_pointer; + } + + inline bool operator==(const TypePtr _Right) const + { // return pointer to class object + return (m_pointer == _Right); + } + + inline bool operator!=(const TypePtr _Right) const + { // return pointer to class object + return (m_pointer != _Right); + } + + inline void Release() + { // release pointer + delete[] m_pointer; + m_pointer = NULL; + } + + inline void Reset(TypePtr _Ptr = NULL) + { // reset pointer + m_pointer = _Ptr; + } + +protected: + TypePtr m_pointer; // the wrapped object pointer + + #ifdef _USE_BOOST + // implement BOOST serialization + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int /*version*/) const + { + ar & m_pointer; + } + template + void load(Archive& ar, const unsigned int /*version*/) + { + TypePtr newPointer; + ar & newPointer; + operator=(newPointer); + } + BOOST_SERIALIZATION_SPLIT_MEMBER() + #endif +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_AUTOPTR_H__ diff --git a/libs/Common/CMakeLists.txt b/libs/Common/CMakeLists.txt new file mode 100644 index 0000000..18899af --- /dev/null +++ b/libs/Common/CMakeLists.txt @@ -0,0 +1,25 @@ +# List sources files +FILE(GLOB LIBRARY_FILES_C "*.cpp") +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_library_with_type(Common "Libs" "" "${cxx_default}" + ${LIBRARY_FILES_C} ${LIBRARY_FILES_H} +) + +# Manually set Common.h as the precompiled header +IF(CMAKE_VERSION VERSION_GREATER_EQUAL 3.16.0) + TARGET_PRECOMPILE_HEADERS(Common PRIVATE "Common.h") +endif() + +# Link its dependencies +TARGET_LINK_LIBRARIES(Common ${Boost_LIBRARIES} ${OpenCV_LIBS}) + +# Install +SET_TARGET_PROPERTIES(Common PROPERTIES + PUBLIC_HEADER "${LIBRARY_FILES_H}") +INSTALL(TARGETS Common + EXPORT OpenMVSTargets + LIBRARY DESTINATION "${INSTALL_LIB_DIR}" + ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" + PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/Common") diff --git a/libs/Common/Common.cpp b/libs/Common/Common.cpp new file mode 100644 index 0000000..54ccc8a --- /dev/null +++ b/libs/Common/Common.cpp @@ -0,0 +1,47 @@ +//////////////////////////////////////////////////////////////////// +// Common.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +// Source file that includes just the standard includes +// Common.pch will be the pre-compiled header +// Common.obj will contain the pre-compiled type information + +#include "Common.h" + +namespace SEACAVE { +#if TD_VERBOSE == TD_VERBOSE_ON +int g_nVerbosityLevel(2); +#endif +#if TD_VERBOSE == TD_VERBOSE_DEBUG +int g_nVerbosityLevel(3); +#endif + +String g_strWorkingFolder; +String g_strWorkingFolderFull; +} // namespace SEACAVE + +#ifdef _USE_BOOST +#ifdef BOOST_NO_EXCEPTIONS +#if (BOOST_VERSION / 100000) > 1 || (BOOST_VERSION / 100 % 1000) > 72 +#include +#endif +namespace boost { + void throw_exception(std::exception const & e) { + VERBOSE("exception thrown: %s", e.what()); + ASSERT("boost exception thrown" == NULL); + exit(EXIT_FAILURE); + } + #if (BOOST_VERSION / 100000) > 1 || (BOOST_VERSION / 100 % 1000) > 72 + void throw_exception(std::exception const & e, boost::source_location const & loc) { + std::ostringstream ostr; ostr << loc; + VERBOSE("exception thrown at %s: %s", ostr.str().c_str(), e.what()); + ASSERT("boost exception thrown" == NULL); + exit(EXIT_FAILURE); + } + #endif +} // namespace boost +#endif +#endif diff --git a/libs/Common/Common.h b/libs/Common/Common.h new file mode 100644 index 0000000..eba19ae --- /dev/null +++ b/libs/Common/Common.h @@ -0,0 +1,308 @@ +//////////////////////////////////////////////////////////////////// +// Common.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef _COMMON_COMMON_H_ +#define _COMMON_COMMON_H_ + + +// D E F I N E S /////////////////////////////////////////////////// + +#include "Config.h" + +// macros controlling the verbosity +#define TD_VERBOSE_OFF 0 +#define TD_VERBOSE_ON 1 +#define TD_VERBOSE_DEBUG 2 +#ifndef TD_VERBOSE +#ifdef _RELEASE +#define TD_VERBOSE TD_VERBOSE_ON +#else +#define TD_VERBOSE TD_VERBOSE_DEBUG +#endif +#endif + +#if TD_VERBOSE == TD_VERBOSE_OFF +#define VERBOSE LOG +#define DEBUG_LEVEL(n,...) +#else +#ifndef VERBOSITY_LEVEL +namespace SEACAVE { extern int g_nVerbosityLevel; } +#define VERBOSITY_LEVEL g_nVerbosityLevel +#endif +#define VERBOSE LOG +#define DEBUG_LEVEL(n,...) { if (n < VERBOSITY_LEVEL) VERBOSE(__VA_ARGS__); } +#endif +#define DEBUG(...) DEBUG_LEVEL(0, __VA_ARGS__) +#define DEBUG_EXTRA(...) DEBUG_LEVEL(1, __VA_ARGS__) +#define DEBUG_ULTIMATE(...) DEBUG_LEVEL(2, __VA_ARGS__) + + +// macros that simplify timing tasks +#define TD_TIMER_OFF 0 +#define TD_TIMER_ON 1 +#ifndef TD_TIMER +#define TD_TIMER TD_TIMER_ON +#endif + +#if TD_TIMER == TD_TIMER_OFF +#define TD_TIMER_START() +#define TD_TIMER_UPDATE() +#define TD_TIMER_GET() 0 +#define TD_TIMER_GET_INT() 0 +#define TD_TIMER_GET_FMT() String() +#define TD_TIMER_STARTD() +#define TD_TIMER_UPDATED() +#endif +#if TD_TIMER == TD_TIMER_ON +#define TD_TIMER_START() TIMER_START() +#define TD_TIMER_UPDATE() TIMER_UPDATE() +#define TD_TIMER_GET() TIMER_GET() +#define TD_TIMER_GET_INT() TIMER_GET_INT() +#define TD_TIMER_GET_FMT() TIMER_GET_FORMAT() +#if TD_VERBOSE == TD_VERBOSE_OFF +#define TD_TIMER_STARTD() +#define TD_TIMER_UPDATED() +#else +#define TD_TIMER_STARTD() TIMER_START() +#define TD_TIMER_UPDATED() TIMER_UPDATE() +#endif +#endif + + +// macros redirecting standard streams to the log +#define LOG_OUT() GET_LOG() //or std::cout +#define LOG_ERR() GET_LOG() //or std::cerr + + +// macros simplifying the task of composing file paths; +// WORKING_FOLDER and WORKING_FOLDER_FULL must be defined as strings +// containing the relative/full path to the working folder +#ifndef WORKING_FOLDER +namespace SEACAVE { +class String; +extern String g_strWorkingFolder; // empty by default (current folder) +extern String g_strWorkingFolderFull; // full path to current folder +} +#define WORKING_FOLDER g_strWorkingFolder // empty by default (current folder) +#define WORKING_FOLDER_FULL g_strWorkingFolderFull // full path to current folder +#endif +#define INIT_WORKING_FOLDER {SEACAVE::Util::ensureValidFolderPath(WORKING_FOLDER); WORKING_FOLDER_FULL = SEACAVE::Util::getFullPath(WORKING_FOLDER);} // initialize working folders +#define MAKE_PATH(str) SEACAVE::Util::getSimplifiedPath(WORKING_FOLDER+SEACAVE::String(str)) // add working directory to the given file name +#define MAKE_PATH_SAFE(str) (SEACAVE::Util::isFullPath((str).c_str()) ? SEACAVE::String(str) : MAKE_PATH(str)) // add working directory to the given file name only if not full path already +#define MAKE_PATH_FULL(p,s) (SEACAVE::Util::isFullPath((s).c_str()) ? SEACAVE::String(s) : SEACAVE::Util::getSimplifiedPath((p)+(s))) // add the given path to the given file name +#define MAKE_PATH_REL(p,s) SEACAVE::Util::getRelativePath(p,s) // remove the given path from the given file name +#define GET_PATH_FULL(str) (SEACAVE::Util::isFullPath((str).c_str()) ? SEACAVE::Util::getFilePath(str) : SEACAVE::Util::getSimplifiedPath(WORKING_FOLDER_FULL+SEACAVE::Util::getFilePath(str))) // retrieve the full path to the given file + + +// macros simplifying the task of managing options +#define DECOPT_SPACE(SPACE) namespace SPACE { \ + void init(); \ + void update(); \ + extern SEACAVE::VoidArr arrFncOpt; \ + extern SEACAVE::CConfigTable oConfig; \ +} +#define DEFOPT_SPACE(SPACE, name) namespace SPACE { \ + SEACAVE::CConfigTable oConfig(name); \ + typedef LPCTSTR (*FNCINDEX)(); \ + typedef void (*FNCINIT)(SEACAVE::IDX); \ + typedef void (*FNCUPDATE)(); \ + VoidArr arrFncOpt; \ + void init() { \ + FOREACH(i, arrFncOpt) \ + ((FNCINIT)arrFncOpt[i])(i); \ + } \ + void update() { \ + FOREACH(i, arrFncOpt) \ + ((FNCUPDATE)arrFncOpt[i])(); \ + } \ +} + +#define DEFVAR_OPTION(SPACE, flags, type, name, title, desc, ...) namespace SPACE { \ + type name; \ + LPCTSTR defval_##name(NULL); \ + void update_##name() { \ + SEACAVE::String::FromString(oConfig[title].val, name); \ + } \ + void init_##name(SEACAVE::IDX idx) { \ + LPCTSTR const vals[] = {__VA_ARGS__}; \ + arrFncOpt[idx] = (void*)update_##name; \ + SMLVALUE& val = oConfig[title]; \ + CFGITEM& opt = *((CFGITEM*)val.data); \ + opt.state = flags; \ + val.val = opt.defval = (defval_##name != NULL ? defval_##name : vals[0]); \ + opt.vals.Insert(opt.defval); \ + for (size_t i=1; i Sphere2f; +typedef TSphere Sphere3f; +typedef TAABB AABB2f; +typedef TAABB AABB3f; +typedef TOBB OBB2f; +typedef TOBB OBB3f; +typedef TRay Ray2f; +typedef TRay Ray3f; +typedef TLine Line2f; +typedef TLine Line3f; +typedef TTriangle Triangle2f; +typedef TTriangle Triangle3f; +typedef TPlane Planef; +typedef TPoint2 Point2f; +typedef TPoint3 Point3f; +typedef TMatrix Vec2f; +typedef TMatrix Vec3f; +typedef TMatrix Vec4f; +typedef TMatrix Matrix2x2f; +typedef TMatrix Matrix3x3f; +typedef TMatrix Matrix3x4f; +typedef TMatrix Matrix4x4f; + +typedef TSphere Sphere2d; +typedef TSphere Sphere3d; +typedef TAABB AABB2d; +typedef TAABB AABB3d; +typedef TOBB OBB2d; +typedef TOBB OBB3d; +typedef TRay Ray2d; +typedef TRay Ray3d; +typedef TLine Line2d; +typedef TLine Line3d; +typedef TTriangle Triangle2d; +typedef TTriangle Triangle3d; +typedef TPlane Planed; +typedef TPoint2 Point2d; +typedef TPoint3 Point3d; +typedef TMatrix Vec2d; +typedef TMatrix Vec3d; +typedef TMatrix Vec4d; +typedef TMatrix Matrix2x2d; +typedef TMatrix Matrix3x3d; +typedef TMatrix Matrix3x4d; +typedef TMatrix Matrix4x4d; + +typedef TSphere Sphere2; +typedef TSphere Sphere3; +typedef TAABB AABB2; +typedef TAABB AABB3; +typedef TOBB OBB2; +typedef TOBB OBB3; +typedef TRay Ray2; +typedef TRay Ray3; +typedef TLine Line2; +typedef TLine Line3; +typedef TTriangle Triangle2; +typedef TTriangle Triangle3; +typedef TPlane Plane; +typedef TPoint2 Point2; +typedef TPoint3 Point3; +typedef TMatrix Vec2; +typedef TMatrix Vec3; +typedef TMatrix Vec4; +typedef TMatrix Matrix2x2; +typedef TMatrix Matrix3x3; +typedef TMatrix Matrix3x4; +typedef TMatrix Matrix4x4; + +typedef TQuaternion Quaternion; +typedef TRMatrixBase RMatrixBase; + +// camera matrix types +typedef Point3 CMatrix; +typedef RMatrixBase RMatrix; +typedef Matrix3x3 KMatrix; +typedef Matrix3x4 PMatrix; + +// reconstructed 3D point type +typedef Vec3 X3D; +typedef SEACAVE::cList X3DArr; + +typedef SEACAVE::cList IndexArr; + +typedef CLISTDEF0(REAL) REALArr; +typedef CLISTDEF0(Point2f) Point2fArr; +typedef CLISTDEF0(Point3f) Point3fArr; +typedef CLISTDEF0(Point2d) Point2dArr; +typedef CLISTDEF0(Point3d) Point3dArr; +typedef CLISTDEF0(Point2) Point2Arr; +typedef CLISTDEF0(Point3) Point3Arr; +typedef CLISTDEF0(Vec4) Vec4Arr; +typedef CLISTDEF0(Matrix3x3) Matrix3x3Arr; +typedef CLISTDEF0(Matrix3x4) Matrix3x4Arr; +typedef CLISTDEF0(CMatrix) CMatrixArr; +typedef CLISTDEF0(RMatrix) RMatrixArr; +typedef CLISTDEF0(KMatrix) KMatrixArr; +typedef CLISTDEF0(PMatrix) PMatrixArr; +typedef CLISTDEF0(ImageRef) ImageRefArr; +typedef CLISTDEF0(Pixel8U) Pixel8UArr; +typedef CLISTDEF0(Pixel32F) Pixel32FArr; +typedef CLISTDEF0(Color8U) Color8UArr; +typedef CLISTDEF0(Color32F) Color32FArr; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +// define specialized cv:DataType<> +DEFINE_CVDATATYPE(SEACAVE::Vec2f) +DEFINE_CVDATATYPE(SEACAVE::Vec3f) +DEFINE_CVDATATYPE(SEACAVE::Vec4f) +DEFINE_CVDATATYPE(SEACAVE::Matrix2x2f) +DEFINE_CVDATATYPE(SEACAVE::Matrix3x3f) +DEFINE_CVDATATYPE(SEACAVE::Matrix3x4f) +DEFINE_CVDATATYPE(SEACAVE::Matrix4x4f) + +DEFINE_CVDATATYPE(SEACAVE::Vec2d) +DEFINE_CVDATATYPE(SEACAVE::Vec3d) +DEFINE_CVDATATYPE(SEACAVE::Vec4d) +DEFINE_CVDATATYPE(SEACAVE::Matrix2x2d) +DEFINE_CVDATATYPE(SEACAVE::Matrix3x3d) +DEFINE_CVDATATYPE(SEACAVE::Matrix3x4d) +DEFINE_CVDATATYPE(SEACAVE::Matrix4x4d) +/*----------------------------------------------------------------*/ + +#endif // _COMMON_COMMON_H_ diff --git a/libs/Common/Config.h b/libs/Common/Config.h new file mode 100644 index 0000000..d74b5c8 --- /dev/null +++ b/libs/Common/Config.h @@ -0,0 +1,281 @@ +//////////////////////////////////////////////////////////////////// +// Config.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_CONFIG_H__ +#define __SEACAVE_CONFIG_H__ + +// Configure everything that needs to be globally known + +#include "ConfigLocal.h" + + +// D E F I N E S /////////////////////////////////////////////////// + +#ifdef _MSC_VER + +// Modify the following defines if you have to target a platform prior to the ones specified below. +// Refer to MSDN for the latest info on corresponding values for different platforms. +#if _MSC_VER > 1400 +#ifndef NTDDI_VERSION // There's now just one symbol to specify the minimum target operating system. +#define NTDDI_VERSION NTDDI_WIN7 // All the other symbols are set automatically to the appropriate values for the target operating system. +#endif +#else +#ifndef WINVER // Allow use of features specific to Windows 95 and Windows NT 4 or later. +#define WINVER 0x0500 // Change this to the appropriate value to target Windows 98 and Windows 2000 or later. +#endif +#ifndef _WIN32_WINNT // Allow use of features specific to Windows NT 4 or later. +#define _WIN32_WINNT 0x0500 // Change this to the appropriate value to target Windows 98 and Windows 2000 or later. +#endif +#ifndef _WIN32_WINDOWS // Allow use of features specific to Windows 98 or later. +#define _WIN32_WINDOWS 0x0410 // Change this to the appropriate value to target Windows Me or later. +#endif +#ifndef _WIN32_IE // Allow use of features specific to IE 4.0 or later. +#define _WIN32_IE 0x0501 // Change this to the appropriate value to target IE 5.0 or later. +#endif +#endif + +#ifndef VC_EXTRALEAN +#define VC_EXTRALEAN // Exclude rarely-used stuff from Windows headers +#endif + +#ifndef STRICT +#define STRICT +#endif + +#define WIN32_MEAN_AND_LEAN + +#define NOMINMAX + +#define _USE_MATH_DEFINES + +#if _MSC_VER >= 1400 +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS 1 +#endif +#ifndef _CRT_SECURE_NO_DEPRECATE +#define _CRT_SECURE_NO_DEPRECATE 1 +#endif +#ifndef _ATL_SECURE_NO_DEPRECATE +#define _ATL_SECURE_NO_DEPRECATE 1 +#endif +#ifndef _CRT_NON_CONFORMING_SWPRINTFS +#define _CRT_NON_CONFORMING_SWPRINTFS 1 +#endif +#ifndef _CRT_SECURE_CPP_OVERLOAD_SECURE_NAMES +#define _CRT_SECURE_CPP_OVERLOAD_SECURE_NAMES 0 // disable automatically overloading CPP names to secure versions +#endif +#if 0 && defined(_DEBUG) && !defined(_ITERATOR_DEBUG_LEVEL) // might not build if linking statically to 3rd party libraries +#define _ITERATOR_DEBUG_LEVEL 1 // disable std iterator debugging even in Debug, as it is very slow +#endif +#endif + + +//---------------------------------------------------------------------- +// For Microsoft Visual C++, externally accessible symbols must be +// explicitly indicated with DLL_API. +// +// The following ifdef block is the standard way of creating macros +// which make exporting from a DLL simpler. All files within this DLL +// are compiled with the DLL_EXPORTS preprocessor symbol defined on the +// command line. In contrast, projects that use (or import) the DLL +// objects do not define the DLL_EXPORTS symbol. This way any other +// project whose source files include this file see DLL_API functions as +// being imported from a DLL, whereas this DLL sees symbols defined with +// this macro as being exported. +//---------------------------------------------------------------------- +#define EXPORT_API __declspec(dllexport) +#define IMPORT_API __declspec(dllimport) +/*----------------------------------------------------------------*/ +#ifdef _USRDLL + #ifdef Common_EXPORTS + #define GENERAL_API EXPORT_API + #define GENERAL_TPL + #else + #define GENERAL_API IMPORT_API + #define GENERAL_TPL extern + #endif +#else + #define GENERAL_API + #define GENERAL_TPL +#endif +/*----------------------------------------------------------------*/ + +// Define platform type +#if _WIN64 +#define _ENVIRONMENT64 +#else +#define _ENVIRONMENT32 +#endif + +#else // _MSC_VER + +#if !defined(_DEBUG) && !defined(NDEBUG) +#define _DEBUG +#endif + +//---------------------------------------------------------------------- +// DLL_API is ignored for all other systems +//---------------------------------------------------------------------- +#define EXPORT_API +#define IMPORT_API +#define GENERAL_API +#define GENERAL_TPL + +// Define platform type +#if __x86_64__ || __ppc64__ +#define _ENVIRONMENT64 +#else +#define _ENVIRONMENT32 +#endif + +#endif // _MSC_VER + + +#if __cplusplus >= 201103L || (__clang_major__ >= 4 || (__clang_major__ >= 3 && __clang_minor__ >= 3)) +#define _SUPPORT_CPP11 +#endif +#if __cplusplus >= 201402L || (__clang_major__ >= 4 || (__clang_major__ >= 3 && __clang_minor__ >= 4)) +#define _SUPPORT_CPP14 +#endif +#if __cplusplus >= 201703L || __clang_major__ >= 5 +#define _SUPPORT_CPP17 +#endif +#if __cplusplus >= 202002L || __clang_major__ >= 10 +#define _SUPPORT_CPP20 +#endif + + +#if defined(__powerpc__) +#define _PLATFORM_PPC 1 +#elif defined(__arm__) || defined (__arm64__) || defined(__aarch64__) || defined(_M_ARM) || defined(_M_ARMT) +#define _PLATFORM_ARM 1 +#else +#define _PLATFORM_X86 1 +#endif + + +#if !defined(_DEBUG) && !defined(_PROFILE) +#define _RELEASE // exclude code useful only for debug +#endif + + + +//optimization flags +#if defined(_MSC_VER) +# define ALIGN(n) __declspec(align(n)) +# define NOINITVTABLE __declspec(novtable) //disable generating code to initialize the vfptr in the constructor(s) and destructor of the class +# define DECRESTRICT __declspec(restrict) //applied to a function declaration or definition that returns a pointer type and tells the compiler that the function returns an object that will not be aliased with any other pointers +# define NOALIAS __declspec(noalias) //applied to a function declaration or definition that returns a pointer type and tells the compiler that the function call does not modify or reference visible global state and only modifies the memory pointed to directly by pointer parameters (first-level indirections) +# define RESTRICT __restrict //applied to a function parameter +# define MEMALLOC __declspec(noalias) __declspec(restrict) +# define DEPRECATED __declspec(deprecated) +# define MAYBEUNUSED +# define HOT +# define COLD +# define THREADLOCAL __declspec(thread) +# define FORCEINLINE __forceinline +#elif defined(__GNUC__) +# define ALIGN(n) __attribute__((aligned(n))) +# define NOINITVTABLE +# define DECRESTRICT +# define NOALIAS +# define RESTRICT __restrict__ +# define MEMALLOC __attribute__ ((__malloc__)) +# define DEPRECATED __attribute__ ((__deprecated__)) +# define MAYBEUNUSED __attribute__ ((unused)) +# define HOT __attribute__((hot)) __attribute__((optimize("-O3"))) __attribute__((optimize("-ffast-math"))) //optimize for speed, even in debug +# define COLD __attribute__((cold)) //optimize for size +# define THREADLOCAL __thread +# define FORCEINLINE inline //__attribute__((always_inline)) +#else +# define ALIGN(n) +# define NOINITVTABLE +# define DECRESTRICT +# define NOALIAS +# define RESTRICT +# define MEMALLOC +# define DEPRECATED +# define MAYBEUNUSED +# define HOT +# define COLD +# define THREADLOCAL __thread +# define FORCEINLINE inline +#endif + +#ifndef _SUPPORT_CPP11 +# define constexpr inline +#endif +#ifdef _SUPPORT_CPP17 +# undef MAYBEUNUSED +# define MAYBEUNUSED [[maybe_unused]] +#endif + +#define SAFE_DELETE(p) { if (p!=NULL) { delete (p); (p)=NULL; } } +#define SAFE_DELETE_ARR(p) { if (p!=NULL) { delete [] (p); (p)=NULL; } } +#define SAFE_FREE(p) { if (p!=NULL) { free(p); (p)=NULL; } } +#define SAFE_RELEASE(p) { if (p!=NULL) { (p)->Release(); (p)=NULL; } } + + +#ifdef _DEBUG + +#ifdef _MSC_VER +#define _DEBUGINFO +#define _CRTDBG_MAP_ALLOC //enable this to show also the filename (DEBUG_NEW should also be defined in each file) +#include +#include +#ifdef _INC_CRTDBG +#define ASSERT(exp) {if (!(exp) && 1 == _CrtDbgReport(_CRT_ASSERT, __FILE__, __LINE__, NULL, #exp)) _CrtDbgBreak();} +#else +#define ASSERT(exp) {if (!(exp)) __debugbreak();} +#endif // _INC_CRTDBG +#define TRACE(...) {TCHAR buffer[2048]; _sntprintf(buffer, 2048, __VA_ARGS__); OutputDebugString(buffer);} +#else // _MSC_VER +#include +#define ASSERT(exp) assert(exp) +#define TRACE(...) +#endif // _MSC_VER + +#else + +#ifdef _RELEASE +#define ASSERT(exp) +#else +#ifdef _MSC_VER +#define ASSERT(exp) {if (!(exp)) __debugbreak();} +#else // _MSC_VER +#define ASSERT(exp) {if (!(exp)) __builtin_trap();} +#endif // _MSC_VER +#endif +#define TRACE(...) + +#endif // _DEBUG + +#define ASSERTM(exp, msg) ASSERT(exp) + +namespace SEACAVE_ASSERT +{ + template struct compile_time_assert; + template <> struct compile_time_assert { enum {value=1}; }; + + template struct assert_are_same_type; + template struct assert_are_same_type { enum{value=1}; }; + + template struct assert_are_not_same_type { enum{value=1}; }; + template struct assert_are_not_same_type {}; +} + +#define STATIC_ASSERT(expression) \ + MAYBEUNUSED typedef char CTA##__LINE__[::SEACAVE_ASSERT::compile_time_assert<(bool)(expression)>::value] + +#define ASSERT_ARE_SAME_TYPE(type1, type2) \ + MAYBEUNUSED typedef char AAST##__LINE__[::SEACAVE_ASSERT::assert_are_same_type::value] + +#define ASSERT_ARE_NOT_SAME_TYPE(type1, type2) \ + MAYBEUNUSED typedef char AANST##__LINE__[::SEACAVE_ASSERT::assert_are_not_same_type::value] +/*----------------------------------------------------------------*/ + +#endif // __SEACAVE_CONFIG_H__ diff --git a/libs/Common/ConfigTable.cpp b/libs/Common/ConfigTable.cpp new file mode 100644 index 0000000..9dbe28f --- /dev/null +++ b/libs/Common/ConfigTable.cpp @@ -0,0 +1,153 @@ +//////////////////////////////////////////////////////////////////// +// ConfigTable.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "ConfigTable.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +/*-----------------------------------------------------------* + * CConfigTable class implementation * + *-----------------------------------------------------------*/ + +/** + * Constructor + */ +CConfigTable::CConfigTable(const String& name) : m_oSML(name), m_pParent(NULL) +{ + m_oSML.SetFncItem(ItemInitData, ItemSaveData, ItemReleaseData); +} + +/** + * Destructor + */ +CConfigTable::~CConfigTable() +{ + if (m_pParent) + m_pParent->RemoveChild(*this); + Release(); +} +/*----------------------------------------------------------------*/ + +/** + * Released all used memory + */ +void CConfigTable::Release() +{ + m_oSML.Release(); +} +/*----------------------------------------------------------------*/ + + +/** + * Create a new child + */ +void CConfigTable::Insert(const String& name) +{ + SML* const pChildSML = new SML(name); + pChildSML->SetFncItem(ItemInitData, ItemSaveData, ItemReleaseData); + const IDX idx = m_oSML.InsertChild(pChildSML); + // if child already exists, delete created node + if (m_oSML.GetArrChildren()[idx] != pChildSML) + delete pChildSML; +} +/*----------------------------------------------------------------*/ + + +/** + * Remove an existing child + */ +void CConfigTable::Remove(const String& name) +{ + m_oSML.DestroyChild(name); +} +/*----------------------------------------------------------------*/ + + +/** + * Get the config table for the given child + */ +const SML& CConfigTable::GetConfig(const String& name) const +{ + const LPSMLARR& arrSML = m_oSML.GetArrChildren(); + const IDX idx = arrSML.FindFirst(&name, SML::CompareName); + ASSERT(idx != LPSMLARR::NO_INDEX); + return *arrSML[idx]; +} // GetConfig +SML& CConfigTable::GetConfig(const String& name) +{ + const LPSMLARR& arrSML = m_oSML.GetArrChildren(); + const IDX idx = arrSML.FindFirst(&name, SML::CompareName); + ASSERT(idx != LPSMLARR::NO_INDEX); + return *arrSML[idx]; +} // GetConfig +/*----------------------------------------------------------------*/ + + +/** + * Load the configuration from a file. + */ +bool CConfigTable::Load(const String& f) +{ + return m_oSML.Load(f); +} // Load +bool CConfigTable::Load(ISTREAM& oStream) +{ + return m_oSML.Load(oStream); +} // Load +/** + * Write to a file the values of this node and its children. + * Set to false the second parameter in order not to save the empty children. + */ +bool CConfigTable::Save(const String& f, SML::SAVEFLAG flags) const +{ + return m_oSML.Save(f, flags); +} // Save +bool CConfigTable::Save(OSTREAM& oStream, SML::SAVEFLAG flags) const +{ + return m_oSML.Save(oStream, flags); +} // Save +/*----------------------------------------------------------------*/ + + +/** + * Create and initialize a config item for the given SML entry. + */ +void STCALL CConfigTable::ItemInitData(const String& key, SMLVALUE& val, void*) +{ + CFGITEM* pItem = new CFGITEM; + pItem->name = key; + val.data = pItem; +} // ItemInitData +/*----------------------------------------------------------------*/ + +/** + * Save a config item for the given SML entry. Return false if this item should not be saved. + */ +bool STCALL CConfigTable::ItemSaveData(const SMLVALUE& val, void*) +{ + const CFGITEM& item = *((const CFGITEM*)val.data); + return !item.state.isSet(CFGITEM::TEMP); +} // ItemSaveData +/*----------------------------------------------------------------*/ + +/** + * Release and destroy the config item for the given SML entry. + */ +void STCALL CConfigTable::ItemReleaseData(SMLVALUE& val, void*) +{ + CFGITEM* pItem = (CFGITEM*)val.data; + val.data = NULL; + delete pItem; +} // ItemReleaseData +/*----------------------------------------------------------------*/ diff --git a/libs/Common/ConfigTable.h b/libs/Common/ConfigTable.h new file mode 100644 index 0000000..3375121 --- /dev/null +++ b/libs/Common/ConfigTable.h @@ -0,0 +1,82 @@ +//////////////////////////////////////////////////////////////////// +// ConfigTable.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_CONFIGTABLE_H__ +#define __SEACAVE_CONFIGTABLE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "SML.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +typedef struct CFGITEM_TYPE { + enum { + NA = 0, // no special states available + TEMP = (1 << 0), // this item lives only this instance and it will not be save + }; + Flags state; + String name; + String desc; + String defval; + StringArr vals; +} CFGITEM; + + +// P R O T O T Y P E S ///////////////////////////////////////////// + +/** + * Configuration table interface. + */ + +class GENERAL_API CConfigTable +{ +public: + CConfigTable(const String&); + ~CConfigTable(); + + void Release(); + + // main methods + void Insert(const String&); + void Remove(const String&); + const SML& GetConfig(const String&) const; + SML& GetConfig(const String&); + const SML& GetConfig() const { return m_oSML; } + SML& GetConfig() { return m_oSML; } + inline SMLVALUE& operator[] (const String& name) { return m_oSML[name]; } + inline IDX InsertChild(CConfigTable& oCfg) { oCfg.SetParent(this); return m_oSML.InsertChild(&oCfg.m_oSML); } + inline void RemoveChild(CConfigTable& oCfg) { oCfg.SetParent(NULL); m_oSML.RemoveChild(oCfg.m_oSML.GetName()); } + + // misc methods + bool Load(const String&); + bool Load(ISTREAM&); + bool Save(const String&, SML::SAVEFLAG=SML::NONE) const; + bool Save(OSTREAM&, SML::SAVEFLAG=SML::NONE) const; + inline const String& GetName() const { return m_oSML.GetName(); } + inline CConfigTable* GetParent() const { return m_pParent; } + inline void SetParent(CConfigTable* pParent) { m_pParent = pParent; } + static void STCALL ItemInitData(const String&, SMLVALUE&, void*); + static bool STCALL ItemSaveData(const SMLVALUE&, void*); + static void STCALL ItemReleaseData(SMLVALUE&, void*); + +private: + SML m_oSML; + CConfigTable* m_pParent; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_CONFIGTABLE_H__ diff --git a/libs/Common/CriticalSection.h b/libs/Common/CriticalSection.h new file mode 100644 index 0000000..b7f03a6 --- /dev/null +++ b/libs/Common/CriticalSection.h @@ -0,0 +1,264 @@ +//////////////////////////////////////////////////////////////////// +// CriticalSection.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_CRITCALSECTION_H__ +#define __SEACAVE_CRITCALSECTION_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Thread.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class CriticalSection +{ +#ifdef _MSC_VER + +public: + CriticalSection() { + InitializeCriticalSection(&cs); + } + ~CriticalSection() { + DeleteCriticalSection(&cs); + } + void Clear() { + DeleteCriticalSection(&cs); + InitializeCriticalSection(&cs); + } + void Enter() { + EnterCriticalSection(&cs); + } + bool TryEnter() { + return (TryEnterCriticalSection(&cs) != 0); + } + void Leave() { + LeaveCriticalSection(&cs); + } + +protected: + CRITICAL_SECTION cs; + +#else + +public: + CriticalSection() { + #ifdef __APPLE__ + pthread_mutex_init(&mtx, NULL); + #else + mtx = PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP; + #endif + } + ~CriticalSection() { pthread_mutex_destroy(&mtx); } + void Clear() { + pthread_mutex_destroy(&mtx); + #ifdef __APPLE__ + pthread_mutex_init(&mtx, NULL); + #else + mtx = PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP; + #endif + } + void Enter() { pthread_mutex_lock(&mtx); } + bool TryEnter() { return (pthread_mutex_trylock(&mtx) == 0); } + void Leave() { pthread_mutex_unlock(&mtx); } + pthread_mutex_t& getMutex() { return mtx; } + +protected: + pthread_mutex_t mtx; + +#endif + +private: + CriticalSection(const CriticalSection&); + CriticalSection& operator=(const CriticalSection&); +}; + +/** +* A fast, non-recursive and unfair implementation of the Critical Section. +* It is meant to be used in situations where the risk for lock conflict is very low, +* i e locks that are held for a very short time. The lock is _not_ recursive, i e if +* the same thread will try to grab the lock it'll hang in a never-ending loop. The lock +* is not fair, i e the first to try to enter a locked lock is not guaranteed to be the +* first to get it when it's freed... +*/ +class FastCriticalSection { +public: + FastCriticalSection() : state(0) {} + + void Clear() { + Thread::safeExchange(state, 0); + } + + void Enter() { + while (Thread::safeCompareExchange(state, 0, 1) != 0) + Thread::yield(); + } + bool TryEnter() { + return (Thread::safeCompareExchange(state, 0, 1) == 0); + } + void Leave() { + Thread::safeDec(state); + } + +protected: + volatile Thread::safe_t state; +}; + +template +class SimpleLock { +public: + SimpleLock(T& aCs) : cs(aCs) { cs.Enter(); } + ~SimpleLock() { cs.Leave(); } +protected: + T& cs; +}; + +template +class SimpleLockTry { +public: + SimpleLockTry(T& aCs) : cs(aCs) { bLocked = cs.TryEnter(); } + ~SimpleLockTry() { if (bLocked) cs.Leave(); } + bool IsLocked() const { return bLocked; } +protected: + T& cs; + bool bLocked; +}; + +typedef SimpleLock Lock; +typedef SimpleLock FastLock; +typedef SimpleLockTry LockTry; +typedef SimpleLockTry FastLockTry; + +class RWLock +{ +public: + RWLock() : cs(), readers(0) {} + ~RWLock() { ASSERT(readers==0); } + void Clear() { cs.Clear(); readers = 0; } + + // Read + void EnterRead() { + Lock l(cs); + ++readers; + } + + bool TryEnterRead() { + LockTry l(cs); + if (!l.IsLocked()) + return false; + ++readers; + return true; + } + + void LeaveRead() { + Lock l(cs); + ASSERT(readers > 0); + --readers; + } + + bool TryLeaveRead() { + LockTry l(cs); + if (!l.IsLocked()) + return false; + ASSERT(readers > 0); + --readers; + return true; + } + + // Write + void EnterWrite() { + cs.Enter(); + while (readers) { + cs.Leave(); + Thread::yield(); + cs.Enter(); + } + } + + bool TryEnterWrite() { + if (cs.TryEnter()) { + if (readers == 0) + return true; + cs.Leave(); + } + return false; + } + + void LeaveWrite() { + cs.Leave(); + } + +private: + RWLock(const RWLock&); + RWLock& operator=(const RWLock&); + +protected: + CriticalSection cs; + unsigned readers; +}; + +class RLock { +public: + RLock(RWLock& aCs) : cs(aCs) { cs.EnterRead(); } + ~RLock() { cs.LeaveRead(); } +private: + RLock(const RLock&); + RLock& operator=(const RLock&); +protected: + RWLock& cs; +}; + +class RLockTry { +public: + RLockTry(RWLock& aCs) : cs(aCs) { bLocked = cs.TryEnterRead(); } + ~RLockTry() { if (bLocked) cs.LeaveRead(); } + bool IsLocked() const { return bLocked; } + bool TryEnter() { return (bLocked = cs.TryEnterRead()); } + bool TryLeave() { return !(bLocked = !cs.TryLeaveRead()); } +private: + RLockTry(const RLockTry&); + RLockTry& operator=(const RLockTry&); +protected: + RWLock& cs; + bool bLocked; +}; + +class WLock { +public: + WLock(RWLock& aCs) : cs(aCs) { cs.EnterWrite(); } + ~WLock() { cs.LeaveWrite(); } +private: + WLock(const WLock&); + WLock& operator=(const WLock&); +protected: + RWLock& cs; +}; + +class WLockTry { +public: + WLockTry(RWLock& aCs) : cs(aCs) { bLocked = cs.TryEnterWrite(); } + ~WLockTry() { if (bLocked) cs.LeaveWrite(); } + bool IsLocked() const { return bLocked; } + bool TryEnter() { return (bLocked = cs.TryEnterWrite()); } +private: + WLockTry(const WLockTry&); + WLockTry& operator=(const WLockTry&); +protected: + RWLock& cs; + bool bLocked; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_CRITCALSECTION_H__ diff --git a/libs/Common/EventQueue.cpp b/libs/Common/EventQueue.cpp new file mode 100644 index 0000000..82f835b --- /dev/null +++ b/libs/Common/EventQueue.cpp @@ -0,0 +1,101 @@ +//////////////////////////////////////////////////////////////////// +// EventQueue.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "EventQueue.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +/*-----------------------------------------------------------* + * EventQueue class implementation * + *-----------------------------------------------------------*/ + +void EventQueue::Clear() +{ + m_cs.Clear(); + m_sem.Clear(); + m_events.Empty(); +} + +void EventQueue::AddEvent(Event* evt) +{ + Lock l(m_cs); + m_events.AddTail(evt); + m_sem.Signal(); +} +void EventQueue::AddEventFirst(Event* evt) +{ + Lock l(m_cs); + m_events.AddHead(evt); + m_sem.Signal(); +} + +Event* EventQueue::GetEvent() +{ + m_sem.Wait(); + Lock l(m_cs); + ASSERT(!m_events.IsEmpty()); + return m_events.RemoveHead(); +} +Event* EventQueue::GetEvent(uint32_t millis) +{ + if (!m_sem.Wait(millis)) + return NULL; + Lock l(m_cs); + if (m_events.IsEmpty()) + return NULL; + return m_events.RemoveHead(); +} +Event* EventQueue::GetEventLast() +{ + m_sem.Wait(); + Lock l(m_cs); + ASSERT(!m_events.IsEmpty()); + return m_events.RemoveTail(); +} +Event* EventQueue::GetEventLast(uint32_t millis) +{ + if (!m_sem.Wait(millis)) + return NULL; + Lock l(m_cs); + if (m_events.IsEmpty()) + return NULL; + return m_events.RemoveTail(); +} +/*----------------------------------------------------------------*/ + +bool EventQueue::IsEmpty() const +{ + Lock l(m_cs); + return m_events.IsEmpty(); +} + +uint_t EventQueue::GetSize() const +{ + Lock l(m_cs); + return m_events.GetSize(); +} +/*----------------------------------------------------------------*/ + + + +/*-----------------------------------------------------------* +* EventThreadPool class implementation * +*-----------------------------------------------------------*/ + +void EventThreadPool::stop() +{ + ThreadPool::stop(); + EventQueue::Clear(); +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/EventQueue.h b/libs/Common/EventQueue.h new file mode 100644 index 0000000..b698785 --- /dev/null +++ b/libs/Common/EventQueue.h @@ -0,0 +1,90 @@ +//////////////////////////////////////////////////////////////////// +// EventQueue.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_EVENTQUEUE_H__ +#define __SEACAVE_EVENTQUEUE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Types.h" +#include "CriticalSection.h" +#include "Semaphore.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class Event +{ +public: + Event(uint32_t _id) : id(_id) {} + virtual ~Event() {} + + uint32_t GetID() const { return id; } + + virtual bool Run(void* /*pArgs*/ = NULL) { return true; } + +protected: + const uint32_t id; +}; +typedef cQueue EVENTQUEUE; + + +/************************************************************************************** + * Events Queue + * -------------- + * basic eventing mechanism + * multi-thread safe + **************************************************************************************/ + +class GENERAL_API EventQueue +{ +public: + EventQueue() {} + ~EventQueue() {} + + void Clear(); // reset the state of the locks and empty the queue + + void AddEvent(Event*); //add a new event to the end of the queue + void AddEventFirst(Event*); //add a new event to the beginning of the queue + Event* GetEvent(); //block until an event arrives and get the first event pending in the queue + Event* GetEvent(uint32_t millis); //block until an event arrives or time expires and get the first event pending in the queue + Event* GetEventLast(); //block until an event arrives and get the last event pending in the queue + Event* GetEventLast(uint32_t millis); //block until an event arrives or time expires and get the last event pending in the queue + + bool IsEmpty() const; //are there any events in the queue? + uint_t GetSize() const; //number of events in the queue + +protected: + Semaphore m_sem; + mutable CriticalSection m_cs; + EVENTQUEUE m_events; +}; +/*----------------------------------------------------------------*/ + + +// basic event and thread pool +class GENERAL_API EventThreadPool : public ThreadPool, public EventQueue +{ +public: + inline EventThreadPool() {} + inline EventThreadPool(size_type nThreads) : ThreadPool(nThreads) {} + inline EventThreadPool(size_type nThreads, Thread::FncStart pfnStarter, void* pData=NULL) : ThreadPool(nThreads, pfnStarter, pData) {} + inline ~EventThreadPool() {} + + void stop(); //stop threads, reset locks state and empty event queue +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_EVENTQUEUE_H__ diff --git a/libs/Common/FastDelegate.h b/libs/Common/FastDelegate.h new file mode 100644 index 0000000..de2b8bc --- /dev/null +++ b/libs/Common/FastDelegate.h @@ -0,0 +1,2105 @@ +// FastDelegate.h +// Efficient delegates in C++ that generate only two lines of asm code! +// Documentation is found at http://www.codeproject.com/cpp/FastDelegate.asp +// +// - Don Clugston, Mar 2004. +// Major contributions were made by Jody Hagins. +// History: +// 24-Apr-04 1.0 * Submitted to CodeProject. +// 28-Apr-04 1.1 * Prevent most unsafe uses of evil static function hack. +// * Improved syntax for horrible_cast (thanks Paul Bludov). +// * Tested on Metrowerks MWCC and Intel ICL (IA32) +// * Compiled, but not run, on Comeau C++ and Intel Itanium ICL. +// 27-Jun-04 1.2 * Now works on Borland C++ Builder 5.5 +// * Now works on /clr "managed C++" code on VC7, VC7.1 +// * Comeau C++ now compiles without warnings. +// * Prevent the virtual inheritance case from being used on +// VC6 and earlier, which generate incorrect code. +// * Improved warning and error messages. Non-standard hacks +// now have compile-time checks to make them safer. +// * implicit_cast used instead of static_cast in many cases. +// * If calling a const member function, a const class pointer can be used. +// * MakeDelegate() global helper function added to simplify pass-by-value. +// * Added fastdelegate.clear() +// 16-Jul-04 1.2.1* Workaround for gcc bug (const member function pointers in templates) +// 30-Oct-04 1.3 * Support for (non-void) return values. +// * No more workarounds in client code! +// MSVC and Intel now use a clever hack invented by John Dlugosz: +// - The FASTDELEGATEDECLARE workaround is no longer necessary. +// - No more warning messages for VC6 +// * Less use of macros. Error messages should be more comprehensible. +// * Added include guards +// * Added FastDelegate::empty() to test if invocation is safe (Thanks Neville Franks). +// * Now tested on VS 2005 Express Beta, PGI C++ +// 24-Dec-04 1.4 * Added DelegateMemento, to allow collections of disparate delegates. +// * <,>,<=,>= comparison operators to allow storage in ordered containers. +// * Substantial reduction of code size, especially the 'Closure' class. +// * Standardised all the compiler-specific workarounds. +// * MFP conversion now works for CodePlay (but not yet supported in the full code). +// * Now compiles without warnings on _any_ supported compiler, including BCC 5.5.1 +// * New syntax: FastDelegate< int (char *, double) >. +// 14-Feb-05 1.4.1* Now treats =0 as equivalent to .clear(), ==0 as equivalent to .empty(). (Thanks elfric). +// * Now tested on Intel ICL for AMD64, VS2005 Beta for AMD64 and Itanium. +// 30-Mar-05 1.5 * Safebool idiom: "if (dg)" is now equivalent to "if (!dg.empty())" +// * Fully supported by CodePlay VectorC +// * Bugfix for Metrowerks: empty() was buggy because a valid MFP can be 0 on MWCC! +// * More optimal assignment,== and != operators for static function pointers. + +#ifndef FASTDELEGATE_H +#define FASTDELEGATE_H + +#include // to allow <,> comparisons + +//////////////////////////////////////////////////////////////////////////////// +// Configuration options +// +//////////////////////////////////////////////////////////////////////////////// + +// Uncomment the following #define for optimally-sized delegates. +// In this case, the generated asm code is almost identical to the code you'd get +// if the compiler had native support for delegates. +// It will not work on systems where sizeof(dataptr) < sizeof(codeptr). +// Thus, it will not work for DOS compilers using the medium model. +// It will also probably fail on some DSP systems. +#define FASTDELEGATE_USESTATICFUNCTIONHACK + +// Uncomment the next line to allow function declarator syntax. +// It is automatically enabled for those compilers where it is known to work. +//#define FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX + +//////////////////////////////////////////////////////////////////////////////// +// Compiler identification for workarounds +// +//////////////////////////////////////////////////////////////////////////////// + +// Compiler identification. It's not easy to identify Visual C++ because +// many vendors fraudulently define Microsoft's identifiers. +#if defined(_MSC_VER) && !defined(__MWERKS__) && !defined(__VECTOR_C) && !defined(__ICL) && !defined(__BORLANDC__) +#define FASTDLGT_ISMSVC + +#if (_MSC_VER <1300) // Many workarounds are required for VC6. +#define FASTDLGT_VC6 +#pragma warning(disable:4786) // disable this ridiculous warning +#endif + +#endif + +// Does the compiler uses Microsoft's member function pointer structure? +// If so, it needs special treatment. +// Metrowerks CodeWarrior, Intel, and CodePlay fraudulently define Microsoft's +// identifier, _MSC_VER. We need to filter Metrowerks out. +#if defined(_MSC_VER) && !defined(__MWERKS__) +#define FASTDLGT_MICROSOFT_MFP + +#if !defined(__VECTOR_C) +// CodePlay doesn't have the __single/multi/virtual_inheritance keywords +#define FASTDLGT_HASINHERITANCE_KEYWORDS +#endif +#endif + +// Does it allow function declarator syntax? The following compilers are known to work: +#if defined(FASTDLGT_ISMSVC) && (_MSC_VER >=1310) // VC 7.1 +#define FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX +#endif + +// Gcc(2.95+), and versions of Digital Mars, Intel and Comeau in common use. +#if defined (__DMC__) || defined(__GNUC__) || defined(__ICL) || defined(__COMO__) +#define FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX +#endif + +// It works on Metrowerks MWCC 3.2.2. From boost.Config it should work on earlier ones too. +#if defined (__MWERKS__) +#define FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX +#endif + +#ifdef __GNUC__ // Workaround GCC bug #8271 + // At present, GCC doesn't recognize constness of MFPs in templates +#define FASTDELEGATE_GCC_BUG_8271 +#endif + + + +//////////////////////////////////////////////////////////////////////////////// +// General tricks used in this code +// +// (a) Error messages are generated by typdefing an array of negative size to +// generate compile-time errors. +// (b) Warning messages on MSVC are generated by declaring unused variables, and +// enabling the "variable XXX is never used" warning. +// (c) Unions are used in a few compiler-specific cases to perform illegal casts. +// (d) For Microsoft and Intel, when adjusting the 'this' pointer, it's cast to +// (char *) first to ensure that the correct number of *bytes* are added. +// +//////////////////////////////////////////////////////////////////////////////// +// Helper templates +// +//////////////////////////////////////////////////////////////////////////////// + + +namespace fastdelegate { +namespace detail { // we'll hide the implementation details in a nested namespace. + +// implicit_cast< > +// I believe this was originally going to be in the C++ standard but +// was left out by accident. It's even milder than static_cast. +// I use it instead of static_cast<> to emphasize that I'm not doing +// anything nasty. +// Usage is identical to static_cast<> +template +inline OutputClass implicit_cast(InputClass input){ + return input; +} + +// horrible_cast< > +// This is truly evil. It completely subverts C++'s type system, allowing you +// to cast from any class to any other class. Technically, using a union +// to perform the cast is undefined behaviour (even in C). But we can see if +// it is OK by checking that the union is the same size as each of its members. +// horrible_cast<> should only be used for compiler-specific workarounds. +// Usage is identical to reinterpret_cast<>. + +// This union is declared outside the horrible_cast because BCC 5.5.1 +// can't inline a function with a nested class, and gives a warning. +template +union horrible_union{ + OutputClass out; + InputClass in; +}; + +template +inline OutputClass horrible_cast(const InputClass input){ + horrible_union u; + // Cause a compile-time error if in, out and u are not the same size. + // If the compile fails here, it means the compiler has peculiar + // unions which would prevent the cast from working. + typedef int ERROR_CantUseHorrible_cast[sizeof(InputClass)==sizeof(u) + && sizeof(InputClass)==sizeof(OutputClass) ? 1 : -1]; + u.in = input; + return u.out; +} + +//////////////////////////////////////////////////////////////////////////////// +// Workarounds +// +//////////////////////////////////////////////////////////////////////////////// + +// Backwards compatibility: This macro used to be necessary in the virtual inheritance +// case for Intel and Microsoft. Now it just forward-declares the class. +#define FASTDELEGATEDECLARE(CLASSNAME) class CLASSNAME; + +// Prevent use of the static function hack with the DOS medium model. +#ifdef __MEDIUM__ +#undef FASTDELEGATE_USESTATICFUNCTIONHACK +#endif + +// DefaultVoid - a workaround for 'void' templates in VC6. +// +// (1) VC6 and earlier do not allow 'void' as a default template argument. +// (2) They also doesn't allow you to return 'void' from a function. +// +// Workaround for (1): Declare a dummy type 'DefaultVoid' which we use +// when we'd like to use 'void'. We convert it into 'void' and back +// using the templates DefaultVoidToVoid<> and VoidToDefaultVoid<>. +// Workaround for (2): On VC6, the code for calling a void function is +// identical to the code for calling a non-void function in which the +// return value is never used, provided the return value is returned +// in the EAX register, rather than on the stack. +// This is true for most fundamental types such as int, enum, void *. +// Const void * is the safest option since it doesn't participate +// in any automatic conversions. But on a 16-bit compiler it might +// cause extra code to be generated, so we disable it for all compilers +// except for VC6 (and VC5). +#ifdef FASTDLGT_VC6 +// VC6 workaround +typedef const void * DefaultVoid; +#else +// On any other compiler, just use a normal void. +typedef void DefaultVoid; +#endif + +// Translate from 'DefaultVoid' to 'void'. +// Everything else is unchanged +template +struct DefaultVoidToVoid { typedef T type; }; + +template <> +struct DefaultVoidToVoid { typedef void type; }; + +// Translate from 'void' into 'DefaultVoid' +// Everything else is unchanged +template +struct VoidToDefaultVoid { typedef T type; }; + +template <> +struct VoidToDefaultVoid { typedef DefaultVoid type; }; + + + +//////////////////////////////////////////////////////////////////////////////// +// Fast Delegates, part 1: +// +// Conversion of member function pointer to a standard form +// +//////////////////////////////////////////////////////////////////////////////// + +// GenericClass is a fake class, ONLY used to provide a type. +// It is vitally important that it is never defined, so that the compiler doesn't +// think it can optimize the invocation. For example, Borland generates simpler +// code if it knows the class only uses single inheritance. + +// Compilers using Microsoft's structure need to be treated as a special case. +#ifdef FASTDLGT_MICROSOFT_MFP + +#ifdef FASTDLGT_HASINHERITANCE_KEYWORDS + // For Microsoft and Intel, we want to ensure that it's the most efficient type of MFP + // (4 bytes), even when the /vmg option is used. Declaring an empty class + // would give 16 byte pointers in this case.... + class __single_inheritance GenericClass; +#endif + // ...but for Codeplay, an empty class *always* gives 4 byte pointers. + // If compiled with the /clr option ("managed C++"), the JIT compiler thinks + // it needs to load GenericClass before it can call any of its functions, + // (compiles OK but crashes at runtime!), so we need to declare an + // empty class to make it happy. + // Codeplay and VC4 can't cope with the unknown_inheritance case either. + class GenericClass {}; +#else + class GenericClass; +#endif + +// The size of a single inheritance member function pointer. +const int SINGLE_MEMFUNCPTR_SIZE = sizeof(void (GenericClass::*)()); + +// SimplifyMemFunc< >::Convert() +// +// A template function that converts an arbitrary member function pointer into the +// simplest possible form of member function pointer, using a supplied 'this' pointer. +// According to the standard, this can be done legally with reinterpret_cast<>. +// For (non-standard) compilers which use member function pointers which vary in size +// depending on the class, we need to use knowledge of the internal structure of a +// member function pointer, as used by the compiler. Template specialization is used +// to distinguish between the sizes. Because some compilers don't support partial +// template specialisation, I use full specialisation of a wrapper struct. + +// general case -- don't know how to convert it. Force a compile failure +template +struct SimplifyMemFunc { + template + inline static GenericClass *Convert(X *pthis, XFuncType function_to_bind, + GenericMemFuncType &bound_func) { + // Unsupported member function type -- force a compile failure. + // (it's illegal to have a array with negative size). + typedef char ERROR_Unsupported_member_function_pointer_on_this_compiler[N-100]; + return 0; + } +}; + +// For compilers where all member func ptrs are the same size, everything goes here. +// For non-standard compilers, only single_inheritance classes go here. +template <> +struct SimplifyMemFunc { + template + inline static GenericClass *Convert(X *pthis, XFuncType function_to_bind, + GenericMemFuncType &bound_func) { +#if defined __DMC__ + // Digital Mars doesn't allow you to cast between arbitrary PMF's, + // even though the standard says you can. The 32-bit compiler lets you + // static_cast through an int, but the DOS compiler doesn't. + bound_func = horrible_cast(function_to_bind); +#else + bound_func = reinterpret_cast(function_to_bind); +#endif + return reinterpret_cast(pthis); + } +}; + +//////////////////////////////////////////////////////////////////////////////// +// Fast Delegates, part 1b: +// +// Workarounds for Microsoft and Intel +// +//////////////////////////////////////////////////////////////////////////////// + + +// Compilers with member function pointers which violate the standard (MSVC, Intel, Codeplay), +// need to be treated as a special case. +#ifdef FASTDLGT_MICROSOFT_MFP + +// We use unions to perform horrible_casts. I would like to use #pragma pack(push, 1) +// at the start of each function for extra safety, but VC6 seems to ICE +// intermittently if you do this inside a template. + +// __multiple_inheritance classes go here +// Nasty hack for Microsoft and Intel (IA32 and Itanium) +template<> +struct SimplifyMemFunc< SINGLE_MEMFUNCPTR_SIZE + sizeof(int) > { + template + inline static GenericClass *Convert(X *pthis, XFuncType function_to_bind, + GenericMemFuncType &bound_func) { + // We need to use a horrible_cast to do this conversion. + // In MSVC, a multiple inheritance member pointer is internally defined as: + union { + XFuncType func; + struct { + GenericMemFuncType funcaddress; // points to the actual member function + int delta; // #BYTES to be added to the 'this' pointer + }s; + } u; + // Check that the horrible_cast will work + typedef int ERROR_CantUsehorrible_cast[sizeof(function_to_bind)==sizeof(u.s)? 1 : -1]; + u.func = function_to_bind; + bound_func = u.s.funcaddress; + return reinterpret_cast(reinterpret_cast(pthis) + u.s.delta); + } +}; + +// virtual inheritance is a real nuisance. It's inefficient and complicated. +// On MSVC and Intel, there isn't enough information in the pointer itself to +// enable conversion to a closure pointer. Earlier versions of this code didn't +// work for all cases, and generated a compile-time error instead. +// But a very clever hack invented by John M. Dlugosz solves this problem. +// My code is somewhat different to his: I have no asm code, and I make no +// assumptions about the calling convention that is used. + +// In VC++ and ICL, a virtual_inheritance member pointer +// is internally defined as: +struct MicrosoftVirtualMFP { + void (GenericClass::*codeptr)(); // points to the actual member function + int delta; // #bytes to be added to the 'this' pointer + int vtable_index; // or 0 if no virtual inheritance +}; +// The CRUCIAL feature of Microsoft/Intel MFPs which we exploit is that the +// m_codeptr member is *always* called, regardless of the values of the other +// members. (This is *not* true for other compilers, eg GCC, which obtain the +// function address from the vtable if a virtual function is being called). +// Dlugosz's trick is to make the codeptr point to a probe function which +// returns the 'this' pointer that was used. + +// Define a generic class that uses virtual inheritance. +// It has a trivial member function that returns the value of the 'this' pointer. +struct GenericVirtualClass : virtual public GenericClass +{ + typedef GenericVirtualClass * (GenericVirtualClass::*ProbePtrType)(); + GenericVirtualClass * GetThis() { return this; } +}; + +// __virtual_inheritance classes go here +template <> +struct SimplifyMemFunc +{ + + template + inline static GenericClass *Convert(X *pthis, XFuncType function_to_bind, + GenericMemFuncType &bound_func) { + union { + XFuncType func; + GenericClass* (X::*ProbeFunc)(); + MicrosoftVirtualMFP s; + } u; + u.func = function_to_bind; + bound_func = reinterpret_cast(u.s.codeptr); + union { + GenericVirtualClass::ProbePtrType virtfunc; + MicrosoftVirtualMFP s; + } u2; + // Check that the horrible_cast<>s will work + typedef int ERROR_CantUsehorrible_cast[sizeof(function_to_bind)==sizeof(u.s) + && sizeof(function_to_bind)==sizeof(u.ProbeFunc) + && sizeof(u2.virtfunc)==sizeof(u2.s) ? 1 : -1]; + // Unfortunately, taking the address of a MF prevents it from being inlined, so + // this next line can't be completely optimised away by the compiler. + u2.virtfunc = &GenericVirtualClass::GetThis; + u.s.codeptr = u2.s.codeptr; + return (pthis->*u.ProbeFunc)(); + } +}; + +#if (_MSC_VER <1300) + +// Nasty hack for Microsoft Visual C++ 6.0 +// unknown_inheritance classes go here +// There is a compiler bug in MSVC6 which generates incorrect code in this case!! +template <> +struct SimplifyMemFunc +{ + template + inline static GenericClass *Convert(X *pthis, XFuncType function_to_bind, + GenericMemFuncType &bound_func) { + // There is an apalling but obscure compiler bug in MSVC6 and earlier: + // vtable_index and 'vtordisp' are always set to 0 in the + // unknown_inheritance case! + // This means that an incorrect function could be called!!! + // Compiling with the /vmg option leads to potentially incorrect code. + // This is probably the reason that the IDE has a user interface for specifying + // the /vmg option, but it is disabled - you can only specify /vmg on + // the command line. In VC1.5 and earlier, the compiler would ICE if it ever + // encountered this situation. + // It is OK to use the /vmg option if /vmm or /vms is specified. + + // Fortunately, the wrong function is only called in very obscure cases. + // It only occurs when a derived class overrides a virtual function declared + // in a virtual base class, and the member function + // points to the *Derived* version of that function. The problem can be + // completely averted in 100% of cases by using the *Base class* for the + // member fpointer. Ie, if you use the base class as an interface, you'll + // stay out of trouble. + // Occasionally, you might want to point directly to a derived class function + // that isn't an override of a base class. In this case, both vtable_index + // and 'vtordisp' are zero, but a virtual_inheritance pointer will be generated. + // We can generate correct code in this case. To prevent an incorrect call from + // ever being made, on MSVC6 we generate a warning, and call a function to + // make the program crash instantly. + typedef char ERROR_VC6CompilerBug[-100]; + return 0; + } +}; + + +#else + +// Nasty hack for Microsoft and Intel (IA32 and Itanium) +// unknown_inheritance classes go here +// This is probably the ugliest bit of code I've ever written. Look at the casts! +// There is a compiler bug in MSVC6 which prevents it from using this code. +template <> +struct SimplifyMemFunc +{ + template + inline static GenericClass *Convert(X *pthis, XFuncType function_to_bind, + GenericMemFuncType &bound_func) { + // The member function pointer is 16 bytes long. We can't use a normal cast, but + // we can use a union to do the conversion. + union { + XFuncType func; + // In VC++ and ICL, an unknown_inheritance member pointer + // is internally defined as: + struct { + GenericMemFuncType m_funcaddress; // points to the actual member function + int delta; // #bytes to be added to the 'this' pointer + int vtordisp; // #bytes to add to 'this' to find the vtable + int vtable_index; // or 0 if no virtual inheritance + } s; + } u; + // Check that the horrible_cast will work + typedef int ERROR_CantUsehorrible_cast[sizeof(XFuncType)==sizeof(u.s)? 1 : -1]; + u.func = function_to_bind; + bound_func = u.s.funcaddress; + int virtual_delta = 0; + if (u.s.vtable_index) { // Virtual inheritance is used + // First, get to the vtable. + // It is 'vtordisp' bytes from the start of the class. + const int * vtable = *reinterpret_cast( + reinterpret_cast(pthis) + u.s.vtordisp ); + + // 'vtable_index' tells us where in the table we should be looking. + virtual_delta = u.s.vtordisp + *reinterpret_cast( + reinterpret_cast(vtable) + u.s.vtable_index); + } + // The int at 'virtual_delta' gives us the amount to add to 'this'. + // Finally we can add the three components together. Phew! + return reinterpret_cast( + reinterpret_cast(pthis) + u.s.delta + virtual_delta); + }; +}; +#endif // MSVC 7 and greater + +#endif // MS/Intel hacks + +} // namespace detail + +//////////////////////////////////////////////////////////////////////////////// +// Fast Delegates, part 2: +// +// Define the delegate storage, and cope with static functions +// +//////////////////////////////////////////////////////////////////////////////// + +// DelegateMemento -- an opaque structure which can hold an arbitrary delegate. +// It knows nothing about the calling convention or number of arguments used by +// the function pointed to. +// It supplies comparison operators so that it can be stored in STL collections. +// It cannot be set to anything other than null, nor invoked directly: +// it must be converted to a specific delegate. + +// Implementation: +// There are two possible implementations: the Safe method and the Evil method. +// DelegateMemento - Safe version +// +// This implementation is standard-compliant, but a bit tricky. +// A static function pointer is stored inside the class. +// Here are the valid values: +// +-- Static pointer --+--pThis --+-- pMemFunc-+-- Meaning------+ +// | 0 | 0 | 0 | Empty | +// | !=0 |(dontcare)| Invoker | Static function| +// | 0 | !=0 | !=0* | Method call | +// +--------------------+----------+------------+----------------+ +// * For Metrowerks, this can be 0. (first virtual function in a +// single_inheritance class). +// When stored stored inside a specific delegate, the 'dontcare' entries are replaced +// with a reference to the delegate itself. This complicates the = and == operators +// for the delegate class. + +// DelegateMemento - Evil version +// +// For compilers where data pointers are at least as big as code pointers, it is +// possible to store the function pointer in the this pointer, using another +// horrible_cast. In this case the DelegateMemento implementation is simple: +// +--pThis --+-- pMemFunc-+-- Meaning---------------------+ +// | 0 | 0 | Empty | +// | !=0 | !=0* | Static function or method call| +// +----------+------------+-------------------------------+ +// * For Metrowerks, this can be 0. (first virtual function in a +// single_inheritance class). +// Note that the Sun C++ and MSVC documentation explicitly state that they +// support static_cast between void * and function pointers. + +class DelegateMemento { +protected: + // the data is protected, not private, because many + // compilers have problems with template friends. + typedef void (detail::GenericClass::*GenericMemFuncType)(); // arbitrary MFP. + detail::GenericClass *m_pthis; + GenericMemFuncType m_pFunction; + +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + typedef void (*GenericFuncPtr)(); // arbitrary code pointer + GenericFuncPtr m_pStaticFunction; +#endif + +public: +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + DelegateMemento() : m_pthis(0), m_pFunction(0), m_pStaticFunction(0) {}; + void clear() { + m_pthis=0; m_pFunction=0; m_pStaticFunction=0; + } +#else + DelegateMemento() : m_pthis(0), m_pFunction(0) {}; + void clear() { m_pthis=0; m_pFunction=0; } +#endif +public: +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + inline bool IsEqual (const DelegateMemento &x) const{ + // We have to cope with the static function pointers as a special case + if (m_pFunction!=x.m_pFunction) return false; + // the static function ptrs must either both be equal, or both be 0. + if (m_pStaticFunction!=x.m_pStaticFunction) return false; + if (m_pStaticFunction!=0) return m_pthis==x.m_pthis; + else return true; + } +#else // Evil Method + inline bool IsEqual (const DelegateMemento &x) const{ + return m_pthis==x.m_pthis && m_pFunction==x.m_pFunction; + } +#endif + // Provide a strict weak ordering for DelegateMementos. + inline bool IsLess(const DelegateMemento &right) const { + // deal with static function pointers first +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + if (m_pStaticFunction !=0 || right.m_pStaticFunction!=0) + return m_pStaticFunction < right.m_pStaticFunction; +#endif + if (m_pthis !=right.m_pthis) return m_pthis < right.m_pthis; + // There are no ordering operators for member function pointers, + // but we can fake one by comparing each byte. The resulting ordering is + // arbitrary (and compiler-dependent), but it permits storage in ordered STL containers. + return memcmp(&m_pFunction, &right.m_pFunction, sizeof(m_pFunction)) < 0; + + } + // BUGFIX (Mar 2005): + // We can't just compare m_pFunction because on Metrowerks, + // m_pFunction can be zero even if the delegate is not empty! + inline bool operator ! () const // Is it bound to anything? + { return m_pthis==0 && m_pFunction==0; } + inline bool empty() const // Is it bound to anything? + { return m_pthis==0 && m_pFunction==0; } +public: + DelegateMemento & operator = (const DelegateMemento &right) { + SetMementoFrom(right); + return *this; + } + inline bool operator <(const DelegateMemento &right) { + return IsLess(right); + } + inline bool operator >(const DelegateMemento &right) { + return right.IsLess(*this); + } + DelegateMemento (const DelegateMemento &right) : + m_pFunction(right.m_pFunction), m_pthis(right.m_pthis) +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + , m_pStaticFunction (right.m_pStaticFunction) +#endif + {} +protected: + void SetMementoFrom(const DelegateMemento &right) { + m_pFunction = right.m_pFunction; + m_pthis = right.m_pthis; +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + m_pStaticFunction = right.m_pStaticFunction; +#endif + } +}; + + +// ClosurePtr<> +// +// A private wrapper class that adds function signatures to DelegateMemento. +// It's the class that does most of the actual work. +// The signatures are specified by: +// GenericMemFunc: must be a type of GenericClass member function pointer. +// StaticFuncPtr: must be a type of function pointer with the same signature +// as GenericMemFunc. +// UnvoidStaticFuncPtr: is the same as StaticFuncPtr, except on VC6 +// where it never returns void (returns DefaultVoid instead). + +// An outer class, FastDelegateN<>, handles the invoking and creates the +// necessary typedefs. +// This class does everything else. + +namespace detail { + +template < class GenericMemFunc, class StaticFuncPtr, class UnvoidStaticFuncPtr> +class ClosurePtr : public DelegateMemento { +public: + // These functions are for setting the delegate to a member function. + + // Here's the clever bit: we convert an arbitrary member function into a + // standard form. XMemFunc should be a member function of class X, but I can't + // enforce that here. It needs to be enforced by the wrapper class. + template < class X, class XMemFunc > + inline void bindmemfunc(X *pthis, XMemFunc function_to_bind ) { + m_pthis = SimplifyMemFunc< sizeof(function_to_bind) > + ::Convert(pthis, function_to_bind, m_pFunction); +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + m_pStaticFunction = 0; +#endif + } + // For const member functions, we only need a const class pointer. + // Since we know that the member function is const, it's safe to + // remove the const qualifier from the 'this' pointer with a const_cast. + // VC6 has problems if we just overload 'bindmemfunc', so we give it a different name. + template < class X, class XMemFunc> + inline void bindconstmemfunc(const X *pthis, XMemFunc function_to_bind) { + m_pthis= SimplifyMemFunc< sizeof(function_to_bind) > + ::Convert(const_cast(pthis), function_to_bind, m_pFunction); +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + m_pStaticFunction = 0; +#endif + } +#ifdef FASTDELEGATE_GCC_BUG_8271 // At present, GCC doesn't recognize constness of MFPs in templates + template < class X, class XMemFunc> + inline void bindmemfunc(const X *pthis, XMemFunc function_to_bind) { + bindconstmemfunc(pthis, function_to_bind); +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + m_pStaticFunction = 0; +#endif + } +#endif + // These functions are required for invoking the stored function + inline GenericClass *GetClosureThis() const { return m_pthis; } + inline GenericMemFunc GetClosureMemPtr() const { return reinterpret_cast(m_pFunction); } + +// There are a few ways of dealing with static function pointers. +// There's a standard-compliant, but tricky method. +// There's also a straightforward hack, that won't work on DOS compilers using the +// medium memory model. It's so evil that I can't recommend it, but I've +// implemented it anyway because it produces very nice asm code. + +#if !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + +// ClosurePtr<> - Safe version +// +// This implementation is standard-compliant, but a bit tricky. +// I store the function pointer inside the class, and the delegate then +// points to itself. Whenever the delegate is copied, these self-references +// must be transformed, and this complicates the = and == operators. +public: + // The next two functions are for operator ==, =, and the copy constructor. + // We may need to convert the m_pthis pointers, so that + // they remain as self-references. + template< class DerivedClass > + inline void CopyFrom (DerivedClass *pParent, const DelegateMemento &x) { + SetMementoFrom(x); + if (m_pStaticFunction!=0) { + // transform self references... + m_pthis=reinterpret_cast(pParent); + } + } + // For static functions, the 'static_function_invoker' class in the parent + // will be called. The parent then needs to call GetStaticFunction() to find out + // the actual function to invoke. + template < class DerivedClass, class ParentInvokerSig > + inline void bindstaticfunc(DerivedClass *pParent, ParentInvokerSig static_function_invoker, + StaticFuncPtr function_to_bind ) { + if (function_to_bind==0) { // cope with assignment to 0 + m_pFunction=0; + } else { + bindmemfunc(pParent, static_function_invoker); + } + m_pStaticFunction=reinterpret_cast(function_to_bind); + } + inline UnvoidStaticFuncPtr GetStaticFunction() const { + return reinterpret_cast(m_pStaticFunction); + } +#else + +// ClosurePtr<> - Evil version +// +// For compilers where data pointers are at least as big as code pointers, it is +// possible to store the function pointer in the this pointer, using another +// horrible_cast. Invocation isn't any faster, but it saves 4 bytes, and +// speeds up comparison and assignment. If C++ provided direct language support +// for delegates, they would produce asm code that was almost identical to this. +// Note that the Sun C++ and MSVC documentation explicitly state that they +// support static_cast between void * and function pointers. + + template< class DerivedClass > + inline void CopyFrom (DerivedClass *pParent, const DelegateMemento &right) { + SetMementoFrom(right); + } + // For static functions, the 'static_function_invoker' class in the parent + // will be called. The parent then needs to call GetStaticFunction() to find out + // the actual function to invoke. + // ******** EVIL, EVIL CODE! ******* + template < class DerivedClass, class ParentInvokerSig> + inline void bindstaticfunc(DerivedClass *pParent, ParentInvokerSig static_function_invoker, + StaticFuncPtr function_to_bind) { + if (function_to_bind==0) { // cope with assignment to 0 + m_pFunction=0; + } else { + // We'll be ignoring the 'this' pointer, but we need to make sure we pass + // a valid value to bindmemfunc(). + bindmemfunc(pParent, static_function_invoker); + } + + // WARNING! Evil hack. We store the function in the 'this' pointer! + // Ensure that there's a compilation failure if function pointers + // and data pointers have different sizes. + // If you get this error, you need to #undef FASTDELEGATE_USESTATICFUNCTIONHACK. + typedef int ERROR_CantUseEvilMethod[sizeof(GenericClass *)==sizeof(function_to_bind) ? 1 : -1]; + m_pthis = horrible_cast(function_to_bind); + // MSVC, SunC++ and DMC accept the following (non-standard) code: +// m_pthis = static_cast(static_cast(function_to_bind)); + // BCC32, Comeau and DMC accept this method. MSVC7.1 needs __int64 instead of long +// m_pthis = reinterpret_cast(reinterpret_cast(function_to_bind)); + } + // ******** EVIL, EVIL CODE! ******* + // This function will be called with an invalid 'this' pointer!! + // We're just returning the 'this' pointer, converted into + // a function pointer! + inline UnvoidStaticFuncPtr GetStaticFunction() const { + // Ensure that there's a compilation failure if function pointers + // and data pointers have different sizes. + // If you get this error, you need to #undef FASTDELEGATE_USESTATICFUNCTIONHACK. + typedef int ERROR_CantUseEvilMethod[sizeof(UnvoidStaticFuncPtr)==sizeof(this) ? 1 : -1]; + return horrible_cast(this); + } +#endif // !defined(FASTDELEGATE_USESTATICFUNCTIONHACK) + + // Does the closure contain this static function? + inline bool IsEqualToStaticFuncPtr(StaticFuncPtr funcptr){ + if (funcptr==0) return empty(); + // For the Evil method, if it doesn't actually contain a static function, this will return an arbitrary + // value that is not equal to any valid function pointer. + else return funcptr==reinterpret_cast(GetStaticFunction()); + } +}; + + +} // namespace detail + +//////////////////////////////////////////////////////////////////////////////// +// Fast Delegates, part 3: +// +// Wrapper classes to ensure type safety +// +//////////////////////////////////////////////////////////////////////////////// + + +// Once we have the member function conversion templates, it's easy to make the +// wrapper classes. So that they will work with as many compilers as possible, +// the classes are of the form +// FastDelegate3 +// They can cope with any combination of parameters. The max number of parameters +// allowed is 8, but it is trivial to increase this limit. +// Note that we need to treat const member functions separately. +// All this class does is to enforce type safety, and invoke the delegate with +// the correct list of parameters. + +// Because of the weird rule about the class of derived member function pointers, +// you sometimes need to apply a downcast to the 'this' pointer. +// This is the reason for the use of "implicit_cast(pthis)" in the code below. +// If CDerivedClass is derived from CBaseClass, but doesn't override SimpleVirtualFunction, +// without this trick you'd need to write: +// MyDelegate(static_cast(&d), &CDerivedClass::SimpleVirtualFunction); +// but with the trick you can write +// MyDelegate(&d, &CDerivedClass::SimpleVirtualFunction); + +// RetType is the type the compiler uses in compiling the template. For VC6, +// it cannot be void. DesiredRetType is the real type which is returned from +// all of the functions. It can be void. + +// Implicit conversion to "bool" is achieved using the safe_bool idiom, +// using member data pointers (MDP). This allows "if (dg)..." syntax +// Because some compilers (eg codeplay) don't have a unique value for a zero +// MDP, an extra padding member is added to the SafeBool struct. +// Some compilers (eg VC6) won't implicitly convert from 0 to an MDP, so +// in that case the static function constructor is not made explicit; this +// allows "if (dg==0) ..." to compile. + +//N=0 +template +class FastDelegate0 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(); + typedef RetType (*UnvoidStaticFunctionPtr)(); + typedef RetType (detail::GenericClass::*GenericMemFn)(); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate0 type; + + // Construction and comparison functions + FastDelegate0() { clear(); } + FastDelegate0(const FastDelegate0 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate0 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate0 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate0 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate0 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate0 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate0(Y *pthis, DesiredRetType (X::* function_to_bind)() ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)()) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate0(const Y *pthis, DesiredRetType (X::* function_to_bind)() const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)() const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate0(DesiredRetType (*function_to_bind)() ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)() ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)()) { + m_Closure.bindstaticfunc(this, &FastDelegate0::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() () const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction() const { + return (*(m_Closure.GetStaticFunction()))(); } +}; + +//N=1 +template +class FastDelegate1 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate1 type; + + // Construction and comparison functions + FastDelegate1() { clear(); } + FastDelegate1(const FastDelegate1 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate1 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate1 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate1 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate1 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate1 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate1(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate1(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate1(DesiredRetType (*function_to_bind)(Param1 p1) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1)) { + m_Closure.bindstaticfunc(this, &FastDelegate1::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1) const { + return (*(m_Closure.GetStaticFunction()))(p1); } +}; + +//N=2 +template +class FastDelegate2 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1, Param2 p2); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1, Param2 p2); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1, Param2 p2); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate2 type; + + // Construction and comparison functions + FastDelegate2() { clear(); } + FastDelegate2(const FastDelegate2 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate2 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate2 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate2 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate2 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate2 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate2(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate2(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate2(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2)) { + m_Closure.bindstaticfunc(this, &FastDelegate2::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1, Param2 p2) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1, p2); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1, Param2 p2) const { + return (*(m_Closure.GetStaticFunction()))(p1, p2); } +}; + +//N=3 +template +class FastDelegate3 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1, Param2 p2, Param3 p3); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate3 type; + + // Construction and comparison functions + FastDelegate3() { clear(); } + FastDelegate3(const FastDelegate3 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate3 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate3 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate3 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate3 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate3 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate3(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate3(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate3(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3)) { + m_Closure.bindstaticfunc(this, &FastDelegate3::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1, Param2 p2, Param3 p3) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1, p2, p3); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1, Param2 p2, Param3 p3) const { + return (*(m_Closure.GetStaticFunction()))(p1, p2, p3); } +}; + +//N=4 +template +class FastDelegate4 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1, Param2 p2, Param3 p3, Param4 p4); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate4 type; + + // Construction and comparison functions + FastDelegate4() { clear(); } + FastDelegate4(const FastDelegate4 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate4 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate4 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate4 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate4 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate4 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate4(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate4(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate4(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4)) { + m_Closure.bindstaticfunc(this, &FastDelegate4::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1, Param2 p2, Param3 p3, Param4 p4) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1, p2, p3, p4); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1, Param2 p2, Param3 p3, Param4 p4) const { + return (*(m_Closure.GetStaticFunction()))(p1, p2, p3, p4); } +}; + +//N=5 +template +class FastDelegate5 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate5 type; + + // Construction and comparison functions + FastDelegate5() { clear(); } + FastDelegate5(const FastDelegate5 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate5 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate5 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate5 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate5 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate5 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate5(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate5(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate5(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5)) { + m_Closure.bindstaticfunc(this, &FastDelegate5::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1, p2, p3, p4, p5); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) const { + return (*(m_Closure.GetStaticFunction()))(p1, p2, p3, p4, p5); } +}; + +//N=6 +template +class FastDelegate6 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate6 type; + + // Construction and comparison functions + FastDelegate6() { clear(); } + FastDelegate6(const FastDelegate6 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate6 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate6 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate6 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate6 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate6 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate6(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate6(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate6(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6)) { + m_Closure.bindstaticfunc(this, &FastDelegate6::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1, p2, p3, p4, p5, p6); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) const { + return (*(m_Closure.GetStaticFunction()))(p1, p2, p3, p4, p5, p6); } +}; + +//N=7 +template +class FastDelegate7 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate7 type; + + // Construction and comparison functions + FastDelegate7() { clear(); } + FastDelegate7(const FastDelegate7 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate7 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate7 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate7 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate7 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate7 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate7(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate7(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate7(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7)) { + m_Closure.bindstaticfunc(this, &FastDelegate7::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1, p2, p3, p4, p5, p6, p7); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) const { + return (*(m_Closure.GetStaticFunction()))(p1, p2, p3, p4, p5, p6, p7); } +}; + +//N=8 +template +class FastDelegate8 { +private: + typedef typename detail::DefaultVoidToVoid::type DesiredRetType; + typedef DesiredRetType (*StaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8); + typedef RetType (*UnvoidStaticFunctionPtr)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8); + typedef RetType (detail::GenericClass::*GenericMemFn)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8); + typedef detail::ClosurePtr ClosureType; + ClosureType m_Closure; +public: + // Typedefs to aid generic programming + typedef FastDelegate8 type; + + // Construction and comparison functions + FastDelegate8() { clear(); } + FastDelegate8(const FastDelegate8 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + void operator = (const FastDelegate8 &x) { + m_Closure.CopyFrom(this, x.m_Closure); } + bool operator ==(const FastDelegate8 &x) const { + return m_Closure.IsEqual(x.m_Closure); } + bool operator !=(const FastDelegate8 &x) const { + return !m_Closure.IsEqual(x.m_Closure); } + bool operator <(const FastDelegate8 &x) const { + return m_Closure.IsLess(x.m_Closure); } + bool operator >(const FastDelegate8 &x) const { + return x.m_Closure.IsLess(m_Closure); } + // Binding to non-const member functions + template < class X, class Y > + FastDelegate8(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) ) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8)) { + m_Closure.bindmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Binding to const member functions. + template < class X, class Y > + FastDelegate8(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + template < class X, class Y > + inline void bind(const Y *pthis, DesiredRetType (X::* function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) const) { + m_Closure.bindconstmemfunc(detail::implicit_cast(pthis), function_to_bind); } + // Static functions. We convert them into a member function call. + // This constructor also provides implicit conversion + FastDelegate8(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) ) { + bind(function_to_bind); } + // for efficiency, prevent creation of a temporary + void operator = (DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) ) { + bind(function_to_bind); } + inline void bind(DesiredRetType (*function_to_bind)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8)) { + m_Closure.bindstaticfunc(this, &FastDelegate8::InvokeStaticFunction, + function_to_bind); } + // Invoke the delegate + RetType operator() (Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) const { + return (m_Closure.GetClosureThis()->*(m_Closure.GetClosureMemPtr()))(p1, p2, p3, p4, p5, p6, p7, p8); } + // Implicit conversion to "bool" using the safe_bool idiom +private: + typedef struct SafeBoolStruct { + int a_data_pointer_to_this_is_0_on_buggy_compilers; + StaticFunctionPtr m_nonzero; + } UselessTypedef; + typedef StaticFunctionPtr SafeBoolStruct::*unspecified_bool_type; +public: + operator unspecified_bool_type() const { + return empty()? 0: &SafeBoolStruct::m_nonzero; + } + // necessary to allow ==0 to work despite the safe_bool idiom + inline bool operator==(StaticFunctionPtr funcptr) { + return m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator!=(StaticFunctionPtr funcptr) { + return !m_Closure.IsEqualToStaticFuncPtr(funcptr); } + inline bool operator ! () const { // Is it bound to anything? + return !m_Closure; } + inline bool empty() const { + return !m_Closure; } + void clear() { m_Closure.clear();} + // Conversion to and from the DelegateMemento storage class + const DelegateMemento & GetMemento() { return m_Closure; } + void SetMemento(const DelegateMemento &any) { m_Closure.CopyFrom(this, any); } + +private: // Invoker for static functions + RetType InvokeStaticFunction(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) const { + return (*(m_Closure.GetStaticFunction()))(p1, p2, p3, p4, p5, p6, p7, p8); } +}; + + +//////////////////////////////////////////////////////////////////////////////// +// Fast Delegates, part 4: +// +// FastDelegate<> class (Original author: Jody Hagins) +// Allows boost::function style syntax like: +// FastDelegate< double (int, long) > +// instead of: +// FastDelegate2< int, long, double > +// +//////////////////////////////////////////////////////////////////////////////// + +#ifdef FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX + +// Declare FastDelegate as a class template. It will be specialized +// later for all number of arguments. +template +class FastDelegate; + +//N=0 +// Specialization to allow use of +// FastDelegate< R ( ) > +// instead of +// FastDelegate0 < R > +template +class FastDelegate< R ( ) > + // Inherit from FastDelegate0 so that it can be treated just like a FastDelegate0 + : public FastDelegate0 < R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate0 < R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=1 +// Specialization to allow use of +// FastDelegate< R ( Param1 ) > +// instead of +// FastDelegate1 < Param1, R > +template +class FastDelegate< R ( Param1 ) > + // Inherit from FastDelegate1 so that it can be treated just like a FastDelegate1 + : public FastDelegate1 < Param1, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate1 < Param1, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=2 +// Specialization to allow use of +// FastDelegate< R ( Param1, Param2 ) > +// instead of +// FastDelegate2 < Param1, Param2, R > +template +class FastDelegate< R ( Param1, Param2 ) > + // Inherit from FastDelegate2 so that it can be treated just like a FastDelegate2 + : public FastDelegate2 < Param1, Param2, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate2 < Param1, Param2, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1, Param2 p2 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=3 +// Specialization to allow use of +// FastDelegate< R ( Param1, Param2, Param3 ) > +// instead of +// FastDelegate3 < Param1, Param2, Param3, R > +template +class FastDelegate< R ( Param1, Param2, Param3 ) > + // Inherit from FastDelegate3 so that it can be treated just like a FastDelegate3 + : public FastDelegate3 < Param1, Param2, Param3, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate3 < Param1, Param2, Param3, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1, Param2 p2, Param3 p3 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=4 +// Specialization to allow use of +// FastDelegate< R ( Param1, Param2, Param3, Param4 ) > +// instead of +// FastDelegate4 < Param1, Param2, Param3, Param4, R > +template +class FastDelegate< R ( Param1, Param2, Param3, Param4 ) > + // Inherit from FastDelegate4 so that it can be treated just like a FastDelegate4 + : public FastDelegate4 < Param1, Param2, Param3, Param4, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate4 < Param1, Param2, Param3, Param4, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=5 +// Specialization to allow use of +// FastDelegate< R ( Param1, Param2, Param3, Param4, Param5 ) > +// instead of +// FastDelegate5 < Param1, Param2, Param3, Param4, Param5, R > +template +class FastDelegate< R ( Param1, Param2, Param3, Param4, Param5 ) > + // Inherit from FastDelegate5 so that it can be treated just like a FastDelegate5 + : public FastDelegate5 < Param1, Param2, Param3, Param4, Param5, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate5 < Param1, Param2, Param3, Param4, Param5, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=6 +// Specialization to allow use of +// FastDelegate< R ( Param1, Param2, Param3, Param4, Param5, Param6 ) > +// instead of +// FastDelegate6 < Param1, Param2, Param3, Param4, Param5, Param6, R > +template +class FastDelegate< R ( Param1, Param2, Param3, Param4, Param5, Param6 ) > + // Inherit from FastDelegate6 so that it can be treated just like a FastDelegate6 + : public FastDelegate6 < Param1, Param2, Param3, Param4, Param5, Param6, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate6 < Param1, Param2, Param3, Param4, Param5, Param6, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=7 +// Specialization to allow use of +// FastDelegate< R ( Param1, Param2, Param3, Param4, Param5, Param6, Param7 ) > +// instead of +// FastDelegate7 < Param1, Param2, Param3, Param4, Param5, Param6, Param7, R > +template +class FastDelegate< R ( Param1, Param2, Param3, Param4, Param5, Param6, Param7 ) > + // Inherit from FastDelegate7 so that it can be treated just like a FastDelegate7 + : public FastDelegate7 < Param1, Param2, Param3, Param4, Param5, Param6, Param7, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate7 < Param1, Param2, Param3, Param4, Param5, Param6, Param7, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + +//N=8 +// Specialization to allow use of +// FastDelegate< R ( Param1, Param2, Param3, Param4, Param5, Param6, Param7, Param8 ) > +// instead of +// FastDelegate8 < Param1, Param2, Param3, Param4, Param5, Param6, Param7, Param8, R > +template +class FastDelegate< R ( Param1, Param2, Param3, Param4, Param5, Param6, Param7, Param8 ) > + // Inherit from FastDelegate8 so that it can be treated just like a FastDelegate8 + : public FastDelegate8 < Param1, Param2, Param3, Param4, Param5, Param6, Param7, Param8, R > +{ +public: + // Make using the base type a bit easier via typedef. + typedef FastDelegate8 < Param1, Param2, Param3, Param4, Param5, Param6, Param7, Param8, R > BaseType; + + // Allow users access to the specific type of this delegate. + typedef FastDelegate SelfType; + + // Mimic the base class constructors. + FastDelegate() : BaseType() { } + + template < class X, class Y > + FastDelegate(Y * pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 )) + : BaseType(pthis, function_to_bind) { } + + template < class X, class Y > + FastDelegate(const Y *pthis, + R (X::* function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 ) const) + : BaseType(pthis, function_to_bind) + { } + + FastDelegate(R (*function_to_bind)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 )) + : BaseType(function_to_bind) { } + void operator = (const BaseType &x) { + *static_cast(this) = x; } +}; + + +#endif //FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX + +//////////////////////////////////////////////////////////////////////////////// +// Fast Delegates, part 5: +// +// MakeDelegate() helper function +// +// MakeDelegate(&x, &X::func) returns a fastdelegate of the type +// necessary for calling x.func() with the correct number of arguments. +// This makes it possible to eliminate many typedefs from user code. +// +//////////////////////////////////////////////////////////////////////////////// + +// Also declare overloads of a MakeDelegate() global function to +// reduce the need for typedefs. +// We need separate overloads for const and non-const member functions. +// Also, because of the weird rule about the class of derived member function pointers, +// implicit downcasts may need to be applied later to the 'this' pointer. +// That's why two classes (X and Y) appear in the definitions. Y must be implicitly +// castable to X. + +// Workaround for VC6. VC6 needs void return types converted into DefaultVoid. +// GCC 3.2 and later won't compile this unless it's preceded by 'typename', +// but VC6 doesn't allow 'typename' in this context. +// So, I have to use a macro. + +#ifdef FASTDLGT_VC6 +#define FASTDLGT_RETTYPE detail::VoidToDefaultVoid::type +#else +#define FASTDLGT_RETTYPE RetType +#endif + +//N=0 +template +FastDelegate0 MakeDelegate(Y* x, RetType (X::*func)()) { + return FastDelegate0(x, func); +} + +template +FastDelegate0 MakeDelegate(Y* x, RetType (X::*func)() const) { + return FastDelegate0(x, func); +} + +//N=1 +template +FastDelegate1 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1)) { + return FastDelegate1(x, func); +} + +template +FastDelegate1 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1) const) { + return FastDelegate1(x, func); +} + +//N=2 +template +FastDelegate2 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2)) { + return FastDelegate2(x, func); +} + +template +FastDelegate2 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2) const) { + return FastDelegate2(x, func); +} + +//N=3 +template +FastDelegate3 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3)) { + return FastDelegate3(x, func); +} + +template +FastDelegate3 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3) const) { + return FastDelegate3(x, func); +} + +//N=4 +template +FastDelegate4 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4)) { + return FastDelegate4(x, func); +} + +template +FastDelegate4 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4) const) { + return FastDelegate4(x, func); +} + +//N=5 +template +FastDelegate5 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5)) { + return FastDelegate5(x, func); +} + +template +FastDelegate5 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5) const) { + return FastDelegate5(x, func); +} + +//N=6 +template +FastDelegate6 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6)) { + return FastDelegate6(x, func); +} + +template +FastDelegate6 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6) const) { + return FastDelegate6(x, func); +} + +//N=7 +template +FastDelegate7 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7)) { + return FastDelegate7(x, func); +} + +template +FastDelegate7 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7) const) { + return FastDelegate7(x, func); +} + +//N=8 +template +FastDelegate8 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8)) { + return FastDelegate8(x, func); +} + +template +FastDelegate8 MakeDelegate(Y* x, RetType (X::*func)(Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8) const) { + return FastDelegate8(x, func); +} + + + // clean up after ourselves... +#undef FASTDLGT_RETTYPE + +} // namespace fastdelegate + +#endif // !defined(FASTDELEGATE_H) + diff --git a/libs/Common/FastDelegateBind.h b/libs/Common/FastDelegateBind.h new file mode 100644 index 0000000..95b017d --- /dev/null +++ b/libs/Common/FastDelegateBind.h @@ -0,0 +1,240 @@ +// FastDelegateBind.h +// Helper file for FastDelegates. Provides bind() function, enabling +// FastDelegates to be rapidly compared to programs using boost::function and boost::bind. +// +// Documentation is found at http://www.codeproject.com/cpp/FastDelegate.asp +// +// Original author: Jody Hagins. +// Minor changes by Don Clugston. +// +// Warning: The arguments to 'bind' are ignored! No actual binding is performed. +// The behaviour is equivalent to boost::bind only when the basic placeholder +// arguments _1, _2, _3, etc are used in order. +// +// HISTORY: +// 1.4 Dec 2004. Initial release as part of FastDelegate 1.4. + + +#ifndef FASTDELEGATEBIND_H +#define FASTDELEGATEBIND_H + +//////////////////////////////////////////////////////////////////////////////// +// FastDelegate bind() +// +// bind() helper function for boost compatibility. +// (Original author: Jody Hagins). +// +// Add another helper, so FastDelegate can be a dropin replacement +// for boost::bind (in a fair number of cases). +// Note the ellipsis, because boost::bind() takes place holders +// but FastDelegate does not care about them. Getting the place holder +// mechanism to work, and play well with boost is a bit tricky, so +// we do the "easy" thing... +// Assume we have the following code... +// using boost::bind; +// bind(&Foo:func, &foo, _1, _2); +// we should be able to replace the "using" with... +// using fastdelegate::bind; +// and everything should work fine... +//////////////////////////////////////////////////////////////////////////////// + +#ifdef FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX + +namespace fastdelegate { + +//N=0 +template +FastDelegate< RetType ( ) > +bind( + RetType (X::*func)( ), + Y * y, + ...) +{ + return FastDelegate< RetType ( ) >(y, func); +} + +template +FastDelegate< RetType ( ) > +bind( + RetType (X::*func)( ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( ) >(y, func); +} + +//N=1 +template +FastDelegate< RetType ( Param1 p1 ) > +bind( + RetType (X::*func)( Param1 p1 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1 ) > +bind( + RetType (X::*func)( Param1 p1 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1 ) >(y, func); +} + +//N=2 +template +FastDelegate< RetType ( Param1 p1, Param2 p2 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1, Param2 p2 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2 ) >(y, func); +} + +//N=3 +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3 ) >(y, func); +} + +//N=4 +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4 ) >(y, func); +} + +//N=5 +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5 ) >(y, func); +} + +//N=6 +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6 ) >(y, func); +} + +//N=7 +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7 ) >(y, func); +} + +//N=8 +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 ), + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 ) >(y, func); +} + +template +FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 ) > +bind( + RetType (X::*func)( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 ) const, + Y * y, + ...) +{ + return FastDelegate< RetType ( Param1 p1, Param2 p2, Param3 p3, Param4 p4, Param5 p5, Param6 p6, Param7 p7, Param8 p8 ) >(y, func); +} + + +#endif //FASTDELEGATE_ALLOW_FUNCTION_TYPE_SYNTAX + +} // namespace fastdelegate + +#endif // !defined(FASTDELEGATEBIND_H) + diff --git a/libs/Common/FastDelegateCPP11.h b/libs/Common/FastDelegateCPP11.h new file mode 100644 index 0000000..26a565b --- /dev/null +++ b/libs/Common/FastDelegateCPP11.h @@ -0,0 +1,379 @@ +/** \file SRDelegate.hpp + * + * This is a C++11 implementation by janezz55(code.google) for the original "The Impossibly Fast C++ Delegates" authored by Sergey Ryazanov. + * + * This is a copy checkouted from https://code.google.com/p/cpppractice/source/browse/trunk/delegate.hpp on 2014/06/07. + * Last change in the chunk was r370 on Feb 9, 2014. + * + * The following modifications were added by Benjamin YanXiang Huang + * - replace light_ptr with std::shared_ptr + * - renamed src file + * + * Reference: + * - http://codereview.stackexchange.com/questions/14730/impossibly-fast-delegate-in-c11 + * - http://www.codeproject.com/Articles/11015/The-Impossibly-Fast-C-Delegates + * - https://code.google.com/p/cpppractice/source/browse/trunk/delegate.hpp +*/ + +#pragma once +#ifndef SRDELEGATE_HPP +#define SRDELEGATE_HPP + +#include +#include +#include +#include +#include +#include + +// VC work around for constexpr and noexcept: VC2013 and below do not support these 2 keywords +#if defined(_MSC_VER) && (_MSC_VER <= 1800) +#define constexpr const +#define noexcept throw() +#endif + +namespace fastdelegate +{ + +template class delegate; + +template +class delegate +{ + using stub_ptr_type = R(*)(void *, A&&...); + + delegate(void * const o, stub_ptr_type const m) noexcept : object_ptr_(o), stub_ptr_(m) {} + +public: + delegate(void) = default; + + delegate(delegate const &) = default; + + delegate(delegate && d) + : object_ptr_(d.object_ptr_), stub_ptr_(d.stub_ptr_), deleter_(d.deleter_), store_(d.store_), store_size_(d.store_size_) + { + d.object_ptr_ = nullptr; + d.stub_ptr_ = nullptr; + d.deleter_ = nullptr; + d.store_ = nullptr; + d.store_size_ = 0; + } + + delegate(::std::nullptr_t const) noexcept : delegate() { } + + template ::value, C>::type> + explicit delegate(C const * const o) noexcept : + object_ptr_(const_cast(o)) + {} + + template {}>::type> + explicit delegate(C const & o) noexcept : + object_ptr_(const_cast(&o)) + {} + + template + delegate(C * const object_ptr, R(C::* const method_ptr)(A...)) + { + *this = from(object_ptr, method_ptr); + } + + template + delegate(C * const object_ptr, R(C::* const method_ptr)(A...) const) + { + *this = from(object_ptr, method_ptr); + } + + template + delegate(C & object, R(C::* const method_ptr)(A...)) + { + *this = from(object, method_ptr); + } + + template + delegate(C const & object, R(C::* const method_ptr)(A...) const) + { + *this = from(object, method_ptr); + } + + template < + typename T, + typename = typename ::std::enable_if::type>::value>::type + > + delegate(T&& f) + : store_(operator new(sizeof(typename ::std::decay::type)) + , functor_deleter::type>) + , store_size_(sizeof(typename ::std::decay::type)) + { + using functor_type = typename ::std::decay::type; + + new(store_.get()) functor_type(::std::forward(f)); + object_ptr_ = store_.get(); + + stub_ptr_ = functor_stub; + deleter_ = deleter_stub; + } + + delegate & operator=(delegate const &) = default; + + delegate & operator=(delegate&& d) + { + object_ptr_ = d.object_ptr_; + stub_ptr_ = d.stub_ptr_; + deleter_ = d.deleter_; + store_ = d.store_; + store_size_ = d.store_size_; + + d.object_ptr_ = nullptr; + d.stub_ptr_ = nullptr; + d.deleter_ = nullptr; + d.store_ = nullptr; + d.store_size_ = 0; + + return *this; + } + + template + delegate & operator=(R(C::* const rhs)(A...)) + { + return *this = from(static_cast(object_ptr_), rhs); + } + + template + delegate & operator=(R(C::* const rhs)(A...) const) + { + return *this = from(static_cast(object_ptr_), rhs); + } + + template < + typename T + , typename = typename ::std::enable_if::type>::value>::type + > + delegate & operator=(T&& f) + { + using functor_type = typename ::std::decay::type; + + if ((sizeof(functor_type) > store_size_) || !store_.unique()) + { + store_.reset(operator new(sizeof(functor_type)), functor_deleter); + store_size_ = sizeof(functor_type); + } + else + deleter_(store_.get()); + + new(store_.get()) functor_type(::std::forward(f)); + object_ptr_ = store_.get(); + + stub_ptr_ = functor_stub; + deleter_ = deleter_stub; + + return *this; + } + + template + static delegate from(void) noexcept + { + return { nullptr, function_stub }; + } + + template + static delegate from(C * const object_ptr) noexcept + { + return { object_ptr, method_stub }; + } + + template + static delegate from(C const * const object_ptr) noexcept + { + return { const_cast(object_ptr), const_method_stub }; + } + + template + static delegate from(C & object) noexcept + { + return { &object, method_stub }; + } + + template + static delegate from(C const & object) noexcept + { + return { const_cast(&object), const_method_stub }; + } + + template + static delegate from(T && f) + { + return ::std::forward(f); + } + + static delegate from(R(* const function_ptr)(A...)) + { + return function_ptr; + } + + template + using member_pair = ::std::pair; + + template + using const_member_pair = ::std::pair; + + template + static delegate from(C * const object_ptr, R(C::* const method_ptr)(A...)) + { + return member_pair(object_ptr, method_ptr); + } + + template + static delegate from(C const * const object_ptr, R(C::* const method_ptr)(A...) const) + { + return const_member_pair(object_ptr, method_ptr); + } + + template + static delegate from(C & object, R(C::* const method_ptr)(A...)) + { + return member_pair(&object, method_ptr); + } + + template + static delegate from(C const & object, R(C::* const method_ptr)(A...) const) + { + return const_member_pair(&object, method_ptr); + } + + void reset(void) + { + stub_ptr_ = nullptr; + store_.reset(); + } + + void reset_stub(void) noexcept { stub_ptr_ = nullptr; } + + void swap(delegate & other) noexcept { ::std::swap(*this, other); } + + bool operator==(delegate const & rhs) const noexcept + { + // comparison between functor and non-functor is left as undefined at the moment. + if (store_size_ && rhs.store_size_) // both functors + return (std::memcmp(store_.get(), rhs.store_.get(), store_size_) == 0) && (stub_ptr_ == rhs.stub_ptr_); + return (object_ptr_ == rhs.object_ptr_) && (stub_ptr_ == rhs.stub_ptr_); + } + + bool operator!=(delegate const & rhs) const noexcept + { + return !operator==(rhs); + } + + bool operator<(delegate const & rhs) const noexcept + { + return (object_ptr_ < rhs.object_ptr_) || + ((object_ptr_ == rhs.object_ptr_) && (stub_ptr_ < rhs.stub_ptr_)); + } + + bool operator==(::std::nullptr_t const) const noexcept + { + return !stub_ptr_; + } + + bool operator!=(::std::nullptr_t const) const noexcept + { + return stub_ptr_; + } + + explicit operator bool() const noexcept + { + return stub_ptr_; + } + + R operator()(A... args) const + { + // assert(stub_ptr); + return stub_ptr_(object_ptr_, ::std::forward(args)...); + } + +private: + friend struct ::std::hash; + + using deleter_type = void (*)(void *); + + void * object_ptr_ = nullptr; + stub_ptr_type stub_ptr_ {}; + + deleter_type deleter_ = nullptr; + + ::std::shared_ptr store_ = nullptr; + ::std::size_t store_size_ = 0; + + template + static void functor_deleter(void * const p) + { + static_cast(p)->~T(); + operator delete(p); + } + + template + static void deleter_stub(void * const p) + { + static_cast(p)->~T(); + } + + template + static R function_stub(void * const, A && ... args) + { + return function_ptr(::std::forward(args)...); + } + + template + static R method_stub(void * const object_ptr, A && ... args) + { + return (static_cast(object_ptr)->*method_ptr)(::std::forward(args)...); + } + + template + static R const_method_stub(void * const object_ptr, A && ... args) + { + return (static_cast(object_ptr)->*method_ptr)(::std::forward(args)...); + } + + template + struct is_member_pair : ::std::false_type { }; + + template + struct is_member_pair< ::std::pair > : ::std::true_type {}; + + template + struct is_const_member_pair : ::std::false_type { }; + + template + struct is_const_member_pair< ::std::pair > : ::std::true_type {}; + + template + static typename ::std::enable_if::value || is_const_member_pair::value), R>::type + functor_stub(void * const object_ptr, A && ... args) + { + return (*static_cast(object_ptr))(::std::forward(args)...); + } + + template + static typename ::std::enable_if::value || is_const_member_pair::value, R>::type + functor_stub(void * const object_ptr, A && ... args) + { + return (static_cast(object_ptr)->first->*static_cast(object_ptr)->second)(::std::forward(args)...); + } +}; + +} + +namespace std +{ +template +struct hash<::fastdelegate::delegate > +{ + size_t operator()(::fastdelegate::delegate const & d) const noexcept + { + auto const seed(hash()(d.object_ptr_)); + return hash()(d.stub_ptr_) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } +}; +} + +#endif // SRDELEGATE_HPP diff --git a/libs/Common/File.h b/libs/Common/File.h new file mode 100644 index 0000000..2e431bc --- /dev/null +++ b/libs/Common/File.h @@ -0,0 +1,686 @@ +//////////////////////////////////////////////////////////////////// +// File.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_FILE_H__ +#define __SEACAVE_FILE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Streams.h" + +// Under both Windows and Unix, the stat function is used for classification + +// Under Gnu/Linux, the following classifications are defined +// source: Gnu/Linux man page for stat(2) http://linux.die.net/man/2/stat +// S_IFMT 0170000 bitmask for the file type bitfields +// S_IFSOCK 0140000 socket (Note this overlaps with S_IFDIR) +// S_IFLNK 0120000 symbolic link +// S_IFREG 0100000 regular file +// S_IFBLK 0060000 block device +// S_IFDIR 0040000 directory +// S_IFCHR 0020000 character device +// S_IFIFO 0010000 FIFO +// There are also some Posix-standard macros: +// S_ISREG(m) is it a regular file? +// S_ISDIR(m) directory? +// S_ISCHR(m) character device? +// S_ISBLK(m) block device? +// S_ISFIFO(m) FIFO (named pipe)? +// S_ISLNK(m) symbolic link? (Not in POSIX.1-1996.) +// S_ISSOCK(m) socket? (Not in POSIX.1-1996.) +// Under Windows, the following are defined: +// source: Header file sys/stat.h distributed with Visual Studio 10 +// _S_IFMT (S_IFMT) 0xF000 file type mask +// _S_IFREG (S_IFREG) 0x8000 regular +// _S_IFDIR (S_IFDIR) 0x4000 directory +// _S_IFCHR (S_IFCHR) 0x2000 character special +// _S_IFIFO 0x1000 pipe + +#ifdef _MSC_VER +#include +// file type tests are not defined for some reason on Windows despite them providing the stat() function! +#define F_OK 0 +#define X_OK 1 +#define W_OK 2 +#define R_OK 4 +// Posix-style macros for Windows +#ifndef S_ISREG +#define S_ISREG(mode) ((mode & _S_IFMT) == _S_IFREG) +#endif +#ifndef S_ISDIR +#define S_ISDIR(mode) ((mode & _S_IFMT) == _S_IFDIR) +#endif +#ifndef S_ISCHR +#define S_ISCHR(mode) ((mode & _S_IFMT) == _S_IFCHR) +#endif +#ifndef S_ISBLK +#define S_ISBLK(mode) (false) +#endif +#ifndef S_ISFIFO +#define S_ISFIFO(mode) ((mode & _S_IFMT) == _S_IFIFO) +#endif +#ifndef S_ISLNK +#define S_ISLNK(mode) (false) +#endif +#ifndef S_ISSOCK +#define S_ISSOCK(mode) (false) +#endif +#else +#include +#include +#define _taccess access +#endif +#ifdef __APPLE__ +#define fdatasync fsync +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + +// size of the stored file size variable +#ifdef LARGEFILESIZE +#define FILESIZE size_f_t +#else +#define FILESIZE size_t +#endif + +// invalid file handle +#ifdef _MSC_VER +#define FILE_INVALID_HANDLE INVALID_HANDLE_VALUE +#else +#define FILE_INVALID_HANDLE int(-1) +#endif + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class GENERAL_API File : public IOStream { +public: + typedef struct FILEINFO_TYPE { + String path; + FILESIZE size; + DWORD attrib; + } FILEINFO; + typedef cList FileInfoArr; + + typedef enum FMCREATE_TYPE { + OPEN = 0x01, + CREATE = 0x02, + TRUNCATE = 0x04 + } FMCREATE; + + typedef enum FMFLAGS_TYPE { + SYNC = 0x01, + NOBUFFER = 0x02, + RANDOM = 0x04, + SEQUENTIAL = 0x08 + } FMFLAGS; + + inline File() : h(FILE_INVALID_HANDLE) { + #ifndef _RELEASE + breakRead = -1; + breakWrite = -1; + #endif + } + inline File(LPCTSTR aFileName, int access, int mode, int flags=0) : h(FILE_INVALID_HANDLE) { + #ifndef _RELEASE + breakRead = -1; + breakWrite = -1; + #endif + File::open(aFileName, access, mode, flags); + } + + virtual ~File() { + File::close(); + } + + #ifdef _SUPPORT_CPP11 + inline File(File&& rhs) : h(rhs.h) { + #ifndef _RELEASE + breakRead = rhs.breakRead; + breakWrite = rhs.breakWrite; + #endif + rhs.h = FILE_INVALID_HANDLE; + } + + inline File& operator=(File&& rhs) { + h = rhs.h; + #ifndef _RELEASE + breakRead = rhs.breakRead; + breakWrite = rhs.breakWrite; + #endif + rhs.h = FILE_INVALID_HANDLE; + return *this; + } + #endif + + bool isOpen() const { + return h != FILE_INVALID_HANDLE; + } + +#ifdef _MSC_VER + typedef enum FMACCESS_TYPE { + READ = GENERIC_READ, + WRITE = GENERIC_WRITE, + RW = READ | WRITE + } FMACCESS; + + typedef enum FMCHECKACCESS_TYPE { + CA_EXIST = F_OK, // existence + CA_WRITE = W_OK, // write + CA_READ = R_OK, // read + CA_RW = R_OK | W_OK, + } FMCHECKACCESS; + + /** + * Open the file specified. + * If there are errors, h is set to FILE_INVALID_HANDLE. + * Use isOpen() to check. + */ + virtual void open(LPCTSTR aFileName, int access, int mode, int flags=0) { + ASSERT(access == WRITE || access == READ || access == (READ | WRITE)); + + close(); + + DWORD m = 0; + if (mode & OPEN) { + if (mode & CREATE) { + m = (mode & TRUNCATE) ? CREATE_ALWAYS : OPEN_ALWAYS; + } else { + m = (mode & TRUNCATE) ? TRUNCATE_EXISTING : OPEN_EXISTING; + } + } else { + ASSERT(mode & CREATE); + m = (mode & TRUNCATE) ? CREATE_ALWAYS : CREATE_NEW; + } + + DWORD f = 0; + if (flags & SYNC) + f |= FILE_FLAG_WRITE_THROUGH; + if (flags & NOBUFFER) + f |= FILE_FLAG_NO_BUFFERING; + if (flags & RANDOM) + f |= FILE_FLAG_RANDOM_ACCESS; + if (flags & SEQUENTIAL) + f |= FILE_FLAG_SEQUENTIAL_SCAN; + + h = ::CreateFile(aFileName, access, FILE_SHARE_READ, NULL, m, f, NULL); + } + + virtual void close() { + if (isOpen()) { + FlushFileBuffers(h); + CloseHandle(h); + h = FILE_INVALID_HANDLE; + } + } + + uint32_t getLastModified() { + ASSERT(isOpen()); + FILETIME f = {0}; + ::GetFileTime(h, NULL, NULL, &f); + return convertTime(&f); + } + + static uint32_t convertTime(FILETIME* f) { + SYSTEMTIME s = { 1970, 1, 0, 1, 0, 0, 0, 0 }; + FILETIME f2 = {0}; + if (::SystemTimeToFileTime(&s, &f2)) { + uint64_t* a = (uint64_t*)f; + uint64_t* b = (uint64_t*)&f2; + *a -= *b; + *a /= (1000LL*1000LL*1000LL/100LL); // 100ns > s + return (uint32_t)*a; + } + return 0; + } + + size_f_t getSize() const override { + ASSERT(isOpen()); + DWORD x; + DWORD l = ::GetFileSize(h, &x); + if ((l == INVALID_FILE_SIZE) && (GetLastError() != NO_ERROR)) + return SIZE_NA; + return (size_f_t)l | ((size_f_t)x)<<32; + } + + virtual bool setSize(size_f_t newSize) { + const size_f_t pos = getPos(); + if (pos == SIZE_NA) + return false; + if (!setPos(newSize)) + return false; + if (!setEOF()) + return false; + if (!setPos(pos)) + return false; + return true; + } + + size_f_t getPos() const override { + ASSERT(isOpen()); + LONG x = 0; + const DWORD l = ::SetFilePointer(h, 0, &x, FILE_CURRENT); + if (l == INVALID_SET_FILE_POINTER) + return SIZE_NA; + return (size_f_t)l | ((size_f_t)x)<<32; + } + + bool setPos(size_f_t pos) override { + ASSERT(isOpen()); + LONG x = (LONG) (pos>>32); + return (::SetFilePointer(h, (DWORD)(pos & 0xffffffff), &x, FILE_BEGIN) != INVALID_SET_FILE_POINTER); + } + + virtual bool setEndPos(size_f_t pos) { + ASSERT(isOpen()); + LONG x = (LONG) (pos>>32); + return (::SetFilePointer(h, (DWORD)(pos & 0xffffffff), &x, FILE_END) != INVALID_SET_FILE_POINTER); + } + + virtual bool movePos(size_f_t pos) { + ASSERT(isOpen()); + LONG x = (LONG) (pos>>32); + return (::SetFilePointer(h, (DWORD)(pos & 0xffffffff), &x, FILE_CURRENT) != INVALID_SET_FILE_POINTER); + } + + size_t read(void* buf, size_t len) override { + ASSERT(isOpen()); + #ifndef _RELEASE + if (breakRead != (size_t)(-1)) { + if (breakRead <= len) { + ASSERT("FILE::read() break" == NULL); + breakRead = -1; + } else { + breakRead -= len; + } + } + #endif + DWORD x; + if (!::ReadFile(h, buf, (DWORD)len, &x, NULL)) + return STREAM_ERROR; + return x; + } + + size_t write(const void* buf, size_t len) override { + ASSERT(isOpen()); + #ifndef _RELEASE + if (breakWrite != (size_t)(-1)) { + if (breakWrite <= len) { + ASSERT("FILE::write() break" == NULL); + breakWrite = -1; + } else { + breakWrite -= len; + } + } + #endif + DWORD x; + if (!::WriteFile(h, buf, (DWORD)len, &x, NULL)) + return STREAM_ERROR; + ASSERT(x == len); + return x; + } + virtual bool setEOF() { + ASSERT(isOpen()); + return (SetEndOfFile(h) != FALSE); + } + + size_t flush() override { + ASSERT(isOpen()); + return (FlushFileBuffers(h) ? 0 : STREAM_ERROR); + } + + virtual bool getInfo(BY_HANDLE_FILE_INFORMATION* fileInfo) { + ASSERT(isOpen()); + return (GetFileInformationByHandle(h, fileInfo) != FALSE); + } + + static uint32_t getAttrib(LPCTSTR aFileName) { + return GetFileAttributes(aFileName); + } + + static bool setAttrib(LPCTSTR aFileName, uint32_t attribs) { + return (SetFileAttributes(aFileName, attribs) != FALSE); + } + + static void deleteFile(LPCTSTR aFileName) { ::DeleteFile(aFileName); } + static bool renameFile(LPCTSTR source, LPCTSTR target) { + if (!::MoveFile(source, target)) { + // Can't move, try copy/delete... + if (!::CopyFile(source, target, FALSE)) + return false; + deleteFile(source); + } + return true; + } + static bool copyFile(LPCTSTR source, LPCTSTR target) { return ::CopyFile(source, target, FALSE) == TRUE; } + + static size_f_t getSize(LPCTSTR aFileName) { + const HANDLE fh = ::CreateFile(aFileName, FILE_READ_ATTRIBUTES, FILE_SHARE_READ, NULL, OPEN_EXISTING, FILE_FLAG_NO_BUFFERING, NULL); + if (fh == FILE_INVALID_HANDLE) + return SIZE_NA; + DWORD x; + DWORD l = ::GetFileSize(fh, &x); + CloseHandle(fh); + if ((l == INVALID_FILE_SIZE) && (GetLastError() != NO_ERROR)) + return SIZE_NA; + return (((size_f_t)l) | (((size_f_t)x)<<32)); + } + + + static size_f_t findFiles(const String& _strPath, const String& strMask, bool bProcessSubdir, FileInfoArr& arrFiles) { + // List all the files. + WIN32_FIND_DATA fd; + HANDLE hFind; + size_f_t totalSize = 0; + String strPath(_strPath); + Util::ensureFolderSlash(strPath); + //Find all the files in this folder. + hFind = FindFirstFile((strPath + strMask).c_str(), &fd); + if (hFind != FILE_INVALID_HANDLE) { + do { + // this is a file that can be used + if (fd.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) + continue; + // Store the file name. + FILEINFO& fileInfo = arrFiles.AddEmpty(); + fileInfo.path = strPath + fd.cFileName; + #ifdef LARGEFILESIZE + fileInfo.size = (((size_f_t)fd.nFileSizeLow) | (((size_f_t)fd.nFileSizeHigh)<<32)); + #else + fileInfo.size = fd.nFileSizeLow; + #endif + fileInfo.attrib = fd.dwFileAttributes; + totalSize += fileInfo.size; + } while (FindNextFile(hFind, &fd)); + FindClose(hFind); + } + //Process the subfolders also... + if (!bProcessSubdir) + return totalSize; + hFind = FindFirstFile((strPath + '*').c_str(), &fd); + if (hFind != FILE_INVALID_HANDLE) { + do { + // if SUBDIR then process that too + if (!(fd.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)) + continue; + if (!_tcscmp(fd.cFileName, _T("."))) + continue; + if (!_tcscmp(fd.cFileName, _T(".."))) + continue; + // Process all subfolders recursively + totalSize += findFiles(strPath + fd.cFileName + PATH_SEPARATOR, strMask, true, arrFiles); + } while (FindNextFile(hFind, &fd)); + FindClose(hFind); + } + return totalSize; + } + +#else // _MSC_VER + + typedef enum FMACCESS_TYPE { + READ = 0x01, + WRITE = 0x02, + RW = READ | WRITE, + } FMACCESS; + + typedef enum FMCHECKACCESS_TYPE { + CA_EXIST = F_OK, // existence + CA_WRITE = W_OK, // write + CA_READ = R_OK, // read + CA_RW = R_OK | W_OK, + CA_EXEC = X_OK, // execute + } FMCHECKACCESS; + + /** + * Open the file specified. + * If there are errors, h is set to FILE_INVALID_HANDLE. + * Use isOpen() to check. + */ + virtual void open(LPCTSTR aFileName, int access, int mode, int flags=0) { + ASSERT(access == WRITE || access == READ || access == (READ | WRITE)); + + close(); + + int m = 0; + if (access == READ) + m |= O_RDONLY; + else if (access == WRITE) + m |= O_WRONLY; + else + m |= O_RDWR; + + if (mode & CREATE) + m |= O_CREAT; + if (mode & TRUNCATE) + m |= O_TRUNC; + + if (flags & SYNC) + m |= O_DSYNC; + #ifndef __APPLE__ + if (flags & NOBUFFER) + m |= O_DIRECT; + #endif + h = ::open(aFileName, m, S_IRUSR | S_IWUSR); + } + + virtual void close() { + if (h != FILE_INVALID_HANDLE) { + ::close(h); + h = FILE_INVALID_HANDLE; + } + } + + uint32_t getLastModified() { + ASSERT(isOpen()); + struct stat s; + if (::fstat(h, &s) == -1) + return 0; + return (uint32_t)s.st_mtime; + } + + size_f_t getSize() const override { + ASSERT(isOpen()); + struct stat s; + if (::fstat(h, &s) == -1) + return SIZE_NA; + return (size_f_t)s.st_size; + } + + size_f_t getPos() const override { + ASSERT(isOpen()); + return (size_f_t)lseek(h, 0, SEEK_CUR); + } + + bool setPos(size_f_t pos) override { ASSERT(isOpen()); return lseek(h, (off_t)pos, SEEK_SET) != (off_t)-1; } + virtual bool setEndPos(size_f_t pos) { ASSERT(isOpen()); return lseek(h, (off_t)pos, SEEK_END) != (off_t)-1; } + virtual bool movePos(size_f_t pos) { ASSERT(isOpen()); return lseek(h, (off_t)pos, SEEK_CUR) != (off_t)-1; } + + size_t read(void* buf, size_t len) override { + ASSERT(isOpen()); + #ifndef _RELEASE + if (breakRead != (size_t)(-1)) { + if (breakRead <= len) { + ASSERT("FILE::read() break" == NULL); + breakRead = -1; + } else { + breakRead -= len; + } + } + #endif + ssize_t x = ::read(h, buf, len); + if (x == -1) + return STREAM_ERROR; + return (size_t)x; + } + + size_t write(const void* buf, size_t len) override { + ASSERT(isOpen()); + #ifndef _RELEASE + if (breakWrite != (size_t)(-1)) { + if (breakWrite <= len) { + ASSERT("FILE::write() break" == NULL); + breakWrite = -1; + } else { + breakWrite -= len; + } + } + #endif + ssize_t x = ::write(h, buf, len); + if (x == -1) + return STREAM_ERROR; + if (x < (ssize_t)len) + return STREAM_ERROR; + return x; + } + + virtual bool setEOF() { + ASSERT(isOpen()); + return (ftruncate(h, (off_t)getPos()) != -1); + } + virtual bool setSize(size_f_t newSize) { + ASSERT(isOpen()); + return (ftruncate(h, (off_t)newSize) != -1); + } + + size_t flush() override { + ASSERT(isOpen()); + return fdatasync(h); + } + + static void deleteFile(LPCTSTR aFileName) { ::remove(aFileName); } + static bool renameFile(LPCTSTR source, LPCTSTR target) { return ::rename(source, target) == 0; } + static bool copyFile(LPCTSTR source, LPCTSTR target) { + std::ifstream src(source, std::ios::binary); + if (!src.is_open()) + return false; + std::ofstream dst(target, std::ios::binary); + if (!dst.is_open()) + return false; + dst << src.rdbuf(); + return true; + } + + static size_f_t getSize(LPCTSTR aFileName) { + struct stat buf; + if (stat(aFileName, &buf) != 0) + return SIZE_NA; + return buf.st_size; + } + +#endif // _MSC_VER + + // test for whether there's something (i.e. folder or file) with this name + // and what access mode is supported + static bool isPresent(LPCTSTR path) { + struct stat buf; + return stat(path, &buf) == 0; + } + static bool access(LPCTSTR path, int mode=CA_EXIST) { + return ::_taccess(path, mode) == 0; + } + // test for whether there's something present and its a folder + static bool isFolder(LPCTSTR path) { + struct stat buf; + if (!(stat(path, &buf) == 0)) + return false; + // If the object is present, see if it is a directory + // this is the Posix-approved way of testing + return S_ISDIR(buf.st_mode); + } + // test for whether there's something present and its a file + // a file can be a regular file, a symbolic link, a FIFO or a socket, but not a device + static bool isFile(LPCTSTR path) { + struct stat buf; + if (!(stat(path, &buf) == 0)) + return false; + // If the object is present, see if it is a file or file-like object + // Note that devices are neither folders nor files + // this is the Posix-approved way of testing + return S_ISREG(buf.st_mode) || S_ISLNK(buf.st_mode) || S_ISSOCK(buf.st_mode) || S_ISFIFO(buf.st_mode); + } + + // time the file was originally created + static time_t getCreated(LPCTSTR path) { + struct stat buf; + if (stat(path, &buf) != 0) return 0; + return buf.st_ctime; + } + // time the file was last modified + static time_t getModified(LPCTSTR path) { + struct stat buf; + if (stat(path, &buf) != 0) return 0; + return buf.st_mtime; + } + // time the file was accessed + static time_t getAccessed(LPCTSTR path) { + struct stat buf; + if (stat(path, &buf) != 0) return 0; + return buf.st_atime; + } + + // set the current folder + static bool setCurrentFolder(LPCTSTR path) { + if (!isFolder(path)) + return false; + #ifdef _MSC_VER + // Windows implementation - this returns non-zero for success + return (SetCurrentDirectory(path) != 0); + #else + // Unix implementation - this returns zero for success + return (chdir(path) == 0); + #endif + } + + template + inline size_t write(const VECTOR& arr) { + const typename VECTOR::IDX nSize(arr.GetSize()); + size_t nBytes(write(&nSize, sizeof(typename VECTOR::IDX))); + nBytes += write(arr.GetData(), arr.GetDataSize()); + return nBytes; + } + + template + inline size_t read(VECTOR& arr) { + typename VECTOR::IDX nSize; + size_t nBytes(read(&nSize, sizeof(typename VECTOR::IDX))); + arr.Resize(nSize); + nBytes += read(arr.GetData(), arr.GetDataSize()); + return nBytes; + } + + enum { LAYER_ID_IN=3 }; + InputStream* getInputStream(int typ=InputStream::LAYER_ID_IN) override { return (typ == LAYER_ID_IN ? static_cast(this) : IOStream::getInputStream(typ)); } + enum { LAYER_ID_OUT=3 }; + OutputStream* getOutputStream(int typ=OutputStream::LAYER_ID_OUT) override { return (typ == LAYER_ID_OUT ? static_cast(this) : IOStream::getOutputStream(typ)); } + +protected: + #ifdef _MSC_VER + HANDLE h; + #else + int h; + #endif + +public: + #ifndef _RELEASE + size_t breakRead; + size_t breakWrite; + #endif + +private: + File(const File&); + File& operator=(const File&); +}; +typedef File* LPFILE; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_FILE_H__ diff --git a/libs/Common/Filters.h b/libs/Common/Filters.h new file mode 100644 index 0000000..ae3d385 --- /dev/null +++ b/libs/Common/Filters.h @@ -0,0 +1,612 @@ +//////////////////////////////////////////////////////////////////// +// Filters.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_FILTERS_H__ +#define __SEACAVE_FILTERS_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Streams.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +template +class BufferedInputStream : public LayerInputStream { +public: + typedef LayerInputStream Base; + + BufferedInputStream(InputStream* aStream, size_t aBufSize) + : Base(aStream), buf(new uint8_t[aBufSize]), bufSize(aBufSize), cache(0), pos(0) { ASSERT(aBufSize > 0); } + virtual ~BufferedInputStream() { + delete[] buf; + } + + size_t read(void* wbuf, size_t len) override { + uint8_t* b = (uint8_t*)wbuf; + const size_t l2 = len; + do { + ASSERT(pos <= cache); + if (pos == cache) { + if (len >= bufSize) { + const size_t r = Base::read(b, len); + if (r == STREAM_ERROR) + return STREAM_ERROR; + return l2 - len + r; + } + pos = 0; + switch (cache = Base::read(buf, bufSize)) { + case 0: + return l2 - len; + case STREAM_ERROR: + return STREAM_ERROR; + } + } + const size_t n = MINF(cache - pos, len); + memcpy(b, buf + pos, n); + b += n; + pos += n; + len -= n; + } while (len > 0); + return l2; + } + + bool setPos(size_f_t wpos) override { + pos = cache = 0; + return Base::setPos(wpos); + } + + size_f_t getPos() const override { + const size_f_t r = Base::getPos(); + if (r == SIZE_NA) + return SIZE_NA; + return r-(cache-pos); + } + + enum { LAYER_ID_IN=1 }; + InputStream* getInputStream(int typ=InputStream::LAYER_ID_IN) override { return (typ == LAYER_ID_IN ? static_cast(this) : Base::getInputStream(typ)); } + +private: + uint8_t* const buf; + const size_t bufSize; + size_t cache; + size_t pos; +}; + + +template +class BufferedOutputStream : public LayerOutputStream { +public: + typedef LayerOutputStream Base; + + BufferedOutputStream(OutputStream* aStream, size_t aBufSize) + : Base(aStream), buf(new uint8_t[aBufSize]), bufSize(aBufSize), pos(0) { } + virtual ~BufferedOutputStream() { + flush(); + delete[] buf; + } + + size_t write(const void* wbuf, size_t len) override { + if (len < bufSize - pos) { + memcpy(buf + pos, (const uint8_t*)wbuf, len); + pos += len; + return len; + } + uint8_t* b = (uint8_t*)wbuf; + const size_t l2 = len; + do { + if (pos == bufSize) { + if (Base::write(buf, bufSize) == STREAM_ERROR) + return STREAM_ERROR; + pos = 0; + if (len < bufSize) { + if (Base::write(b, len) == STREAM_ERROR) + return STREAM_ERROR; + break; + } + } + const size_t n = MINF(bufSize - pos, len); + memcpy(buf + pos, b, n); + b += n; + pos += n; + len -= n; + } while (len > 0); + return l2; + } + + size_f_t getSize() const override { + const size_f_t r = Base::getSize(); + if (r == SIZE_NA) + return SIZE_NA; + return r + pos; + } + + bool setPos(size_f_t wpos) override { + if (pos > 0) { + const size_t ret = Base::write(buf, pos); + pos = 0; + if (ret == STREAM_ERROR) + return false; + } + return Base::setPos(wpos); + } + + size_f_t getPos() const override { + const size_f_t r = Base::getPos(); + if (r == SIZE_NA) + return SIZE_NA; + return r + pos; + } + + size_t flush() override { + size_t ret = 0; + if (pos > 0) { + ret = Base::write(buf, pos); + pos = 0; + if (ret == STREAM_ERROR) + return STREAM_ERROR; + } + return ret + Base::flush(); + } + + enum { LAYER_ID_OUT=1 }; + OutputStream* getOutputStream(int typ=OutputStream::LAYER_ID_OUT) override { return (typ == LAYER_ID_OUT ? static_cast(this) : Base::getOutputStream(typ)); } + +private: + uint8_t* const buf; + const size_t bufSize; + size_t pos; +}; +/*----------------------------------------------------------------*/ + + + +template +class FilteredInputStream : public LayerInputStream { +public: + typedef LayerInputStream Base; + + FilteredInputStream(InputStream* pFile, size_t nFilteredSize, size_t nBufSize, void* pData) + : Base(pFile), buf(new uint8_t[nBufSize]), filteredSize(nFilteredSize), bufSize(nBufSize), valid(0), pos(0), filter(pData), more(true) {} + virtual ~FilteredInputStream() { + delete[] buf; + } + + /** + * Read data through filter, keep calling until len returns 0. + * @param rbuf Data buffer + * @param len Buffer size on entry, bytes actually read on exit + * @return Length of data in buffer + */ + size_t read(void* rbuf, size_t len) override { + uint8_t* rb = (uint8_t*)rbuf; + + const size_t l2 = len; + while (more && len) { + if (pos == valid) { + valid = Base::read(buf, bufSize); + if (valid == STREAM_ERROR) + return STREAM_ERROR; + pos = 0; + } + size_t m = valid - pos; + size_t n = len; + more = filter(buf + pos, m, rb, n); + pos += m; + rb += n; + len -= n; + } + return l2-len; + } + + size_f_t getSize() const override { + return filteredSize; + } + + bool setPos(size_f_t wpos) override { + valid = pos = 0; + return Base::setPos(wpos); + } + + size_f_t getPos() const override { + const size_f_t r = Base::getPos(); + if (r == SIZE_NA) + return SIZE_NA; + return r-(valid-pos); + } + + enum { LAYER_ID_IN=2 }; + InputStream* getInputStream(int typ=InputStream::LAYER_ID_IN) override { return (typ == LAYER_ID_IN ? static_cast(this) : Base::getInputStream(typ)); } + +private: + uint8_t* const buf; + const size_t filteredSize; + const size_t bufSize; + size_t valid; + size_t pos; + Filter filter; + bool more; +}; + + +template +class FilteredOutputStream : public LayerOutputStream { +public: + typedef LayerOutputStream Base; + + FilteredOutputStream(OutputStream* aFile, size_t aBufSize, void* pData) + : Base(aFile), buf(new uint8_t[aBufSize]), bufSize(aBufSize), filter(pData), flushed(false) {} + virtual ~FilteredOutputStream() { + flush(); + delete[] buf; + } + + size_t write(const void* wbuf, size_t len) override { + if (flushed) + return STREAM_ERROR; + + const uint8_t* wb = (const uint8_t*)wbuf; + size_t written = 0; + while (len > 0) { + size_t n = bufSize; + size_t m = len; + + const bool more = filter(wb, m, buf, n); + wb += m; + len -= m; + + const size_t r = Base::write(buf, n); + if (r == STREAM_ERROR) + return STREAM_ERROR; + written += r; + + if (!more) { + if (len > 0) + return STREAM_ERROR; + flushed = true; + return written; + } + } + return written; + } + + size_t flush() override { + if (flushed) + return Base::flush(); + + flushed = true; + size_t written = 0; + + while (true) { + size_t n = bufSize; + size_t zero = 0; + bool more = filter(NULL, zero, buf, n); + + written += Base::write(buf, n); + + if (!more) + break; + } + const size_t r = Base::flush(); + if (r == STREAM_ERROR) + return STREAM_ERROR; + return written + r; + } + + enum { LAYER_ID_OUT=2 }; + OutputStream* getOutputStream(int typ=OutputStream::LAYER_ID_OUT) override { return (typ == LAYER_ID_OUT ? static_cast(this) : Base::getOutputStream(typ)); } + +private: + uint8_t* const buf; + const size_t bufSize; + Filter filter; + bool flushed; +}; +/*----------------------------------------------------------------*/ + + + +#ifdef _UNICODE +#define TOKEN_SIZE 2 //in bytes +#else +#define TOKEN_SIZE 1 //in bytes +#endif +#define TOKEN_MAXBUF (2048*TOKEN_SIZE) //in bytes +#define TOKEN_MAXIGN 32 //in TCHARs + +template +class TokenInputStream : public LayerInputStream { +public: + typedef LayerInputStream Base; + + TokenInputStream(InputStream* aStream, TCHAR aToken=_T('\n')) + : Base(aStream), pos(TOKEN_MAXBUF), eos(false), token(aToken) { arrIgnore[0] = _T('\0'); } + virtual ~TokenInputStream() { + } + + void emptyBuffer() { + setPos(getPos()); + } + + TCHAR getToken() const { + return token; + } + + void setToken(TCHAR aToken) { + token = aToken; + } + + LPCTSTR getTrimTokens() const { + return arrIgnore; + } + + size_t setTrimTokens(LPCTSTR szIgnore) { + const size_t len = _tcslen(szIgnore); + if (len >= TOKEN_MAXIGN) + return 0; + _tcscpy(arrIgnore, szIgnore); + return len; + } + + LPTSTR trimFrontLine(LPTSTR line) { + while (line[0] != _T('\0')) { + if (_tcschr(arrIgnore, line[0]) == NULL) + return line; + ++line; + } + return line; + } + + LPTSTR trimFrontLine(MemFile& memFile) { + memFile.ensureSize(1); + LPTSTR line = (LPTSTR)memFile.getData(); + line[memFile.getSizeLeft()] = _T('\0'); + LPTSTR newLine = trimFrontLine(line); + memFile.movePos(newLine-line); + return newLine; + } + + size_t trimBackLine(LPTSTR line) { + const size_t len = _tcslen(line); + size_t i = len; + while (i > 0 && line[--i] != _T('\0')) { + if (_tcschr(arrIgnore, line[i]) == NULL) + return len-(i+1); + line[i] = _T('\0'); + } + return len; + } + + size_t trimBackLine(MemFile& memFile) { + memFile.ensureSize(1); + LPTSTR line = (LPTSTR)memFile.getData(); + line[memFile.getSizeLeft()] = _T('\0'); + const size_t trimedSize = trimBackLine(line); + memFile.moveSize(-((size_f_t)trimedSize)); + return trimedSize; + } + + // revert the deleted token during the last readLine() + void restoreToken() { + LPTSTR line = (LPTSTR)buf; + ASSERT(pos > 0); + line[--pos] = token; + } + + size_t readLine(LPTSTR wbuf, size_t len) { + if (eos) + return 0; + uint8_t* b = (uint8_t*)wbuf; + const size_t l2 = len; + // process chunks of TOKEN_MAXBUF bytes + while (len >= TOKEN_MAXBUF / TOKEN_SIZE) { + const size_t n = read(b, TOKEN_MAXBUF); + if (n == STREAM_ERROR) + return STREAM_ERROR; + *((TCHAR*)(b+n)) = _T('\0'); + LPTSTR t = _tcschr((TCHAR*)b, token); + // if token found... + if (t != NULL) { + // ... set the end of line and return it + t[0] = _T('\0'); + if (n == TOKEN_SIZE && len != 1) { + eos = true; + return l2-len; + } + ++t; + const size_t bytesParsed = (uint8_t*)t-b; + const size_t bytesNotParsed = n - bytesParsed; + pos = TOKEN_MAXBUF - bytesNotParsed; + // store the unprocessed data + memcpy(buf+pos, (uint8_t*)t, bytesNotParsed); + len -= (bytesParsed - TOKEN_SIZE) / TOKEN_SIZE; + return l2-len; + } + len -= n / TOKEN_SIZE; + // if end of stream return + if (n < TOKEN_MAXBUF) { + eos = true; + return l2-len; + } + b += n; + // if we reached the end of the buffer, signal it + if (len == 0) + return l2+1; + } + // process the last sub-chunk part + const size_t n = read(b, len*TOKEN_SIZE); + if (n == STREAM_ERROR) + return STREAM_ERROR; + *((TCHAR*)(b+n)) = _T('\0'); + LPTSTR t = _tcschr((TCHAR*)b, token); + // if token found... + if (t != NULL) { + // ... set the end of line and return it + t[0] = _T('\0'); + if (n == TOKEN_SIZE && len != 1) { + eos = true; + return l2-len; + } + ++t; + const size_t bytesParsed = (uint8_t*)t-b; + const size_t bytesNotParsed = n - bytesParsed; + pos = TOKEN_MAXBUF - bytesNotParsed; + // store the unprocessed data + memcpy(buf+pos, (uint8_t*)t, bytesNotParsed); + len -= (bytesParsed - TOKEN_SIZE) / TOKEN_SIZE; + return l2-len; + } + // if end of stream return + if (n < len * TOKEN_SIZE) { + eos = true; + return n / TOKEN_SIZE; + } + // we reached the end of the buffer, signal it + return l2+1; + } + + size_t readLine(MemFile& memFile) + { + if (eos) + return 0; + // make sure we read one full line + const size_f_t oldSize = memFile.getSize(); + while (true) { + memFile.ensureSize((4096+1)*TOKEN_SIZE); + const size_t ret = readLine((LPTSTR)(memFile.getBuffer()+memFile.getSize()), 4096); + if (ret == STREAM_ERROR) + return STREAM_ERROR; + if (ret <= 4096) { + memFile.growSize(ret*TOKEN_SIZE); + break; + } + memFile.growSize(4096*TOKEN_SIZE); + } + return size_t(memFile.getSize()-oldSize); + } + + // read all to the memfile + size_t read(MemFile& memFile) + { + if (eos) + return 0; + const size_t len = (size_t)(Base::getSize() - getPos()); + memFile.ensureSize(len); + const size_t ret = read(memFile.getBuffer()+memFile.getSize(), len); + if (ret == STREAM_ERROR) + return STREAM_ERROR; + memFile.growSize(ret); + return ret; + } + + size_t read(void* wbuf, size_t len) override { + size_t n = 0; + if (pos < TOKEN_MAXBUF) { + n = TOKEN_MAXBUF-pos; + if (n > len) + n = len; + memcpy(wbuf, buf+pos, n); + len -= n; + pos += n; + } + if (len == 0) + return n; + const size_t r = Base::read((uint8_t*)wbuf+n, len); + if (r == STREAM_ERROR) + return STREAM_ERROR; + return n+r; + } + + bool setPos(size_f_t wpos) override { + eos = false; + pos = TOKEN_MAXBUF; + return Base::setPos(wpos); + } + + size_f_t getPos() const override { + const size_f_t r = Base::getPos(); + if (r == SIZE_NA) + return SIZE_NA; + return r-(TOKEN_MAXBUF-pos); + } + + bool isEOS() const { + return eos; + } + + enum { LAYER_ID_IN=5 }; + InputStream* getInputStream(int typ=InputStream::LAYER_ID_IN) override { return (typ == LAYER_ID_IN ? static_cast(this) : Base::getInputStream(typ)); } + +private: + uint8_t buf[TOKEN_MAXBUF+TOKEN_SIZE]; + size_t pos; + bool eos; + TCHAR token; + TCHAR arrIgnore[TOKEN_MAXIGN]; +}; +/*----------------------------------------------------------------*/ + + + +template +class MaskInputStream : public LayerInputStream { +public: + typedef LayerInputStream Base; + + MaskInputStream(InputStream* aStream, size_f_t nPos, size_f_t nSize) + : Base(aStream), startPos(nPos), size(nSize), pos(0) { } + virtual ~MaskInputStream() { } + + size_t read(void* wbuf, size_t len) override { + if (pos >= size) + return 0; + if (!Base::setPos(startPos + pos)) + return STREAM_ERROR; + if (pos+(size_f_t)len > size) + len = (size_t)(size - pos); + const size_t r = Base::read(wbuf, len); + if (r == STREAM_ERROR) + return STREAM_ERROR; + pos += r; + return r; + } + + size_f_t getSize() const override { + return size; + } + + bool setPos(size_f_t wpos) override { + if (wpos > size) + pos = size; + else + pos = wpos; + return true; + } + + size_f_t getPos() const override { + return pos; + } + + enum { LAYER_ID_IN=6 }; + InputStream* getInputStream(int typ=InputStream::LAYER_ID_IN) override { return (typ == LAYER_ID_IN ? static_cast(this) : Base::getInputStream(typ)); } + +private: + const size_f_t size; + const size_f_t startPos; + size_f_t pos; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_FILTERS_H__ diff --git a/libs/Common/HTMLDoc.h b/libs/Common/HTMLDoc.h new file mode 100644 index 0000000..8fd21b5 --- /dev/null +++ b/libs/Common/HTMLDoc.h @@ -0,0 +1,457 @@ +/* + * Modified version of: + * + * @file htmlDoc.h + * @brief Simple HTML document writer and SVG drawer + * @author Pierre MOULON + * + * Copyright (c) 2011, 2012, 2013 Pierre MOULON + * All rights reserved. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#ifndef _HTMLDOC_H_ +#define _HTMLDOC_H_ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + +#define JSXCHART_BORDER 0.2f + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace HTML { + +inline const std::string htmlMarkup(const std::string& markup, const std::string& text) { + std::ostringstream os; + os << '<'<< markup<<'>' << text << "' <<"\n"; + return os.str(); +} +inline const std::string htmlMarkup(const std::string& markup, const std::string& attributes, const std::string& text) { + std::ostringstream os; + os << '<'<' << text << "' <<"\n"; + return os.str(); +} +inline const std::string htmlOpenMarkup(const std::string& markup, const std::string& attributes) { + std::ostringstream os; + os << '<'<" <<"\n"; + return os.str(); +} + +inline const std::string htmlComment(const std::string& text) { + std::ostringstream os; + os << "" << "\n"; + return os.str(); +} + +/// Return a chain in the form attributes="val" +template +inline const std::string quotedAttributes(const std::string& attributes, const T & val) { + std::ostringstream os; + os << attributes << "='" << val << '\''; + return os.str(); +} + +/// Return a chain of the value T +template +inline const std::string toString(const T& val) { + std::ostringstream os; + os << val; + return os.str(); +} + +class htmlDocumentStream +{ +public: + htmlDocumentStream(const std::string& title) { + htmlStream << "\n"; + htmlStream << htmlMarkup("head", + "\n" + "\n" + "\n"); + htmlStream << htmlMarkup("title",title); + } + + htmlDocumentStream(const std::string& title, + const std::vector& vec_css, + const std::vector& vec_js) + { + htmlStream << "\n\n"; + htmlStream << htmlMarkup("title",title); + // CSS and JS resources + for (std::vector::const_iterator iter = vec_css.begin(); iter != vec_css.end(); ++iter) + htmlStream << "\n"; + for (std::vector::const_iterator iter = vec_js.begin(); iter != vec_js.end(); ++iter) + htmlStream << "\n"; + htmlStream << "\n"; + } + + void pushInfo(const std::string& text) { + htmlStream << text; + } + + std::string getDoc() { + return htmlMarkup("html", htmlStream.str()); + } + + std::ostringstream htmlStream; +}; + + +/// Class to draw with the JSXGraph library in HTML page. +class JSXGraphWrapper +{ +public: + typedef float TRANGE; + typedef std::pair< std::pair, std::pair > RANGE; + + JSXGraphWrapper() { + cpt = 0; + } + + void reset() { + stream.str(""); + stream.precision(4); + //stream.setf(std::ios::fixed,std::ios::floatfield); + cpt = 0; + } + + void init(unsigned W, unsigned H, LPCTSTR szGraphName=NULL) { + reset(); + std::string strGraphName; + if (szGraphName == NULL) { + strGraphName = SEACAVE::Util::getUniqueName(); + szGraphName = strGraphName.c_str(); + } + stream << + "\n" + "
\n" + "\n"; + } + + std::string toStr() const { + return stream.str(); + } + + template + static inline RANGE autoViewport(TX maxValX, TY maxValY, TX minValX, TY minValY) { + //Use the value with a little margin + const TX rangeX = maxValX-minValX; + const TY rangeY = maxValY-minValY; + return std::make_pair( + std::make_pair(-JSXCHART_BORDER*rangeX+minValX,JSXCHART_BORDER*rangeX+maxValX), + std::make_pair(-JSXCHART_BORDER*rangeY+minValY,JSXCHART_BORDER*rangeY+maxValY)); + } + template + static RANGE autoViewport(const VECTX& vec_x, const VECTY& vec_y) { + typedef typename VECTX::value_type TX; + typedef typename VECTY::value_type TY; + if (vec_x.empty() || vec_y.empty() || vec_x.size() != vec_y.size()) + return RANGE(); + //For X values + const TX minValX = *std::min_element(vec_x.begin(), vec_x.end()); + const TX maxValX = *std::max_element(vec_x.begin(), vec_x.end()); + //For Y values + const TY minValY = *std::min_element(vec_y.begin(), vec_y.end()); + const TY maxValY = *std::max_element(vec_y.begin(), vec_y.end()); + return autoViewport(maxValX, maxValY, minValX, minValY); + } + template + static RANGE autoViewport(const VECTY& vec_y, bool bForceY0=true) { + typedef T TX; + typedef typename VECTY::value_type TY; + if (vec_y.empty()) + return RANGE(); + //For X values + const TX minValX = TX(0); + const TX maxValX = static_cast(vec_y.size()); + //For Y values + const TY minValY = (bForceY0 ? TY(0) : *std::min_element(vec_y.begin(), vec_y.end())); + const TY maxValY = *std::max_element(vec_y.begin(), vec_y.end()); + return autoViewport(maxValX, maxValY, minValX, minValY); + } + + std::ostringstream stream; + size_t cpt; //increment for variable +}; +/*----------------------------------------------------------------*/ + +} // namespace HTML + + + +namespace SVG { + +/// Basic SVG style +class svgStyle +{ +public: + svgStyle():_sFillCol(""), _sStrokeCol("black"), _sToolTip(""), _fillOpacity(1.f), _strokeW(1.f), _strokeOpacity(1.f) {} + + // Configure fill color + svgStyle& fill(const std::string& col, float opacity = 1.f) + { _sFillCol = col; _fillOpacity = opacity; return *this; } + + // Configure stroke color and width + svgStyle& stroke(const std::string& col, float witdh = 1.f, float opacity = 1.f) + { _sStrokeCol = col; _strokeW = witdh; _strokeOpacity = opacity; return *this; } + + // Configure with no stroke + svgStyle& noStroke() + { _sStrokeCol = ""; _strokeW = 0.f; _strokeOpacity = 0.f; return *this; } + + // Configure tooltip + svgStyle& tooltip(const std::string& sTooltip) + { _sToolTip = sTooltip; return *this; } + + const std::string getSvgStream() const { + std::ostringstream os; + if (!_sStrokeCol.empty()) { + os << " stroke='" << _sStrokeCol << "' stroke-width='" << _strokeW << "'"; + if (_strokeOpacity < 1) + os << " stroke-opacity='" << _strokeOpacity << "'"; + } + if (!_sFillCol.empty()) { + os << " fill='" << _sFillCol << "'"; + if (_fillOpacity < 1) + os << " fill-opacity='" << _fillOpacity << "'"; + } else { + os << " fill='none'"; + } + if (!_sToolTip.empty()) { + os << " tooltip='enable'>" << "" << _sToolTip << ""; + } + return os.str(); + } + + bool bTooltip() const { return !_sToolTip.empty();} + + std::string _sFillCol, _sStrokeCol, _sToolTip; + float _fillOpacity, _strokeW, _strokeOpacity; +}; + + +/// Basic class to handle simple SVG draw. +/// You can draw line, square, rectangle, text and image (xlink) +class svgDrawer +{ +public: + ///Constructor + svgDrawer(size_t W = 0, size_t H = 0) { + svgStream << + "\n" + "\n" + " 0 && H > 0) + svgStream << + " width='" << W << "px' height='"<< H << "px'" + " preserveAspectRatio='xMinYMin meet'" + " viewBox='0 0 " << W << ' ' << H <<"'"; + + svgStream << + " xmlns='http://www.w3.org/2000/svg'" + " xmlns:xlink='http://www.w3.org/1999/xlink'" + " version='1.1'>\n"; + } + ///Circle draw -> x,y position and radius + void drawCircle(float cx, float cy, float r, const svgStyle& style) { + svgStream + << "\n" : "/>\n"); + } + ///Line draw -> start and end point + void drawLine(float ax, float ay, float bx, float by, const svgStyle& style) { + svgStream + << "\n" : "/>\n"); + } + + ///Reference to an image (path must be relative to the SVG file) + void drawImage(const std::string& simagePath, int W, int H, + int posx = 0, int posy = 0, float opacity =1.f) + { + svgStream << + "\n"; + } + + ///Square draw -> x,y position and size + void drawSquare(float cx, float cy, float W, const svgStyle& style) { + drawRectangle(cx, cy, W, W, style); + } + + ///Circle draw -> x,y position and width and height + void drawRectangle(float cx, float cy, float W, float H, const svgStyle& style) { + svgStream + << "\n" : "/>\n"); + } + + ///Text display -> x,y position, font size + void drawText(float cx, float cy, const std::string& stext, const std::string& scol = "", const std::string& sattr = "", float fontSize = 1.f) { + svgStream << "" << stext << "\n"; + } + template< typename DataInputIteratorX, typename DataInputIteratorY> + void drawPolyline(DataInputIteratorX xStart, DataInputIteratorX xEnd, + DataInputIteratorY yStart, DataInputIteratorY /*yEnd*/, + const svgStyle& style) + { + svgStream << "\n" : "/>\n"); + } + + ///Close the svg tag. + std::ostringstream& closeSvgFile() { + svgStream << ""; + return svgStream; + } + + std::ostringstream svgStream; +}; + +/// Helper to draw a SVG histogram +/// ____ +/// | | ___ | +/// | |__| | | +/// | | | | | +/// -----------| +struct svgHisto +{ + template + std::string draw(const VECT& vec_value, + const std::pair& range, + float W, float H) + { + if (vec_value.empty()) + return ""; + + //-- Max value + typedef typename VECT::value_type T; + const T maxi = *max_element(vec_value.begin(), vec_value.end()); + const size_t n = vec_value.size(); + + const float scaleFactorY = H / static_cast(maxi); + const float scaleFactorX = W / static_cast(n); + + svgDrawer svgStream; + + for (typename VECT::const_iterator iter = vec_value.begin(); iter != vec_value.end(); ++iter) + { + const T dist = std::distance(vec_value.begin(), iter); + const T& val = *iter; + std::ostringstream os; + os << '(' << range.first + dist/float(n) * (range.second-range.first) << ',' << val << ')'; + svgStyle style = svgStyle().fill("blue").stroke("black", 1.0).tooltip(os.str()); + svgStream.drawRectangle( + scaleFactorX * dist, H-val * scaleFactorY, + scaleFactorX, val * scaleFactorY, + style); + //_________ + //| |_________ + //| || | + //| || | + //| || | + //0 sFactorX 2*sFactorX + } + svgStyle styleAxis = svgStyle().stroke("black", 1.0f); + // Draw X Axis + svgStream.drawText(.05f*W, 1.2f*H, HTML::toString(range.first), "black", "", .1f*H); + svgStream.drawText(W, 1.2*H, HTML::toString(range.second), "black", "", .1f*H); + svgStream.drawLine(0, 1.1f*H, W, 1.1f*H, styleAxis); + // Draw Y Axis + svgStream.drawText(1.2f*W, .1f*H, HTML::toString(maxi), "black", "", .1f*H); + svgStream.drawText(1.2f*W, H, "0", "black", "", .1f*H); + svgStream.drawLine(1.1f*W, 0, 1.1f*W, H, styleAxis); + + return svgStream.closeSvgFile().str(); + } +}; +/*----------------------------------------------------------------*/ + +} // namespace SVG + +#endif // _HTMLDOC_H_ diff --git a/libs/Common/HalfFloat.h b/libs/Common/HalfFloat.h new file mode 100644 index 0000000..3f38c83 --- /dev/null +++ b/libs/Common/HalfFloat.h @@ -0,0 +1,159 @@ +//////////////////////////////////////////////////////////////////// +// HalfFoat.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_HALFFLOAT_H__ +#define __SEACAVE_HALFFLOAT_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + + +// hfloat - a 16-bit floating point number class +// +// Can represent positive and negative numbers whose magnitude is between +// roughly 6.1e-5 and 6.5e+4 with a relative error of 9.8e-4; numbers +// smaller than 6.1e-5 can be represented with an absolute error of +// 6.0e-8. All integers from -2048 to +2048 can be represented exactly. +// Supports Denormals-as-zero (DAZ), but does not support infinities or NaN. +// +// Only conversions from half to float are lossless. +class GENERAL_API hfloat +{ +public: + typedef short Type; + + inline hfloat() : val(fromFloat(float())) {} + explicit inline hfloat(float v) : val(fromFloat(v)) {} + template + inline hfloat(T v) : val(fromFloat(static_cast(v))) {} + + inline hfloat& operator = (float v) { + val = fromFloat(v); + return *this; + } + template + inline hfloat& operator = (T v) { + val = fromFloat(static_cast(v)); + return *this; + } + + inline operator float() const { + return toFloat(val); + } + inline hfloat operator - () const { + return fromFloat2HFloat(-toFloat(val)); + } + inline hfloat operator + (hfloat v) { + return fromFloat2HFloat(toFloat(val) + toFloat(v.val)); + } + inline hfloat operator + (float v) { + return fromFloat2HFloat(toFloat(val) + v); + } + inline hfloat& operator += (hfloat v) { + return *this = *this + v; + } + inline hfloat& operator += (float v) { + return *this = *this + v; + } + inline hfloat operator - (hfloat v) { + return fromFloat2HFloat(toFloat(val) - toFloat(v.val)); + } + inline hfloat operator - (float v) { + return fromFloat2HFloat(toFloat(val) - v); + } + inline hfloat& operator -= (hfloat v) { + return *this = *this - v; + } + inline hfloat& operator -= (float v) { + return *this = *this - v; + } + inline hfloat operator * (hfloat v) { + return fromFloat2HFloat(toFloat(val) * toFloat(v.val)); + } + inline hfloat operator * (float v) { + return fromFloat2HFloat(toFloat(val) * v); + } + inline hfloat& operator *= (hfloat v) { + return *this = *this * v; + } + inline hfloat& operator *= (float v) { + return *this = *this * v; + } + inline hfloat operator / (hfloat v) { + return fromFloat2HFloat(toFloat(val) / toFloat(v.val)); + } + inline hfloat operator / (float v) { + return fromFloat2HFloat(toFloat(val) / v); + } + inline hfloat& operator /= (hfloat v) { + return *this = *this / v; + } + inline hfloat& operator /= (float v) { + return *this = *this / v; + } + + static float min() { return 6.1e-5f; } + static float max() { return 6.5e+4f; } + + static inline short fromFloat(float f) { + ASSERT(ISFINITE(f) && (ABS(f) == 0.f || ABS(f) >= min()) && ABS(f) <= max()); + // ~8 clock cycles on modern x86-64 + const CastF2I ufi(f); + int32_t t1 = ufi.i & 0x7fffffff; // Non-sign bits + int32_t t2 = ufi.i & 0x80000000; // Sign bit + int32_t t3 = ufi.i & 0x7f800000; // Exponent + t1 >>= 13; // Align mantissa on MSB + t2 >>= 16; // Shift sign bit into position + t1 -= 0x1c000; // Adjust bias + t1 = (t3 < 0x38800000) ? 0 : t1; // Flush-to-zero + t1 = (t3 > 0x47000000) ? 0x7bff : t1; // Clamp-to-max + t1 = (t3 == 0 ? 0 : t1); // Denormals-as-zero + return (short)(t1 | t2); // Re-insert sign bit + } + static inline float toFloat(short h) { + // ~6 clock cycles on modern x86-64 + CastF2I ufi; + int32_t t1 = h & 0x7fff; // Non-sign bits + int32_t t2 = h & 0x8000; // Sign bit + int32_t t3 = h & 0x7c00; // Exponent + t1 <<= 13; // Align mantissa on MSB + t2 <<= 16; // Shift sign bit into position + t1 += 0x38000000; // Adjust bias + t1 = (t3 == 0 ? 0 : t1); // Denormals-as-zero + ufi.i = t1 | t2; // Re-insert sign bit + return ufi.f; + } + + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & val; + } + #endif + +protected: + static inline hfloat fromFloat2HFloat(float f) { + return TAliasCast(fromFloat(f)).i; + } + +protected: + Type val; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_HALFFLOAT_H__ diff --git a/libs/Common/Hash.h b/libs/Common/Hash.h new file mode 100644 index 0000000..ccb9163 --- /dev/null +++ b/libs/Common/Hash.h @@ -0,0 +1,303 @@ +//////////////////////////////////////////////////////////////////// +// Hash.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_HASH_H__ +#define __SEACAVE_HASH_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +/************************************************************************************** + * StringHash template + * --------------- + * compile-time string hash template + **************************************************************************************/ + +class StringHash +{ +private: + uint32_t m_val; + + template inline uint32_t _Hash(const char (&str)[N]) const + { + typedef const char (&truncated_str)[N-2]; + return str[N-1] + 65599 * (str[N-2] + 65599 * _Hash((truncated_str)str)); + } + inline uint32_t _Hash(const char (&str)[3]) const { return str[2] + 65599 * (str[1] + 65599 * str[0]); } + inline uint32_t _Hash(const char (&str)[2]) const { return str[1] + 65599 * str[0]; } + +public: + template StringHash(const char (&str)[N]) { m_val = _Hash(str); } + inline operator uint32_t() const { return m_val; } +}; + + +/************************************************************************************** + * cHashTable template + * --------------- + * simple hash template (performs relatively well for less than 300 elements) + **************************************************************************************/ + +#define KEY_NA 0xFFFF +#define KEY_MAXSIZE 0x1000000 + +template +class cHashTable +{ +public: + typedef uint32_t Key; + typedef uint16_t Idx; + typedef uint32_t Size; + typedef cList Values; + typedef cList Keys; + typedef cList Indices; + +protected: + Keys m_keys; // Array of unique keys that have been set + Values m_values; // Matching array of unique values that have been set + Indices m_indices; // 1-based index array referencing the two arrays above + +public: + cHashTable() : m_indices(32) { m_indices.Memset(0); } + ~cHashTable() { Release(); } + + inline void Release() + { + m_keys.Release(); + m_values.Release(); + m_indices.Release(); + } + + inline bool IsEmpty() const { return m_indices.IsEmpty(); } + inline size_t GetSize() const { return m_keys.GetSize(); } + inline const Type* GetBegin() const { return m_values.GetData(); } + inline const Type* GetEnd() const { return m_values.GetData()+m_values.GetSize(); } + inline Type* GetBegin() { return m_values.GetData(); } + inline Type* GetEnd() { return m_values.GetData()+m_values.GetSize(); } + + inline const Indices& GetArrIndices() const { return m_indices; } + inline const Keys& GetArrKeys() const { return m_keys; } + inline const Values& GetArrValues() const { return m_values; } + inline Values& GetArrValues() { return m_values; } + +private: + inline Size _StaticKeyToID(Key key, Size size) const { return key & (size-1); } + inline Size _KeyToID(Key key) const { return _StaticKeyToID(key, (Size)m_indices.GetSize()); } + inline Idx _IDToIndex(Size id) const { return m_indices[id] - 1; } + inline Idx _KeyToIndex(Key key) const { return (m_indices.IsEmpty() ? KEY_NA : _IDToIndex(_KeyToID(key))); } + + // Completely discards then rebuilds all indices based on the current set of keys + void _RebuildIndices() + { + // Clear all current memory + m_indices.Memset(0); + // Run through all keys and recreate the indices + for (Idx i=0; i0; ) + { + if (m_indices[--i] > index) + { + --m_indices[i]; + } + } + } + } + + // Retrieves a value from the hash -- but it will only be valid if it exists, so be careful. In most + // cases you will either want to use an Exists() check first, or simply use the Find function. + inline const Type& operator [] (Key key) const + { + const Idx index = _KeyToIndex(key); + return m_values[index]; + } + + // Retrieves a value from the hash, inserting a new one if necessary + Type& operator [] (Key key) + { + // Get the index for this key + const Idx index = _KeyToIndex(key); + + if (index != KEY_NA) + { + // If we found a valid entry, we need to match the actual key + const Key oldKey = m_keys[index]; + + // If the key matches, return the value + if (oldKey == key) + { + return m_values[index]; + } + else + { + // Setting the key was unsuccessful due to another entry colliding with our key; + // we must expand the indices until we find a set of keys that will match. + Size newSize = m_indices.GetSize(); + while (newSize < (KEY_MAXSIZE>>1)) + { + newSize = newSize << 1; + // Find the next best size for the hash that would make both keys unique + if (_StaticKeyToID(key, newSize) != _StaticKeyToID(oldKey, newSize)) + { + m_indices.ResizeExact(newSize); + _RebuildIndices(); + break; + } + } + // Critical error if we didn't find a working size + ASSERT(_StaticKeyToID(key, newSize) != _StaticKeyToID(oldKey, newSize)); + } + } + + // assert the total number of values stored is in Idx range + ASSERT(m_keys.GetSize() < (Size)((Idx)-1)); + + // Append the new key to the end + m_keys.Insert(key); + + // Add this new entry to the index list using the current array size as the 1-based index + m_indices[_KeyToID(key)] = (Idx)m_keys.GetSize(); + + // Return the value + return m_values.AddEmpty(); + } + +public: + // Fowler / Noll / Vo (FNV) Hash + // magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ + static inline Key HashKeyFNV(const uint8_t* data, Size size) + { + Key hash = 2166136261U; //InitialFNV + for (size_t i = 0; i < size; i++) + { + hash = hash ^ (data[i]); // xor the low 8 bits + hash = hash * 16777619; // multiply by the magic number (FNVMultiple) + } + return hash; + } + // Function that calculates a hash value from a given data + // tested for random binary data (3 <= size <= 33) and performs VERY bad + static inline Key HashKeyR5(const uint8_t* data, Size size) + { + Key key = 0; + Key offset = 1357980759; + for (Size i=0; i> 15); + offset = key ^ (((~offset) >> 7) | (offset << 25)); + } + return key; + } + static inline Key HashKey(const uint8_t* data, Size size) { return HashKeyFNV(data, size); } + static inline Key HashKey(LPCTSTR sz) { return HashKey((const uint8_t*)sz, (Size)_tcslen(sz)); } + static inline Key HashKey(const String& str) { return HashKey((const uint8_t*)str.c_str(), (Size)str.size()); } + + // Convenience functions + inline Type* Find (LPCTSTR key) { return Find ( HashKey(key) ); } + inline Type* Find (const String& key) { return Find ( HashKey(key) ); } + inline const Type* Find (LPCTSTR key) const { return Find ( HashKey(key) ); } + inline const Type* Find (const String& key) const { return Find ( HashKey(key) ); } + inline void Delete (LPCTSTR key) { Delete( HashKey(key) ); } + inline void Delete (const String& key) { Delete( HashKey(key.c_str()) ); } + inline bool Exists (LPCTSTR key) const { return Exists( HashKey(key) ); } + inline bool Exists (const String& key) const { return Exists( HashKey(key.c_str()) ); } + inline Type& operator [] (LPCTSTR key) { return (*this)[ HashKey(key) ]; } + inline Type& operator [] (const String& key) { return (*this)[ HashKey(key.c_str()) ]; } + inline const Type& operator [] (LPCTSTR key) const { return (*this)[ HashKey(key) ]; } + inline const Type& operator [] (const String& key) const { return (*this)[ HashKey(key.c_str()) ]; } +}; + + +/************************************************************************************** + * cMapWrap template + * --------------- + * STL map wrapper + **************************************************************************************/ + +template +class cMapWrap : public std::unordered_map +{ +public: + typedef typename std::unordered_map Base; + typedef typename Base::const_iterator const_iterator; + typedef typename Base::iterator iterator; + +public: + cMapWrap() {} + ~cMapWrap() {} + + inline void Release() { this->clear(); } + inline bool IsEmpty() const { return this->empty(); } + inline size_t GetSize() const { return this->size(); } + inline const_iterator GetBegin() const { return this->begin(); } + inline const_iterator GetEnd() const { return this->end(); } + inline iterator GetBegin() { return this->begin(); } + inline iterator GetEnd() { return this->end(); } + inline const_iterator Find(const Key& key) const { return this->find(key); } + inline iterator Find(const Key& key) { return this->find(key); } + inline iterator Insert(const Key& key, bool& bExisted) { return Insert(key, Type(), bExisted); } + inline iterator Insert(const Key& key, const Type& val, bool& bExisted) { + const std::pair ret(this->emplace(key, val)); + bExisted = !ret.second; + return ret.first; + } +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_HASH_H__ diff --git a/libs/Common/Line.h b/libs/Common/Line.h new file mode 100644 index 0000000..408a1a8 --- /dev/null +++ b/libs/Common/Line.h @@ -0,0 +1,95 @@ +//////////////////////////////////////////////////////////////////// +// Line.h +// +// Copyright 2023 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_LINE_H__ +#define __SEACAVE_LINE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// Generic line class represented as two points +template +class TLine +{ + STATIC_ASSERT(DIMS > 1 && DIMS <= 3); + +public: + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TAABB AABB; + typedef SEACAVE::TRay RAY; + enum { numScalar = (2*DIMS) }; + enum { numParams = numScalar-1 }; + + POINT pt1, pt2; // line description + + //--------------------------------------- + + inline TLine() {} + inline TLine(const POINT& pt1, const POINT& pt2); + template + inline TLine(const TLine&); + + inline void Set(const POINT& pt1, const POINT& pt2); + + int Optimize(const POINT*, size_t, int maxIters=100); + template + int Optimize(const POINT*, size_t, const RobustNormFunctor& robust, int maxIters=100); + + inline TYPE GetLength() const; + inline TYPE GetLengthSq() const; + inline POINT GetCenter() const; + inline VECTOR GetDir() const; + inline VECTOR GetNormDir() const; + inline RAY GetRay() const; + + inline bool IsSame(const TLine&, TYPE th) const; + + bool Intersects(const AABB& aabb) const; + bool Intersects(const AABB& aabb, TYPE& t) const; + + inline TYPE DistanceSq(const POINT&) const; + inline TYPE Distance(const POINT&) const; + + inline TYPE Classify(const POINT&) const; + inline POINT ProjectPoint(const POINT&) const; + + inline TYPE& operator [] (BYTE i) { ASSERT(i + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & pt1; + ar & pt2; + } + #endif +}; // class TLine +/*----------------------------------------------------------------*/ + +template +struct FitLineOnline : FitPlaneOnline { + template TPoint3 GetLine(TLine& line) const; +}; +/*----------------------------------------------------------------*/ + + +#include "Line.inl" +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_LINE_H__ diff --git a/libs/Common/Line.inl b/libs/Common/Line.inl new file mode 100644 index 0000000..bd7897c --- /dev/null +++ b/libs/Common/Line.inl @@ -0,0 +1,215 @@ +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + + +// C L A S S ////////////////////////////////////////////////////// + +template +inline TLine::TLine(const POINT& _pt1, const POINT& _pt2) + : + pt1(_pt1), pt2(_pt2) +{ + ASSERT(!ISZERO((_pt1-_pt2).norm())); +} // constructor +template +template +inline TLine::TLine(const TLine& rhs) + : + pt1(rhs.pt1.template cast()), pt2(rhs.pt2.template cast()) +{ +} // copy constructor +/*----------------------------------------------------------------*/ + + +// set attributes +template +inline void TLine::Set(const POINT& _pt1, const POINT& _pt2) +{ + ASSERT(!ISZERO((_pt1-_pt2).norm())); + pt1 = _pt1; + pt2 = _pt2; +} +/*----------------------------------------------------------------*/ + + +// least squares refinement of the line to the given 3D point set +// (return the number of iterations) +template +template +int TLine::Optimize(const POINT* points, size_t size, const RobustNormFunctor& robust, int maxIters) +{ + ASSERT(DIMS == 3); + ASSERT(size >= numParams); + struct OptimizationFunctor { + const POINT* points; + size_t size; + double scale; + const RobustNormFunctor& robust; + // construct with the data points + OptimizationFunctor(const POINT* _points, size_t _size, const RobustNormFunctor& _robust) + : points(_points), size(_size), robust(_robust) { ASSERT(size < std::numeric_limits::max()); } + static void Residuals(const double* x, int nPoints, const void* pData, double* fvec, double* fjac, int* /*info*/) { + const OptimizationFunctor& data = *reinterpret_cast(pData); + ASSERT((size_t)nPoints == data.size && fvec != NULL && fjac == NULL); + TLine line; + for (int j=0; j())); + } + } functor(points, size, robust); + double arrParams[numParams]; + for (int j=0; j +int TLine::Optimize(const POINT* points, size_t size, int maxIters) +{ + const auto identity = [](double x) { return x; }; + return Optimize(points, size, identity, maxIters); +} // Optimize +/*----------------------------------------------------------------*/ + + +// get attributes +template +inline TYPE TLine::GetLength() const +{ + return (pt2 - pt1).norm(); +} +template +inline TYPE TLine::GetLengthSq() const +{ + return (pt2 - pt1).squaredNorm(); +} +template +inline typename TLine::POINT TLine::GetCenter() const +{ + return (pt2 + pt1) / TYPE(2); +} +template +inline typename TLine::VECTOR TLine::GetDir() const +{ + return (pt2 - pt1); +} +template +inline typename TLine::VECTOR TLine::GetNormDir() const +{ + return (pt2 - pt1).normalized(); +} +template +inline typename TLine::RAY TLine::GetRay() const +{ + return RAY(pt1, GetNormDir()); +} +/*----------------------------------------------------------------*/ + + +template +inline bool TLine::IsSame(const TLine& line, TYPE th) const +{ + const TYPE thSq(SQUARE(th)); + const VECTOR l(pt2-pt1); + const TYPE invLenSq(INVERT(l.squaredNorm())); + const VECTOR r1(pt1-line.pt1); + const TYPE dSq1((l.cross(r1)).squaredNorm()*invLenSq); + if (dSq1 > thSq) + return false; + const VECTOR r2(pt1-line.pt2); + const TYPE dSq2((l.cross(r2)).squaredNorm()*invLenSq); + return dSq2 <= thSq; +} +/*----------------------------------------------------------------*/ + + +// test for intersection with aabb +template +bool TLine::Intersects(const AABB &aabb) const +{ + return GetRay().Intersects(aabb); +} // Intersects(AABB) +template +bool TLine::Intersects(const AABB &aabb, TYPE& t) const +{ + return GetRay().Intersects(aabb, t); +} // Intersects(AABB) +/*----------------------------------------------------------------*/ + +// Computes the distance between the line and a point. +template +inline TYPE TLine::DistanceSq(const POINT& pt) const +{ + const VECTOR l(pt2-pt1), r(pt1-pt); + if (DIMS == 2) + return TYPE(SQUARE(l[0]*r[1]-r[0]*l[1])/(l[0]*l[0]+l[1]*l[1])); + ASSERT(DIMS == 3); + return TYPE((l.cross(r)).squaredNorm()/l.squaredNorm()); +} // DistanceSq(POINT) +template +inline TYPE TLine::Distance(const POINT& pt) const +{ + return SQRT(DistanceSq(pt)); +} // Distance(POINT) +/*----------------------------------------------------------------*/ + + +// Computes the position on the line segment of the point projection. +// Returns 0 if it coincides with the first point, and 1 if it coincides with the second point. +template +inline TYPE TLine::Classify(const POINT& p) const +{ + const VECTOR vL(pt2 - pt1); + ASSERT(!ISZERO(vL.squaredNorm())); + const VECTOR vP(p - pt1); + return vL.dot(vP) / vL.squaredNorm(); +} // Classify(POINT) +// Calculate point's projection on this line (closest point to this line). +template +inline typename TLine::POINT TLine::ProjectPoint(const POINT& p) const +{ + const VECTOR vL(pt2 - pt1); + ASSERT(!ISZERO(vL.squaredNorm())); + const VECTOR vP(p - pt1); + return pt1 + vL * (vL.dot(vP) / vL.squaredNorm()); +} // ProjectPoint +/*----------------------------------------------------------------*/ + + +template +template +TPoint3 FitLineOnline::GetLine(TLine& line) const +{ + TPoint3 avg, dir; + const TPoint3 quality(this->GetModel(avg, dir)); + const TPoint3 pt2(avg+dir); + line.Set(TPoint3(avg), TPoint3(pt2)); + return TPoint3(quality); +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/LinkLib.h b/libs/Common/LinkLib.h new file mode 100644 index 0000000..dc793f6 --- /dev/null +++ b/libs/Common/LinkLib.h @@ -0,0 +1,111 @@ +//////////////////////////////////////////////////////////////////// +// LinkLib.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_LINKLIB_H__ +#define __SEACAVE_LINKLIB_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#ifdef _MSC_VER +#else +#include +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + + +/************************************************************************************** + * CLinkLib + * --------------- + * manage dynamic linked libraries + **************************************************************************************/ + +class GENERAL_API CLinkLib +{ +public: + #ifdef _MSC_VER + typedef HINSTANCE LibID; + #else + typedef void* LibID; + #endif + + CLinkLib() : m_hLib(NULL) + { + } + + CLinkLib(const String& dllName) : m_hLib(NULL) + { + Load(dllName); + } + + CLinkLib(const CLinkLib& lib) : m_hLib(lib.m_hLib) + { + ((CLinkLib&)lib).m_hLib = NULL; + } + + ~CLinkLib() + { + Free(); + } + + bool Load(const String& dllName) + { + Free(); + #ifdef _MSC_VER + m_hLib = LoadLibrary(dllName); + #else + m_hLib = dlopen(dllName, RTLD_NOW); + #endif + return (m_hLib != NULL); + } + + void Free() + { + if (!IsLoaded()) + return; + #ifdef _MSC_VER + FreeLibrary(m_hLib); + #else + dlclose(m_hLib); + #endif + m_hLib = NULL; + } + + void* GetFunction(const String& funcName) const + { + #ifdef _MSC_VER + return GetProcAddress(m_hLib, funcName); + #else + return dlsym(m_hLib, funcName); + #endif + } + + bool IsLoaded() const + { + return (m_hLib != NULL); + } + + LibID GetLibID() const + { + return m_hLib; + } + +protected: + LibID m_hLib; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_LINKLIB_H__ diff --git a/libs/Common/List.h b/libs/Common/List.h new file mode 100644 index 0000000..fbe108d --- /dev/null +++ b/libs/Common/List.h @@ -0,0 +1,1704 @@ +//////////////////////////////////////////////////////////////////// +// List.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_LIST_H__ +#define __SEACAVE_LIST_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include + + +// D E F I N E S /////////////////////////////////////////////////// + +#ifndef _USE_VECTORINTERFACE +#define _USE_VECTORINTERFACE 1 +#endif + +// cList index type +#ifdef _SUPPORT_CPP11 +#define ARR2IDX(arr) typename std::remove_reference::type::size_type +#define SIZE2IDX(arr) typename std::remove_const::type>::type +#else +#define ARR2IDX(arr) IDX +#define SIZE2IDX(arr) IDX +#endif + +// cList iterator by index +#ifndef FOREACH +#define FOREACH(var, arr) for (ARR2IDX(arr) var=0, var##Size=(arr).size(); var0; ) +#endif +// cList iterator by pointer +#ifndef FOREACHPTR +#define FOREACHPTR(var, arr) for (auto var=(arr).begin(), var##End=(arr).end(); var!=var##End; ++var) +#endif +#ifndef RFOREACHPTR +#define RFOREACHPTR(var, arr) for (auto var=(arr).end(), var##Begin=(arr).begin(); var--!=var##Begin; ) +#endif + +// raw data array iterator by index +#ifndef FOREACHRAW +#define FOREACHRAW(var, sz) for (SIZE2IDX(sz) var=0, var##Size=(sz); var0; ) +#endif +// raw data array iterator by pointer +#ifndef FOREACHRAWPTR +#define FOREACHRAWPTR(var, arr, sz) for (auto var=(arr), var##End=var+sz; var!=var##End; ++var) +#endif +#ifndef RFOREACHRAWPTR +#define RFOREACHRAWPTR(var, arr, sz) for (auto var##Begin=(arr), var=var##Begin+sz; var--!=var##Begin; ) +#endif + +// constructs a cList reference to a given raw data array +#ifndef CLISTREFRAW +#define CLISTREFRAW(CLIST, var, arr, sz) uint8_t _ArrData##var[sizeof(CLIST)]; new(_ArrData##var) CLIST(sz, const_cast(arr)); const CLIST& var(*reinterpret_cast(_ArrData##var)) +#endif +// constructs a cList reference to a given std::_vector +#ifndef CLISTREFVECTOR +#define CLISTREFVECTOR(CLIST, var, vec) uint8_t _ArrData##var[sizeof(CLIST)]; new(_ArrData##var) CLIST(vec.size(), const_cast(&vec[0])); const CLIST& var(*reinterpret_cast(_ArrData##var)) +#endif + +#define CLISTDEF0(TYPE) SEACAVE::cList< TYPE, const TYPE&, 0 > +#define CLISTDEF2(TYPE) SEACAVE::cList< TYPE, const TYPE&, 2 > +#define CLISTDEF0IDX(TYPE,IDXTYPE) SEACAVE::cList< TYPE, const TYPE&, 0, 16, IDXTYPE > +#define CLISTDEFIDX(TYPE,IDXTYPE) SEACAVE::cList< TYPE, const TYPE&, 1, 16, IDXTYPE > +#define CLISTDEF2IDX(TYPE,IDXTYPE) SEACAVE::cList< TYPE, const TYPE&, 2, 16, IDXTYPE > + +#ifndef STCALL +#define STCALL +#endif + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +typedef size_t IDX; +#define NO_IDX DECLARE_NO_INDEX(IDX) + +/************************************************************************************** + * List template + * -------------- + * fast array + * - set "useConstruct" to 0 for not using constructors/destructors + * when adding/removing elements to the array + * - set "useConstruct" to 1 for using memcpy/memmove when moving arrays + * instead of copy-constructors + * - set "useConstruct" to 2 for always using constructors/destructors + * - set "grow" to the number of elements to increase the allocated array + * when more space is needed + * - if using sorting, the operator < needs to be implemented (the + * operator == is also needed if argument is not ARG_TYPE) + **************************************************************************************/ + +template < + typename TYPE, + typename ARG_TYPE=const TYPE&, + int useConstruct=1, + int grow=16, + typename IDX_TYPE=IDX> +class cList +{ +public: + typedef TYPE Type; + typedef ARG_TYPE ArgType; + typedef IDX_TYPE IDX; + typedef int (STCALL *TFncCompare)(const void* elem, const void* key); //returns 0 if equal, otherwise <0 or >0 respectively + + // construct an empty list + inline cList() : _size(0), _vectorSize(0), _vector(NULL) + { + } + + // construct a list containing size initialized elements + cList(IDX size) : _size(size), _vectorSize(size), _vector((TYPE*)operator new[] (size * sizeof(TYPE))) + { + ASSERT(size > 0 && size < NO_INDEX); + _ArrayConstruct(_vector, size); + } + + // construct a list containing size initialized elements and allocated space for _reserved elements + cList(IDX size, IDX _reserved) : _size(size), _vectorSize(_reserved), _vector((TYPE*)operator new[] (_reserved * sizeof(TYPE))) + { + ASSERT(_reserved >= size && _reserved < NO_INDEX); + _ArrayConstruct(_vector, size); + } + + // copy constructor: creates a deep-copy of the given list + cList(const cList& rList) : _size(rList._size), _vectorSize(rList._vectorSize), _vector(NULL) + { + if (_vectorSize == 0) { + ASSERT(_size == 0); + return; + } + _vector = (TYPE*)(operator new[] (_vectorSize * sizeof(TYPE))); + _ArrayCopyConstruct(_vector, rList._vector, _size); + } + #ifdef _SUPPORT_CPP11 + // copy constructor: creates a move-copy of the given list + cList(cList&& rList) : _size(rList._size), _vectorSize(rList._vectorSize), _vector(rList._vector) + { + rList._Init(); + } + #endif + + // constructor a list from a raw data array + explicit inline cList(TYPE* pDataBegin, TYPE* pDataEnd) : _size((IDX)(pDataEnd-pDataBegin)), _vectorSize(_size) + { + if (_vectorSize == 0) + return; + _vector = (TYPE*) operator new[] (_vectorSize * sizeof(TYPE)); + _ArrayCopyConstruct(_vector, pDataBegin, _size); + } + + // constructor a list from a raw data array, taking ownership of the array memory + explicit inline cList(IDX nSize, TYPE* pData) : _size(nSize), _vectorSize(nSize), _vector(pData) + { + } + + inline ~cList() + { + _Release(); + } + + // copy the content from the given list + inline cList& operator=(const cList& rList) + { + return CopyOf(rList); + } + + inline cList& CopyOf(const cList& rList, bool bForceResize=false) + { + if (this == &rList) + return *this; + if (bForceResize || _vectorSize < rList._vectorSize) { + _Release(); + _vectorSize = rList._vectorSize; + _vector = (TYPE*) operator new[] (_vectorSize * sizeof(TYPE)); + _ArrayCopyConstruct(_vector, rList._vector, rList._size); + } else { + if (_size >= rList._size) { + _ArrayDestruct(_vector+rList._size, _size-rList._size); + _ArrayCopyRestrict(_vector, rList._vector, rList._size); + } else { + _ArrayCopyRestrict(_vector, rList._vector, _size); + _ArrayCopyConstruct(_vector+_size, rList._vector+_size, rList._size-_size); + } + } + _size = rList._size; + return *this; + } + + inline cList& CopyOf(const TYPE* pData, IDX nSize, bool bForceResize=false) + { + if (_vector == pData) + return *this; + if (bForceResize || _vectorSize < nSize) { + _Release(); + _vectorSize = nSize; + _vector = (TYPE*) operator new[] (_vectorSize * sizeof(TYPE)); + _ArrayCopyConstruct(_vector, pData, nSize); + } else { + if (_size >= nSize) { + _ArrayDestruct(_vector+nSize, _size-nSize); + _ArrayCopyRestrict(_vector, pData, nSize); + } else { + _ArrayCopyRestrict(_vector, pData, _size); + _ArrayCopyConstruct(_vector+_size, pData+_size, nSize-_size); + } + } + _size = nSize; + return *this; + } + + // release current list and swap the content with the given list + inline cList& CopyOfRemove(cList& rList) + { + if (this == &rList) + return *this; + _Release(); + _size = rList._size; + _vectorSize = rList._vectorSize; + _vector = rList._vector; + rList._vector = NULL; + rList._size = rList._vectorSize = 0; + return *this; + } + + inline cList& Join(const cList& rList) + { + if (this == &rList || rList._size == 0) + return *this; + const IDX newSize = _size + rList._size; + Reserve(newSize); + _ArrayCopyConstruct(_vector+_size, rList._vector, rList._size); + _size = newSize; + return *this; + } + inline cList& Join(const TYPE* pData, IDX nSize) + { + const IDX newSize = _size + nSize; + Reserve(newSize); + _ArrayCopyConstruct(_vector+_size, pData, nSize); + _size = newSize; + return *this; + } + + template + inline cList& JoinFunctor(IDX nSize, const Functor& functor) { + Reserve(_size + nSize); + if (useConstruct) { + for (IDX n=0; n(_vector+_size, rList._vector, rList._size); + _size = newSize; + rList._size = 0; + return *this; + } + + // Swap the elements of the two lists. + inline cList& Swap(cList& rList) + { + if (this == &rList) + return *this; + const IDX tmpSize = _size; + _size = rList._size; + rList._size = tmpSize; + const IDX tmpVectorSize = _vectorSize; + _vectorSize = rList._vectorSize; + rList._vectorSize = tmpVectorSize; + TYPE* const tmpVector = _vector; + _vector = rList._vector; + rList._vector = tmpVector; + return *this; + } + + // Swap the two elements. + inline void Swap(IDX idx1, IDX idx2) + { + ASSERT(idx1 < _size && idx2 < _size); + TYPE tmp = _vector[idx1]; + _vector[idx1] = _vector[idx2]; + _vector[idx2] = tmp; + } + + inline bool operator==(const cList& rList) const { + if (_size != rList._size) + return false; + for (IDX i = 0; i < _size; ++i) + if (_vector[i] != rList._vector[i]) + return false; + return true; + } + + // Set the allocated memory (normally used for types without constructor). + inline void Memset(uint8_t val) + { + memset(_vector, val, _size * sizeof(TYPE)); + } + inline void MemsetValue(ARG_TYPE val) + { + std::fill_n(_vector, _size, val); + } + + // Delete the old array, and create a new one of the desired size; + // the old elements are deleted and the array is filled with new empty ones. + inline void Reset(IDX newSize) + { + _Release(); + _vectorSize = newSize; + _size = newSize; + if (newSize == 0) { + _vector = NULL; + return; + } + _vector = (TYPE*) operator new[] (newSize * sizeof(TYPE)); + _ArrayConstruct(_vector, newSize); + } + + // Pre-allocate memory for the array at the desired size; + // the old elements are kept. + inline void Reserve(IDX needVectorSize) + { + if (_vectorSize < needVectorSize) + _Grow(needVectorSize); + } + // Same as above, but only the extra needed space is passed. + inline void ReserveExtra(IDX needVectorExtraSize) + { + if (_vectorSize < _size+needVectorExtraSize) + _Grow(_vectorSize+MAXF(needVectorExtraSize,(IDX)grow)); + } + + // Set the size of the array at the new desired size; + // the old elements are kept, but only the ones that are below the new size. + // If increased, the array is filled with new empty elements. + inline void Resize(IDX newSize) + { + if (newSize == _size) + return; + if (newSize < _size) + return _Shrink(newSize); + Reserve(newSize); + _ArrayConstruct(_vector+_size, newSize-_size); + _size = newSize; + } + inline void ResizeExact(IDX newSize) + { + if (newSize == _size) + return; + if (newSize < _size) + return _ShrinkExact(newSize); + Reserve(newSize); + _ArrayConstruct(_vector+_size, newSize-_size); + _size = newSize; + } + + // Free unused memory. + inline void ReleaseFree() + { + if (_size < _vectorSize) + _ShrinkExact(_size); + } + + inline IDX GetIndex(const TYPE* pElem) const + { + ASSERT(pElem-_vector < _size); + return (IDX)(pElem-_vector); + } + + inline const TYPE& operator[](IDX index) const + { + ASSERT(index < _size); + return _vector[index]; + } + inline TYPE& operator[](IDX index) + { + ASSERT(index < _size); + return _vector[index]; + } + + inline TYPE* GetData() const + { + return _vector; + } + inline size_t GetDataSize() const + { + return sizeof(TYPE)*_size; + } + + inline TYPE* Begin() const + { + return _vector; + } + + inline TYPE* End() const + { + return _vector+_size; + } + + inline const TYPE& First() const + { + ASSERT(_size > 0); + return _vector[0]; + } + inline TYPE& First() + { + ASSERT(_size > 0); + return _vector[0]; + } + + inline const TYPE& Last() const + { + ASSERT(_size > 0); + return _vector[_size-1]; + } + inline TYPE& Last() + { + ASSERT(_size > 0); + return _vector[_size-1]; + } + + // Adds a new empty element at the end of the array. + inline TYPE& AddEmpty() + { + if (_vectorSize <= _size) + _Grow(_vectorSize + grow); + if (useConstruct) + return *(new(_vector + (_size++)) TYPE); + else + return _vector[_size++]; + } + inline TYPE* AddEmpty(IDX numElems) + { + const IDX newSize = _size + numElems; + if (_vectorSize < newSize) + _Grow(newSize + grow); + Type* const pData = _vector + _size; + _ArrayConstruct(pData, numElems); + _size = newSize; + return pData; + } + + // Adds a new empty element at the end of the array and pass the arguments to its constructor. + #ifdef _SUPPORT_CPP11 + template + inline TYPE& AddConstruct(Args&&... args) + { + if (_vectorSize <= _size) + _Grow(_vectorSize + grow); + return *(new(_vector + (_size++)) TYPE(std::forward(args)...)); + } + #else + template + inline TYPE& AddConstruct(Args... args) + { + if (_vectorSize <= _size) + _Grow(_vectorSize + grow); + return *(new(_vector + (_size++)) TYPE(args...)); + } + #endif + + inline IDX InsertEmpty() + { + if (_vectorSize <= _size) + _Grow(_vectorSize + grow); + if (useConstruct) + new(_vector + _size) TYPE; + return _size++; + } + + // Adds the new element at the end of the array. + #ifdef _SUPPORT_CPP11 + template + inline void Insert(T&& elem) + { + if (_vectorSize <= _size) + _Grow(_vectorSize + grow); + if (useConstruct) + new(_vector+(_size++)) TYPE(std::forward(elem)); + else + _vector[_size++] = std::forward(elem); + } + #else + inline void Insert(ARG_TYPE elem) + { + if (_vectorSize <= _size) + _Grow(_vectorSize + grow); + if (useConstruct) + new(_vector+(_size++)) TYPE(elem); + else + _vector[_size++] = elem; + } + #endif + + #ifdef _SUPPORT_CPP11 + template + inline void SetAt(IDX index, T&& elem) + { + if (_size <= index) + Resize(index + 1); + _vector[index] = std::forward(elem); + } + #else + inline void SetAt(IDX index, ARG_TYPE elem) + { + if (_size <= index) + Resize(index + 1); + _vector[index] = elem; + } + #endif + + #ifdef _SUPPORT_CPP11 + template + inline void AddAt(IDX index, T&& elem) + { + if (index < _size) + return InsertAt(index, std::forward(elem)); + const IDX newSize = index + 1; + if (_vectorSize <= newSize) + _Grow(newSize + grow); + _ArrayConstruct(_vector+_size, index-_size); + if (useConstruct) + new(_vector+index) TYPE(std::forward(elem)); + else + _vector[index] = std::forward(elem); + ++_size; + } + #else + inline void AddAt(IDX index, ARG_TYPE elem) + { + if (index < _size) + return InsertAt(index, elem); + const IDX newSize = index + 1; + if (_vectorSize <= newSize) + _Grow(newSize + grow); + _ArrayConstruct(_vector+_size, index-_size); + if (useConstruct) + new(_vector+index) TYPE(elem); + else + _vector[index] = elem; + ++_size; + } + #endif + + #ifdef _SUPPORT_CPP11 + template + inline void InsertAt(IDX index, T&& elem) + { + if (useConstruct) + new(AllocateAt(index)) TYPE(std::forward(elem)); + else + *AllocateAt(index) = std::forward(elem); + } + #else + inline void InsertAt(IDX index, ARG_TYPE elem) + { + if (useConstruct) + new(AllocateAt(index)) TYPE(elem); + else + *AllocateAt(index) = elem; + } + #endif + + // Same as Insert, but the constructor is not called. + inline TYPE* Allocate() + { + if (_vectorSize <= _size) + _Grow(_vectorSize + grow); + return _vector+_size++; + } + inline TYPE* AllocateAt(IDX index) + { + ASSERT(index <= _size); + const IDX move(_size-index); + Allocate(); + _ArrayMoveConstruct(_vector+index+1, _vector+index, move); + return _vector+index; + } + + inline IDX InsertSort(ARG_TYPE elem) + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE compElem(_vector[i]); + if (elem < compElem) + l2 = i; + else if (compElem < elem) + l1 = i+1; + else { + InsertAt(i, elem); + return i; + } + } + InsertAt(l1, elem); + return l1; + } + + inline IDX InsertSortPtr(ARG_TYPE elem) + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE compElem(_vector[i]); + if (*elem < *compElem) + l2 = i; + else if (*compElem < *elem) + l1 = i+1; + else { + InsertAt(i, elem); + return i; + } + } + InsertAt(l1, elem); + return l1; + } + + inline IDX InsertSort(ARG_TYPE elem, TFncCompare xCompare) + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + const int res(xCompare(_vector+i, &elem)); + if (res == 0) { + InsertAt(i, elem); + return i; + } + if (res < 0) + l1 = i+1; + else + l2 = i; + } + InsertAt(l1, elem); + return l1; + } + + inline TYPE& GetNth(IDX index) + { + ASSERT(index < _size); + TYPE* const nth(Begin()+index); + std::nth_element(Begin(), nth, End()); + return *nth; + } + template ::value,TYPE,REAL>::type> + inline RTYPE GetMedian() + { + ASSERT(_size > 0); + if (_size%2) + return static_cast(GetNth(_size >> 1)); + TYPE* const nth(Begin() + (_size>>1)); + std::nth_element(Begin(), nth, End()); + TYPE* const nth1(nth-1); + std::nth_element(Begin(), nth1, nth); + return (static_cast(*nth1) + static_cast(*nth)) / RTYPE(2); + } + + inline TYPE GetMean() + { + return std::accumulate(Begin(), End(), TYPE(0)) / _size; + } + + inline ArgType GetMax() const { + return *std::max_element(Begin(), End()); + } + template + inline ArgType GetMax(const Functor& functor) const { + return *std::max_element(Begin(), End(), functor); + } + inline IDX GetMaxIdx() const { + return static_cast(std::max_element(Begin(), End()) - Begin()); + } + template + inline IDX GetMaxIdx(const Functor& functor) const { + return static_cast(std::max_element(Begin(), End(), functor) - Begin()); + } + #ifdef _SUPPORT_CPP11 + inline std::pair GetMinMax() const { + const auto minmax(std::minmax_element(Begin(), End())); + return std::pair(*minmax.first, *minmax.second); + } + template + inline std::pair GetMinMax(const Functor& functor) const { + const auto minmax(std::minmax_element(Begin(), End(), functor)); + return std::pair(*minmax.first, *minmax.second); + } + inline std::pair GetMinMaxIdx() const { + const auto minmax(std::minmax_element(Begin(), End())); + return std::make_pair(static_cast(minmax.first-Begin()), static_cast(minmax.second-Begin())); + } + template + inline std::pair GetMinMaxIdx(const Functor& functor) const { + const auto minmax(std::minmax_element(Begin(), End(), functor)); + return std::make_pair(static_cast(minmax.first-Begin()), static_cast(minmax.second-Begin())); + } + #endif + + inline TYPE& PartialSort(IDX index) + { + TYPE* const nth(Begin()+index); + std::partial_sort(Begin(), nth, End()); + return *nth; + } + + inline void Sort() + { + std::sort(Begin(), End()); + } + template + inline void Sort(const Functor& functor) + { + std::sort(Begin(), End(), functor); + } + + inline bool IsSorted() const + { + #ifdef _SUPPORT_CPP11 + return std::is_sorted(Begin(), End()); + #else + if (_size < 2) + return true; + IDX i = _size-1; + do { + ARG_TYPE elem1 = _vector[i]; + ARG_TYPE elem0 = _vector[--i]; + if (elem1 < elem0) + return false; + } while (i > 0); + return true; + #endif + } + template + inline bool IsSorted(const Functor& functor) const + { + #ifdef _SUPPORT_CPP11 + return std::is_sorted(Begin(), End(), functor); + #else + if (_size < 2) + return true; + IDX i = _size-1; + do { + ARG_TYPE elem1 = _vector[i]; + ARG_TYPE elem0 = _vector[--i]; + if (functor(elem1, elem0)) + return false; + } while (i > 0); + return true; + #endif + } + + inline std::pair InsertSortUnique(ARG_TYPE elem) + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE compElem(_vector[i]); + if (elem < compElem) + l2 = i; + else if (compElem < elem) + l1 = i+1; + else + return std::make_pair(i,true); + } + InsertAt(l1, elem); + return std::make_pair(l1, false); + } + + inline std::pair InsertSortUniquePtr(ARG_TYPE elem) + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE compElem(_vector[i]); + if (*elem < *compElem) + l2 = i; + else if (*compElem < *elem) + l1 = i+1; + else + return std::make_pair(i, true); + } + InsertAt(l1, elem); + return std::make_pair(l1, false); + } + + inline std::pair InsertSortUnique(ARG_TYPE elem, TFncCompare xCompare) + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + const int res(xCompare(_vector+i, &elem)); + if (res == 0) + return std::make_pair(i, true); + if (res < 0) + l1 = i + 1; + else + l2 = i; + } + InsertAt(l1, elem); + return std::make_pair(l1, false); + } + + // insert given element if it is in the top N + template + inline void StoreTop(ARG_TYPE elem) { + const IDX idx(FindFirstEqlGreater(elem)); + if (idx < _size) { + if (_size >= N) + RemoveLast(); + InsertAt(idx, elem); + } else if (_size < N) { + Insert(elem); + } + } + inline void StoreTop(ARG_TYPE elem, IDX N) { + const IDX idx(FindFirstEqlGreater(elem)); + if (idx < _size) { + if (_size >= N) + RemoveLast(); + InsertAt(idx, elem); + } else if (_size < N) { + Insert(elem); + } + } + + inline IDX FindFirst(ARG_TYPE searchedKey) const + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (searchedKey < key) + l2 = i; + else if (key < searchedKey) + l1 = i + 1; + else + return i; + } + return NO_INDEX; + } + + inline IDX FindFirstPtr(ARG_TYPE searchedKey) const + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (*searchedKey < *key) + l2 = i; + else if (*key < *searchedKey) + l1 = i + 1; + else + return i; + } + return NO_INDEX; + } + + template + inline IDX FindFirst(const SEARCH_TYPE& searchedKey) const + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (key == searchedKey) + return i; + if (key < searchedKey) + l1 = i + 1; + else + l2 = i; + } + return NO_INDEX; + } + + template + inline IDX FindFirstPtr(const SEARCH_TYPE& searchedKey) const + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (*key == searchedKey) + return i; + if (*key < searchedKey) + l1 = i + 1; + else + l2 = i; + } + return NO_INDEX; + } + + inline IDX FindFirst(const void* searchedKey, TFncCompare xCompare) const + { + IDX l1(0), l2(_size); + while (l1 < l2) { + IDX i((l1 + l2) >> 1); + const int res(xCompare(_vector+i, searchedKey)); + if (res == 0) + return i; + if (res < 0) + l1 = i + 1; + else + l2 = i; + } + return NO_INDEX; + } + + inline IDX FindFirstBelow(ARG_TYPE searchedKey) const + { + if (_size == 0) return NO_INDEX; + IDX l1(0), l2(_size); + do { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (searchedKey < key) + l2 = i; + else if (key < searchedKey) + l1 = i+1; + else { + while (i-- && !(_vector[i] < searchedKey)); + return i; + } + } while (l1 < l2); + return l1-1; + } + + template + inline IDX FindFirstBelow(const SEARCH_TYPE& searchedKey) const + { + if (_size == 0) return NO_INDEX; + IDX l1(0), l2(_size); + do { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (key == searchedKey) { + while (i-- && _vector[i] == searchedKey); + return i; + } + if (key < searchedKey) + l1 = i+1; + else + l2 = i; + } while (l1 < l2); + return l1-1; + } + + inline IDX FindFirstEqlGreater(ARG_TYPE searchedKey) const + { + if (_size == 0) return 0; + IDX l1(0), l2(_size); + do { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (searchedKey < key) + l2 = i; + else if (key < searchedKey) + l1 = i+1; + else { + while (i-- && !(_vector[i] < searchedKey)); + return i+1; + } + } while (l1 < l2); + return l1; + } + + template + inline IDX FindFirstEqlGreater(const SEARCH_TYPE& searchedKey) const + { + if (_size == 0) return 0; + IDX l1(0), l2(_size); + do { + IDX i((l1 + l2) >> 1); + ARG_TYPE key(_vector[i]); + if (key == searchedKey) { + while (i-- && _vector[i] == searchedKey); + return i+1; + } + if (key < searchedKey) + l1 = i+1; + else + l2 = i; + } while (l1 < l2); + return l1; + } + + // find the matching "elem" by brute-force (front to back) + // returns the first element found + inline IDX Find(ARG_TYPE elem) const + { + for (IDX i = 0; i < _size; ++i) + if (_vector[i] == elem) + return i; + return NO_INDEX; + } + template + inline IDX Find(const SEARCH_TYPE& searchedKey) const + { + for (IDX i = 0; i < _size; ++i) + if (_vector[i] == searchedKey) + return i; + return NO_INDEX; + } + template + inline IDX FindFunc(const Functor& functor) const + { + for (IDX i = 0; i < _size; ++i) + if (functor(_vector[i])) + return i; + return NO_INDEX; + } + + // find the matching "elem" by brute-force (back to front) + // returns the first element found + inline IDX RFind(ARG_TYPE elem) const + { + IDX i(_size); + while (i) + if (_vector[--i] == elem) + return i; + return NO_INDEX; + } + template + inline IDX RFind(const SEARCH_TYPE& searchedKey) const + { + IDX i(_size); + while (i) + if (_vector[--i] == searchedKey) + return i; + return NO_INDEX; + } + template + inline IDX RFindFunc(const Functor& functor) const + { + IDX i(_size); + while (i) + if (functor(_vector[--i])) + return i; + return NO_INDEX; + } + + // call a function or lambda on each element + template + inline void ForEach(const Functor& functor) const + { + FOREACH(i, *this) + functor(i); + } + template + inline void ForEachPtr(const Functor& functor) const + { + FOREACHPTR(ptr, *this) + functor(ptr); + } + template + inline void ForEachRef(const Functor& functor) const + { + FOREACHPTR(ptr, *this) + functor(*ptr); + } + // same, but in reverse order + template + inline void RForEach(const Functor& functor) const + { + RFOREACH(i, *this) + functor(i); + } + template + inline void RForEachPtr(const Functor& functor) const + { + RFOREACHPTR(ptr, *this) + functor(ptr); + } + template + inline void RForEachRef(const Functor& functor) const + { + RFOREACHPTR(ptr, *this) + functor(*ptr); + } + + // Erase each element matching "elem". + inline void Remove(ARG_TYPE elem) + { + IDX i = _size; + while (i) + if (_vector[--i] == elem) + RemoveAt(i); + } + + inline ARG_TYPE RemoveTail() + { + ASSERT(_size); + ASSERT(!useConstruct); + return _vector[--_size]; + } + + inline void RemoveLast() + { + ASSERT(_size); + _ArrayDestruct(_vector+(--_size), 1); + } + + inline void RemoveLast(IDX count) + { + ASSERT(count <= _size); + _ArrayDestruct(_vector+(_size-=count), count); + } + + inline void RemoveAt(IDX index) + { + ASSERT(index < _size); + if (index+1 == _size) + RemoveLast(); + else + _ArrayMoveCopy(_vector+index, _vector+(--_size), 1); + } + + inline void RemoveAt(IDX index, IDX count) + { + ASSERT(index+count <= _size); + if (index+count == _size) { + RemoveLast(count); + } else { + _size -= count; + const IDX move(_size-index); + TYPE* const vectorEnd(_vector+_size); + if (move < count) { + const IDX del(count-move); + _ArrayMoveCopy(_vector+index, vectorEnd+del, move); + _ArrayDestruct(vectorEnd, del); + } else { + _ArrayMoveCopy(_vector+index, vectorEnd, count); + } + } + } + + inline void RemoveAtMove(IDX index) + { + ASSERT(index < _size); + if (index+1 == _size) { + RemoveLast(); + } else { + _ArrayDestruct(_vector+index, 1); + _ArrayMoveConstructFwd(_vector+index, _vector+index+1, (--_size)-index); + } + } + + inline void RemoveAtMove(IDX index, IDX count) + { + ASSERT(index+count <= _size); + if (index+count == _size) { + RemoveLast(count); + } else { + _ArrayDestruct(_vector+index, count); + _ArrayMoveConstructFwd(_vector+index, _vector+index+count, (_size-=count)-index); + } + } + + inline void Empty() + { + _ArrayDestruct(_vector, _size); + _size = 0; + } + + // same as Empty(), plus free all allocated memory + inline void Release() + { + _Release(); + _Init(); + } + + // Discard all stored data and initialize it as an empty array. + // (note: call this only when you know what you are doing, + // you have to deallocate yourself all elements and _vector data) + inline void Reset() + { + _Init(); + } + + // Delete also the pointers (take care to use this function only if the elements are pointers). + inline void EmptyDelete() + { + while (_size) + delete _vector[--_size]; + } + + // same as EmptyDelete(), plus free all allocated memory + inline void ReleaseDelete() + { + EmptyDelete(); + operator delete[] (_vector); + _vector = NULL; + _vectorSize = 0; + } + + inline IDX GetCapacity() const + { + return _vectorSize; + } + + inline IDX GetSize() const + { + return _size; + } + + inline bool IsEmpty() const + { + return (_size == 0); + } + + static inline unsigned GetConstructType() + { + return useConstruct; + } + static inline IDX GetGrowSize() + { + return grow; + } + +protected: + // Free all memory. + inline void _Release() + { + _ArrayDestruct(_vector, _size); + operator delete[] (_vector); + } + + // Initialize array. + inline void _Init() + { + _vector = NULL; + _vectorSize = _size = 0; + } + + // Increase the size of the array at least to the specified amount. + inline void _Grow(IDX newVectorSize) + { + ASSERT(newVectorSize > _vectorSize); + // grow by 50% or at least to minNewVectorSize + const IDX expoVectorSize(_vectorSize + (_vectorSize>>1)); + if (newVectorSize < expoVectorSize) + newVectorSize = expoVectorSize; + // allocate a larger chunk of memory, copy the data and delete the old chunk + TYPE* const tmp(_vector); + _vector = (TYPE*) operator new[] (newVectorSize * sizeof(TYPE)); + _ArrayMoveConstruct(_vector, tmp, _size); + _vectorSize = newVectorSize; + operator delete[] (tmp); + } + + // Decrease the size of the array at the specified new size. + inline void _Shrink(IDX newSize) + { + ASSERT(newSize <= _size); + _ArrayDestruct(_vector+newSize, _size-newSize); + _size = newSize; + } + inline void _ShrinkExact(IDX newSize) + { + _Shrink(newSize); + _vectorSize = newSize; + if (newSize == 0) { + operator delete[] (_vector); + _vector = NULL; + } else { + TYPE* const tmp(_vector); + _vector = (TYPE*) operator new[] (_vectorSize * sizeof(TYPE)); + _ArrayMoveConstruct(_vector, tmp, _vectorSize); + operator delete[] (tmp); + } + } + + // Implement construct/destruct for the array elements. + static inline void _ArrayConstruct(TYPE* RESTRICT dst, IDX n) + { + if (useConstruct) { + while (n--) + new(dst+n) TYPE; + } + } + static inline void _ArrayCopyConstruct(TYPE* RESTRICT dst, const TYPE* RESTRICT src, IDX n) + { + if (useConstruct) { + while (n--) + new(dst+n) TYPE(src[n]); + } else { + memcpy((void*)dst, (const void*)src, n*sizeof(TYPE)); + } + } + static inline void _ArrayDestruct(TYPE* dst, IDX n) + { + if (useConstruct) { + while (n--) + (dst+n)->~TYPE(); + } + } + // Implement copy/move for the array elements. + static inline void _ArrayCopyRestrict(TYPE* RESTRICT dst, const TYPE* RESTRICT src, IDX n) + { + if (useConstruct) { + while (n--) + dst[n] = src[n]; + } else { + memcpy((void*)dst, (const void*)src, n*sizeof(TYPE)); + } + } + static inline void _ArrayMoveConstructFwd(TYPE* dst, TYPE* src, IDX n) + { + ASSERT(dst != src); + if (useConstruct > 1) { + for (IDX i=0; i~TYPE(); + } + } else { + memmove((void*)dst, (const void*)src, n*sizeof(TYPE)); + } + } + template + static inline void _ArrayMoveConstruct(TYPE* dst, TYPE* src, IDX n) + { + ASSERT(dst != src); + if (useConstruct > 1) { + while (n--) { + new(dst+n) TYPE(src[n]); + (src+n)->~TYPE(); + } + } else { + const size_t _size(sizeof(TYPE)*n); + if (bRestrict) + memcpy((void*)dst, (const void*)src, _size); + else + memmove((void*)dst, (const void*)src, _size); + } + } + template + static inline void _ArrayMoveCopy(TYPE* dst, TYPE* src, IDX n) + { + ASSERT(dst != src); + if (useConstruct > 1) { + while (n--) { + dst[n] = src[n]; + (src+n)->~TYPE(); + } + } else { + const size_t _size(sizeof(TYPE)*n); + if (useConstruct == 1) + while (n--) + (dst+n)->~TYPE(); + if (bRestrict) + memcpy((void*)dst, (const void*)src, _size); + else + memmove((void*)dst, (const void*)src, _size); + } + } + +protected: + IDX _size; + IDX _vectorSize; + TYPE* _vector; + +public: + enum : IDX { NO_INDEX = DECLARE_NO_INDEX(IDX) }; + +#if _USE_VECTORINTERFACE != 0 +public: + typedef IDX size_type; + typedef Type value_type; + typedef value_type* iterator; + typedef const value_type* const_iterator; + typedef value_type& reference; + typedef const value_type& const_reference; + typedef std::vector VectorType; + inline cList(const VectorType& rList) { CopyOf(&rList[0], rList.size()); } + #ifdef _SUPPORT_CPP11 + inline cList(std::initializer_list l) : _size(0), _vectorSize((size_type)l.size()), _vector(NULL) { ASSERT(l.size()_vector, elem); } + #ifdef _SUPPORT_CPP11 + template + inline reference emplace_back(Args&&... args) { return AddConstruct(std::forward(args)...); } + inline void push_back(value_type&& elem) { AddConstruct(elem); } + #endif + inline void push_back(const_reference elem) { Insert(elem); } + inline void pop_back() { RemoveLast(); } + inline void reserve(size_type newSize) { Reserve(newSize); } + inline void resize(size_type newSize) { Resize(newSize); } + inline void erase(const_iterator it) { RemoveAtMove(it-this->_vector); } + inline const_iterator cdata() const { return GetData(); } + inline const_iterator cbegin() const { return Begin(); } + inline const_iterator cend() const { return End(); } + inline iterator data() const { return GetData(); } + inline iterator begin() const { return Begin(); } + inline iterator end() const { return End(); } + inline ArgType front() const { return First(); } + inline ArgType back() const { return Last(); } + inline reference front() { return First(); } + inline reference back() { return Last(); } + inline void swap(cList& rList) { Swap(rList); } +#endif + +#ifdef _USE_BOOST +protected: + // implement BOOST serialization + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int /*version*/) const { + ar & _size; + ar & boost::serialization::make_array(_vector, _size); + } + template + void load(Archive& ar, const unsigned int /*version*/) { + IDX newSize; + ar & newSize; + Resize(newSize); + ar & boost::serialization::make_array(_vector, _size); + } + BOOST_SERIALIZATION_SPLIT_MEMBER() +#endif +}; +/*----------------------------------------------------------------*/ + +template +inline bool ValidIDX(const IDX_TYPE& idx) { + return (idx != DECLARE_NO_INDEX(IDX_TYPE)); +} +/*----------------------------------------------------------------*/ + + +// some test functions +template +inline bool cListTest(unsigned iters) { + for (unsigned i=0; i arrR; + cList arr0; + cList arr1; + cList arr2; + cList arrC; + for (unsigned i=0; i= arrR.size()) + continue; + arrR.erase(arrR.begin()+nDel, arrR.begin()+nDel+nCount); + arr0.RemoveAtMove(nDel, nCount); + arr1.RemoveAtMove(nDel, nCount); + arr2.RemoveAtMove(nDel, nCount); + } + if (arrR.size() != arr0.GetSize() || + arrR.size() != arr1.GetSize() || + arrR.size() != arr2.GetSize()) { + ASSERT("there is a problem" == NULL); + return false; + } + for (size_t i=0; i arrS(1+RAND()%(2*elems)); + for (size_t i=0; i<6; ++i) { + arrS.Insert(RAND()); + } + arrS.RemoveLast(RAND()%arrS.GetSize()); + arrS.CopyOf(&arrR[0], arrR.size()); + for (size_t i=0; i<6; ++i) { + arrS.Insert(RAND()); + } + arrS.RemoveLast(6); + for (size_t i=0; i +class cListFixed { +public: + typedef TYPE Type; + typedef const TYPE& ArgType; + typedef unsigned IDX; + enum {MAX_SIZE = N}; + + inline cListFixed() : _size(0) {} + inline void CopyOf(const TYPE* pData, IDX nSize) { + memcpy(_vector, pData, nSize); + _size = nSize; + } + inline bool IsEmpty() const { + return (_size == 0); + } + inline IDX GetSize() const { + return _size; + } + inline const TYPE* Begin() const { + return _vector; + } + inline TYPE* Begin() { + return _vector; + } + inline const TYPE* End() const { + return _vector+_size; + } + inline TYPE* End() { + return _vector+_size; + } + inline const TYPE& First() const { + ASSERT(_size > 0); + return _vector[0]; + } + inline TYPE& First() { + ASSERT(_size > 0); + return _vector[0]; + } + inline const TYPE& Last() const { + ASSERT(_size > 0); + return _vector[_size-1]; + } + inline TYPE& Last() { + ASSERT(_size > 0); + return _vector[_size-1]; + } + inline const TYPE& operator[](IDX index) const { + ASSERT(index < _size); + return _vector[index]; + } + inline TYPE& operator[](IDX index) { + ASSERT(index < _size); + return _vector[index]; + } + inline bool operator==(const cListFixed& rList) const { + if (_size != rList._size) + return false; + for (IDX i = 0; i < _size; ++i) + if (_vector[i] != rList._vector[i]) + return false; + return true; + } + inline TYPE& AddEmpty() { + ASSERT(_size < N); + return _vector[_size++]; + } + inline void InsertAt(IDX index, ArgType elem) { + ASSERT(_size < N); + memmove(_vector+index+1, _vector+index, sizeof(TYPE)*(_size++ - index)); + _vector[index] = elem; + } + inline void Insert(ArgType elem) { + ASSERT(_size < N); + _vector[_size++] = elem; + } + inline void RemoveLast() { + ASSERT(_size); + --_size; + } + inline void RemoveAt(IDX index) { + ASSERT(index < _size); + if (index+1 == _size) + RemoveLast(); + else + memcpy(_vector+index, _vector+(--_size), sizeof(TYPE)); + } + inline void Empty() { + _ArrayDestruct(_vector, _size); + _size = 0; + } + inline void Sort() { + std::sort(Begin(), End()); + } + inline IDX Find(ArgType elem) const + { + IDX i = _size; + while (i) + if (_vector[--i] == elem) + return i; + return NO_INDEX; + } + inline IDX FindFirstEqlGreater(ArgType searchedKey) const { + if (_size == 0) return 0; + IDX l1 = 0, l2 = _size; + do { + IDX i = (l1 + l2) >> 1; + const TYPE& key = _vector[i]; + if (key == searchedKey) { + while (i-- && _vector[i] == searchedKey); + return i+1; + } + if (key < searchedKey) + l1 = i+1; + else + l2 = i; + } while (l1 < l2); + return l1; + } + inline void StoreTop(ArgType elem) { + const IDX idx(FindFirstEqlGreater(elem)); + if (idx < GetSize()) { + if (GetSize() >= N) + RemoveLast(); + InsertAt(idx, elem); + } else if (GetSize() < N) { + Insert(elem); + } + } + +protected: + TYPE _vector[N]; + IDX _size; + +public: + static const IDX NO_INDEX; + +#ifdef _USE_BOOST +protected: + // implement BOOST serialization + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & _size; + ar & boost::serialization::make_array(_vector, _size); + } +#endif +}; +template +const typename cListFixed::IDX cListFixed::NO_INDEX(DECLARE_NO_INDEX(IDX)); +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_LIST_H__ diff --git a/libs/Common/Log.cpp b/libs/Common/Log.cpp new file mode 100644 index 0000000..da19bd1 --- /dev/null +++ b/libs/Common/Log.cpp @@ -0,0 +1,453 @@ +//////////////////////////////////////////////////////////////////// +// Log.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "Log.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +/*-----------------------------------------------------------* + * Log class implementation * + *-----------------------------------------------------------*/ + +#ifndef DEFAULT_LOGTYPE +Log::LogType Log::g_appType; +#endif + +/** + * Constructor + */ +Log::Log() +{ + // generate default log type + #ifndef DEFAULT_LOGTYPE + String appName = Util::getAppName(); + appName = (Util::getFileExt(appName) == _T(".exe") ? Util::getFileName(appName) : Util::getFileNameExt(appName)); + Idx n = MINF((Idx)appName.length(), (Idx)LOGTYPE_SIZE); + _tcsncpy(g_appType.szName, appName, n); + while (n < LOGTYPE_SIZE) + g_appType.szName[n++] = _T(' '); + g_appType.szName[LOGTYPE_SIZE] = _T('\0'); + #endif + ResetTypes(); +} + +void Log::RegisterListener(ClbkRecordMsg clbk) +{ + ASSERT(m_arrRecordClbk != NULL); + if (m_arrRecordClbk == NULL) + return; + m_arrRecordClbk->Insert(clbk); +} +void Log::UnregisterListener(ClbkRecordMsg clbk) +{ + ASSERT(m_arrRecordClbk != NULL); + if (m_arrRecordClbk == NULL) + return; + m_arrRecordClbk->Remove(clbk); +} + +//Register a new type of log messages (LOGTYPE_SIZE chars) +Log::Idx Log::RegisterType(LPCTSTR lt) +{ + ASSERT(strlen(lt) == LOGTYPE_SIZE); + const Idx idx = (Idx)m_arrLogTypes.GetSize(); + LogType& logType = m_arrLogTypes.AddEmpty(); + Idx n = MINF((Idx)strlen(lt), (Idx)LOGTYPE_SIZE); + _tcsncpy(logType.szName, lt, n); + while (n < LOGTYPE_SIZE) + logType.szName[n++] = _T(' '); + logType.szName[LOGTYPE_SIZE] = _T('\0'); + return idx; +} + +/** + * Empty the array with registered log types + */ +void Log::ResetTypes() +{ + m_arrLogTypes.Empty(); +} + +void Log::Write(LPCTSTR szFormat, ...) +{ + if (m_arrRecordClbk == NULL) + return; + va_list args; + va_start(args, szFormat); + _Record(NO_ID, szFormat, args); + va_end(args); +} +void Log::Write(Idx lt, LPCTSTR szFormat, ...) +{ + if (m_arrRecordClbk == NULL) + return; + va_list args; + va_start(args, szFormat); + _Record(lt, szFormat, args); + va_end(args); +} + +/** + * Write message to the log if this exists + * -> IN: Idx - log type + * LPCTSTR - format message + * ... - values + */ +void Log::_Record(Idx lt, LPCTSTR szFormat, va_list args) +{ + ASSERT(m_arrRecordClbk != NULL); + if (m_arrRecordClbk->IsEmpty()) + return; + + // Format a message by adding the date (auto adds new line) + TCHAR szTime[256]; + TCHAR szBuffer[2048]; + #if defined(LOG_DATE) || defined(LOG_TIME) + TCHAR* szPtrTime = szTime; + #ifdef _MSC_VER + SYSTEMTIME st; + GetLocalTime(&st); + #ifdef LOG_THREAD + WLock l(m_lock); + #endif + #ifdef LOG_DATE + szPtrTime += GetDateFormat(LOCALE_USER_DEFAULT,0,&st,_T("dd'.'MM'.'yy"),szPtrTime,80)-1; + #endif + #if defined(LOG_DATE) && defined(LOG_TIME) + szPtrTime[0] = _T('-'); ++szPtrTime; + #endif + #ifdef LOG_TIME + szPtrTime += GetTimeFormat(LOCALE_USER_DEFAULT,0,&st,_T("HH':'mm':'ss"),szPtrTime,80)-1; + #endif + #else // _MSC_VER + const time_t t = time(NULL); + const struct tm *tmp = localtime(&t); + #ifdef LOG_DATE + szPtrTime += strftime(szPtrTime, 80, "%y.%m.%d", tmp); + #endif + #if defined(LOG_DATE) && defined(LOG_TIME) + szPtrTime[0] = _T('-'); ++szPtrTime; + #endif + #ifdef LOG_TIME + szPtrTime += strftime(szPtrTime, 80, "%H:%M:%S", tmp); + #endif + #endif // _MSC_VER + #endif // LOG_DATE || LOG_TIME + #ifdef DEFAULT_LOGTYPE + LPCTSTR const logType(lt 2048) { + // not enough space for the full string, reprint dynamically + m_message.FormatSafe("%s [%s] %s" LINE_SEPARATOR_STR, szTime, logType, String::FormatStringSafe(szFormat, args).c_str()); + } else { + // enough space for all the string, print directly + m_message.Format("%s [%s] %s" LINE_SEPARATOR_STR, szTime, logType, szBuffer); + } + TRACE(m_message); + + // signal listeners + FOREACHPTR(pClbk, *m_arrRecordClbk) + (*pClbk)(m_message); +} +/*----------------------------------------------------------------*/ + + +/*-----------------------------------------------------------* + * LogFile class implementation * + *-----------------------------------------------------------*/ + +/** + * Constructor + */ +LogFile::LogFile() +{ +} + +bool LogFile::Open(LPCTSTR logName) +{ + Util::ensureFolder(logName); + m_ptrFile = new File(logName, File::WRITE, File::CREATE | File::TRUNCATE); + if (!m_ptrFile->isOpen()) { + m_ptrFile = NULL; + return false; + } + GET_LOG().RegisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogFile::Record, this)); + return true; +} +void LogFile::Close() +{ + if (m_ptrFile == NULL) + return; + GET_LOG().UnregisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogFile::Record, this)); + m_ptrFile = NULL; +} + +void LogFile::Pause() +{ + if (m_ptrFile == NULL) + return; + GET_LOG().UnregisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogFile::Record, this)); +} +void LogFile::Play() +{ + if (m_ptrFile == NULL) + return; + GET_LOG().RegisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogFile::Record, this)); +} + +void LogFile::Record(const String& msg) +{ + ASSERT(m_ptrFile != NULL); + m_ptrFile->print(msg); + m_ptrFile->flush(); +} +/*----------------------------------------------------------------*/ + + +/*-----------------------------------------------------------* + * LogConsole class implementation * + *-----------------------------------------------------------*/ + +#define MAX_CONSOLE_WIDTH 100 +#define MAX_CONSOLE_LINES 2000 + +#ifdef _MSC_VER +#include +#define STDIN_FILENO 0 /* file descriptor for stdin */ +#define STDOUT_FILENO 1 /* file descriptor for stdout */ +#define STDERR_FILENO 2 /* file descriptor for stderr */ +#else +#include +#endif +#include + +class LogConsoleOutbuf : public std::streambuf { +public: + LogConsoleOutbuf() { + setp(0, 0); + } + + virtual int_type overflow(int_type c = traits_type::eof()) { + return fputc(c, stdout) == EOF ? traits_type::eof() : c; + } +}; + +/** + * Constructor + */ +LogConsole::LogConsole() + : + #ifdef _USE_COSOLEFILEHANDLES + m_fileIn(NULL), m_fileOut(NULL), m_fileErr(NULL), + #else + m_fileIn(-1), m_fileOut(-1), m_fileErr(-1), + #endif + m_cout(NULL), m_coutOld(NULL), + m_cerr(NULL), m_cerrOld(NULL), + bManageConsole(false) +{ +} + +bool LogConsole::IsOpen() const +{ + #ifdef _USE_COSOLEFILEHANDLES + return (m_fileOut != NULL || m_fileIn != NULL || m_fileErr != NULL); + #else + return (m_fileOut != -1 || m_fileIn != -1 || m_fileErr != -1); + #endif +} + +#ifdef _MSC_VER + +void LogConsole::Open() +{ + if (IsOpen()) + return; + + // allocate a console for this app + bManageConsole = (AllocConsole()!=FALSE?true:false); + + // capture std::cout and std::cerr + if (bManageConsole && !m_cout) { + // set std::cout to use our custom streambuf + m_cout = new LogConsoleOutbuf; + m_coutOld = std::cout.rdbuf(m_cout); + // use same buffer for std::cerr as well + m_cerr = NULL; + m_cerrOld = std::cerr.rdbuf(m_cout); + } + + // set the screen buffer to be big enough to let us scroll text + CONSOLE_SCREEN_BUFFER_INFO coninfo; + GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &coninfo); + coninfo.dwSize.X = MAX_CONSOLE_WIDTH; // does not resize the console window + coninfo.dwSize.Y = MAX_CONSOLE_LINES; + SetConsoleScreenBufferSize(GetStdHandle(STD_OUTPUT_HANDLE), coninfo.dwSize); + + #if 1 && defined(_USE_COSOLEFILEHANDLES) + // redirect standard stream to the console + m_fileIn = freopen("CONIN$", "r", stdin); + m_fileOut = freopen("CONOUT$", "w", stdout); + // there isn't any CONERR$, so that we merge stderr into CONOUT$ + // http://msdn.microsoft.com/en-us/library/windows/desktop/ms683231%28v=vs.85%29.aspx + m_fileErr = freopen("CONOUT$", "w", stderr); + #elif 1 && defined(_USE_COSOLEFILEHANDLES) + // Fix up the allocated console so that output works in all cases. + // Change std handles to refer to new console handles. Before doing so, ensure + // that stdout/stderr haven't been redirected to a valid file + // See http://support.microsoft.com/kb/105305/en-us + // stdout + if (_fileno(stdout) == -1 || _get_osfhandle(fileno(stdout)) == -1) { + int hCrt = ::_open_osfhandle((intptr_t)GetStdHandle(STD_OUTPUT_HANDLE), _O_TEXT); + if (hCrt != -1) { + m_fileOut = ::_fdopen(hCrt, "w"); + if (m_fileOut) + *stdout = *m_fileOut; + } + } + // stderr + if (_fileno(stderr) == -1 || _get_osfhandle(fileno(stderr)) == -1) { + int hCrt = ::_open_osfhandle((intptr_t)::GetStdHandle(STD_ERROR_HANDLE), _O_TEXT); + if (hCrt != -1) { + m_fileErr = ::_fdopen(hCrt, "w"); + if (m_fileErr) + *stderr = *m_fileErr; + } + } + // stdin + if (_fileno(stdin) == -1 || _get_osfhandle(fileno(stdin)) == -1) { + int hCrt = ::_open_osfhandle((intptr_t)::GetStdHandle(STD_INPUT_HANDLE), _O_TEXT); + if (hCrt != -1) { + m_fileIn = ::_fdopen(hCrt, "r"); + if (m_fileIn) + *stdin = *m_fileIn; + } + } + #else + int hConHandle; + // redirect unbuffered STDIN to the console + hConHandle = _open_osfhandle((intptr_t)GetStdHandle(STD_INPUT_HANDLE), _O_TEXT); + m_fileIn = _dup(STDIN_FILENO); + _dup2(hConHandle, STDIN_FILENO); + _close(hConHandle); + setvbuf(stdin, NULL, _IONBF, 0); + // redirect unbuffered STDOUT to the console + hConHandle = _open_osfhandle((intptr_t)GetStdHandle(STD_OUTPUT_HANDLE), _O_TEXT); + m_fileOut = _dup(STDOUT_FILENO); + _dup2(hConHandle, STDOUT_FILENO); + _close(hConHandle); + setvbuf(stdout, NULL, _IONBF, 0); + // redirect unbuffered STDERR to the console + hConHandle = _open_osfhandle((intptr_t)GetStdHandle(STD_ERROR_HANDLE), _O_TEXT); + m_fileErr = _dup(STDERR_FILENO); + _dup2(hConHandle, STDERR_FILENO); + _close(hConHandle); + setvbuf(stderr, NULL, _IONBF, 0); + // make cout, wcout, cin, wcin, wcerr, cerr, wclog and clog + // point to console as well + std::ios::sync_with_stdio(); + #endif + + // register with our log system + GET_LOG().RegisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogConsole::Record, this)); +} + +void LogConsole::Close() +{ + if (!IsOpen()) + return; + GET_LOG().UnregisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogConsole::Record, this)); + #ifdef _USE_COSOLEFILEHANDLES + // close console stream handles + fclose(m_fileIn); m_fileIn = NULL; + fclose(m_fileOut); m_fileOut = NULL; + fclose(m_fileErr); m_fileErr = NULL; + #else + // restore STDIN + _dup2(m_fileIn, STDIN_FILENO); + _close(m_fileIn); + m_fileIn = -1; + // restore STDOUT + _dup2(m_fileOut, STDOUT_FILENO); + _close(m_fileOut); + m_fileOut = -1; + // restore STDERR + _dup2(m_fileErr, STDERR_FILENO); + _close(m_fileErr); + m_fileErr = -1; + #endif + // close console + if (bManageConsole) { + if (m_cout) { + // set std::cout to the original streambuf + std::cout.rdbuf(m_coutOld); + std::cerr.rdbuf(m_cerrOld); + delete m_cout; m_cout = NULL; + } + FreeConsole(); + } + #ifndef _USE_COSOLEFILEHANDLES + std::ios::sync_with_stdio(); + #endif +} + +void LogConsole::Record(const String& msg) +{ + ASSERT(IsOpen()); + printf(msg); + fflush(stdout); +} + +#else + +void LogConsole::Open() +{ + if (IsOpen()) + return; + ++m_fileIn; + // register with our log system + GET_LOG().RegisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogConsole::Record, this)); +} + +void LogConsole::Close() +{ + if (!IsOpen()) + return; + // unregister with our log system + GET_LOG().UnregisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogConsole::Record, this)); + --m_fileIn; +} + +void LogConsole::Record(const String& msg) +{ + ASSERT(IsOpen()); + printf(_T("%s"), msg.c_str()); + fflush(stdout); +} + +#endif // _MSC_VER + +void LogConsole::Pause() +{ + if (IsOpen()) + GET_LOG().UnregisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogConsole::Record, this)); +} +void LogConsole::Play() +{ + if (IsOpen()) + GET_LOG().RegisterListener(DELEGATEBINDCLASS(Log::ClbkRecordMsg, &LogConsole::Record, this)); +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/Log.h b/libs/Common/Log.h new file mode 100644 index 0000000..44eb14d --- /dev/null +++ b/libs/Common/Log.h @@ -0,0 +1,211 @@ +//////////////////////////////////////////////////////////////////// +// Log.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_LOG_H__ +#define __SEACAVE_LOG_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + +// use file handles to access console +#define _USE_COSOLEFILEHANDLES + +//#define LOG_DATE // add date info for every log +#define LOG_TIME // add time info for every log +#define LOG_THREAD // make log multi-thread safe +#define LOG_STREAM // add stream support (operator <<) +#define LOGTYPE_SIZE 8 +#define DEFAULT_LOGTYPE _T("App ") + +#define DECLARE_LOG() \ + protected: static const Log::Idx ms_nLogType; +#define DEFINE_LOG(classname, log) \ + const Log::Idx classname::ms_nLogType(REGISTER_LOG(log)); + +#ifdef LOG_THREAD +#include "CriticalSection.h" +#endif + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class GENERAL_API Log +{ + DECLARE_SINGLETON(Log); + +public: + typedef uint32_t Idx; + typedef DELEGATE ClbkRecordMsg; + typedef cList ClbkRecordMsgArray; + typedef CSharedPtr ClbkRecordMsgArrayPtr; + +public: + // log methods + void Open() { m_arrRecordClbk = new ClbkRecordMsgArray; } + void Close() { m_arrRecordClbk = NULL; } + void Join(Log& log) { m_arrRecordClbk = log.m_arrRecordClbk; } + void RegisterListener(ClbkRecordMsg); + void UnregisterListener(ClbkRecordMsg); + Idx RegisterType(LPCTSTR); + void ResetTypes(); + void Write(LPCTSTR, ...); + void Write(Idx, LPCTSTR, ...); + + #ifdef LOG_STREAM + template inline Log& operator<<(const T& val) { + #ifdef LOG_THREAD + Lock l(m_cs); + std::ostringstream& ostr = m_streams[__THREAD__]; + #else + std::ostringstream& ostr = m_stream; + #endif + ostr << val; + const std::string& line = ostr.str(); + if (!line.empty() && *(line.end()-1) == _T('\n')) { + Write(line.substr(0, line.size()-1).c_str()); + ostr.str(_T("")); + } + return *this; + } + // the type of std::cout + typedef std::basic_ostream > CoutType; + // the function signature of std::endl + typedef CoutType& (*StandardEndLine)(CoutType&); + // define an operator<< to take in std::endl + inline Log& operator<<(StandardEndLine) { + #ifdef LOG_THREAD + Lock l(m_cs); + std::ostringstream& ostr = m_streams[__THREAD__]; + #else + std::ostringstream& ostr = m_stream; + #endif + Write(ostr.str().c_str()); + ostr.str(_T("")); + return *this; + } + #endif + +protected: + // write a message of a certain type to the log + void _Record(Idx, LPCTSTR, va_list); + +protected: + struct LogType { + TCHAR szName[LOGTYPE_SIZE+1]; + inline operator LPCTSTR () const { return szName; } + inline operator LPTSTR () { return szName; } + }; + typedef cList LogTypeArr; + + // log members + String m_message; // last recorded message + ClbkRecordMsgArrayPtr m_arrRecordClbk;// the array with all registered listeners + LogTypeArr m_arrLogTypes; // the array with all the registered log types + + #ifdef LOG_THREAD + // threading + RWLock m_lock; // mutex used to ensure multi-thread safety + #endif + + #ifdef LOG_STREAM + // streaming + #ifdef LOG_THREAD + typedef std::unordered_map StreamMap; + StreamMap m_streams; // stream object used to handle one log with operator << (one for each thread) + CriticalSection m_cs; // mutex used to ensure multi-thread safety for accessing m_streams + #else + std::ostringstream m_stream; // stream object used to handle one log with operator << + #endif + #endif + + // static + #ifndef DEFAULT_LOGTYPE + static LogType g_appType; + #endif +}; +#define GET_LOG() SEACAVE::Log::GetInstance() +#define OPEN_LOG() GET_LOG().Open() +#define CLOSE_LOG() GET_LOG().Close() +#define JOIN_LOG(log) GET_LOG().Join(log) +#define REGISTER_LOG(lt) GET_LOG().RegisterType(lt) +#define LOG GET_LOG().Write +#define SLOG(msg) GET_LOG() << msg +#ifndef _RELEASE +#define LOGV LOG // include extra details in the log +#else +#define LOGV(...) +#endif +/*----------------------------------------------------------------*/ + + +class GENERAL_API LogFile +{ + DECLARE_SINGLETON(LogFile); + +public: + ~LogFile() { Close(); } + + // log methods + bool Open(LPCTSTR); + void Close(); + void Pause(); + void Play(); + void Record(const String&); + +protected: + FilePtr m_ptrFile; // the log file +}; +#define GET_LOGFILE() LogFile::GetInstance() +#define OPEN_LOGFILE(log) GET_LOGFILE().Open(log) +#define CLOSE_LOGFILE() GET_LOGFILE().Close() +/*----------------------------------------------------------------*/ + + +class GENERAL_API LogConsole +{ + DECLARE_SINGLETON(LogConsole); + +public: + ~LogConsole() { Close(); } + + bool IsOpen() const; + + // log methods + void Open(); + void Close(); + void Pause(); + void Play(); + void Record(const String&); + +protected: + #ifdef _USE_COSOLEFILEHANDLES + typedef FILE* StreamHandle; + #else + typedef int StreamHandle; + #endif + StreamHandle m_fileIn; // the log file in + StreamHandle m_fileOut; // the log file out + StreamHandle m_fileErr; // the log file error + std::streambuf* m_cout; // the redirected cout stream + std::streambuf* m_coutOld; // the original cout stream + std::streambuf* m_cerr; // the redirected cerr stream + std::streambuf* m_cerrOld; // the original cout stream + bool bManageConsole; // remember if the console is created here or is an existing console +}; +#define GET_LOGCONSOLE() LogConsole::GetInstance() +#define OPEN_LOGCONSOLE() GET_LOGCONSOLE().Open() +#define CLOSE_LOGCONSOLE() GET_LOGCONSOLE().Close() +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_LOG_H__ diff --git a/libs/Common/MemFile.h b/libs/Common/MemFile.h new file mode 100644 index 0000000..86a0ce1 --- /dev/null +++ b/libs/Common/MemFile.h @@ -0,0 +1,182 @@ +//////////////////////////////////////////////////////////////////// +// MemFile.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_MEMFILE_H__ +#define __SEACAVE_MEMFILE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Streams.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class GENERAL_API MemFile : public IOStream { +public: + MemFile() : m_buffer(NULL), m_sizeBuffer(0), m_size(0), m_pos(0) { + } + MemFile(size_t initialSize) : m_buffer(new BYTE[initialSize]), m_sizeBuffer(initialSize), m_size(0), m_pos(0) { + } + MemFile(BYTE* data, size_t size) : m_buffer(data), m_sizeBuffer(0), m_size(size), m_pos(0) { + } + virtual ~MemFile() { + close(); + } + + static LPCSTR getClassType() { return "MemFile"; } + virtual LPCSTR getClassName() const { return MemFile::getClassType(); } + + bool isOpen() const { return m_buffer != NULL; } + + virtual void close() { + if (!isOpen()) + return; + if (m_sizeBuffer) + delete[] m_buffer; + m_sizeBuffer = 0; + m_buffer = NULL; + m_size = 0; + m_pos = 0; + } + + virtual size_f_t getSizeBuffer() const { + return m_sizeBuffer; + } + + virtual size_f_t getSizeLeft() const { + return m_size - m_pos; + } + + virtual size_f_t getSize() const { + return m_size; + } + + virtual bool setSize(size_f_t newSize) { + ASSERT(newSize >= 0); + if (newSize > m_sizeBuffer) + setMaxSize(newSize); + m_size = newSize; + if (m_pos > m_size) + m_pos = m_size; + return true; + } + + virtual bool growSize(size_f_t deltaSize) { + return setSize(m_size+deltaSize); + } + + virtual bool moveSize(size_f_t delataSize) { + return setSize(m_size + delataSize); + } + + virtual bool setMaxSize(size_f_t newSize) { + ASSERT(newSize > m_sizeBuffer); + // grow by 50% or at least to minNewVectorSize + const size_f_t expoSize(m_sizeBuffer + (m_sizeBuffer>>1)); + if (newSize < expoSize) + newSize = expoSize; + // allocate a larger chunk of memory, copy the data and delete the old chunk + BYTE* const tmp(m_buffer); + m_buffer = new BYTE[(size_t)newSize]; + if (!m_buffer) { + m_buffer = tmp; + return false; + } + if (m_size > newSize) { + m_size = newSize; + if (m_pos > m_size) + m_pos = m_size; + } + memcpy(m_buffer, tmp, (size_t)m_size); + if (m_sizeBuffer) + delete[] tmp; + m_sizeBuffer = newSize; + return true; + } + + virtual bool growMaxSize(size_f_t deltaSize) { + return setMaxSize(m_sizeBuffer+deltaSize); + } + + virtual bool ensureSize(size_f_t extraSize) { + const size_f_t newSize = m_size + extraSize; + if (newSize > m_sizeBuffer) + return setMaxSize(newSize); + return true; + } + + virtual size_f_t getPos() const { + return m_pos; + } + + virtual bool setPos(size_f_t pos) { + if (pos > m_size && !setSize(pos)) + return false; + m_pos = pos; + return true; + } + + virtual bool movePos(size_f_t delataPos) { + return setPos(m_pos + delataPos); + } + + virtual bool isEOF() { + return (m_pos == m_size); + } + + virtual size_t read(void* buf, size_t len) { + if (m_pos >= m_size) + return 0; + if (m_pos+(size_f_t)len > m_size) + len = (size_t)(m_size-m_pos); + memcpy(buf, m_buffer+m_pos, len); + m_pos += len; + return len; + } + + virtual size_t write(const void* buf, size_t len) { + const size_f_t endSize = m_pos+len; + if (endSize > m_size && !setSize(endSize)) + return 0; + memcpy(m_buffer+m_pos, buf, len); + m_pos += len; + return len; + } + + virtual BYTE* getData() { + return m_buffer + m_pos; + } + + virtual BYTE* getBuffer() { + return m_buffer; + } + + virtual size_t flush() { + return 0; + } + +protected: + BYTE* m_buffer; //buffer where we will store the data + size_f_t m_sizeBuffer; //size of the whole buffer, 0 if no buffer or not allocated here + size_f_t m_size; //size of the stored data + size_f_t m_pos; //current position in the stored data + +private: + MemFile(const MemFile&); + MemFile& operator=(const MemFile&); +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_MEMFILE_H__ diff --git a/libs/Common/OBB.h b/libs/Common/OBB.h new file mode 100644 index 0000000..adf321e --- /dev/null +++ b/libs/Common/OBB.h @@ -0,0 +1,127 @@ +//////////////////////////////////////////////////////////////////// +// OBB.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_OBB_H__ +#define __SEACAVE_OBB_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +template +class TAABB; + +template +class TRay; + +// Basic oriented bounding-box class +template +class TOBB +{ + STATIC_ASSERT(DIMS > 0 && DIMS <= 3); + +public: + typedef TYPE Type; + typedef Eigen::Matrix POINT; + typedef Eigen::Matrix MATRIX; + typedef SEACAVE::TAABB AABB; + typedef SEACAVE::TRay RAY; + typedef unsigned ITYPE; + typedef Eigen::Matrix TRIANGLE; + enum { numCorners = (DIMS==1 ? 2 : (DIMS==2 ? 4 : 8)) }; // 2^DIMS + enum { numScalar = (5*DIMS) }; + + MATRIX m_rot; // rotation matrix from world to local (orthonormal axes) + POINT m_pos; // translation from local to world (center-point) + POINT m_ext; // bounding box extents in local (half axis length) + + //--------------------------------------- + + inline TOBB() {} + inline TOBB(bool); + inline TOBB(const AABB&); + inline TOBB(const MATRIX& rot, const POINT& ptMin, const POINT& ptMax); + inline TOBB(const POINT* pts, size_t n); + inline TOBB(const POINT* pts, size_t n, const TRIANGLE* tris, size_t s); + template + inline TOBB(const TOBB&); + + inline void Set(const AABB&); // build from AABB + inline void Set(const MATRIX& rot, const POINT& ptMin, const POINT& ptMax); // build from rotation matrix from world to local, and local min/max corners + inline void Set(const POINT* pts, size_t n); // build from points + inline void Set(const POINT* pts, size_t n, const TRIANGLE* tris, size_t s); // build from triangles + inline void Set(const MATRIX& C, const POINT* pts, size_t n); // build from covariance matrix + inline void SetRotation(const MATRIX& C); // build rotation only from covariance matrix + inline void SetBounds(const POINT* pts, size_t n); // build size and center only from given points + + inline void BuildBegin(); // start online build for computing the rotation + inline void BuildAdd(const POINT&); // add a new point to the online build + inline void BuildEnd(); // end online build for computing the rotation + + inline bool IsValid() const; + + inline TOBB& Enlarge(TYPE); + inline TOBB& EnlargePercent(TYPE); + + inline void Translate(const POINT&); + inline void Transform(const MATRIX&); + + inline POINT GetCenter() const; + inline void GetCenter(POINT&) const; + + inline POINT GetSize() const; + inline void GetSize(POINT&) const; + + inline void GetCorners(POINT pts[numCorners]) const; + inline AABB GetAABB() const; + + inline TYPE GetVolume() const; + + bool Intersects(const POINT&) const; + + inline TYPE& operator [] (BYTE i) { ASSERT(i> (std::istream& st, TOBB& obb) { + st >> obb.m_rot; + st >> obb.m_pos; + st >> obb.m_ext; + return st; + } + + #ifdef _USE_BOOST + // implement BOOST serialization + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & m_rot; + ar & m_pos; + ar & m_ext; + } + #endif +}; // class TOBB +/*----------------------------------------------------------------*/ + + +#include "OBB.inl" +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_OBB_H__ diff --git a/libs/Common/OBB.inl b/libs/Common/OBB.inl new file mode 100644 index 0000000..6a8dd76 --- /dev/null +++ b/libs/Common/OBB.inl @@ -0,0 +1,401 @@ +//////////////////////////////////////////////////////////////////// +// OBB.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +template +inline TOBB::TOBB(bool) + : + m_rot(MATRIX::Identity()), + m_pos(POINT::Zero()), + m_ext(POINT::Zero()) +{ +} +template +inline TOBB::TOBB(const AABB& aabb) +{ + Set(aabb); +} +template +inline TOBB::TOBB(const MATRIX& rot, const POINT& ptMin, const POINT& ptMax) +{ + Set(rot, ptMin, ptMax); +} +template +inline TOBB::TOBB(const POINT* pts, size_t n) +{ + Set(pts, n); +} +template +inline TOBB::TOBB(const POINT* pts, size_t n, const TRIANGLE* tris, size_t s) +{ + Set(pts, n, tris, s); +} // constructor +template +template +inline TOBB::TOBB(const TOBB& rhs) + : + m_rot(rhs.m_rot.template cast()), + m_pos(rhs.m_pos.template cast()), + m_ext(rhs.m_ext.template cast()) +{ +} // copy constructor +/*----------------------------------------------------------------*/ + + +template +inline void TOBB::Set(const AABB& aabb) +{ + m_rot.setIdentity(); + m_pos = aabb.GetCenter(); + m_ext = aabb.GetSize()/TYPE(2); +} + +// build from rotation matrix from world to local, and local min/max corners +template +inline void TOBB::Set(const MATRIX& rot, const POINT& ptMin, const POINT& ptMax) +{ + m_rot = rot; + m_pos = (ptMax + ptMin) * TYPE(0.5); + m_ext = (ptMax - ptMin) * TYPE(0.5); +} + +// Inspired from "Fitting Oriented Bounding Boxes" by James Gregson +// http://jamesgregson.blogspot.ro/2011/03/latex-test.html + +// build an OBB from a vector of input points. This +// method just forms the covariance matrix and hands +// it to the build_from_covariance_matrix method +// which handles fitting the box to the points +template +inline void TOBB::Set(const POINT* pts, size_t n) +{ + ASSERT(n >= DIMS); + + // loop over the points to find the mean point + // location and to build the covariance matrix; + // note that we only have + // to build terms for the upper triangular + // portion since the matrix is symmetric + POINT mu(POINT::Zero()); + TYPE cxx=0, cxy=0, cxz=0, cyy=0, cyz=0, czz=0; + for (size_t i=0; i +inline void TOBB::Set(const POINT* pts, size_t n, const TRIANGLE* tris, size_t s) +{ + ASSERT(n >= DIMS); + + // loop over the triangles this time to find the + // mean location + POINT mu(POINT::Zero()); + TYPE Am=0; + TYPE cxx=0, cxy=0, cxz=0, cyy=0, cyz=0, czz=0; + for (size_t i=0; i +inline void TOBB::Set(const MATRIX& C, const POINT* pts, size_t n) +{ + // extract rotation from the covariance matrix + SetRotation(C); + // extract size and center from the given points + SetBounds(pts, n); +} +// method to set the OBB rotation which produce a box oriented according to +// the covariance matrix C (only the rotations is set) +template +inline void TOBB::SetRotation(const MATRIX& C) +{ + // extract the eigenvalues and eigenvectors from C + const Eigen::SelfAdjointEigenSolver es(C); + ASSERT(es.info() == Eigen::Success); + // find the right, up and forward vectors from the eigenvectors + // and set the rotation matrix using the eigenvectors + ASSERT(es.eigenvalues()(0) < es.eigenvalues()(1) && es.eigenvalues()(1) < es.eigenvalues()(2)); + m_rot = es.eigenvectors().transpose(); + if (m_rot.determinant() < 0) + m_rot = -m_rot; +} +// method to set the OBB center and size that contains the given points +// the rotations should be already set +template +inline void TOBB::SetBounds(const POINT* pts, size_t n) +{ + ASSERT(n >= DIMS); + ASSERT(ISEQUAL((m_rot*m_rot.transpose()).trace(), TYPE(3)) && ISEQUAL(m_rot.determinant(), TYPE(1))); + + // build the bounding box extents in the rotated frame + const TYPE tmax = std::numeric_limits::max(); + POINT minim(tmax, tmax, tmax), maxim(-tmax, -tmax, -tmax); + for (size_t i=0; i p_prime(0)) minim(0) = p_prime(0); + if (minim(1) > p_prime(1)) minim(1) = p_prime(1); + if (minim(2) > p_prime(2)) minim(2) = p_prime(2); + if (maxim(0) < p_prime(0)) maxim(0) = p_prime(0); + if (maxim(1) < p_prime(1)) maxim(1) = p_prime(1); + if (maxim(2) < p_prime(2)) maxim(2) = p_prime(2); + } + + // set the center of the OBB to be the average of the + // minimum and maximum, and the extents be half of the + // difference between the minimum and maximum + const POINT center((maxim+minim)*TYPE(0.5)); + m_pos = m_rot.transpose() * center; + m_ext = (maxim-minim)*TYPE(0.5); +} // Set +/*----------------------------------------------------------------*/ + + +template +inline void TOBB::BuildBegin() +{ + m_rot = MATRIX::Zero(); + m_pos = POINT::Zero(); + m_ext = POINT::Zero(); +} +template +inline void TOBB::BuildAdd(const POINT& p) +{ + // store mean in m_pos + m_pos += p; + // store covariance params in m_rot + m_rot(0,0) += p(0)*p(0); + m_rot(0,1) += p(0)*p(1); + m_rot(0,2) += p(0)*p(2); + m_rot(1,0) += p(1)*p(1); + m_rot(1,1) += p(1)*p(2); + m_rot(1,2) += p(2)*p(2); + // store count in m_ext + ++(*((size_t*)m_ext.data())); +} +template +inline void TOBB::BuildEnd() +{ + const TYPE invN(TYPE(1)/TYPE(*((size_t*)m_ext.data()))); + const TYPE cxx = (m_rot(0,0) - m_pos(0)*m_pos(0)*invN)*invN; + const TYPE cxy = (m_rot(0,1) - m_pos(0)*m_pos(1)*invN)*invN; + const TYPE cxz = (m_rot(0,2) - m_pos(0)*m_pos(2)*invN)*invN; + const TYPE cyy = (m_rot(1,0) - m_pos(1)*m_pos(1)*invN)*invN; + const TYPE cyz = (m_rot(1,1) - m_pos(1)*m_pos(2)*invN)*invN; + const TYPE czz = (m_rot(1,2) - m_pos(2)*m_pos(2)*invN)*invN; + + // now build the covariance matrix + MATRIX C; + C(0,0) = cxx; C(0,1) = cxy; C(0,2) = cxz; + C(1,0) = cxy; C(1,1) = cyy; C(1,2) = cyz; + C(2,0) = cxz; C(2,1) = cyz; C(2,2) = czz; + SetRotation(C); +} // Build +/*----------------------------------------------------------------*/ + + +// check if the oriented bounding box has positive size +template +inline bool TOBB::IsValid() const +{ + return m_ext.minCoeff() > TYPE(0); +} // IsValid +/*----------------------------------------------------------------*/ + + +template +inline TOBB& TOBB::Enlarge(TYPE x) +{ + m_ext.array() += x; + return *this; +} +template +inline TOBB& TOBB::EnlargePercent(TYPE x) +{ + m_ext *= x; + return *this; +} // Enlarge +/*----------------------------------------------------------------*/ + + +// Update the box by the given pos delta. +template +inline void TOBB::Translate(const POINT& d) +{ + m_pos += d; +} +// Update the box by the given transform. +template +inline void TOBB::Transform(const MATRIX& m) +{ + m_rot = m * m_rot; + m_pos = m * m_pos; +} +/*----------------------------------------------------------------*/ + + +template +inline typename TOBB::POINT TOBB::GetCenter() const +{ + return m_pos; +} +template +inline void TOBB::GetCenter(POINT& ptCenter) const +{ + ptCenter = m_pos; +} // GetCenter +/*----------------------------------------------------------------*/ + + +template +inline typename TOBB::POINT TOBB::GetSize() const +{ + return m_ext*2; +} +template +inline void TOBB::GetSize(POINT& ptSize) const +{ + ptSize = m_ext*2; +} // GetSize +/*----------------------------------------------------------------*/ + + +template +inline void TOBB::GetCorners(POINT pts[numCorners]) const +{ + if (DIMS == 2) { + const POINT pEAxis[2] = { + m_rot.row(0)*m_ext[0], + m_rot.row(1)*m_ext[1] + }; + const POINT pos(m_rot.transpose()*m_pos); + pts[0] = pos - pEAxis[0] - pEAxis[1]; + pts[1] = pos + pEAxis[0] - pEAxis[1]; + pts[2] = pos + pEAxis[0] + pEAxis[1]; + pts[3] = pos - pEAxis[0] + pEAxis[1]; + } + if (DIMS == 3) { + const POINT pEAxis[3] = { + m_rot.row(0)*m_ext[0], + m_rot.row(1)*m_ext[1], + m_rot.row(2)*m_ext[2] + }; + const POINT pos(m_rot.transpose()*m_pos); + pts[0] = pos - pEAxis[0] - pEAxis[1] - pEAxis[2]; + pts[1] = pos - pEAxis[0] - pEAxis[1] + pEAxis[2]; + pts[2] = pos + pEAxis[0] - pEAxis[1] - pEAxis[2]; + pts[3] = pos + pEAxis[0] - pEAxis[1] + pEAxis[2]; + pts[4] = pos + pEAxis[0] + pEAxis[1] - pEAxis[2]; + pts[5] = pos + pEAxis[0] + pEAxis[1] + pEAxis[2]; + pts[6] = pos - pEAxis[0] + pEAxis[1] - pEAxis[2]; + pts[7] = pos - pEAxis[0] + pEAxis[1] + pEAxis[2]; + } +} // GetCorners +// constructs the corner of the aligned bounding box in world space +template +inline typename TOBB::AABB TOBB::GetAABB() const +{ + POINT pts[numCorners]; + GetCorners(pts); + return AABB(pts, numCorners); +} // GetAABB +/*----------------------------------------------------------------*/ + +// computes the volume of the OBB, which is a measure of +// how tight the fit is (better OBBs will have smaller volumes) +template +inline TYPE TOBB::GetVolume() const +{ + return m_ext.prod()*numCorners; +} +/*----------------------------------------------------------------*/ + + +template +bool TOBB::Intersects(const POINT& pt) const +{ + const POINT dist(m_rot * (pt - m_pos)); + if (DIMS == 2) { + return ABS(dist[0]) <= m_ext[0] + && ABS(dist[1]) <= m_ext[1]; + } + if (DIMS == 3) { + return ABS(dist[0]) <= m_ext[0] + && ABS(dist[1]) <= m_ext[1] + && ABS(dist[2]) <= m_ext[2]; + } +} // Intersects(POINT) +/*----------------------------------------------------------------*/ diff --git a/libs/Common/Octree.h b/libs/Common/Octree.h new file mode 100644 index 0000000..de1bbad --- /dev/null +++ b/libs/Common/Octree.h @@ -0,0 +1,237 @@ +//////////////////////////////////////////////////////////////////// +// Octree.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_OCTREE_H__ +#define __SEACAVE_OCTREE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "AABB.h" +#include "Ray.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// basic octree class +// each item should define the operator const POINT_TYPE& returning its center; +// at build time, a functor must be supplied that returns true as long as +// the current cell should be split farther, given its number of items and radius, ex: +// [](Octree::IDX_TYPE size, Octree::Type radius) { +// return size > SIZE && radius > RADIUS; +// } +// where: +// SIZE is the minimum number of items contained by the cell so that this to be divided further +// RADIUS is the minimum size of the cell allowed to be divided further +// both conditions represent exclusive limits and both should be true for the division to take place +template +class TOctree +{ + STATIC_ASSERT(DIMS > 0 && DIMS <= 3); + +public: + typedef TYPE Type; + typedef typename ITEMARR_TYPE::Type ITEM_TYPE; + typedef typename ITEMARR_TYPE::IDX IDX_TYPE; + typedef SEACAVE::cList IDXARR_TYPE; + typedef Eigen::Matrix POINT_TYPE; + typedef SEACAVE::TAABB AABB_TYPE; + typedef uint32_t SIZE_TYPE; + + class CELL_TYPE { + public: + typedef struct { + POINT_TYPE center; // center of the current cell + } NODE_TYPE; + typedef struct { + IDX_TYPE idxBegin; // index in the global array of the first item contained by this cell + SIZE_TYPE size; // number of items contained by this cell in the global array + DATA_TYPE data; // user data associated with this leaf + } LEAF_TYPE; + enum { numChildren = (2<<(DIMS-1)) }; + + public: + inline CELL_TYPE(); + inline ~CELL_TYPE(); + + inline void Release(); + inline void Swap(CELL_TYPE&); + + inline unsigned ComputeChild(const POINT_TYPE& item) const; + static void ComputeCenter(POINT_TYPE []); + static inline POINT_TYPE ComputeChildCenter(const POINT_TYPE&, TYPE, unsigned); + + inline bool IsLeaf() const { return (m_child==NULL); } + inline const CELL_TYPE& GetChild(int i) const { ASSERT(!IsLeaf() && i CELLPTRARR_TYPE; + + struct IndexInserter { + IDXARR_TYPE& indices; + IndexInserter(IDXARR_TYPE& _indices) : indices(_indices) {} + void operator()(IDX_TYPE idx) { indices.Insert(idx); } + void operator()(const IDX_TYPE* idices, SIZE_TYPE size) { indices.Join(idices, size); } + }; + + struct CellInserter { + CELLPTRARR_TYPE& cells; + CellInserter(CELLPTRARR_TYPE& _cells) : cells(_cells) {} + void operator()(CELL_TYPE& cell) { cells.Insert(&cell); } + }; + +public: + inline TOctree() {} + template + inline TOctree(const ITEMARR_TYPE&, Functor split); + template + inline TOctree(const ITEMARR_TYPE&, const AABB_TYPE&, Functor split); + + inline void Release(); + inline void Swap(TOctree&); + + template + void Insert(const ITEMARR_TYPE&, Functor split); + template + void Insert(const ITEMARR_TYPE&, const AABB_TYPE&, Functor split); + + template + inline void CollectCells(INSERTER&) const; + template + void CollectCells(const CELL_TYPE&, INSERTER&) const; + template + inline void ParseCells(PARSER&); + + template + inline void Collect(INSERTER& inserter, const AABB_TYPE& aabb) const; + inline void Collect(IDXARR_TYPE& indices, const AABB_TYPE& aabb) const; + inline void Collect(IDX_TYPE maxNeighbors, IDXARR_TYPE& indices, const AABB_TYPE& aabb) const; + + template + inline void Collect(INSERTER& inserter, const POINT_TYPE& center, TYPE radius) const; + inline void Collect(IDXARR_TYPE& indices, const POINT_TYPE& center, TYPE radius) const; + inline void Collect(IDX_TYPE maxNeighbors, IDXARR_TYPE& indices, const POINT_TYPE& center, TYPE radius) const; + + template + inline void Collect(INSERTER& inserter, const COLLECTOR& collector) const; + template + inline void Collect(IDXARR_TYPE& indices, const COLLECTOR& collector) const; + + template + inline void Traverse(const TFrustum&, INSERTER&) const; + template + inline void Traverse(const TFrustum&, IDXARR_TYPE&) const; + template + inline void TraverseCells(const TFrustum&, PARSER&); + template + inline void TraverseCells(const TFrustum&, CELLPTRARR_TYPE&); + + template + void SplitVolume(float maxArea, AREAESTIMATOR& areaEstimator, CHUNKINSERTER& chunkInserter); + + inline const CELL_TYPE& GetRoot() const { return m_root; } + inline TYPE GetRadius() const { return m_radius; } + inline AABB_TYPE GetAabb() const { return m_root.GetAabb(m_radius); } + inline bool IsEmpty() const { return m_indices.empty(); } + inline size_t GetNumItems() const { return m_indices.size(); } + inline const IDXARR_TYPE& GetIndexArr() const { return m_indices; } + inline const ITEM_TYPE* GetItems() const { return m_items; } + inline void ResetItems() { m_items = NULL; } + +protected: + template + struct _InsertData { + enum : IDX_TYPE { NO_INDEX = DECLARE_NO_INDEX(IDX_TYPE) }; + IDXARR_TYPE successors; // single connected list of next item indices + Functor split; // used to decide if a cell needs to be split farther + }; + template + void _Insert(CELL_TYPE&, const POINT_TYPE& center, TYPE radius, IDX_TYPE start, IDX_TYPE size, _InsertData&); + + template + void _ParseCells(CELL_TYPE&, TYPE, PARSER&); + + template + void _Collect(const CELL_TYPE&, const AABB_TYPE&, INSERTER&) const; + template + void _Collect(const CELL_TYPE&, TYPE, const COLLECTOR&, INSERTER&) const; + + template + void _Traverse(const CELL_TYPE&, TYPE, const TFrustum&, INSERTER&) const; + template + void _TraverseCells(CELL_TYPE&, TYPE, const TFrustum&, PARSER&); + + template + void _SplitVolume(const CELL_TYPE& parentCell, TYPE parentRadius, unsigned idxChild, float maxArea, AREAESTIMATOR& areaEstimator, CHUNKINSERTER& chunkInserter, const UnsignedArr& indices=UnsignedArr{0,1,2,3,4,5,6,7}); + +protected: + const ITEM_TYPE* m_items; // original input items (the only condition is that every item to resolve to a position) + IDXARR_TYPE m_indices; // indices to input items re-arranged spatially (as dictated by the octree) + CELL_TYPE m_root; // first cell of the tree (always of Node type) + TYPE m_radius; // size of the sphere containing all cells + +public: + typedef struct DEBUGINFO_TYPE { + size_t memSize; // total memory used + size_t memStruct; // memory used for the tree structure + size_t memItems; // memory used for the contained items + size_t numItems; // number of contained items + size_t numNodes; // total nodes... + size_t numLeaves; // ... from which this number of leaves + size_t minDepth; // minimum tree depth + size_t maxDepth; // maximum tree depth + float avgDepth; // average tree depth + void Init() { memset(this, 0, sizeof(DEBUGINFO_TYPE)); } + void operator += (const DEBUGINFO_TYPE& r) { + avgDepth = avgDepth*numNodes + r.avgDepth*r.numNodes; + memSize += r.memSize; memStruct += r.memStruct; memItems += r.memItems; + numItems += r.numItems; numNodes += r.numNodes; numLeaves += r.numLeaves; + if (minDepth > r.minDepth) minDepth = r.minDepth; + if (maxDepth < r.maxDepth) maxDepth = r.maxDepth; + avgDepth /= numNodes; + } + } DEBUGINFO; + + void GetDebugInfo(DEBUGINFO* =NULL, bool bPrintStats=false) const; + static void LogDebugInfo(const DEBUGINFO&); + +protected: + void _GetDebugInfo(const CELL_TYPE&, unsigned, DEBUGINFO&) const; +}; // class TOctree +/*----------------------------------------------------------------*/ + + +#include "Octree.inl" +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_OCTREE_H__ diff --git a/libs/Common/Octree.inl b/libs/Common/Octree.inl new file mode 100644 index 0000000..0a55929 --- /dev/null +++ b/libs/Common/Octree.inl @@ -0,0 +1,851 @@ +//////////////////////////////////////////////////////////////////// +// Octree.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + +#ifdef _USE_OPENMP +// minimum number of polygons for which we do multi-threading +#define OCTREE_MIN_ITEMS_MINTHREAD 1024*2 +#endif + + +// S T R U C T S /////////////////////////////////////////////////// + +template +inline TOctree::CELL_TYPE::CELL_TYPE() + : + m_child(NULL) +{ +} // constructor +template +inline TOctree::CELL_TYPE::~CELL_TYPE() +{ + delete[] m_child; +} // destructor +/*----------------------------------------------------------------*/ + + +template +inline void TOctree::CELL_TYPE::Release() +{ + delete[] m_child; + m_child = NULL; +} // Release +// swap the two octrees +template +inline void TOctree::CELL_TYPE::Swap(CELL_TYPE& rhs) +{ + std::swap(m_child, rhs.m_child); + if (IsLeaf()) + std::swap(m_leaf, rhs.m_leaf); + else + std::swap(m_node, rhs.m_node); +} // Swap +/*----------------------------------------------------------------*/ + + +// compute item's index corresponding to the containing cell +template +inline unsigned TOctree::CELL_TYPE::ComputeChild(const POINT_TYPE& item) const +{ + ASSERT(!IsLeaf()); + unsigned idx = 0; + if (item[0] >= Node().center[0]) + idx |= (1<<0); + if (DIMS > 1) + if (item[1] >= Node().center[1]) + idx |= (1<<1); + if (DIMS > 2) + if (item[2] >= Node().center[2]) + idx |= (1<<2); + return idx; +} // ComputeChild +/*----------------------------------------------------------------*/ + +template +void TOctree::CELL_TYPE::ComputeCenter(POINT_TYPE centers[]) +{ + if (DIMS == 1) { + centers[0] << -1; + centers[1] << 1; + } else + if (DIMS == 2) { + centers[0] << -1,-1; + centers[1] << 1,-1; + centers[2] << -1, 1; + centers[3] << 1, 1; + } else + if (DIMS == 3) { + centers[0] << -1,-1,-1; + centers[1] << 1,-1,-1; + centers[2] << -1, 1,-1; + centers[3] << 1, 1,-1; + centers[4] << -1,-1, 1; + centers[5] << 1,-1, 1; + centers[6] << -1, 1, 1; + centers[7] << 1, 1, 1; + } +} // ComputeCenter +/*----------------------------------------------------------------*/ + +template +inline typename TOctree::POINT_TYPE TOctree::CELL_TYPE::ComputeChildCenter(const POINT_TYPE& center, TYPE radius, unsigned idxChild) +{ + struct CENTERARR_TYPE { + POINT_TYPE child[CELL_TYPE::numChildren]; + inline CENTERARR_TYPE() { CELL_TYPE::ComputeCenter(child); } + }; + static const CENTERARR_TYPE centers; + return center + centers.child[idxChild] * radius; +} // ComputeChildCenter +/*----------------------------------------------------------------*/ + + +// count the number of items contained by the given octree-cell +template +size_t TOctree::CELL_TYPE::GetNumItemsHeld() const +{ + if (IsLeaf()) + return GetNumItems(); + size_t numItems = 0; + for (int i=0; i +template +inline TOctree::TOctree(const ITEMARR_TYPE& items, Functor split) +{ + Insert(items, split); +} +template +template +inline TOctree::TOctree(const ITEMARR_TYPE& items, const AABB_TYPE& aabb, Functor split) +{ + Insert(items, aabb, split); +} // constructor +/*----------------------------------------------------------------*/ + + +// destroy tree +template +inline void TOctree::Release() +{ + m_indices.Release(); + m_root.Release(); +} // Release +// swap the two octrees +template +inline void TOctree::Swap(TOctree& rhs) +{ + std::swap(m_items, rhs.m_items); + m_indices.Swap(rhs.m_indices); + m_root.Swap(rhs.m_root); + std::swap(m_radius, rhs.m_radius); +} // Swap +/*----------------------------------------------------------------*/ + + +// add the given item to the tree +template +template +void TOctree::_Insert(CELL_TYPE& cell, const POINT_TYPE& center, TYPE radius, IDX_TYPE start, IDX_TYPE size, _InsertData& insertData) +{ + ASSERT(size > 0); + // if this child cell needs to be divided further + if (bForceSplit || insertData.split(size, radius)) { + // init node and proceed recursively + ASSERT(cell.m_child == NULL); + cell.m_child = new CELL_TYPE[CELL_TYPE::numChildren]; + cell.Node().center = center; + struct ChildData { + enum { ESTART=0, EEND=CELL_TYPE::numChildren, ESIZE=CELL_TYPE::numChildren*2, EALL=CELL_TYPE::numChildren*3}; + IDX_TYPE data[EALL]; + ChildData() { memset(data, 0, sizeof(IDX_TYPE)*EALL); } + inline IDX_TYPE Start(unsigned i) const { return data[ESTART+i]; } + inline IDX_TYPE& Start(unsigned i) { return data[ESTART+i]; } + inline IDX_TYPE End(unsigned i) const { return data[EEND+i]; } + inline IDX_TYPE& End(unsigned i) { return data[EEND+i]; } + inline IDX_TYPE Size(unsigned i) const { return data[ESIZE+i]; } + inline IDX_TYPE& Size(unsigned i) { return data[ESIZE+i]; } + } childD; + IDX_TYPE idx(start); + for (IDX_TYPE i=0; i::NO_INDEX); + const TYPE childRadius(radius / TYPE(2)); + for (unsigned i=0; i::NO_INDEX; // mark the end of child successors + const POINT_TYPE childCenter(CELL_TYPE::ComputeChildCenter(center, childRadius, i)); + _Insert(child, childCenter, childRadius, childD.Start(i), childD.Size(i), insertData); + } + } else { + // init leaf + cell.Leaf().idxBegin = m_indices.size(); + cell.Leaf().size = (SIZE_TYPE)size; + for (IDX_TYPE idx=start; idx!=_InsertData::NO_INDEX; idx=insertData.successors[idx]) + m_indices.push_back(idx); + } +} // _Insert +/*----------------------------------------------------------------*/ + +template +template +inline void TOctree::Insert(const ITEMARR_TYPE& items, const AABB_TYPE& aabb, Functor split) +{ + Release(); + m_items = items.data(); + // create root as node, even if we do not need to divide + m_indices.Reserve(items.size()); + // divide cell + const POINT_TYPE center = aabb.GetCenter(); + m_radius = aabb.GetSize().maxCoeff()/Type(2); + // single connected list of next item indices + _InsertData insertData = {items.size(), split}; + std::iota(insertData.successors.begin(), insertData.successors.end(), IDX_TYPE(1)); + insertData.successors.back() = _InsertData::NO_INDEX; + // setup each cell + _Insert(m_root, center, m_radius, 0, items.size(), insertData); +} +template +template +inline void TOctree::Insert(const ITEMARR_TYPE& items, Functor split) +{ + ASSERT(!items.IsEmpty()); + ASSERT(sizeof(POINT_TYPE) == sizeof(typename ITEMARR_TYPE::Type)); + AABB_TYPE aabb((const POINT_TYPE*)items.data(), items.size()); + aabb.Enlarge(ZEROTOLERANCE()*TYPE(10)); + Insert(items, aabb, split); +} // Insert +/*----------------------------------------------------------------*/ + + +template +template +void TOctree::CollectCells(const CELL_TYPE& cell, INSERTER& inserter) const +{ + if (cell.IsLeaf()) { + inserter(m_indices.data()+cell.GetFirstItemIdx(), cell.GetNumItems()); + return; + } + for (int i=0; i +template +void TOctree::_ParseCells(CELL_TYPE& cell, TYPE radius, PARSER& parser) +{ + if (cell.IsLeaf()) { + parser(cell, radius); + return; + } + const TYPE childRadius = radius / TYPE(2); + for (int i=0; i +template +void TOctree::CollectCells(INSERTER& inserter) const +{ + CollectCells(m_root, inserter); +} +// calls parser for each leaf of the octree (the CELL_TYPE operator has to be defined) +template +template +void TOctree::ParseCells(PARSER& parser) +{ + _ParseCells(m_root, m_radius, parser); +} +/*----------------------------------------------------------------*/ + + +// find all items contained by the given bounding box +template +template +void TOctree::_Collect(const CELL_TYPE& cell, const AABB_TYPE& aabb, INSERTER& inserter) const +{ + if (cell.IsLeaf()) { + // add all items contained by the bounding-box + for (IDX_TYPE i=0; i +template +void TOctree::_Collect(const CELL_TYPE& cell, TYPE radius, const COLLECTOR& collector, INSERTER& inserter) const +{ + ASSERT(!cell.IsLeaf()); + const TYPE childRadius = radius / TYPE(2); + for (int i=0; i +template +inline void TOctree::Collect(INSERTER& inserter, const AABB_TYPE& aabb) const +{ + _Collect(m_root, aabb, inserter); +} +template +inline void TOctree::Collect(IDXARR_TYPE& indices, const AABB_TYPE& aabb) const +{ + IndexInserter inserter(indices); + _Collect(m_root, aabb, inserter); +} + +template +template +inline void TOctree::Collect(INSERTER& inserter, const POINT_TYPE& center, TYPE radius) const +{ + _Collect(m_root, AABB_TYPE(center, radius), inserter); +} +template +inline void TOctree::Collect(IDXARR_TYPE& indices, const POINT_TYPE& center, TYPE radius) const +{ + IndexInserter inserter(indices); + _Collect(m_root, AABB_TYPE(center, radius), inserter); +} + +template +template +inline void TOctree::Collect(INSERTER& inserter, const COLLECTOR& collector) const +{ + _Collect(m_root, m_radius, collector, inserter); +} +template +template +inline void TOctree::Collect(IDXARR_TYPE& indices, const COLLECTOR& collector) const +{ + IndexInserter inserter(indices); + _Collect(m_root, m_radius, collector, inserter); +} + +template +inline void TOctree::Collect(IDX_TYPE maxNeighbors, IDXARR_TYPE& indices, const AABB_TYPE& aabb) const +{ + _Collect(m_root, aabb, IndexInserter(indices)); + if (indices.size() > maxNeighbors) { + // keep only the closest neighbors + typedef TIndexScore ItemIndexScore; + typedef cList ItemIndexScoreArr; + ItemIndexScoreArr indexscores(indices.size()); + const POINT_TYPE center(aabb.GetCenter()); + FOREACH(i, indices) { + const IDX_TYPE& idx = indices[i]; + const TYPE score(-(center-m_items[idx]).squaredNorm()); + indexscores[i] = ItemIndexScore(idx,score); + } + indices.Empty(); + indexscores.Sort(); + for (IDX_TYPE i=0; i +inline void TOctree::Collect(IDX_TYPE maxNeighbors, IDXARR_TYPE& indices, const POINT_TYPE& center, TYPE radius) const +{ + Collect(maxNeighbors, indices, AABB_TYPE(center, radius)); +} // Collect +/*----------------------------------------------------------------*/ + + +// walk through the tree and collect visible indices +template +template +void TOctree::_Traverse(const CELL_TYPE& cell, TYPE radius, const TFrustum& frustum, INSERTER& inserter) const +{ + ASSERT(!cell.IsLeaf()); + switch (frustum.Classify(cell.GetAabb(radius))) { + case CLIPPED: { + const TYPE childRadius = radius / TYPE(2); + for (int i=0; i +template +void TOctree::_TraverseCells(CELL_TYPE& cell, TYPE radius, const TFrustum& frustum, PARSER& parser) +{ + ASSERT(!cell.IsLeaf()); + switch (frustum.Classify(cell.GetAabb(radius))) { + case CLIPPED: { + const TYPE childRadius = radius / TYPE(2); + for (int i=0; i +template +inline void TOctree::Traverse(const TFrustum& frustum, INSERTER& inserter) const +{ + _Traverse(m_root, m_radius, frustum, inserter); +} +template +template +inline void TOctree::Traverse(const TFrustum& frustum, IDXARR_TYPE& indices) const +{ + _Traverse(m_root, m_radius, frustum, IndexInserter(indices)); +} +template +template +inline void TOctree::TraverseCells(const TFrustum& frustum, PARSER& parser) +{ + _TraverseCells(m_root, m_radius, frustum, parser); +} +template +template +inline void TOctree::TraverseCells(const TFrustum& frustum, CELLPTRARR_TYPE& leaves) +{ + _TraverseCells(m_root, m_radius, frustum, CellInserter(leaves)); +} // Traverse +/*----------------------------------------------------------------*/ + +template +template +void TOctree::_SplitVolume(const CELL_TYPE& parentCell, TYPE parentRadius, unsigned idxChild, float maxArea, AREAESTIMATOR& areaEstimator, CHUNKINSERTER& chunkInserter, const UnsignedArr& indices) +{ + ASSERT(!indices.empty()); + typedef std::pair PairIndices; + struct GenerateSamples { + const UnsignedArr& indices; + const unsigned numSamples; + const unsigned halfSamples; + const unsigned numCommonAxis; + POINT_TYPE centers[8]; + cList arrHalfIndices; + GenerateSamples(const UnsignedArr& _indices) + : indices(_indices), numSamples((unsigned)indices.size()), halfSamples(numSamples/2), numCommonAxis(halfSamples==4?1:2), arrHalfIndices(0, numSamples) + { + ASSERT(indices.size()%2 == 0 && indices.IsSorted()); + ASSERT(halfSamples == 4 || halfSamples == 2); + CELL_TYPE::ComputeCenter(centers); + UnsignedArr samples(halfSamples); + for (unsigned hs=0; hs= maxArea) + ++numOverAreas; + if (numOverAreas == indices.size()) { + for (unsigned c: indices) + if (childArea[c] > 0) + _SplitVolume(cell, radius, c, maxArea, areaEstimator, chunkInserter); + return; + } + // split mesh children and retain the components with surface smaller than the given area + const cList halfIndices(std::move(GenerateSamples(indices).arrHalfIndices)); + IDX bestSplit(NO_ID); + float bestArea(0); + Point2f bestAs; + FOREACH(idx, halfIndices) { + const PairIndices& pairHalfIndices = halfIndices[idx]; + ASSERT(pairHalfIndices.first.size() == pairHalfIndices.second.size()); + Point2f as(Point2f::ZERO); + for (unsigned i=0; i qIndicesFirst(std::move(GenerateSamples(pairHalfIndices.first).arrHalfIndices)); + const cList qIndicesSecond(std::move(GenerateSamples(pairHalfIndices.second).arrHalfIndices)); + ASSERT(qIndicesFirst.size() == qIndicesSecond.size()); + FOREACH(q, qIndicesFirst) { + const PairIndices& qFirst = qIndicesFirst[q]; + const PairIndices& qSecond = qIndicesSecond[q]; + Eigen::Vector4f as(Eigen::Vector4f::Zero()); + for (unsigned i=0; i 0) { + // store found clusters + for (unsigned i=0; i<4; ++i) { + if (bestAs[i] < maxArea) { + chunkInserter(cell, radius, bestQIndices[i]); + } else { + _SplitVolume(cell, radius, bestQIndices[i][0], maxArea, areaEstimator, chunkInserter); + _SplitVolume(cell, radius, bestQIndices[i][1], maxArea, areaEstimator, chunkInserter); + } + } + return; + } + } + // split each child + for (unsigned c: indices) { + if (childArea[c] == 0) + continue; + if (childArea[c] < maxArea) + chunkInserter(cell, radius, UnsignedArr{c}); + else + _SplitVolume(cell, radius, c, maxArea, areaEstimator, chunkInserter); + } +} +template +template +void TOctree::SplitVolume(float maxArea, AREAESTIMATOR& areaEstimator, CHUNKINSERTER& chunkInserter) +{ + CELL_TYPE parent; + parent.m_child = new CELL_TYPE[1]; + parent.m_child[0].m_child = m_root.m_child; + parent.m_child[0].Node() = m_root.Node(); + parent.Node().center = m_root.Node().center + POINT_TYPE::Constant(m_radius); + _SplitVolume(parent, m_radius*TYPE(2), 0, maxArea, areaEstimator, chunkInserter); + parent.m_child[0].m_child = NULL; +} // SplitVolume +/*----------------------------------------------------------------*/ + + +template +void TOctree::_GetDebugInfo(const CELL_TYPE& cell, unsigned nDepth, DEBUGINFO& info) const +{ + if (cell.IsLeaf()) { + if (info.minDepth > nDepth) + info.minDepth = nDepth; + if (info.maxDepth < nDepth) + info.maxDepth = nDepth; + info.avgDepth += nDepth; + info.numLeaves++; + return; + } + nDepth++; + info.numNodes++; + for (int i=0; i +void TOctree::GetDebugInfo(DEBUGINFO* pInfo, bool bPrintStats) const +{ + DEBUGINFO localInfo; + DEBUGINFO& info = (pInfo ? *pInfo : localInfo); + info.Init(); + _GetDebugInfo(m_root, 0, info); + info.avgDepth /= info.numLeaves; + info.numItems = GetNumItems(); + info.numNodes += info.numLeaves; + info.memStruct = info.numNodes*sizeof(CELL_TYPE) + sizeof(TOctree); + info.memItems = sizeof(IDX_TYPE)*info.numItems; + info.memSize = info.memStruct + info.memItems; + if (pInfo == NULL || bPrintStats) + LogDebugInfo(info); +} // GetDebugInfo +/*----------------------------------------------------------------*/ + +template +void TOctree::LogDebugInfo(const DEBUGINFO& info) +{ + //VERBOSE("NoItems: %d; Mem %s; MemItems %s; MemStruct %s; AvgMemStruct %.2f%%%%; NoNodes %d; NoLeaf %d; AvgLeaf %.2f%%%%; AvgDepth %.2f; MinDepth %d; MaxDepth %d", + VERBOSE("NumItems %d; Mem %s (%s items, %s struct - %.2f%%%%); NumNodes %d (leaves %d - %.2f%%%%); Depth %.2f (%d min, %d max)", + info.numItems, + Util::formatBytes(info.memSize).c_str(), Util::formatBytes(info.memItems).c_str(), Util::formatBytes(info.memStruct).c_str(), double(info.memStruct)*100.0/info.memSize, + info.numNodes, info.numLeaves, float(info.numLeaves*100)/info.numNodes, + info.avgDepth, info.minDepth, info.maxDepth); +} // LogDebugInfo +/*----------------------------------------------------------------*/ + +// if everything works fine, this function should return true +template +inline bool OctreeTest(unsigned iters, unsigned maxItems=1000, bool bRandom=true) { + STATIC_ASSERT(DIMS > 0 && DIMS <= 3); + srand(bRandom ? (unsigned)time(NULL) : 0); + typedef Eigen::Matrix POINT_TYPE; + typedef CLISTDEF0(POINT_TYPE) TestArr; + typedef TOctree TestTree; + const TYPE ptMinData[] = {0,0,0}, ptMaxData[] = {640,480,240}; + typename TestTree::AABB_TYPE aabb; + aabb.Set(Eigen::Map(ptMinData), Eigen::Map(ptMaxData)); + aabb.Enlarge(ZEROTOLERANCE()*TYPE(10)); + unsigned nTotalMatches = 0; + unsigned nTotalMissed = 0; + unsigned nTotalExtra = 0; + #ifndef _RELEASE + typename TestTree::DEBUGINFO_TYPE totalInfo; + totalInfo.Init(); + #endif + for (unsigned iter=0; iter(RAND()%ROUND2INT(ptMaxData[j])); + // random query point + POINT_TYPE pt; + for (int j=0; j(RAND()%ROUND2INT(ptMaxData[j])); + const TYPE radius(TYPE(3+RAND()%30)); + // build octree and find interest items + TestTree tree(items, aabb, [](typename TestTree::IDX_TYPE size, typename TestTree::Type radius) { + return size > 16 && radius > 10; + }); + typename TestTree::IDXARR_TYPE indices; + tree.Collect(indices, pt, radius); + // find interest items by brute force + typename TestTree::IDXARR_TYPE trueIndices; + #if 1 + // use square bound + typename TestTree::AABB_TYPE aabbQuery(pt, radius); + FOREACH(i, items) + if (aabbQuery.Intersects(items[i])) + trueIndices.Insert(i); + #else + // use circle bound + FOREACH(i, items) + if ((items[i]-pt).norm() < radius) + trueIndices.Insert(i); + #endif + // compare results + unsigned nMatches = 0; + FOREACH(i, trueIndices) { + const typename TestTree::IDX_TYPE idx = trueIndices[i]; + FOREACH(j, indices) { + if (indices[j] == idx) { + ++nMatches; + break; + } + } + } + nTotalMatches += nMatches; + nTotalMissed += (unsigned)trueIndices.size()-nMatches; + nTotalExtra += (unsigned)indices.size()-nMatches; + #ifndef _RELEASE + // print stats + typename TestTree::DEBUGINFO_TYPE info; + tree.GetDebugInfo(&info); + totalInfo += info; + #endif + } + #ifndef _RELEASE + TestTree::LogDebugInfo(totalInfo); + VERBOSE("Test %s (TotalMissed %d, TotalExtra %d)", (nTotalMissed == 0 && nTotalExtra == 0 ? "successful" : "FAILED"), nTotalMissed, nTotalExtra); + #endif + return (nTotalMissed == 0 && nTotalExtra == 0); +} diff --git a/libs/Common/Plane.h b/libs/Common/Plane.h new file mode 100644 index 0000000..7b396e9 --- /dev/null +++ b/libs/Common/Plane.h @@ -0,0 +1,161 @@ +//////////////////////////////////////////////////////////////////// +// Plane.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_PLANE_H__ +#define __SEACAVE_PLANE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// Basic hyper-plane class +// (plane represented in Hessian Normal Form: n.x+d=0 <=> ax+by+cz+d=0) +template +class TPlane +{ + STATIC_ASSERT(DIMS > 0 && DIMS <= 3); + +public: + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TAABB AABB; + typedef SEACAVE::TRay RAY; + enum { numScalar = DIMS+1 }; + enum { numParams = numScalar-1 }; + + VECTOR m_vN; // plane normal vector + TYPE m_fD; // distance to origin + + //--------------------------------------- + + inline TPlane() {} + inline TPlane(const VECTOR&, TYPE); + inline TPlane(const VECTOR&, const POINT&); + inline TPlane(const POINT&, const POINT&, const POINT&); + inline TPlane(const TYPE p[DIMS+1]); + inline TPlane(const Eigen::Matrix&); + + inline void Set(const VECTOR&, TYPE); + inline void Set(const VECTOR&, const POINT&); + inline void Set(const POINT&, const POINT&, const POINT&); + inline void Set(const TYPE p[DIMS+1]); + inline void Set(const Eigen::Matrix&); + + int Optimize(const POINT*, size_t, int maxIters=100); + template + int Optimize(const POINT*, size_t, const RobustNormFunctor& robust, int maxIters=100); + + inline void Invalidate(); + inline bool IsValid() const; + + inline void Negate(); + inline TPlane Negated() const; + + inline TYPE Distance(const TPlane&) const; + inline TYPE Distance(const POINT&) const; + inline TYPE DistanceAbs(const POINT&) const; + + inline POINT ProjectPoint(const POINT&) const; + + inline GCLASS Classify(const POINT&) const; + inline GCLASS Classify(const AABB&) const; + + bool Clip(const RAY&, TYPE, RAY*, RAY*) const; + + bool Intersects(const POINT& p0, const POINT& p1, const POINT& p2) const; + bool Intersects(const TPlane& plane, RAY& ray) const; + bool Intersects(const AABB& aabb) const; + + inline TYPE& operator [] (BYTE i) { ASSERT(i + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & m_vN; + ar & m_fD; + } + #endif +}; // class TPlane +/*----------------------------------------------------------------*/ + +template +struct FitPlaneOnline { + TYPEW sumX, sumSqX, sumXY, sumXZ; + TYPEW sumY, sumSqY, sumYZ; + TYPEW sumZ, sumSqZ; + size_t size; + FitPlaneOnline(); + void Update(const TPoint3& P); + TPoint3 GetModel(TPoint3& avg, TPoint3& dir) const; + template TPoint3 GetPlane(TPlane& plane) const; +}; +/*----------------------------------------------------------------*/ + + +// Basic 3D frustum class +// (represented as 6 planes oriented toward outside the frustum volume) +template +class TFrustum +{ + STATIC_ASSERT(DIMS > 0 && DIMS <= 6); + +public: + typedef Eigen::Matrix MATRIX4x4; + typedef Eigen::Matrix MATRIX3x4; + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TPlane PLANE; + typedef SEACAVE::TSphere SPHERE; + typedef SEACAVE::TAABB AABB; + enum { numCorners = (1<<3) }; + + PLANE m_planes[DIMS]; // left, right, top, bottom, near and far planes + + //--------------------------------------- + + inline TFrustum() {} + inline TFrustum(const MATRIX4x4&, TYPE width, TYPE height, TYPE nearZ=TYPE(0.0001), TYPE farZ=TYPE(1000)); + inline TFrustum(const MATRIX3x4&, TYPE width, TYPE height, TYPE nearZ=TYPE(0.0001), TYPE farZ=TYPE(1000)); + + void Set(const MATRIX4x4&, TYPE width, TYPE height, TYPE nearZ=TYPE(0.0001), TYPE farZ=TYPE(1000)); + void Set(const MATRIX3x4&, TYPE width, TYPE height, TYPE nearZ=TYPE(0.0001), TYPE farZ=TYPE(1000)); + void Set(const VECTOR corners[numCorners]); + void SetProjectionGL(const MATRIX4x4&); + + GCLASS Classify(const POINT&) const; + GCLASS Classify(const SPHERE&) const; + GCLASS Classify(const AABB&) const; + + inline TYPE& operator [] (BYTE i) { ASSERT(i + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & m_planes; + } + #endif +}; // class TPlane +/*----------------------------------------------------------------*/ + + +#include "Plane.inl" +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_PLANE_H__ diff --git a/libs/Common/Plane.inl b/libs/Common/Plane.inl new file mode 100644 index 0000000..e1956cb --- /dev/null +++ b/libs/Common/Plane.inl @@ -0,0 +1,623 @@ +//////////////////////////////////////////////////////////////////// +// Plane.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +// Construct plane given its normal and the distance from the origin. +template +inline TPlane::TPlane(const VECTOR& vN, TYPE fD) + : + m_vN(vN), m_fD(fD) +{ +} +// Construct plane given its normal and a point on the plane. +template +inline TPlane::TPlane(const VECTOR& vN, const POINT& p) + : + m_vN(vN), m_fD(-vN.dot(p)) +{ +} +// Construct plane given three points on the plane. +template +inline TPlane::TPlane(const POINT& p0, const POINT& p1, const POINT& p2) +{ + Set(p0, p1, p2); +} +// Construct plane given its standard equation: Ax + By + Cz + D = 0 +template +inline TPlane::TPlane(const TYPE p[DIMS+1]) +{ + Set(p); +} +// Construct plane given its standard equation: Ax + By + Cz + D = 0 +template +inline TPlane::TPlane(const Eigen::Matrix& v) +{ + Set(v); +} // constructors +/*----------------------------------------------------------------*/ + +template +inline void TPlane::Set(const VECTOR& vN, TYPE fD) +{ + m_vN = vN; + m_fD = fD; +} +template +inline void TPlane::Set(const VECTOR& vN, const POINT& p) +{ + m_vN = vN; + m_fD = -vN.dot(p); +} +template +inline void TPlane::Set(const POINT& p0, const POINT& p1, const POINT& p2) +{ + const VECTOR vcEdge1 = p1 - p0; + const VECTOR vcEdge2 = p2 - p0; + m_vN = vcEdge1.cross(vcEdge2).normalized(); + m_fD = -m_vN.dot(p0); +} +template +inline void TPlane::Set(const TYPE p[DIMS+1]) +{ + const Eigen::Map vN(p); + const TYPE invD(INVERT(vN.norm())); + Set(vN*invD, p[DIMS]*invD); +} +template +inline void TPlane::Set(const Eigen::Matrix& v) +{ + const VECTOR vN = v.template topLeftCorner<3,1>(); + const TYPE invD(INVERT(vN.norm())); + Set(vN*invD, v(DIMS)*invD); +} // Set +/*----------------------------------------------------------------*/ + + +// least squares refinement of the given plane to the 3D point set +// (return the number of iterations) +template +template +int TPlane::Optimize(const POINT* points, size_t size, const RobustNormFunctor& robust, int maxIters) +{ + ASSERT(DIMS == 3); + ASSERT(size >= numParams); + struct OptimizationFunctor { + const POINT* points; + const size_t size; + const RobustNormFunctor& robust; + // construct with the data points + OptimizationFunctor(const POINT* _points, size_t _size, const RobustNormFunctor& _robust) + : points(_points), size(_size), robust(_robust) { ASSERT(size < (size_t)std::numeric_limits::max()); } + static void Residuals(const double* x, int nPoints, const void* pData, double* fvec, double* fjac, int* /*info*/) { + const OptimizationFunctor& data = *reinterpret_cast(pData); + ASSERT((size_t)nPoints == data.size && fvec != NULL && fjac == NULL); + TPlane plane; { + Point3d N; + plane.m_fD = x[0]; + Dir2Normal(reinterpret_cast(x[1]), N); + plane.m_vN = N; + } + for (size_t i=0; i())); + } + } functor(points, size, robust); + double arrParams[numParams]; { + arrParams[0] = (double)m_fD; + const Point3d N(m_vN.x(), m_vN.y(), m_vN.z()); + Normal2Dir(N, reinterpret_cast(arrParams[1])); + } + lm_control_struct control = {1.e-6, 1.e-7, 1.e-8, 1.e-7, 100.0, maxIters}; // lm_control_float; + lm_status_struct status; + lmmin(numParams, arrParams, (int)size, &functor, OptimizationFunctor::Residuals, &control, &status); + switch (status.info) { + //case 4: + case 5: + case 6: + case 7: + case 8: + case 9: + case 10: + case 11: + case 12: + DEBUG_ULTIMATE("error: refine plane: %s", lm_infmsg[status.info]); + return 0; + } + // set plane + { + Point3d N; + Dir2Normal(reinterpret_cast(arrParams[1]), N); + Set(Cast(N), (TYPE)arrParams[0]); + } + return status.nfev; +} +template +int TPlane::Optimize(const POINT* points, size_t size, int maxIters) +{ + const auto identity = [](double x) { return x; }; + return Optimize(points, size, identity, maxIters); +} // Optimize +/*----------------------------------------------------------------*/ + + +template +inline void TPlane::Invalidate() +{ + m_fD = std::numeric_limits::max(); +} // Invalidate +template +inline bool TPlane::IsValid() const +{ + return m_fD != std::numeric_limits::max(); +} // IsValid +/*----------------------------------------------------------------*/ + + +template +inline void TPlane::Negate() +{ + m_vN = -m_vN; + m_fD = -m_fD; +} // Negate +template +inline TPlane TPlane::Negated() const +{ + return TPlane(-m_vN, -m_fD); +} // Negated +/*----------------------------------------------------------------*/ + + +template +inline TYPE TPlane::Distance(const TPlane& p) const +{ + return ABS(m_fD - p.m_fD); +} +/*----------------------------------------------------------------*/ + +// Calculate distance to point. Plane normal must be normalized. +template +inline TYPE TPlane::Distance(const POINT& p) const +{ + return m_vN.dot(p) + m_fD; +} +template +inline TYPE TPlane::DistanceAbs(const POINT& p) const +{ + return ABS(Distance(p)); +} +/*----------------------------------------------------------------*/ + + +// Calculate point's projection on this plane (closest point to this plane). +template +inline typename TPlane::POINT TPlane::ProjectPoint(const POINT& p) const +{ + return p - m_vN*Distance(p); +} +/*----------------------------------------------------------------*/ + + +// Classify point to plane. +template +inline GCLASS TPlane::Classify(const POINT& p) const +{ + const TYPE f(Distance(p)); + if (f > ZEROTOLERANCE()) return FRONT; + if (f < -ZEROTOLERANCE()) return BACK; + return PLANAR; +} +/*----------------------------------------------------------------*/ + + +// Classify bounding box to plane. +template +inline GCLASS TPlane::Classify(const AABB& aabb) const +{ + const GCLASS classMin = Classify(aabb.ptMin); + const GCLASS classMax = Classify(aabb.ptMax); + if (classMin == classMax) return classMin; + return CLIPPED; +} +/*----------------------------------------------------------------*/ + + +// clips a ray into two segments if it intersects the plane +template +bool TPlane::Clip(const RAY& ray, TYPE fL, RAY* pF, RAY* pB) const +{ + POINT ptHit = POINT::ZERO; + + // ray intersects plane at all? + if (!ray.Intersects(*this, false, fL, NULL, &ptHit)) + return false; + + GCLASS n = Classify(ray.m_pOrig); + + // ray comes from planes backside + if ( n == BACK ) { + if (pB) pB->Set(ray.m_pOrig, ray.m_vDir); + if (pF) pF->Set(ptHit, ray.m_vDir); + } + // ray comes from planes front side + else if ( n == FRONT ) { + if (pF) pF->Set(ray.m_pOrig, ray.m_vDir); + if (pB) pB->Set(ptHit, ray.m_vDir); + } + + return true; +} // Clip(Ray) +/*----------------------------------------------------------------*/ + + +// Intersection of two planes. +// Returns the line of intersection. +// http://paulbourke.net/geometry/pointlineplane/ +template +bool TPlane::Intersects(const TPlane& plane, RAY& ray) const +{ + // if crossproduct of normals 0 than planes parallel + const VECTOR vCross(m_vN.cross(plane.m_vN)); + const TYPE fSqrLength(vCross.squaredNorm()); + if (fSqrLength < ZEROTOLERANCE()) + return false; + + // find line of intersection + #if 0 + // the general case + const TYPE fN00 = m_vN.squaredNorm(); + const TYPE fN01 = m_vN.dot(plane.m_vN); + const TYPE fN11 = plane.m_vN.squaredNorm(); + const TYPE fDet = fN01*fN01 - fN00*fN11; + const TYPE fInvDet = INVERT(fDet); + const TYPE fC0 = (fN11*m_fD - fN01*plane.m_fD) * fInvDet; + const TYPE fC1 = (fN00*plane.m_fD - fN01*m_fD) * fInvDet; + #else + // plane normals assumed to be normalized vectors + ASSERT(ISEQUAL(m_vN.norm(), TYPE(1))); + ASSERT(ISEQUAL(plane.m_vN.norm(), TYPE(1))); + const TYPE fN01 = m_vN.dot(plane.m_vN); + const TYPE fInvDet = INVERT(fN01*fN01-TYPE(1)); + const TYPE fC0 = (m_fD - fN01*plane.m_fD) * fInvDet; + const TYPE fC1 = (plane.m_fD - fN01*m_fD) * fInvDet; + #endif + + ray.m_vDir = vCross * RSQRT(fSqrLength); + ray.m_pOrig = m_vN*fC0 + plane.m_vN*fC1; + return true; +} // Intersects(Plane) +/*----------------------------------------------------------------*/ + + +// Intersection of a plane with a triangle. If all vertices of the +// triangle are on the same side of the plane, no intersection occurred. +template +bool TPlane::Intersects(const POINT& p0, const POINT& p1, const POINT& p2) const +{ + const GCLASS n(Classify(p0)); + if ((n == Classify(p1)) && + (n == Classify(p2))) + return false; + return true; +} // Intersects(Tri) +/*----------------------------------------------------------------*/ + + +// Intersection with AABB. Search for AABB diagonal that is most +// aligned to plane normal. Test its two vertices against plane. +// (M�ller/Haines, "Real-Time Rendering") +template +bool TPlane::Intersects(const AABB& aabb) const +{ + POINT Vmin, Vmax; + + // x component + if (m_vN(0) >= TYPE(0)) { + Vmin(0) = aabb.ptMin(0); + Vmax(0) = aabb.ptMax(0); + } + else { + Vmin(0) = aabb.ptMax(0); + Vmax(0) = aabb.ptMin(0); + } + + // y component + if (m_vN(1) >= TYPE(0)) { + Vmin(1) = aabb.ptMin(1); + Vmax(1) = aabb.ptMax(1); + } + else { + Vmin(1) = aabb.ptMax(1); + Vmax(1) = aabb.ptMin(1); + } + + // z component + if (m_vN(2) >= TYPE(0)) { + Vmin(2) = aabb.ptMin(2); + Vmax(2) = aabb.ptMax(2); + } + else { + Vmin(2) = aabb.ptMax(2); + Vmax(2) = aabb.ptMin(2); + } + + if (((m_vN * Vmin) + m_fD) > TYPE(0)) + return false; + + if (((m_vN * Vmax) + m_fD) >= TYPE(0)) + return true; + + return false; +} // Intersects(AABB) +/*----------------------------------------------------------------*/ + + +// same as above, but online version +template +FitPlaneOnline::FitPlaneOnline() + : sumX(0), sumSqX(0), sumXY(0), sumXZ(0), sumY(0), sumSqY(0), sumYZ(0), sumZ(0), sumSqZ(0), size(0) +{ +} +template +void FitPlaneOnline::Update(const TPoint3& P) +{ + const TYPEW X((TYPEW)P.x), Y((TYPEW)P.y), Z((TYPEW)P.z); + sumX += X; sumSqX += X*X; sumXY += X*Y; sumXZ += X*Z; + sumY += Y; sumSqY += Y*Y; sumYZ += Y*Z; + sumZ += Z; sumSqZ += Z*Z; + ++size; +} +template +TPoint3 FitPlaneOnline::GetModel(TPoint3& avg, TPoint3& dir) const +{ + const TYPEW avgX(sumX/(TYPEW)size), avgY(sumY/(TYPEW)size), avgZ(sumZ/(TYPEW)size); + // assemble covariance (lower-triangular) matrix + typedef Eigen::Matrix Mat3x3; + Mat3x3 A; + A(0,0) = sumSqX - TYPEW(2)*sumX*avgX + avgX*avgX*(TYPEW)size; + A(1,0) = sumXY - sumX*avgY - avgX*sumY + avgX*avgY*(TYPEW)size; + A(1,1) = sumSqY - TYPEW(2)*sumY*avgY + avgY*avgY*(TYPEW)size; + A(2,0) = sumXZ - sumX*avgZ - avgX*sumZ + avgX*avgZ*(TYPEW)size; + A(2,1) = sumYZ - sumY*avgZ - avgY*sumZ + avgY*avgZ*(TYPEW)size; + A(2,2) = sumSqZ - TYPEW(2)*sumZ*avgZ + avgZ*avgZ*(TYPEW)size; + // the plane normal is simply the eigenvector corresponding to least eigenvalue + const int nAxis(bFitLineMode ? 2 : 0); + const Eigen::SelfAdjointEigenSolver es(A); + ASSERT(ISEQUAL(es.eigenvectors().col(nAxis).norm(), TYPEW(1))); + avg = TPoint3(avgX,avgY,avgZ); + dir = es.eigenvectors().col(nAxis); + const TYPEW* const vals(es.eigenvalues().data()); + ASSERT(vals[0] <= vals[1] && vals[1] <= vals[2]); + return *reinterpret_cast*>(vals); +} +template +template +TPoint3 FitPlaneOnline::GetPlane(TPlane& plane) const +{ + TPoint3 avg, dir; + const TPoint3 quality(GetModel(avg, dir)); + plane.Set(TPoint3(dir), TPoint3(avg)); + return TPoint3(quality); +} +/*----------------------------------------------------------------*/ + + +// Least squares fits a plane to a 3D point set. +// See http://www.geometrictools.com/Documentation/LeastSquaresFitting.pdf +// Returns a fitting quality (1 - lambda_min/lambda_max): +// 1 is best (zero variance orthogonally to the fitting line) +// 0 is worst (isotropic case, returns a plane with default direction) +template +TYPE FitPlane(const TPoint3* points, size_t size, TPlane& plane) { + // compute a point on the plane, which is shown to be the centroid of the points + const Eigen::Map< const Eigen::Matrix > vPoints((const TYPE*)points, size, 3); + const TPoint3 c(vPoints.colwise().mean()); + + // assemble covariance matrix; matrix numbering: + // 0 + // 1 2 + // 3 4 5 + Eigen::Matrix A(Eigen::Matrix::Zero()); + FOREACHRAWPTR(pPt, points, size) { + const TPoint3 X(*pPt - c); + A(0,0) += X.x*X.x; + A(1,0) += X.x*X.y; + A(1,1) += X.y*X.y; + A(2,0) += X.x*X.z; + A(2,1) += X.y*X.z; + A(2,2) += X.z*X.z; + } + + // the plane normal is simply the eigenvector corresponding to least eigenvalue + const Eigen::SelfAdjointEigenSolver< Eigen::Matrix > es(A); + ASSERT(ISEQUAL(es.eigenvectors().col(0).norm(), TYPE(1))); + plane.Set(es.eigenvectors().col(0), c); + const TYPE* const vals(es.eigenvalues().data()); + ASSERT(vals[0] <= vals[1] && vals[1] <= vals[2]); + return TYPE(1) - vals[0]/vals[1]; +} +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +// Construct frustum given a projection matrix. +template +inline TFrustum::TFrustum(const MATRIX4x4& m, TYPE w, TYPE h, TYPE n, TYPE f) +{ + Set(m, w, h, n, f); +} +template +inline TFrustum::TFrustum(const MATRIX3x4& m, TYPE w, TYPE h, TYPE n, TYPE f) +{ + Set(m, w, h, n, f); +} // Constructor +/*----------------------------------------------------------------*/ + + +// Set frustum planes, normals pointing outwards, from SfM projection matrix and image plane details +template +void TFrustum::Set(const MATRIX4x4& m, TYPE w, TYPE h, TYPE n, TYPE f) +{ + const VECTOR ltn(0,0,n), rtn(w*n,0,n), lbn(0,h*n,n), rbn(w*n,h*n,n); + const VECTOR ltf(0,0,f), rtf(w*f,0,f), lbf(0,h*f,f), rbf(w*f,h*f,f); + const MATRIX4x4 inv(m.inverse()); + const VECTOR corners[] = { + (inv*ltn.homogeneous()).template topRows<3>(), + (inv*rtn.homogeneous()).template topRows<3>(), + (inv*lbn.homogeneous()).template topRows<3>(), + (inv*rbn.homogeneous()).template topRows<3>(), + (inv*ltf.homogeneous()).template topRows<3>(), + (inv*rtf.homogeneous()).template topRows<3>(), + (inv*lbf.homogeneous()).template topRows<3>(), + (inv*rbf.homogeneous()).template topRows<3>() + }; + Set(corners); +} +template +void TFrustum::Set(const MATRIX3x4& m, TYPE w, TYPE h, TYPE n, TYPE f) +{ + MATRIX4x4 M(MATRIX4x4::Identity()); + M.template topLeftCorner<3,4>() = m; + Set(M, w, h, n, f); +} +// Set frustum planes, normals pointing outwards, from the given corners +template +void TFrustum::Set(const VECTOR corners[numCorners]) +{ + // left clipping plane + m_planes[0].Set(corners[0], corners[4], corners[6]); + if (DIMS > 1) // right clipping plane + m_planes[1].Set(corners[1], corners[7], corners[5]); + if (DIMS > 2) // top clipping plane + m_planes[2].Set(corners[0], corners[5], corners[4]); + if (DIMS > 3) // bottom clipping plane + m_planes[3].Set(corners[2], corners[6], corners[7]); + if (DIMS > 4) // near clipping plane + m_planes[4].Set(corners[0], corners[2], corners[3]); + if (DIMS > 5) // far clipping plane + m_planes[5].Set(corners[4], corners[5], corners[7]); +} // Set +// Set frustum planes, normals pointing outwards, from the OpenGL projection-view matrix +template +void TFrustum::SetProjectionGL(const MATRIX4x4& mProjectionView) +{ + // left clipping plane + m_planes[0].Set(-mProjectionView.row(3) - mProjectionView.row(0)); + if (DIMS > 1) // right clipping plane + m_planes[1].Set(-mProjectionView.row(3) + mProjectionView.row(0)); + if (DIMS > 2) // top clipping plane + m_planes[2].Set(-mProjectionView.row(3) + mProjectionView.row(1)); + if (DIMS > 3) // bottom clipping plane + m_planes[3].Set(-mProjectionView.row(3) - mProjectionView.row(1)); + if (DIMS > 4) // near clipping plane + m_planes[4].Set(-mProjectionView.row(3) - mProjectionView.row(2)); + if (DIMS > 5) // far clipping plane + m_planes[5].Set(-mProjectionView.row(3) + mProjectionView.row(2)); +} +/*----------------------------------------------------------------*/ + + +/** + * Culls POINT to n sided frustum. Normals pointing outwards. + * -> IN: POINT - point to be tested + * OUT: VISIBLE - point inside frustum + * CULLED - point outside frustum + */ +template +GCLASS TFrustum::Classify(const POINT& p) const +{ + // check if on the front side of any of the planes + for (int i=0; i IN: POINT - center of the sphere to be tested + * TYPE - radius of the sphere to be tested + * OUT: VISIBLE - sphere inside frustum + * CLIPPED - sphere clipped by frustum + * CULLED - sphere outside frustum + */ +template +GCLASS TFrustum::Classify(const SPHERE& s) const +{ + // compute distances to each of the planes + for (int i=0; i s.radius) + return CULLED; + // if the distance is between +- radius, the sphere intersects the frustum + if (ABS(dist) < s.radius) + return CLIPPED; + } // for + // otherwise sphere is fully in view + return VISIBLE; +} + +/** + * Culls AABB to n sided frustum. Normals pointing outwards. + * -> IN: AABB - bounding box to be tested + * OUT: VISIBLE - aabb totally inside frustum + * CLIPPED - aabb clipped by frustum + * CULLED - aabb totally outside frustum + */ +template +GCLASS TFrustum::Classify(const AABB& aabb) const +{ + bool bIntersects = false; + + // find and test extreme points + for (int i=0; i= TYPE(0)) { + ptPlaneMin(0) = aabb.ptMin(0); + ptPlaneMax(0) = aabb.ptMax(0); + } else { + ptPlaneMin(0) = aabb.ptMax(0); + ptPlaneMax(0) = aabb.ptMin(0); + } + // y coordinate + if (plane.m_vN(1) >= TYPE(0)) { + ptPlaneMin(1) = aabb.ptMin(1); + ptPlaneMax(1) = aabb.ptMax(1); + } else { + ptPlaneMin(1) = aabb.ptMax(1); + ptPlaneMax(1) = aabb.ptMin(1); + } + // z coordinate + if (plane.m_vN(2) >= TYPE(0)) { + ptPlaneMin(2) = aabb.ptMin(2); + ptPlaneMax(2) = aabb.ptMax(2); + } else { + ptPlaneMin(2) = aabb.ptMax(2); + ptPlaneMax(2) = aabb.ptMin(2); + } + + if (plane.m_vN.dot(ptPlaneMin) > -plane.m_fD) + return CULLED; + + if (plane.m_vN.dot(ptPlaneMax) >= -plane.m_fD) + bIntersects = true; + } // for + + if (bIntersects) return CLIPPED; + return VISIBLE; +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/Queue.h b/libs/Common/Queue.h new file mode 100644 index 0000000..a11bcf7 --- /dev/null +++ b/libs/Common/Queue.h @@ -0,0 +1,296 @@ +//////////////////////////////////////////////////////////////////// +// Queue.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_QUEUE_H__ +#define __SEACAVE_QUEUE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +/************************************************************************************** +// Queue template +**************************************************************************************/ +template +class cQueue +{ +public: + cQueue(int_t xSize = 16) : + first(-1), last(-1), vectorSize(0), vector(NULL) + { + if (xSize > 0) + Grow(xSize); + } + + ~cQueue() + { + Release(); + } + + void Grow(int_t gr) + { + ASSERT(gr > 0); + if (IsEmpty()) { + ASSERT(first == -1 && last == -1); + operator delete[] (vector); + vectorSize += gr; + vector = (TYPE*) operator new[] (vectorSize * sizeof(TYPE)); + return; + } + TYPE* tmp = (TYPE*) operator new[] ((vectorSize + gr) * sizeof(TYPE)); + if (first > last) { + memcpy(tmp, vector + first, (vectorSize - first) * sizeof(TYPE)); + memcpy(tmp + (vectorSize - first), vector, (last + 1) * sizeof(TYPE)); + last += vectorSize - first; + } else { + memcpy(tmp, vector + first, (last - first + 1) * sizeof(TYPE)); + last = last - first; + } + first = 0; + vectorSize += gr; + operator delete[] (vector); + vector = tmp; + } + + inline TYPE* GetData() + { + return vector; + } + + inline void PushHead() + { + _PushHead(); + if (useConstruct) + new(vector+(--first)) TYPE(); + else + --first; + } + inline void PushTail() + { + _PushTail(); + if (useConstruct) + new(vector+(++last)) TYPE(); + else + ++last; + } + + inline TYPE& AddEmptyHead() + { + _PushHead(); + if (useConstruct) + return *(new(vector+(--first)) TYPE()); + else + return vector[--first]; + } + inline TYPE& AddEmptyTail() + { + _PushTail(); + if (useConstruct) + return *(new(vector+(++last)) TYPE()); + else + return vector[++last]; + } + + inline void AddHead(ARG_TYPE elem) + { + _PushHead(); + if (useConstruct) + new(vector+(--first)) TYPE(elem); + else + vector[--first] = elem; + } + inline void AddTail(ARG_TYPE elem) + { + _PushTail(); + if (useConstruct) + new(vector+(++last)) TYPE(elem); + else + vector[++last] = elem; + } + + inline TYPE& GetHead() const + { + ASSERT(first >= 0 && last >= 0); + return vector[first]; + } + inline TYPE& GetTail() const + { + ASSERT(first >= 0 && last >= 0); + return vector[last]; + } + + inline TYPE& GetHead(uint_t delta) const + { + ASSERT(first >= 0 && last >= 0 && delta < GetSize()); + delta += first; + if ((int_t)delta < vectorSize) + return vector[delta]; + return vector[delta-vectorSize]; + } + inline TYPE& GetTail(uint_t delta) const + { + ASSERT(first >= 0 && last >= 0 && delta < GetSize()); + if ((int_t)delta <= last) + return vector[last-delta]; + return vector[vectorSize-(delta-last)]; + } + + inline void PopHead() + { + ASSERT(first >= 0 && last >= 0); + if (useConstruct) + (vector+first)->~TYPE(); + if (first > last) { + if (++first >= vectorSize) + first = 0; + } else { + if (++first > last) + first = last = -1; + } + } + inline void PopTail() + { + ASSERT(first >= 0 && last >= 0); + if (useConstruct) + (vector+last)->~TYPE(); + if (last >= first) { + if (--last < first) + first = last = -1; + } else { + if (--last < 0) + last = vectorSize-1; + } + } + + inline TYPE RemoveHead() + { + ASSERT(first >= 0 && last >= 0); + if (useConstruct) { + TYPE elem(vector[first]); + PopHead(); + return elem; + } else { + TYPE& elem(vector[first]); + PopHead(); + return elem; + } + } + inline TYPE RemoveTail() + { + ASSERT(first >= 0 && last >= 0); + if (useConstruct) { + TYPE elem(vector[last]); + PopTail(); + return elem; + } else { + TYPE& elem(vector[last]); + PopTail(); + return elem; + } + } + + inline uint_t GetSize() const + { + if (first < 0) + return 0; + if (first > last) // circular + return (vectorSize - (first - last) + 1); + return uint_t(last - first + 1); + } + + inline void Empty() + { + first = last = -1; + } + + // same as Empty(), plus free all allocated memory + inline void Release() + { + ASSERT((vector != NULL && vectorSize > 0) || (vector == NULL && vectorSize == 0)); + if (vector != NULL) { + if (useConstruct) { + if (first > last) { + for (; first < vectorSize; first++) + (vector+first)->~TYPE(); + for (; last >= 0; last--) + (vector+last)->~TYPE(); + } else if (first >= 0) { + for (; first <= last; first++) + (vector+first)->~TYPE(); + } + } + operator delete[] (vector); + vector = NULL; + vectorSize = 0; + } + Empty(); + } + + inline bool IsEmpty() const + { + return (first < 0); + } + + inline TYPE& operator[](uint_t index) const + { + return GetHead(index); + } + +protected: + inline void _PushHead() + { + if (first > last) { + if (last + 1 == first) { + Grow(xGrow); + first = vectorSize; + } + } else { + if (first <= 0) { + if (last + 1 >= vectorSize) + Grow(xGrow); + first = vectorSize; + } + if (last < 0) + last = first - 1; + } + } + inline void _PushTail() + { + if (last >= first) { + if (last + 1 >= vectorSize) { + if (first <= 0) + Grow(xGrow); + else + last = -1; + } + if (first < 0) + first = 0; + } else { + if (last + 1 == first) + Grow(xGrow); + } + } + +protected: + int_t first; + int_t last; + int_t vectorSize; + TYPE* vector; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_QUEUE_H__ diff --git a/libs/Common/Random.h b/libs/Common/Random.h new file mode 100644 index 0000000..6be44ad --- /dev/null +++ b/libs/Common/Random.h @@ -0,0 +1,164 @@ +//////////////////////////////////////////////////////////////////// +// Random.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_RANDOM_H__ +#define __SEACAVE_RANDOM_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// Stateless random number generation +// uniform random number generation +FORCEINLINE float random() { + return RANDOM(); +} +FORCEINLINE double randomd() { + return RANDOM(); +} +FORCEINLINE long double randomld() { + return RANDOM(); +} +STATIC_ASSERT(RAND_MAX < 2147483648); // integer randomRange assumes this is capped +template +FORCEINLINE T randomRange(T nMin, T nMax) { + ASSERT(nMin <= nMax && nMax-nMin+1 < 8589934596); // not to overflow a uint64_t + return nMin + T((uint64_t(nMax-nMin)*RAND()+RAND_MAX/2)/RAND_MAX); +} +template<> +FORCEINLINE float randomRange(float fMin, float fMax) { + return fMin + (fMax - fMin) * random(); +} +template<> +FORCEINLINE double randomRange(double fMin, double fMax) { + return fMin + (fMax - fMin) * randomd(); +} +template<> +FORCEINLINE long double randomRange(long double fMin, long double fMax) { + return fMin + (fMax - fMin) * randomld(); +} +template +FORCEINLINE T randomMeanRange(T mean, T delta/*=(max-min)/2*/) { + ASSERT(delta >= 0 && delta*2+1 < 8589934596); // not to overflow a uint64_t + return (mean + T((uint64_t(delta)*2*RAND()+RAND_MAX/2)/RAND_MAX)) - delta; +} +template<> +FORCEINLINE float randomMeanRange(float mean, float delta/*=(max-min)/2*/) { + return mean + delta * (2.f * random() - 1.f); +} +template<> +FORCEINLINE double randomMeanRange(double mean, double delta/*=(max-min)/2*/) { + return mean + delta * (2.0 * randomd() - 1.0); +} +template<> +FORCEINLINE long double randomMeanRange(long double mean, long double delta/*=(max-min)/2*/) { + return mean + delta * (2.0L * randomld() - 1.0L); +} +// gaussian random number generation +template +FORCEINLINE T gaussian(T val, T sigma) { + return EXP(-SQUARE(val/sigma)/2)/(SQRT(T(M_PI*2))*sigma); +} +template +FORCEINLINE T randomGaussian(T mean, T sigma) { + T x, y, r2; + do { + x = T(-1) + T(2) * RANDOM(); + y = T(-1) + T(2) * RANDOM(); + r2 = x * x + y * y; + } while (r2 > T(1) || r2 == T(0)); + return mean + sigma * y * SQRT(T(-2) * LOGN(r2) / r2); +} +template +FORCEINLINE T randomGaussian(T sigma) { + return randomGaussian(T(0), sigma); +} +FORCEINLINE float randomGaussian() { + return randomGaussian(0.f, 1.f); +} +FORCEINLINE double randomGaussiand() { + return randomGaussian(0.0, 1.0); +} +FORCEINLINE long double randomGaussianld() { + return randomGaussian(0.0L, 1.0L); +} +/*----------------------------------------------------------------*/ + + +// Encapsulates state for random number generation +// based on C++11 random number generator functionality +struct Random : std::mt19937 { + typedef std::mt19937 generator_type; + + Random() : generator_type(std::random_device()()) {} + Random(result_type seed) : generator_type(seed) {} + + // integer randomRange assumes this is capped + STATIC_ASSERT(max() < 4294967296); + + // returns a uniform random number in the range [0, 1] + template + FORCEINLINE typename std::enable_if::value, T>::type random() { + return (T)random()/(T)max(); + } + // returns a uniform random number in the range [0, max()] + template + FORCEINLINE typename std::enable_if::value, T>::type random() { + return (T)operator()(); + } + + // returns a uniform random number in the range [nMin, nMax] + template + FORCEINLINE typename std::enable_if::value, T>::type randomRange(T nMin, T nMax) { + return nMin + (nMax-nMin) * random(); + } + template + FORCEINLINE typename std::enable_if::value, T>::type randomRange(T nMin, T nMax) { + ASSERT(nMin <= nMax && nMax-nMin+1 < 4294967297); // not to overflow a uint64_t + return nMin + (T)(((uint64_t)(nMax-nMin) * random() + max()/2)/max()); + } + + // returns a uniform random number in the range [mean-delta, mean+delta] + template + FORCEINLINE typename std::enable_if::value, T>::type randomMeanRange(T mean, T delta/*=(max-min)/2*/) { + return mean + delta * (T(2) * random() - T(1)); + } + template + FORCEINLINE typename std::enable_if::value, T>::type randomMeanRange(T mean, T delta/*=(max-min)/2*/) { + ASSERT(delta >= 0 && delta*T(2)+1 < 4294967297); // not to overflow a uint64_t + return mean + (T)(((uint64_t)delta*2 * random() + max()/2)/max()) - delta; + } + + // returns a uniform random number in the range [nMin, nMax] using std implementation + template + FORCEINLINE typename std::enable_if::value, T>::type randomUniform(T nMin, T nMax) { + return std::uniform_real_distribution(nMin, nMax)(*this); + } + template + FORCEINLINE typename std::enable_if::value, T>::type randomUniform(T nMin, T nMax) { + return std::uniform_int_distribution(nMin, nMax)(*this); + } + + // returns a gaussian random number using std implementation + template + FORCEINLINE T randomGaussian(T mean, T stddev) { + return std::normal_distribution(mean, stddev)(*this); + } +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_RANDOM_H__ diff --git a/libs/Common/Ray.h b/libs/Common/Ray.h new file mode 100644 index 0000000..8549a2e --- /dev/null +++ b/libs/Common/Ray.h @@ -0,0 +1,226 @@ +//////////////////////////////////////////////////////////////////// +// Ray.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_RAY_H__ +#define __SEACAVE_RAY_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// Basic triangle class +template +class TTriangle +{ + STATIC_ASSERT(DIMS > 1 && DIMS <= 3); + +public: + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TAABB AABB; + typedef SEACAVE::TPlane PLANE; + enum { numScalar = (3*DIMS) }; + + POINT a, b, c; // triangle vertices + + //--------------------------------------- + + inline TTriangle() {} + inline TTriangle(const POINT&, const POINT&, const POINT&); + + inline void Set(const POINT&, const POINT&, const POINT&); + + inline POINT GetCenter() const; + inline AABB GetAABB() const; + inline PLANE GetPlane() const; + + inline const POINT& operator [] (BYTE i) const { ASSERT(i<3); return (&a)[i]; } + inline POINT& operator [] (BYTE i) { ASSERT(i<3); return (&a)[i]; } +}; // class TTriangle +/*----------------------------------------------------------------*/ + + +// Basic ray class +template +class TRay +{ + STATIC_ASSERT(DIMS > 1 && DIMS <= 3); + +public: + typedef Eigen::Matrix MATRIX; + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TTriangle TRIANGLE; + typedef SEACAVE::TSphere SPHERE; + typedef SEACAVE::TAABB AABB; + typedef SEACAVE::TOBB OBB; + typedef SEACAVE::TPlane PLANE; + enum { numScalar = (2*DIMS) }; + + VECTOR m_vDir; // ray direction (normalized) + POINT m_pOrig; // ray origin + + //--------------------------------------- + + inline TRay() {} + inline TRay(const POINT& pOrig, const VECTOR& vDir); + inline TRay(const POINT& pt0, const POINT& pt1, bool bPoints); + + inline void Set(const POINT& pOrig, const VECTOR& vDir); + inline void SetFromPoints(const POINT& pt0, const POINT& pt1); + inline TYPE SetFromPointsLen(const POINT& pt0, const POINT& pt1); + inline void DeTransform(const MATRIX&); // move to matrix space + + template + bool Intersects(const TRIANGLE&, TYPE *t) const; + template + bool Intersects(const TRIANGLE&, TYPE fL, TYPE *t) const; + bool Intersects(const PLANE& plane, bool bCull, + TYPE *t, POINT* pPtHit) const; + inline TYPE IntersectsDist(const PLANE& plane) const; + inline POINT Intersects(const PLANE& plane) const; + bool Intersects(const PLANE& plane, bool bCull, + TYPE fL, TYPE *t, POINT* pPtHit) const; + bool Intersects(const SPHERE& sphere) const; + bool Intersects(const SPHERE& sphere, TYPE& t) const; + bool Intersects(const AABB& aabb) const; + bool Intersects(const AABB& aabb, TYPE& t) const; + bool Intersects(const AABB& aabb, TYPE fL, TYPE *t) const; + bool Intersects(const OBB& obb) const; + bool Intersects(const OBB& obb, TYPE &t) const; + bool Intersects(const OBB& obb, TYPE fL, TYPE *t) const; + bool Intersects(const TRay& ray, TYPE& s) const; + bool Intersects(const TRay& ray, POINT& p) const; + bool Intersects(const TRay& ray, TYPE& s1, TYPE& s2) const; + bool IntersectsAprox(const TRay& ray, POINT& p) const; + bool IntersectsAprox2(const TRay& ray, POINT& p) const; + + inline TYPE CosAngle(const TRay&) const; + inline bool Coplanar(const TRay&) const; + inline bool Parallel(const TRay&) const; + + inline TYPE Classify(const POINT&) const; + inline POINT ProjectPoint(const POINT&) const; + bool DistanceSq(const POINT&, TYPE&) const; + bool Distance(const POINT&, TYPE&) const; + TYPE DistanceSq(const POINT&) const; + TYPE Distance(const POINT&) const; + POINT GetPoint(TYPE) const; + + TRay operator * (const MATRIX&) const; // matrix multiplication + inline TRay& operator *= (const MATRIX&); // matrix multiplication + + inline TYPE& operator [] (BYTE i) { ASSERT(i<6); return m_vDir.data()[i]; } + inline TYPE operator [] (BYTE i) const { ASSERT(i<6); return m_vDir.data()[i]; } +}; // class TRay +/*----------------------------------------------------------------*/ + + +// Basic cylinder class +template +class TCylinder +{ + STATIC_ASSERT(DIMS > 1 && DIMS <= 3); + +public: + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TRay RAY; + typedef SEACAVE::TSphere SPHERE; + enum { numScalar = (2*DIMS+2) }; + + RAY ray; // central ray defining starting the point and direction + TYPE radius; // cylinder radius + TYPE minHeight; // cylinder heights satisfying: + TYPE maxHeight; // std::numeric_limits::lowest() <= minHeight <= 0 < maxHeight <= std::numeric_limits::max() + + //--------------------------------------- + + inline TCylinder() {} + + inline TCylinder(const RAY& _ray, TYPE _radius, TYPE _minHeight=TYPE(0), TYPE _maxHeight=std::numeric_limits::max()) + : ray(_ray), radius(_radius), minHeight(_minHeight), maxHeight(_maxHeight) { ASSERT(TYPE(0) >= minHeight && minHeight < maxHeight); } + + inline bool IsFinite() const { return maxHeight < std::numeric_limits::max() && minHeight > std::numeric_limits::lowest(); } + inline TYPE GetLength() const { return maxHeight - minHeight; } + + bool Intersects(const SPHERE&) const; + + inline GCLASS Classify(const POINT&, TYPE& t) const; +}; // class TCylinder +/*----------------------------------------------------------------*/ + + +// Basic cone class +template +class TCone +{ + STATIC_ASSERT(DIMS > 1 && DIMS <= 3); + +public: + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TRay RAY; + typedef SEACAVE::TSphere SPHERE; + enum { numScalar = (2*DIMS+3) }; + + RAY ray; // ray origin is the cone vertex and ray direction is the cone axis direction + TYPE angle; // cone angle, in radians, must be inside [0, pi/2] + TYPE minHeight; // heights satisfying: + TYPE maxHeight; // 0 <= minHeight < maxHeight <= std::numeric_limits::max() + + //--------------------------------------- + + inline TCone() {} + inline TCone(const RAY& _ray, TYPE _angle, TYPE _minHeight=TYPE(0), TYPE _maxHeight=std::numeric_limits::max()) + : ray(_ray), angle(_angle), minHeight(_minHeight), maxHeight(_maxHeight) { ASSERT(TYPE(0) <= minHeight && minHeight < maxHeight); } +}; // class TCone + +// Structure used to compute the intersection between a cone and the given sphere/point +template +class TConeIntersect +{ + STATIC_ASSERT(DIMS > 1 && DIMS <= 3); + +public: + typedef Eigen::Matrix VECTOR; + typedef Eigen::Matrix POINT; + typedef SEACAVE::TCone CONE; + typedef SEACAVE::TSphere SPHERE; + + const CONE& cone; + + // cache angle derivatives, to avoid calling trigonometric functions in geometric queries + const TYPE cosAngle, sinAngle, sinAngleSq, cosAngleSq, invSinAngle; + + //--------------------------------------- + + inline TConeIntersect(const CONE& _cone) + : cone(_cone), cosAngle(COS(cone.angle)), sinAngle(SIN(cone.angle)), + sinAngleSq(SQUARE(sinAngle)), cosAngleSq(SQUARE(cosAngle)), + invSinAngle(TYPE(1)/sinAngle) {} + + bool operator()(const SPHERE&) const; + + GCLASS Classify(const POINT&, TYPE& t) const; +}; // class TConeIntersect +/*----------------------------------------------------------------*/ + + +#include "Ray.inl" +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_RAY_H__ diff --git a/libs/Common/Ray.inl b/libs/Common/Ray.inl new file mode 100644 index 0000000..34d4f45 --- /dev/null +++ b/libs/Common/Ray.inl @@ -0,0 +1,951 @@ +//////////////////////////////////////////////////////////////////// +// Ray.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +template +inline TTriangle::TTriangle(const POINT& p0, const POINT& p1, const POINT& p2) + : + a(p0), b(p1), c(p2) +{ +} // constructors +/*----------------------------------------------------------------*/ + +// set attributes +template +inline void TTriangle::Set(const POINT& p0, const POINT& p1, const POINT& p2) +{ + a = p0; + b = p1; + c = p2; +} +/*----------------------------------------------------------------*/ + + +// get center +template +inline typename TTriangle::POINT TTriangle::GetCenter() const +{ + return (a + b + c) / TYPE(3); +} +// get AABB +template +inline typename TTriangle::AABB TTriangle::GetAABB() const +{ + return AABB(& operator [] (0), 3); +} +// get plane +template +inline typename TTriangle::PLANE TTriangle::GetPlane() const +{ + return PLANE(a, b, c); +} +/*----------------------------------------------------------------*/ + + +// S T R U C T S /////////////////////////////////////////////////// + +template +inline TRay::TRay(const POINT& pOrig, const VECTOR& vDir) + : + m_vDir(vDir), m_pOrig(pOrig) +{ + ASSERT(ISEQUAL(m_vDir.squaredNorm(), TYPE(1))); +} // constructors +template +inline TRay::TRay(const POINT& pt0, const POINT& pt1, bool /*bPoints*/) + : + m_vDir((pt1-pt0).normalized()), m_pOrig(pt0) +{ +} // constructors +/*----------------------------------------------------------------*/ + +// set attributes +template +inline void TRay::Set(const POINT& pOrig, const VECTOR& vDir) +{ + m_vDir = vDir; + m_pOrig = pOrig; + ASSERT(ISEQUAL(m_vDir.squaredNorm(), TYPE(1))); +} +template +inline void TRay::SetFromPoints(const POINT& pt0, const POINT& pt1) +{ + m_vDir = (pt1-pt0).normalized(); + m_pOrig = pt0; +} +template +inline TYPE TRay::SetFromPointsLen(const POINT& pt0, const POINT& pt1) +{ + m_vDir = pt1-pt0; + const TYPE len = m_vDir.norm(); + if (len > TYPE(0)) + m_vDir *= TYPE(1)/len; + m_pOrig = pt0; + return len; +} +/*----------------------------------------------------------------*/ + + +// transform ray into matrix space +template +inline void TRay::DeTransform(const MATRIX& _m) +{ + STATIC_ASSERT(DIMS == 3); + MATRIX m(_m); + + // invert translation + typedef Eigen::Matrix VECTOR4; + const VECTOR4 vcOrig(m_pOrig[0]-m(3,0), m_pOrig[1]-m(3,1), m_pOrig[2]-m(3,2)); + + // delete it from matrix + m(3,0) = m(3,1) = m(3,2) = TYPE(0); + + // invert matrix and apply to ray + const MATRIX mInv = m.inverse(); + m_pOrig = vcOrig * mInv; + m_vDir = VECTOR4(m_vDir) * mInv; +} +/*----------------------------------------------------------------*/ + + +// Transform the ray by a matrix. +template +TRay TRay::operator*(const MATRIX& m) const +{ + TRay ray; + ray.SetFromPoints(m_pOrig*m, (m_pOrig+m_vDir)*m); + return ray; +} // operator * +template +inline TRay& TRay::operator*=(const MATRIX& m) +{ + *this = operator * (m); + return *this; +} // operator *= +/*----------------------------------------------------------------*/ + + +// test for intersection with triangle +template +template +bool TRay::Intersects(const TRIANGLE& tri, TYPE *pt) const +{ + const VECTOR edge1(tri.b - tri.a); + const VECTOR edge2(tri.c - tri.a); + const VECTOR pvec(m_vDir.cross(edge2)); + const TYPE det(edge1.dot(pvec)); + // check if ray and triangle plane are parallel + if ((bCull ? det : ABS(det)) < ZEROTOLERANCE() * TYPE(0.01)) + return false; + const TYPE invDet(TYPE(1) / det); + const VECTOR tvec(m_pOrig - tri.a); + const TYPE u(tvec.dot(pvec) * invDet); + // check if intersection is outside of the line segment defined by points a and c + if (u < -ZEROTOLERANCE() * TYPE(10)) + return false; + const VECTOR qvec(tvec.cross(edge1)); + const TYPE v(m_vDir.dot(qvec) * invDet); + // check if intersection is outside of the line segment defined by points a and b + if (v < -ZEROTOLERANCE() * TYPE(10)) + return false; + // check if intersection is outside of the line segment defined by points b and c + if (u + v > TYPE(1) + ZEROTOLERANCE() * TYPE(10)) + return false; + const TYPE t(edge2.dot(qvec) * invDet); + // check if intersection is behind the ray origin + if (bCull && t < -ZEROTOLERANCE()) + return false; + if (pt) + *pt = t; + return true; +} // Intersects(Tri) +/*----------------------------------------------------------------*/ + +// test for intersection with triangle at certain length (line segment); +// same as above, but test distance to intersection vs segment length +template +template +bool TRay::Intersects(const TRIANGLE& tri, TYPE fL, TYPE *t) const +{ + TYPE _t; + TYPE* const pt(t ? t : &_t); + Intersects(tri, pt); + // collision but not on segment? + return *pt <= fL; +} // Intersects(Tri at length) +/*----------------------------------------------------------------*/ + + +// test if the ray intersects the given sphere +template +bool TRay::Intersects(const SPHERE& sphere) const +{ + TYPE dSq; + if (!DistanceSq(sphere.center, dSq)) + return false; + return dSq <= SQUARE(sphere.radius); +} +// same as above, but returns also the distance on the ray corresponding to the closest point +template +bool TRay::Intersects(const SPHERE& sphere, TYPE& t) const +{ + const VECTOR a(sphere.center - m_pOrig); + t = a.dot(m_vDir); + // point behind the ray origin + if (t < TYPE(0)) + return false; + const TYPE dSq((a - m_vDir*t).squaredNorm()); + return dSq <= SQUARE(sphere.radius); +} // Intersects(Sphere) +/*----------------------------------------------------------------*/ + + +template +bool TRay::Intersects(const AABB &aabb) const +{ + bool to_infinity = true; + TYPE _min, _max; + // first on x value + if (m_vDir[0] == TYPE(0)) { + if (m_pOrig[0] < aabb.ptMin[0] || m_pOrig[0] > aabb.ptMax[0]) + return false; // NO_INTERSECTION + } else { + if (m_vDir[0] > TYPE(0)) { + _min = (aabb.ptMin[0]-m_pOrig[0])/m_vDir[0]; + _max = (aabb.ptMax[0]-m_pOrig[0])/m_vDir[0]; + } else { + _min = (aabb.ptMax[0]-m_pOrig[0])/m_vDir[0]; + _max = (aabb.ptMin[0]-m_pOrig[0])/m_vDir[0]; + } + to_infinity = false; + } + // now on y value + if (m_vDir[1] == TYPE(0)) { + if (m_pOrig[1] < aabb.ptMin[1] || m_pOrig[1] > aabb.ptMax[1]) + return false; // NO_INTERSECTION + } else { + TYPE newmin, newmax; + if (m_vDir[1] > TYPE(0)) { + newmin = (aabb.ptMin[1]-m_pOrig[1])/m_vDir[1]; + newmax = (aabb.ptMax[1]-m_pOrig[1])/m_vDir[1]; + } else { + newmin = (aabb.ptMax[1]-m_pOrig[1])/m_vDir[1]; + newmax = (aabb.ptMin[1]-m_pOrig[1])/m_vDir[1]; + } + #if 0 + if (to_infinity) { + _min = newmin; + _max = newmax; + } else { + #else + if (to_infinity) + return true; + { + #endif + if (newmin > _min) + _min = newmin; + if (newmax < _max) + _max = newmax; + if (_max < _min) + return false; // NO_INTERSECTION + } + to_infinity = false; + } + // now on z value + if (DIMS == 3) { + if (m_vDir[2] == TYPE(0)) { + if (m_pOrig[2] < aabb.ptMin[2] || m_pOrig[2] > aabb.ptMax[2]) + return false; // NO_INTERSECTION + } else { + TYPE newmin, newmax; + if (m_vDir[2] > TYPE(0)) { + newmin = (aabb.ptMin[2]-m_pOrig[2])/m_vDir[2]; + newmax = (aabb.ptMax[2]-m_pOrig[2])/m_vDir[2]; + } else { + newmin = (aabb.ptMax[2]-m_pOrig[2])/m_vDir[2]; + newmax = (aabb.ptMin[2]-m_pOrig[2])/m_vDir[2]; + } + #if 0 + if (to_infinity) { + _min = newmin; + _max = newmax; + } else { + #else + if (to_infinity) + return true; + { + #endif + if (newmin > _min) + _min = newmin; + if (newmax < _max) + _max = newmax; + if (_max < _min) + return false; // NO_INTERSECTION + } + to_infinity = false; + } + } + ASSERT(!to_infinity); + #if 0 + if (_max < _min) + return true; // POINT_INTERSECTION + #endif + return true; // SEGMENT_INTERSECTION +} // Intersects(AABB) + +// test for intersection with aabb, original code by Andrew Woo, +// from "Geometric Tools...", Morgan Kaufmann Publ., 2002 +template +bool TRay::Intersects(const AABB &aabb, TYPE& t) const +{ + TYPE t0, t1, tmp; + TYPE tNear(-999999); + TYPE tFar ( 999999); + + // first pair of planes + if (ISZERO(m_vDir[0])) { + if ((m_pOrig[0] < aabb.ptMin[0]) || + (m_pOrig[0] > aabb.ptMax[0])) + return false; + } + t0 = (aabb.ptMin[0] - m_pOrig[0]) / m_vDir[0]; + t1 = (aabb.ptMax[0] - m_pOrig[0]) / m_vDir[0]; + if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; } + if (t0 > tNear) tNear = t0; + if (t1 < tFar) tFar = t1; + if (tNear > tFar) return false; + if (tFar < TYPE(0)) return false; + + // second pair of planes + if (ISZERO(m_vDir[1])) { + if ((m_pOrig[1] < aabb.ptMin[1]) || + (m_pOrig[1] > aabb.ptMax[1]) ) + return false; + } + t0 = (aabb.ptMin[1] - m_pOrig[1]) / m_vDir[1]; + t1 = (aabb.ptMax[1] - m_pOrig[1]) / m_vDir[1]; + if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; } + if (t0 > tNear) tNear = t0; + if (t1 < tFar) tFar = t1; + if (tNear > tFar) return false; + if (tFar < TYPE(0)) return false; + + if (DIMS == 3) { + // third pair of planes + if (ISZERO(m_vDir[2])) { + if ((m_pOrig[2] < aabb.ptMin[2]) || + (m_pOrig[2] > aabb.ptMax[2]) ) + return false; + } + t0 = (aabb.ptMin[2] - m_pOrig[2]) / m_vDir[2]; + t1 = (aabb.ptMax[2] - m_pOrig[2]) / m_vDir[2]; + if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; } + if (t0 > tNear) tNear = t0; + if (t1 < tFar) tFar = t1; + if (tNear > tFar) return false; + if (tFar < TYPE(0)) return false; + } + + t = (tNear > TYPE(0) ? tNear : tFar); + return true; +} // Intersects(AABB) + +// test for intersection with aabb, original code by Andrew Woo, +// from "Geometric Tools...", Morgan Kaufmann Publ., 2002 +template +bool TRay::Intersects(const AABB &aabb, TYPE fL, TYPE *t) const +{ + TYPE t0, t1, tmp, tFinal; + TYPE tNear(-999999); + TYPE tFar ( 999999); + + // first pair of planes + if (ISZERO(m_vDir[0])) { + if ((m_pOrig[0] < aabb.ptMin[0]) || + (m_pOrig[0] > aabb.ptMax[0]) ) + return false; + } + t0 = (aabb.ptMin[0] - m_pOrig[0]) / m_vDir[0]; + t1 = (aabb.ptMax[0] - m_pOrig[0]) / m_vDir[0]; + if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; } + if (t0 > tNear) tNear = t0; + if (t1 < tFar) tFar = t1; + if (tNear > tFar) return false; + if (tFar < TYPE(0)) return false; + + // second pair of planes + if (ISZERO(m_vDir[1])) { + if ((m_pOrig[1] < aabb.ptMin[1]) || + (m_pOrig[1] > aabb.ptMax[1]) ) + return false; + } + t0 = (aabb.ptMin[1] - m_pOrig[1]) / m_vDir[1]; + t1 = (aabb.ptMax[1] - m_pOrig[1]) / m_vDir[1]; + if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; } + if (t0 > tNear) tNear = t0; + if (t1 < tFar) tFar = t1; + if (tNear > tFar) return false; + if (tFar < TYPE(0)) return false; + + if (DIMS == 3) { + // third pair of planes + if (ISZERO(m_vDir[2])) { + if ((m_pOrig[2] < aabb.ptMin[2]) || + (m_pOrig[2] > aabb.ptMax[2]) ) + return false; + } + t0 = (aabb.ptMin[2] - m_pOrig[2]) / m_vDir[2]; + t1 = (aabb.ptMax[2] - m_pOrig[2]) / m_vDir[2]; + if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; } + if (t0 > tNear) tNear = t0; + if (t1 < tFar) tFar = t1; + if (tNear > tFar) return false; + if (tFar < 0) return false; + } + + tFinal = (tNear > TYPE(0) ? tNear : tFar); + + if (tFinal > fL) return false; + if (t) *t = tFinal; + return true; +} // Intersects(AABB) at length +/*----------------------------------------------------------------*/ + + +template +bool TRay::Intersects(const OBB& obb) const +{ + TYPE t; + return Intersects(obb, t); +} // Intersects(OBB) + +// test for intersection with obb, slaps method +template +bool TRay::Intersects(const OBB& obb, TYPE& t) const +{ + TYPE e, f, t1, t2, temp; + TYPE tmin(-999999); + TYPE tmax( 999999); + + const POINT vP = obb.m_pos - m_pOrig; + + // 1st slap + e = obb.m_rot.row(0) * vP; + f = obb.m_rot.row(0) * m_vDir; + if (!ISZERO(f)) { + t1 = (e + obb.m_ext[0]) / f; + t2 = (e - obb.m_ext[0]) / f; + + if (t1 > t2) { temp=t1; t1=t2; t2=temp; } + if (t1 > tmin) tmin = t1; + if (t2 < tmax) tmax = t2; + if (tmin > tmax) return false; + if (tmax < 0.0f) return false; + } else + if (((-e - obb.m_ext[0]) > 0.0f) || ((-e + obb.m_ext[0]) < 0.0f)) + return false; + + // 2nd slap + e = obb.m_rot.row(1) * vP; + f = obb.m_rot.row(1) * m_vDir; + if (!ISZERO(f)) { + t1 = (e + obb.m_ext[1]) / f; + t2 = (e - obb.m_ext[1]) / f; + + if (t1 > t2) { temp=t1; t1=t2; t2=temp; } + if (t1 > tmin) tmin = t1; + if (t2 < tmax) tmax = t2; + if (tmin > tmax) return false; + if (tmax < 0.0f) return false; + } else + if (((-e - obb.m_ext[1]) > 0.0f) || ((-e + obb.m_ext[1]) < 0.0f)) + return false; + + // 3rd slap + e = obb.m_rot.row(2) * vP; + f = obb.m_rot.row(2) * m_vDir; + if (!ISZERO(f)) { + t1 = (e + obb.m_ext[2]) / f; + t2 = (e - obb.m_ext[2]) / f; + + if (t1 > t2) { temp=t1; t1=t2; t2=temp; } + if (t1 > tmin) tmin = t1; + if (t2 < tmax) tmax = t2; + if (tmin > tmax) return false; + if (tmax < 0.0f) return false; + } else + if (((-e - obb.m_ext[2]) > 0) || ((-e + obb.m_ext[2]) < 0)) + return false; + + if (tmin > 0) { + if (t) *t = tmin; + return true; + } + + t = tmax; + return true; +} // Intersects(AABB) + +// test for intersection with obb at certain length (line segment), +// slaps method but compare result if true to length prior return. +template +bool TRay::Intersects(const OBB& obb, TYPE fL, TYPE *t) const +{ + TYPE e, f, t1, t2, temp; + TYPE tmin(-999999); + TYPE tmax( 999999); + + const POINT vP = obb.m_pos - m_pOrig; + + // 1st slap + e = obb.m_rot.row(0) * vP; + f = obb.m_rot.row(0) * m_vDir; + if (!ISZERO(f)) { + t1 = (e + obb.m_ext[0]) / f; + t2 = (e - obb.m_ext[0]) / f; + + if (t1 > t2) { temp=t1; t1=t2; t2=temp; } + if (t1 > tmin) tmin = t1; + if (t2 < tmax) tmax = t2; + if (tmin > tmax) return false; + if (tmax < 0.0f) return false; + } else + if (((-e - obb.m_ext[0]) > 0) || ((-e + obb.m_ext[0]) < 0)) + return false; + + // 2nd slap + e = obb.m_rot.row(1) * vP; + f = obb.m_rot.row(1) * m_vDir; + if (!ISZERO(f)) { + t1 = (e + obb.m_ext[1]) / f; + t2 = (e - obb.m_ext[1]) / f; + + if (t1 > t2) { temp=t1; t1=t2; t2=temp; } + if (t1 > tmin) tmin = t1; + if (t2 < tmax) tmax = t2; + if (tmin > tmax) return false; + if (tmax < 0.0f) return false; + } else + if (((-e - obb.m_ext[1]) > 0) || ((-e + obb.m_ext[1]) < 0)) + return false; + + // 3rd slap + e = obb.m_rot.row(2) * vP; + f = obb.m_rot.row(2) * m_vDir; + if (!ISZERO(f)) { + t1 = (e + obb.m_ext[2]) / f; + t2 = (e - obb.m_ext[2]) / f; + + if (t1 > t2) { temp=t1; t1=t2; t2=temp; } + if (t1 > tmin) tmin = t1; + if (t2 < tmax) tmax = t2; + if (tmin > tmax) return false; + if (tmax < 0.0f) return false; + } else + if (((-e - obb.m_ext[2]) > 0) || ((-e + obb.m_ext[2]) < 0)) + return false; + + if ((tmin > 0) && (tmin <= fL)) { + if (t) *t = tmin; + return true; + } + + // intersection on line but not on segment + if (tmax > fL) return false; + + if (t) *t = tmax; + + return true; +} // Intersects(AABB) at length +/*----------------------------------------------------------------*/ + + +// Intersection with PLANE from origin till infinity. +template +bool TRay::Intersects(const PLANE& plane, bool bCull, TYPE *t, POINT* pPtHit) const +{ + const TYPE Vd(plane.m_vN.dot(m_vDir)); + + // ray parallel to plane + if (ISZERO(Vd)) + return false; + + // normal pointing away from ray dir + // => intersection backface if any + if (bCull && (Vd > TYPE(0))) + return false; + + const TYPE Vo(-plane.Distance(m_pOrig)); + + const TYPE _t(Vo / Vd); + + // intersection behind ray origin + if (_t < TYPE(0)) + return false; + + if (pPtHit) + (*pPtHit) = m_pOrig + (m_vDir * _t); + + if (t) + (*t) = _t; + + return true; +} // Intersects(PLANE) +// same as above, but no checks +template +inline TYPE TRay::IntersectsDist(const PLANE& plane) const +{ + const TYPE Vd(plane.m_vN.dot(m_vDir)); + const TYPE Vo(-plane.Distance(m_pOrig)); + return SAFEDIVIDE(Vo, Vd); +} // IntersectsDist(PLANE) +template +inline typename TRay::POINT TRay::Intersects(const PLANE& plane) const +{ + return m_pOrig + (m_vDir * IntersectsDist(plane)); +} // Intersects(PLANE) +/*----------------------------------------------------------------*/ + + +// Intersection with PLANE at distance fL. +template +bool TRay::Intersects(const PLANE& plane, bool bCull, TYPE fL, TYPE *t, POINT* pPtHit) const +{ + const TYPE Vd = plane.m_vN.dot(m_vDir); + + // ray parallel to plane + if (ISZERO(Vd)) + return false; + + // normal pointing away from ray dir + // => intersection backface if any + if (bCull && (Vd > TYPE(0))) + return false; + + const TYPE Vo(-plane.Distance(m_pOrig)); + + const TYPE _t(Vo / Vd); + + // intersection behind ray origin or beyond valid range + if ((_t < TYPE(0)) || (_t > fL)) + return false; + + if (pPtHit) + (*pPtHit) = m_pOrig + (m_vDir * _t); + + if (t) + (*t) = _t; + + return true; +} // Intersects(PLANE) +/*----------------------------------------------------------------*/ + + +// Intersection with another ray. +// Rays assumed to be coplanar; returns only the distance from the origin of the first ray. +// P = R.origin + s * R.dir +// http://mathworld.wolfram.com/Line-LineIntersection.html +// (works also for 2D lines if z components is set to 0) +template +bool TRay::Intersects(const TRay& ray, TYPE& s) const +{ + const POINT dir(ray.m_pOrig - m_pOrig); + const POINT n(m_vDir.cross(ray.m_vDir)); + if (!ISZERO(dir.dot(n))) + return false; // lines are not coplanar + s = dir.cross(ray.m_vDir).dot(n) / n.squaredNorm(); + return true; +} // Intersects(Ray) +// Same as above, but returns the actual intersection point: +template +bool TRay::Intersects(const TRay& ray, POINT& p) const +{ + TYPE s; + if (!Intersects(ray, s)) + return false; // lines are not coplanar + if (s < TYPE(0)) + return false; // intersection behind ray origin + p = m_pOrig + m_vDir * s; + return true; +} // Intersects(Ray) +// No coplanarity assumption; returns the shortest line segment connecting the two rays: +// P1 = R1.origin + s1 * R1.dir +// P2 = R2.origin + s2 * R2.dir +// http://paulbourke.net/geometry/pointlineplane/ +template +bool TRay::Intersects(const TRay& ray, TYPE& s1, TYPE& s2) const +{ + const TYPE d4321(ray.m_vDir.dot(m_vDir)); + const TYPE d4343(ray.m_vDir.dot(ray.m_vDir)); + const TYPE d2121(m_vDir.dot(m_vDir)); + const TYPE denom = d2121 * d4343 - d4321 * d4321; + if (ISZERO(denom)) + return false; // lines are parallel + const POINT dir(m_pOrig - ray.m_pOrig); + const TYPE d1321(dir.dot(m_vDir)); + const TYPE d1343(dir.dot(ray.m_vDir)); + const TYPE numer = d1343 * d4321 - d1321 * d4343; + s1 = numer / denom; + s2 = (d1343 + d4321 * s1) / d4343; + return true; +} // Intersects(Ray) +// Same as above, but returns only middle point of the shortest line segment: +// P = (P1 + P2) / 2 +template +bool TRay::IntersectsAprox(const TRay& ray, POINT& p) const +{ + TYPE s1, s2; + if (!Intersects(ray, s1, s2)) + return false; + const POINT P1 = m_pOrig + m_vDir * s1; + const POINT P2 = ray.m_pOrig + ray.m_vDir * s2; + p = (P1+P2)*TYPE(0.5); + return true; +} // Intersects(Ray) +// Same as above, but returns the point on the given ray. +// Returns false if intersection not in front of main ray. +template +bool TRay::IntersectsAprox2(const TRay& ray, POINT& p) const +{ + TYPE s1, s2; + if (!Intersects(ray, s1, s2)) + return false; + if (s1 < TYPE(0)) + return false; // intersection behind main ray origin + p = ray.m_pOrig + ray.m_vDir * s2; + return true; +} // Intersects(Ray) +/*----------------------------------------------------------------*/ + + +// computes the angle between the two lines +template +inline TYPE TRay::CosAngle(const TRay& ray) const +{ + ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1))); + ASSERT(ISEQUAL(ray.m_vDir.norm(), TYPE(1))); + return m_vDir.dot(ray.m_vDir); +} +// tests if the two lines are coplanar (intersect) +template +inline bool TRay::Coplanar(const TRay& ray) const +{ + const POINT dir(ray.m_pOrig - m_pOrig); + const POINT n(m_vDir.cross(ray.m_vDir)); + return ISZERO(dir.dot(n)); +} +// tests if the two lines are parallel +template +inline bool TRay::Parallel(const TRay& ray) const +{ + return ISZERO(m_vDir.cross(ray.m_vDir).norm()); +} +/*----------------------------------------------------------------*/ + + +// Computes the position on the ray of the point projection. +// Returns 0 if it coincides with the ray origin, positive value if in front, and negative if behind. +template +inline TYPE TRay::Classify(const POINT& pt) const +{ + ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1))); + const VECTOR a(pt - m_pOrig); + return a.dot(m_vDir); +} // Classify(POINT) +template +inline typename TRay::POINT TRay::ProjectPoint(const POINT& pt) const +{ + ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1))); + return m_pOrig + m_vDir*Classify(pt); +} // ProjectPoint +/*----------------------------------------------------------------*/ + +// Computes the distance between the ray and a point. +// Returns false if the point is projecting behind the ray origin. +template +bool TRay::DistanceSq(const POINT& pt, TYPE& d) const +{ + const VECTOR a(pt - m_pOrig); + const TYPE LenACos(a.dot(m_vDir)); + // point behind the ray origin + if (LenACos < TYPE(0)) + return false; + d = (a - m_vDir*LenACos).squaredNorm(); + return true; +} // DistanceSq(POINT) +template +bool TRay::Distance(const POINT& pt, TYPE& d) const +{ + const VECTOR a(pt - m_pOrig); + const TYPE LenACos(a.dot(m_vDir)); + // point behind the ray origin + if (LenACos < TYPE(0)) + return false; + d = (a - m_vDir*LenACos).norm(); + return true; +} // Distance(POINT) +// Same as above, but returns the distance even if the point projection is behind the origin. +template +TYPE TRay::DistanceSq(const POINT& pt) const +{ + ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1))); + const VECTOR a(pt - m_pOrig); + const VECTOR b(a - m_vDir); + return b.cross(a).squaredNorm(); +} // DistanceSq(POINT) +template +TYPE TRay::Distance(const POINT& pt) const +{ + return SQRT(DistanceSq(pt)); +} // Distance(POINT) +// Get the point on the ray at distance t from origin +template +typename TRay::POINT TRay::GetPoint(TYPE t) const +{ + return m_pOrig + m_vDir * t; +} // GetPoint +/*----------------------------------------------------------------*/ + + +// S T R U C T S /////////////////////////////////////////////////// + +template +bool TCylinder::Intersects(const SPHERE& sphere) const +{ + struct Check { + static bool Intersection(const RAY& ray, const SPHERE& sphere, const VECTOR& CmV, TYPE t, TYPE radius, TYPE height) { + const POINT O(ray.m_pOrig + ray.m_vDir * height); + const VECTOR a(sphere.center - O); + if (a.squaredNorm() <= SQUARE(sphere.radius)) + return true; // ray origin inside the sphere + const POINT D((CmV - ray.m_vDir * t).normalized()); + const TYPE d = a.dot(D); + if (d < TYPE(0)) + return false; // intersection behind the ray origin + const TYPE dSq((a - D*d).squaredNorm()); + const TYPE srSq = SQUARE(sphere.radius); + if (dSq <= srSq) { + const TYPE r = d - SQRT(srSq-dSq); + ASSERT(r >= TYPE(0)); + return r <= radius; // intersection before the ray end + } + return false; + } + }; + const VECTOR CmV(sphere.center - ray.m_pOrig); + const TYPE t(ray.m_vDir.dot(CmV)); + // sphere projects behind the cylinder origin + if (t < minHeight) { + if (t+sphere.radius < minHeight) + return false; + return Check::Intersection(ray, sphere, CmV, t, radius, minHeight); + } + // sphere projects after the cylinder end + if (t > maxHeight) { + if (t-sphere.radius > maxHeight) + return false; + return Check::Intersection(ray, sphere, CmV, t, radius, maxHeight); + } + const TYPE lenSq((CmV - ray.m_vDir * t).squaredNorm()); + return lenSq <= SQUARE(sphere.radius + radius); +} // Intersects +/*----------------------------------------------------------------*/ + +// Classify point to cylinder. +template +GCLASS TCylinder::Classify(const POINT& p, TYPE& t) const +{ + ASSERT(ISEQUAL(ray.m_vDir.norm(), TYPE(1))); + const VECTOR D(p - ray.m_pOrig); + t = ray.m_vDir.dot(D); + if (t < minHeight) return BACK; + if (t > maxHeight) return FRONT; + const TYPE rSq((D - ray.m_vDir*t).squaredNorm()); + const TYPE radiusSq(SQUARE(radius)); + if (rSq > radiusSq) return CULLED; + if (rSq < radiusSq) return VISIBLE; + return PLANAR; +} // Classify +/*----------------------------------------------------------------*/ + + +// S T R U C T S /////////////////////////////////////////////////// + +template +bool TConeIntersect::operator()(const SPHERE& sphere) const +{ + const VECTOR CmV(sphere.center - cone.ray.m_pOrig); + const VECTOR D(CmV + cone.ray.m_vDir * (sphere.radius*invSinAngle)); + TYPE e = D.dot(cone.ray.m_vDir); + if (e <= TYPE(0) || e*e < D.squaredNorm()*cosAngleSq) + return false; + e = CmV.dot(cone.ray.m_vDir); + if (e-sphere.radius > cone.maxHeight) + return false; + if (e < cone.minHeight) { + const TYPE lenSq = CmV.squaredNorm(); + if (e*e >= lenSq*sinAngleSq) + return lenSq <= SQUARE(sphere.radius); + } + return true; +} // Intersect +/*----------------------------------------------------------------*/ + +// Classify point to cone. +template +GCLASS TConeIntersect::Classify(const POINT& p, TYPE& t) const +{ + ASSERT(ISEQUAL(cone.ray.m_vDir.norm(), TYPE(1))); + const VECTOR D(p - cone.ray.m_pOrig); + t = cone.ray.m_vDir.dot(D); + if (ISZERO(t)) + return PLANAR; + if (t < cone.minHeight) return BACK; + if (t > cone.maxHeight) return FRONT; + ASSERT(!ISZERO(D.norm())); + const TYPE tSq(SQUARE(t)); + const TYPE dSq(cosAngleSq*D.squaredNorm()); + if (tSq < dSq) return CULLED; + if (tSq > dSq) return VISIBLE; + return PLANAR; +} // Classify +/*----------------------------------------------------------------*/ + +template +bool TestRayTriangleIntersection(unsigned iters) { + typedef SEACAVE::TTriangle Triangle; + typedef SEACAVE::TRay Ray; + typedef typename Ray::POINT Point; + typedef typename Point::Scalar Type; + constexpr Type zeroEps(std::is_same::value ? 0.01f : 0.00001f); + for (unsigned iter=0, lives=iters/100; iter(triangle, &t) && !lives--) + return false; + const Point _center(rayCenter.GetPoint(t)); + if ((_center-center).norm() > zeroEps && !lives--) + return false; + const BYTE o((BYTE)(RAND()%3)); + const Point side(((triangle[o].template cast()+triangle[(o+1)%3].template cast()) / 2.0).template cast()); + const Ray raySide(rayCenter.m_pOrig, side, true); + if (!raySide.template Intersects(triangle, &t) && !lives--) + return false; + const Point _side(raySide.GetPoint(t)); + if ((_side-side).norm() > zeroEps && !lives--) + return false; + } + return true; +} diff --git a/libs/Common/Rotation.h b/libs/Common/Rotation.h new file mode 100644 index 0000000..ebc6b85 --- /dev/null +++ b/libs/Common/Rotation.h @@ -0,0 +1,595 @@ +//////////////////////////////////////////////////////////////////// +// Rotation.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_ROTATION_H__ +#define __SEACAVE_ROTATION_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + +#ifndef _RELEASE +#define _CPC_DEBUG +#endif + +#define CPC_ERROR(msg) { SLOG(msg); ASSERT(false); } +#if TD_VERBOSE != TD_VERBOSE_OFF +#define CPC_SLOG(msg) if (VERBOSITY_LEVEL > 0) SLOG(msg) +#define CPC_LOG(t, msg) CPC_SLOG(msg) +#else +#define CPC_SLOG(msg) +#define CPC_LOG(t, msg) +#endif + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// generic matrix struct +#define QUATERNION_EPSILON 1E-10 // DBL_EPSILON, too small (dherzog) + +// forward declaration to avoid mutual inclusion +template class TRMatrixBase; + + /** @class TQuaternion + * @test tested with TestRotationConversion.cpp + @ingroup g_geometry + @brief class for rotation with axis and angle + + Quaternions can be used for rotation around an axis through the + origin by spcecifing the normal direction vector of the axis and + an angle. + + The rotation for positive angles is clockwise, + when looking in direction of the axis. + + Contains qx=[0], qy=[1], qz=[2], qw=[3]. + + Not every quaternion describes a rotation. + Only quaternions of the form: + + qx = x * sin(phi/2) , which is the imaginary part i + + qy = y * sin(phi/2) , which is the imaginary part j + + qz = z * sin(phi/2) , which is the imaginary part k + + qw = cos(phi/2), , which is the real part + + where (x, y, z) is the normalized direction vector of the axis + and phi is the angle of rtoation. + + Some useful quaternions: + x y z w Description + 0 0 0 1 Identity quaternion, + no rotation + 1 0 0 0 180' turn around X axis + 0 1 0 0 180' turn around Y axis + 0 0 1 0 180' turn around Z axis + sqrt(0.5) 0 0 sqrt(0.5) 90' rotation around X axis + 0 sqrt(0.5) 0 sqrt(0.5) 90' rotation around Y axis + 0 0 sqrt(0.5) sqrt(0.5) 90' rotation around Z axis + -sqrt(0.5) 0 0 sqrt(0.5) -90' rotation around X axis + 0 -sqrt(0.5) 0 sqrt(0.5) -90' rotation around Y axis + 0 0 -sqrt(0.5) sqrt(0.5) -90' rotation around Z axis + + @author grest 06/2003 + */ +template +class TQuaternion : public TMatrix +{ +public: + typedef TMatrix Base; + typedef TMatrix Mat; + typedef TMatrix Vec; + typedef TMatrix Rot; + + using Base::val; + +public: + static const TQuaternion IDENTITY; + +public: + inline ~TQuaternion(); + + inline TQuaternion(); + + inline TQuaternion(const Base& q); + + inline TQuaternion(TYPE i, TYPE j, TYPE k, TYPE r); + + inline TQuaternion(const Rot& R); + + inline void SetIdentity(); + + /** + * Sets all parts of quaternion in mathematical order, + * real part first, then 1.,2. and 3. imaginary part + */ + inline void SetQuaternion(TYPE real, TYPE i, TYPE j, TYPE k); + + /** returns the Inverse rotation TQuaternion + */ + inline TQuaternion Inverse() const; + + /** inverts this, by changing the rotation axis by *=-1 + */ + inline void Invert(); + + /** makes TQuaternion-representation unique, ensuring vector lies + in upper (by real part) hemisphere. (inplace) + */ + inline void MakeUnique(); + + /** quaternion multiplication: this= this * quat + */ + inline void Mult(const TQuaternion &quat); + + /** quaternion multiplication: res = this * arg + */ + inline void Mult(const TQuaternion &arg, + TQuaternion &res) const; + + /** quaternion multiplication: this = quat * this + */ + inline void MultLeft(const TQuaternion &quat); + + /** rotates the given Vector with the quaternion ( q v q* ) + the resulting vector is given in res + @returns 0 in case of no error + @author Daniel Grest, June 2003 + */ + inline int MultVec(const Vec &vec, Vec &res) const; + + /* rets result from MultVec() + @author herzog 2005-07-15 */ + inline Vec MultVec(const Vec &vec) const; + + /** Computes this^(scale), which scales the angle from axis/angle- + representation with 'scale'. This is the same like inter-/extra- + polating between (0,0,0,1)-quaternion and this! + */ + TQuaternion Power(const TYPE & scale) const; + + /** Linear interpolation between this and given quaternion. + @note Quaternions are assumed to be unit quaternions! */ + TQuaternion InterpolateLinear(const TQuaternion &to, const TYPE & t) const; + + /** Spherical interpolation between this and given quaternion. + @note Quaternions are assumed to be unit quaternions! */ + TQuaternion Interpolate(const TQuaternion &to, const TYPE & t) const; + + /** sets the quaternion with given rotation axis and angle (in rad) + @returns 0 in case of no error + @author Daniel Grest, June 2003 + */ + inline void SetValueAsAxisRad(const Vec &axis, TYPE angle); + + /** sets the quaternion with given rotation axis and angle (in rad) + @returns 0 in case of no error + @author Daniel Grest, June 2003 + */ + + inline void SetValueAsAxisRad(TYPE axisX, TYPE axisY, TYPE axisZ, TYPE angle); + + /** Returns matrix which expresses (left) quaternion multiplication. + * A quaternions stored in an object can be understood as a 4-vector + * q =(ix iy iz r)^T. + * Left multiplying a quaternion q2 with a quaternion q1 can be + * expressed in terms of matrix multiplication as + * qRes = q1*q2 = M(q1)*q2. + * This method returns the matrix M(*this). + */ + Mat GetQuaternionMultMatrixLeft() const; + + /** Returns matrix which expresses right quaternion multiplication. + * Right multiplying a quaternion q1 with quaternion q2 can be + * expressed as qRes = q1*q2 = N(q2)*q1. + * This method returns the matrix N(*this). + */ + Mat GetQuaternionMultMatrixRight() const; + + /** Returns rotation axis. See GetRotationAxisAngle(). */ + Vec GetRotationAxis() const; + + /** Returns rotation angle notation in radians. + Note that the returned angle resides in the interval [0,PI)! */ + TYPE GetRotationAngle() const; + + /** Returns rotation in axis and angle notation (angle in radians). + Note that the returned angle resides in the interval [0,PI) and + the axis points into the according direction! + @author woelk 12 2002 */ + int GetAxisAngle(Vec& axis, TYPE& angle) const; + + /** Sets quaternion as concatenated rotations around + x,y,z-axis; this = q_x * q_y * q_z (BIAS-RMatrix conform) + @author herzog 2005-07-19 */ + int SetXYZ(TYPE radX, TYPE radY, TYPE radZ); + + /** Sets quaternion as concatenated rotations around + x,y,z-axis; this = q_z * q_y * q_x (BIAS-RMatrix conform) + @author herzog 2005-07-19 */ + int SetZYX(TYPE radX, TYPE radY, TYPE radZ); + + + /** Scales quaternion to unit length, i.e. norm equals 1. */ + inline void Normalize() { + const TYPE n = norm(*this); + if (ISZERO(n)) + *this = IDENTITY; + else + *this *= TYPE(1) / n; + } + inline bool IsNormalized() const { + return ISEQUAL(norm(*this), TYPE(1)); + } + + /** assignment operator + @author grest 06 2003 */ + TQuaternion& operator=(const TQuaternion& vec); + + /** Enforces rigid coupling constraint for this and the other given unit + quaternion (i.e. equal rotation angle resp. scalar part of both). + Computes direct simple interpolation of both unit quaternion where + the scalar parts of the resulting unit quaternion are identical. + This solution can be used as initial guess for numerical refinement. + @author esquivel 02/2012 */ + void EnforceRigidCouplingConstraint(TQuaternion &other); + + /** Enforces rigid coupling constraint for all quaternion in given vector. + @author esquivel 02/2012 */ + static void EnforceRigidCouplingConstraint(std::vector< TQuaternion > &quats); +}; // class +template const TQuaternion TQuaternion::IDENTITY(0,0,0,1); +/*----------------------------------------------------------------*/ +typedef TQuaternion QuaternionF; +typedef TQuaternion QuaternionD; +/*----------------------------------------------------------------*/ + + +// generic rotation matrix struct +#define ROTATION_MATRIX_EPSILON DBL_EPSILON + +/** @class TRMatrixBase + @ingroup g_geometry + @brief 3D rotation matrix + @author frahm, woelk **/ +template +class TRMatrixBase : public TMatrix +{ +public: + typedef TMatrix Base; + typedef Base Mat; + typedef TMatrix Vec; + typedef TQuaternion Quat; + typedef typename Base::Base BaseBase; + + using Base::val; + +public: + /** @brief Constructor setting matrix to identity */ + inline TRMatrixBase(); + inline TRMatrixBase(const BaseBase& rhs) : Base(rhs) {} + template inline TRMatrixBase(const cv::Matx& rhs) : Base(rhs) {} + + /** @brief Copy constructor from 3x3 matrix + @attention Orthonormality of matrix is enforced automatically! */ + inline TRMatrixBase(const Mat& mat); + + /** @brief Initialization from parametrized rotation (axis-angle) */ + inline TRMatrixBase(const Vec& rot); + + /** @brief Initialization from rotation axis w and angle phi (in rad) using Rodrigues' formula */ + inline TRMatrixBase(const Vec& w, const TYPE phi); + + /** @brief Initialization from quaternion */ + inline TRMatrixBase(const Quat& q); + + /** @brief Initialization with the rotation from roll/pitch/yaw (in rad) */ + inline TRMatrixBase(TYPE roll, TYPE pitch, TYPE yaw); + + /** @brief Initialization with the rotation from direction dir0 to direction dir1 */ + inline TRMatrixBase(const Vec& dir0, const Vec& dir1); + + template inline TRMatrixBase& operator = (const cv::Matx& rhs) { BaseBase::operator = (rhs); return *this; } + inline TRMatrixBase& operator = (const cv::Mat& rhs) { BaseBase::operator = (rhs); return *this; } + #ifdef _USE_EIGEN + inline TRMatrixBase& operator = (const typename Base::EMat& rhs) { Base::operator = (rhs); return *this; } + #endif + + /** @brief Set Euler angles (in rad) in order XYZ + + Set angles either in order 1. z, 2. y, 3. x and fixed axes + or in order 1. x, 2. y, 3. z and moving axes. + Angles are measured as in mathematics (counter-clockwise), i.e. + Set(x, 0, 0) results in the following matrix: + + | 1 0 0 | + | 0 cos(x) -sin(x) | + | 0 sin(x) cos(x) | + + (*this) = Rx*Ry*Rz = + + | c(y)c(z) -c(y)s(z) s(y) | + | c(z)s(x)s(y)+c(x)s(z) c(x)c(z)-s(x)s(y)s(z) -c(y)s(x) | + | -c(x)c(z)s(y)+s(x)s(z) c(z)s(x)+c(x)s(y)s(z) c(x)c(y) | + + with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ + + @author frahm, woelk */ + void SetXYZ(TYPE PhiX, TYPE PhiY, TYPE PhiZ); + + + /** @brief Set Euler angles (in rad) in order XYZ from vector */ + inline void SetXYZ(const Vec& r) + { SetXYZ(r[0], r[1], r[2]); } + + /** @brief Set Euler angles (in rad) in order ZYX + + Set angles either in order 1. z, 2. y, 3. x and moving axes + or in order 1. x, 2. y, 3. z and fixed axes. + Angles are measured as in mathematics (counter-clockwise), i.e. + Set(x, 0, 0) results in the following matrix: + + | 1 0 0 | + | 0 cos(x) -sin(x) | + | 0 sin(x) cos(x) | + + (*this) = Rz*Ry*Rx = + + | c(y)c(z) s(x)s(y)c(z)-c(x)s(z) c(x)s(y)c(z)+s(x)s(z) | + | c(y)s(z) s(x)s(y)s(z)+c(x)c(z) c(x)s(y)s(z)-s(x)s(z) | + | -s(y) s(x)c(y) c(x)c(y) | + + with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ + + @author frahm, woelk */ + void SetZYX(TYPE PhiX, TYPE PhiY, TYPE PhiZ); + + /** @brief Set Euler angles (in rad) in order ZYX from vector */ + inline void SetZYX(const Vec& r) + { SetZYX(r[0], r[1], r[2]); } + + /** @brief Set Euler angles (in rad) in order ZXY + + Set angles either in order 1. z, 2. x, 3. y and moving axes + or in order 1. y, 2. x, 3. z and fixed axes. + Angles are measured as in mathematics (counter-clockwise), i.e. + Set(x, 0, 0) results in the following matrix: + + | 1 0 0 | + | 0 cos(x) -sin(x) | + | 0 sin(x) cos(x) | + + (*this) = Rz*Rx*Ry = + + | c(y)c(z)-s(x)s(y)s(z), c(x)s(z), s(y)c(z)+s(x)c(y)s(z) | + | s(z)c(y)+s(x)s(y)c(z), c(x)c(z), s(y)*s(z)+s(x)-c(y)c(z) | + | -c(x)s(y) , s(x) , c(x)c(y) | + + with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ + + @author haase */ + void SetZXY(TYPE PhiX, TYPE PhiY, TYPE PhiZ); + + /** @brief Set Euler angles (in rad) in order ZXY from vector */ + inline void SetZXY(const Vec& r) + { SetZXY(r[0], r[1], r[2]); } + + /** @brief Set Euler angles (in rad) in order YXZ + + Set angles either in order 1. y, 2. x, 3. z and moving axes + or in order 1. z, 2. x, 3. y and fixed axes. + Angles are measured as in mathematics (counter-clockwise), i.e. + Set(x, 0, 0) results in the following matrix: + + | 1 0 0 | + | 0 cos(x) sin(x) | + | 0 -sin(x) cos(x) | + + (*this) = Rz*Rx*Ry = + + | c(y)c(z)+s(x)s(y)s(z), c(x)s(z), -s(y)c(z)+s(x)c(y)s(z) | + | -s(z)c(y)+s(x)s(y)c(z), c(x)c(z), -s(y)*-s(z)+s(x)c(y)c(z) | + | c(x)s(y) , -s(x) , c(x)c(y) | + + with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ + + @author haase */ + void SetYXZ(TYPE PhiZ, TYPE PhiX, TYPE PhiY); + + /** @brief Set Euler angles (in rad) in order YXZ from vector */ + inline void SetYXZ(const Vec& r) + { SetYXZ(r[0], r[1], r[2]); } + + /** @brief Set from rotation axis w and angle phi (in rad) + @param w Axis vector w will be normalized to length 1, so we need + |w|>1e-6 if phi != 0, otherwise an exception is thrown + @param phi Rotation angle is given in radians + @author evers, woelk */ + void Set(const Vec& w, TYPE phi); + + /** set this matrix from 3 vectors each representing a column*/ + void SetFromColumnVectors(const Vec& v0, + const Vec& v1, + const Vec& v2); + + /** set this matrix from 3 vectors, each representing a row */ + void SetFromRowVectors(const Vec& v0, + const Vec& v1, + const Vec& v2); + + /** @brief Set from rotation axis * angle (modified Rodrigues vector) + @author evers */ + void SetFromAxisAngle(const Vec& w); + + /* @brief Set rotation matrix from an orthogonal basis given in world + coordinate system (WCS) + + As R' is expressing the transformation from WCS to ICS (image + coordinate system), R is ICS to WCS and hereby represents the ICS + base vectors expressed in WCS. These are the *rows* of R because + the transformation is the scalar product. + + Assume xxx,yyy,zzz are right-hand orthogonal (RHS), e.g. the orthogonal + image coordinate system (ICS) base vectors expressed in world + coordinates (WCS). + (Inverse doesn't make sense because base would be identity then). + Normal vectors of length 1 are computed. + + @todo Warning if (xxx,yyy,zzz) are not an orthogonal basis! + + @author jw 09/2003, added exception */ + void SetFromOrthogonalBasis(const Vec &xxx, const Vec &yyy, const Vec &zzz); + + /** @brief Set rotation matrix from two vectors from an orthonormal basis + @param xh represents the first base vector and is left unchanged + @param vy should be orthogonal to xh, it is orthogonalized otherwise + + You can think of this routine as computing R from an image plane + given by two (usually orthogonal) base vectors. + If the given base vectors are not orthogonal, xh is kept and yv is + orthogonalized appropriately. + + @author jw */ + void SetFromHV(const Vec& xh, const Vec& vy); + + /** @brief Create rotation matrix that rotates from dir0 to dir1 + @param dir0 represents the first (reference) direction vector + @param dir1 represents the second (target) direction vector + @author cDc */ + TRMatrixBase& SetFromDir2Dir(const Vec& dir0, const Vec& dir1); + + /** @brief Calculates quaternion representation for this rotation matrix + @attention Scalar part of quaternion will always be non-negative + for sake of uniqueness of the resulting quaternion! + @author woelk 12/2003 + @author esquivel 03/2011 (changed algorithm, bugfix) */ + void GetQuaternion(Quat& q) const; + inline Quat GetQuaternion() const { Quat q; GetQuaternion(q); return q; } + + /** @brief Set rotation matrix from a quaternion + @author grest 06/2003 */ + void SetFromQuaternion(const Quat& q); + + /** @brief Set rotation matrix from direction and up vector + @note This is openGL conform, similar to gluLookAt(), i.e. if + direction is (0,0,-1) and up is (0,1,0) the resulting matrix is identity. + @author grest 12/2005 */ + void SetFromDirUpGL(const Vec& viewDir, const Vec& viewUp); + + /** @brief Set rotation matrix from direction and up vector */ + void SetFromDirUp(const Vec& viewDir, const Vec& viewUp); + + /** @brief Set rotation matrix from an eye point and a look-at target point and the up vector */ + void LookAt(const Vec& from, const Vec& to, const Vec& up); + + // get parametrized rotation (axis-angle) from the rotation matrix + inline void SetRotationAxisAngle(const Vec& rot); + + // modify the rotation matrix by the given parametrized delta rotation (axis-angle) + inline void Apply(const Vec& delta); + + // set rotation matrix to the given parametrized rotation (axis-angle) + inline Vec GetRotationAxisAngle() const; + + /** @brief Calculates angle and rotation axis representation for + this rotation matrix + @param angle Rotation angle is returned in radians + @author woelk 12/2003 */ + void GetRotationAxisAngle(Vec& axis, TYPE& angle) const; + + /** @brief Interface for axis component of GetRotationAxisAngle() */ + Vec GetRotationAxis() const; + + /** @brief Interface for angle component of GetRotationAxisAngle() */ + TYPE GetRotationAngle() const; + + /** @brief Get Euler angles for this rotation matrix in order XYZ + @attention Representation is not unique and has singularities at + +-pi/2. Assume for example (*this) = Rx * Ry * Rz, then we have + either Euler angles in order 1. x, 2. y, 3. z and moving axes + or in order 1. z, 2. y, 3. x and fixed axes. + @author woelk 01/2003 */ + int GetRotationAnglesXYZ(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const; + + /** @brief Get Euler angles for this rotation matrix in order XYZ + @see GetRotationAnglesXYZ(TYPE&, TYPE&, TYPE&) */ + inline int GetRotationAnglesXYZ(Vec& r) const + { return GetRotationAnglesXYZ(r[0], r[1], r[2]); } + + /** @brief Get Euler angles for this rotation matrix in order YZX + @attention Representation is not unique and has singularities at + +-pi/2. Assume for example (*this) = Rz * Ry * Rx, then we have + either Euler angles in order 1. x, 2. y, 3. z and fixed axes + or in order 1. z, 2. y, 3. x and moving axes. + @author woelk 01 2003 */ + int GetRotationAnglesZYX(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const; + + /** @brief Get Euler angles for this rotation matrix in order ZYX + @see GetRotationAnglesZYX(TYPE&, TYPE&, TYPE&) */ + inline int GetRotationAnglesZYX(Vec& r) const + { return GetRotationAnglesZYX(r[0], r[1], r[2]); } + + /** @brief Get Euler angles for this rotation matrix in order ZXY + @attention Representation is not unique and has singularities at + +-pi/2. Rotation order ZXY refers to rotation around point/vector! + @author haase 2007 */ + int GetRotationAnglesZXY(TYPE& PhiZ, TYPE& PhiX, TYPE& PhiY) const; + + /** @brief Get Euler angles for this rotation matrix in order ZXY + @see GetRotationAnglesZXY(TYPE&, TYPE&, TYPE&) */ + inline int GetRotationAnglesZXY(Vec& r) const + { return GetRotationAnglesZXY(r[2], r[0], r[1]); } + + /** @brief Get Euler angles for this rotation matrix in order YXZ + @attention Representation is not unique and has singularities at + +-pi/2. Rotation order YXZ refers to rotation with moving axes! + @author haase 2007 */ + int GetRotationAnglesYXZ(TYPE& PhiY, TYPE& PhiX, TYPE& PhiZ) const; + + /** @brief Get Euler angles for this rotation matrix in order YXZ + @see GetRotationAnglesYXZ(TYPE&, TYPE&, TYPE&) */ + inline int GetRotationAnglesYXZ(Vec& r) const + { return GetRotationAnglesYXZ(r[1], r[0], r[2]); } + + /** @brief Check that the matrix is a valid rotation matrix (orthogonal) */ + inline bool IsValid() const { + // the trace should be three and the determinant should be one + #if 1 + return (ISEQUAL((float)cv::determinant(*this), 1.f) && ISEQUAL((float)cv::trace((*this)*(*this).t()), 3.f)); + #else + return (ISEQUAL((TYPE)cv::determinant(*this), TYPE(1)) && ISEQUAL((TYPE)cv::trace((*this)*(*this).t()), TYPE(3))); + #endif + } + + /** @brief Check if this is a rotation matrix, i.e. if the determinant + is +1 and the columns are orthonormal + @param eps Numerical limit for constraint evaluation + @param verbose Show reason in case of failure */ + bool Check(const TYPE eps = std::numeric_limits::epsilon(), + int verbose = 0) const; + + /** @brief Enforce orthogonality constraint on rotation matrix and + sets determinant to +1 */ + void EnforceOrthogonality(); +}; // class +/*----------------------------------------------------------------*/ +typedef TRMatrixBase RMatrixBaseF; +typedef TRMatrixBase RMatrixBaseD; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#include "Rotation.inl" + +#endif // __SEACAVE_ROTATION_H__ + diff --git a/libs/Common/Rotation.inl b/libs/Common/Rotation.inl new file mode 100644 index 0000000..81b2d00 --- /dev/null +++ b/libs/Common/Rotation.inl @@ -0,0 +1,1563 @@ +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + + +// G L O B A L S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// C L A S S ////////////////////////////////////////////////////// + +template +inline TQuaternion::~TQuaternion() +{} + +template +inline TQuaternion::TQuaternion() + : Base() +{} + +template +inline TQuaternion::TQuaternion(const Base& q) + : Base(q) +{} + +template +inline TQuaternion::TQuaternion(TYPE i, TYPE j, TYPE k, TYPE r) + : Base(cv::Vec(i, j, k, r)) +{} + +template +inline TQuaternion::TQuaternion(const Rot& R) + : Base(((const TRMatrixBase&)R).GetQuaternion()) +{} + + +template +inline void TQuaternion::Invert() +{ + Base::operator()(0) *=-1; + Base::operator()(1) *=-1; + Base::operator()(2) *=-1; +} + +template +inline TQuaternion TQuaternion::Inverse() const +{ + TQuaternion res(*this); + res.Invert(); + return res; +} + +template +inline void TQuaternion::MakeUnique() +{ + //project quaternion to upper hemisphere (first ima.part >= 0) + TYPE *qThis = this->val; + if ((qThis[3] <= QUATERNION_EPSILON) + &&(qThis[3] >= -QUATERNION_EPSILON)) { + if ((qThis[1] <= QUATERNION_EPSILON) + &&(qThis[1] >= -QUATERNION_EPSILON)) { + if ((qThis[2] <= QUATERNION_EPSILON) + &&(qThis[2] >= -QUATERNION_EPSILON)) { + if (qThis[0] < 0.0) (*this) *= -1.0; + } else if (qThis[2] < 0.0) { + (*this) *= -1.0; + } + } else if (qThis[1] < 0.0) { + (*this) *= -1.0; + } + } else if (qThis[3] < 0.0) { + (*this) *= -1.0; + } +} + +template +inline void TQuaternion::Mult(const TQuaternion &quat) +{ + TYPE s1, s2, s3, s4, s5, s6, s7, s8, s9, t; + TYPE *qThis = this->val; + + s1 = (qThis[2] - qThis[1]) * (quat[1] - quat[2]); + s2 = (qThis[3] + qThis[0]) * (quat[3] + quat[0]); + s3 = (qThis[3] - qThis[0]) * (quat[1] + quat[2]); + s4 = (qThis[2] + qThis[1]) * (quat[3] - quat[0]); + s5 = (qThis[2] - qThis[0]) * (quat[0] - quat[1]); + s6 = (qThis[2] + qThis[0]) * (quat[0] + quat[1]); + s7 = (qThis[3] + qThis[1]) * (quat[3] - quat[2]); + s8 = (qThis[3] - qThis[1]) * (quat[3] + quat[2]); + + s9 = s6 + s7 + s8; + + t = (s5 + s9) / 2.0f; + + qThis[3] = s1 + t - s6; + qThis[0] = s2 + t - s9; + qThis[1] = s3 + t - s8; + qThis[2] = s4 + t - s7; +} + +template +inline void TQuaternion::Mult(const TQuaternion &arg, TQuaternion &res) const +{ + res = *this; + res.Mult(arg); +} + +template +inline void TQuaternion::MultLeft(const TQuaternion &quatLeft) +{ + TYPE s1, s2, s3, s4, s5, s6, s7, s8, s9, t; + TYPE *qThis = this->val; + + s1 = (quatLeft[2] - quatLeft[1]) * (qThis[1] - qThis[2]); + s2 = (quatLeft[3] + quatLeft[0]) * (qThis[3] + qThis[0]); + s3 = (quatLeft[3] - quatLeft[0]) * (qThis[1] + qThis[2]); + s4 = (quatLeft[2] + quatLeft[1]) * (qThis[3] - qThis[0]); + s5 = (quatLeft[2] - quatLeft[0]) * (qThis[0] - qThis[1]); + s6 = (quatLeft[2] + quatLeft[0]) * (qThis[0] + qThis[1]); + s7 = (quatLeft[3] + quatLeft[1]) * (qThis[3] - qThis[2]); + s8 = (quatLeft[3] - quatLeft[1]) * (qThis[3] + qThis[2]); + + s9 = s6 + s7 + s8; + + t = (s5 + s9) / 2.0f; + + qThis[3] = s1 + t - s6; + qThis[0] = s2 + t - s9; + qThis[1] = s3 + t - s8; + qThis[2] = s4 + t - s7; +} + +/** rotates the given Vector with the quaternion ( q v q* ) +the resulting vector is given in res +@returns 0 in case of no error +@author Daniel Grest, June 2003 +*/ +template +inline int TQuaternion::MultVec(const Vec &vec, Vec &res) const { + TYPE rx,ry,rz; + TYPE Vx= vec[0], Vy=vec[1], Vz=vec[2]; + const TYPE *q = this->val; + { + TYPE QwQx, QwQy, QwQz, QxQy, QxQz, QyQz; + QwQx = q[3] * q[0]; QwQy = q[3] * q[1]; QwQz = q[3] * q[2]; + QxQy = q[0] * q[1]; QxQz = q[0] * q[2]; QyQz = q[1] * q[2]; + rx = 2* (Vy * (-QwQz + QxQy) + Vz *( QwQy + QxQz)); + ry = 2* (Vx * ( QwQz + QxQy) + Vz *(-QwQx + QyQz)); + rz = 2* (Vx * (-QwQy + QxQz) + Vy *( QwQx + QyQz)); + } + TYPE QwQw, QxQx, QyQy, QzQz; + QwQw= q[3]*q[3]; QxQx = q[0]*q[0]; QyQy = q[1]*q[1]; QzQz= q[2]*q[2]; + rx+= Vx * (QwQw + QxQx - QyQy - QzQz); + ry+= Vy * (QwQw - QxQx + QyQy - QzQz); + rz+= Vz * (QwQw - QxQx - QyQy + QzQz); + res[0]=rx; res[1]=ry; res[2]=rz; + return 0; +} + +template +inline typename TQuaternion::Vec TQuaternion::MultVec(const Vec &vec) const +{ + Vec r; + MultVec(vec,r); + return r; +} + +template +inline void TQuaternion::SetIdentity() +{ + Base::operator()(0) = Base::operator()(1) = Base::operator()(2) = 0; + Base::operator()(3) = 1; +} + +template +inline void TQuaternion::SetQuaternion(TYPE real, TYPE i, + TYPE j, TYPE k) +{ + //Components are stored in Vec4 in order imaginary part 1,2,3 and real part + this->Set(i,j,k,real); +} + +template +inline void TQuaternion::SetValueAsAxisRad(const Vec &axis, TYPE angle) +{ + SetValueAsAxisRad(axis[0], axis[1], axis[2], angle); +} + +template +inline void TQuaternion::SetValueAsAxisRad(TYPE x, TYPE y, TYPE z, TYPE w) +{ + TYPE rTmp = TYPE(sqrt(x * x + y * y + z * z)); + + if(rTmp > QUATERNION_EPSILON) + { + rTmp = TYPE(sin(w / 2.0f)) / rTmp; + + Base::operator()(0) = x * rTmp; + Base::operator()(1) = y * rTmp; + Base::operator()(2) = z * rTmp; + Base::operator()(3) = TYPE(cos(w / 2.0f)); + } + else + { + SetIdentity(); + } +} + +template +typename TQuaternion::Mat TQuaternion::GetQuaternionMultMatrixLeft() const +{ + Mat res; + + res(0,0) = (*this)[3]; + res(0,1) = -(*this)[2]; + res(0,2) = (*this)[1]; + res(0,3) = (*this)[0]; + + res(1,0) = (*this)[2]; + res(1,1) = (*this)[3]; + res(1,2) = -(*this)[0]; + res(1,3) = (*this)[1]; + + res(2,0) = -(*this)[1]; + res(2,1) = (*this)[0]; + res(2,2) = (*this)[3]; + res(2,3) = (*this)[2]; + + res(3,0) = -(*this)[0]; + res(3,1) = -(*this)[1]; + res(3,2) = -(*this)[2]; + res(3,3) = (*this)[3]; + + return res; +} + +template +typename TQuaternion::Mat TQuaternion::GetQuaternionMultMatrixRight() const +{ + Mat res; + + res(0,0) = (*this)[3]; + res(0,1) = (*this)[2]; + res(0,2) = -(*this)[1]; + res(0,3) = (*this)[0]; + + res(1,0) = -(*this)[2]; + res(1,1) = (*this)[3]; + res(1,2) = (*this)[0]; + res(1,3) = (*this)[1]; + + res(2,0) = (*this)[1]; + res(2,1) = -(*this)[0]; + res(2,2) = (*this)[3]; + res(2,3) = (*this)[2]; + + res(3,0) = -(*this)[0]; + res(3,1) = -(*this)[1]; + res(3,2) = -(*this)[2]; + res(3,3) = (*this)[3]; + + return res; +} + +template +int TQuaternion::GetAxisAngle(Vec& axis, TYPE& angle) const +{ + // nicked from OpenSG + int res=0; + Vec q(cv::Vec((*this)[0], (*this)[1], (*this)[2])); + TYPE len = TYPE(norm(q)); + + if(len > QUATERNION_EPSILON) { + q *= TYPE(1.0) / len; + axis[0] = q[0]; + axis[1] = q[1]; + axis[2] = q[2]; + angle = TYPE(2.0 * acos((*this)[3])); + } else { + axis[0] = 0.0; + axis[1] = 0.0; + axis[2] = 1.0; + angle = 0.0; + } + return res; +} + +template +typename TQuaternion::Vec TQuaternion::GetRotationAxis() const +{ + Vec r(cv::Vec((*this)[0], (*this)[1], (*this)[2])); + TYPE len = TYPE(norm(r)); + if (len > QUATERNION_EPSILON) + return r * (1.0 / len); + else + return Vec(cv::Vec(0,0,1)); +} + +template +TYPE TQuaternion::GetRotationAngle() const +{ + const TYPE angle = TYPE(2 * acos((*this)[3])); + return std::isnan(angle) && ABS(ABS((*this)[3]) - TYPE(1)) < QUATERNION_EPSILON ? + TYPE(0) : + angle; +} + +template +TQuaternion TQuaternion::Power(const TYPE & scale) const +{ + TQuaternion res = *this; + if ((scale < 1e-7) && (scale > -1e-7)) { + //scale near zero -> return identity + res.SetIdentity(); + } else if ((res[3] > 0.999999) || (res[3] < -0.999999)) { + //this is near identity (numerical unstable) -> return identity + //res = *this + } else { + //else use slerp interpolation between identity and this with scale + TYPE temp1 = sin(scale * acos(res[3]))/sin(acos(res[3])); + TYPE res3 = res[3]; + res *= temp1; + res[3] += TYPE(sin((1.0-scale) * acos(res3))/sin(acos(res3))); + } + return res; +} + +template +TQuaternion TQuaternion::InterpolateLinear(const TQuaternion &to, const TYPE & t) const +{ + BIASASSERT(t >= 0.0 && t <= 1.0); + TQuaternion p = *this; + TQuaternion q = to; + //p.Normalize(); + //q.Normalize(); + if (t < 1e-7) { + //t near zero -> return p + return p; + } else if (1.0-t < 1e-7) { + //t near one -> return q + return q; + } + //else use linear interpolation between this and q + TQuaternion res; + const TYPE cosPhi = (TYPE)p.ScalarProduct(q); + const TYPE a = (TYPE)1.0 - t; + const TYPE b = (cosPhi < 0 ? -t : t); + res[0] = a*p[0] + b*q[0]; + res[1] = a*p[1] + b*q[1]; + res[2] = a*p[2] + b*q[2]; + res[3] = a*p[3] + b*q[3]; + res.Normalize(); + return res; +} + +template +TQuaternion TQuaternion:: + Interpolate(const TQuaternion &to, const TYPE & t) const +{ + BIASASSERT(t >= 0.0 && t <= 1.0); + TQuaternion p = *this; + TQuaternion q = to; + //p.Normalize(); + //q.Normalize(); + if (t < 1e-7) { + //t near zero -> return p + return p; + } else if (1.0-t < 1e-7) { + //t near one -> return q + return q; + } + TQuaternion res; + TYPE a, b; + TYPE cosPhi = (TYPE)p.ScalarProduct(q); + //adjust angle if necessary + if (cosPhi < 0) { + q.MultiplyIP(-1.0); + cosPhi = -cosPhi; + } + if (cosPhi > 0.9999 || cosPhi < -0.9999) { + // p is very close to q, do linear interpolation instead of slerp + a = (TYPE)1.0 - t; + b = (cosPhi < 0 ? -t : t); + } else { + //else use slerp interpolation between p and q + const TYPE phi = acos(cosPhi < 0 ? -cosPhi : cosPhi); + const TYPE sinPhi = sin(phi); + a = sin(((TYPE)1.0 - t) * phi) / sinPhi; + b = sin(t * phi) / sinPhi; + if (cosPhi < 0) b = -b; + } + res[0] = a*p[0] + b*q[0]; + res[1] = a*p[1] + b*q[1]; + res[2] = a*p[2] + b*q[2]; + res[3] = a*p[3] + b*q[3]; + res.Normalize(); + return res; +} + +template +int TQuaternion::SetXYZ(TYPE radX, TYPE radY, TYPE radZ) +{ + TQuaternion q_x; + TQuaternion q_y; + TQuaternion q_z; + + q_x.SetValueAsAxisRad( 1.,0.,0., radX ); + q_y.SetValueAsAxisRad( 0.,1.,0., radY ); + q_z.SetValueAsAxisRad( 0.,0.,1., radZ ); + + q_x.Mult(q_y); + q_x.Mult(q_z); + + *this = q_x; + + return 0; +} + +template +int TQuaternion::SetZYX(TYPE radX, TYPE radY, TYPE radZ) +{ + TQuaternion q_x; + TQuaternion q_y; + TQuaternion q_z; + + q_x.SetValueAsAxisRad( 1.,0.,0., radX ); + q_y.SetValueAsAxisRad( 0.,1.,0., radY ); + q_z.SetValueAsAxisRad( 0.,0.,1., radZ ); + + q_z.Mult(q_y); + q_z.Mult(q_x); + + *this = q_z; + + return 0; +} + +template +TQuaternion& TQuaternion::operator=(const TQuaternion& vec) +{ + memcpy((void *)val, (void *)vec.val, sizeof(TQuaternion)); + return *this; +} + +template +void TQuaternion::EnforceRigidCouplingConstraint(TQuaternion &other) +{ + // @todo check unit norm constraint of this and other quaternion!! + + // make this and other quaternion unique + MakeUnique(); + other.MakeUnique(); + + // get aliases for both quaternions + TQuaternion &q0 = *this, &q1 = other; + + // interpolate equal angle + q0[3] = q1[3] = TYPE(0.5) * (q0[3] + q1[3]); + + // re-enforce unit length constraint for q0 and q1 + TYPE q0norm = q0[0]*q0[0] + q0[1]*q0[1] + q0[2]*q0[2]; + TYPE q1norm = q1[0]*q1[0] + q1[1]*q1[1] + q1[2]*q1[2]; + if (q0norm > TYPE(1e-8) && q1norm > TYPE(1e-8)) { + TYPE qscale0 = (TYPE)sqrt(double((1-q0[3]*q0[3])/q0norm)); + TYPE qscale1 = (TYPE)sqrt(double((1-q1[3]*q1[3])/q1norm)); + for (int l = 0; l < 3; l++) { + q0[l] *= qscale0; + q1[l] *= qscale1; + } + } + + // @todo check unit norm constraint of resulting quaternions!! +} + +template +void TQuaternion::EnforceRigidCouplingConstraint(std::vector > &quats) +{ + // @todo check unit norm constraint of all quaternions!! + + // make all quaternions unique + const int n = quats.size(); + for (int i = 0; i < n; i++) + quats[i].MakeUnique(); + + // interpolate equal angle + double a = 0; + for (int i = 0; i < n; i++) + a += quats[i][3]; + a /= TYPE(n); + for (int i = 0; i < n; i++) + quats[i][3] = a; + + // re-enforce unit length constraint for all quaternions + for (int i = 0; i < n; i++) { + TQuaternion &q = quats[i]; + TYPE qnorm = q[0]*q[0] + q[1]*q[1] + q[2]*q[2]; + if (qnorm > TYPE(1e-8)) { + TYPE qscale = (TYPE)sqrt(double((1-q[3]*q[3])/qnorm)); + for (int l = 0; l < 3; l++) + q[l] *= qscale; + } + } + + // @todo check unit norm constraint of resulting quaternions!! +} +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +// accuracy for constraint check in Check() function calls +#define R_CONSTRAINT_ACCURACY 1e-5 //ZEROTOLERANCE() + +// if rotation angle is below this value, assume zero rotation +#define MINIMUM_ROTATION 1e-7 + +template +inline TRMatrixBase::TRMatrixBase() + : Base(Base::IDENTITY) +{} + +template +inline TRMatrixBase::TRMatrixBase(const Mat& mat) + : Base(mat) +{ + ASSERT(Check(R_CONSTRAINT_ACCURACY)); +} + +template +inline TRMatrixBase::TRMatrixBase(const Vec& w, TYPE phi) +{ + Set(w, phi); +} + +template +inline TRMatrixBase::TRMatrixBase(const Vec& rot) +{ + SetRotationAxisAngle(rot); +} + +template +inline TRMatrixBase::TRMatrixBase(const Quat& q) +{ + SetFromQuaternion(q); +} + +template +inline TRMatrixBase::TRMatrixBase(TYPE roll, TYPE pitch, TYPE yaw) +{ + SetXYZ(roll, pitch, yaw); +} +template +inline TRMatrixBase::TRMatrixBase(const Vec& dir0, const Vec& dir1) +{ + SetFromDir2Dir(dir0, dir1); +} + + +template +void TRMatrixBase::SetXYZ(TYPE PhiX, TYPE PhiY, TYPE PhiZ) +{ + #if 0 + TRMatrixBase Rx(TRMatrixBase::IDENTITY); + TRMatrixBase Ry(TRMatrixBase::IDENTITY); + TRMatrixBase Rz(TRMatrixBase::IDENTITY); + + /* set Rz, Ry, Rx as rotation matrices */ + Rz(0,0) = cos(PhiZ); + Rz(0,1) = -sin(PhiZ); + Rz(1,0) = -Rz(0,1); + Rz(1,1) = Rz(0,0); + //Rz(2,2) = 1; + + Ry(0,0) = cos(PhiY); + Ry(0,2) = sin(PhiY); + //Ry(1,1) = 1; + Ry(2,0) = -Ry(0,2); //-sin(PhiY); + Ry(2,2) = Ry(0,0); //cos(PhiY); + + //Rx(0,0) = 1; + Rx(1,1) = cos(PhiX); + Rx(1,2) = -sin(PhiX); + Rx(2,1) = -Rx(1,2); //sin(PhiX); + Rx(2,2) = Rx(1,1); //cos(PhiX); + + *this = Rx*Ry*Rz; + #else + const TYPE sin_x = sin(PhiX); + const TYPE sin_y = sin(PhiY); + const TYPE sin_z = sin(PhiZ); + const TYPE cos_x = cos(PhiX); + const TYPE cos_y = cos(PhiY); + const TYPE cos_z = cos(PhiZ); + val[0] = cos_y * cos_z; + val[1] = -cos_y * sin_z; + val[2] = sin_y; + val[3] = cos_x * sin_z + cos_z * sin_x * sin_y; + val[4] = cos_x * cos_z - sin_x * sin_y * sin_z; + val[5] = -cos_y * sin_x; + val[6] = sin_x * sin_z - cos_x * cos_z * sin_y; + val[7] = cos_z * sin_x + cos_x * sin_y * sin_z; + val[8] = cos_x * cos_y; + #endif +} + + +template +void TRMatrixBase::SetZYX(TYPE PhiX, TYPE PhiY, TYPE PhiZ) +{ + TRMatrixBase Rx(TRMatrixBase::IDENTITY); + TRMatrixBase Ry(TRMatrixBase::IDENTITY); + TRMatrixBase Rz(TRMatrixBase::IDENTITY); + + /* set Rz, Ry, Rx as rotation matrices */ + Rz(0,0) = cos(PhiZ); + Rz(0,1) = -sin(PhiZ); + Rz(1,0) = -Rz(0,1); + Rz(1,1) = Rz(0,0); + //Rz(2,2) = 1; + + Ry(0,0) = cos(PhiY); + Ry(0,2) = sin(PhiY); + //Ry(1,1) = 1; + Ry(2,0) = -Ry(0,2); //-sin(PhiY); + Ry(2,2) = Ry(0,0); //cos(PhiY); + + //Rx(0,0) = 1; + Rx(1,1) = cos(PhiX); + Rx(1,2) = -sin(PhiX); + Rx(2,1) = -Rx(1,2); //sin(PhiX); + Rx(2,2) = Rx(1,1); //cos(PhiX); + + *this = Rz*Ry*Rx; +} + + +template +void TRMatrixBase::SetYXZ(TYPE PhiY, TYPE PhiX, TYPE PhiZ) +{ + TRMatrixBase Rx(TRMatrixBase::IDENTITY); + TRMatrixBase Ry(TRMatrixBase::IDENTITY); + TRMatrixBase Rz(TRMatrixBase::IDENTITY); + + /* set Rz, Ry, Rx as rotation matrices */ + Rz(0,0) = cos(PhiZ); + Rz(0,1) = -sin(PhiZ); + Rz(1,0) = -Rz(0,1); + Rz(1,1) = Rz(0,0); + + + Ry(0,0) = cos(PhiY); + Ry(0,2) = sin(PhiY); + + Ry(2,0) = -Ry(0,2); //-sin(PhiY); + Ry(2,2) = Ry(0,0); //cos(PhiY); + + + Rx(1,1) = cos(PhiX); + Rx(1,2) = -sin(PhiX); + Rx(2,1) = -Rx(1,2); //sin(PhiX); + Rx(2,2) = Rx(1,1); //cos(PhiX); + + *this = Ry*Rx*Rz; +} + + +template +void TRMatrixBase::SetZXY(TYPE PhiX, TYPE PhiY, TYPE PhiZ) +{ + TRMatrixBase Rx(TRMatrixBase::IDENTITY); + TRMatrixBase Ry(TRMatrixBase::IDENTITY); + TRMatrixBase Rz(TRMatrixBase::IDENTITY); + + /* set Rz, Ry, Rx as rotation matrices */ + Rz(0,0) = cos(PhiZ); + Rz(0,1) = -sin(PhiZ); + Rz(1,0) = -Rz(0,1); + Rz(1,1) = Rz(0,0); + + + Ry(0,0) = cos(PhiY); + Ry(0,2) = sin(PhiY); + + Ry(2,0) = -Ry(0,2); //-sin(PhiY); + Ry(2,2) = Ry(0,0); //cos(PhiY); + + + Rx(1,1) = cos(PhiX); + Rx(1,2) = -sin(PhiX); + Rx(2,1) = -Rx(1,2); //sin(PhiX); + Rx(2,2) = Rx(1,1); //cos(PhiX); + + *this = Rz*Rx*Ry; +} + + + +template +void TRMatrixBase::Set(const Vec& wa, TYPE phi) +{ + // zero rotation results in identity matrix + if (ISZERO(phi)) { + *this = Base::IDENTITY; + return; + } + + const TYPE wnorm(norm(wa)); + if (wnorm < std::numeric_limits::epsilon()) { + CPC_ERROR("Vector "< +void TRMatrixBase::SetFromColumnVectors(const Vec& v0, const Vec& v1, const Vec& v2) +{ + val[0] = v0[0]; + val[1] = v1[0]; + val[2] = v2[0]; + val[3] = v0[1]; + val[4] = v1[1]; + val[5] = v2[1]; + val[6] = v0[2]; + val[7] = v1[2]; + val[8] = v2[2]; + ASSERT(Check(R_CONSTRAINT_ACCURACY)); +} + + +template +void TRMatrixBase::SetFromRowVectors(const Vec& v0, const Vec& v1, const Vec& v2) +{ + val[0] = v0[0]; + val[1] = v0[1]; + val[2] = v0[2]; + val[3] = v1[0]; + val[4] = v1[1]; + val[5] = v1[2]; + val[6] = v2[0]; + val[7] = v2[1]; + val[8] = v2[2]; + ASSERT(Check(R_CONSTRAINT_ACCURACY)); +} + + +template +void TRMatrixBase::SetFromAxisAngle(const Vec& w) +{ + #if 0 + const TYPE dAngle(norm(w)); + Set(w*INVERT(dAngle), dAngle); + #else + SetRotationAxisAngle(w); + #endif +} + + +template +void TRMatrixBase::GetQuaternion(Quat& quat) const +{ + ASSERT(Check(R_CONSTRAINT_ACCURACY)); + #if 1 + + // Conversion method from OpenSG + // @attention Removed old conversion in Linux build which had numerical + // issues for rotations close to 180 degree! (esquivel 03/2011) + + TYPE tr = Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2); + + if (tr > 0.0) + { + // Use default formula if trace is large enough + TYPE s = sqrt(tr + 1.0); + + quat[3] = s * 0.5; + + s *= 2.0; + quat[0] = (Base::operator()(2,1) - Base::operator()(1,2)) / s; + quat[1] = (Base::operator()(0,2) - Base::operator()(2,0)) / s; + quat[2] = (Base::operator()(1,0) - Base::operator()(0,1)) / s; + } + else + { + // Use alternate formula using largest trace element + TYPE s; + unsigned i; + unsigned j; + unsigned k; + + // Find largest trace element i and its successors j, k + if(Base::operator()(1,1) > Base::operator()(0,0)) + i = 1; + else + i = 0; + if(Base::operator()(2,2) > Base::operator()(i,i)) + i = 2; + + j = (i + 1) % 3; + k = (j + 1) % 3; + + // Compute discriminator using largest trace element + s = sqrt(Base::operator()(i,i) - (Base::operator()(j,j) + Base::operator()(k,k)) + 1.0); + + // Compute quaternion entries + quat[i] = s * 0.5; + s *= 2.0; + + quat[j] = (Base::operator()(i,j) + Base::operator()(j,i)) / s; + quat[k] = (Base::operator()(i,k) + Base::operator()(k,i)) / s; + + quat[3] = (Base::operator()(k,j) - Base::operator()(j,k) ) / s; + } + + #elif 1 + + // Conversion method using derivation depending on max. denominator + // @see http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86.94_quaternion + // @deprecated + + // Determine max. denominator + TYPE d[4] = { 1 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2), + 1 - Base::operator()(0,0) + Base::operator()(1,1) - Base::operator()(2,2), + 1 - Base::operator()(0,0) - Base::operator()(1,1) + Base::operator()(2,2), + 1 + Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2) }; + int maxidx = 0; + for (int i = 1; i < 4; i++) { + if (d[maxidx] < d[i]) maxidx = i; + } + + // Compute quaternion starting with max. denominator + if (maxidx == 0) { + quat[0]=0.5*sqrt(1 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2)); + quat[3]=0.25*(Base::operator()(2,1)-Base::operator()(1,2))/quat[0]; + quat[1]=0.25*(Base::operator()(0,1)+Base::operator()(1,0))/quat[0]; + quat[2]=0.25*(Base::operator()(0,2)+Base::operator()(2,0))/quat[0]; + } else if (maxidx == 1) { + quat[1]=0.5*sqrt(1 - Base::operator()(0,0) + Base::operator()(1,1) - Base::operator()(2,2)); + quat[3]=0.25*(Base::operator()(0,2)-Base::operator()(2,0))/quat[1]; + quat[2]=0.25*(Base::operator()(1,2)+Base::operator()(2,1))/quat[1]; + quat[0]=0.25*(Base::operator()(1,0)+Base::operator()(0,1))/quat[1]; + } else if (maxidx == 2) { + quat[2]=0.5*sqrt(1 - Base::operator()(0,0) - Base::operator()(1,1) + Base::operator()(2,2)); + quat[3]=0.25*(Base::operator()(1,0)-Base::operator()(0,1))/quat[2]; + quat[0]=0.25*(Base::operator()(2,0)+Base::operator()(0,2))/quat[2]; + quat[1]=0.25*(Base::operator()(2,1)+Base::operator()(1,2))/quat[2]; + } else { + quat[3]=0.5*sqrt(1 + Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2)); + quat[0]=0.25*(Base::operator()(2,1)-Base::operator()(1,2))/quat[3]; + quat[1]=0.25*(Base::operator()(0,2)-Base::operator()(2,0))/quat[3]; + quat[2]=0.25*(Base::operator()(1,0)-Base::operator()(0,1))/quat[3]; + } + + #elif 1 + + // More compact implementation of the conversion method from OpenSG + // @deprecated + + TYPE trace = Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2); + if ( trace > 0 ) { + TYPE s = 2.0 * sqrt(trace + 1.0); + quat[3] = 0.25 * s; + quat[0] = ( Base::operator()(2,1) - Base::operator()(1,2) ) / s; + quat[1] = ( Base::operator()(0,2) - Base::operator()(2,0) ) / s; + quat[2] = ( Base::operator()(1,0) - Base::operator()(0,1) ) / s; + } else { + if ( Base::operator()(0,0) > Base::operator()(1,1) && Base::operator()(0,0) > Base::operator()(2,2) ) { + TYPE s = 2.0 * sqrt( 1.0 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2)); + quat[3] = (Base::operator()(2,1) - Base::operator()(1,2) ) / s; + quat[0] = 0.25 * s; + quat[1] = (Base::operator()(0,1) + Base::operator()(1,0) ) / s; + quat[2] = (Base::operator()(0,2) + Base::operator()(2,0) ) / s; + } else if (Base::operator()(1,1) > Base::operator()(2,2)) { + TYPE s = 2.0 * sqrt( 1.0 + Base::operator()(1,1) - Base::operator()(0,0) - Base::operator()(2,2)); + quat[3] = (Base::operator()(0,2) - Base::operator()(2,0) ) / s; + quat[0] = (Base::operator()(0,1) + Base::operator()(1,0) ) / s; + quat[1] = 0.25 * s; + quat[2] = (Base::operator()(1,2) + Base::operator()(2,1) ) / s; + } else { + TYPE s = 2.0 * sqrt( 1.0 + Base::operator()(2,2) - Base::operator()(0,0) - Base::operator()(1,1) ); + quat[3] = (Base::operator()(1,0) - Base::operator()(0,1) ) / s; + quat[0] = (Base::operator()(0,2) + Base::operator()(2,0) ) / s; + quat[1] = (Base::operator()(1,2) + Base::operator()(2,1) ) / s; + quat[2] = 0.25 * s; + } + } + + #else + + // Old conversion method + // @see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm + // @deprecated + // @attention Computes wrong quaternion for rotation close to 180 degree! + + quat[3] = sqrt( max( 0.0, 1.0 + Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2) ) ) / 2.0; + quat[0] = sqrt( max( 0.0, 1.0 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2) ) ) / 2.0; + quat[1] = sqrt( max( 0.0, 1.0 - Base::operator()(0,0) + Base::operator()(1,1) - Base::operator()(2,2) ) ) / 2.0; + quat[2] = sqrt( max( 0.0, 1.0 - Base::operator()(0,0) - Base::operator()(1,1) + Base::operator()(2,2) ) ) / 2.0; + quat[0] = copysign( quat[0], Base::operator()(2,1) - Base::operator()(1,2) ) ; + quat[1] = copysign( quat[1], Base::operator()(0,2) - Base::operator()(2,0) ) ; + quat[2] = copysign( quat[2], Base::operator()(1,0) - Base::operator()(0,1) ) ; + + #endif + + // Normalize resulting quaternion + quat.Normalize(); + + // Correct quaternion so that scalar part is non-negative + if (quat[3] < 0) quat *= -1.0; +} + + +template +void TRMatrixBase::GetRotationAxisAngle(Vec& Axis, TYPE& Angle) const +{ + // nicked from OpenSG + GetQuaternion().GetAxisAngle(Axis, Angle); +} + + +template +typename TRMatrixBase::Vec TRMatrixBase::GetRotationAxis() const +{ + Vec Axis; + TYPE Angle; + GetRotationAxisAngle(Axis, Angle); + return Axis; +} + + +template +TYPE TRMatrixBase::GetRotationAngle() const +{ + Vec Axis; + TYPE Angle; + GetRotationAxisAngle(Axis, Angle); + return Angle; +} + + +template +int TRMatrixBase::GetRotationAnglesXYZ(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const +{ + #if 1 + if (Base::operator()(0,2) < 1) + { + if (Base::operator()(0,2) > -1) + { + PhiY = asin(Base::operator()(0,2)); + PhiX = atan2(-Base::operator()(1,2), Base::operator()(2,2)); + PhiZ = atan2(-Base::operator()(0,1), Base::operator()(0,0)); + } + else + { // WARNING. Not a unique solution. + PhiY = -FHALF_PI; + PhiX = -atan2(Base::operator()(1,0), Base::operator()(1,1)); + PhiZ = 0.f; // any angle works + } + } + else + { // WARNING. Not a unique solution. + PhiY = FHALF_PI; + PhiX = atan2(Base::operator()(1,0), Base::operator()(1,1)); + PhiZ = 0.f; // any angle works + } + #else + if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) { + ASSERT("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!" == NULL); + return -1; + } + PhiY = asin(Base::operator()(0,2)); + if (ISINFORNAN(PhiY)) return -1; + // changed this from atan to atan2 (koeser) + PhiX = atan2(-Base::operator()(1,2),Base::operator()(2,2)); + if (ISINFORNAN(PhiX)) return -2; + PhiZ = atan2(-Base::operator()(0,1),Base::operator()(0,0)); + if (ISINFORNAN(PhiZ)) return -3; + #endif +// #ifdef _CPC_DEBUG +// TRMatrixBase R; +// R.SetXYZ(PhiX, PhiY, PhiZ); +// if (R == (*this)) +// res = 0; +// else +// res = -1; +// #endif + return 0; +} + + +template +int TRMatrixBase::GetRotationAnglesZXY(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const +{ + if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) { + DEBUG("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!"); + return -1; + } + + PhiZ = atan2(-Base::operator()(0,1),Base::operator()(1,1)); + if (ISINFORNAN(PhiZ)) return -3; + + PhiX = asin(Base::operator()(2,1)); + if (ISINFORNAN(PhiX)) return -2; + PhiY = atan2(-Base::operator()(2,0),Base::operator()(2,2)); + if (ISINFORNAN(PhiY)) return -1; + + + // #ifdef _CPC_DEBUG + // TRMatrixBase R; + // R.SetZXY(PhiX, PhiY, PhiZ); + // cout<<"Erg R: "< +int TRMatrixBase::GetRotationAnglesYXZ(TYPE& PhiY, TYPE& PhiX, TYPE& PhiZ) const +{ + if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) { + DEBUG("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!"); + return -1; + } + + PhiZ = atan2(Base::operator()(0,2),Base::operator()(2,2)); + if (ISINFORNAN(PhiZ)) return -3; + + PhiX = asin(-Base::operator()(1,2)); + if (ISINFORNAN(PhiX)) return -2; + PhiY = atan2(Base::operator()(1,0),Base::operator()(1,1)); + if (ISINFORNAN(PhiY)) return -1; + + + // #ifdef _CPC_DEBUG + // TRMatrixBase R; + // R.SetZXY(PhiX, PhiY, PhiZ); + // cout<<"Erg R: "< +int TRMatrixBase::GetRotationAnglesZYX(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const +{ + if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) { + DEBUG("Rotation for Y-axis is +-PI/2, cannot decompose X-/Z-component!"); + return -1; + } + PhiY = asin(-Base::operator()(2,0)); + if (ISINFORNAN(PhiY)) return -1; + // changed this from atan to atan2 (koeser) + PhiX = atan2(Base::operator()(2,1),Base::operator()(2,2)); + if (ISINFORNAN(PhiX)) return -2; + PhiZ = atan2(Base::operator()(1,0),Base::operator()(0,0)); + if (ISINFORNAN(PhiZ)) return -3; +// #ifdef _CPC_DEBUG +// TRMatrixBase R; +// R.SetZYX(PhiX, PhiY, PhiZ); +// if (R == (*this)) +// res = 0; +// else +// res = -1; +// #endif + return 0; +} + + +template +void TRMatrixBase::SetFromQuaternion(const Quat& qu) +{ + // fast version, see below for old code: + const TYPE nrm = 1.0/norm(qu); + const TYPE q0 = qu[0]*nrm; + const TYPE q1 = qu[1]*nrm; + const TYPE q2 = qu[2]*nrm; + const TYPE q3 = qu[3]*nrm; + + TYPE* pData = val; + + *pData++ = 1.0 - 2.0 * (q1 * q1 + q2 * q2); + *pData++ = 2.0 * (q0 * q1 - q2 * q3); + *pData++ = 2.0 * (q2 * q0 + q1 * q3); + + *pData++ = 2.0 * (q0 * q1 + q2 * q3); + *pData++ = 1.0 - 2.0 * (q2 * q2 + q0 * q0); + *pData++ = 2.0 * (q1 * q2 - q0 * q3); + + *pData++ = 2.0 * (q2 * q0 - q1 * q3); + *pData++ = 2.0 * (q1 * q2 + q0 * q3); + *pData++ = 1.0 - 2.0 * (q1 * q1 + q0 * q0); + + +#ifdef WANT_TO_COMPARE_WITH_OLD_CODE + + Quat q = qu; // de-const + q.Normalize(); + + // nicked from OpenSG + TRMatrixBase RCompare(MatrixZero); + RCompare[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]); + RCompare[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]); + RCompare[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]); + + RCompare[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]); + RCompare[1][1] = 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]); + RCompare[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]); + + RCompare[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]); + RCompare[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]); + RCompare[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]); + + for (unsigned i=0; i<3; i++) { + for (unsigned j=0; j<3; j++) { + if (!equal(Base::operator()(i,j), RCompare[i][j], TYPE(1e-12))) { + SLOG("Matrices are not same !"<<*this<<" "< +void TRMatrixBase::SetFromOrthogonalBasis(const Vec& xxx, const Vec& yyy, const Vec& zzz) +{ + SetFromRowVectors(normalized(xxx), + normalized(yyy), + normalized(zzz)); +} + + +template +void TRMatrixBase::SetFromHV(const Vec& xxx, const Vec& yyy) +{ + // normalize + const Vec x0(normalized(xxx)); + + // compute base z in rhs: + const Vec z0(normalized(cross(xxx, yyy))); + + // compute y base orthogonal in rhs + const Vec y0(normalized(cross(z0, x0))); + + SetFromColumnVectors(x0, y0, z0); +} + + +template +TRMatrixBase& TRMatrixBase::SetFromDir2Dir(const Vec& dir0, const Vec& dir1) +{ + ASSERT(ISEQUAL(norm(dir0), TYPE(1))); + ASSERT(ISEQUAL(norm(dir1), TYPE(1))); + const TYPE cos01(CLAMP(dir1.dot(dir0), TYPE(-1), TYPE(1))); + const TYPE sin01Sq(TYPE(1) - SQUARE(cos01)); + if (sin01Sq > EPSILONTOLERANCE()) { + const Vec v(cross(dir0, dir1)); + const Mat V(CreateCrossProductMatrix3T(v)); + *this = Mat::IDENTITY + V + (V*V)*((TYPE(1)-cos01)/sin01Sq); + } else { + *this = Mat::ZERO; + if (cos01 > TYPE(0)) { + Base::operator()(0,0) = TYPE(1); + Base::operator()(1,1) = TYPE(1); + Base::operator()(2,2) = TYPE(1); + } else { + Base::operator()(0,0) = TYPE(-1); + Base::operator()(1,1) = TYPE(-1); + Base::operator()(2,2) = TYPE(-1); + } + } + return *this; +} + + +template +void TRMatrixBase::SetFromDirUpGL(const Vec& viewDir, const Vec& viewUp) +{ + ASSERT(ISEQUAL(norm(viewDir), TYPE(1))); + ASSERT(ISEQUAL(norm(viewUp), TYPE(1))); + const Vec right(normalized(cross(viewDir, viewUp))); + const Vec up(normalized(cross(right, viewDir))); + const Vec forward(viewDir * TYPE(-1)); // convert to right handed system + SetFromColumnVectors(right, up, forward); +} + +template +void TRMatrixBase::SetFromDirUp(const Vec& viewDir, const Vec& viewUp) +{ + ASSERT(ISEQUAL(norm(viewDir), TYPE(1))); + ASSERT(ISEQUAL(norm(viewUp), TYPE(1))); + const Vec right(normalized(cross(viewDir, viewUp))); + const Vec up(normalized(cross(viewDir, right))); + const Vec& forward(viewDir); + SetFromColumnVectors(right, up, forward); +} + +template +void TRMatrixBase::LookAt(const Vec& from, const Vec& to, const Vec& up) +{ + SetFromDirUp(normalized(to-from), up); +} + + +template +inline void TRMatrixBase::SetRotationAxisAngle(const Vec& rot) +{ + #if 0 + // set rotation using Rodriguez formula + *this = Mat::IDENTITY; + const TYPE thetaSq = normSq(rot); + if (thetaSq < TYPE(1e-12)) + return; + const TYPE theta = sqrt(thetaSq); + // make cross product matrix of rot + Mat J; + J(0,0) = TYPE(0); J(0,1) = -rot.z; J(0,2) = rot.y; + J(1,0) = rot.z; J(1,1) = TYPE(0); J(1,2) = -rot.x; + J(2,0) = -rot.y; J(2,1) = rot.x; J(2,2) = TYPE(0); + // compute R matrix + const Mat J2 = J * J; + const TYPE c1 = sin(theta)/theta; + const TYPE c2 = (TYPE(1)-cos(theta))/thetaSq; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + (*this)(i,j) += c1*J(i,j) + c2*J2(i,j); + #elif !defined(_USE_EIGEN) + // set rotation using Rodriguez formula + const TYPE x2 = rot.x*rot.x; + const TYPE y2 = rot.y*rot.y; + const TYPE z2 = rot.z*rot.z; + const TYPE t2 = x2+y2+z2; + if (t2 > TYPE(1e-12)) { + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + const TYPE t = sqrt(t2); + const TYPE ct = /*t==0.0?0.5:*/(TYPE(1)-cos(t))/t2; + const TYPE st = /*t==0.0?1.0:*/sin(t)/t; + val[0] = TYPE(1) - (y2 + z2)*ct; + val[1] = rot.x*rot.y*ct - rot.z*st; + val[2] = rot.z*rot.x*ct + rot.y*st; + val[3] = rot.x*rot.y*ct + rot.z*st; + val[4] = TYPE(1) - (z2 + x2)*ct; + val[5] = rot.y*rot.z*ct - rot.x*st; + val[6] = rot.z*rot.x*ct - rot.y*st; + val[7] = rot.y*rot.z*ct + rot.x*st; + val[8] = TYPE(1) - (x2 + y2)*ct; + } else { + // At zero, we switch to using the first order Taylor expansion. + val[0] = TYPE(1); + val[1] =-rot.z; + val[2] = rot.y; + val[3] = rot.z; + val[4] = TYPE(1); + val[5] =-rot.x; + val[6] =-rot.y; + val[7] = rot.x; + val[8] = TYPE(1); + } + #else + *this = Eigen::SO3(rot).get_matrix(); + #endif +} + +template +inline void TRMatrixBase::Apply(const Vec& delta) +{ + const TRMatrixBase dR(delta); + *this = dR * (*this); +} + +template +inline typename TRMatrixBase::Vec TRMatrixBase::GetRotationAxisAngle() const +{ + #if 0 + // get parametrized rotation using quaternions + Vec angle_axis; + TYPE angle; + GetQuaternion().GetAxisAngle(angle_axis, angle); + angle_axis.Normalize(); + angle_axis *= angle; + return angle_axis; + #elif 0 + // get parametrized rotation using Rodriguez formula + Vec angle_axis; + // x = k * 2 * sin(theta), where k is the axis of rotation. + angle_axis[0] = (*this)(2, 1) - (*this)(1, 2); + angle_axis[1] = (*this)(0, 2) - (*this)(2, 0); + angle_axis[2] = (*this)(1, 0) - (*this)(0, 1); + static const TYPE kZero(0); + static const TYPE kOne(1); + static const TYPE kTwo(2); + // Since the right hand side may give numbers just above 1.0 or + // below -1.0 leading to atan misbehaving, we threshold. + const TYPE costheta(MINF(MAXF(((*this)(0, 0) + (*this)(1, 1) + (*this)(2, 2) - kOne)/kTwo, TYPE(-1.0)), kOne)); + // sqrt is guaranteed to give non-negative results, so we only + // threshold above. + const TYPE sintheta(MINF(norm(angle_axis)/kTwo, kOne)); + // Use the arctan2 to get the right sign on theta + const TYPE theta(atan2(sintheta, costheta)); + // Case 1: sin(theta) is large enough, so dividing by it is not a + // problem. We do not use abs here, because while jets.h imports + // std::abs into the namespace, here in this file, abs resolves to + // the int version of the function, which returns zero always. + // + // We use a threshold much larger then the machine epsilon, because + // if sin(theta) is small, not only do we risk overflow but even if + // that does not occur, just dividing by a small number will result + // in numerical garbage. So we play it safe. + static const TYPE kThreshold(TYPE(1e-12)); + if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { + angle_axis *= theta / (kTwo * sintheta); + return angle_axis; + } + // Case 2: theta ~ 0, means sin(theta) ~ theta to a good + // approximation. + if (costheta > kZero) { + angle_axis *= TYPE(0.5); + return angle_axis; + } + // Case 3: theta ~ pi, this is the hard case. Since theta is large, + // and sin(theta) is small. Dividing theta by sin(theta) will either + // give an overflow or worse still numerically meaningless results. + // Thus we use an alternate more complicated and expensive formula. + #ifndef _USE_EIGEN + // Since cos(theta) is negative, division by (1-cos(theta)) cannot + // overflow. + const TYPE inv_one_minus_costheta(kOne / (kOne - costheta)); + // We now compute the absolute value of coordinates of the axis + // vector using the diagonal entries of R. To resolve the sign of + // these entries, we compare the sign of angle_axis[i]*sin(theta) + // with the sign of sin(theta). If they are the same, then + // angle_axis[i] should be positive, otherwise negative. + for (int i = 0; i < 3; ++i) { + angle_axis[i] = theta * sqrt(((*this)(i, i) - costheta) * inv_one_minus_costheta); + if (((sintheta < kZero) && (angle_axis[i] > kZero)) || + ((sintheta > kZero) && (angle_axis[i] < kZero))) + angle_axis[i] = -angle_axis[i]; + } + #else + Eigen::AngleAxis aa; + aa.fromRotationMatrix(Base::EMatMap((TYPE*)this)); + angle_axis[0] = aa.angle() * aa.axis()[0]; + angle_axis[1] = aa.angle() * aa.axis()[1]; + angle_axis[2] = aa.angle() * aa.axis()[2]; + #endif + return angle_axis; + #else + return reinterpret_cast*>(this)->ln(); + #endif +} + + +template +bool TRMatrixBase::Check(const TYPE eps, int verbose) const +{ + // check determinant + bool ok = equal(TYPE(cv::determinant(*this)), TYPE(1), eps); + // check unit column vector length + Vec cl[3]; + for (unsigned i = 0; i < 3 && ok; i++) { + cl[i] = Base::col(i); + ok = equal(norm(cl[i]), TYPE(1), eps); + } + // check orthogonality + if (ok) + ok = equal(cl[0].dot(cl[1]), TYPE(0), eps); + if (ok) + ok = equal(cl[0].dot(cl[2]), TYPE(0), eps); + if (ok) + ok = equal(cl[1].dot(cl[2]), TYPE(0), eps); + #if TD_VERBOSE != TD_VERBOSE_OFF + // print the reason in case of failure + if (!ok && VERBOSITY_LEVEL > verbose) { + std::ostringstream res(" "); + const TYPE det(cv::determinant(*this)); + if (!equal(det, TYPE(1), eps)) { + res << "det != 1 (det=" << std::setprecision(30) << det << ") "; + } + for (unsigned i = 0; i < 3; i++) { + cl[i] = Base::col(i); + if (!equal(norm(cl[i]), TYPE(1), eps)) { + res << "col[" << i << "].NormL2() != 1) (= " + << norm(cl[i]) << ") "; + } + } + if (!equal(cl[0].dot(cl[1]), TYPE(0), eps)) { + res << "col[0].ScalarProduct(c[1]) != 0 (= " + << cl[0].dot(cl[1]) << ") "; + } + if (!equal(cl[0].dot(cl[2]), TYPE(0), eps)) { + res << "col[0].ScalarProduct(c[2]) != 0 (= " + << cl[0].dot(cl[2]) << ") "; + } + if (!equal(cl[1].dot(cl[2]), TYPE(0), eps)) { + res << "col[1].ScalarProduct(c[2]) != 0 (= " + << cl[1].dot(cl[2]) << ") "; + } + SLOG("Rotation matrix is invalid: " << res.str() << std::endl); + } + #endif + return ok; +} + +template +void TRMatrixBase::EnforceOrthogonality() +{ + #if 1 + // use SVD decomposition to find the closest orthogonal matrix + const cv::SVD svd(*this, cv::SVD::MODIFY_A|cv::SVD::FULL_UV); + *this = svd.u*svd.vt; + #elif 1 + // replaces the 3 x 3 matrix with the nearest (least squares) orthogonal matrix; + // a combination of Babylonian iteration and Berthold KP Horn's formula R = M (MT M)^-1/2 + TYPE* r = this->val; + TYPE m[9], ri[9], x[9]; + memcpy(m, r, 9*sizeof(TYPE)); + //By trial and error I found that 4 iterates always works unless your matrix is waaay off. Limit 10. + for (unsigned iterate = 0; iterate != 10; iterate++) { + //Invert r. Use inverse and not transpose since r is only approximately orthogonal (transpose will result in slow, oscillating convergence). + InvertMatrix3x3(r, ri); + + //Get x = ri m + mt r. + x[0] = m[6]*r[6]+ri[2]*m[6]+m[3]*r[3]+ri[1]*m[3]+m[0]*ri[0]+m[0]*r[0]; + x[1] = m[6]*r[7]+ri[2]*m[7]+m[3]*r[4]+ri[1]*m[4]+m[0]*r[1]+ri[0]*m[1]; + x[2] = m[6]*r[8]+ri[2]*m[8]+m[3]*r[5]+ri[1]*m[5]+m[0]*r[2]+ri[0]*m[2]; + x[3] = r[6]*m[7]+ri[5]*m[6]+m[3]*ri[4]+r[3]*m[4]+m[0]*ri[3]+r[0]*m[1]; + x[4] = m[7]*r[7]+ri[5]*m[7]+m[4]*ri[4]+m[4]*r[4]+m[1]*ri[3]+m[1]*r[1]; + x[5] = m[7]*r[8]+ri[5]*m[8]+m[4]*r[5]+ri[4]*m[5]+m[2]*ri[3]+m[1]*r[2]; + x[6] = m[6]*ri[8]+r[6]*m[8]+m[3]*ri[7]+m[0]*ri[6]+r[3]*m[5]+r[0]*m[2]; + x[7] = m[7]*ri[8]+r[7]*m[8]+m[4]*ri[7]+m[1]*ri[6]+r[4]*m[5]+r[1]*m[2]; + x[8] = m[8]*ri[8]+m[8]*r[8]+m[5]*ri[7]+m[2]*ri[6]+m[5]*r[5]+m[2]*r[2]; + + //Invert into ri. + InvertMatrix3x3(x, ri); + + //Replace r with 2 m ri, where ri is as replaced above. + r[0] = 2*(m[2]*ri[6]+m[1]*ri[3]+m[0]*ri[0]); + r[1] = 2*(m[2]*ri[7]+m[1]*ri[4]+m[0]*ri[1]); + r[2] = 2*(m[2]*ri[8]+m[1]*ri[5]+m[0]*ri[2]); + r[3] = 2*(m[5]*ri[6]+ri[3]*m[4]+ri[0]*m[3]); + r[4] = 2*(m[5]*ri[7]+m[4]*ri[4]+ri[1]*m[3]); + r[5] = 2*(m[5]*ri[8]+m[4]*ri[5]+ri[2]*m[3]); + r[6] = 2*(ri[6]*m[8]+ri[3]*m[7]+ri[0]*m[6]); + r[7] = 2*(ri[7]*m[8]+ri[4]*m[7]+ri[1]*m[6]); + r[8] = 2*(m[8]*ri[8]+ri[5]*m[7]+ri[2]*m[6]); + + //Check how off it is. + x[0] = ABS(r[0]*r[0] + r[3]*r[3] + r[6]*r[6] - TYPE(1)); + x[1] = ABS(r[1]*r[1] + r[4]*r[4] + r[7]*r[7] - TYPE(1)); + x[2] = ABS(r[2]*r[2] + r[5]*r[5] + r[8]*r[8] - TYPE(1)); + x[3] = ABS(r[0]*r[1] + r[3]*r[4] + r[6]*r[7]); + x[4] = ABS(r[1]*r[2] + r[4]*r[5] + r[7]*r[8]); + x[5] = ABS(r[2]*r[0] + r[5]*r[3] + r[8]*r[6]); + + //Max norm, by all means, really make it as accurate as possible. + TYPE maxElement(-FLT_MAX); + for (unsigned n=6; n != 0; ) + if(x[--n] > maxElement) + maxElement = x[n]; + if (maxElement < TYPE(1e-15)) return; + } + #else + // Matrix Orthogonalization + // by Eric Raible from "Graphics Gems", Academic Press, 1990 + // + // Reorthogonalize matrix R - that is find an orthogonal matrix that is + // "close" to R by computing an approximation to the orthogonal matrix + // + // T -1/2 + // RC = R(R R) + // T -1 + // [RC is orthogonal because (RC) = (RC) ] + // -1/2 + // To compute C, we evaluate the Taylor expansion of F(x) = (I + x) + // (where x = C - I) about x=0. + // This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ... + // By default, limit should be 8. + const unsigned max_limit = 8; + static const TYPE coef[10] = { 1, -1/2., 3/8., -5/16., 35/128., -63/256., 231/1024., -429/2048., 6435/32768., -12155/65536. }; + Base& R(*this); + const Base RtR(R.t()*R); + const Base X(RtR - Matrix3x3::IDENTITY); /* RtR - I */ + Base X_power(Matrix3x3::IDENTITY); /* X^0 */ + Base Sum(Matrix3x3::IDENTITY); /* coef[0] * X^0 */ + const unsigned limit(MINF(max_limit, (unsigned)10)); + for (unsigned power=1; power +inline bool IsRotationMatrix(const TMatrix& R) { + ASSERT(sizeof(TMatrix) == sizeof(TRMatrixBase)); + return ((const TRMatrixBase&)R).IsValid(); +} // IsRotationMatrix +template +inline bool IsRotationMatrix(const Eigen::Matrix& R) { + // the trace should be three and the determinant should be one + return (ISEQUAL(R.determinant(), TYPE(1)) && ISEQUAL((R*R.transpose()).trace(), TYPE(3))); +} // IsRotationMatrix +/*----------------------------------------------------------------*/ + +// enforce matrix orthogonality +template +inline void EnsureRotationMatrix(TMatrix& R) { + ASSERT(sizeof(TMatrix) == sizeof(TRMatrixBase)); + ((TRMatrixBase&)R).EnforceOrthogonality(); +} // EnsureRotationMatrix +/*----------------------------------------------------------------*/ + + +// compute the distance on SO(3) between the two given rotations +// using log(R) as in: "Efficient and Robust Large-Scale Rotation Averaging", 2013 +// same result as above, but returns directly the angle +template +inline TYPE ComputeAngleSO3(const TMatrix& I) { + return TYPE(norm(((const TRMatrixBase&)I).GetRotationAxisAngle())); +} // ComputeAngleSO3 +template +FORCEINLINE TYPE ComputeAngleSO3(const TMatrix& R1, const TMatrix& R2) { + return ComputeAngleSO3(TMatrix(R1*R2.t())); +} // ComputeAngleSO3 +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE diff --git a/libs/Common/SML.cpp b/libs/Common/SML.cpp new file mode 100644 index 0000000..56409fd --- /dev/null +++ b/libs/Common/SML.cpp @@ -0,0 +1,419 @@ +//////////////////////////////////////////////////////////////////// +// SML.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "SML.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define SML_AUTOVALUES_OFF 0 +#define SML_AUTOVALUES_ON 1 +#ifndef SML_AUTOVALUES +#define SML_AUTOVALUES SML_AUTOVALUES_ON +#endif + +#define SML_SECTIONOPENBRACKET _T("{") +#define SML_SECTIONCLOSEBRACKET _T("}") +#define SML_NAMEOPENBRACKET _T("[") +#define SML_NAMECLOSEBRACKET _T("]") +#define SML_NAMETOKEN _T("=") +#define SML_VALUETOKEN _T("\n") +#define SML_INDENT _T("\t") + +#define SML_TOKEN_SECTIONOPENBRACKET SML_SECTIONOPENBRACKET[0] +#define SML_TOKEN_SECTIONCLOSEBRACKET SML_SECTIONCLOSEBRACKET[0] +#define SML_TOKEN_NAMEOPENBRACKET SML_NAMEOPENBRACKET[0] +#define SML_TOKEN_NAMECLOSEBRACKET SML_NAMECLOSEBRACKET[0] +#define SML_TOKEN_NAMETOKEN SML_NAMETOKEN[0] +#define SML_TOKEN_VALUETOKEN SML_VALUETOKEN[0] +#define SML_TOKEN_INDENT SML_INDENT[0] +#define SML_TOKEN_IGNORECHARS _T("\n\r\t ") + + +// S T R U C T S /////////////////////////////////////////////////// + +/*-----------------------------------------------------------* + * SML class implementation * + *-----------------------------------------------------------*/ + +/** + * Constructor + */ +SML::SML(const String& name) + : + m_strName(name), + m_fncInitItem(NULL), + m_fncSaveItem(NULL), + m_fncReleaseItem(NULL), + m_fncItemData(NULL) +{ +} + +/** + * Destructor + */ +SML::~SML() +{ + Release(); +} +/*----------------------------------------------------------------*/ + +/** + * Released all used memory for this nod and its children + */ +void SML::Release() +{ + m_arrChildren.ReleaseDelete(); + if (m_fncReleaseItem != NULL) { + for (SMLITEMMAP::iterator it=GetBegin(); it!=GetEnd(); ++it) + m_fncReleaseItem(it->second, m_fncItemData); + } + SMLITEMMAP::Release(); +} +/*----------------------------------------------------------------*/ + + +/** + * Load the values from a file; + * all children are generated if they do not exist + */ +bool SML::Load(const String& fileName) +{ + File oStream(fileName, File::READ, File::OPEN); + if (!oStream.isOpen()) + return false; + return Load(oStream); +} +bool SML::Load(ISTREAM& oStream) +{ + // create the section token filter and mem-file + MemFile memFile; + TokenIStream filter(&oStream); + filter.setTrimTokens(SML_TOKEN_IGNORECHARS); + // and parse section + return ParseSection(filter, memFile); +} +bool SML::ParseSection(TokenIStream& filter, MemFile& memFile) +{ + while (true) { + // find first section start (NameOpenBracket) + filter.setToken(SML_TOKEN_NAMEOPENBRACKET); + filter.readLine(memFile); + + // parse values before the new section or end of section + const size_f_t posMemFile = memFile.getPos(); + MemFile valuesMemFile; + TokenIStream sectionFilter(&memFile, SML_TOKEN_SECTIONCLOSEBRACKET); + const size_f_t lenValues = sectionFilter.readLine(valuesMemFile); + if (ParseValues(valuesMemFile) == false) + return false; // Parse Error: invalid values + ASSERT(valuesMemFile.getSize() == 0); + + // if end of section found, return + if (!sectionFilter.isEOS()) { + // if a name open bracket was found before the EOS, + // then restore it for the parent next ParseSection() + memFile.setPos(posMemFile+(lenValues+1)*sizeof(TCHAR)); + if (!filter.isEOS()) + filter.restoreToken(); + break; + } + else { + ASSERT(memFile.getSize()-posMemFile == lenValues*(size_f_t)sizeof(TCHAR)); + ASSERT(posMemFile == 0 || memFile.getSize()-posMemFile == memFile.getSizeLeft() || memFile.getSizeLeft() == 0); + memFile.setSize(0); + } + + // if no more data, return + if (filter.isEOS()) + break; + + // parse child section name + filter.setToken(SML_TOKEN_NAMECLOSEBRACKET); + const size_t lenName = filter.readLine(memFile); + ASSERT(!filter.isEOS()); + if (lenName == 0) + return false; // Parse Error: invalid section name + const String strChildName((LPCTSTR)memFile.getData(), lenName); + memFile.growSize(-((size_f_t)(lenName*sizeof(TCHAR)))); + ASSERT(memFile.getSize() == 0); + // create the child with the given name + const IDX idxChild = CreateChildUnique(strChildName); + LPSML const pChild = m_arrChildren[idxChild]; + pChild->SetFncItem(m_fncInitItem, m_fncSaveItem, m_fncReleaseItem, m_fncItemData); + + // parse child section + filter.setToken(SML_TOKEN_SECTIONOPENBRACKET); + filter.readLine(memFile); + filter.trimBackLine(memFile); + ASSERT(memFile.getSize() == 0); + if (pChild->ParseSection(filter, memFile) == false) + return false; + } + return true; +} +bool SML::ParseValues(MemFile& valuesMemFile) +{ + if (valuesMemFile.getSize() == 0) + return true; + // loop through all name/value pairs + MemFile memLine; + TokenIStream filterLine(&valuesMemFile); + filterLine.setTrimTokens(SML_TOKEN_IGNORECHARS); + filterLine.setToken(SML_TOKEN_VALUETOKEN); + MemFile memValue; + TokenIStream filterValue(&memLine); + filterValue.setTrimTokens(SML_TOKEN_IGNORECHARS); + filterValue.setToken(SML_TOKEN_NAMETOKEN); + do { + // parse value name and value + filterLine.readLine(memLine); + filterLine.trimBackLine(memLine); + filterLine.trimFrontLine(memLine); + if (memLine.isEOF()) + continue; // empty line, return + // parse value name + const size_t lenNameValueReal = (size_t)memLine.getSizeLeft(); + const size_t lenName = filterValue.readLine(memValue); + #if SML_AUTOVALUES == SML_AUTOVALUES_ON + String szName; + if (filterValue.trimBackLine(memValue) == lenName || lenNameValueReal == lenName) { + // no name found, auto generate the name + szName = _T("Item") + String::ToString(size()); + } else { + // read the name + szName = (LPCTSTR)memValue.getData(); + memValue.setSize(0); + } + #else + filterValue.trimBackLine(memValue); + String szName = (LPCTSTR)memValue.getData(); + ASSERT(!filterValue.isEOS() && !szName.IsEmpty()); + if (filterValue.isEOS() || szName.IsEmpty()) { + memValue.setSize(0); + memLine.setSize(0); + filterValue.setPos(0); + continue; // Parse Error: invalid syntax ('=' not found) + } + ASSERT(szName.size() == memValue.getSizeLeft()); + memValue.setSize(0); + #endif + SMLVALUE& val = operator[](szName); + // parse value + filterValue.read(memValue); + LPCTSTR szValue = filterValue.trimFrontLine(memValue); + val.val = szValue; + ASSERT((size_f_t)_tcslen(szValue) == memValue.getSizeLeft()); + memValue.setSize(0); + memLine.setSize(0); + filterValue.setPos(0); + } while (!filterLine.isEOS()); + // all file processed, safe to reset it to 0 + valuesMemFile.setSize(0); + return true; +} +/*----------------------------------------------------------------*/ + + +/** + * Write to a file the values of this node and its children. + * Set to false the second parameter in order not to save the empty children. + */ +bool SML::Save(const String& fileName, SAVEFLAG flags) const +{ + File oStream(fileName, File::WRITE, File::CREATE | File::TRUNCATE); + if (!oStream.isOpen()) + return false; + return Save(oStream, flags); +} +bool SML::Save(OSTREAM& oStream, SAVEFLAG flags) const +{ + // save all values and all children + return SaveIntern(oStream, _T(""), flags); +} +bool SML::SaveBracketize(OSTREAM& oStream, const String& strIndent, SAVEFLAG flags) const +{ + // save its name + oStream.print(_T("%s" SML_NAMEOPENBRACKET "%s" SML_NAMECLOSEBRACKET "\n"), strIndent.c_str(), m_strName.c_str()); + oStream.print(_T("%s" SML_SECTIONOPENBRACKET "\n"), strIndent.c_str()); + // save all values and all children + SaveIntern(oStream, strIndent+SML_TOKEN_INDENT, flags); + // close bracket + oStream.print(_T("%s" SML_SECTIONCLOSEBRACKET "\n"), strIndent.c_str()); + return true; +} +bool SML::SaveIntern(OSTREAM& oStream, const String& strIndent, SAVEFLAG flags) const +{ + const Flags& flgs = (const Flags&)flags; + if (flgs.isAnySet(SORT | SORTINV)) { + // sort values alphabetically and save them + StringArr lines(0, GetSize()); + for (SMLITEMMAP::const_iterator item=GetBegin(); item!=GetEnd(); ++item) + if (!m_fncSaveItem || m_fncSaveItem((*item).second, m_fncItemData)) + lines.InsertSort(String::FormatString(_T("%s%s " SML_NAMETOKEN " %s" SML_VALUETOKEN), strIndent.c_str(), (*item).first.c_str(), (*item).second.val.c_str()), + (flgs.isAnySet(SORT) ? String::CompareAlphabetically : String::CompareAlphabeticallyInv)); + FOREACH(l, lines) + oStream.print(lines[l]); + } else { + // save all values directly + for (SMLITEMMAP::const_iterator item=GetBegin(); item!=GetEnd(); ++item) + if (!m_fncSaveItem || m_fncSaveItem((*item).second, m_fncItemData)) + oStream.print(_T("%s%s " SML_NAMETOKEN " %s" SML_VALUETOKEN), strIndent.c_str(), (*item).first.c_str(), (*item).second.val.c_str()); + } + // save now all children + bool bFirst = IsEmpty(); + FOREACH(i, m_arrChildren) { + const SML* pSML = m_arrChildren[i]; + // skip empty children + if (!flgs.isSet(SAVEEMPTY) && pSML->IsEmpty() && pSML->m_arrChildren.IsEmpty()) + continue; + // insert a new line to separate from the above section (only for visual aspect) + if (bFirst) + bFirst = false; + else + oStream.print(_T("\n")); + // save child + pSML->SaveBracketize(oStream, strIndent, flags); + } + return true; +} +/*----------------------------------------------------------------*/ + + +/** + * Create and insert a new child + */ +IDX SML::CreateChild(const String& strChildName) +{ + return InsertChild(new SML(strChildName)); +} +IDX SML::CreateChildUnique(const String& strChildName) +{ + return InsertChildUnique(new SML(strChildName)); +} +/*----------------------------------------------------------------*/ + + +/** + * Insert a new child + */ +IDX SML::InsertChild(const LPSML pSML) +{ + return m_arrChildren.InsertSort(pSML, SML::Compare); +} +IDX SML::InsertChildUnique(const LPSML pSML) +{ + const std::pair res(m_arrChildren.InsertSortUnique(pSML, SML::Compare)); + if (res.second) + delete pSML; + return res.first; +} +/*----------------------------------------------------------------*/ + + +/** + * Remove an existing child + */ +void SML::RemoveChild(IDX idx) +{ + m_arrChildren.RemoveAtMove(idx); +} +/*----------------------------------------------------------------*/ + +/** + * Remove and destroy an existing child + */ +void SML::DestroyChild(IDX idx) +{ + delete m_arrChildren[idx]; + RemoveChild(idx); +} +/*----------------------------------------------------------------*/ + + +/** + * Retrieve an child by its name; NO_INDEX if unexistent + */ +IDX SML::GetChild(const String& name) const +{ + return m_arrChildren.FindFirst(&name, SML::CompareName); +} +/*----------------------------------------------------------------*/ + + +/** + * Retrieve an item; NULL if unexistent + */ +const SMLVALUE* SML::GetValue(const String& key) const +{ + const_iterator it = Find(key); + if (it == GetEnd()) + return NULL; + return &(it->second); +} +/*----------------------------------------------------------------*/ + + +/** + * Insert or retrieve an item; initialize it if necessary + */ +SMLVALUE& SML::GetValue(const String& key) +{ + bool bExisted; + SMLVALUE& val = (*Insert(key, bExisted)).second; + if (!bExisted && m_fncInitItem != NULL) + m_fncInitItem(key, val, m_fncItemData); + return val; +} +/*----------------------------------------------------------------*/ + + +/** + * Retrieve an unnamed item by index + */ +const SMLVALUE& SML::GetValue(IDX idx) const +{ + ASSERT(idx < this->size()); + const String szName(_T("Item") + String::ToString(idx)); + return this->at(szName); +} +/*----------------------------------------------------------------*/ + + +/** + * Reset items' init and release functions + */ +void SML::SetFncItem(TFncInitItem fncInit, TFncSaveItem fncSave, TFncReleaseItem fncRelease, void* data) +{ + m_fncInitItem = fncInit; + m_fncSaveItem = fncSave; + m_fncReleaseItem = fncRelease; + m_fncItemData = data; +} +/*----------------------------------------------------------------*/ + + +/** + * Compare two SML nodes by name + */ +int STCALL SML::Compare(const void* l, const void* r) +{ + return _tcscmp((*((const SML**)l))->GetName(), (*((const SML**)r))->GetName()); +} +/*----------------------------------------------------------------*/ + +/** + * Compare a name and a SML node name + */ +int STCALL SML::CompareName(const void* l, const void* r) +{ + return _tcscmp((*((const SML**)l))->GetName(), ((const String*)r)->c_str()); +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/SML.h b/libs/Common/SML.h new file mode 100644 index 0000000..c016d93 --- /dev/null +++ b/libs/Common/SML.h @@ -0,0 +1,116 @@ +//////////////////////////////////////////////////////////////////// +// SML.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_SML_H__ +#define __SEACAVE_SML_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Filters.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +typedef struct SMLVALUE_TYPE { + String val; //item's value + void* data; //user data + SMLVALUE_TYPE() : data(NULL) {} + SMLVALUE_TYPE(const SMLVALUE_TYPE& r) : data(r.data) { ((SMLVALUE_TYPE&)r).data = NULL; } + ~SMLVALUE_TYPE() { delete data; } +} SMLVALUE; + +class SML; +typedef SML* LPSML; +typedef cList LPSMLARR; + +//typedef cHashTable SMLITEMMAP; +typedef cMapWrap SMLITEMMAP; + +/************************************************************************************** + * Simple Markup Language + * -------------- + * fast and easy to use markup language; + * contains a map of (name, value) pairs + **************************************************************************************/ + +class GENERAL_API SML : public SMLITEMMAP +{ +public: + typedef void (STCALL *TFncInitItem)(const String&, SMLVALUE&, void*); + typedef bool (STCALL *TFncSaveItem)(const SMLVALUE&, void*); + typedef void (STCALL *TFncReleaseItem)(SMLVALUE&, void*); + + enum SAVEFLAG { + NONE = 0, + SAVEEMPTY = (1 << 0), //save empty children + SORT = (1 << 1), //sort all entries alphabetically + SORTINV = (1 << 2), //sort all entries inverse alphabetically + }; + +public: + SML(const String& =String()); + ~SML(); + + void Release(); + + // main methods + bool Load(const String&); + bool Load(ISTREAM&); + bool Save(const String&, SAVEFLAG=NONE) const; + bool Save(OSTREAM&, SAVEFLAG=NONE) const; + IDX CreateChild(const String& =String()); + IDX CreateChildUnique(const String& =String()); + IDX InsertChild(const LPSML); + IDX InsertChildUnique(const LPSML); + void RemoveChild(IDX); + inline void RemoveChild(const String& name) { RemoveChild(GetChild(name)); } + void DestroyChild(IDX); + inline void DestroyChild(const String& name) { DestroyChild(GetChild(name)); } + IDX GetChild(const String&) const; + const SMLVALUE* GetValue(const String&) const; + SMLVALUE& GetValue(const String&); + const SMLVALUE& GetValue(IDX) const; + inline SMLVALUE& operator[] (const String& key) { return GetValue(key); } + + // misc methods + inline const String& GetName() const { return m_strName; } + inline LPSML GetChild(IDX idx) const { return m_arrChildren[idx]; } + inline const LPSMLARR& GetArrChildren() const { return m_arrChildren; } + inline LPSMLARR& GetArrChildren() { return m_arrChildren; } + void SetFncItem(TFncInitItem, TFncSaveItem, TFncReleaseItem, void* data=NULL); + +public: + static int STCALL Compare(const void*, const void*); + static int STCALL CompareName(const void*, const void*); + +private: + typedef TokenInputStream TokenIStream; + + bool ParseSection(TokenIStream&, MemFile&); + bool ParseValues(MemFile&); + bool SaveBracketize(OSTREAM&, const String&, SAVEFLAG) const; + bool SaveIntern(OSTREAM&, const String&, SAVEFLAG) const; + +private: + const String m_strName; // node name + LPSMLARR m_arrChildren; // the array with all the sub-nodes + TFncInitItem m_fncInitItem; // callback function used to initialize each item + TFncSaveItem m_fncSaveItem; // callback function used to save each item + TFncReleaseItem m_fncReleaseItem; // callback function used to release each item + void* m_fncItemData; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_SML_H__ diff --git a/libs/Common/Sampler.inl b/libs/Common/Sampler.inl new file mode 100644 index 0000000..e22b21f --- /dev/null +++ b/libs/Common/Sampler.inl @@ -0,0 +1,274 @@ +// Copyright (c) 2012-2015 OpenMVG. +// Copyright (c) 2012-2015 Pierre MOULON. +// Copyright (c) 2015 Romuald Perrot. +// Copyright (c) 2015 cDc@seacave. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +namespace Sampler { + +// Sampling functors +// These functors computes weight associated to each pixels +// +// For a (relative) sampling position x (in [0,1]) between two (consecutive) points : +// +// A .... x ......... B +// +// w[0] is the weight associated to A +// w[1] is the weight associated to B +// +// Note: The following functors generalize the sampling to more than two neighbors +// They all contains the width variable that specify the number of neighbors used for sampling +// +// All contain the operator() with the following definition: +// +// @brief Computes weight associated to neighboring pixels +// @author Romuald Perrot +// @param x Sampling position +// @param[out] weight Sampling factors associated to the neighboring +// @note weight must be at least width length +// Linear sampling (ie: linear interpolation between two pixels) +template +struct Linear { + typedef TYPE Type; + + enum { halfWidth = 1 }; + enum { width = halfWidth*2 }; + + inline Linear() {} + + inline void operator() (const TYPE x, TYPE* const weight) const { + weight[0] = TYPE(1) - x; + weight[1] = x; + } +}; + +// Cubic interpolation between 4 pixels +// +// Interpolation weight is for A,B,C and D pixels given a x position as illustrated as follow : +// +// A B x C D +// +// Cubic Convolution Interpolation for Digital Image Processing , R. Keys, eq(4) +// +// The cubic filter family with the two free parameters usually named B and C: +// if |x|<1 +// ((12-9B-6C)|x|^3 + (-18+12B+6C)|x|^2 + (6-2B))/6 +// if 1<=|x|<2 +// ((-B-6C)|x|^3 + (6B+30C)|x|^2 + (-12B-48C)|x| + (8B+24C))/6 +template +struct Cubic { + typedef TYPE Type; + + enum { halfWidth = 2 }; + enum { width = halfWidth*2 }; + + // Sharpness coefficient used to control sharpness of the cubic curve (between 0.5 to 0.75) + // cubic(0,1/2): 0.5 gives better mathematically result (ie: approximation at 3 order precision - Catmull-Rom) + const TYPE sharpness; + inline Cubic(const TYPE& _sharpness=TYPE(0.5)) : sharpness(_sharpness) {} + + inline void operator() (const TYPE x, TYPE* const weight) const { + // remember : + // A B x C D + + // weight[0] -> weight for A + // weight[1] -> weight for B + // weight[2] -> weight for C + // weight[3] -> weight for D + + weight[0] = CubicInter12(x + TYPE(1)); + weight[1] = CubicInter01(x); + weight[2] = CubicInter01(TYPE(1) - x); + weight[3] = CubicInter12(TYPE(2) - x); + } + + // Cubic interpolation for x (in [0,1]) + inline TYPE CubicInter01(const TYPE x) const { + // A = -sharpness + // f(x) = (A + 2) * x^3 - (A + 3) * x^2 + 1 + return ((TYPE(2)-sharpness)*x - (TYPE(3)-sharpness))*x*x + TYPE(1); + } + // Cubic interpolation for x (in [1,2]) + inline TYPE CubicInter12(const TYPE x) const { + // A = -sharpness + // f(x) = A * x^3 - 5 * A * x^2 + 8 * A * x - 4 * A + return (((TYPE(5)-x)*x - TYPE(8))*x + TYPE(4))*sharpness; + } +}; + +// Sampler spline16 -> Interpolation on 4 points used for 2D ressampling (16 = 4x4 sampling) +// Cubic interpolation with 0-derivative at edges (ie at A and D points) +// See Helmut Dersch for more details +// +// Some refs : +// - http://forum.doom9.org/archive/index.php/t-147117.html +// - http://avisynth.nl/index.php/Resampling +// - http://www.ipol.im/pub/art/2011/g_lmii/ +// +// The idea is to consider 3 cubic splines (f1,f2,f3) in the sampling interval : +// +// A f1 B f2 C f3 D +// +// with curves defined as follow : +// f1(x) = a1 x^3 + b1 x^2 + c1 x + d1 +// f2(x) = a2 x^3 + b2 x^2 + c2 x + d2 +// f3(x) = a3 x^3 + b2 x^2 + c3 x + d3 +// +// We want to compute spline coefs for A,B,C,D assuming that: +// y0 = coef[A] = f1(-1) +// y1 = coef[B] = f1(0) = f2(0) +// y2 = coef[C] = f2(1) = f3(1) +// y3 = coef[D] = f3(2) +// +// coef are computed using the following constraints : +// Curve is continuous, ie: +// f1(0) = f2(0) +// f2(1) = f3(1) +// First derivative are equals, ie: +// f1'(0) = f2'(0) +// f2'(1) = f3'(1) +// Second derivative are equals, ie: +// f1''(0) = f2''(0) +// f2''(1) = f3''(0) +// Curve is, at boundary, with second derivative set to zero (it's a constraint introduced by Dersch), ie: +// f1''(-1) = 0 +// f3''(2) = 0 +// +// Then, you can solve for (a1,a2,a3,b1,b2,b3,c1,c2,c3,d1,d2,d3) +// +// for ex, for curve f2 you find : +// +// d2 = y1 // easy since y1 = f2(0) +// c2 = - 7/15 y0 - 1/5 y1 + 4/5 y2 - 2/15 y3 +// b2 = 4/5 y0 - 9/5 y1 + 6/5 y2 - 1/5 y3 +// a2 = - 1/3 y0 + y1 - y2 + 1/3 y3 +// +// +// When you have coefs, you just have to express your curve as a linear combination of the control points, fort ex +// with f2 : +// +// +// f2(x) = w0(x) * y0 + w1(x) + y1 + w2(x) * y2 + w3(x) * y3 +// +// with : +// +// w0(x) = - 1/3 * x^3 + 4/5 * x^2 - 7/15 * x +// w1(x) = x^3 - 9/5 * x^2 - 1/5 * x + 1 +// w2(x) = -x^3 + 6/5 * x^2 + 4/5 * x +// w3(x) = 1/3 * x^3 - 1/5 * x^2 - 2/15 * x +// +// substituting boundary conditions gives the correct coefficients for y0,y1,y2,y3 giving the final sampling scheme +template +struct Spline16 { + typedef TYPE Type; + + enum { halfWidth = 2 }; + enum { width = halfWidth*2 }; + + inline Spline16() {} + + inline void operator() (const TYPE x, TYPE* const weight) const { + weight[0] = ((TYPE(-1) / TYPE(3) * x + TYPE(4) / TYPE(5)) * x - TYPE(7) / TYPE(15)) * x; + weight[1] = ((x - TYPE(9) / TYPE(5)) * x - TYPE(1) / TYPE(5)) * x + TYPE(1); + weight[2] = ((TYPE(6) / TYPE(5) - x) * x + TYPE(4) / TYPE(5)) * x; + weight[3] = ((TYPE(1) / TYPE(3) * x - TYPE(1) / TYPE(5)) * x - TYPE(2) / TYPE(15)) * x; + } +}; + +// Sampler spline 36 +// Same as spline 16 but on 6 neighbors (used for 6x6 frame) +template +struct Spline36 { + typedef TYPE Type; + + enum { halfWidth = 3 }; + enum { width = halfWidth*2 }; + + inline Spline36() {} + + inline void operator() (const TYPE x, TYPE* const weight) const { + weight[0] = ((TYPE(1) / TYPE(11) * x - TYPE(45) / TYPE(209)) * x + TYPE(26) / TYPE(209)) * x; + weight[1] = ((TYPE(-6) / TYPE(11) * x + TYPE(270) / TYPE(209)) * x - TYPE(156) / TYPE(209)) * x; + weight[2] = ((TYPE(13) / TYPE(11) * x - TYPE(453) / TYPE(209)) * x - TYPE(3) / TYPE(209)) * x + TYPE(1); + weight[3] = ((TYPE(-13) / TYPE(11) * x + TYPE(288) / TYPE(209)) * x + TYPE(168) / TYPE(209)) * x; + weight[4] = ((TYPE(6) / TYPE(11) * x - TYPE(72) / TYPE(209)) * x - TYPE(42) / TYPE(209)) * x; + weight[5] = ((TYPE(-1) / TYPE(11) * x + TYPE(12) / TYPE(209)) * x + TYPE(7) / TYPE(209)) * x; + } +}; + +// Sampler spline 64 +// Same as spline 16 but on 8 neighbors (used for 8x8 frame) +template +struct Spline64 { + typedef TYPE Type; + + enum { halfWidth = 4 }; + enum { width = halfWidth*2 }; + + inline Spline64() {} + + inline void operator() (const TYPE x, TYPE* const weight) const { + weight[0] = ((TYPE(-1) / TYPE(41) * x + TYPE(168) / TYPE(2911)) * x - TYPE(97) / TYPE(2911)) * x; + weight[1] = ((TYPE(6) / TYPE(41) * x - TYPE(1008) / TYPE(2911)) * x + TYPE(582) / TYPE(2911)) * x; + weight[2] = ((TYPE(-24) / TYPE(41) * x + TYPE(4032) / TYPE(2911)) * x - TYPE(2328) / TYPE(2911)) * x; + weight[3] = ((TYPE(49) / TYPE(41) * x - TYPE(6387) / TYPE(2911)) * x - TYPE(3) / TYPE(2911)) * x + TYPE(1); + weight[4] = ((TYPE(-49) / TYPE(41) * x + TYPE(4050) / TYPE(2911)) * x + TYPE(2340) / TYPE(2911)) * x; + weight[5] = ((TYPE(24) / TYPE(41) * x - TYPE(1080) / TYPE(2911)) * x - TYPE(624) / TYPE(2911)) * x; + weight[6] = ((TYPE(-6) / TYPE(41) * x + TYPE(270) / TYPE(2911)) * x + TYPE(156) / TYPE(2911)) * x; + weight[7] = ((TYPE(1) / TYPE(41) * x - TYPE(45) / TYPE(2911)) * x - TYPE(26) / TYPE(2911)) * x; + } +}; + +// Sample image at a specified position +// @param image to be sampled +// @param sampler used to make the sampling +// @param pt X and Y-coordinate of sampling +// @return sampled value +template +inline TYPE Sample(const IMAGE& image, const SAMPLER& sampler, const POINT& pt) +{ + typedef typename SAMPLER::Type T; + + // integer position of sample (x,y) + const int grid_x(FLOOR2INT(pt.x)); + const int grid_y(FLOOR2INT(pt.y)); + + // compute difference between exact pixel location and sample + const T dx(pt.x-(T)grid_x); + const T dy(pt.y-(T)grid_y); + + // get sampler weights + T coefs_x[SAMPLER::width]; + sampler(dx, coefs_x); + T coefs_y[SAMPLER::width]; + sampler(dy, coefs_y); + + // Sample a grid around specified grid point + TYPE res(0); + for (int i = 0; i < SAMPLER::width; ++i) { + // get current i value + // +1 for correct scheme (draw it to be convinced) + const int cur_i(grid_y + 1 + i - SAMPLER::halfWidth); + // handle out of range + if (cur_i < 0 || cur_i >= image.rows) + continue; + for (int j = 0; j < SAMPLER::width; ++j) { + // get current j value + // +1 for the same reason + const int cur_j(grid_x + 1 + j - SAMPLER::halfWidth); + // handle out of range + if (cur_j < 0 || cur_j >= image.cols) + continue; + // sample input image and weight according to sampler + const T w = coefs_x[j] * coefs_y[i]; + const TYPE pixel = image(cur_i, cur_j); + res += pixel * w; + } + } + return res; +} + +} // namespace Sampler diff --git a/libs/Common/Semaphore.h b/libs/Common/Semaphore.h new file mode 100644 index 0000000..1281c51 --- /dev/null +++ b/libs/Common/Semaphore.h @@ -0,0 +1,170 @@ +//////////////////////////////////////////////////////////////////// +// Semaphore.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_SEMAPHORE_H__ +#define __SEACAVE_SEMAPHORE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Thread.h" + +#ifndef _MSC_VER +#ifdef _SUPPORT_CPP11 +#include +#include +#else +#include "CriticalSection.h" +#include +#endif +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class Semaphore +{ +#ifdef _MSC_VER +public: + Semaphore(unsigned count=0) { + h = CreateSemaphore(NULL, count, MAXLONG, NULL); + }; + ~Semaphore() { + CloseHandle(h); + }; + + void Clear(unsigned count=0) { + CloseHandle(h); + h = CreateSemaphore(NULL, count, MAXLONG, NULL); + } + + void Signal() { + ReleaseSemaphore(h, 1, NULL); + } + void Signal(unsigned count) { + ASSERT(count > 0); + ReleaseSemaphore(h, count, NULL); + } + + void Wait() { + WaitForSingleObject(h, INFINITE); + } + bool Wait(uint32_t millis) { + return WaitForSingleObject(h, millis) == WAIT_OBJECT_0; + } + +protected: + HANDLE h; + +#elif !defined(_SUPPORT_CPP11) +// pthread implementation +public: + Semaphore(unsigned c=0) : count(c) { pthread_cond_init(&cond, NULL); } + ~Semaphore() { pthread_cond_destroy(&cond); } + + void Clear(unsigned c=0) { + cs.Clear(); + pthread_cond_destroy(&cond); + pthread_cond_init(&cond, NULL); + count = c; + } + + void Signal() { + Lock l(cs); + ++count; + pthread_cond_signal(&cond); + } + void Signal(unsigned c) { + ASSERT(c > 0); + for (unsigned i=0; i lock{mtx}; + count = c; + } + + void Signal() { + std::lock_guard lock{mtx}; + ++count; + cv.notify_one(); + } + void Signal(unsigned c) { + ASSERT(c > 0); + for (unsigned i=0; i lock{mtx}; + cv.wait(lock, [&] { return count > 0; }); + --count; + } + bool Wait(uint32_t millis) { + std::unique_lock lock{mtx}; + if (!cv.wait_for(lock, std::chrono::milliseconds(millis), [&] { return count > 0; })) + return false; + --count; + return true; + } + +protected: + std::condition_variable cv; + std::mutex mtx; + unsigned count; +#endif + +private: + Semaphore(const Semaphore&); + Semaphore& operator=(const Semaphore&); +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_SEMAPHORE_H__ diff --git a/libs/Common/SharedPtr.h b/libs/Common/SharedPtr.h new file mode 100644 index 0000000..3488802 --- /dev/null +++ b/libs/Common/SharedPtr.h @@ -0,0 +1,209 @@ +//////////////////////////////////////////////////////////////////// +// SharedPtr.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_SHAREDPTR_H__ +#define __SEACAVE_SHAREDPTR_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +/************************************************************************************** + * CSharedPtr template + * --------------- + * shared smart pointer + **************************************************************************************/ + +template +struct SharedRef { + typedef TYPE TYPE_REF; + + #ifdef SEACAVE_NO_MULTITHREAD + TYPE_REF val; + #else + volatile TYPE_REF val; + #endif + + inline SharedRef() {} + inline SharedRef(TYPE_REF v) : val(v) {} + + inline TYPE_REF Inc() { + #ifdef SEACAVE_NO_MULTITHREAD + ASSERT(val >= 0); + return ++val; + #else + return Thread::safeInc(val); + #endif + } + inline TYPE_REF Dec() { + #ifdef SEACAVE_NO_MULTITHREAD + ASSERT(val > 0); + return --val; + #else + return Thread::safeDec(val); + #endif + } + + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + #ifdef SEACAVE_NO_MULTITHREAD + ar & val; + #else + ar & (TYPE_REF&)val; + #endif + } + #endif +}; +/*----------------------------------------------------------------*/ + + +template +class CSharedPtr +{ +protected: + typedef SharedRef TYPE_REF; + typedef TYPE* TYPE_PTR; + +public: + inline CSharedPtr() : m_pointer(NULL), m_pNoRef(NULL) + { // empty construct + } + + inline explicit CSharedPtr(TYPE_PTR _Ptr) : m_pointer(_Ptr), m_pNoRef(_Ptr ? new TYPE_REF(1) : NULL) + { // construct from object pointer + } + + inline CSharedPtr(const CSharedPtr& _Right) : m_pointer(_Right.m_pointer), m_pNoRef(_Right.m_pNoRef) + { // construct by assuming pointer from _Right CSharedPtr + IncRef(); + } + + inline ~CSharedPtr() + { // destroy the object + DecRef(); + } + + CSharedPtr& operator=(const CSharedPtr& _Right) + { // assign compatible _Right (assume pointer) + if (this != &_Right) + { + ASSERT(m_pointer != _Right.m_pointer || m_pNoRef == _Right.m_pNoRef); + DecRef(); + m_pointer = _Right.m_pointer; + m_pNoRef = _Right.m_pNoRef; + IncRef(); + } + return (*this); + } + + CSharedPtr& operator=(TYPE_PTR _Ptr) + { // assign compatible _Right (assume pointer) + if (m_pointer != _Ptr) + { + DecRef(); + m_pointer = _Ptr; + m_pNoRef = (_Ptr ? new TYPE_REF(1) : NULL); + } + return (*this); + } + + inline TYPE& operator*() const + { // return designated value + ASSERT(m_pointer); + return (*m_pointer); + } + + inline TYPE* operator->() const + { // return pointer to class object + ASSERT(m_pointer); + return m_pointer; + } + + inline operator TYPE_PTR() const + { // return pointer to class object + return m_pointer; + } + + inline bool operator==(const CSharedPtr& _Right) const + { // return pointer to class object + return (m_pointer == _Right.m_pointer); + } + + inline bool operator!=(const CSharedPtr& _Right) const + { // return pointer to class object + return (m_pointer != _Right.m_pointer); + } + + inline bool operator==(const void* _Right) const + { // return pointer to class object + return (m_pointer == _Right); + } + + inline bool operator!=(const void* _Right) const + { // return pointer to class object + return (m_pointer != _Right); + } + + void Release() + { // release pointer + DecRef(); + m_pointer = NULL; + m_pNoRef = NULL; + } + +protected: + inline void IncRef() + { + if (m_pointer == NULL) + return; + ASSERT(m_pNoRef); + m_pNoRef->Inc(); + } + + inline void DecRef() + { + if (m_pointer == NULL) + return; + ASSERT(m_pNoRef); + if (m_pNoRef->Dec() == 0) + { + delete m_pointer; + m_pointer = NULL; + delete m_pNoRef; + m_pNoRef = NULL; + } + } + + TYPE_PTR m_pointer; // the wrapped object pointer + TYPE_REF* m_pNoRef; // number of references to this pointer + +#ifdef _USE_BOOST +protected: + // implement BOOST serialization + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & m_pointer; + ar & m_pNoRef; + } +#endif +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_SHAREDPTR_H__ diff --git a/libs/Common/Sphere.h b/libs/Common/Sphere.h new file mode 100644 index 0000000..f5d79e5 --- /dev/null +++ b/libs/Common/Sphere.h @@ -0,0 +1,56 @@ +//////////////////////////////////////////////////////////////////// +// Sphere.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_SPHERE_H__ +#define __SEACAVE_SPHERE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// Basic sphere class +template +class TSphere +{ + STATIC_ASSERT(DIMS > 1 && DIMS <= 3); + +public: + typedef TYPE Type; + typedef Eigen::Matrix POINT; + + POINT center; // sphere center point + TYPE radius; // sphere radius + + //--------------------------------------- + + inline TSphere() {} + inline TSphere(const POINT& c, TYPE r) : center(c), radius(r) {} + inline TSphere(const POINT& p1, const POINT& p2, const POINT& p3); + + inline void Set(const POINT& c, TYPE r); + inline void Set(const POINT& p1, const POINT& p2, const POINT& p3); + + inline void Enlarge(TYPE); + inline void EnlargePercent(TYPE); + + inline GCLASS Classify(const POINT&) const; +}; // class TSphere +/*----------------------------------------------------------------*/ + +#include "Sphere.inl" +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_SPHERE_H__ diff --git a/libs/Common/Sphere.inl b/libs/Common/Sphere.inl new file mode 100644 index 0000000..6a46826 --- /dev/null +++ b/libs/Common/Sphere.inl @@ -0,0 +1,80 @@ +//////////////////////////////////////////////////////////////////// +// Sphere.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +template +inline TSphere::TSphere(const POINT& p1, const POINT& p2, const POINT& p3) +{ + Set(p1, p2, p3); +} // constructor +/*----------------------------------------------------------------*/ + + +template +inline void TSphere::Set(const POINT& c, TYPE r) +{ + center = c; + radius = r; +} +template +inline void TSphere::Set(const POINT& p1, const POINT& p2, const POINT& p3) +{ + // compute relative distances + TYPE A((p1 - p2).squaredNorm()); + TYPE B((p2 - p3).squaredNorm()); + TYPE C((p3 - p1).squaredNorm()); + + // re-orient triangle (make A longest side) + const POINT *a(&p3), *b(&p1), *c(&p2); + if (B < C) std::swap(B, C), std::swap(b, c); + if (A < B) std::swap(A, B), std::swap(a, b); + + // if obtuse + if (B + C <= A) { + // just use longest diameter + radius = SQRT(A) / TYPE(2); + center = (*b + *c) / TYPE(2); + } else { + // otherwise circumscribe (http://en.wikipedia.org/wiki/Circumscribed_circle) + const TYPE cos_a(SQUARE(B + C - A) / (B*C*TYPE(4))); + radius = SQRT(A / ((TYPE(1) - cos_a)*TYPE(4))); + const POINT alpha(*a - *c), beta(*b - *c); + const POINT alphaXbeta(alpha.cross(beta)); + center = (beta * alpha.squaredNorm() - alpha * beta.squaredNorm()).cross(alphaXbeta) / + (alphaXbeta.squaredNorm() * TYPE(2)) + *c; + } +} +/*----------------------------------------------------------------*/ + + +template +inline void TSphere::Enlarge(TYPE x) +{ + radius += x; +} +template +inline void TSphere::EnlargePercent(TYPE x) +{ + radius *= x; +} // Enlarge +/*----------------------------------------------------------------*/ + + +// Classify point to sphere. +template +inline GCLASS TSphere::Classify(const POINT& p) const +{ + if ((center - p).squaredNorm() > SQUARE(radius)) + return CULLED; + return VISIBLE; +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/Streams.h b/libs/Common/Streams.h new file mode 100644 index 0000000..3c02237 --- /dev/null +++ b/libs/Common/Streams.h @@ -0,0 +1,244 @@ +//////////////////////////////////////////////////////////////////// +// Streams.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_STREAMS_H__ +#define __SEACAVE_STREAMS_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "AutoPtr.h" + + +// D E F I N E S /////////////////////////////////////////////////// + +#define SIZE_NA ((size_f_t)-1) +#define STREAM_ERROR ((size_t)-1) + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class InputStream; +typedef InputStream ISTREAM; + +class OutputStream; +typedef OutputStream OSTREAM; + +class IOStream; +typedef IOStream IOSTREAM; + +class GENERAL_API NOINITVTABLE Stream { +public: + virtual ~Stream() { } + + // Identify stream type + virtual InputStream* getInputStream(int) { return NULL; } + virtual OutputStream* getOutputStream(int) { return NULL; } + virtual IOStream* getIOStream(int, int) { return NULL; } + + // Get the length of the stream. + // SIZE_NA if there were errors. + virtual size_f_t getSize() const = 0; + // Position the stream at the given offset from the beginning. + virtual bool setPos(size_f_t pos) = 0; + // Get the current position in the stream (offset from the beginning). + // SIZE_NA if there were errors. + virtual size_f_t getPos() const = 0; +}; + +class GENERAL_API NOINITVTABLE InputStream : public Stream { +public: + virtual ~InputStream() { } + /** + * Call this function until it returns 0 to get all bytes. + * @return The number of bytes read. len reflects the number of bytes + * actually read from the stream source in this call. + * STREAM_ERROR if there were errors. + */ + virtual size_t read(void* buf, size_t len) = 0; + + enum { LAYER_ID_IN=0 }; + InputStream* getInputStream(int typ=LAYER_ID_IN) override { return (typ == LAYER_ID_IN ? this : (InputStream*)NULL); } +}; + +class GENERAL_API NOINITVTABLE OutputStream : public Stream { +public: + virtual ~OutputStream() { } + + /** + * @return The actual number of bytes written. len bytes will always be + * consumed, but fewer or more bytes may actually be written, + * for example if the stream is being compressed. + * STREAM_ERROR if there were errors. + */ + virtual size_t write(const void* buf, size_t len) = 0; + inline size_t print(LPCTSTR szFormat, ...) { + va_list args; + va_start(args, szFormat); + TCHAR szBuffer[2048]; + const size_t len((size_t)_vsntprintf(szBuffer, 2048, szFormat, args)); + if (len > 2048) { + const size_t count((size_t)_vsctprintf(szFormat, args)); + ASSERT(count != (size_t)-1); + CAutoPtrArr szBufferDyn(new TCHAR[count+1]); + _vsntprintf(szBufferDyn, count+1, szFormat, args); + va_end(args); + return write(szBufferDyn, count); + } + va_end(args); + return write(szBuffer, len); + } + inline size_t print(const std::string& str) { + return write(str.c_str(), str.length()); + } + template + inline OutputStream& operator<<(const T& val) { + std::ostringstream ostr; + ostr << val; + print(ostr.str()); + return *this; + } + /** + * This must be called before destroying the object to make sure all data + * is properly written. Note that some implementations + * might not need it... + * + * @return The actual number of bytes written. + * STREAM_ERROR if there were errors. + */ + virtual size_t flush() = 0; + + enum { LAYER_ID_OUT=0 }; + OutputStream* getOutputStream(int typ=LAYER_ID_OUT) override { return (typ == LAYER_ID_OUT ? this : (OutputStream*)NULL); } +}; + +class GENERAL_API NOINITVTABLE IOStream : public InputStream, public OutputStream { +public: + InputStream* getInputStream(int typ=LAYER_ID_IN) override { return InputStream::getInputStream(typ); } + OutputStream* getOutputStream(int typ=LAYER_ID_OUT) override { return OutputStream::getOutputStream(typ); } + IOStream* getIOStream(int typIn=LAYER_ID_IN, int typOut=LAYER_ID_OUT) override { + return ((InputStream*)this)->getInputStream(typIn) != NULL && + ((OutputStream*)this)->getOutputStream(typOut) != NULL ? this : (IOStream*)NULL; + } +}; +/*----------------------------------------------------------------*/ + + + +template +class LayerInputStream : public InputStream { +public: + LayerInputStream(InputStream* aStream) : s(aStream) { ASSERT(s != NULL); } + virtual ~LayerInputStream() noexcept { if (managed) delete s; } + + InputStream* getInputStream(int typ=InputStream::LAYER_ID_IN) override { return s->getInputStream(typ); } + + size_t read(void* wbuf, size_t len) override { + return s->read(wbuf, len); + } + + size_f_t getSize() const override { + return s->getSize(); + } + + bool setPos(size_f_t wpos) override { + return s->setPos(wpos); + } + + size_f_t getPos() const override { + return s->getPos(); + } + +protected: + InputStream* const s; +}; + +template +class LayerOutputStream : public OutputStream { +public: + LayerOutputStream(OutputStream* aStream) : s(aStream) {} + virtual ~LayerOutputStream() noexcept { if (managed) delete s; } + + OutputStream* getOutputStream(int typ=OutputStream::LAYER_ID_OUT) override { return s->getOutputStream(typ); } + + size_t write(const void* buf, size_t len) override { + return s->write(buf, len); + } + + size_t flush() override { + return s->flush(); + } + + size_f_t getSize() const override { + return s->getSize(); + } + + bool setPos(size_f_t wpos) override { + return s->setPos(wpos); + } + + size_f_t getPos() const override { + return s->getPos(); + } + +protected: + OutputStream* const s; +}; +/*----------------------------------------------------------------*/ + + + +template +class LayerIOStream : public IOStream { +public: + LayerIOStream(InputStream* aStream) : s(aStream), sIn(aStream), sOut(NULL) { ASSERT(aStream != NULL); } + LayerIOStream(OutputStream* aStream) : s(aStream), sIn(NULL), sOut(aStream) { ASSERT(aStream != NULL); } + LayerIOStream(InputStream* streamIn, OutputStream* streamOut) : s(NULL), sIn(streamIn), sOut(streamOut) { ASSERT(sIn != NULL || sOut != NULL); } + virtual ~LayerIOStream() noexcept { if (managed) { delete sIn; delete sOut; } } + + InputStream* getInputStream(int typ=InputStream::LAYER_ID_IN) override { return (sIn ? sIn->getInputStream(typ) : NULL); } + OutputStream* getOutputStream(int typ=OutputStream::LAYER_ID_OUT) override { return (sOut ? sOut->getOutputStream(typ) : NULL); } + + size_f_t getSize() const override { + return s->getSize(); + } + + bool setPos(size_f_t wpos) override { + return s->setPos(wpos); + } + + size_f_t getPos() const override { + return s->getPos(); + } + + size_t read(void* wbuf, size_t len) override { + return sIn->read(wbuf, len); + } + + size_t write(const void* buf, size_t len) override { + return sOut->write(buf, len); + } + + size_t flush() override { + return sOut->flush(); + } + + operator InputStream* () const { return sIn; } + operator OutputStream* () const { return sOut; } + +protected: + Stream* const s; + InputStream* const sIn; + OutputStream* const sOut; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_STREAMS_H__ diff --git a/libs/Common/Strings.h b/libs/Common/Strings.h new file mode 100644 index 0000000..aae49c8 --- /dev/null +++ b/libs/Common/Strings.h @@ -0,0 +1,239 @@ +//////////////////////////////////////////////////////////////////// +// Strings.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_STRING_H__ +#define __SEACAVE_STRING_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Streams.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +/// String class: enhanced std::string +class GENERAL_API String : public std::string +{ +public: + typedef std::string Base; + +public: + inline String() {} + inline String(LPCTSTR sz) : Base(sz) {} + inline String(const Base& str) : Base(str) {} + inline String(size_t n, value_type v) : Base(n, v) {} + inline String(LPCTSTR sz, size_t count) : Base(sz, count) {} + inline String(LPCTSTR sz, size_t offset, size_t count) : Base(sz, offset, count) {} + #ifdef _SUPPORT_CPP11 + inline String(Base&& rhs) : Base(std::forward(rhs)) {} + inline String(String&& rhs) : Base(std::forward(rhs)) {} + inline String(const String& rhs) : Base(rhs) {} + + inline String& operator=(Base&& rhs) { Base::operator=(std::forward(rhs)); return *this; } + inline String& operator=(String&& rhs) { Base::operator=(std::forward(rhs)); return *this; } + #endif + inline String& operator=(TCHAR rhs) { Base::operator=(rhs); return *this; } + inline String& operator=(LPCTSTR rhs) { Base::operator=(rhs); return *this; } + inline String& operator=(const String& rhs) { Base::operator=(rhs); return *this; } + + inline String& operator+=(TCHAR rhs) { *this = (*this) + rhs; return *this; } + inline String& operator+=(LPCTSTR rhs) { *this = (*this) + rhs; return *this; } + inline String& operator+=(const Base& rhs) { *this = (*this) + rhs; return *this; } + inline String& operator+=(const String& rhs) { *this = (*this) + rhs; return *this; } + + inline void Release() { return clear(); } + inline bool IsEmpty() const { return empty(); } + + inline operator LPCTSTR() const { return c_str(); } + + String& Format(LPCTSTR szFormat, ...) { + va_list args; + va_start(args, szFormat); + TCHAR szBuffer[2048]; + const size_t len((size_t)_vsntprintf(szBuffer, 2048, szFormat, args)); + if (len > 2048) { + *this = FormatStringSafe(szFormat, args); + va_end(args); + } else { + va_end(args); + this->assign(szBuffer, len); + } + return *this; + } + String& FormatSafe(LPCTSTR szFormat, ...) { + va_list args; + va_start(args, szFormat); + const size_t len((size_t)_vsctprintf(szFormat, args)); + ASSERT(len != (size_t)-1); + TCHAR* szBuffer(new TCHAR[len]); + _vsntprintf(szBuffer, len, szFormat, args); + va_end(args); + this->assign(szBuffer, len); + delete[] szBuffer; + return *this; + } + static String FormatString(LPCTSTR szFormat, ...) { + va_list args; + va_start(args, szFormat); + TCHAR szBuffer[2048]; + const size_t len((size_t)_vsntprintf(szBuffer, 2048, szFormat, args)); + if (len > 2048) { + const String str(FormatStringSafe(szFormat, args)); + va_end(args); + return str; + } + va_end(args); + return String(szBuffer, len); + } + static inline String FormatStringSafe(LPCTSTR szFormat, va_list args) { + const size_t len((size_t)_vsctprintf(szFormat, args)); + ASSERT(len != (size_t)-1); + TCHAR* szBuffer(new TCHAR[len]); + _vsntprintf(szBuffer, len, szFormat, args); + String str(szBuffer, len); + delete[] szBuffer; + return str; + } + + inline void ToUpper(String& out) const { + out.resize(size()); + std::transform(begin(), end(), out.begin(), [](TCHAR c) { return (TCHAR)std::toupper(c); }); + } + inline String ToUpper() const { + String str; + ToUpper(str); + return str; + } + + inline void ToLower(String& out) const { + out.resize(size()); + std::transform(begin(), end(), out.begin(), [](TCHAR c) { return (TCHAR)std::tolower(c); }); + } + inline String ToLower() const { + String str; + ToLower(str); + return str; + } + + inline void Save(OSTREAM& oStream) const { + const WORD nSize = (WORD)size(); + oStream.write(&nSize, sizeof(WORD)); + oStream.write(c_str(), nSize); + } + inline void Load(ISTREAM& oStream) { + WORD nSize; + oStream.read(&nSize, sizeof(WORD)); + if (nSize == 0) { + clear(); + return; + } + char* pBuffer = new char[nSize]; + oStream.read(pBuffer, nSize); + assign(pBuffer, nSize); + delete[] pBuffer; + } + + template + static String ToString(const T& val) { + std::ostringstream os; + os << val; + return os.str(); + } + template + static String ToStringHex(const T& val) { + std::ostringstream os; + os << std::hex << val; + return os.str(); + } + + template + static void FromString(const String& str, T& val) { + std::istringstream is(str); + is >> val; + } + template + static T FromString(const String& str, const T& def) { + T val(def); + FromString(str, val); + return val; + } + template + static T FromString(const String& str) { + T val; + FromString(str, val); + return val; + } + template + inline void From(T& val) const { + FromString(*this, val); + } + template + inline T From(const T& def) const { + T val(def); + From(val); + return val; + } + template + inline T From() const { + T val; + From(val); + return val; + } + + static int CompareAlphabetically(const void* elem, const void* key) { return _tcscmp(((const String*)elem)->c_str(), ((const String*)key)->c_str()); } + static int CompareAlphabeticallyInv(const void* elem, const void* key) { return _tcscmp(((const String*)key)->c_str(), ((const String*)elem)->c_str()); } + +#ifdef _USE_BOOST +protected: + // implement BOOST serialization + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object(*this); + } +#endif +}; +/*----------------------------------------------------------------*/ + +inline String operator+(const String& lhs, TCHAR rhs) { return std::operator+(lhs, rhs); } +inline String operator+(const String& lhs, LPCTSTR rhs) { return std::operator+(lhs, rhs); } +inline String operator+(const String& lhs, const std::string& rhs) { return std::operator+(lhs, rhs); } +inline String operator+(TCHAR lhs, const String& rhs) { return std::operator+(lhs, rhs); } +inline String operator+(LPCTSTR lhs, const String& rhs) { return std::operator+(lhs, rhs); } +inline String operator+(const std::string& lhs, const String& rhs) { return std::operator+(lhs, rhs); } +inline String operator+(const String& lhs, const String& rhs) { return std::operator+(lhs, rhs); } +#ifdef _SUPPORT_CPP11 +inline String operator+(String&& lhs, TCHAR rhs) { return std::operator+(std::forward(lhs), rhs); } +inline String operator+(String&& lhs, LPCTSTR rhs) { return std::operator+(std::forward(lhs), rhs); } +inline String operator+(String&& lhs, const std::string& rhs) { return std::operator+(std::forward(lhs), rhs); } +inline String operator+(TCHAR lhs, String&& rhs) { return std::operator+(lhs, std::forward(rhs)); } +inline String operator+(LPCTSTR lhs, String&& rhs) { return std::operator+(lhs, std::forward(rhs)); } +inline String operator+(const std::string& lhs, String&& rhs) { return std::operator+(lhs, std::forward(rhs)); } +inline String operator+(const String& lhs, String&& rhs) { return std::operator+(lhs, std::forward(rhs)); } +inline String operator+(String&& lhs, const String& rhs) { return std::operator+(std::forward(lhs), rhs); } +inline String operator+(String&& lhs, String&& rhs) { return std::operator+(std::forward(lhs), std::forward(rhs)); } +#endif +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + + +namespace std { +//namespace tr1 { +// Specializations for unordered containers +template <> struct hash : public hash{}; +//} // namespace tr1 +template <> struct equal_to : public equal_to{}; +} // namespace std + +#endif // __SEACAVE_STRING_H__ diff --git a/libs/Common/Thread.h b/libs/Common/Thread.h new file mode 100644 index 0000000..9ccacb3 --- /dev/null +++ b/libs/Common/Thread.h @@ -0,0 +1,435 @@ +//////////////////////////////////////////////////////////////////// +// Thread.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_THREAD_H__ +#define __SEACAVE_THREAD_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#ifdef _MSC_VER +#include +#ifdef _SUPPORT_CPP11 +#include +#else +#include +#endif +#else +#include +#include +#include +#include +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +/************************************************************************************** + * Thread + * -------------- + * basic thread control + **************************************************************************************/ + +class GENERAL_API Thread +{ +public: + #ifdef _ENVIRONMENT64 + typedef int64_t safe_t; + #else + typedef int32_t safe_t; + #endif + + enum Priority { + IDLE = -2, + LOW = -1, + NORMAL = 0, + HIGH = 1 + }; + + #ifdef _MSC_VER + + typedef HANDLE thread_t; + typedef void* (STCALL *FncStart)(void*); + + typedef struct STARTER_DATA { + FncStart fnStarter; + void* pData; + STARTER_DATA(FncStart _fnStarter, void* _pData) : fnStarter(_fnStarter), pData(_pData) {} + } StarterData; + + Thread() : threadHandle(NULL), threadId(0) {} + virtual ~Thread() { stop(); } + + inline bool isRunning() const { return (threadHandle != NULL); } + + bool start(FncStart pfnStarter, void* pData=NULL) { + join(); + return ((threadHandle = CreateThread(NULL, 0, &starterConvertor, new StarterData(pfnStarter, pData), 0, &threadId)) != NULL); + } + inline bool start() { + return start(&starter, this); + } + void stop() { + if (threadHandle) { + TerminateThread(threadHandle, -1); + CloseHandle(threadHandle); + threadHandle = NULL; + threadId = 0; + } + } + void join() { + if (threadHandle == NULL) + return; + WaitForSingleObject(threadHandle, INFINITE); + CloseHandle(threadHandle); + threadHandle = NULL; + threadId = 0; + } + + inline void setThreadPriority(Priority p) const { ::SetThreadPriority(threadHandle, convertPriority(p)); } + inline Priority getThreadPriority() const { return convertPriority((PriorityOS)::GetThreadPriority(threadHandle)); } + static inline void setThreadPriority(thread_t th, Priority p) { ::SetThreadPriority(th, convertPriority(p)); } + static inline Priority getThreadPriority(thread_t th) { return convertPriority((PriorityOS)::GetThreadPriority(th)); } + + static inline void sleep(uint32_t millis) { ::Sleep(millis); } + static inline void yield() { ::Sleep(0); } + static inline thread_t currentThread() { return ::GetCurrentThread(); } + static uint32_t hardwareConcurrency() { + SYSTEM_INFO info={{0}}; + GetSystemInfo(&info); + return info.dwNumberOfProcessors; + } + + STATIC_ASSERT(sizeof(int32_t)==sizeof(LONG)); + static inline int32_t safeInc(volatile int32_t& v) { return InterlockedIncrement((volatile LONG*)&v); }; + static inline int32_t safeDec(volatile int32_t& v) { return InterlockedDecrement((volatile LONG*)&v); }; + static inline int32_t safeExchange(volatile int32_t& target, int32_t value) { return InterlockedExchange((volatile LONG*)&target, value); }; + static inline int32_t safeCompareExchange(volatile int32_t& target, int32_t comp, int32_t value) { return InterlockedCompareExchange((volatile LONG*)&target, value, comp); }; + + STATIC_ASSERT(sizeof(int64_t)==sizeof(LONGLONG)); + static inline int64_t safeInc(volatile int64_t& v) { return InterlockedIncrement64((volatile LONGLONG*)&v); }; + static inline int64_t safeDec(volatile int64_t& v) { return InterlockedDecrement64((volatile LONGLONG*)&v); }; + static inline int64_t safeExchange(volatile int64_t& target, int64_t value) { return InterlockedExchange64((volatile LONGLONG*)&target, value); }; + static inline int64_t safeCompareExchange(volatile int64_t& target, int64_t comp, int64_t value) { return InterlockedCompareExchange64((volatile LONGLONG*)&target, value, comp); }; + + #else //_MSC_VER + + typedef pthread_t thread_t; + typedef void* (STCALL *FncStart)(void*); + + Thread() : threadHandle(0) {} + virtual ~Thread() { stop(); } + + inline bool isRunning() const { return (threadHandle != 0); } + + bool start(FncStart pfnStarter, void* pData=NULL) { + join(); + return (pthread_create(&threadHandle, NULL, pfnStarter, pData) == 0); + } + bool start() { + return start(&starter, this); + } + void stop() { + if (threadHandle != 0) { + pthread_detach(threadHandle); + threadHandle = 0; + } + } + void join() { + if (threadHandle) { + pthread_join(threadHandle, 0); + threadHandle = 0; + } + } + + inline void setThreadPriority(Priority p) const { setThreadPriority(threadHandle, p); } + inline Priority getThreadPriority() const { return getThreadPriority(threadHandle); } + static void setThreadPriority(thread_t th, Priority p) { + struct sched_param param; + param.sched_priority = convertPriority(p); + pthread_setschedparam(th, SCHED_OTHER, ¶m); + } + static Priority getThreadPriority(thread_t th) { + struct sched_param param; + int policy; + pthread_getschedparam(th, &policy, ¶m); + return convertPriority((PriorityOS)param.sched_priority); + } + + static void sleep(uint32_t millis) { ::usleep(millis*1000); } + static void yield() { ::sched_yield(); } + static inline thread_t currentThread(){ return pthread_self(); } + static uint32_t hardwareConcurrency() { return sysconf(_SC_NPROCESSORS_ONLN); } + + static inline int32_t safeInc(volatile int32_t& v) { return __sync_add_and_fetch(&v, 1); } + static inline int32_t safeDec(volatile int32_t& v) { return __sync_sub_and_fetch(&v, 1); } + static inline int32_t safeExchange(volatile int32_t& target, int32_t value) { return __sync_val_compare_and_swap(&target, target, value); } + static inline int32_t safeCompareExchange(volatile int32_t& target, int32_t comp, int32_t value) { return __sync_val_compare_and_swap(&target, comp, value); } + + static inline int64_t safeInc(volatile int64_t& v) { return __sync_add_and_fetch(&v, 1); } + static inline int64_t safeDec(volatile int64_t& v) { return __sync_sub_and_fetch(&v, 1); } + static inline int64_t safeExchange(volatile int64_t& target, int64_t value) { return __sync_val_compare_and_swap(&target, target, value); } + static inline int64_t safeCompareExchange(volatile int64_t& target, int64_t comp, int64_t value) { return __sync_val_compare_and_swap(&target, comp, value); } + + #endif //_MSC_VER + + static unsigned getMaxThreads(unsigned threads) { + if (threads == 1) + return 1; + const unsigned maxThreads = hardwareConcurrency(); + if (threads > 0 && threads < maxThreads) + return threads; + return maxThreads; + } + +protected: + virtual void run() {} + +protected: + #ifdef _MSC_VER + + enum PriorityOS { + OS_IDLE = THREAD_PRIORITY_IDLE, + OS_LOW = THREAD_PRIORITY_BELOW_NORMAL, + OS_NORMAL = THREAD_PRIORITY_NORMAL, + OS_HIGH = THREAD_PRIORITY_ABOVE_NORMAL + }; + + static DWORD WINAPI starterConvertor(void* p) + { + CAutoPtr pData((StarterData*)p); + return (DWORD)reinterpret_cast(pData->fnStarter(pData->pData)); + } + + #else //_MSC_VER + + enum PriorityOS { + OS_IDLE = 19, + OS_LOW = 10, + OS_NORMAL = 0, + OS_HIGH = -10 + }; + + #endif //_MSC_VER + + static inline PriorityOS convertPriority(Priority p) { + switch (p) { + case IDLE: return OS_IDLE; + case LOW: return OS_LOW; + case NORMAL: return OS_NORMAL; + case HIGH: return OS_HIGH; + } + return OS_NORMAL; + } + static inline Priority convertPriority(PriorityOS p) { + switch (p) { + case OS_IDLE: return IDLE; + case OS_LOW: return LOW; + case OS_NORMAL: return NORMAL; + case OS_HIGH: return HIGH; + } + return NORMAL; + } + + static void* starter(void* p) + { + ((Thread*)p)->run(); + return NULL; + } + +protected: + thread_t threadHandle; + #ifdef _MSC_VER + DWORD threadId; + #endif +}; +/*----------------------------------------------------------------*/ + + + +/************************************************************************************** + * ThreadPool + * -------------- + * basic thread pool + **************************************************************************************/ + +class GENERAL_API ThreadPool +{ +public: + typedef uint32_t size_type; + typedef Thread value_type; + typedef value_type* iterator; + typedef const value_type* const_iterator; + typedef value_type& reference; + typedef const value_type& const_reference; + typedef CLISTDEFIDX(Thread,size_type) Threads; + +public: + inline ThreadPool() {} + inline ThreadPool(size_type nThreads) : _threads(nThreads>0?nThreads:Thread::hardwareConcurrency()) {} + inline ThreadPool(size_type nThreads, Thread::FncStart pfnStarter, void* pData=NULL) : _threads(nThreads>0?nThreads:Thread::hardwareConcurrency()) { start(pfnStarter, pData); } + inline ~ThreadPool() { join(); } + + #ifdef _SUPPORT_CPP11 + inline ThreadPool(ThreadPool&& rhs) : _threads(std::forward(rhs._threads)) { + } + + inline ThreadPool& operator=(ThreadPool&& rhs) { + _threads.Swap(rhs._threads); + return *this; + } + #endif + + // wait for all running threads to finish and resize threads array + void resize(size_type nThreads) { + join(); + _threads.resize(nThreads); + } + // start all threads with the given function + bool start(Thread::FncStart pfnStarter, void* pData=NULL) { + for (Thread& thread: _threads) + if (!thread.start(pfnStarter, pData)) + return false; + return true; + } + // wait for all running threads to finish + void join() { + for (Thread& thread: _threads) + thread.join(); + } + // stop all threads + void stop() { + for (Thread& thread: _threads) + thread.stop(); + } + // wait for all running threads to finish and release threads array + void Release() { + join(); + _threads.Release(); + } + + inline bool empty() const { return _threads.empty(); } + inline size_type size() const { return _threads.size(); } + inline const_iterator cbegin() const { return _threads.cbegin(); } + inline const_iterator cend() const { return _threads.cend(); } + inline const_iterator begin() const { return _threads.begin(); } + inline const_iterator end() const { return _threads.end(); } + inline iterator begin() { return _threads.begin(); } + inline iterator end() { return _threads.end(); } + inline const_reference operator[](size_type index) const { return _threads[index]; } + inline reference operator[](size_type index) { return _threads[index]; } + +protected: + Threads _threads; +}; +/*----------------------------------------------------------------*/ + + + +/************************************************************************************** + * Process + * -------------- + * basic process control + **************************************************************************************/ + +class GENERAL_API Process +{ +public: + enum Priority { + IDLE = -3, + LOW = -2, + BELOWNORMAL = -1, + NORMAL = 0, + ABOVENORMAL = 1, + HIGH = 2, + REALTIME = 3 + }; + + #ifdef _MSC_VER + + typedef HANDLE process_t; + + static inline process_t getCurrentProcessID() { return ::GetCurrentProcess(); } + static inline void setProcessPriority(process_t id, Priority p) { ::SetPriorityClass(id, convertPriority(p)); } + static inline Priority getProcessPriority(process_t id) { return convertPriority((PriorityOS)::GetPriorityClass(id)); } + + #else //_MSC_VER + + typedef id_t process_t; + + static inline process_t getCurrentProcessID() { return ::getpid(); } + static inline void setProcessPriority(process_t id, Priority p) { ::setpriority(PRIO_PROCESS, id, convertPriority(p)); } + static inline Priority getProcessPriority(process_t id) { return convertPriority((PriorityOS)::getpriority(PRIO_PROCESS, id)); } + + #endif //_MSC_VER + + static inline void setCurrentProcessPriority(Priority p) { setProcessPriority(getCurrentProcessID(), p); } + static inline Priority getCurrentProcessPriority() { return getProcessPriority(getCurrentProcessID()); } + +protected: + + #ifdef _MSC_VER + + enum PriorityOS { + OS_IDLE = IDLE_PRIORITY_CLASS, + OS_LOW = PROCESS_MODE_BACKGROUND_BEGIN, + OS_BELOWNORMAL = BELOW_NORMAL_PRIORITY_CLASS, + OS_NORMAL = NORMAL_PRIORITY_CLASS, + OS_ABOVENORMAL = ABOVE_NORMAL_PRIORITY_CLASS, + OS_HIGH = HIGH_PRIORITY_CLASS, + OS_REALTIME = REALTIME_PRIORITY_CLASS + }; + + #else //_MSC_VER + + enum PriorityOS { + OS_IDLE = 19, + OS_LOW = 15, + OS_BELOWNORMAL = 10, + OS_NORMAL = 0, + OS_ABOVENORMAL = -10, + OS_HIGH = -15, + OS_REALTIME = -20 + }; + + #endif //_MSC_VER + + static inline PriorityOS convertPriority(Priority p) { + switch (p) { + case IDLE: return OS_IDLE; + case LOW: return OS_LOW; + case BELOWNORMAL: return OS_BELOWNORMAL; + case NORMAL: return OS_NORMAL; + case ABOVENORMAL: return OS_ABOVENORMAL; + case HIGH: return OS_HIGH; + case REALTIME: return OS_REALTIME; + } + return OS_NORMAL; + } + static inline Priority convertPriority(PriorityOS p) { + switch (p) { + case OS_IDLE: return IDLE; + case OS_LOW: return LOW; + case OS_BELOWNORMAL: return BELOWNORMAL; + case OS_NORMAL: return NORMAL; + case OS_ABOVENORMAL: return ABOVENORMAL; + case OS_HIGH: return HIGH; + case OS_REALTIME: return REALTIME; + } + return NORMAL; + } +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_THREAD_H__ diff --git a/libs/Common/Timer.cpp b/libs/Common/Timer.cpp new file mode 100644 index 0000000..56e6af0 --- /dev/null +++ b/libs/Common/Timer.cpp @@ -0,0 +1,165 @@ +//////////////////////////////////////////////////////////////////// +// Timer.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "Timer.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +/** + * Constructor + */ +Timer::Timer(uint8_t nHH, uint8_t nMM) +{ + m_fElapsedTime = 1.f; + m_fTimeScale = 1.f; + #ifdef FIX_FPS + m_fCurrentFramesTime = 0.f; + m_nCurrentFrames = 0; + m_nLastSecFrames = 0; + #endif // FIX_FPS + + // set clock + m_fClock = 0.f; + if (nHH > 23) + nHH = 0; + if (nMM > 59) + nMM = 0; + m_nHH = nHH; + m_nMM = nMM; + m_nSS = 0; + + m_nCrntTime = GetSysTime(); +} // constructor +/*----------------------------------------------------------------*/ + + +Timer::~Timer() +{ +} +/*----------------------------------------------------------------*/ + + +/** + * Counts the actual time and adjusts clock. + */ +void Timer::Update() +{ + const SysType timeNow = GetSysTime(); + m_fElapsedTime = SysTime2Time(timeNow - m_nCrntTime); + m_nCrntTime = timeNow; + + #ifdef FIX_FPS + // compute FPS + ++m_nCurrentFrames; + if ((m_fCurrentFramesTime += m_fElapsedTime) >= 1.f) { + m_nLastSecFrames = (uint32_t)((Type)m_nCurrentFrames/m_fCurrentFramesTime); + m_fCurrentFramesTime = 0.f; + m_nCurrentFrames = 0; + } + #endif // FIX_FPS + + // adjust clock by seconds passed + m_fClock += (m_fElapsedTime * m_fTimeScale); + if (m_fClock >= 1.f) { + m_nSS++; + m_fClock = 0.f; + } + if (m_nSS >= 60) { + m_nMM++; + m_nSS = 0; + } + if (m_nMM >= 60) { + m_nHH++; + m_nMM = 0; + } + if (m_nHH >= 24) + m_nHH = 0; +} // Update +/*----------------------------------------------------------------*/ + + +/** + * Sets the clock to any given starting time. + */ +void Timer::SetClock(uint8_t nHH, uint8_t nMM) +{ + // set clock + if (nHH > 23) + nHH = 0; + if (nMM > 59) + nMM = 0; + + m_nHH = nHH; + m_nMM = nMM; +} // SetClock +/*----------------------------------------------------------------*/ + + +/** + * Gives you a time string with hours, minutes and seconds as return + * and hours and/or minutes as reference parameters. + */ +LPTSTR Timer::GetClock(uint8_t* nHH, uint8_t* nMM, LPTSTR szChar) const +{ + if (nHH != NULL) + *nHH = m_nHH; + if (nMM != NULL) + *nMM = m_nMM; + + if (szChar == NULL) + return NULL; + + _sntprintf(szChar, 32, "%.2d:%.2d:%.2d", m_nHH, m_nMM, m_nSS); + return szChar; +} // GetClock +/*----------------------------------------------------------------*/ + + +#ifdef TIMER_OLDSUPPORT +// Check performance counter +bool HasPerfCounter() +{ + SysType nPerfCnt; + return QueryPerformanceFrequency((LARGE_INTEGER*)&nPerfCnt) == TRUE; +} +const bool Timer::ms_bPerfFlag = HasPerfCounter(); +/*----------------------------------------------------------------*/ +#endif + + +// Get system time factor used to convert to milliseconds +Timer::Type GetSysTimeFactor() +{ + #ifdef _MSC_VER + Timer::SysType nPerfCnt; + const BOOL bPerfFlag = QueryPerformanceFrequency((LARGE_INTEGER*)&nPerfCnt); + #ifdef TIMER_OLDSUPPORT + // either QueryPerformanceCounter or GetTickCount + return (bPerfFlag ? 1000.f / nPerfCnt : 1.f); + #else + ASSERT(bPerfFlag); + return 1000.f / nPerfCnt; + #endif + #else // _MSC_VER + return 0.001f; + #endif // _MSC_VER +} +const Timer::Type Timer::ms_fTimeFactor = GetSysTimeFactor(); +/*----------------------------------------------------------------*/ + +Timer::Type Timer::GetTimeFactor() +{ + return ms_fTimeFactor; +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/Timer.h b/libs/Common/Timer.h new file mode 100644 index 0000000..8caac8d --- /dev/null +++ b/libs/Common/Timer.h @@ -0,0 +1,141 @@ +//////////////////////////////////////////////////////////////////// +// Timer.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_TIMER_H__ +#define __SEACAVE_TIMER_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + +#ifdef _MSC_VER +//#define TIMER_OLDSUPPORT +#endif +#define FIX_FPS + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class GENERAL_API Timer +{ +public: + typedef int64_t SysType; + typedef float Type; + +public: + Timer(uint8_t nHH=0, uint8_t nMM=0); + ~Timer(); + + void Update(); + + void SetClock(uint8_t nHH, uint8_t nMM); + LPTSTR GetClock(uint8_t* nHH, uint8_t* nMM, LPTSTR szChar) const; + + #ifdef FIX_FPS + inline uint32_t GetFPS() const { return m_nLastSecFrames; } + #else + inline uint32_t GetFPS() const { return (uint32_t)(1.f / m_fElapsedTime); } + #endif // FIX_FPS + + inline Type GetRealCurrent() const { return SysTime2Time(m_nCrntTime); } + inline Type GetCurrent() const { return m_fTimeScale * SysTime2Time(m_nCrntTime); } + inline Type GetRealElapsed() const { return m_fElapsedTime; } + inline Type GetElapsed() const { return m_fTimeScale * m_fElapsedTime; } + inline Type GetDelay(Type d) const { return m_fTimeScale * d; } + inline Type GetScale() const { return m_fTimeScale; } + inline void SetScale(Type fFactor) { m_fTimeScale = fFactor; } + +private: + SysType m_nCrntTime; // current time + Type m_fElapsedTime; // time elapsed since previous frame + Type m_fTimeScale; // slowdown or speedup + uint8_t m_nHH; // clock time hours + uint8_t m_nMM; // clock time minutes + uint8_t m_nSS; // clock time seconds + Type m_fClock; // sum up milliseconds + #ifdef FIX_FPS + Type m_fCurrentFramesTime; // time elapsed for the current frames + uint32_t m_nCurrentFrames; // counter of the current frames + uint32_t m_nLastSecFrames; // counter of the frames in the last second + #endif // FIX_FPS + +public: + // get current time in system units + static inline SysType GetSysTime() { + #ifdef _MSC_VER + #ifdef TIMER_OLDSUPPORT + if (!ms_bPerfFlag) + return GetTickCount(); + #endif + SysType nTime; + QueryPerformanceCounter((LARGE_INTEGER*)&nTime); + return nTime; + #else + timeval tv; + gettimeofday(&tv, NULL); + return (tv.tv_sec * 1000000) + tv.tv_usec; + #endif // _MSC_VER + } + // get milliseconds scaling factor for time + static Type GetTimeFactor(); + // get current time in milliseconds + static inline Type GetTimeMs() { + return GetTimeFactor() * GetSysTime(); + } + // get current time in seconds + static inline Type GetTime() { + return 0.001f * GetTimeFactor() * GetSysTime(); + } + // convert given time to milliseconds + static inline Type SysTime2TimeMs(SysType t) { + return GetTimeFactor() * t; + } + // convert given time to seconds + static inline Type SysTime2Time(SysType t) { + return 0.001f * GetTimeFactor() * t; + } + +protected: + #ifdef TIMER_OLDSUPPORT + static const bool ms_bPerfFlag; // flag for timer to use + #endif + static const Type ms_fTimeFactor; // milliseconds scaling factor for time +}; +/*----------------------------------------------------------------*/ + + +class GENERAL_API AutoTimer +{ +public: + typedef Timer::Type Type; +public: + AutoTimer(Type& duration) : m_duration(duration) { m_duration = Timer::GetTime(); } + ~AutoTimer() { m_duration = Timer::GetTime() - m_duration; } +protected: + Type& m_duration; +}; + +class GENERAL_API AutoAddTimer +{ +public: + typedef Timer::Type Type; +public: + AutoAddTimer(Type& duration) : m_duration(duration) { m_lastTime = Timer::GetTime(); } + ~AutoAddTimer() { m_duration += Timer::GetTime() - m_lastTime; } +protected: + Type& m_duration; + Type m_lastTime; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_TIMER_H__ diff --git a/libs/Common/Types.cpp b/libs/Common/Types.cpp new file mode 100644 index 0000000..7d7b8c9 --- /dev/null +++ b/libs/Common/Types.cpp @@ -0,0 +1,78 @@ +//////////////////////////////////////////////////////////////////// +// Types.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "Types.h" + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + + +// G L O B A L S /////////////////////////////////////////////////// + +#ifndef _MSC_VER +int _vscprintf(LPCSTR format, va_list pargs) { + va_list argcopy; + va_copy(argcopy, pargs); + const int retval(vsnprintf(NULL, 0, format, argcopy)); + va_end(argcopy); + return retval; +} +#endif +/*----------------------------------------------------------------*/ + + +namespace SEACAVE { + +const ColorType::value_type ColorType::ONE(255); +const ColorType::alt_type ColorType::ALTONE(1.f); + +const ColorType::value_type ColorType::ONE(255); +const ColorType::alt_type ColorType::ALTONE(1.f); + +const ColorType::value_type ColorType::ONE(1.f); +const ColorType::alt_type ColorType::ALTONE(255); + +const ColorType::value_type ColorType::ONE(1.0); +const ColorType::alt_type ColorType::ALTONE(255); +/*----------------------------------------------------------------*/ + + +// print matrix +template +String cvMat2String(const TYPE* M, uint32_t rows, uint32_t cols, uint32_t step, LPCSTR format) { + String str; + char buf[32]; + if (step == 0) + step = rows; + for (uint32_t i=0; i(), (uint32_t)M.rows, (uint32_t)M.cols, (uint32_t)M.step1(), format); + case CV_64F: return cvMat2String(M.ptr(), (uint32_t)M.rows, (uint32_t)M.cols, (uint32_t)M.step1(), format); + } + return String(); +} +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + + +// C L A S S ////////////////////////////////////////////////////// + diff --git a/libs/Common/Types.h b/libs/Common/Types.h new file mode 100644 index 0000000..9339f26 --- /dev/null +++ b/libs/Common/Types.h @@ -0,0 +1,2825 @@ +//////////////////////////////////////////////////////////////////// +// Types.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_TYPES_H__ +#define __SEACAVE_TYPES_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#ifdef _MSC_VER +#include +#include +#else +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif +#ifdef _SUPPORT_CPP11 +#ifdef __clang__ +#include +#else +#include +#endif +#include +#include +#include +#else +#include +#endif +#ifdef _SUPPORT_CPP17 +#if !defined(__GNUC__) || (__GNUC__ > 7) +#include +#endif +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef _USE_OPENMP +#include +#endif + +// Function delegate functionality +#ifdef _SUPPORT_CPP11 +#include "FastDelegateCPP11.h" +#define DELEGATE fastdelegate::delegate +#define DELEGATEBIND(DLGT, FNC) DLGT::from< FNC >() +#define DELEGATEBINDCLASS(DLGT, FNC, OBJ) DLGT::from(*OBJ, FNC) +#else +#include "FastDelegate.h" +#include "FastDelegateBind.h" +#define DELEGATE fastdelegate::FastDelegate +#define DELEGATEBIND(DLGT, FNC) fastdelegate::bind(FNC) +#define DELEGATEBINDCLASS(DLGT, FNC, OBJ) fastdelegate::bind(FNC, OBJ) +#endif + +// include usual boost libraries +#ifdef _USE_BOOST +#if 1 +// disable exception support +#define BOOST_NO_UNREACHABLE_RETURN_DETECTION +#define BOOST_EXCEPTION_DISABLE +#define BOOST_NO_EXCEPTIONS +#endif +#ifdef BOOST_NO_EXCEPTIONS +#include +#endif +#define BOOST_NO_UNREACHABLE_RETURN_DETECTION +// include headers that implement serialization support +#include +#include +#include +#include +#include +#include +#include +#include +#if (BOOST_VERSION / 100000) > 1 || (BOOST_VERSION / 100 % 1000) > 55 +#include +#include +#endif +#include +// include headers that define an input and output archive +#include +#include +// include headers that define memory pools +#include +#include +#endif + +#ifdef _USE_EIGEN +#if defined(_MSC_VER) +#pragma warning (push) +#pragma warning (disable : 4244) // 'argument': conversion from '__int64' to 'int', possible loss of data +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(_MSC_VER) +#pragma warning (pop) +#endif +#endif + +#pragma push_macro("free") +#undef free +#pragma push_macro("DEBUG") +#undef DEBUG +#include +#if CV_MAJOR_VERSION > 2 || CV_MINOR_VERSION > 3 +#include +#endif +#include +#ifdef HAVE_OPENCV_GPU +#if CV_MAJOR_VERSION > 2 +#include +namespace cv { namespace gpu = cuda; } +#else +#include +#endif +#endif +#pragma pop_macro("DEBUG") +#pragma pop_macro("free") + +#ifdef _USE_SSE +#include +#include +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + +// In order to create a singleton class +// add this define in your class; +#define DECLARE_SINGLETON(SClass) \ + protected: SClass(); SClass(const SClass&); \ + public: static inline SClass& GetInstance() { static SClass instance; return instance; } +// or add this define and declare GetInstance() method in the .cpp +// so that the singleton is unique even across modules; +#define DEFINE_SINGLETON(SClass) \ + protected: SClass(); SClass(const SClass&); \ + public: static SClass& GetInstance(); +// or add this declare ms_pInstance; +#define DECLARE_SINGLETONPTR(SClass) \ + SClass* SClass::ms_pInstance(NULL); \ + SClass*& SClass::GetInstanceRef() { return ms_pInstance; } \ + SClass* SClass::GetInstancePtr() { ASSERT(ms_pInstance); return ms_pInstance; } \ + SClass& SClass::GetInstance() { ASSERT(ms_pInstance); return *ms_pInstance; } +// and this define in the class definition. +#define DEFINE_SINGLETONPTR(SClass) \ + protected: SClass(); SClass(const SClass&); static SClass* ms_pInstance; \ + public: static SClass*& GetInstanceRef(); \ + public: static SClass* GetInstancePtr(); \ + public: static SClass& GetInstance(); + +#ifndef CALL_MEMBER_FN +#define CALL_MEMBER_FN(object,ptrToMember) ((object).*(ptrToMember)) +#endif + +#ifndef __PROCESS__ +# ifdef _MSC_VER +# define __PROCESS__ ((unsigned)GetCurrentProcessId()) +# else +# define __PROCESS__ ((unsigned)getpid()) +# endif +#endif + +#ifndef __THREAD__ +# ifdef _MSC_VER +# define __THREAD__ ((unsigned)GetCurrentThreadId()) +# elif defined(__APPLE__) +# include +inline pid_t GetCurrentThreadId() { uint64_t tid64; pthread_threadid_np(NULL, &tid64); return (pid_t)tid64; } +# define __THREAD__ ((unsigned)GetCurrentThreadId()) +# else +# include +# define __THREAD__ ((unsigned)((pid_t)syscall(SYS_gettid))) +# endif +#endif + +#ifndef STCALL +# if defined(_MSC_VER) +# define STCALL __cdecl +# elif defined(__OS2__) +# if defined (__GNUC__) && __GNUC__ < 4 +# define STCALL _cdecl +# else +# /* On other compilers on OS/2, we use the _System calling convention */ +# /* to be compatible with every compiler */ +# define STCALL _System +# endif +# elif defined(__GNUC__) +# define STCALL __attribute__((__cdecl__)) +# else +# define STCALL +# endif +#endif // STCALL + + +// T Y P E D E F I N E S ///////////////////////////////////////// + +// +// Type defines + +#ifndef _MSC_VER +typedef int32_t HRESULT; + +typedef unsigned char BYTE; +typedef unsigned short WORD; +typedef unsigned int DWORD; +typedef uint64_t QWORD; + +typedef char CHAR; +typedef CHAR* LPSTR; +typedef const CHAR* LPCSTR; +typedef CHAR TCHAR; +typedef LPSTR LPTSTR; +typedef LPCSTR LPCTSTR; + +#define _tcslen strlen +#define _tcscpy strcpy +#define _tcsncpy strncpy +#define _tcschr strchr +#define _tcsrchr strrchr +#define _tcscmp strcmp +#define _tcsncmp strncmp +#define _tcsicmp strcasecmp +#define _tcsnicmp strncasecmp +#define _tcsncicmp(s1,s2,s) strncasecmp(s1, s2, (s)*sizeof(TCHAR)) +#define _stprintf sprintf +#define _sntprintf snprintf +#define _vsntprintf vsnprintf +#define _vsctprintf _vscprintf + +int _vscprintf(LPCSTR format, va_list pargs); + +#define _T(s) s +#endif //_MSC_VER + +#define DECLARE_NO_INDEX(...) std::numeric_limits<__VA_ARGS__>::max() + +#ifndef MAKEWORD +#define MAKEWORD(a, b) ((WORD)(((BYTE)(((DWORD)(a)) & 0xff)) | ((WORD)((BYTE)(((DWORD)(b)) & 0xff))) << 8)) +#endif +#ifndef MAKELONG +#define MAKELONG(a, b) ((DWORD)(((WORD)(((DWORD)(a)) & 0xffff)) | ((DWORD)((WORD)(((DWORD)(b)) & 0xffff))) << 16)) +#endif +#ifndef LOWORD +#define LOWORD(l) ((WORD)(((DWORD)(l)) & 0xffff)) +#endif +#ifndef HIWORD +#define HIWORD(l) ((WORD)((((DWORD)(l)) >> 16) & 0xffff)) +#endif +#ifndef LOBYTE +#define LOBYTE(w) ((BYTE)(((WORD)(w)) & 0xff)) +#endif +#ifndef HIBYTE +#define HIBYTE(w) ((BYTE)((((WORD)(w)) >> 8) & 0xff)) +#endif + +#ifndef MAX_PATH +#define MAX_PATH 260 +#endif + +#ifndef NULL +#define NULL 0 +#endif + +#ifdef max +#undef max +#endif +#ifdef min +#undef min +#endif + +#ifndef MINF +#define MINF std::min +#endif +#ifndef MAXF +#define MAXF std::max +#endif + +#ifndef RAND +#define RAND std::rand +#endif + +namespace SEACAVE { + +// signed and unsigned types of the size of the architecture +// (32 or 64 bit for x86 and respectively x64) +#ifdef _ENVIRONMENT64 +typedef int64_t int_t; +typedef uint64_t uint_t; +#else +typedef int32_t int_t; +typedef uint32_t uint_t; +#endif + +// type used for the size of the files +typedef int64_t size_f_t; + +// type used as the default floating number precision +typedef double REAL; + +// invalid index +constexpr uint32_t NO_ID = DECLARE_NO_INDEX(uint32_t); + +template +struct RealType { typedef typename std::conditional::value, TYPE, REALTYPE>::type type; }; + +template +inline T MINF3(const T& x1, const T& x2, const T& x3) { + return MINF(MINF(x1, x2), x3); +} +template +inline T MAXF3(const T& x1, const T& x2, const T& x3) { + return MAXF(MAXF(x1, x2), x3); +} + +template +FORCEINLINE T RANDOM() { return T(RAND())/RAND_MAX; } + +template +union TAliasCast { + T1 f; + T2 i; + inline TAliasCast() {} + inline TAliasCast(T1 v) : f(v) {} + inline TAliasCast(T2 v) : i(v) {} + inline TAliasCast& operator = (T1 v) { f = v; return *this; } + inline TAliasCast& operator = (T2 v) { i = v; return *this; } + inline operator T1 () const { return f; } +}; +typedef TAliasCast CastF2I; +typedef TAliasCast CastD2I; + +} // namespace SEACAVE + +#if defined(_MSC_VER) +# define __LITTLE_ENDIAN 0 +# define __BIG_ENDIAN 1 +# define __PDP_ENDIAN 2 +# define __BYTE_ORDER __LITTLE_ENDIAN +#elif defined(__APPLE__) +# include +#elif defined(__GNUC__) +# include +#endif + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Strings.h" +#include "AutoPtr.h" +#include "List.h" +#include "Thread.h" +#include "SharedPtr.h" +#include "Queue.h" +#include "Hash.h" +#include "Timer.h" +#include "CriticalSection.h" +#include "Semaphore.h" +#include "Util.h" +#include "File.h" +#include "MemFile.h" +#include "LinkLib.h" + +namespace SEACAVE { + +typedef class GENERAL_API CSharedPtr FilePtr; + +typedef class GENERAL_API CSharedPtr ISTREAMPTR; +typedef ISTREAM* LPISTREAM; + +typedef class GENERAL_API CSharedPtr OSTREAMPTR; +typedef OSTREAM* LPOSTREAM; + +typedef class GENERAL_API CSharedPtr IOSTREAMPTR; +typedef IOSTREAM* LPIOSTREAM; + +typedef class GENERAL_API cList VoidArr; +typedef class GENERAL_API cList LPCTSTRArr; +typedef class GENERAL_API cList StringArr; +typedef class GENERAL_API cList IDXArr; +typedef class GENERAL_API cList Unsigned8Arr; +typedef class GENERAL_API cList UnsignedArr; +typedef class GENERAL_API cList Unsigned32Arr; +typedef class GENERAL_API cList Unsigned64Arr; +typedef class GENERAL_API cList SizeArr; +typedef class GENERAL_API cList IntArr; +typedef class GENERAL_API cList BoolArr; +typedef class GENERAL_API cList FloatArr; +typedef class GENERAL_API cList DoubleArr; + +} // namespace SEACAVE + +#include "Log.h" +#include "EventQueue.h" +#include "SML.h" +#include "ConfigTable.h" +#include "HTMLDoc.h" + + +// D E F I N E S /////////////////////////////////////////////////// + +// +// Constant defines + +// everything went smooth +#define _OK ((HRESULT)0L) + +// just reports no errors +#define _CANCEL 0x82000000 + +// general error message +#define _FAIL 0x82000001 + +// specific error messages +#define _CREATEAPI 0x82000002 +#define _CREATEDEVICE 0x82000003 +#define _CREATEBUFFER 0x82000004 +#define _INVALIDPARAM 0x82000005 +#define _INVALIDID 0x82000006 +#define _BUFFERSIZE 0x82000007 +#define _BUFFERLOCK 0x82000008 +#define _NOTCOMPATIBLE 0x82000009 +#define _OUTOFMEMORY 0x8200000a +#define _FILENOTFOUND 0x8200000b +#define _INVALIDFILE 0x8200000c +#define _NOSHADERSUPPORT 0x8200000d +#define _NOSERVERFOUND 0x8200000e +#define _WOULDBLOCK 0x8200000f + +#ifndef SUCCEEDED +#define SUCCEEDED(hr) (((HRESULT)(hr)) >= 0) +#endif +#ifndef FAILED +#define FAILED(hr) (((HRESULT)(hr)) < 0) +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + +#define RGBA(r, g, b, a) ((DWORD)(((a) << 24) | ((r) << 16) | ((g) << 8) | (b))) +#define RGBC(clr) (RGBA((BYTE)((clr).fR*255), (BYTE)((clr).fG*255), (BYTE)((clr).fB*255), (BYTE)((clr).fA*255))) +#define RGB24TO8(r,g,b) ((BYTE)((((WORD)r)*30+((WORD)g)*59+((WORD)b)*11)/100)) +#define RGB24TO16(r,g,b) ((((WORD)(((BYTE)(r))>>3))<<11) | (((WORD)(((BYTE)(g))>>2))<<5) | ((WORD)(((BYTE)(b))>>3))) +#define RGB16TOR(rgb) (((BYTE)(((WORD)(rgb))>>11))<<3) +#define RGB16TOG(rgb) (((BYTE)((((WORD)(rgb))&0x07E0)>>5))<<2) +#define RGB16TOB(rgb) (((BYTE)(((WORD)(rgb))&0x001F))<<3) + +#define TIMER_START() SEACAVE::Timer::SysType timerStart = SEACAVE::Timer::GetSysTime() +#define TIMER_UPDATE() timerStart = SEACAVE::Timer::GetSysTime() +#define TIMER_GET() SEACAVE::Timer::SysTime2TimeMs(SEACAVE::Timer::GetSysTime() - timerStart) +#define TIMER_GET_INT() ((SEACAVE::Timer::SysType)TIMER_GET()) +#define TIMER_GET_FORMAT() SEACAVE::Util::formatTime(TIMER_GET_INT()) + + +// D E F I N E S /////////////////////////////////////////////////// + +#ifndef CHECK +#define CHECK(exp) { if (!(exp)) { VERBOSE("Check failed: " #exp); abort(); } } +#endif +#ifndef ABORT +#define ABORT(msg) { VERBOSE("error: " #msg); exit(-1); } +#endif + +#ifndef _USE_MATH_DEFINES +/** e */ +#ifndef M_E +#define M_E 2.7182818284590452353602874713527 +#endif +/** ln(2) */ +#ifndef M_LN2 +#define M_LN2 0.69314718055994530941723212145818 +#endif +/** ln(10) */ +#ifndef M_LN10 +#define M_LN10 2.3025850929940456840179914546844 +#endif +/** pi */ +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif +/** pi/2 */ +#ifndef M_PI_2 +#define M_PI_2 1.5707963267948966192313216916398 +#endif +/** 1/pi */ +#ifndef M_1_PI +#define M_1_PI 0.31830988618379067153776752674503 +#endif +/** 2/pi */ +#ifndef M_2_PI +#define M_2_PI 0.63661977236758134307553505349006 +#endif +/** 2*sqrt(pi) */ +#ifndef M_2_SQRTPI +#define M_2_SQRTPI 1.1283791670955125738961589031216 +#endif +/** sqrt(2) */ +#ifndef M_SQRT2 +#define M_SQRT2 1.4142135623730950488016887242097 +#endif +/** sqrt(1/2) */ +#ifndef M_SQRT1_2 +#define M_SQRT1_2 0.70710678118654752440084436210485 +#endif +#endif + +// constants +#define TWO_PI 6.283185307179586476925286766559 +#define PI 3.1415926535897932384626433832795 +#define HALF_PI 1.5707963267948966192313216916398 +#define SQRT_2PI 2.506628274631000502415765284811 +#define INV_TWO_PI 0.15915494309189533576888376337251 +#define INV_PI 0.31830988618379067153776752674503 +#define INV_HALF_PI 0.63661977236758134307553505349006 +#define INV_SQRT_2PI 0.39894228040143267793994605993439 +#define D2R(d) ((d)*(PI/180.0)) // degree to radian +#define R2D(r) ((r)*(180.0/PI)) // radian to degree +#define SQRT_2 1.4142135623730950488016887242097 +#define SQRT_3 1.7320508075688772935274463415059 +#define LOG_2 0.30102999566398119521373889472449 +#define LN_2 0.69314718055994530941723212145818 +#define ZERO_TOLERANCE (1e-7) +#define INV_ZERO (1e+14) + +// float constants +#define FTWO_PI ((float)TWO_PI) +#define FPI ((float)PI) +#define FHALF_PI ((float)HALF_PI) +#define FSQRT_2PI ((float)SQRT_2PI) +#define FINV_TWO_PI ((float)INV_TWO_PI) +#define FINV_PI ((float)INV_PI) +#define FINV_HALF_PI ((float)INV_HALF_PI) +#define FINV_SQRT_2PI ((float)INV_SQRT_2PI) +#define FD2R(d) ((d)*(FPI/180.f)) // degree to radian +#define FR2D(r) ((r)*(180.f/FPI)) // radian to degree +#define FSQRT_2 ((float)SQRT_2) +#define FSQRT_3 ((float)SQRT_3) +#define FLOG_2 ((float)LOG_2) +#define FLN_2 ((float)LN_2) +#define FZERO_TOLERANCE 0.0001f +#define FINV_ZERO 1000000.f + +#define GCLASS unsigned +#define FRONT 0 +#define BACK 1 +#define PLANAR 2 +#define CLIPPED 3 +#define CULLED 4 +#define VISIBLE 5 + + +// M A C R O S ///////////////////////////////////////////////////// + +#define FLOOR SEACAVE::Floor2Int +#define FLOOR2INT SEACAVE::Floor2Int +#define CEIL SEACAVE::Ceil2Int +#define CEIL2INT SEACAVE::Ceil2Int +#define ROUND SEACAVE::Round2Int +#define ROUND2INT SEACAVE::Round2Int +#define SIN std::sin +#define ASIN std::asin +#define COS std::cos +#define ACOS std::acos +#define TAN std::tan +#define ATAN std::atan +#define ATAN2 std::atan2 +#define POW std::pow +#define POWI SEACAVE::powi +#define LOG2I SEACAVE::log2i + + +namespace SEACAVE { + +// F U N C T I O N S /////////////////////////////////////////////// + +template +struct MakeIdentity { using type = T; }; +template +using MakeSigned = typename std::conditional::value,std::make_signed,SEACAVE::MakeIdentity>::type; + +template +constexpr T1 Cast(const T2& v) { + return static_cast(v); +} + +template +constexpr T& NEGATE(T& a) { + return (a = -a); +} +template +constexpr T SQUARE(const T& a) { + return a * a; +} +template +constexpr T CUBE(const T& a) { + return a * a * a; +} +template +inline T SQRT(const T& a) { + return T(sqrt(a)); +} +template +inline T EXP(const T& a) { + return T(exp(a)); +} +template +inline T LOGN(const T& a) { + return T(log(a)); +} +template +inline T LOG10(const T& a) { + return T(log10(a)); +} +template +constexpr T powi(T base, unsigned exp) { + T result(1); + while (exp) { + if (exp & 1) + result *= base; + exp >>= 1; + base *= base; + } + return result; +} +constexpr int log2i(unsigned val) { + int ret = -1; + while (val) { + val >>= 1; + ++ret; + } + return ret; +} +template constexpr inline int log2i() { return 1+log2i<(N>>1)>(); } +template <> constexpr inline int log2i<0>() { return -1; } +template <> constexpr inline int log2i<1>() { return 0; } +template <> constexpr inline int log2i<2>() { return 1; } + +template +inline T arithmeticSeries(T n, T a1=1, T d=1) { + return (n*(a1*2+(n-1)*d))/2; +} +template +constexpr T factorial(T n) { + T ret = 1; + while (n > 1) + ret *= n--; + return ret; +} +template +constexpr T combinations(const T& n, const T& k) { + ASSERT(n >= k); + #if 1 + T num = n; + const T den = factorial(k); + for (T i=n-k+1; i +inline float FPOW2(float p) { + if (bSafe && p < -126.f) { + return 0.f; + } else { + ASSERT(p >= -126.f); + CastF2I v; + v.i = static_cast((1 << 23) * (p + 126.94269504f)); + return v.f; + } +} +template +inline float FEXP(float v) { + return FPOW2(1.44269504f * v); +} + +// Inverse of the square root +// Compute a fast 1 / sqrtf(v) approximation +inline float RSQRT(float v) { + #ifdef _FAST_INVSQRT + // This code supposedly originates from Id-software + const float halfV = v * 0.5f; + (int32_t&)v = 0x5f3759df - (((int32_t&)v) >> 1); + // Iterations of the Newton's method + v = v * (1.5f - halfV * v * v); + v = v * (1.5f - halfV * v * v); + return v * (1.5f - halfV * v * v); + #else + return 1.f / SQRT(v); + #endif +} +inline double RSQRT(const double& x) { + #ifdef _FAST_INVSQRT + double v = x; + const double halfV = v * 0.5; + (int64_t&)v = 0x5fe6ec85e7de30daLL - (((int64_t&)v) >> 1); + // Iterations of the Newton's method + v = v * (1.5 - halfV * v * v); + v = v * (1.5 - halfV * v * v); + v = v * (1.5 - halfV * v * v); + return v * (1.5 - halfV * v * v); + #else + return 1.0 / SQRT(x); + #endif +} + +// approximate tanh +template +inline T TANH(const T& x) { + const T x2 = x*x; + #if 0 + // Taylor series expansion (very inaccurate) + return x*(1.0 + x2*(-T(1)/T(3) + x2*(T(2)/T(15) + x2*(-T(17)/T(315) + x2*(T(62)/T(2835) - x2*(T(1382)/T(155925))))))); + #else + // Lambert's continued fraction + const T den = (((x2+T(378))*x2+T(17325))*x2+T(135135))*x; + const T div = ((x2*T(28)+T(3150))*x2+T(62370))*x2+T(135135); + return den/div; + #endif +} +/*----------------------------------------------------------------*/ + + +// Cubic root functions +// cube root approximation using bit hack for 32-bit float (5 decimals) +// (exploits the properties of IEEE 754 floating point numbers +// by leveraging the fact that their binary representation is close to a log2 representation) +inline float cbrt5(float x) { + #if 0 + CastF2I c(x); + c.i = ((c.i-(127<<23))/3+(127<<23)); + #else + TAliasCast c(x); + c.i = c.i/3 + 709921077u; + #endif + return c.f; +} +// cube root approximation using bit hack for 64-bit float +// adapted from Kahan's cbrt (5 decimals) +inline double cbrt5(double x) { + TAliasCast c(0.0), d(x); + c.i[1] = d.i[1]/3 + 715094163u; + return c.f; +} +// iterative cube root approximation using Halley's method +// faster convergence than Newton's method: (R/(a*a)+a*2)/3 +template +FORCEINLINE T cbrt_halley(const T& a, const T& R) { + const T a3 = a*a*a; + const T a3R = a3+R; + return a * (a3R + R) / (a3 + a3R); +} +// fast cubic root (variable precision) +template +FORCEINLINE T fast_cbrt(const T& x) { + return cbrt_halley(fast_cbrt(x), x); +} +template<> +FORCEINLINE double fast_cbrt(const double& x) { + return cbrt_halley((double)cbrt5((float)x), x); +} +template<> +FORCEINLINE float fast_cbrt(const float& x) { + return cbrt_halley(cbrt5(x), x); +} +// default cubic root function +FORCEINLINE float CBRT(float x) { + #ifdef _FAST_CBRT + return fast_cbrt(x); + #else + return POW(x, 1.0f/3.0f); + #endif +} +FORCEINLINE double CBRT(const double& x) { + #ifdef _FAST_CBRT + return fast_cbrt(x); + #else + return POW(x, 1.0/3.0); + #endif +} +/*----------------------------------------------------------------*/ + + +#if defined(__GNUC__) + +FORCEINLINE int PopCnt(uint32_t bb) { + return __builtin_popcount(bb); +} +FORCEINLINE int PopCnt(uint64_t bb) { + return __builtin_popcountll(bb); +} +FORCEINLINE int PopCnt15(uint64_t bb) { + return __builtin_popcountll(bb); +} +FORCEINLINE int PopCntSparse(uint64_t bb) { + return __builtin_popcountll(bb); +} + +#elif defined(_USE_SSE) && defined(_M_AMD64) // 64 bit windows + +FORCEINLINE int PopCnt(uint32_t bb) { + return (int)_mm_popcnt_u32(bb); +} +FORCEINLINE int PopCnt(uint64_t bb) { + return (int)_mm_popcnt_u64(bb); +} +FORCEINLINE int PopCnt15(uint64_t bb) { + return (int)_mm_popcnt_u64(bb); +} +FORCEINLINE int PopCntSparse(uint64_t bb) { + return (int)_mm_popcnt_u64(bb); +} + +#else + +// general purpose population count +template +constexpr int PopCnt(T bb) +{ + STATIC_ASSERT(std::is_integral::value && std::is_unsigned::value); + return std::bitset(bb).count(); +} +template<> +inline int PopCnt(uint64_t bb) { + const uint64_t k1 = (uint64_t)0x5555555555555555; + const uint64_t k2 = (uint64_t)0x3333333333333333; + const uint64_t k3 = (uint64_t)0x0F0F0F0F0F0F0F0F; + const uint64_t k4 = (uint64_t)0x0101010101010101; + bb -= (bb >> 1) & k1; + bb = (bb & k2) + ((bb >> 2) & k2); + bb = (bb + (bb >> 4)) & k3; + return (bb * k4) >> 56; +} +// faster version assuming not more than 15 bits set, used in mobility +// eval, posted on CCC forum by Marco Costalba of Stockfish team +inline int PopCnt15(uint64_t bb) { + unsigned w = unsigned(bb >> 32), v = unsigned(bb); + v -= (v >> 1) & 0x55555555; // 0-2 in 2 bits + w -= (w >> 1) & 0x55555555; + v = ((v >> 2) & 0x33333333) + (v & 0x33333333); // 0-4 in 4 bits + w = ((w >> 2) & 0x33333333) + (w & 0x33333333); + v += w; // 0-8 in 4 bits + v *= 0x11111111; + return int(v >> 28); +} +// version faster on sparsely populated bitboards +inline int PopCntSparse(uint64_t bb) { + int count = 0; + while (bb) { + count++; + bb &= bb - 1; + } + return count; +} + +#endif +/*----------------------------------------------------------------*/ + + +#ifdef _FAST_FLOAT2INT +// fast float to int conversion +// (xs routines at stereopsis: http://www.stereopsis.com/sree/fpu2006.html by Sree Kotay) +const double _float2int_doublemagic = 6755399441055744.0; //2^52 * 1.5, uses limited precision to floor +const double _float2int_doublemagicdelta = (1.5e-8); +const double _float2int_doublemagicroundeps = (.5f-_float2int_doublemagicdelta); //almost .5f = .5f - 1e^(number of exp bit) +FORCEINLINE int CRound2Int(const double& x) { + const CastD2I c(x + _float2int_doublemagic); + ASSERT(int32_t(floor(x+.5)) == c.i); + return c.i; +} +#endif +template +FORCEINLINE INTTYPE Floor2Int(float x) { + #ifdef _FAST_FLOAT2INT + return CRound2Int(double(x)-_float2int_doublemagicroundeps); + #else + return static_cast(floor(x)); + #endif +} +template +FORCEINLINE INTTYPE Floor2Int(double x) { + #ifdef _FAST_FLOAT2INT + return CRound2Int(x-_float2int_doublemagicroundeps); + #else + return static_cast(floor(x)); + #endif +} +template +FORCEINLINE INTTYPE Ceil2Int(float x) { + #ifdef _FAST_FLOAT2INT + return CRound2Int(double(x)+_float2int_doublemagicroundeps); + #else + return static_cast(ceil(x)); + #endif +} +template +FORCEINLINE INTTYPE Ceil2Int(double x) { + #ifdef _FAST_FLOAT2INT + return CRound2Int(x+_float2int_doublemagicroundeps); + #else + return static_cast(ceil(x)); + #endif +} +template +FORCEINLINE INTTYPE Round2Int(float x) { + #ifdef _FAST_FLOAT2INT + return CRound2Int(double(x)+_float2int_doublemagicdelta); + #else + return static_cast(floor(x+.5f)); + #endif +} +template +FORCEINLINE INTTYPE Round2Int(double x) { + #ifdef _FAST_FLOAT2INT + return CRound2Int(x+_float2int_doublemagicdelta); + #else + return static_cast(floor(x+.5)); + #endif +} +/*----------------------------------------------------------------*/ + + +// INTERPOLATION + +// Linear interpolation +inline float lerp(float u, float v, float x) +{ + return u + (v - u) * x; +} +template +inline Type lerp(const Type& u, const Type& v, float x) +{ + return u + (v - u) * x; +} + +// Cubic interpolation +inline float cerp(float u0, float u1, float u2, float u3, float x) +{ + const float p((u3 - u2) - (u0 - u1)); + const float q((u0 - u1) - p); + const float r(u2 - u0); + return x * (x * (x * p + q) + r) + u1; +} +template +inline Type cerp(const Type& u0, const Type& u1, const Type& u2, const Type& u3, float x) +{ + const Type p((u3 - u2) - (u0 - u1)); + const Type q((u0 - u1) - p); + const Type r(u2 - u0); + return x * (x * (x * p + q) + r) + u1; +} +/*----------------------------------------------------------------*/ + + +// S T R U C T S /////////////////////////////////////////////////// + +#ifdef _USE_SSE + +// define utile functions to deal with SSE operations + +struct ALIGN(16) sse_vec4f { + union { + float v[4]; + struct { + float x; + float y; + float z; + float w; + }; + }; + inline sse_vec4f() {} + inline sse_vec4f(const float* p) : x(p[0]), y(p[1]), z(p[2]), w(p[3]) {} + inline sse_vec4f(float f0, float f1, float f2, float f3) : x(f0), y(f1), z(f2), w(f3) {} + inline operator const float*() const {return v;} + inline operator float*() {return v;} +}; + +struct ALIGN(16) sse_vec2d { + union { + double v[2]; + struct { + double x; + double y; + }; + }; + inline sse_vec2d() {} + inline sse_vec2d(const double* p) : x(p[0]), y(p[1]) {} + inline sse_vec2d(const double& f0, const double& f1) : x(f0), y(f1) {} + inline operator const double*() const {return v;} + inline operator double*() {return v;} +}; + +struct sse_f_t { + typedef __m128 sse_t; + typedef const sse_t& arg_sse_t; + typedef float real_t; + inline sse_f_t() {} + inline sse_f_t(const sse_t& p) : v(p) {} + inline sse_f_t(real_t p) : v(load1(p)) {} + inline sse_f_t(const real_t* p) : v(load(p)) {} + inline sse_f_t(real_t f0, real_t f1, real_t f2, real_t f3) : v(set(f0,f1,f2,f3)) {} + inline operator sse_t() const {return v;} + inline operator sse_t&() {return v;} + inline sse_t operator ==(sse_t s) const {return cmpeq(v,s);} + inline sse_t operator =(sse_t s) {return v=s;} + inline sse_t operator +(sse_t s) const {return add(v,s);} + inline sse_t operator +=(sse_t s) {return v=add(v,s);} + inline sse_t operator -(sse_t s) const {return sub(v,s);} + inline sse_t operator -=(sse_t s) {return v=sub(v,s);} + inline sse_t operator *(sse_t s) const {return mul(v,s);} + inline sse_t operator *=(sse_t s) {return v=mul(v,s);} + inline sse_t operator /(sse_t s) const {return div(v,s);} + inline sse_t operator /=(sse_t s) {return v=div(v,s);} + inline void get(real_t* p) const {store(p,v);} + static inline sse_t zero() {return _mm_setzero_ps();} + static inline sse_t load1(real_t p) {return _mm_load1_ps(&p);} + static inline sse_t load(const real_t* p) {return _mm_load_ps(p);} + static inline sse_t loadu(const real_t* p) {return _mm_loadu_ps(p);} + static inline sse_t set(real_t f0, real_t f1, real_t f2, real_t f3) {return _mm_set_ps(f0,f1,f2,f3);} + static inline void store(real_t *p, sse_t s){_mm_store_ps(p,s);} + static inline void storeu(real_t *p, sse_t s){_mm_storeu_ps(p,s);} + static inline sse_t add(sse_t s1, sse_t s2) {return _mm_add_ps(s1,s2);} + static inline sse_t sub(sse_t s1, sse_t s2) {return _mm_sub_ps(s1,s2);} + static inline sse_t mul(sse_t s1, sse_t s2) {return _mm_mul_ps(s1,s2);} + static inline sse_t div(sse_t s1, sse_t s2) {return _mm_div_ps(s1,s2);} + static inline sse_t min(sse_t s1, sse_t s2) {return _mm_min_ps(s1,s2);} + static inline sse_t max(sse_t s1, sse_t s2) {return _mm_max_ps(s1,s2);} + static inline sse_t cmpeq(sse_t s1, sse_t s2){return _mm_cmpeq_ps(s1,s2);} + static inline sse_t sqrt(sse_t s) {return _mm_sqrt_ps(s);} + static inline sse_t rsqrt(sse_t s) {return _mm_rsqrt_ps(s);} + static inline int floor2int(real_t f) {return _mm_cvtt_ss2si(_mm_load_ss(&f));} + #ifdef _WIN32 + static inline real_t sum(sse_t s) {return (s.m128_f32[0]+s.m128_f32[2])+(s.m128_f32[1]+s.m128_f32[3]);} + static inline real_t sum3(sse_t s) {return (s.m128_f32[0]+s.m128_f32[2])+s.m128_f32[1];} + #else + static inline real_t sum(sse_t s) {real_t *f = (real_t*)(&s); return (f[0]+f[2])+(f[1]+f[3]);} + static inline real_t sum3(sse_t s) {real_t *f = (real_t*)(&s); return (f[0]+f[2])+f[1];} + #endif + /* + static inline real_t dot(sse_t s1, sse_t s2) { + sse_t temp = _mm_dp_ps(s1, s2, 0xF1); + real_t* f = (real_t*)(&temp); return f[0]; + } + */ + static real_t dot(const real_t* a, const real_t* b, size_t size) { + const real_t* const end = a+size; + const size_t iters = (size>>2); + real_t fres = 0.f; + if (iters) { + const real_t* const e = a+(iters<<2); + sse_t mres = zero(); + do { + mres = _mm_add_ps(mres, _mm_mul_ps(_mm_loadu_ps(a), _mm_loadu_ps(b))); + a += 4; b += 4; + } while (a < e); + fres = sum(mres); + } + while (a>1); + real_t fres = 0.0; + if (iters) { + const real_t* const e = a+(iters<<1); + sse_t mres = zero(); + do { + mres = _mm_add_pd(mres, _mm_mul_pd(_mm_loadu_pd(a), _mm_loadu_pd(b))); + a += 2; b += 2; + } while (a < e); + fres = sum(mres); + } + while (a +inline bool ISFINITE(const _Tp* x, size_t n) { for (size_t i=0; i +inline bool ISINSIDE(_Tp v,_Tp l0,_Tp l1) { ASSERT(l0 +inline bool ISINSIDES(_Tp v,_Tp l0,_Tp l1) { return l0 < l1 ? ISINSIDE(v, l0, l1) : ISINSIDE(v, l1, l0); } + +template +inline _Tp CLAMP(_Tp v, _Tp l0, _Tp l1) { ASSERT(l0<=l1); return MINF(MAXF(v, l0), l1); } +template +inline _Tp CLAMPS(_Tp v, _Tp l0, _Tp l1) { return l0 <= l1 ? CLAMP(v, l0, l1) : CLAMP(v, l1, l0); } + +template +inline _Tp SIGN(_Tp x) { if (x > _Tp(0)) return _Tp(1); if (x < _Tp(0)) return _Tp(-1); return _Tp(0); } + +template +inline _Tp ABS(_Tp x) { return std::abs(x); } + +template +constexpr _Tp ZEROTOLERANCE() { return _Tp(0); } +template<> +constexpr float ZEROTOLERANCE() { return FZERO_TOLERANCE; } +template<> +constexpr double ZEROTOLERANCE() { return ZERO_TOLERANCE; } + +template +constexpr _Tp EPSILONTOLERANCE() { return std::numeric_limits<_Tp>::epsilon(); } +template<> +constexpr float EPSILONTOLERANCE() { return 0.00001f; } +template<> +constexpr double EPSILONTOLERANCE() { return 1e-10; } + +inline bool ISZERO(float x) { return ABS(x) < FZERO_TOLERANCE; } +inline bool ISZERO(double x) { return ABS(x) < ZERO_TOLERANCE; } + +inline bool ISEQUAL(float x, float v) { return ABS(x-v) < FZERO_TOLERANCE; } +inline bool ISEQUAL(double x, double v) { return ABS(x-v) < ZERO_TOLERANCE; } + +inline float INVZERO(float) { return FINV_ZERO; } +inline double INVZERO(double) { return INV_ZERO; } +template +inline _Tp INVZERO(_Tp) { return std::numeric_limits<_Tp>::max(); } + +template +inline _Tp INVERT(_Tp x) { return (x==_Tp(0) ? INVZERO(x) : _Tp(1)/x); } + +template +inline _Tp SAFEDIVIDE(_Tp x, _Tp y) { return (y==_Tp(0) ? INVZERO(y) : x/y); } +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + + +#include "Random.h" +#include "HalfFloat.h" + + +namespace SEACAVE { + +// P R O T O T Y P E S ///////////////////////////////////////////// + +template class TMatrix; +template class TAABB; +template class TRay; +template class TPlane; + +// 2D point struct +template +class TPoint2 : public cv::Point_ +{ +public: + typedef TYPE Type; + typedef cv::Point_ Base; + typedef cv::Size Size; + typedef cv::Vec cvVec; + typedef cv::Matx Vec; + typedef cv::Matx VecT; + #ifdef _USE_EIGEN + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(TYPE,2) + typedef Eigen::Matrix EVec; + typedef Eigen::Map CEVecMap; + typedef Eigen::Map EVecMap; + #endif + + using Base::x; + using Base::y; + + static const TPoint2 ZERO; + static const TPoint2 INF; + +public: + inline TPoint2() {} + inline TPoint2(const Size& rhs) : Base(rhs) {} + template inline TPoint2(const cv::Point_& rhs) : Base((Type)rhs.x,(Type)rhs.y) {} + template inline TPoint2(const cv::Matx& rhs) : Base(rhs(0),rhs(1)) {} + template inline TPoint2(const cv::Matx& rhs) : Base(rhs(0),rhs(1)) {} + #ifdef _USE_EIGEN + inline TPoint2(const EVec& rhs) { operator EVecMap () = rhs; } + #endif + explicit inline TPoint2(const TYPE& _x) : Base(_x,_x) {} + inline TPoint2(const TYPE& _x, const TYPE& _y) : Base(_x,_y) {} + explicit inline TPoint2(const cv::Point3_& pt) : Base(pt.x/pt.z,pt.y/pt.z) {} + + template inline TPoint2& operator = (const cv::Point_& rhs) { Base::operator = (rhs); return *this; } + template inline TPoint2& operator = (const cv::Matx& rhs) { operator Vec& () = rhs; return *this; } + template inline TPoint2& operator = (const cv::Matx& rhs) { operator VecT& () = rhs; return *this; } + #ifdef _USE_EIGEN + inline TPoint2& operator = (const EVec& rhs) { operator EVecMap () = rhs; return *this; } + #endif + + // conversion to another data type + template inline operator TPoint2 () const { return TPoint2((T)x,(T)y); } + + // pointer to the first element access + inline const TYPE* ptr() const { return &x; } + inline TYPE* ptr() { return &x; } + + // 1D element access + inline const TYPE& operator [](size_t i) const { ASSERT(i>=0 && i<2); return ptr()[i]; } + inline TYPE& operator [](size_t i) { ASSERT(i>=0 && i<2); return ptr()[i]; } + + // Access point as Size equivalent + inline operator const Size& () const { return *((const Size*)this); } + inline operator Size& () { return *((Size*)this); } + + // Access point as vector equivalent + inline operator const Vec& () const { return *((const Vec*)this); } + inline operator Vec& () { return *((Vec*)this); } + + // Access point as transposed vector equivalent + inline operator const VecT& () const { return *((const VecT*)this); } + inline operator VecT& () { return *((VecT*)this); } + + #ifdef _USE_EIGEN + // Access point as Eigen equivalent + inline operator EVec () const { return CEVecMap((const TYPE*)this); } + // Access point as Eigen::Map equivalent + inline operator const CEVecMap () const { return CEVecMap((const TYPE*)this); } + inline operator EVecMap () { return EVecMap((TYPE*)this); } + #endif + + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object(*this); + } + #endif +}; +template const TPoint2 TPoint2::ZERO(0,0); +template const TPoint2 TPoint2::INF(std::numeric_limits::infinity(),std::numeric_limits::infinity()); +/*----------------------------------------------------------------*/ +typedef TPoint2 Point2i; +typedef TPoint2 Point2hf; +typedef TPoint2 Point2f; +typedef TPoint2 Point2d; +/*----------------------------------------------------------------*/ + + +// 3D point struct +template +class TPoint3 : public cv::Point3_ +{ +public: + typedef TYPE Type; + typedef cv::Point3_ Base; + typedef cv::Vec cvVec; + typedef cv::Matx Vec; + typedef cv::Matx VecT; + #ifdef _USE_EIGEN + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(TYPE,3) + typedef Eigen::Matrix EVec; + typedef Eigen::Map CEVecMap; + typedef Eigen::Map EVecMap; + #endif + + using Base::x; + using Base::y; + using Base::z; + + static const TPoint3 ZERO; + static const TPoint3 INF; + +public: + inline TPoint3() {} + template inline TPoint3(const cv::Point3_& rhs) : Base((Type)rhs.x,(Type)rhs.y,(Type)rhs.z) {} + template inline TPoint3(const cv::Matx& rhs) : Base(rhs(0),rhs(1),rhs(2)) {} + template inline TPoint3(const cv::Matx& rhs) : Base(rhs(0),rhs(1),rhs(2)) {} + #ifdef _USE_EIGEN + inline TPoint3(const EVec& rhs) { operator EVecMap () = rhs; } + #endif + explicit inline TPoint3(const TYPE& _x) : Base(_x,_x,_x) {} + inline TPoint3(const TYPE& _x, const TYPE& _y, const TYPE& _z) : Base(_x,_y,_z) {} + template inline TPoint3(const cv::Point_& pt, const T& _z=T(1)) : Base(pt.x,pt.y,_z) {} + template inline TPoint3(const cv::Point_& pt, const T2& _z) : Base(pt.x,pt.y,_z) {} + + template inline TPoint3& operator = (const cv::Point3_& rhs) { Base::operator = (rhs); return *this; } + template inline TPoint3& operator = (const cv::Matx& rhs) { operator Vec& () = rhs; return *this; } + template inline TPoint3& operator = (const cv::Matx& rhs) { operator VecT& () = rhs; return *this; } + #ifdef _USE_EIGEN + inline TPoint3& operator = (const EVec& rhs) { operator EVecMap () = rhs; return *this; } + #endif + + // conversion to another data type + template inline operator TPoint3 () const { return TPoint3((T)x,(T)y,(T)z); } + + // pointer to the first element access + inline const TYPE* ptr() const { return &x; } + inline TYPE* ptr() { return &x; } + + // 1D element access + inline const TYPE& operator [](BYTE i) const { ASSERT(i<3); return ptr()[i]; } + inline TYPE& operator [](BYTE i) { ASSERT(i<3); return ptr()[i]; } + + // Access point as vector equivalent + inline operator const Vec& () const { return *reinterpret_cast(this); } + inline operator Vec& () { return *reinterpret_cast(this); } + + // Access point as transposed vector equivalent + inline operator const VecT& () const { return *reinterpret_cast(this); } + inline operator VecT& () { return *reinterpret_cast(this); } + + #ifdef _USE_EIGEN + // Access point as Eigen equivalent + inline operator EVec () const { return CEVecMap((const TYPE*)this); } + // Access point as Eigen::Map equivalent + inline operator const EVecMap () const { return CEVecMap((const TYPE*)this); } + inline operator EVecMap () { return EVecMap((TYPE*)this); } + #endif + + // rotate point using the given parametrized rotation (axis-angle) + inline void RotateAngleAxis(const TPoint3& rot) { return (*this) = RotateAngleAxis((*this), rot); } + static TPoint3 RotateAngleAxis(const TPoint3& X, const TPoint3& rot); + + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object(*this); + } + #endif +}; +template const TPoint3 TPoint3::ZERO(0,0,0); +template const TPoint3 TPoint3::INF(std::numeric_limits::infinity(),std::numeric_limits::infinity(),std::numeric_limits::infinity()); +/*----------------------------------------------------------------*/ +typedef TPoint3 Point3i; +typedef TPoint3 Point3hf; +typedef TPoint3 Point3f; +typedef TPoint3 Point3d; +/*----------------------------------------------------------------*/ + + +// matrix struct +template +class TMatrix : public cv::Matx +{ +public: + typedef TYPE Type; + typedef cv::Matx Base; + typedef cv::Vec Vec; + #ifdef _USE_EIGEN + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(TYPE,m*n) + typedef Eigen::Matrix1?Eigen::RowMajor:Eigen::Default)> EMat; + typedef Eigen::Map CEMatMap; + typedef Eigen::Map EMatMap; + #endif + + using Base::val; + using Base::channels; + + enum { elems = m*n }; + + static const TMatrix ZERO; + static const TMatrix IDENTITY; + static const TMatrix INF; + +public: + inline TMatrix() {} + template inline TMatrix(const cv::Matx& rhs) : Base(rhs) {} + template inline TMatrix(const cv::Point_& rhs) : Base(rhs.x, rhs.y) {} + template inline TMatrix(const cv::Point3_& rhs) : Base(rhs.x, rhs.y, rhs.z) {} + inline TMatrix(const cv::Mat& rhs) : Base(rhs) {} + #ifdef _USE_EIGEN + inline TMatrix(const EMat& rhs) { operator EMatMap () = rhs; } + #endif + + TMatrix(TYPE v0); //!< 1x1 matrix + TMatrix(TYPE v0, TYPE v1); //!< 1x2 or 2x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2); //!< 1x3 or 3x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3); //!< 1x4, 2x2 or 4x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4); //!< 1x5 or 5x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5); //!< 1x6, 2x3, 3x2 or 6x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6); //!< 1x7 or 7x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7); //!< 1x8, 2x4, 4x2 or 8x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8); //!< 1x9, 3x3 or 9x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8, TYPE v9); //!< 1x10, 2x5 or 5x2 or 10x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, + TYPE v4, TYPE v5, TYPE v6, TYPE v7, + TYPE v8, TYPE v9, TYPE v10, TYPE v11); //!< 1x12, 2x6, 3x4, 4x3, 6x2 or 12x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, + TYPE v4, TYPE v5, TYPE v6, TYPE v7, + TYPE v8, TYPE v9, TYPE v10, TYPE v11, + TYPE v12, TYPE v13); //!< 1x14, 2x7, 7x2 or 14x1 matrix + TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, + TYPE v4, TYPE v5, TYPE v6, TYPE v7, + TYPE v8, TYPE v9, TYPE v10, TYPE v11, + TYPE v12, TYPE v13, TYPE v14, TYPE v15); //!< 1x16, 4x4 or 16x1 matrix + explicit TMatrix(const TYPE* vals); //!< initialize from a plain array + + TMatrix(const TMatrix& a, const TMatrix& b, cv::Matx_AddOp) : Base(a, b, cv::Matx_AddOp()) {} + TMatrix(const TMatrix& a, const TMatrix& b, cv::Matx_SubOp) : Base(a, b, cv::Matx_SubOp()) {} + template TMatrix(const TMatrix& a, TYPE2 alpha, cv::Matx_ScaleOp) : Base(a, alpha, cv::Matx_ScaleOp()) {} + TMatrix(const TMatrix& a, const TMatrix& b, cv::Matx_MulOp) : Base(a, b, cv::Matx_MulOp()) {} + TMatrix(const TMatrix& a, const TMatrix& b, cv::Matx_DivOp) : Base(a, b, cv::Matx_DivOp()) {} + template TMatrix(const TMatrix& a, const TMatrix& b, cv::Matx_MatMulOp) : Base(a, b, cv::Matx_MatMulOp()) {} + TMatrix(const TMatrix& a, cv::Matx_TOp) : Base(a, cv::Matx_TOp()) {} + + template inline TMatrix& operator = (const cv::Matx& rhs) { Base::operator = (rhs); return *this; } + inline TMatrix& operator = (const cv::Mat& rhs) { Base::operator = (rhs); return *this; } + #ifdef _USE_EIGEN + inline TMatrix& operator = (const EMat& rhs) { operator EMatMap () = rhs; return *this; } + #endif + + inline bool IsEqual(const Base&) const; + inline bool IsEqual(const Base&, TYPE eps) const; + + // 1D element access + inline const TYPE& operator [](size_t i) const { ASSERT(i(this); } + inline operator Vec& () { return *reinterpret_cast(this); } + + #ifdef _USE_EIGEN + // Access point as Eigen equivalent + inline operator EMat () const { return CEMatMap((const TYPE*)val); } + // Access point as Eigen::Map equivalent + inline operator CEMatMap() const { return CEMatMap((const TYPE*)val); } + inline operator EMatMap () { return EMatMap((TYPE*)val); } + #endif + + // calculate right null-space of this matrix ([n,n-m]) + inline TMatrix RightNullSpace(int flags = 0) const; + // calculate right/left null-vector of this matrix ([n/m,1]) + inline TMatrix RightNullVector(int flags = 0) const; + inline TMatrix LeftNullVector(int flags = 0) const; + + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object(*this); + } + #endif +}; +template const TMatrix TMatrix::ZERO(TMatrix::zeros()); +template const TMatrix TMatrix::IDENTITY(TMatrix::eye()); +template const TMatrix TMatrix::INF(TMatrix::all(std::numeric_limits::infinity())); +/*----------------------------------------------------------------*/ + + +// generic matrix struct +template +class TDMatrix : public cv::Mat_ +{ +public: + typedef TYPE Type; + typedef cv::Mat_ Base; + typedef cv::Size Size; + #ifdef _USE_EIGEN + typedef Eigen::Matrix EMat; + typedef Eigen::Map CEMatMap; + typedef Eigen::Map EMatMap; + #endif + + using Base::rows; + using Base::cols; + using Base::data; + using Base::step; + using Base::dims; + +public: + inline TDMatrix() {} + inline TDMatrix(const Base& rhs) : Base(rhs) {} + inline TDMatrix(const cv::Mat& rhs) : Base(rhs) {} + inline TDMatrix(const cv::MatExpr& rhs) : Base(rhs) {} + inline TDMatrix(int _rows, int _cols) : Base(_rows, _cols) {} + inline TDMatrix(int _rows, int _cols, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(_rows, _cols, _data, _step) {} + inline TDMatrix(const Size& sz) : Base(sz) {} + inline TDMatrix(const Size& sz, const TYPE& v) : Base(sz, v) {} + inline TDMatrix(const Size& sz, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(sz.height, sz.width, _data, _step) {} + #ifdef _USE_EIGEN + inline TDMatrix(const EMat& rhs) { operator EMatMap () = rhs; } + #endif + + inline TDMatrix& operator = (const Base& rhs) { Base::operator=(rhs); return *this; } + inline TDMatrix& operator = (const cv::MatExpr& rhs) { Base::operator=(rhs); return *this; } + #ifdef _USE_EIGEN + inline TDMatrix& operator = (const EMat& rhs) { operator EMatMap () = rhs; return *this; } + #endif + + /// Construct the 2D matrix with the desired size and init its elements + inline void construct(int _rows, int _cols) { + Base::create(_rows, _cols); + for (int i=0; i(i,j)) TYPE; + } + inline void construct(const Size& sz) { construct(sz.height, sz.width); } + inline void destroy() { + for (int i=0; i(i,j)->~TYPE(); + } + + /// Set all elements to the given value + inline void memset(uint8_t v) { ASSERT(dims == 2 && cv::Mat::isContinuous()); ::memset(data, v, row_stride()*rows); } + inline void fill(const TYPE& v) { ASSERT(dims == 2); for (int i=0; i(i,j) = v; } + + /// What is the row stride of the matrix? + inline size_t row_stride() const { ASSERT(dims == 2); return step[0]; } + /// What is the elem stride of the matrix? + inline size_t elem_stride() const { ASSERT(dims == 2 && step[1] == sizeof(TYPE)); return step[1]; } + /// Compute the area of the 2D matrix + inline int area() const { ASSERT(dims == 2); return cols*rows; } + + /// Is this coordinate inside the 2D matrix? + template + static inline typename std::enable_if::value,bool>::type isInside(const cv::Point_& pt, const cv::Size& size) { + return pt.x>=T(0) && pt.y>=T(0) && pt.x + static inline typename std::enable_if::value,bool>::type isInside(const cv::Point_& pt, const cv::Size& size) { + return pt.x>=T(0) && pt.y>=T(0) && pt.x<=T(size.width) && pt.y<=T(size.height); + } + template + inline bool isInside(const cv::Point_& pt) const { + return isInside(pt, Base::size()); + } + + /// Is this coordinate inside the 2D matrix, and not too close to the edges? + /// @param border: the size of the border + inline bool isInsideWithBorder(const Size& pt, int border) const { + return pt.width>=border && pt.height>=border && pt.width + inline typename std::enable_if::value,bool>::type isInsideWithBorder(const cv::Point_& pt, int border) const { + return pt.x>=border && pt.y>=border && pt.x + inline typename std::enable_if::value,bool>::type isInsideWithBorder(const cv::Point_& pt, int border) const { + return pt.x>=T(border) && pt.y>=T(border) && pt.x<=T(Base::size().width-(border+1)) && pt.y<=T(Base::size().height-(border+1)); + } + template + inline typename std::enable_if::value,bool>::type isInsideWithBorder(const cv::Point_& pt) const { + return pt.x>=border && pt.y>=border && pt.x + inline typename std::enable_if::value,bool>::type isInsideWithBorder(const cv::Point_& pt) const { + return pt.x>=T(border) && pt.y>=T(border) && pt.x<=T(Base::size().width-(border+1)) && pt.y<=T(Base::size().height-(border+1)); + } + + template + static inline void clip(TPoint2& ptMin, TPoint2& ptMax, const cv::Size& size) { + if (ptMin.x < T(border)) + ptMin.x = T(border); + if (ptMin.y < T(border)) + ptMin.y = T(border); + if (ptMax.x >= T(size.width-border)) + ptMax.x = T(size.width-(border+1)); + if (ptMax.y >= T(size.height-border)) + ptMax.y = T(size.height-(border+1)); + } + template + inline void clip(TPoint2& ptMin, TPoint2& ptMax) const { + clip(ptMin, ptMax, Base::size()); + } + + /// Remove the given element from the vector + inline void remove(int idx) { + // replace the removed element by the last one and decrease the size + ASSERT(rows == 1 || cols == 1); + const int last = area()-1; + Base::operator()(idx) = Base::operator()(last); + Base::resize((size_t)last); + } + + /** @author koeser + @warning very slow, generic implementation (order "n!"), better to use + matrix decomposition (see BIAS/MathAlgo/Lapack.hh) */ + inline TYPE getDetSquare() const; + + /** @brief computes the adjoint matrix + @author Christian Beder */ + inline TDMatrix getAdjoint() const; + + /** @brief compute square system matrix dest = A^T * A + @param dest holds result of Transpose * this + + If you want to solve A * x = b, where A has more rows than columns, + a common technique is to solve x = (A^T * A)^-1 * A^T * b. + This function provides a fast way to compute A^T*A from A. + @author grest/koeser */ + inline void getSystemMatrix(TDMatrix& dest) const; + + /** @brief componentwise: this = 0.5(this + this^T) yields symmetric matrix + only allowed for square shaped matrices + @author koeser 01/2007 */ + inline void makeSymmetric(); + + /** Return the L1 norm: |a| + |b| + |c| + ... + @author Ingo Thomsen + @date 04/11/2002 + @status untested **/ + inline TYPE getNormL1() const; + + /** Return the L2 norm: a^2 + b^2 + c^2 + ... + @author woelk 07/2004 */ + inline double getNormL2() const; + + /** Kronecker-product with matrix, result in dest */ + void Kronecker(const TDMatrix& B, TDMatrix& dest) const; + + /** @brief swaps two rows + @author woelk 05/2008 www.vision-n.de */ + void SwapRows(int i, int r); + + /** @brief use the Gauss Jordan Algorithm to transform the matrix to + reduced row echelon form. + @author woelk 05/2008 www.vision-n.de */ + void GaussJordan(); + + //! more convenient forms of row and element access operators + const TYPE& operator [](size_t i) const { return cv::Mat::at((int)i); } + TYPE& operator [](size_t i) { return cv::Mat::at((int)i); } + + /// Access an element from the matrix. Bounds checking is only performed in debug mode. + inline const TYPE& operator [] (const Size& pos) const { + ASSERT(isInside(pos) && elem_stride() == sizeof(TYPE)); + return ((const TYPE*)data)[pos.height*row_stride() + pos.width]; + } + inline TYPE& operator [] (const Size& pos) { + ASSERT(isInside(pos) && elem_stride() == sizeof(TYPE)); + return ((TYPE*)data)[pos.height*row_stride() + pos.width]; + } + + /// pointer to the beginning of the matrix data + inline const TYPE* getData() const { ASSERT(cv::Mat::empty() || cv::Mat::isContinuous()); return (const TYPE*)data; } + inline TYPE* getData() { ASSERT(cv::Mat::empty() || cv::Mat::isContinuous()); return (TYPE*)data; } + + #ifdef _USE_EIGEN + // Access point as Eigen equivalent + inline operator EMat () const { return CEMatMap(getData(), rows, cols); } + // Access point as Eigen::Map equivalent + inline operator const CEMatMap () const { return CEMatMap(getData(), rows, cols); } + inline operator EMatMap () { return EMatMap(getData(), rows, cols); } + #endif + + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object(*this); + } + #endif +}; +/*----------------------------------------------------------------*/ +typedef TDMatrix DMatrix; +typedef TDMatrix DMatrix8S; +typedef TDMatrix DMatrix8U; +typedef TDMatrix DMatrix32S; +typedef TDMatrix DMatrix32U; +typedef TDMatrix DMatrix32F; +typedef TDMatrix DMatrix64F; +typedef CLISTDEF2(DMatrix) DMatrixArr; +typedef CLISTDEF2(cv::Mat) MatArr; +/*----------------------------------------------------------------*/ + + +// generic vector struct +template +class TDVector : public TDMatrix +{ +public: + typedef TYPE Type; + typedef TDMatrix Base; + typedef typename Base::Base BaseBase; + typedef cv::Size Size; + + using Base::rows; + using Base::cols; + using Base::data; + +public: + inline TDVector() {} + inline TDVector(const Base& rhs) : Base(rhs) {} + inline TDVector(const cv::Mat& rhs) : Base(rhs) {} + inline TDVector(const cv::MatExpr& rhs) : Base(rhs) {} + inline TDVector(int _rows) : Base(_rows, 1) {} + inline TDVector(int _rows, const TYPE& v) : Base(Size(_rows,1), v) {} + inline TDVector(int _rows, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(_rows, 1, _data, _step) {} + + inline TDVector& operator = (const Base& rhs) { BaseBase::operator=(rhs); return *this; } + inline TDVector& operator = (const BaseBase& rhs) { BaseBase::operator=(rhs); return *this; } + inline TDVector& operator = (const cv::MatExpr& rhs) { BaseBase::operator=(rhs); return *this; } + + //! equivalent to Mat::create(_rows, 1, DataType<_Tp>::type) + inline void create(int _rows) { BaseBase::create(_rows, 1); } + + /** outer product, constructs a matrix. + Often written as v * v^T for col vectors + @author Daniel Grest, Oct 2002 + @status tested */ + TDMatrix getOuterProduct(const TDVector& v) const; + + /** kronecker product + @author woelk 08/2004 */ + void getKroneckerProduct(const TDVector& arg, TDVector& dst) const; +}; +/*----------------------------------------------------------------*/ +typedef TDVector DVector; +typedef TDVector DVector8S; +typedef TDVector DVector8U; +typedef TDVector DVector32S; +typedef TDVector DVector32U; +typedef TDVector DVector32F; +typedef TDVector DVector64F; +typedef CLISTDEF2(DVector) DVectorArr; +/*----------------------------------------------------------------*/ + + +// generic color type +// Here the color order means the order the data is stored on this machine. +// For example BGR means that the blue byte is stored first and the red byte last, +// which correspond to the RGB format on a little-endian machine. +#define _COLORMODE_BGR 1 // little-endian +#define _COLORMODE_RGB 2 // big-endian +#ifndef _COLORMODE +#define _COLORMODE _COLORMODE_BGR +#endif + +template class ColorType +{ +public: + typedef TYPE value_type; + typedef value_type alt_type; + static const value_type ONE; + static const alt_type ALTONE; +}; +template<> class ColorType +{ +public: + typedef uint8_t value_type; + typedef float alt_type; + static const value_type ONE; + static const alt_type ALTONE; +}; +template<> class ColorType +{ +public: + typedef uint32_t value_type; + typedef float alt_type; + static const value_type ONE; + static const alt_type ALTONE; +}; +template<> class ColorType +{ +public: + typedef float value_type; + typedef uint8_t alt_type; + static const value_type ONE; + static const alt_type ALTONE; +}; +template<> class ColorType +{ +public: + typedef double value_type; + typedef uint8_t alt_type; + static const value_type ONE; + static const alt_type ALTONE; +}; +/*----------------------------------------------------------------*/ + +template +struct TPixel { + union { + struct { + #if _COLORMODE == _COLORMODE_BGR + TYPE b; + TYPE g; + TYPE r; + #endif + #if _COLORMODE == _COLORMODE_RGB + TYPE r; + TYPE g; + TYPE b; + #endif + }; + struct { + TYPE c0; + TYPE c1; + TYPE c2; + }; + TYPE c[3]; + }; + typedef typename ColorType::alt_type ALT; + typedef TYPE Type; + typedef TPoint3 Pnt; + static const TPixel BLACK; + static const TPixel WHITE; + static const TPixel GRAY; + static const TPixel RED; + static const TPixel GREEN; + static const TPixel BLUE; + static const TPixel YELLOW; + static const TPixel MAGENTA; + static const TPixel CYAN; + // init + inline TPixel() {} + template inline TPixel(const TPixel& p) + #if _COLORMODE == _COLORMODE_BGR + : b(TYPE(p.b)), g(TYPE(p.g)), r(TYPE(p.r)) {} + #endif + #if _COLORMODE == _COLORMODE_RGB + : r(TYPE(p.r)), g(TYPE(p.g)), b(TYPE(p.b)) {} + #endif + inline TPixel(TYPE _r, TYPE _g, TYPE _b) + #if _COLORMODE == _COLORMODE_BGR + : b(_b), g(_g), r(_r) {} + #endif + #if _COLORMODE == _COLORMODE_RGB + : r(_r), g(_g), b(_b) {} + #endif + inline TPixel(const Pnt& col) : c0(col.x), c1(col.y), c2(col.z) {} + explicit inline TPixel(uint32_t col) + #if _COLORMODE == _COLORMODE_BGR + : b(TYPE(col&0xFF)), g(TYPE((col>>8)&0xFF)), r(TYPE((col>>16)&0xFF)) {} + #endif + #if _COLORMODE == _COLORMODE_RGB + : r(TYPE((col>>16)&0xFF)), g(TYPE((col>>8)&0xFF)), b(TYPE(col&0xFF)) {} + #endif + // set/get from default type + inline TPixel& set(TYPE _r, TYPE _g, TYPE _b) { r = _r; g = _g; b = _b; return *this; } + inline TPixel& set(const TYPE* clr) { c[0] = clr[0]; c[1] = clr[1]; c[2] = clr[2]; return *this; } + inline void get(TYPE& _r, TYPE& _g, TYPE& _b) const { _r = r; _g = g; _b = b; } + inline void get(TYPE* clr) const { clr[0] = c[0]; clr[1] = c[1]; clr[2] = c[2]; } + // set/get from alternative type + inline TPixel& set(ALT _r, ALT _g, ALT _b) { r = TYPE(_r); g = TYPE(_g); b = TYPE(_b); return *this; } + inline TPixel& set(const ALT* clr) { c[0] = TYPE(clr[0]); c[1] = TYPE(clr[1]); c[2] = TYPE(clr[2]); return *this; } + inline void get(ALT& _r, ALT& _g, ALT& _b) const { _r = ALT(r); _g = ALT(g); _b = ALT(b); } + inline void get(ALT* clr) const { clr[0] = ALT(c[0]); clr[1] = ALT(c[1]); clr[2] = ALT(c[2]); } + template inline TPixel::value || !std::is_same::value,T>::type> cast() const { return TPixel(T(r), T(g), T(b)); } + template inline TPixel::value && std::is_same::value,T>::type> cast() const { + return TPixel( + (uint8_t)CLAMP(ROUND2INT(r), 0, 255), + (uint8_t)CLAMP(ROUND2INT(g), 0, 255), + (uint8_t)CLAMP(ROUND2INT(b), 0, 255) + ); + } + // set/get as vector + inline const TYPE& operator[](size_t i) const { ASSERT(i<3); return c[i]; } + inline TYPE& operator[](size_t i) { ASSERT(i<3); return c[i]; } + // access as point equivalent + template inline operator TPoint3() const { return TPoint3(T(c[0]), T(c[1]), T(c[2])); } + // access as vector equivalent + inline operator const Pnt& () const { return *((const Pnt*)this); } + inline operator Pnt& () { return *((Pnt*)this); } + // access as cv::Scalar equivalent + inline operator cv::Scalar () const { return cv::Scalar(c[0], c[1], c[2], TYPE(0)); } + // compare + inline bool operator==(const TPixel& col) const { return (memcmp(c, col.c, sizeof(TPixel)) == 0); } + inline bool operator!=(const TPixel& col) const { return (memcmp(c, col.c, sizeof(TPixel)) != 0); } + // operators + inline TPixel operator*(const TPixel& v) const { return TPixel(r*v.r, g*v.g, b*v.b); } + template inline TPixel operator*(T v) const { return TPixel((TYPE)(v*r), (TYPE)(v*g), (TYPE)(v*b)); } + template inline TPixel& operator*=(T v) { return (*this = operator*(v)); } + inline TPixel operator/(const TPixel& v) const { return TPixel(r/v.r, g/v.g, b/v.b); } + template inline TPixel operator/(T v) const { return operator*(T(1)/v); } + template inline TPixel& operator/=(T v) { return (*this = operator/(v)); } + inline TPixel operator+(const TPixel& v) const { return TPixel(r+v.r, g+v.g, b+v.b); } + template inline TPixel operator+(T v) const { return TPixel((TYPE)(r+v), (TYPE)(g+v), (TYPE)(b+v)); } + template inline TPixel& operator+=(T v) { return (*this = operator+(v)); } + inline TPixel operator-(const TPixel& v) const { return TPixel(r-v.r, g-v.g, b-v.b); } + template inline TPixel operator-(T v) const { return TPixel((TYPE)(r-v), (TYPE)(g-v), (TYPE)(b-v)); } + template inline TPixel& operator-=(T v) { return (*this = operator-(v)); } + inline uint32_t toDWORD() const { return RGBA((uint8_t)r, (uint8_t)g, (uint8_t)b, (uint8_t)0); } + // tools + template + static TPixel colorRamp(VT v, VT vmin, VT vmax); + static TPixel gray2color(ALT v); + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & c; + } + #endif +}; +template <> inline TPixel& TPixel::set(uint8_t _r, uint8_t _g, uint8_t _b) { r = float(_r)/255; g = float(_g)/255; b = float(_b)/255; return *this; } +template <> inline void TPixel::get(uint8_t& _r, uint8_t& _g, uint8_t& _b) const { _r = (uint8_t)CLAMP(ROUND2INT(r*255), 0, 255); _g = (uint8_t)CLAMP(ROUND2INT(g*255), 0, 255); _b = (uint8_t)CLAMP(ROUND2INT(b*255), 0, 255); } +template <> inline TPixel& TPixel::set(float _r, float _g, float _b) { r = (uint8_t)CLAMP(ROUND2INT(_r*255), 0, 255); g = (uint8_t)CLAMP(ROUND2INT(_g*255), 0, 255); b = (uint8_t)CLAMP(ROUND2INT(_b*255), 0, 255); return *this; } +template <> inline void TPixel::get(float& _r, float& _g, float& _b) const { _r = float(r)/255; _g = float(g)/255; _b = float(b)/255; } +/*----------------------------------------------------------------*/ +typedef TPixel Pixel8U; +typedef TPixel Pixel32F; +typedef TPixel Pixel64F; +/*----------------------------------------------------------------*/ + +template +struct TColor { + union { + struct { + #if _COLORMODE == _COLORMODE_BGR + TYPE b; + TYPE g; + TYPE r; + TYPE a; + #endif + #if _COLORMODE == _COLORMODE_RGB + TYPE a; + TYPE r; + TYPE g; + TYPE b; + #endif + }; + struct { + TYPE c0; + TYPE c1; + TYPE c2; + TYPE c3; + }; + TYPE c[4]; + }; + typedef typename ColorType::alt_type ALT; + typedef TYPE Type; + typedef TPixel Pxl; + typedef TPoint3 Pnt; + static const TColor BLACK; + static const TColor WHITE; + static const TColor GRAY; + static const TColor RED; + static const TColor GREEN; + static const TColor BLUE; + static const TColor YELLOW; + static const TColor MAGENTA; + static const TColor CYAN; + // init + inline TColor() {} + template inline TColor(const TColor& p) + : r(TYPE(p.r)), g(TYPE(p.g)), b(TYPE(p.b)), a(TYPE(p.a)) {} + inline TColor(TYPE _r, TYPE _g, TYPE _b, TYPE _a=ColorType::ONE) + : r(_r), g(_g), b(_b), a(_a) {} + inline TColor(const Pxl& col, TYPE _a=ColorType::ONE) + : r(col.r), g(col.g), b(col.b), a(_a) {} + #if _COLORMODE == _COLORMODE_BGR + inline TColor(const Pnt& col, TYPE _a=ColorType::ONE) + : b(col.x), g(col.y), r(col.z), a(_a) {} + #endif + #if _COLORMODE == _COLORMODE_RGB + inline TColor(const Pnt& col, TYPE _a=ColorType::ONE) + : r(col.x), g(col.y), b(col.z), a(_a) {} + #endif + explicit inline TColor(uint32_t col) + : r(TYPE((col>>16)&0xFF)), g(TYPE((col>>8)&0xFF)), b(TYPE(col&0xFF)), a(TYPE((col>>24)&0xFF)) {} + // set/get from default type + inline TColor& set(TYPE _r, TYPE _g, TYPE _b, TYPE _a=ColorType::ONE) { r = _r; g = _g; b = _b; a = _a; return *this; } + inline TColor& set(const TYPE* clr) { c[0] = clr[0]; c[1] = clr[1]; c[2] = clr[2]; c[3] = clr[3]; return *this; } + inline void get(TYPE& _r, TYPE& _g, TYPE& _b, TYPE& _a) const { _r = r; _g = g; _b = b; _a = a; } + inline void get(TYPE* clr) const { clr[0] = c[0]; clr[1] = c[1]; clr[2] = c[2]; clr[3] = c[3]; } + // set/get from alternative type + inline TColor& set(ALT _r, ALT _g, ALT _b, ALT _a=ColorType::ALTONE) { r = TYPE(_r); g = TYPE(_g); b = TYPE(_b); a = TYPE(_a); return *this; } + inline TColor& set(const ALT* clr) { c[0] = TYPE(clr[0]); c[1] = TYPE(clr[1]); c[2] = TYPE(clr[2]); c[3] = TYPE(clr[3]); return *this; } + inline void get(ALT& _r, ALT& _g, ALT& _b, ALT& _a) const { _r = ALT(r); _g = ALT(g); _b = ALT(b); _a = ALT(a); } + inline void get(ALT* clr) const { clr[0] = ALT(c[0]); clr[1] = ALT(c[1]); clr[2] = ALT(c[2]); clr[3] = ALT(c[3]); } + template inline TColor::value || !std::is_same::value,T>::type> cast() const { return TColor(T(r), T(g), T(b), T(a)); } + template inline TColor::value && std::is_same::value,T>::type> cast() const { + return TColor( + (uint8_t)CLAMP(ROUND2INT(r), 0, 255), + (uint8_t)CLAMP(ROUND2INT(g), 0, 255), + (uint8_t)CLAMP(ROUND2INT(b), 0, 255), + (uint8_t)CLAMP(ROUND2INT(a), 0, 255) + ); + } + // set/get as vector + inline const TYPE& operator[](size_t i) const { ASSERT(i<4); return c[i]; } + inline TYPE& operator[](size_t i) { ASSERT(i<4); return c[i]; } + // access as pixel equivalent + inline operator const Pxl& () const { return *((const Pxl*)this); } + inline operator Pxl& () { return *((Pxl*)this); } + // access as point equivalent + inline operator const Pnt& () const { return *((const Pnt*)this); } + inline operator Pnt& () { return *((Pnt*)this); } + // access as cv::Scalar equivalent + inline operator cv::Scalar () const { return cv::Scalar(c[0], c[1], c[2], c[3]); } + // compare + inline bool operator==(const TColor& col) const { return (memcmp(c, col.c, sizeof(TColor)) == 0); } + inline bool operator!=(const TColor& col) const { return (memcmp(c, col.c, sizeof(TColor)) != 0); } + // operators + inline TColor operator*(const TColor& v) const { return TColor(r*v.r, g*v.g, b*v.b, a*v.a); } + template inline TColor operator*(T v) const { return TColor((TYPE)(v*r), (TYPE)(v*g), (TYPE)(v*b), (TYPE)(v*a)); } + template inline TColor& operator*=(T v) { return (*this = operator*(v)); } + inline TColor operator/(const TColor& v) const { return TColor(r/v.r, g/v.g, b/v.b, a/v.a); } + template inline TColor operator/(T v) const { return operator*(T(1)/v); } + template inline TColor& operator/=(T v) { return (*this = operator/(v)); } + inline TColor operator+(const TColor& v) const { return TColor(r+v.r, g+v.g, b+v.b, a+v.a); } + template inline TColor operator+(T v) const { return TColor((TYPE)(r+v), (TYPE)(g+v), (TYPE)(b+v), (TYPE)(a+v)); } + template inline TColor& operator+=(T v) { return (*this = operator+(v)); } + inline TColor operator-(const TColor& v) const { return TColor(r-v.r, g-v.g, b-v.b, a-v.a); } + template inline TColor operator-(T v) const { return TColor((TYPE)(r-v), (TYPE)(g-v), (TYPE)(b-v), (TYPE)(a-v)); } + template inline TColor& operator-=(T v) { return (*this = operator-(v)); } + inline uint32_t toDWORD() const { return RGBA((uint8_t)r, (uint8_t)g, (uint8_t)b, (uint8_t)a); } + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & c; + } + #endif +}; +template <> inline TColor& TColor::set(uint8_t _r, uint8_t _g, uint8_t _b, uint8_t _a) { r = float(_r)/255; g = float(_g)/255; b = float(_b)/255; a = float(_a)/255; return *this; } +template <> inline void TColor::get(uint8_t& _r, uint8_t& _g, uint8_t& _b, uint8_t& _a) const { _r = uint8_t(r*255); _g = uint8_t(g*255); _b = uint8_t(b*255); _a = uint8_t(a*255); } +template <> inline TColor& TColor::set(float _r, float _g, float _b, float _a) { r = uint8_t(_r*255); g = uint8_t(_g*255); b = uint8_t(_b*255); a = uint8_t(_a*255); return *this; } +template <> inline void TColor::get(float& _r, float& _g, float& _b, float& _a) const { _r = float(r)/255; _g = float(g)/255; _b = float(b)/255; _a = float(a)/255; } +template <> inline bool TColor::operator==(const TColor& col) const { return (*((const uint32_t*)c) == *((const uint32_t*)col.c)); } +template <> inline bool TColor::operator!=(const TColor& col) const { return (*((const uint32_t*)c) != *((const uint32_t*)col.c)); } +template <> inline uint32_t TColor::toDWORD() const { return *((const uint32_t*)c); } +/*----------------------------------------------------------------*/ +typedef TColor Color8U; +typedef TColor Color32F; +typedef TColor Color64F; +/*----------------------------------------------------------------*/ + + +// structure containing the image pixels +typedef Point2i ImageRef; +template +class TImage : public TDMatrix +{ +public: + typedef TYPE Type; + typedef TDMatrix Base; + typedef cv::Size Size; + typedef typename Base::Base BaseBase; + + using Base::rows; + using Base::cols; + using Base::data; + +public: + inline TImage() {} + inline TImage(const Base& rhs) : Base(rhs) {} + inline TImage(const cv::Mat& rhs) : Base(rhs) {} + inline TImage(const cv::MatExpr& rhs) : Base(rhs) {} + inline TImage(int _rows, int _cols) : Base(_rows, _cols) {} + inline TImage(const Size& sz) : Base(sz) {} + inline TImage(const Size& sz, const TYPE& v) : Base(sz, v) {} + inline TImage(const Size& sz, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(sz.height, sz.width, _data, _step) {} + #ifdef _SUPPORT_CPP11 + inline TImage(cv::Mat&& rhs) : Base(std::forward(rhs)) {} + + inline TImage& operator = (cv::Mat&& rhs) { BaseBase::operator=(std::forward(rhs)); return *this; } + #endif + inline TImage& operator = (const Base& rhs) { BaseBase::operator=(rhs); return *this; } + inline TImage& operator = (const BaseBase& rhs) { BaseBase::operator=(rhs); return *this; } + inline TImage& operator = (const cv::MatExpr& rhs) { BaseBase::operator=(rhs); return *this; } + + /// What is the image size? + inline int width() const { return cols; } + inline int height() const { return rows; } + + /// What is the pixel stride of the image? + inline size_t pixel_stride() const { return Base::elem_stride(); } + + inline const TYPE& getPixel(int y, int x) const; + + template + TYPE sample(const TPoint2& pt) const; + template + TYPE sampleSafe(const TPoint2& pt) const; + + template + bool sample(TV& v, const TPoint2& pt, const Functor& functor) const; + template + bool sampleSafe(TV& v, const TPoint2& pt, const Functor& functor) const; + template + TYPE sample(const TPoint2& pt, const Functor& functor, const TYPE& dv) const; + template + TYPE sampleSafe(const TPoint2& pt, const Functor& functor, const TYPE& dv) const; + + template + INTERTYPE sample(const SAMPLER& sampler, const TPoint2& pt) const; + + template + void toGray(TImage& out, int code, bool bNormalize=false, bool bSRGB=false) const; + + static cv::Size computeResize(const cv::Size& size, REAL scale); + static cv::Size computeResize(const cv::Size& size, REAL scale, unsigned resizes); + unsigned computeMaxResolution(unsigned& level, unsigned minImageSize=320, unsigned maxImageSize=INT_MAX) const; + static unsigned computeMaxResolution(unsigned width, unsigned height, unsigned& level, unsigned minImageSize=320, unsigned maxImageSize=INT_MAX); + + template + static void RasterizeTriangle(const TPoint2& v1, const TPoint2& v2, const TPoint2& v3, PARSER& parser); + template + static void RasterizeTriangleBary(const TPoint2& v1, const TPoint2& v2, const TPoint2& v3, PARSER& parser); + template + static void RasterizeTriangleDepth(TPoint3 p1, TPoint3 p2, TPoint3 p3, PARSER& parser); + + template + static void DrawLine(const TPoint2& p1, const TPoint2& p2, PARSER& parser); + + typedef void (STCALL *FncDrawPointAntialias) (const ImageRef&, const ImageRef&, float, float, void*); + static bool DrawLineAntialias(Point2f x1, Point2f x2, FncDrawPointAntialias fncDrawPoint, void* pData=NULL); + static bool DrawLineAntialias(const ImageRef& x1, const ImageRef& x2, FncDrawPointAntialias fncDrawPoint, void* pData=NULL); + + template + void DilateMean(TImage& out, const TYPE& invalid) const; + + bool Load(const String&); + bool Save(const String&) const; + + #ifndef _RELEASE + void Show(const String& winname, int delay=0, bool bDestroy=true) const; + #endif + + #ifdef _USE_BOOST + // serialize + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object(*this); + } + #endif +}; +/*----------------------------------------------------------------*/ +typedef TImage Image8U; +typedef TImage Image16U; +typedef TImage Image16F; +typedef TImage Image32F; +typedef TImage Image64F; +typedef TImage Image8U3; +typedef TImage Image8U4; +typedef TImage Image32F3; +typedef TImage Image32F4; +/*----------------------------------------------------------------*/ + + +template +class TBitMatrix +{ +public: + typedef TYPE Type; + typedef cv::Size Size; + struct Index { + size_t idx; + Type flag; + inline Index() {} + inline Index(size_t _idx, Type _flag) : idx(_idx), flag(_flag) {} + }; + + enum { numBitsPerCell = sizeof(Type)*8 }; + enum { numBitsShift = LOG2I() }; + +public: + inline TBitMatrix() : data(NULL) {} + inline TBitMatrix(int _rows, int _cols=1) : rows(_rows), cols(_cols), data(rows && cols ? new Type[computeLength(rows, cols)] : NULL) {} + inline TBitMatrix(int _rows, int _cols, uint8_t v) : rows(_rows), cols(_cols), data(rows && cols ? new Type[computeLength(rows, cols)] : NULL) { if (!empty()) memset(v); } + inline TBitMatrix(const Size& sz) : rows(sz.height), cols(sz.width), data(rows && cols ? new Type[computeLength(sz)] : NULL) {} + inline TBitMatrix(const Size& sz, uint8_t v) : rows(sz.height), cols(sz.width), data(rows && cols ? new Type[computeLength(sz)] : NULL) { if (!empty()) memset(v); } + inline ~TBitMatrix() { delete[] data; } + + inline void create(int _rows, int _cols=1) { + if (!empty() && rows == _rows && cols == _cols) + return; + rows = _rows; cols = _cols; + if (rows <=0 || cols <= 0) { + release(); + return; + } + delete[] data; + data = new Type[length()]; + } + inline void create(const Size& sz) { create(sz.height, sz.width); } + inline void release() { delete[] data; data = NULL; } + inline void memset(uint8_t v) { ASSERT(!empty()); ::memset(data, v, sizeof(Type)*length()); } + inline void swap(TBitMatrix& m) { + union {int i; Type* d;} tmp; + tmp.i = rows; rows = m.rows; m.rows = tmp.i; + tmp.i = cols; cols = m.cols; m.cols = tmp.i; + tmp.d = data; data = m.data; m.data = tmp.d; + } + + inline void And(const TBitMatrix& m) { + ASSERT(rows == m.rows && cols == m.cols); + int i = length(); + while (i-- > 0) + data[i] &= m.data[i]; + } + inline void Or(const TBitMatrix& m) { + ASSERT(rows == m.rows && cols == m.cols); + int i = length(); + while (i-- > 0) + data[i] |= m.data[i]; + } + inline void XOr(const TBitMatrix& m) { + ASSERT(rows == m.rows && cols == m.cols); + int i = length(); + while (i-- > 0) + data[i] ^= m.data[i]; + } + + inline bool empty() const { return data == NULL; } + inline Size size() const { return Size(cols, rows); } + inline int area() const { return cols*rows; } + inline int length() const { return computeLength(rows, cols); } + + inline bool operator() (int i) const { return isSet(i); } + inline bool operator() (int i, int j) const { return isSet(i,j); } + inline bool operator() (const ImageRef& ir) const { return isSet(ir); } + + inline bool isSet(int i) const { ASSERT(!empty() && i=0 && pt.height>=0 && pt.width + inline bool isInside(const cv::Point_& pt) const { + return int(pt.x)>=0 && int(pt.y)>=0 && int(pt.x)=border && pt.height>=border && pt.width + inline bool isInsideWithBorder(const cv::Point_& pt, int border) const { + return int(pt.x)>=border && int(pt.y)>=border && int(pt.x) + inline bool isInsideWithBorder(const cv::Point_& pt) const { + return int(pt.x)>=border && int(pt.y)>=border && int(pt.x)>numBitsShift; } + static inline int computeLength(int _rows, int _cols) { return computeLength(_rows*_cols); } + static inline int computeLength(const Size& sz) { return computeLength(sz.area()); } + + static inline Index computeIndex(int i) { return Index(i>>numBitsShift, Type(1)<<(i&(numBitsPerCell-1))); } + static inline Index computeIndex(int i, int j, int stride) { return computeIndex(i*stride+j); } + static inline Index computeIndex(const ImageRef& ir, int stride) { return computeIndex(ir.y*stride+ir.x); } + + //! the number of rows and columns + int rows, cols; + //! pointer to the data + Type* data; + +#ifdef _USE_BOOST +protected: + // implement BOOST serialization + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int /*version*/) const { + if (empty()) { + const int sz(0); + ar & sz; + return; + } + ar & cols; + ar & rows; + ar & boost::serialization::make_array(data, length()); + } + template + void load(Archive& ar, const unsigned int /*version*/) { + release(); + ar & cols; + if (cols == 0) + return; + ar & rows; + create(rows, cols); + ar & boost::serialization::make_array(data, length()); + } + BOOST_SERIALIZATION_SPLIT_MEMBER() +#endif +}; +/*----------------------------------------------------------------*/ +typedef TBitMatrix BitMatrix; +/*----------------------------------------------------------------*/ + + +// weighted accumulator class that operates on arbitrary types +template +struct TAccumulator { + typedef TYPE Type; + typedef ACCUMTYPE AccumType; + typedef WEIGHTTYPE WeightType; + + AccumType value; + WeightType weight; + + inline TAccumulator() : value(INITTO(static_cast(NULL), 0)), weight(0) {} + inline TAccumulator(const Type& v, const WeightType& w) : value(v), weight(w) {} + inline bool IsEmpty() const { return weight <= 0; } + // adds the given weighted value to the internal value + inline void Add(const Type& v, const WeightType& w) { + value += v*w; + weight += w; + } + inline TAccumulator& operator +=(const TAccumulator& accum) { + value += accum.value; + weight += accum.weight; + return *this; + } + inline TAccumulator operator +(const TAccumulator& accum) const { + return TAccumulator( + value + accum.value, + weight + accum.weight + ); + } + // subtracts the given weighted value to the internal value + inline void Sub(const Type& v, const WeightType& w) { + value -= v*w; + weight -= w; + } + inline TAccumulator& operator -=(const TAccumulator& accum) { + value -= accum.value; + weight -= accum.weight; + return *this; + } + inline TAccumulator operator -(const TAccumulator& accum) const { + return TAccumulator( + value - accum.value, + weight - accum.weight + ); + } + // returns the normalized version of the internal value + inline AccumType NormalizedFull() const { + return value / weight; + } + inline Type Normalized() const { + return Type(NormalizedFull()); + } + #ifdef _USE_BOOST + // implement BOOST serialization + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & value; + ar & weight; + } + #endif +}; +/*----------------------------------------------------------------*/ + + +// structure used for sorting some indices by their score (decreasing by default) +template +struct TIndexScore { + IndexType idx; + ScoreType score; + inline TIndexScore() {} + inline TIndexScore(IndexType _idx, ScoreType _score) : idx(_idx), score(_score) {} + // compare by index (increasing) + inline bool operator<(IndexType i) const { return (idx < i); } + inline bool operator==(IndexType i) const { return (idx == i); } + // compare by score (decreasing) + inline bool operator<(const TIndexScore& r) const { return (score > r.score); } + inline bool operator==(const TIndexScore& r) const { return (score == r.score); } + static bool STCALL CompareByIndex(const TIndexScore& l, const TIndexScore& r) { return (l.idx < r.idx); } + static bool STCALL CompareByScore(const TIndexScore& l, const TIndexScore& r) { return (r.score < l.score); } + #ifdef _USE_BOOST + // implement BOOST serialization + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & idx; + ar & score; + } + #endif +}; +/*----------------------------------------------------------------*/ +typedef TIndexScore IndexScore; +typedef CLISTDEF0(IndexScore) IndexScoreArr; +/*----------------------------------------------------------------*/ + + +// structure describing a pair of indices as one long index +struct PairIdx { + typedef uint64_t PairIndex; + typedef uint32_t Index; + union { + PairIndex idx; + Index indices[2]; + struct { + #if __BYTE_ORDER == __LITTLE_ENDIAN + Index j; + Index i; + #else + Index i; + Index j; + #endif + }; + }; + inline PairIdx() {} + inline PairIdx(PairIndex _idx) : idx(_idx) {} + inline PairIdx(Index _i, Index _j) + #if __BYTE_ORDER == __LITTLE_ENDIAN + : j(_j), i(_i) {} + #else + : i(_i), j(_j) {} + #endif + // get index + inline operator PairIndex () const { return idx; } + inline Index operator[](unsigned n) const { + ASSERT(n < 2); + #if __BYTE_ORDER == __LITTLE_ENDIAN + return indices[(n+1)%2]; + #else + return indices[n]; + #endif + } + inline Index& operator[](unsigned n) { + ASSERT(n < 2); + #if __BYTE_ORDER == __LITTLE_ENDIAN + return indices[(n+1)%2]; + #else + return indices[n]; + #endif + } + // compare by index (increasing) + inline bool operator<(const PairIdx& r) const { return (idx < r.idx); } + inline bool operator==(const PairIdx& r) const { return (idx == r.idx); } +}; +/*----------------------------------------------------------------*/ +typedef CLISTDEF0(PairIdx) PairIdxArr; +inline PairIdx MakePairIdx(uint32_t idxImageA, uint32_t idxImageB) { + return (idxImageA inline String cvMat2String(const TMatrix& mat, LPCSTR format="% 10.4f ") { return cvMat2String(cv::Mat(mat), format); } +template inline String cvMat2String(const TPoint3& pt, LPCSTR format="% 10.4f ") { return cvMat2String(cv::Mat(pt), format); } +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + + +#ifdef _USE_EIGEN + +// Implement SO3 and SO2 lie groups +// as in TooN library: https://github.com/edrosten/TooN +// Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk) + +namespace Eigen { + +/// Class to represent a three-dimensional rotation matrix. Three-dimensional rotation +/// matrices are members of the Special Orthogonal Lie group SO3. This group can be parameterized +/// three numbers (a vector in the space of the Lie Algebra). In this class, the three parameters are the +/// finite rotation vector, i.e. a three-dimensional vector whose direction is the axis of rotation +/// and whose length is the angle of rotation in radians. Exponentiating this vector gives the matrix, +/// and the logarithm of the matrix gives this vector. +template +class SO3 +{ +public: + template + friend std::istream& operator>>(std::istream& is, SO3

& rhs); + + typedef Matrix Mat3; + typedef Matrix Vec3; + + /// Default constructor. Initializes the matrix to the identity (no rotation) + inline SO3() : mat(Mat3::Identity()) {} + + /// Construct from a rotation matrix. + inline SO3(const Mat3& rhs) : mat(rhs) {} + + /// Construct from the axis of rotation (and angle given by the magnitude). + inline SO3(const Vec3& v) : mat(exp(v)) {} + + /// creates an SO3 as a rotation that takes Vector a into the direction of Vector b + /// with the rotation axis along a ^ b. If |a ^ b| == 0, it creates the identity rotation. + /// An assertion will fail if Vector a and Vector b are in exactly opposite directions. + /// @param a source Vector + /// @param b target Vector + SO3(const Vec3& a, const Vec3& b) { + ASSERT(a.size() == 3); + ASSERT(b.size() == 3); + Vec3 n(a.cross(b)); + const Precision nrmSq(n.squaredNorm()); + if (nrmSq == Precision(0)) { + // check that the vectors are in the same direction if cross product is 0; if not, + // this means that the rotation is 180 degrees, which leads to an ambiguity in the rotation axis + ASSERT(a.dot(b) >= Precision(0)); + mat = Mat3::Identity(); + return; + } + n *= Precision(1)/sqrt(nrmSq); + Mat3 R1; + R1.col(0) = a.normalized(); + R1.col(1) = n; + R1.col(2) = R1.col(0).cross(n); + mat.col(0) = b.normalized(); + mat.col(1) = n; + mat.col(2) = mat.col(0).cross(n); + mat = mat * R1.transpose(); + } + + /// Assignment operator from a general matrix. This also calls coerce() + /// to make sure that the matrix is a valid rotation matrix. + inline SO3& operator=(const Mat3& rhs) { + mat = rhs; + coerce(); + return *this; + } + + /// Modifies the matrix to make sure it is a valid rotation matrix. + void coerce() { + mat.row(0).normalize(); + const Precision d01(mat.row(0).dot(mat.row(1))); + mat.row(1) -= mat.row(0) * d01; + mat.row(1).normalize(); + const Precision d02(mat.row(0).dot(mat.row(2))); + mat.row(2) -= mat.row(0) * d02; + const Precision d12(mat.row(1).dot(mat.row(2))); + mat.row(2) -= mat.row(1) * d12; + mat.row(2).normalize(); + // check for positive determinant <=> right handed coordinate system of row vectors + ASSERT(mat.row(0).cross(mat.row(1)).dot(mat.row(2)) > 0); + } + + /// Exponentiate a vector in the Lie algebra to generate a new SO3. + /// See the Detailed Description for details of this vector. + inline Mat3 exp(const Vec3& vect) const; + + /// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra. + /// See the Detailed Description for details of this vector. + inline Vec3 ln() const; + + /// Right-multiply by another rotation matrix + template + inline SO3& operator *=(const SO3

& rhs) { + *this = *this * rhs; + return *this; + } + + /// Right-multiply by another rotation matrix + inline SO3 operator *(const SO3& rhs) const { return SO3(*this, rhs); } + + /// Returns the SO3 as a Matrix<3> + inline const Mat3& get_matrix() const { return mat; } + + /// Returns the i-th generator. The generators of a Lie group are the basis + /// for the space of the Lie algebra. For %SO3, the generators are three + /// \f$3\times3\f$ matrices representing the three possible (linearized) + /// rotations. + inline static Mat3 generator(int i) { + Mat3 result(Mat3::Zero()); + result((i+1)%3,(i+2)%3) = Precision(-1); + result((i+2)%3,(i+1)%3) = Precision( 1); + return result; + } + + /// Returns the i-th generator times pos + inline static Vec3 generator_field(int i, const Vec3& pos) { + Vec3 result; + result(i) = Precision(0); + result((i+1)%3) = -pos((i+2)%3); + result((i+2)%3) = pos((i+1)%3); + return result; + } + + template + inline SO3(const SO3& a, const SO3& b) : mat(a.get_matrix()*b.get_matrix()) {} + +protected: + Mat3 mat; + + #ifdef _USE_BOOST + // implement BOOST serialization + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int /*version*/) const + { + Vec3 comp(ln()); + ar & comp; + } + template + void load(Archive& ar, const unsigned int /*version*/) + { + Vec3 comp; + ar & comp; + mat = exp(comp); + } + BOOST_SERIALIZATION_SPLIT_MEMBER() + #endif +}; +/*----------------------------------------------------------------*/ + + +/// Class to represent a two-dimensional rotation matrix. Two-dimensional rotation +/// matrices are members of the Special Orthogonal Lie group SO2. This group can be parameterized +/// with one number (the rotation angle). +template +class SO2 +{ +public: + template + friend std::istream& operator>>(std::istream&, SO2

&); + + typedef Matrix Mat2; + + /// Default constructor. Initializes the matrix to the identity (no rotation) + inline SO2() : mat(Mat2::Identity()) {} + + /// Construct from a rotation matrix. + inline SO2(const Mat2& rhs) : mat(rhs) {} + + /// Construct from an angle. + inline SO2(const Precision l) : mat(exp(l)) {} + + /// Assignment operator from a general matrix. This also calls coerce() + /// to make sure that the matrix is a valid rotation matrix. + inline SO2& operator=(const Mat2& rhs) { + mat = rhs; + coerce(); + return *this; + } + + /// Modifies the matrix to make sure it is a valid rotation matrix. + inline void coerce() { + mat.row(0).normalize(); + mat.row(1) = (mat.row(1) - mat.row(0) * (mat.row(0).dot(mat.row(1)))).normalized(); + } + + /// Exponentiate an angle in the Lie algebra to generate a new SO2. + inline Mat2 exp(const Precision& d) const; + + /// extracts the rotation angle from the SO2 + inline Precision ln() const; + + /// Self right-multiply by another rotation matrix + inline SO2& operator *=(const SO2& rhs) { + mat = mat*rhs.get_matrix(); + return *this; + } + + /// Right-multiply by another rotation matrix + inline SO2 operator *(const SO2& rhs) const { return SO2(*this, rhs); } + + /// Returns the SO2 as a Matrix<2> + inline const Mat2& get_matrix() const { return mat; } + + /// returns generator matrix + inline static Mat2 generator() { + Mat2 result; + result(0,0) = Precision(0); result(0,1) = Precision(-1); + result(1,0) = Precision(1); result(1,1) = Precision(0); + return result; + } + +protected: + Mat2 mat; + + #ifdef _USE_BOOST + // implement BOOST serialization + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int /*version*/) const + { + Precision comp(ln()); + ar & comp; + } + template + void load(Archive& ar, const unsigned int /*version*/) + { + Precision comp; + ar & comp; + mat = exp(comp); + } + BOOST_SERIALIZATION_SPLIT_MEMBER() + #endif +}; +/*----------------------------------------------------------------*/ + +} // namespace Eigen + +#endif // _USE_EIGEN + +#include "../Math/LMFit/lmmin.h" +#include "Types.inl" +#include "Util.inl" +#include "Rotation.h" +#include "Sphere.h" +#include "AABB.h" +#include "OBB.h" +#include "Plane.h" +#include "Ray.h" +#include "Line.h" +#include "Octree.h" +#include "UtilCUDA.h" + +#endif // __SEACAVE_TYPES_H__ diff --git a/libs/Common/Types.inl b/libs/Common/Types.inl new file mode 100644 index 0000000..b2d6ed2 --- /dev/null +++ b/libs/Common/Types.inl @@ -0,0 +1,4039 @@ +//////////////////////////////////////////////////////////////////// +// Types.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace std { + +//namespace tr1 { +// Specializations for unordered containers +template <> struct hash +{ + typedef SEACAVE::ImageRef argument_type; + typedef size_t result_type; + result_type operator()(const argument_type& v) const { + return std::hash()((const uint64_t&)v); + } +}; +//} // namespace tr1 + +} // namespace std + + +#if CV_MAJOR_VERSION > 3 +template +INT_TYPE cvRANSACUpdateNumIters(REAL_TYPE p, REAL_TYPE ep, INT_TYPE modelPoints, INT_TYPE maxIters) +{ + ASSERT(p>=0 && p<=1); + ASSERT(ep>=0 && ep<=1); + // avoid inf's & nan's + REAL_TYPE num = MAXF(REAL_TYPE(1)-p, SEACAVE::EPSILONTOLERANCE()); + REAL_TYPE denom = REAL_TYPE(1)-POWI(REAL_TYPE(1)-ep, modelPoints); + if (denom < SEACAVE::EPSILONTOLERANCE()) + return 0; + num = SEACAVE::LOGN(num); + denom = SEACAVE::LOGN(denom); + return (denom >= 0 || -num >= (-denom)*maxIters ? maxIters : ROUND2INT(num/denom)); +} +#endif + +namespace cv { + +#if CV_MAJOR_VERSION > 2 +template<> class DataType +{ +public: + typedef unsigned value_type; + typedef value_type work_type; + typedef value_type channel_type; + typedef value_type vec_type; + enum { generic_type = 0, + depth = CV_32S, + channels = 1, + fmt = (int)'i', + type = CV_MAKETYPE(depth, channels) + }; +}; +#else +#if CV_MINOR_VERSION < 4 +template static inline +_AccTp normL2Sqr(const _Tp* a, int n) +{ + _AccTp s = 0; + int i=0; + #if CV_ENABLE_UNROLLED + for (; i <= n - 4; i += 4) { + _AccTp v0 = a[i], v1 = a[i+1], v2 = a[i+2], v3 = a[i+3]; + s += v0*v0 + v1*v1 + v2*v2 + v3*v3; + } + #endif + for (; i < n; i++) { + _AccTp v = a[i]; + s += v*v; + } + return s; +} +#endif + +#if CV_MINOR_VERSION < 4 || CV_SUBMINOR_VERSION < 11 +// Convenience creation functions. In the far future, there may be variadic templates here. +template +Ptr makePtr() +{ + return Ptr(new T()); +} +template +Ptr makePtr(const A1& a1) +{ + return Ptr(new T(a1)); +} +template +Ptr makePtr(const A1& a1, const A2& a2) +{ + return Ptr(new T(a1, a2)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3) +{ + return Ptr(new T(a1, a2, a3)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3, const A4& a4) +{ + return Ptr(new T(a1, a2, a3, a4)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3, const A4& a4, const A5& a5) +{ + return Ptr(new T(a1, a2, a3, a4, a5)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3, const A4& a4, const A5& a5, const A6& a6) +{ + return Ptr(new T(a1, a2, a3, a4, a5, a6)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3, const A4& a4, const A5& a5, const A6& a6, const A7& a7) +{ + return Ptr(new T(a1, a2, a3, a4, a5, a6, a7)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3, const A4& a4, const A5& a5, const A6& a6, const A7& a7, const A8& a8) +{ + return Ptr(new T(a1, a2, a3, a4, a5, a6, a7, a8)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3, const A4& a4, const A5& a5, const A6& a6, const A7& a7, const A8& a8, const A9& a9) +{ + return Ptr(new T(a1, a2, a3, a4, a5, a6, a7, a8, a9)); +} +template +Ptr makePtr(const A1& a1, const A2& a2, const A3& a3, const A4& a4, const A5& a5, const A6& a6, const A7& a7, const A8& a8, const A9& a9, const A10& a10) +{ + return Ptr(new T(a1, a2, a3, a4, a5, a6, a7, a8, a9, a10)); +} +#endif + +// property implementation macros +#define CV_IMPL_PROPERTY_RO(type, name, member) \ + inline type get##name() const { return member; } + +#define CV_HELP_IMPL_PROPERTY(r_type, w_type, name, member) \ + CV_IMPL_PROPERTY_RO(r_type, name, member) \ + inline void set##name(w_type val) { member = val; } + +#define CV_HELP_WRAP_PROPERTY(r_type, w_type, name, internal_name, internal_obj) \ + r_type get##name() const { return internal_obj.get##internal_name(); } \ + void set##name(w_type val) { internal_obj.set##internal_name(val); } + +#define CV_IMPL_PROPERTY(type, name, member) CV_HELP_IMPL_PROPERTY(type, type, name, member) +#define CV_IMPL_PROPERTY_S(type, name, member) CV_HELP_IMPL_PROPERTY(type, const type &, name, member) + +#define CV_WRAP_PROPERTY(type, name, internal_name, internal_obj) CV_HELP_WRAP_PROPERTY(type, type, name, internal_name, internal_obj) +#define CV_WRAP_PROPERTY_S(type, name, internal_name, internal_obj) CV_HELP_WRAP_PROPERTY(type, const type &, name, internal_name, internal_obj) + +#define CV_WRAP_SAME_PROPERTY(type, name, internal_obj) CV_WRAP_PROPERTY(type, name, name, internal_obj) +#define CV_WRAP_SAME_PROPERTY_S(type, name, internal_obj) CV_WRAP_PROPERTY_S(type, name, name, internal_obj) +#endif + +//! copy every second source image element to the destination image +inline void downsample2x(InputArray _src, OutputArray _dst) +{ + Mat src(_src.getMat()); + #if 1 + _dst.create((src.rows+1)/2, (src.cols+1)/2, src.type()); + #else + if (_dst.empty()) { + // create a new matrix + _dst.create(src.rows/2, src.cols/2, src.type()); + } else { + // overwrite elements in the existing matrix + ASSERT(src.rows > 0 && (unsigned)(src.rows-_dst.size().height*2) <= 1); + ASSERT(src.cols > 0 && (unsigned)(src.cols-_dst.size().width*2) <= 1); + ASSERT(src.type() == _dst.type()); + } + #endif + Mat dst(_dst.getMat()); + ASSERT(src.elemSize() == dst.elemSize()); + switch (src.elemSize()) { + case 1: + for (int i=0; i(i,j) = src.at(2*i,2*j); + break; + case 2: + for (int i=0; i(i,j) = src.at(2*i,2*j); + break; + case 4: + for (int i=0; i(i,j) = src.at(2*i,2*j); + break; + case 8: + for (int i=0; i(i,j) = src.at(2*i,2*j); + break; + default: + for (int i=0; i 0 && src.rows*2-1 <= _dst.size().height); + ASSERT(src.cols > 0 && src.cols*2-1 <= _dst.size().width); + ASSERT(src.type() == _dst.type()); + } + Mat dst(_dst.getMat()); + ASSERT(src.elemSize() == dst.elemSize()); + switch (src.elemSize()) { + case 1: + for (int i=0; i(2*i,2*j) = src.at(i,j); + break; + case 2: + for (int i=0; i(2*i,2*j) = src.at(i,j); + break; + case 4: + for (int i=0; i(2*i,2*j) = src.at(i,j); + break; + case 8: + for (int i=0; i(2*i,2*j) = src.at(i,j); + break; + default: + for (int i=0; i const typename ColorType::value_type ColorType::ONE(1); +template const typename ColorType::alt_type ColorType::ALTONE(1); + +template const TPixel TPixel::BLACK (0, 0, 0); +template const TPixel TPixel::WHITE (ColorType::ONE, ColorType::ONE, ColorType::ONE); +template const TPixel TPixel::GRAY (0.8f*ColorType::ONE, 0.8f*ColorType::ONE, 0.8f*ColorType::ONE); +template const TPixel TPixel::RED (ColorType::ONE, 0, 0); +template const TPixel TPixel::GREEN (0, ColorType::ONE, 0); +template const TPixel TPixel::BLUE (0, 0, ColorType::ONE); +template const TPixel TPixel::YELLOW (ColorType::ONE, ColorType::ONE, 0); +template const TPixel TPixel::MAGENTA (ColorType::ONE, 0, ColorType::ONE); +template const TPixel TPixel::CYAN (0, ColorType::ONE, ColorType::ONE); + +template const TColor TColor::BLACK (0, 0, 0, ColorType::ONE); +template const TColor TColor::WHITE (ColorType::ONE, ColorType::ONE, ColorType::ONE, ColorType::ONE); +template const TColor TColor::GRAY (0.8f*ColorType::ONE, 0.8f*ColorType::ONE, 0.8f*ColorType::ONE, ColorType::ONE); +template const TColor TColor::RED (ColorType::ONE, 0, 0, ColorType::ONE); +template const TColor TColor::GREEN (0, ColorType::ONE, 0, ColorType::ONE); +template const TColor TColor::BLUE (0, 0, ColorType::ONE, ColorType::ONE); +template const TColor TColor::YELLOW (ColorType::ONE, ColorType::ONE, 0, ColorType::ONE); +template const TColor TColor::MAGENTA (ColorType::ONE, 0, ColorType::ONE, ColorType::ONE); +template const TColor TColor::CYAN (0, ColorType::ONE, ColorType::ONE, ColorType::ONE); + +#ifdef _SUPPORT_CPP11 +template +inline typename std::enable_if::value, size_t>::type +SizeOfArray(const A&) { + return std::extent::value; +} +#else +template +inline size_t +SizeOfArray(const T(&)[N]) { + return N; +} +#endif + + +// C L A S S ////////////////////////////////////////////////////// + +// square +template +FORCEINLINE SEACAVE::TPoint2 SQUARE(const SEACAVE::TPoint2& v) +{ + return SEACAVE::TPoint2(SQUARE(v.x), SQUARE(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 SQUARE(const SEACAVE::TPoint3& v) +{ + return SEACAVE::TPoint3(SQUARE(v.x), SQUARE(v.y), SQUARE(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix SQUARE(const SEACAVE::TMatrix& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPoint2 SQUARE(const cv::Point_& v) +{ + return SEACAVE::TPoint2(SQUARE(v.x), SQUARE(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 SQUARE(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(SQUARE(v.x), SQUARE(v.y), SQUARE(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix SQUARE(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPixel SQUARE(const SEACAVE::TPixel& v) +{ + return SEACAVE::TPixel(SQUARE(v.r), SQUARE(v.g), SQUARE(v.b)); +} +template +FORCEINLINE SEACAVE::TColor SQUARE(const SEACAVE::TColor& v) +{ + return SEACAVE::TColor(SQUARE(v.r), SQUARE(v.g), SQUARE(v.b), SQUARE(v.a)); +} + +// sqrt +template +FORCEINLINE SEACAVE::TPoint2 SQRT(const SEACAVE::TPoint2& v) +{ + return SEACAVE::TPoint2(SQRT(v.x), SQRT(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 SQRT(const SEACAVE::TPoint3& v) +{ + return SEACAVE::TPoint3(SQRT(v.x), SQRT(v.y), SQRT(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix SQRT(const SEACAVE::TMatrix& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPoint2 SQRT(const cv::Point_& v) +{ + return SEACAVE::TPoint2(SQRT(v.x), SQRT(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 SQRT(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(SQRT(v.x), SQRT(v.y), SQRT(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix SQRT(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPixel SQRT(const SEACAVE::TPixel& v) +{ + return SEACAVE::TPixel(SQRT(v.r), SQRT(v.g), SQRT(v.b)); +} +template +FORCEINLINE SEACAVE::TColor SQRT(const SEACAVE::TColor& v) +{ + return SEACAVE::TColor(SQRT(v.r), SQRT(v.g), SQRT(v.b), SQRT(v.a)); +} + +// exp +template +FORCEINLINE SEACAVE::TPoint2 EXP(const SEACAVE::TPoint2& v) +{ + return SEACAVE::TPoint2(EXP(v.x), EXP(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 EXP(const SEACAVE::TPoint3& v) +{ + return SEACAVE::TPoint3(EXP(v.x), EXP(v.y), EXP(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix EXP(const SEACAVE::TMatrix& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPoint2 EXP(const cv::Point_& v) +{ + return SEACAVE::TPoint2(EXP(v.x), EXP(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 EXP(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(EXP(v.x), EXP(v.y), EXP(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix EXP(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPixel EXP(const SEACAVE::TPixel& v) +{ + return SEACAVE::TPixel(EXP(v.r), EXP(v.g), EXP(v.b)); +} +template +FORCEINLINE SEACAVE::TColor EXP(const SEACAVE::TColor& v) +{ + return SEACAVE::TColor(EXP(v.r), EXP(v.g), EXP(v.b), EXP(v.a)); +} + +// loge +template +FORCEINLINE SEACAVE::TPoint2 LOGN(const SEACAVE::TPoint2& v) +{ + return SEACAVE::TPoint2(LOGN(v.x), LOGN(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 LOGN(const SEACAVE::TPoint3& v) +{ + return SEACAVE::TPoint3(LOGN(v.x), LOGN(v.y), LOGN(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix LOGN(const SEACAVE::TMatrix& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPoint2 LOGN(const cv::Point_& v) +{ + return SEACAVE::TPoint2(LOGN(v.x), LOGN(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 LOGN(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(LOGN(v.x), LOGN(v.y), LOGN(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix LOGN(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPixel LOGN(const SEACAVE::TPixel& v) +{ + return SEACAVE::TPixel(LOGN(v.r), LOGN(v.g), LOGN(v.b)); +} +template +FORCEINLINE SEACAVE::TColor LOGN(const SEACAVE::TColor& v) +{ + return SEACAVE::TColor(LOGN(v.r), LOGN(v.g), LOGN(v.b), LOGN(v.a)); +} + +// abs +template +FORCEINLINE SEACAVE::TPoint2 ABS(const SEACAVE::TPoint2& v) +{ + return SEACAVE::TPoint2(ABS(v.x), ABS(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 ABS(const SEACAVE::TPoint3& v) +{ + return SEACAVE::TPoint3(ABS(v.x), ABS(v.y), ABS(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix ABS(const SEACAVE::TMatrix& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE SEACAVE::TPoint2 ABS(const cv::Point_& v) +{ + return SEACAVE::TPoint2(ABS(v.x), ABS(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint3 ABS(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(ABS(v.x), ABS(v.y), ABS(v.z)); +} +template +FORCEINLINE SEACAVE::TMatrix ABS(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i +FORCEINLINE bool ISZERO(const cv::Point_& v) +{ + return (ISZERO(v.x) && ISZERO(v.y)); +} +template +FORCEINLINE bool ISZERO(const cv::Point3_& v) +{ + return (ISZERO(v.x) && ISZERO(v.y) && ISZERO(v.z)); +} +template +FORCEINLINE bool ISZERO(const cv::Matx& v) +{ + for (int i=0; i +FORCEINLINE bool ISEQUAL(const cv::Point_& v1, const cv::Point_& v2) +{ + return (ISEQUAL(v1.x,v2.x) && ISEQUAL(v1.y,v2.y)); +} +template +FORCEINLINE bool ISEQUAL(const cv::Point3_& v1, const cv::Point3_& v2) +{ + return (ISEQUAL(v1.x,v2.x) && ISEQUAL(v1.y,v2.y) && ISEQUAL(v1.z,v2.z)); +} +template +FORCEINLINE bool ISEQUAL(const cv::Matx& v1, const cv::Matx& v2) +{ + for (int i=0; i +FORCEINLINE SEACAVE::TPoint2 Floor2Int(const cv::Point_& v) +{ + return SEACAVE::TPoint2(FLOOR2INT(v.x), FLOOR2INT(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint2 Ceil2Int(const cv::Point_& v) +{ + return SEACAVE::TPoint2(CEIL2INT(v.x), CEIL2INT(v.y)); +} +template +FORCEINLINE SEACAVE::TPoint2 Round2Int(const cv::Point_& v) +{ + return SEACAVE::TPoint2(ROUND2INT(v.x), ROUND2INT(v.y)); +} + +template +FORCEINLINE SEACAVE::TPoint3 Floor2Int(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(FLOOR2INT(v.x), FLOOR2INT(v.y), FLOOR2INT(v.z)); +} +template +FORCEINLINE SEACAVE::TPoint3 Ceil2Int(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(CEIL2INT(v.x), CEIL2INT(v.y), CEIL2INT(v.z)); +} +template +FORCEINLINE SEACAVE::TPoint3 Round2Int(const cv::Point3_& v) +{ + return SEACAVE::TPoint3(ROUND2INT(v.x), ROUND2INT(v.y), ROUND2INT(v.z)); +} + +template +FORCEINLINE SEACAVE::TMatrix Floor2Int(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i(v.val[i]); + return nv; +} +template +FORCEINLINE SEACAVE::TMatrix Ceil2Int(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i(v.val[i]); + return nv; +} +template +FORCEINLINE SEACAVE::TMatrix Round2Int(const cv::Matx& v) +{ + SEACAVE::TMatrix nv; + for (int i=0; i(v.val[i]); + return nv; +} + +// ISFINITE +template +FORCEINLINE bool ISFINITE(const SEACAVE::TPoint2& v) +{ + return (ISFINITE(v.x) && ISFINITE(v.y)); +} +template +FORCEINLINE bool ISFINITE(const SEACAVE::TPoint3& v) +{ + return (ISFINITE(v.x) && ISFINITE(v.y) && ISFINITE(v.z)); +} +template +FORCEINLINE bool ISFINITE(const SEACAVE::TDMatrix& v) +{ + ASSERT(v.isContinuous()); + return ISFINITE(v.val, v.area()); +} +template +FORCEINLINE bool ISFINITE(const cv::Point_& v) +{ + return (ISFINITE(v.x) && ISFINITE(v.y)); +} +template +FORCEINLINE bool ISFINITE(const cv::Point3_& v) +{ + return (ISFINITE(v.x) && ISFINITE(v.y) && ISFINITE(v.z)); +} +template +FORCEINLINE bool ISFINITE(const cv::Matx& v) +{ + ASSERT(v.isContinuous()); + return ISFINITE(v.val, m*n); +} +template +FORCEINLINE bool ISFINITE(const Eigen::Matrix& m) +{ + return ISFINITE(m.data(), m.size()); +} + + +// initializing both scalar and matrix variables +template +FORCEINLINE Scalar INITTO(const Scalar*, Value v) +{ + return static_cast(v); +} +template +FORCEINLINE TPoint2 INITTO(const TPoint2*, Value v) +{ + return TPoint2(static_cast(v)); +} +template +FORCEINLINE TPoint3 INITTO(const TPoint3*, Value v) +{ + return TPoint3(static_cast(v)); +} +template +FORCEINLINE Eigen::Matrix INITTO(const Eigen::Matrix*, Value v) +{ + return Eigen::Matrix::Constant(static_cast(v)); +} +/*----------------------------------------------------------------*/ + +template +inline IDX MinIndex(const TYPE* data, IDX size) { + ASSERT(size>0); + IDX idx = 0; + for (IDX i=1; i data[i]) + idx = i; + return idx; +} +template +inline IDX MaxIndex(const TYPE* data, IDX size) { + ASSERT(size>0); + IDX idx = 0; + for (IDX i=1; i +inline ACCTYPE dot(const TYPE* a, const TYPE* b, size_t n) { + ACCTYPE s(0); + const TYPE* const last = a+n; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + s += ACCTYPE(a[0])*ACCTYPE(b[0]) + ACCTYPE(a[1])*ACCTYPE(b[1]) + ACCTYPE(a[2])*ACCTYPE(b[2]) + ACCTYPE(a[3])*ACCTYPE(b[3]); + a += 4; b += 4; + } + #endif + while (a < last) + s += ACCTYPE(*a++) * ACCTYPE(*b++); + return s; +} +template +inline ACCTYPE dot(const TYPE* a, const TYPE* b) { + ACCTYPE s(0); + const TYPE* const last = a+N; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + s += ACCTYPE(a[0])*ACCTYPE(b[0]) + ACCTYPE(a[1])*ACCTYPE(b[1]) + ACCTYPE(a[2])*ACCTYPE(b[2]) + ACCTYPE(a[3])*ACCTYPE(b[3]); + a += 4; b += 4; + } + #endif + while (a < last) + s += ACCTYPE(*a++) * ACCTYPE(*b++); + return s; +} +#ifdef _USE_SSE +template <> +inline float dot(const float* l, const float* r, size_t size) { + return sse_f_t::dot(l, r, size); +} +template <> +inline double dot(const double* l, const double* r, size_t size) { + return sse_d_t::dot(l, r, size); +} +#endif + +template +inline TYPE dot(const TMatrix& l, const TMatrix& r) { + return ((const cv::Vec&)l).dot((const cv::Vec&)r); +} +template +inline TYPE dot(const cv::Matx& l, const cv::Matx& r) { + return ((const cv::Vec&)l).dot((const cv::Vec&)r); +} + +inline float dot(float v1, float v2) { + return v1 * v2; +} +inline double dot(double v1, double v2) { + return v1 * v2; +} +template +inline TYPE dot(const cv::Point_* v1, const cv::Point_* v2, size_t size) { + return dot((const TYPE*)v1, (const TYPE*)v2, size*2); +} +template +inline TYPE dot(const cv::Point3_* v1, const cv::Point3_* v2, size_t size) { + return dot((const TYPE*)v1, (const TYPE*)v2, size*3); +} +template +inline TYPE dot(const cv::Vec* v1, const cv::Vec* v2, size_t size) { + return dot((const TYPE*)v1, (const TYPE*)v2, size*m); +} + +template +inline TMatrix cross(const TMatrix& l, const TMatrix& r) { + return ((const typename TMatrix::Vec&)l).cross((const typename TMatrix::Vec&)r); +} +template +inline TMatrix cross(const cv::Matx& l, const cv::Matx& r) { + return ((const typename TMatrix::Vec&)l).cross((const typename TMatrix::Vec&)r); +} + +template +inline typename RealType::type normSq(const cv::Point_& v) { + typedef typename RealType::type real; + return SQUARE((real)v.x)+SQUARE((real)v.y); +} +template +inline typename RealType::type normSq(const cv::Point3_& v) { + typedef typename RealType::type real; + return SQUARE((real)v.x)+SQUARE((real)v.y)+SQUARE((real)v.z); +} +template +inline typename RealType::type normSq(const TMatrix& v) { + typedef typename RealType::type real; + return cv::normL2Sqr(v.val, m*n); +} +template +inline typename RealType::type normSq(const cv::Matx& v) { + typedef typename RealType::type real; + return cv::normL2Sqr(v.val, m*n); +} +template +inline typename RealType::type normSq(const TDMatrix& v) { + typedef typename RealType::type real; + return cv::normL2Sqr(v.cv::Mat::template ptr(), v.area()); +} +template +inline typename RealType::type normSq(const cv::Mat_& v) { + typedef typename RealType::type real; + return cv::normL2Sqr(v.cv::Mat::template ptr(), v.cols*v.rows); +} + +inline REAL normSq(int v) { + return SQUARE((REAL)v); +} +inline REAL normSq(unsigned v) { + return SQUARE((REAL)v); +} +inline float normSq(float v) { + return SQUARE(v); +} +inline double normSq(double v) { + return SQUARE(v); +} +template +inline typename RealType::type normSq(const cv::Point_* v, size_t size) { + typedef typename RealType::type real; + return cv::normL2Sqr((const TYPE*)v, size*2); +} +template +inline typename RealType::type normSq(const cv::Point3_* v, size_t size) { + typedef typename RealType::type real; + return cv::normL2Sqr((const TYPE*)v, size*3); +} +template +inline typename RealType::type normSq(const cv::Vec* v, size_t size) { + typedef typename RealType::type real; + return cv::normL2Sqr((const TYPE*)v, size*m); +} + +template +inline ACCTYPE normSq(const TYPE* a, size_t n) { + ACCTYPE s(0); + const TYPE* const last = a+n; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + s += SQUARE(ACCTYPE(a[0])) + SQUARE(ACCTYPE(a[1])) + SQUARE(ACCTYPE(a[2])) + SQUARE(ACCTYPE(a[3])); + a += 4; + } + #endif + while (a < last) + s += SQUARE(ACCTYPE(*a++)); + return s; +} +template +inline ACCTYPE normSq(const TYPE* a) { + ACCTYPE s(0); + const TYPE* const last = a+N; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + s += SQUARE(ACCTYPE(a[0])) + SQUARE(ACCTYPE(a[1])) + SQUARE(ACCTYPE(a[2])) + SQUARE(ACCTYPE(a[3])); + a += 4; + } + #endif + while (a < last) + s += SQUARE(ACCTYPE(*a++)); + return s; +} + +template +inline ACCTYPE normSq(const TYPE* a, const TYPE* b, size_t n) { + ACCTYPE s(0); + const TYPE* const last = a+n; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + const INTTYPE v0(a[0] - b[0]), v1(a[1] - b[1]), v2(a[2] - b[2]), v3(a[3] - b[3]); + s += ACCTYPE(v0*v0 + v1*v1 + v2*v2 + v3*v3); + a += 4; b += 4; + } + #endif + while (a < last) { + const INTTYPE v(*a++ - *b++); + s += ACCTYPE(v*v); + } + return s; +} +template +inline ACCTYPE normSq(const TYPE* a, const TYPE* b) { + ACCTYPE s(0); + const TYPE* const last = a+N; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + const INTTYPE v0(a[0] - b[0]), v1(a[1] - b[1]), v2(a[2] - b[2]), v3(a[3] - b[3]); + s += ACCTYPE(v0*v0 + v1*v1 + v2*v2 + v3*v3); + a += 4; b += 4; + } + #endif + while (a < last) { + const INTTYPE v(*a++ - *b++); + s += ACCTYPE(v*v); + } + return s; +} + +template +inline ACCTYPE normSqDelta(TYPE* a, size_t n, const ACCTYPE avg) { + ACCTYPE s(0); + const TYPE* const last = a+n; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + const ACCTYPE v0(a[0]-=avg), v1(a[1]-=avg), v2(a[2]-=avg), v3(a[3]-=avg); + s += v0*v0 + v1*v1 + v2*v2 + v3*v3; + a += 4; + } + #endif + while (a < last) { + const ACCTYPE v(*a++ -= avg); + s += v*v; + } + return s; +} +template +inline ACCTYPE normSqDelta(TYPE* a, const ACCTYPE avg) { + ACCTYPE s(0); + const TYPE* const last = a+N; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + const ACCTYPE v0(a[0]-=avg), v1(a[1]-=avg), v2(a[2]-=avg), v3(a[3]-=avg); + s += v0*v0 + v1*v1 + v2*v2 + v3*v3; + a += 4; + } + #endif + while (a < last) { + const ACCTYPE v(*a++ -= avg); + s += v*v; + } + return s; +} + +template +inline ACCTYPE normSqZeroMean(TYPE* ptr, size_t n) { + TYPE* a = ptr; + ACCTYPE avg(0); + const TYPE* const last = a+n; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + avg += ACCTYPE(a[0]) + ACCTYPE(a[1]) + ACCTYPE(a[2]) + ACCTYPE(a[3]); + a += 4; + } + #endif + while (a < last) + avg += ACCTYPE(*a++); + avg /= (ACCTYPE)n; + + a = ptr; + ACCTYPE s(0); + #if 1 // unrolled + while (a < lastgroup) { + const ACCTYPE v0(a[0]-=avg), v1(a[1]-=avg), v2(a[2]-=avg), v3(a[3]-=avg); + s += v0*v0 + v1*v1 + v2*v2 + v3*v3; + a += 4; + } + #endif + while (a < last) { + const ACCTYPE v(*a++ -= avg); + s += v*v; + } + return s; +} +template +inline ACCTYPE normSqZeroMean(TYPE* ptr) { + TYPE* a = ptr; + ACCTYPE avg(0); + const TYPE* const last = a+N; + #if 1 // unrolled + const TYPE* const lastgroup = last-3; + while (a < lastgroup) { + avg += ACCTYPE(a[0]) + ACCTYPE(a[1]) + ACCTYPE(a[2]) + ACCTYPE(a[3]); + a += 4; + } + #endif + while (a < last) + avg += ACCTYPE(*a++); + avg /= (ACCTYPE)N; + + a = ptr; + ACCTYPE s(0); + #if 1 // unrolled + while (a < lastgroup) { + const ACCTYPE v0(a[0]-=avg), v1(a[1]-=avg), v2(a[2]-=avg), v3(a[3]-=avg); + s += v0*v0 + v1*v1 + v2*v2 + v3*v3; + a += 4; + } + #endif + while (a < last) { + const ACCTYPE v(*a++ -= avg); + s += v*v; + } + return s; +} + +template +inline typename RealType::type norm(const TPoint2& v) { + return SQRT(normSq(v)); +} +template +inline typename RealType::type norm(const TPoint3& v) { + return SQRT(normSq(v)); +} +template +inline typename RealType::type norm(const TMatrix& v) { + typedef typename RealType::type real; + return SQRT(cv::normL2Sqr(v.val, m*n)); +} +template +inline typename RealType::type norm(const TDMatrix& v) { + typedef typename RealType::type real; + return SQRT(cv::normL2Sqr(v.cv::Mat::template ptr(), v.area())); +} + +template +inline TPoint2 normalized(const TPoint2& v) { + return cv::normalize((const typename TPoint2::cvVec&)v); +} +template +inline TPoint2 normalized(const cv::Point_& v) { + return cv::normalize((const typename TPoint2::cvVec&)v); +} +template +inline TPoint3 normalized(const TPoint3& v) { + return cv::normalize((const typename TPoint3::cvVec&)v); +} +template +inline TPoint3 normalized(const cv::Point3_& v) { + return cv::normalize((const typename TPoint3::cvVec&)v); +} +template +inline TMatrix normalized(const TMatrix& v) { + return cv::normalize((const typename TMatrix::Vec&)v); +} +template +inline TMatrix normalized(const cv::Matx& v) { + return cv::normalize((const typename TMatrix::Vec&)v); +} + +template +inline void normalize(TPoint2& v) { + (typename TPoint2::cvVec&)v = cv::normalize((const typename TPoint2::cvVec&)v); +} +template +inline void normalize(cv::Point_& v) { + (typename TPoint2::cvVec&)v = cv::normalize((const typename TPoint2::cvVec&)v); +} +template +inline void normalize(TPoint3& v) { + (typename TPoint3::cvVec&)v = cv::normalize((const typename TPoint3::cvVec&)v); +} +template +inline void normalize(cv::Point3_& v) { + (typename TPoint3::cvVec&)v = cv::normalize((const typename TPoint3::cvVec&)v); +} +template +inline void normalize(TMatrix& v) { + (typename TMatrix::Vec&)v = cv::normalize((const typename TMatrix::Vec&)v); +} +template +inline void normalize(cv::Matx& v) { + (typename TMatrix::Vec&)v = cv::normalize((const typename TMatrix::Vec&)v); +} + +template +inline TYPE area(const TPoint2& v1, const TPoint2& v2) { + return ABS(v1.x * v2.y - v1.y * v2.x); +} +template +inline TYPE area(const cv::Point_& v1, const cv::Point_& v2) { + return ABS(v1.x * v2.y - v1.y * v2.x); +} +template +inline TYPE area(const TPoint3& v1, const TPoint3& v2) { + return norm(cross(v1,v2)); +} +template +inline TYPE area(const cv::Point3_& v1, const cv::Point3_& v2) { + return norm(cross(v1,v2)); +} +/*----------------------------------------------------------------*/ + + +/** @brief comparison function for floating point values + See http://www.boost.org/libs/test/doc/components/test_tools/floating_point_comparison.html */ +template // left == right +inline bool equal(const _Tp& left, const _Tp& right, const _Tp& = std::numeric_limits<_Tp>::epsilon()) +{ return (left==right); } +template<> +inline bool equal(const double& left, const double& right, const double& eps) { + // the code below does not work when one value is zero + if (left==0. || right==0.) + return (ABS(left-right) <= eps); + // compare two non-zero values + return (ABS(left-right) <= eps*MAXF(ABS(right),ABS(left))); +} +template<> +inline bool equal(const float& left, const float& right, const float& eps) { + // the code below does not work when one value is zero + if (left==0. || right==0.) + return (ABS(left-right) <= eps); + // compare two non-zero values + return (ABS(left-right) <= eps*MAXF(ABS(right),ABS(left))); +} + +/** @brief comparison function for floating point values */ +template // left < right +inline bool less(const _Tp& left, const _Tp& right, const _Tp& eps = std::numeric_limits<_Tp>::epsilon()) +{ if (left==0. || right==0.) return (right-left) > eps; + return ((right-left) > eps * MINF(right,left)); } + +/** @brief comparison function for floating point values */ +template // left <= right +inline bool lessEqual(const _Tp& left, const _Tp& right, const _Tp& eps = std::numeric_limits<_Tp>::epsilon()) +{ return (less(left, right, eps) || equal(left, right, eps)); } + +/** @brief comparison function for floating point values */ +template // left > right +inline bool greater(const _Tp& left, const _Tp& right, const _Tp& eps = std::numeric_limits<_Tp>::epsilon()) +{ if (left==0. || right==0.) return (left-right) > eps; + return ((left-right) > eps * MINF(right,left)); } + +/** @brief comparison function for floating point values */ +template // left >= right +inline bool greaterEqual(const _Tp& left, const _Tp& right, const _Tp& eps = std::numeric_limits<_Tp>::epsilon()) +{ return (greater(left,right) || equal(left, right, eps)); } +/*----------------------------------------------------------------*/ + + +/** @brief determine the number of decimals to store, w.g. to file + @returns the number of decimals between + lower bound guaranteed precision (digits10) + and the upper bound max. precision (nr. of binary digits) */ +#ifdef WIN32 +# pragma warning(push, 2) +#endif // WIN32 +template +inline unsigned decimalsToStore() { + if (std::numeric_limits<_Tp>::is_integer){ + // integer types can display exactly one decimal more than their guaranteed precision digits + return std::numeric_limits<_Tp>::digits10+1; + } else { + //return std::numeric_limits<_Tp>::digits10; // lower bound + return std::numeric_limits<_Tp>::digits; // upper bound + } +} +#ifdef WIN32 +# pragma warning(pop) +#endif // WIN32 +/*----------------------------------------------------------------*/ + + +// operators + +// TPoint2 operators +#if CV_MAJOR_VERSION > 2 +template +inline TPoint2 operator/(const TPoint2& pt, TYPEM m) { + return TPoint2(pt.x/m, pt.y/m); +} +template +inline TPoint2 operator/(const TPoint2& pt, TYPEM m) { + const float invm(INVERT(float(m))); + return TPoint2(invm*pt.x, invm*pt.y); +} +template +inline TPoint2 operator/(const TPoint2& pt, TYPEM m) { + const double invm(INVERT(double(m))); + return TPoint2(invm*pt.x, invm*pt.y); +} + +template +inline TPoint2& operator/=(TPoint2& pt, TYPEM m) { + pt.x /= m; pt.y /= m; + return pt; +} +template +inline TPoint2& operator/=(TPoint2& pt, TYPEM m) { + const float invm(INVERT(float(m))); + pt.x *= invm; pt.y *= invm; + return pt; +} +template +inline TPoint2& operator/=(TPoint2& pt, TYPEM m) { + const double invm(INVERT(double(m))); + pt.x *= invm; pt.y *= invm; + return pt; +} +#else +template +inline TPoint2 operator/(const cv::Point_& pt, TYPEM m) { + return TPoint2(pt.x/m, pt.y/m); +} +template +inline TPoint2 operator/(const cv::Point_& pt, TYPEM m) { + const float invm(INVERT(float(m))); + return TPoint2(invm*pt.x, invm*pt.y); +} +template +inline TPoint2 operator/(const cv::Point_& pt, TYPEM m) { + const double invm(INVERT(double(m))); + return TPoint2(invm*pt.x, invm*pt.y); +} + +template +inline TPoint2& operator/=(cv::Point_& pt, TYPEM m) { + pt.x /= m; pt.y /= m; + return (TPoint2&)pt; +} +template +inline TPoint2& operator/=(cv::Point_& pt, TYPEM m) { + const float invm(INVERT(float(m))); + pt.x *= invm; pt.y *= invm; + return (TPoint2&)pt; +} +template +inline TPoint2& operator/=(cv::Point_& pt, TYPEM m) { + const double invm(INVERT(double(m))); + pt.x *= invm; pt.y *= invm; + return (TPoint2&)pt; +} +#endif + +template +inline TPoint2 operator/(const cv::Point_& pt0, const cv::Point_& pt1) { + return TPoint2(pt0.x/pt1.x, pt0.y/pt1.y); +} +template +inline TPoint2& operator/=(cv::Point_& pt0, const cv::Point_& pt1) { + pt0.x/=pt1.x; pt0.y/=pt1.y; + return (TPoint2&)pt0; +} + +template +inline TPoint2 operator*(const cv::Point_& pt0, const cv::Point_& pt1) { + return TPoint2(pt0.x*pt1.x, pt0.y*pt1.y); +} +template +inline TPoint2& operator*=(cv::Point_& pt0, const cv::Point_& pt1) { + pt0.x*=pt1.x; pt0.y*=pt1.y; + return (TPoint2&)pt0; +} + +template +inline TPoint2 cvtPoint2(const TPoint2& p) { + return TPoint2(TTO(p.x), TTO(p.y)); +} + +// TPoint3 operators +#if CV_MAJOR_VERSION > 2 +template +inline TPoint3 operator/(const TPoint3& pt, TYPEM m) { + return TPoint3(pt.x/m, pt.y/m, pt.z/m); +} +template +inline TPoint3 operator/(const TPoint3& pt, TYPEM m) { + const float invm(INVERT(float(m))); + return TPoint3(invm*pt.x, invm*pt.y, invm*pt.z); +} +template +inline TPoint3 operator/(const TPoint3& pt, TYPEM m) { + const double invm(INVERT(double(m))); + return TPoint3(invm*pt.x, invm*pt.y, invm*pt.z); +} + +template +inline TPoint3& operator/=(TPoint3& pt, TYPEM m) { + pt.x /= m; pt.y /= m; pt.z /= m; + return pt; +} +template +inline TPoint3& operator/=(TPoint3& pt, TYPEM m) { + const float invm(INVERT(float(m))); + pt.x *= invm; pt.y *= invm; pt.z *= invm; + return pt; +} +template +inline TPoint3& operator/=(TPoint3& pt, TYPEM m) { + const double invm(INVERT(double(m))); + pt.x *= invm; pt.y *= invm; pt.z *= invm; + return pt; +} +#else +template +inline TPoint3 operator/(const cv::Point3_& pt, TYPEM m) { + return TPoint3(pt.x/m, pt.y/m, pt.z/m); +} +template +inline TPoint3 operator/(const cv::Point3_& pt, TYPEM m) { + const float invm(INVERT(float(m))); + return TPoint3(invm*pt.x, invm*pt.y, invm*pt.z); +} +template +inline TPoint3 operator/(const cv::Point3_& pt, TYPEM m) { + const double invm(INVERT(double(m))); + return TPoint3(invm*pt.x, invm*pt.y, invm*pt.z); +} + +template +inline TPoint3& operator/=(cv::Point3_& pt, TYPEM m) { + pt.x /= m; pt.y /= m; pt.z /= m; + return (TPoint3&)pt; +} +template +inline TPoint3& operator/=(cv::Point3_& pt, TYPEM m) { + const float invm(INVERT(float(m))); + pt.x *= invm; pt.y *= invm; pt.z *= invm; + return (TPoint3&)pt; +} +template +inline TPoint3& operator/=(cv::Point3_& pt, TYPEM m) { + const double invm(INVERT(double(m))); + pt.x *= invm; pt.y *= invm; pt.z *= invm; + return (TPoint3&)pt; +} +#endif + +template +inline TPoint3 operator/(const cv::Point3_& pt0, const cv::Point3_& pt1) { + return TPoint3(pt0.x/pt1.x, pt0.y/pt1.y, pt0.z/pt1.z); +} +template +inline TPoint3& operator/=(cv::Point3_& pt0, const cv::Point3_& pt1) { + pt0.x/=pt1.x; pt0.y/=pt1.y; pt0.z/=pt1.z; + return (TPoint3&)pt0; +} + +template +inline TPoint3 operator*(const cv::Point3_& pt0, const cv::Point3_& pt1) { + return TPoint3(pt0.x*pt1.x, pt0.y*pt1.y, pt0.z*pt1.z); +} +template +inline TPoint3& operator*=(cv::Point3_& pt0, const cv::Point3_& pt1) { + pt0.x*=pt1.x; pt0.y*=pt1.y; pt0.z*=pt1.z; + return (TPoint3&)pt0; +} + +template +inline TPoint3 cvtPoint3(const TPoint3& p) { + return TPoint3(TTO(p.x), TTO(p.y), TTO(p.z)); +} + +// TPixel operators +template +inline TPixel operator/(const TPixel& pt, TYPEM m) { + const TYPEM invm(INVERT(m)); + return TPixel(invm*pt.r, invm*pt.g, invm*pt.b); +} +template +inline TPixel& operator/=(TPixel& pt, TYPEM m) { + const TYPEM invm(INVERT(m)); + pt.r *= invm; pt.g *= invm; pt.b *= invm; + return pt; +} +template +inline TPixel operator/(const TPixel& pt0, const TPixel& pt1) { + return TPixel(pt0.r/pt1.r, pt0.g/pt1.g, pt0.b/pt1.b); +} +template +inline TPixel& operator/=(TPixel& pt0, const TPixel& pt1) { + pt0.r/=pt1.r; pt0.g/=pt1.g; pt0.b/=pt1.b; + return pt0; +} +template +inline TPixel operator*(const TPixel& pt0, const TPixel& pt1) { + return TPixel(pt0.r*pt1.r, pt0.g*pt1.g, pt0.b*pt1.b); +} +template +inline TPixel& operator*=(TPixel& pt0, const TPixel& pt1) { + pt0.r*=pt1.r; pt0.g*=pt1.g; pt0.b*=pt1.b; + return pt0; +} + +// TColor operators +template +inline TColor operator/(const TColor& pt, TYPEM m) { + const TYPEM invm(INVERT(m)); + return TColor(invm*pt.r, invm*pt.g, invm*pt.b, invm*pt.a); +} +template +inline TColor& operator/=(TColor& pt, TYPEM m) { + const TYPEM invm(INVERT(m)); + pt.r *= invm; pt.g *= invm; pt.b *= invm; pt.a *= invm; + return pt; +} +template +inline TColor operator/(const TColor& pt0, const TColor& pt1) { + return TColor(pt0.r/pt1.r, pt0.g/pt1.g, pt0.b/pt1.b, pt0.a/pt1.a); +} +template +inline TColor& operator/=(TColor& pt0, const TColor& pt1) { + pt0.r/=pt1.r; pt0.g/=pt1.g; pt0.b/=pt1.b; pt0.a/=pt1.a; + return pt0; +} +template +inline TColor operator*(const TColor& pt0, const TColor& pt1) { + return TColor(pt0.r*pt1.r, pt0.g*pt1.g, pt0.b*pt1.b, pt0.a*pt1.a); +} +template +inline TColor& operator*=(TColor& pt0, const TColor& pt1) { + pt0.r*=pt1.r; pt0.g*=pt1.g; pt0.b*=pt1.b; pt0.a*=pt1.a; + return pt0; +} + +// TMatrix operators +template +inline TMatrix operator + (const TMatrix& m1, const TMatrix& m2) { + return TMatrix(m1, m2, cv::Matx_AddOp()); +} +template +inline TMatrix operator + (const TMatrix& m1, const cv::Matx& m2) { + return cv::Matx(m1, m2, cv::Matx_AddOp()); +} +template +inline TMatrix operator + (const cv::Matx& m1, const TMatrix& m2) { + return TMatrix(m1, m2, cv::Matx_AddOp()); +} + +template +inline TMatrix operator - (const TMatrix& m1, const TMatrix& m2) { + return TMatrix(m1, m2, cv::Matx_SubOp()); +} +template +inline TMatrix operator - (const TMatrix& m1, const cv::Matx& m2) { + return TMatrix(m1, m2, cv::Matx_SubOp()); +} +template +inline TMatrix operator - (const cv::Matx& m1, const TMatrix& m2) { + return TMatrix(m1, m2, cv::Matx_SubOp()); +} +template +inline TMatrix operator - (const TMatrix& M) { + return TMatrix(M, TYPE(-1), cv::Matx_ScaleOp()); +} + +template +inline TMatrix operator * (const TMatrix& m1, const TMatrix& m2) { + return TMatrix(m1, m2, cv::Matx_MatMulOp()); +} + +template +inline TMatrix operator / (const TMatrix& mat, TYPE2 v) { + typedef typename std::conditional::value,TYPE2,REAL>::type real_t; + return TMatrix(mat, real_t(1)/v, cv::Matx_ScaleOp()); +} +template +inline TMatrix& operator /= (TMatrix& mat, TYPE2 v) { + return mat = mat/v; +} + +// TImage operators +template +inline TImage cvtImage(const TImage& image) { + TImage img(image.size()); + for (int r=0; r +FORCEINLINE SEACAVE::TPoint2 operator/(TYPEM n, const cv::Point_& d) { + return SEACAVE::TPoint2(n/d.x, n/d.y); +} +template +FORCEINLINE SEACAVE::TPoint3 operator/(TYPEM n, const cv::Point3_& d) { + return SEACAVE::TPoint3(n/d.x, n/d.y, n/d.z); +} +template +FORCEINLINE SEACAVE::TPixel operator/(TYPEM n, const SEACAVE::TPixel& d) { + return SEACAVE::TPixel(n/d.r, n/d.g, n/d.b); +} +template +FORCEINLINE SEACAVE::TColor operator/(TYPEM n, const SEACAVE::TColor& d) { + return SEACAVE::TColor(n/d.r, n/d.g, n/d.b, n/d.a); +} +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + + +// Informative template class for OpenCV "scalars" +#define DEFINE_GENERIC_CVDATATYPE(tp,ctp) namespace cv { \ +template<> class DataType< tp > { \ +public: \ + typedef tp value_type; \ + typedef value_type work_type; \ + typedef ctp channel_type; \ + typedef value_type vec_type; \ + enum { \ + generic_type = 0, \ + depth = DataDepth::value, \ + channels = sizeof(value_type)/sizeof(channel_type), \ + fmt = DataDepth::fmt, \ + type = CV_MAKETYPE(depth, channels) \ + }; \ +}; } +#define DEFINE_CVDATATYPE(tp) DEFINE_GENERIC_CVDATATYPE(tp,tp::Type) + +#define DEFINE_GENERIC_CVDATADEPTH(tp,ctp) namespace cv { \ +template<> class DataDepth< tp > { \ +public: \ + enum { \ + value = DataDepth< ctp >::value, \ + fmt = DataDepth< ctp >::fmt \ + }; \ +}; } +#define DEFINE_CVDATADEPTH(tp) DEFINE_GENERIC_CVDATADEPTH(tp,tp::Type) + +// define specialized cv:DataType<> +DEFINE_CVDATADEPTH(SEACAVE::hfloat) +DEFINE_CVDATADEPTH(SEACAVE::cuint32_t) + +// define specialized cv:DataType<> +DEFINE_CVDATATYPE(SEACAVE::hfloat) +DEFINE_CVDATATYPE(SEACAVE::cuint32_t) +DEFINE_GENERIC_CVDATATYPE(uint64_t,double) +/*----------------------------------------------------------------*/ +DEFINE_CVDATATYPE(SEACAVE::Point2i) +DEFINE_CVDATATYPE(SEACAVE::Point2hf) +DEFINE_CVDATATYPE(SEACAVE::Point2f) +DEFINE_CVDATATYPE(SEACAVE::Point2d) +/*----------------------------------------------------------------*/ +DEFINE_CVDATATYPE(SEACAVE::Point3i) +DEFINE_CVDATATYPE(SEACAVE::Point3hf) +DEFINE_CVDATATYPE(SEACAVE::Point3f) +DEFINE_CVDATATYPE(SEACAVE::Point3d) +/*----------------------------------------------------------------*/ +DEFINE_CVDATATYPE(SEACAVE::Pixel8U) +DEFINE_CVDATATYPE(SEACAVE::Pixel32F) +DEFINE_CVDATATYPE(SEACAVE::Pixel64F) +/*----------------------------------------------------------------*/ +DEFINE_CVDATATYPE(SEACAVE::Color8U) +DEFINE_CVDATATYPE(SEACAVE::Color32F) +DEFINE_CVDATATYPE(SEACAVE::Color64F) +/*----------------------------------------------------------------*/ +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DMatrix8S, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DMatrix8U, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DMatrix32S, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DMatrix32U, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DMatrix32F, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DMatrix64F, uint8_t) +/*----------------------------------------------------------------*/ +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DVector8S, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DVector8U, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DVector32S, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DVector32U, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DVector32F, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::DVector64F, uint8_t) +/*----------------------------------------------------------------*/ +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image8U, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image16F, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image32F, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image64F, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image8U3, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image8U4, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image32F3, uint8_t) +DEFINE_GENERIC_CVDATATYPE(SEACAVE::Image32F4, uint8_t) +/*----------------------------------------------------------------*/ + + +namespace SEACAVE { + +namespace CONVERT { + +// convert sRGB to/from linear value +// (see http://en.wikipedia.org/wiki/SRGB) +template +constexpr T sRGB2RGB(T x) { + return x <= T(0.04045) ? x * (T(1)/T(12.92)) : POW((x + T(0.055)) * (T(1)/T(1.055)), T(2.4)); +} +template +constexpr T RGB2sRGB(T x) { + return x <= T(0.0031308) ? T(12.92) * x : T(1.055) * POW(x, T(1)/T(2.4)) - T(0.055); +} +static const CAutoPtrArr g_ptrsRGB82RGBf([]() { + float* const buffer = new float[256]; + for (int i=0; i<256; ++i) + buffer[i] = sRGB2RGB(float(i)/255.f); + return buffer; +}()); + +// color conversion helper structures +template +struct NormRGB_t { + const TO v; + inline NormRGB_t(TI _v) : v(TO(_v)*(TO(1)/TO(255))) {} + inline operator TO () const { return v; } +}; +template +struct RGBUnNorm_t { + const TO v; + inline RGBUnNorm_t(TI _v) : v(TO(_v)*TO(255)) {} + inline operator TO () const { return v; } +}; +template +struct RoundF2U_t { + const TO v; + inline RoundF2U_t(TI _v) : v(ROUND2INT(_v)) {} + inline operator TO () const { return v; } +}; +template +struct sRGB2RGB_t { + const TO v; + inline sRGB2RGB_t(TI _v) : v(sRGB2RGB(TO(_v))) {} + inline operator TO () const { return v; } +}; +template +struct NormsRGB2RGB_t { + const TO v; + inline NormsRGB2RGB_t(TI _v) : v(sRGB2RGB(TO(_v)*(TO(1)/TO(255)))) {} + inline operator TO () const { return v; } +}; +template <> +struct NormsRGB2RGB_t { + const float v; + inline NormsRGB2RGB_t(uint8_t _v) : v(g_ptrsRGB82RGBf[_v]) {} + inline operator float () const { return v; } +}; +template +struct NormsRGB2RGBUnNorm_t { + const TO v; + inline NormsRGB2RGBUnNorm_t(TI _v) : v(sRGB2RGB(TO(_v)*(TO(1)/TO(255)))*TO(255)) {} + inline operator TO () const { return v; } +}; +template <> +struct NormsRGB2RGBUnNorm_t { + const float v; + inline NormsRGB2RGBUnNorm_t(uint8_t _v) : v(g_ptrsRGB82RGBf[_v]*255.f) {} + inline operator float () const { return v; } +}; + +} // namespace CONVERT +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +template +struct OppositeType { + typedef FLT Type; +}; +template <> +struct OppositeType { + typedef double Type; +}; +template <> +struct OppositeType { + typedef float Type; +}; +// Point2 +template +inline cv::Point_ Cast(const cv::Point_& pt) { + return pt; +} +template +inline TPoint2 Cast(const TPoint2& pt) { + return pt; +} +// Point3 +template +inline cv::Point3_ Cast(const cv::Point3_& pt) { + return pt; +} +template +inline TPoint3 Cast(const TPoint3& pt) { + return pt; +} +// Pixel +template +inline TPixel Cast(const TPixel& pt) { + return pt; +} +// Color +template +inline TColor Cast(const TColor& pt) { + return pt; +} +// Matrix +template +inline TMatrix Cast(const TMatrix& v) { + return v; +} +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +template +TPoint3 TPoint3::RotateAngleAxis(const TPoint3& pt, const TPoint3& angle_axis) +{ + TPoint3 result; + const TYPE theta2(normSq(angle_axis)); + if (theta2 > TYPE(1e-12)) { + // Away from zero, use the Rodriguez formula + // + // result = pt costheta + + // (w x pt) * sintheta + + // w (w . pt) (1 - costheta) + // + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + // + const TYPE theta(SQRT(theta2)); + const TPoint3 w(angle_axis*(TYPE(1)/theta)); + const TYPE costheta(COS(theta)); + const TYPE sintheta(SIN(theta)); + const TPoint3 w_cross_pt(w.cross(pt)); + const TYPE w_dot_pt(w.dot(pt)); + result = pt*costheta + w_cross_pt*sintheta + w*(TYPE(1.0)-costheta)*w_dot_pt; + } else { + // Near zero, the first order Taylor approximation of the rotation + // matrix R corresponding to a vector w and angle w is + // + // R = I + hat(w) * sin(theta) + // + // But sintheta ~ theta and theta * w = angle_axis, which gives us + // + // R = I + hat(w) + // + // and actually performing multiplication with the point pt, gives us + // R * pt = pt + w x pt. + // + // Switching to the Taylor expansion at zero helps avoid all sorts + // of numerical nastiness. + const TPoint3 w_cross_pt(angle_axis.cross(pt)); + result = pt + w_cross_pt; + } + return result; +} +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +template +inline TMatrix::TMatrix(TYPE v0) +{ + STATIC_ASSERT(channels >= 1); + val[0] = v0; + for (int i = 1; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1) +{ + STATIC_ASSERT(channels >= 2); + val[0] = v0; val[1] = v1; + for (int i = 2; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2) +{ + STATIC_ASSERT(channels >= 3); + val[0] = v0; val[1] = v1; val[2] = v2; + for (int i = 3; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3) +{ + STATIC_ASSERT(channels >= 4); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + for (int i = 4; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4) +{ + STATIC_ASSERT(channels >= 5); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; val[4] = v4; + for (int i = 5; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5) +{ + STATIC_ASSERT(channels >= 6); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; + for (int i = 6; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6) +{ + STATIC_ASSERT(channels >= 7); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; val[6] = v6; + for (int i = 7; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7) +{ + STATIC_ASSERT(channels >= 8); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7; + for (int i = 8; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8) +{ + STATIC_ASSERT(channels >= 9); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7; + val[8] = v8; + for (int i = 9; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8, TYPE v9) +{ + STATIC_ASSERT(channels >= 10); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7; + val[8] = v8; val[9] = v9; + for (int i = 10; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8, TYPE v9, TYPE v10, TYPE v11) +{ + STATIC_ASSERT(channels >= 12); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7; + val[8] = v8; val[9] = v9; val[10] = v10; val[11] = v11; + for (int i = 12; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8, TYPE v9, TYPE v10, TYPE v11, TYPE v12, TYPE v13) +{ + STATIC_ASSERT(channels == 14); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7; + val[8] = v8; val[9] = v9; val[10] = v10; val[11] = v11; + val[12] = v12; val[13] = v13; + for (int i = 14; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8, TYPE v9, TYPE v10, TYPE v11, TYPE v12, TYPE v13, TYPE v14, TYPE v15) +{ + STATIC_ASSERT(channels >= 16); + val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; + val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7; + val[8] = v8; val[9] = v9; val[10] = v10; val[11] = v11; + val[12] = v12; val[13] = v13; val[14] = v14; val[15] = v15; + for (int i = 16; i < channels; i++) + val[i] = TYPE(0); +} +template +inline TMatrix::TMatrix(const TYPE* values) +{ + for (int i = 0; i < channels; i++) + val[i] = values[i]; +} + + +template +inline bool TMatrix::IsEqual(const Base& rhs) const +{ + for (int i=0; i +inline bool TMatrix::IsEqual(const Base& rhs, TYPE eps) const +{ + for (int i=0; i +inline TMatrix TMatrix::RightNullSpace(int flags /*= 0*/) const +{ + STATIC_ASSERT(n > m); + const cv::SVD svd(*this, flags|cv::SVD::FULL_UV); + // the orthonormal basis of the null space is formed by the columns + // of svd.vt such that the corresponding singular values are 0 (n - m or svd.vt.cols - svd.w.rows) + ASSERT(svd.vt.rows == n && svd.vt.rows == svd.vt.cols && svd.w.rows == m); + // return result (last rows transposed) + return svd.vt.rowRange(m,n).t(); +} +// calculate right null-vector of the matrix ([n,1]) +template +inline TMatrix TMatrix::RightNullVector(int flags /*= 0*/) const +{ + const cv::SVD svd(*this, (m >= n ? flags : flags|cv::SVD::FULL_UV)); + // the singular values could be checked for numerical stability + // return result (last row transposed) + ASSERT(svd.vt.rows == n); + return *svd.vt.ptr< const TMatrix >(n-1); +} +// calculate left null-vector of the matrix ([m,1]) +template +inline TMatrix TMatrix::LeftNullVector(int flags /*= 0*/) const +{ + return TMatrix(Base::t()).RightNullVector(flags); +} +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +template +inline TYPE TDMatrix::getDetSquare() const +{ + ASSERT(cols == rows); + const int dim = rows; + const int subdim = dim-1; + // end of recursion in 1x1: + if (subdim==0) + return Base::operator()(0,0); + // end of recursion in 2x2: + if (subdim==1) + return (Base::operator()(0,0)*Base::operator()(1,1)- + Base::operator()(1,0)*Base::operator()(0,1)); + TYPE d = 0; + TDMatrix SubMatrix(subdim, subdim); + for (int sub=0; sub +inline TDMatrix TDMatrix::getAdjoint() const +{ + ASSERT(rows==cols); + TDMatrix Out(cols,rows); + TDMatrix SubMatrix(rows-1,cols-1); + for (int i=0; i +inline void TDMatrix::getSystemMatrix(TDMatrix& dest) const +{ + // resize result to square matrix + dest = TDMatrix::zeros(cols, cols); + /// Hessian is symmetric! so first diagonal then lower left + // diagonal + for (int col = 0; col +inline void TDMatrix::makeSymmetric() +{ + ASSERT(cols == rows); + const int num = cols; + for (int r=0; r +inline TYPE TDMatrix::getNormL1() const +{ + TYPE result = 0; + for (const TYPE* dataP = getData()+Base::total()-1; dataP >= getData(); dataP--) + result += ABS(*dataP); + return result; +} + + +template +inline double TDMatrix::getNormL2() const +{ + double result = 0; + for (const TYPE* dataP = getData()+Base::total()-1; dataP >= getData(); dataP--) + result += (double)((*dataP) * (*dataP)); + return SQRT(result); +} + +/** Kronecker-product with matrix B, result in dest */ +template +void TDMatrix::Kronecker(const TDMatrix& B, TDMatrix& dest) const +{ + const int A_rows = rows; + const int A_cols = cols; + const int B_rows = B.rows; + const int B_cols = B.cols; + + dest.newsize(A_rows*B_rows,A_cols*B_cols); + for (int i=0; i +void TDMatrix::SwapRows(int i, int r) +{ + TYPE* const rowi = Base::operator[](i); + TYPE* const rowr = Base::operator[](r); + for (int c=0; c +void TDMatrix::GaussJordan() +{ + //const bool verbose = false; + const int numr = rows, numc = cols; + const int max = MINF(numr, numc); + ///<< offset to right from diagonal, the working column is given by r+offs + int offs = 0; + for (int r=0; r=0; --r) { + // search for leading element + int c; + for (c=r; c=0; --row) { + const TYPE wval=Base::operator()(row,c); + for (int col=0; col +TDMatrix TDVector::getOuterProduct(const TDVector& v) const { + TDMatrix mat(rows,v.rows); + TYPE rVal; + for (int row=0; row +void TDVector::getKroneckerProduct(const TDVector& arg, TDVector& dst) const +{ + const int s1=rows, s2=arg.rows; + int l=0; + dst.newsize(s1*s2); + for (int i=0; i +template +TPixel TPixel::colorRamp(VT v, VT vmin, VT vmax) +{ + if (v < vmin) + v = vmin; + if (v > vmax) + v = vmax; + const TYPE dv((TYPE)(vmax - vmin)); + TPixel c(1,1,1); // white + if (v < vmin + (VT)(TYPE(0.25) * dv)) { + c.r = TYPE(0); + c.g = TYPE(4) * (v - vmin) / dv; + } else if (v < vmin + (VT)(TYPE(0.5) * dv)) { + c.r = TYPE(0); + c.b = TYPE(1) + TYPE(4) * (vmin + TYPE(0.25) * dv - v) / dv; + } else if (v < vmin + (VT)(TYPE(0.75) * dv)) { + c.r = TYPE(4) * (v - vmin - TYPE(0.5) * dv) / dv; + c.b = TYPE(0); + } else { + c.g = TYPE(1) + TYPE(4) * (vmin + TYPE(0.75) * dv - v) / dv; + c.b = TYPE(0); + } + return c; +} + +// Gray values are expected in the range [0, 1] and converted to RGB values. +template +TPixel TPixel::gray2color(ALT gray) +{ + ASSERT(ALT(0) <= gray && gray <= ALT(1)); + // Jet colormap inspired by Matlab. + auto const Interpolate = [](ALT val, ALT y0, ALT x0, ALT y1, ALT x1) -> ALT { + return (val - x0) * (y1 - y0) / (x1 - x0) + y0; + }; + auto const Base = [&Interpolate](ALT val) -> ALT { + if (val <= ALT(0.125)) { + return ALT(0); + } else if (val <= ALT(0.375)) { + return Interpolate(ALT(2) * val - ALT(1), ALT(0), ALT(-0.75), ALT(1), ALT(-0.25)); + } else if (val <= ALT(0.625)) { + return ALT(1); + } else if (val <= ALT(0.87)) { + return Interpolate(ALT(2) * val - ALT(1), ALT(1), ALT(0.25), ALT(0), ALT(0.75)); + } else { + return ALT(0); + } + }; + return TPixel().set( + Base(gray + ALT(0.25)), + Base(gray), + Base(gray - ALT(0.25)) + ); +} +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +// Find a pixel inside the image +template +inline const TYPE& TImage::getPixel(int y, int x) const +{ + if (x < 0) + x = 0; + else if (x >= cols) + x = cols-1; + if (y < 0) + y = 0; + else if (y >= rows) + y = rows-1; + return BaseBase::operator()(y,x); +} +/*----------------------------------------------------------------*/ + + +// sample by bilinear interpolation +template +template +TYPE TImage::sample(const TPoint2& pt) const +{ + const int lx((int)pt.x); + const int ly((int)pt.y); + const T x(pt.x-lx), x1(T(1)-x); + const T y(pt.y-ly), y1(T(1)-y); + return (BaseBase::operator()( ly, lx)*x1 + BaseBase::operator()( ly, lx+1)*x)*y1 + + (BaseBase::operator()(ly+1,lx)*x1 + BaseBase::operator()(ly+1,lx+1)*x)*y; +} +template +template +TYPE TImage::sampleSafe(const TPoint2& pt) const +{ + const int lx((int)pt.x); + const int ly((int)pt.y); + const T x(pt.x-lx), x1(T(1)-x); + const T y(pt.y-ly), y1(T(1)-y); + return (getPixel( ly, lx)*x1 + getPixel( ly, lx+1)*x)*y1 + + (getPixel(ly+1,lx)*x1 + getPixel(ly+1,lx+1)*x)*y; +} +/*----------------------------------------------------------------*/ + + +// sample by bilinear interpolation, using only pixels that meet the user condition +template +template +bool TImage::sample(TV& v, const TPoint2& pt, const Functor& functor) const +{ + const int lx((int)pt.x); + const int ly((int)pt.y); + const T x(pt.x-lx), x1(T(1)-x); + const T y(pt.y-ly), y1(T(1)-y); + const TYPE& x0y0(BaseBase::operator()(ly , lx )); const bool b00(functor(x0y0)); + const TYPE& x1y0(BaseBase::operator()(ly , lx+1)); const bool b10(functor(x1y0)); + const TYPE& x0y1(BaseBase::operator()(ly+1, lx )); const bool b01(functor(x0y1)); + const TYPE& x1y1(BaseBase::operator()(ly+1, lx+1)); const bool b11(functor(x1y1)); + if (!b00 && !b10 && !b01 && !b11) + return false; + v = TV(y1*(x1*Cast(b00 ? x0y0 : (b10 ? x1y0 : (b01 ? x0y1 : x1y1))) + x*Cast(b10 ? x1y0 : (b00 ? x0y0 : (b11 ? x1y1 : x0y1)))) + + y *(x1*Cast(b01 ? x0y1 : (b11 ? x1y1 : (b00 ? x0y0 : x1y0))) + x*Cast(b11 ? x1y1 : (b01 ? x0y1 : (b10 ? x1y0 : x0y0))))); + return true; +} +template +template +bool TImage::sampleSafe(TV& v, const TPoint2& pt, const Functor& functor) const +{ + const int lx((int)pt.x); + const int ly((int)pt.y); + const T x(pt.x-lx), x1(T(1)-x); + const T y(pt.y-ly), y1(T(1)-y); + const TYPE& x0y0(getPixel(ly , lx )); const bool b00(functor(x0y0)); + const TYPE& x1y0(getPixel(ly , lx+1)); const bool b10(functor(x1y0)); + const TYPE& x0y1(getPixel(ly+1, lx )); const bool b01(functor(x0y1)); + const TYPE& x1y1(getPixel(ly+1, lx+1)); const bool b11(functor(x1y1)); + if (!b00 && !b10 && !b01 && !b11) + return false; + v = TV(y1*(x1*Cast(b00 ? x0y0 : (b10 ? x1y0 : (b01 ? x0y1 : x1y1))) + x*Cast(b10 ? x1y0 : (b00 ? x0y0 : (b11 ? x1y1 : x0y1)))) + + y *(x1*Cast(b01 ? x0y1 : (b11 ? x1y1 : (b00 ? x0y0 : x1y0))) + x*Cast(b11 ? x1y1 : (b01 ? x0y1 : (b10 ? x1y0 : x0y0))))); + return true; +} +// same as above, but using default value if the condition is not met +template +template +TYPE TImage::sample(const TPoint2& pt, const Functor& functor, const TYPE& dv) const +{ + const int lx((int)pt.x); + const int ly((int)pt.y); + const T x(pt.x-lx), x1(T(1)-x); + const T y(pt.y-ly), y1(T(1)-y); + const TYPE& x0y0(BaseBase::operator()(ly , lx )); const bool b00(functor(x0y0)); + const TYPE& x1y0(BaseBase::operator()(ly , lx+1)); const bool b10(functor(x1y0)); + const TYPE& x0y1(BaseBase::operator()(ly+1, lx )); const bool b01(functor(x0y1)); + const TYPE& x1y1(BaseBase::operator()(ly+1, lx+1)); const bool b11(functor(x1y1)); + return TYPE(y1*(x1*Cast(b00 ? x0y0 : dv) + x*Cast(b10 ? x1y0 : dv)) + + y *(x1*Cast(b01 ? x0y1 : dv) + x*Cast(b11 ? x1y1 : dv))); +} +template +template +TYPE TImage::sampleSafe(const TPoint2& pt, const Functor& functor, const TYPE& dv) const +{ + const int lx((int)pt.x); + const int ly((int)pt.y); + const T x(pt.x-lx), x1(T(1)-x); + const T y(pt.y-ly), y1(T(1)-y); + const TYPE& x0y0(getPixel(ly , lx )); const bool b00(functor(x0y0)); + const TYPE& x1y0(getPixel(ly , lx+1)); const bool b10(functor(x1y0)); + const TYPE& x0y1(getPixel(ly+1, lx )); const bool b01(functor(x0y1)); + const TYPE& x1y1(getPixel(ly+1, lx+1)); const bool b11(functor(x1y1)); + return TYPE(y1*(x1*Cast(b00 ? x0y0 : dv) + x*Cast(b10 ? x1y0 : dv)) + + y *(x1*Cast(b01 ? x0y1 : dv) + x*Cast(b11 ? x1y1 : dv))); +} + +// same as above, sample image at a specified position, but using the given sampler +#include "Sampler.inl" +template +template +INTERTYPE TImage::sample(const SAMPLER& sampler, const TPoint2& pt) const +{ + return Sampler::Sample< TImage, SAMPLER, TPoint2, INTERTYPE >(*this, sampler, pt); +} + +// convert color image to gray +template +template +void TImage::toGray(TImage& out, int code, bool bNormalize, bool bSRGB) const +{ + #if 1 + typedef typename RealType::type Real; + ASSERT(code==cv::COLOR_RGB2GRAY || code==cv::COLOR_RGBA2GRAY || code==cv::COLOR_BGR2GRAY || code==cv::COLOR_BGRA2GRAY); + static const Real coeffsRGB[] = {Real(0.299), Real(0.587), Real(0.114)}; + static const Real coeffsBGR[] = {Real(0.114), Real(0.587), Real(0.299)}; + const Real* coeffs; + switch (code) { + case cv::COLOR_BGR2GRAY: + case cv::COLOR_BGRA2GRAY: + coeffs = coeffsBGR; + break; + case cv::COLOR_RGB2GRAY: + case cv::COLOR_RGBA2GRAY: + coeffs = coeffsRGB; + break; + default: + ASSERT("Unsupported image format" == NULL); + } + const Real &cb(coeffs[0]), &cg(coeffs[1]), &cr(coeffs[2]); + if (out.rows!=rows || out.cols!=cols) + out.create(rows, cols); + ASSERT(cv::Mat::isContinuous()); + ASSERT(out.cv::Mat::isContinuous()); + const int scn(this->cv::Mat::channels()); + T* dst = out.cv::Mat::template ptr(); + T* const dstEnd = dst + out.area(); + typedef typename cv::DataType::channel_type ST; + if (bSRGB) { + if (bNormalize) { + typedef typename CONVERT::NormsRGB2RGB_t ColConv; + for (const ST* src=cv::Mat::template ptr(); dst!=dstEnd; src+=scn) + *dst++ = T(cb*ColConv(src[0]) + cg*ColConv(src[1]) + cr*ColConv(src[2])); + } else { + typedef typename CONVERT::NormsRGB2RGBUnNorm_t ColConv; + for (const ST* src=cv::Mat::template ptr(); dst!=dstEnd; src+=scn) + *dst++ = T(cb*ColConv(src[0]) + cg*ColConv(src[1]) + cr*ColConv(src[2])); + } + } else { + if (bNormalize) { + typedef typename CONVERT::NormRGB_t ColConv; + for (const ST* src=cv::Mat::template ptr(); dst!=dstEnd; src+=scn) + *dst++ = T(cb*ColConv(src[0]) + cg*ColConv(src[1]) + cr*ColConv(src[2])); + } else { + for (const ST* src=cv::Mat::template ptr(); dst!=dstEnd; src+=scn) + *dst++ = T(cb*src[0] + cg*src[1] + cr*src[2]); + } + } + #else + cv::Mat cimg; + convertTo(cimg, cv::DataType::type); + cv::cvtColor(cimg, out, code); + if (bNormalize) + out *= T(1)/T(255); + #endif +} +/*----------------------------------------------------------------*/ + + +// compute scaled size such that the biggest dimension is scaled as desired +// and the smaller one maintains the aspect ratio as best as it can +template +cv::Size TImage::computeResize(const cv::Size& size, REAL scale) +{ + return cv::Size( + cv::saturate_cast((REAL)size.width*scale), + cv::saturate_cast((REAL)size.height*scale)); +} +// compute the final scaled size by performing successive resizes +// with the given scale value +template +cv::Size TImage::computeResize(const cv::Size& size, REAL scale, unsigned resizes) +{ + cv::Size scaledSize(size); + while (resizes-- > 0) + scaledSize = computeResize(scaledSize, scale); + return scaledSize; +} + +// compute image scale for a given max and min resolution +template +unsigned TImage::computeMaxResolution(unsigned width, unsigned height, unsigned& level, unsigned minImageSize, unsigned maxImageSize) +{ + // consider the native resolution the max(width,height) + const unsigned imageSize = MAXF(width, height); + // if the max level it's used, return original image size + if (level == 0) + return MINF(imageSize, maxImageSize); + // compute the resolution corresponding to the desired level + unsigned size = (imageSize >> level); + // if the image is too small + if (size < minImageSize) { + // start from the max level + level = 0; + while ((imageSize>>(level+1)) >= minImageSize) + ++level; + size = (imageSize>>level); + } + return MINF(size, maxImageSize); +} +template +unsigned TImage::computeMaxResolution(unsigned& level, unsigned minImageSize, unsigned maxImageSize) const +{ + return computeMaxResolution((unsigned)width(), (unsigned)height(), level, minImageSize, maxImageSize); +} +/*----------------------------------------------------------------*/ + + +// Raster the given triangle and output the position of each pixel of the triangle; +// based on "Advanced Rasterization" by Nick (Nicolas Capens) +// http://devmaster.net/forums/topic/1145-advanced-rasterization +template +template +void TImage::RasterizeTriangle(const TPoint2& v1, const TPoint2& v2, const TPoint2& v3, PARSER& parser) +{ + // 28.4 fixed-point coordinates + const int_t Y1 = ROUND2INT(T(16) * v1.y); + const int_t Y2 = ROUND2INT(T(16) * v2.y); + const int_t Y3 = ROUND2INT(T(16) * v3.y); + + const int_t X1 = ROUND2INT(T(16) * v1.x); + const int_t X2 = ROUND2INT(T(16) * v2.x); + const int_t X3 = ROUND2INT(T(16) * v3.x); + + // Deltas + const int_t DX12 = X1 - X2; + const int_t DX23 = X2 - X3; + const int_t DX31 = X3 - X1; + + const int_t DY12 = Y1 - Y2; + const int_t DY23 = Y2 - Y3; + const int_t DY31 = Y3 - Y1; + + // Fixed-point deltas + const int_t FDX12 = DX12 << 4; + const int_t FDX23 = DX23 << 4; + const int_t FDX31 = DX31 << 4; + + const int_t FDY12 = DY12 << 4; + const int_t FDY23 = DY23 << 4; + const int_t FDY31 = DY31 << 4; + + // Bounding rectangle + int minx = (int)((MINF3(X1, X2, X3) + 0xF) >> 4); + int maxx = (int)((MAXF3(X1, X2, X3) + 0xF) >> 4); + int miny = (int)((MINF3(Y1, Y2, Y3) + 0xF) >> 4); + int maxy = (int)((MAXF3(Y1, Y2, Y3) + 0xF) >> 4); + + // Block size, standard 8x8 (must be power of two) + const int q = 8; + + // Start in corner of 8x8 block + minx &= ~(q - 1); + miny &= ~(q - 1); + + // Half-edge constants + int_t C1 = DY12 * X1 - DX12 * Y1; + int_t C2 = DY23 * X2 - DX23 * Y2; + int_t C3 = DY31 * X3 - DX31 * Y3; + + // Correct for fill convention + if (DY12 < 0 || (DY12 == 0 && DX12 > 0)) C1++; + if (DY23 < 0 || (DY23 == 0 && DX23 > 0)) C2++; + if (DY31 < 0 || (DY31 == 0 && DX31 > 0)) C3++; + + // Loop through blocks + int pixy = miny; + for (int y = miny; y < maxy; y += q) + { + for (int x = minx; x < maxx; x += q) + { + // Corners of block + const int_t x0 = int_t(x) << 4; + const int_t x1 = int_t(x + q - 1) << 4; + const int_t y0 = int_t(y) << 4; + const int_t y1 = int_t(y + q - 1) << 4; + + // Evaluate half-space functions + const bool a00 = C1 + DX12 * y0 - DY12 * x0 > 0; + const bool a10 = C1 + DX12 * y0 - DY12 * x1 > 0; + const bool a01 = C1 + DX12 * y1 - DY12 * x0 > 0; + const bool a11 = C1 + DX12 * y1 - DY12 * x1 > 0; + const int a = (a00 << 0) | (a10 << 1) | (a01 << 2) | (a11 << 3); + + const bool b00 = C2 + DX23 * y0 - DY23 * x0 > 0; + const bool b10 = C2 + DX23 * y0 - DY23 * x1 > 0; + const bool b01 = C2 + DX23 * y1 - DY23 * x0 > 0; + const bool b11 = C2 + DX23 * y1 - DY23 * x1 > 0; + const int b = (b00 << 0) | (b10 << 1) | (b01 << 2) | (b11 << 3); + + const bool c00 = C3 + DX31 * y0 - DY31 * x0 > 0; + const bool c10 = C3 + DX31 * y0 - DY31 * x1 > 0; + const bool c01 = C3 + DX31 * y1 - DY31 * x0 > 0; + const bool c11 = C3 + DX31 * y1 - DY31 * x1 > 0; + const int c = (c00 << 0) | (c10 << 1) | (c01 << 2) | (c11 << 3); + + // Skip block when outside an edge + if (a == 0x0 || b == 0x0 || c == 0x0) continue; + + int nowpixy = pixy; + + // Accept whole block when totally covered + if (a == 0xF && b == 0xF && c == 0xF) + { + for (int iy = 0; iy < q; iy++) + { + for (int ix = x; ix < x + q; ix++) + parser(ImageRef(ix,nowpixy)); + + ++nowpixy; + } + } + else // Partially covered block + { + int_t CY1 = C1 + DX12 * y0 - DY12 * x0; + int_t CY2 = C2 + DX23 * y0 - DY23 * x0; + int_t CY3 = C3 + DX31 * y0 - DY31 * x0; + + for (int iy = y; iy < y + q; iy++) + { + int_t CX1 = CY1; + int_t CX2 = CY2; + int_t CX3 = CY3; + + for (int ix = x; ix < x + q; ix++) + { + if (CX1 > 0 && CX2 > 0 && CX3 > 0) + parser(ImageRef(ix,nowpixy)); + + CX1 -= FDY12; + CX2 -= FDY23; + CX3 -= FDY31; + } + + CY1 += FDX12; + CY2 += FDX23; + CY3 += FDX31; + + ++nowpixy; + } + } + } + + pixy += q; + } +} + +// same as above, but raster a triangle using barycentric coordinates: +// https://www.scratchapixel.com/lessons/3d-basic-rendering/rasterization-practical-implementation +template +template +void TImage::RasterizeTriangleBary(const TPoint2& v1, const TPoint2& v2, const TPoint2& v3, PARSER& parser) +{ + // compute bounding-box fully containing the triangle + const TPoint2 boxMin(MINF3(v1.x, v2.x, v3.x), MINF3(v1.y, v2.y, v3.y)); + const TPoint2 boxMax(MAXF3(v1.x, v2.x, v3.x), MAXF3(v1.y, v2.y, v3.y)); + // check the bounding-box intersects the image + const cv::Size size(parser.Size()); + if (boxMax.x < T(0) || boxMin.x > T(size.width - 1) || + boxMax.y < T(0) || boxMin.y > T(size.height - 1)) + return; + // clip bounding-box to be fully contained by the image + ImageRef boxMinI(FLOOR2INT(boxMin)); + ImageRef boxMaxI(CEIL2INT(boxMax)); + Base::clip(boxMinI, boxMaxI, size); + // ignore back oriented triangles (negative area) + const T area(EdgeFunction(v1, v2, v3)); + if (CULL && area <= 0) + return; + // parse all pixels inside the bounding-box + const T invArea(T(1) / area); + for (int y = boxMinI.y; y <= boxMaxI.y; ++y) { + for (int x = boxMinI.x; x <= boxMaxI.x; ++x) { + const ImageRef pt(x, y); + const TPoint2 p(Cast(pt)); + // discard point if not in triangle; + // testing only for negative barycentric coordinates + // guarantees all will be in [0,1] at the end of all checks + const T b1(EdgeFunction(v2, v3, p) * invArea); + if (b1 < 0) + continue; + const T b2(EdgeFunction(v3, v1, p) * invArea); + if (b2 < 0) + continue; + const T b3(EdgeFunction(v1, v2, p) * invArea); + if (b3 < 0) + continue; + // output pixel + parser(pt, TPoint3(b1, b2, b3)); + } + } +} + +// drawing line between 2 points from left to right +// papb -> pcpd +// pa, pb, pc, pd must then be sorted before +template +inline void _ProcessScanLine(int y, const TPoint3& pa, const TPoint3& pb, const TPoint3& pc, const TPoint3& pd, PARSER& parser) { + // Thanks to current Y, we can compute the gradient to compute others values like + // the starting X (sx) and ending X (ex) to draw between + // if pa.y == pb.y or pc.y == pd.y, gradient is forced to 1 + const T gradient1 = CLAMP(pa.y != pb.y ? (T(y) - pa.y) / (pb.y - pa.y) : T(1), T(0), T(1)); + const T gradient2 = CLAMP(pc.y != pd.y ? (T(y) - pc.y) / (pd.y - pc.y) : T(1), T(0), T(1)); + + const int sx = FLOOR2INT(lerp(pa.x, pb.x, gradient1)); + const int ex = FLOOR2INT(lerp(pc.x, pd.x, gradient2)); + + // starting Z & ending Z + const T z1 = lerp(pa.z, pb.z, gradient1); + const T z2 = lerp(pc.z, pd.z, gradient2); + + // drawing a line from left (sx) to right (ex) + const T invd = T(1) / (T)(ex - sx); + for (int x = sx; x < ex; ++x) { + const T gradient = T(x - sx) * invd; + ASSERT(gradient >= T(0) && gradient <= T(1)); + const T z = lerp(z1, z2, gradient); + parser(ImageRef(x,y), z); + } +} +// Raster the given triangle and output the position and depth of each pixel of the triangle; +// based on "Learning how to write a 3D software engine � Rasterization & Z-Buffering" by Nick (David Rousset) +// http://blogs.msdn.com/b/davrous/archive/2013/06/21/tutorial-part-4-learning-how-to-write-a-3d-software-engine-in-c-ts-or-js-rasterization-amp-z-buffering.aspx +template +template +void TImage::RasterizeTriangleDepth(TPoint3 p1, TPoint3 p2, TPoint3 p3, PARSER& parser) +{ + // sorting the points in order to always have this order on screen p1, p2 & p3 + // with p1 always up (thus having the Y the lowest possible to be near the top screen) + // then p2 between p1 & p3 + if (p1.y > p2.y) + std::swap(p1, p2); + if (p2.y > p3.y) + std::swap(p2, p3); + if (p1.y > p2.y) + std::swap(p1, p2); + + // computing lines' directions (slopes) + // http://en.wikipedia.org/wiki/Slope + const T dP1P2 = (p2.y - p1.y > 0 ? (p2.x - p1.x) / (p2.y - p1.y) : T(0)); + const T dP1P3 = (p3.y - p1.y > 0 ? (p3.x - p1.x) / (p3.y - p1.y) : T(0)); + + // first case where triangles are like that: + // P1 + // - + // -- + // - - + // - - + // - - P2 + // - - + // - - + // - + // P3 + if (dP1P2 > dP1P3) { + for (int y = (int)p1.y; y <= (int)p3.y; ++y) { + if (y < p2.y) + _ProcessScanLine(y, p1, p3, p1, p2, parser); + else + _ProcessScanLine(y, p1, p3, p2, p3, parser); + } + } + // second case where triangles are like that: + // P1 + // - + // -- + // - - + // - - + // P2 - - + // - - + // - - + // - + // P3 + else { + for (int y = (int)p1.y; y <= (int)p3.y; ++y) { + if (y < p2.y) + _ProcessScanLine(y, p1, p2, p1, p3, parser); + else + _ProcessScanLine(y, p2, p3, p1, p3, parser); + } + } +} + + +template +template +void TImage::DrawLine(const TPoint2& p1, const TPoint2& p2, PARSER& parser) +{ + #if 0 + const TPoint2 d(ABS(p2 - p1)); + const int sx(p1.xd.y ? d.x : -d.y)/2), e2; + + const ImageRef x2(p2); + ImageRef p(p1); + while (true) { + parser(p); + if (p == x2) break; + e2 = err; + if (e2 >-d.x) { err -= d.y; p.x += sx; } + if (e2 < d.y) { err += d.x; p.y += sy; } + } + #else + // Bresenham's line algorithm + // https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm + T x1(p1.x), y1(p1.y), x2(p2.x), y2(p2.y); + const bool steep(ABS(y2 - y1) > ABS(x2 - x1)); + if (steep) { + std::swap(x1, y1); + std::swap(x2, y2); + } + if (x1 > x2) { + std::swap(x1, x2); + std::swap(y1, y2); + } + const T dx(x2 - x1); + const T dy(ABS(y2 - y1)); + + T error(dx / 2.f); + int y(ROUND2INT(y1)); + const int ystep((y1 < y2) ? 1 : -1); + const int maxX(ROUND2INT(x2)); + for (int x=ROUND2INT(x1); x +bool TImage::DrawLineAntialias(Point2f x1, Point2f x2, + FncDrawPointAntialias fncDrawPoint, void* pData) +{ + bool revert = false; + const Point2f dx = x2 - x1; + if (ABS(dx.x) > ABS(dx.y)) { + const ImageRef delta(0,1); + if (x2.x < x1.x) { + std::swap(x1, x2); + revert = true; + } + + const float gradient = dx.y / dx.x; + int xpxl1 = round_(x1.x); + float yend = x1.y + gradient*(float(xpxl1) - x1.x); + int ypxl1 = ipart_(yend); + float xgap = rfpart_(x1.x + 0.5); + float tmp = fpart_(yend)*xgap; + fncDrawPoint(ImageRef(xpxl1, ypxl1), delta, xgap-tmp, tmp, pData); + + float intery = yend + gradient; + int xpxl2 = round_(x2.x); + yend = x2.y + gradient*(float(xpxl2) - x2.x); + int ypxl2 = ipart_(yend); + for (int x=xpxl1+1; x +bool TImage::DrawLineAntialias(const ImageRef& x1, const ImageRef& x2, + FncDrawPointAntialias fncDrawPoint, void* pData) +{ + const ImageRef dx = x2 - x1; + if (ABS(dx.x) > ABS(dx.y)) { + const ImageRef delta(0,1); + const float gradient = (float)dx.y / dx.x; + float intery = float(x1.y) + gradient; + if (x2.x < x1.x) { + for (int x=x1.x-1; x>x2.x; --x) { + const float tmp = fpart_(intery); + fncDrawPoint(ImageRef(x, ipart_(intery)), delta, 1.f-tmp, tmp, pData); + intery += gradient; + } + return true; + } + else { + for (int x=x1.x+1; xx2.y; --y) { + const float tmp = fpart_(interx); + fncDrawPoint(ImageRef(ipart_(interx), y), delta, 1.f-tmp, tmp, pData); + interx += gradient; + } + return true; + } + else { + for (int y=x1.y+1; y +template +void TImage::DilateMean(TImage& dst, const TYPE& invalid) const +{ + TImage out; + Base::copyTo(out); + const int RowsEnd(rows-HalfSize); + const int ColsEnd(cols-HalfSize); + for (int r=HalfSize; r 0) + out(r,c) = (n > 1 ? vo/n : vo); + } + } + dst = out; +} +/*----------------------------------------------------------------*/ + + +template +bool TImage::Load(const String& fileName) +{ + if (Util::getFileExt(fileName).ToLower() == ".pfm") { + if (Base::depth() != CV_32F) + return false; + File fImage(fileName, File::READ, File::OPEN); + if (!fImage.isOpen()) + return false; + ASSERT(sizeof(float) == 4); + int i; + char buffer[128]; + // check header + for (i=0; i<4; ++i) { + if (fImage.read(buffer+i, 1) != 1) + return false; + if (buffer[i] == '\n') + break; + } + if (buffer[0] != 'P' || buffer[1] != 'f' || buffer[i] != '\n') + return false; + // read resolution + int w, h; + for (i=0; i<127; ++i) { + if (fImage.read(buffer+i, 1) != 1) + return false; + if (buffer[i] == '\n') + break; + } + buffer[i] = 0; + if (sscanf(buffer, "%d %d", &w, &h) != 2) + return false; + // read number of channels + double sc; + for (i=0; i<127; ++i) { + if (fImage.read(buffer+i, 1) != 1) + return false; + if (buffer[i] == '\n') + break; + } + buffer[i] = 0; + if (sscanf(buffer, "%lf", &sc) != 1) + return false; + const bool bLittleEndian(sc < 0); + #if __BYTE_ORDER == __LITTLE_ENDIAN + ASSERT(bLittleEndian); + #else + ASSERT(!bLittleEndian); + #endif + const int nChannels(bLittleEndian ? -((int)sc) : (int)sc); + if (nChannels != Base::channels()) + return false; + Base::create(h, w); + ASSERT(sizeof(float)*Base::channels() == Base::step.p[1]); + const size_t rowbytes((size_t)Base::size.p[1]*Base::step.p[1]); + for (int i=rows; i>0; ) + if (fImage.read(cv::Mat::template ptr(--i), rowbytes) != rowbytes) + return false; + return true; + } + cv::Mat img(cv::imread(fileName, cv::IMREAD_UNCHANGED)); + if (img.empty()) { + VERBOSE("error: loading image '%s'", fileName.c_str()); + return false; + } + if (img.channels() != Base::channels()) { + if (img.channels() == 3 && Base::channels() == 1) + cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); + else if (img.channels() == 1 && Base::channels() == 3) + cv::cvtColor(img, img, cv::COLOR_GRAY2BGR); + else if (img.channels() == 4 && Base::channels() == 1) + cv::cvtColor(img, img, cv::COLOR_BGRA2GRAY); + else if (img.channels() == 1 && Base::channels() == 4) + cv::cvtColor(img, img, cv::COLOR_GRAY2BGRA); + else if (img.channels() == 4 && Base::channels() == 3) + cv::cvtColor(img, img, cv::COLOR_BGRA2BGR); + } + if (img.type() == Base::type()) + cv::swap(img, *this); + else + img.convertTo(*this, Base::type()); + return true; +} +/*----------------------------------------------------------------*/ + +template +bool TImage::Save(const String& fileName) const +{ + std::vector compression_params; + const String ext(Util::getFileExt(fileName).ToLower()); + if (ext == ".png") { + compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION); + compression_params.push_back(6); + } else + if (ext == ".jpg") { + compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); + compression_params.push_back(95); + } else + if (ext == ".pfm") { + if (Base::depth() != CV_32F) + return false; + Util::ensureFolder(fileName); + File fImage(fileName, File::WRITE, File::CREATE | File::TRUNCATE); + if (!fImage.isOpen()) + return false; + ASSERT(sizeof(float) == 4); + #if __BYTE_ORDER == __LITTLE_ENDIAN + static const double scale(-1.0); + #else + static const double scale(1.0); + #endif + fImage.print("Pf\n%d %d\n%lf\n", width(), height(), scale*Base::channels()); + ASSERT(sizeof(float)*Base::channels() == Base::step.p[1]); + const size_t rowbytes = (size_t)Base::size.p[1]*Base::step.p[1]; + for (int i=rows; i>0; ) + fImage.write(cv::Mat::template ptr(--i), rowbytes); + return true; + } + + try { + if (!cv::imwrite(fileName, *this, compression_params)) { + VERBOSE("error: saving image '%s'", fileName.c_str()); + return false; + } + } + catch (std::runtime_error& ex) { + VERBOSE("error: saving image '%s' (exception: %s)", fileName.c_str(), ex.what()); + return false; + } + return true; +} +/*----------------------------------------------------------------*/ + +#ifndef _RELEASE +template +void TImage::Show(const String& winname, int delay, bool bDestroy) const +{ + cv::imshow(winname, *this); + cv::waitKey(delay); + if (bDestroy) + cv::destroyWindow(winname); +} +/*----------------------------------------------------------------*/ +#endif + + +// C L A S S ////////////////////////////////////////////////////// + +// Create a derivative kernel, such that you can take the +// derivative of an image by convolving with the kernel horizontally and vertically +inline const TMatrix& CreateDerivativeKernel3x3() { + static const double kernel[3*3] = { + -1.0/8.0, 0.0, 1.0/8.0, + -2.0/8.0, 0.0, 2.0/8.0, + -1.0/8.0, 0.0, 1.0/8.0 + }; + return *((const TMatrix*)kernel); +} +inline const TMatrix& CreateDerivativeKernel3x3xx() { + static const double kernel[3*3] = { + 1.0/6.0, -2.0/6.0, 1.0/6.0, + 4.0/6.0, -8.0/6.0, 4.0/6.0, + 1.0/6.0, -2.0/6.0, 1.0/6.0 + }; + return *((const TMatrix*)kernel); +} +inline const TMatrix& CreateDerivativeKernel3x3xy() { + static const double kernel[3*3] = { + 1.0/4.0, 0.0, -1.0/4.0, + 0.0, 0.0, 0.0, + -1.0/4.0, 0.0, 1.0/4.0 + }; + return *((const TMatrix*)kernel); +} +// same as above, but using central differences like approach: Noise Robust Gradient Operators +// (see: http://www.holoborodko.com/pavel/image-processing/edge-detection) +inline const TMatrix& CreateDerivativeKernel3x5() { + static const double kernel[3*5] = { + -1.0/32.0, -2.0/32.0, 0.0, 2.0/32.0, 1.0/32.0, + -2.0/32.0, -4.0/32.0, 0.0, 4.0/32.0, 2.0/32.0, + -1.0/32.0, -2.0/32.0, 0.0, 2.0/32.0, 1.0/32.0 + }; + return *((const TMatrix*)kernel); +} +inline const TMatrix& CreateDerivativeKernel5x7() { + static const double kernel[5*7] = { + -1.0/512.0, -4.0/512.0, -5.0/512.0, 0.0, 5.0/512.0, 4.0/512.0, 1.0/512.0, + -4.0/512.0, -16.0/512.0, 20.0/512.0, 0.0, 20.0/512.0, 16.0/512.0, 4.0/512.0, + -6.0/512.0, -24.0/512.0, 30.0/512.0, 0.0, 30.0/512.0, 24.0/512.0, 6.0/512.0, + -4.0/512.0, -16.0/512.0, 20.0/512.0, 0.0, 20.0/512.0, 16.0/512.0, 4.0/512.0, + -1.0/512.0, -4.0/512.0, -5.0/512.0, 0.0, 5.0/512.0, 4.0/512.0, 1.0/512.0 + }; + return *((const TMatrix*)kernel); +} + +// Zero mean Gaussian. +inline double Gaussian(double x, double sigma) { + return 1/sqrt(2*M_PI*sigma*sigma) * exp(-(x*x/2.0/sigma/sigma)); +} +// 2D gaussian (zero mean) +// (see: (9) in http://mathworld.wolfram.com/GaussianFunction.html) +inline double Gaussian2D(double x, double y, double sigma) { + return 1.0/(2.0*M_PI*sigma*sigma) * exp( -(x*x+y*y)/(2.0*sigma*sigma)); +} +inline double GaussianDerivative(double x, double sigma) { + return -x / sigma / sigma * Gaussian(x, sigma); +} +// Solve the inverse of the Gaussian for positive x. +inline double GaussianInversePositive(double y, double sigma) { + return sqrt(-2.0 * sigma * sigma * log(y * sigma * sqrt(2.0*M_PI))); +} + +// Compute the Gaussian kernel width corresponding to the given sigma. +inline int ComputeGaussianKernelWidth(double sigma) { + ASSERT(sigma >= 0.0); + // 0.004 implies a 3 pixel kernel with 1 pixel sigma. + const double truncation_factor(0.004); + // Calculate the kernel size based on sigma such that it is odd. + const double precisehalfwidth(GaussianInversePositive(truncation_factor, sigma)); + int width = ROUND2INT(2*precisehalfwidth); + if (width % 2 == 0) + width++; + return width; +} +// Compute a Gaussian kernel, such that you can compute the blurred image +// by convolving with the kernel horizontally then vertically. +inline void ComputeGaussianKernel(double sigma, DVector64F& kernel) { + const int width(ComputeGaussianKernelWidth(sigma)); + // Calculate the gaussian kernel and its derivative. + kernel.create(width); + kernel.memset(0); + const int halfwidth(width / 2); + for (int i = -halfwidth; i <= halfwidth; ++i) + kernel(i + halfwidth) = Gaussian(i, sigma); + // Since images should not get brighter or darker, normalize. + cv::normalize(kernel, kernel, 1.0, 0.0, cv::NORM_L1); +} +// Compute a Gaussian kernel and derivative, such that you can take the +// derivative of an image by convolving with the kernel horizontally then the +// derivative vertically to get (eg) the y derivative. +inline void ComputeGaussianKernel(double sigma, DVector64F& kernel, DVector64F& derivative) { + const int width(ComputeGaussianKernelWidth(sigma)); + // Calculate the gaussian kernel and its derivative. + kernel.create(width); + derivative.create(width); + kernel.memset(0); + derivative.memset(0); + const int halfwidth(width / 2); + for (int i = -halfwidth; i <= halfwidth; ++i) { + kernel(i + halfwidth) = Gaussian(i, sigma); + derivative(i + halfwidth) = GaussianDerivative(i, sigma); + } + // Since images should not get brighter or darker, normalize. + cv::normalize(kernel, kernel, 1.0, 0.0, cv::NORM_L1); + // Normalize the derivative differently. See + // www.cs.duke.edu/courses/spring03/cps296.1/handouts/Image%20Processing.pdf + double factor(0); + for (int i = -halfwidth; i <= halfwidth; ++i) { + factor -= derivative(i+halfwidth)*i; + } + derivative /= factor; +} + +template +void FastConvolve(const DVector64F& kernel, int width, int height, + const Type* src, int src_stride, int src_line_stride, + Type* dst, int dst_stride) { + double coefficients[2 * size + 1]; + for (int k = 0; k < 2 * size + 1; ++k) { + coefficients[k] = kernel(2 * size - k); + } + // Fast path: if the kernel has a certain size, use the constant sized loops. + for (int y = 0; y < height; ++y) { + for (int x = 0; x < width; ++x) { + double sum(0); + for (int k = -size; k <= size; ++k) { + if (vertical) { + if (y + k >= 0 && y + k < height) { + sum += double(src[k * src_line_stride]) * coefficients[k + size]; + } + } else { + if (x + k >= 0 && x + k < width) { + sum += double(src[k * src_stride]) * coefficients[k + size]; + } + } + } + dst[0] = static_cast(sum); + src += src_stride; + dst += dst_stride; + } + } +} +template +void Convolve(const TImage& in, + const DVector64F& kernel, + TImage& out) { + ASSERT(kernel.rows % 2 == 1); + ASSERT(&in != &out); + + const int width = in.width(); + const int height = in.height(); + out.create(in.size()); + + const int src_line_stride = in.step1(0); + const int src_stride = in.step1(1); + const int dst_stride = out.step1(1); + const Type* src = in.cv::Mat::template ptr(); + Type* dst = out.cv::Mat::template ptr(); + + // Use a dispatch table to make most convolutions used in practice use the + // fast path. + const int half_width(kernel.rows / 2); + switch (half_width) { + #define static_convolution(size) case size: \ + FastConvolve(kernel, width, height, src, src_stride, \ + src_line_stride, dst, dst_stride); break; + static_convolution(1) + static_convolution(2) + static_convolution(3) + static_convolution(4) + static_convolution(5) + static_convolution(6) + static_convolution(7) + #undef static_convolution + default: + for (int y = 0; y < height; ++y) { + for (int x = 0; x < width; ++x) { + double sum = 0; + // Slow path: this loop cannot be unrolled. + for (int k = -half_width; k <= half_width; ++k) { + if (vertical) { + if (y + k >= 0 && y + k < height) { + sum += double(src[k * src_line_stride]) * kernel(2 * half_width - (k + half_width)); + } + } else { + if (x + k >= 0 && x + k < width) { + sum += double(src[k * src_stride]) * kernel(2 * half_width - (k + half_width)); + } + } + } + dst[0] = static_cast(sum); + src += src_stride; + dst += dst_stride; + } + } + } +} +template +inline void ConvolveHorizontal(const TImage& in, const DVector64F& kernel, TImage& out) { + Convolve(in, kernel, out); +} +template +inline void ConvolveVertical(const TImage& in, const DVector64F& kernel, TImage& out) { + Convolve(in, kernel, out); +} + +// Compute the gaussian blur of an image, and store the results in one channel. +template +void BlurredImage(const TImage& in, TImage& blurred, double sigma=0.32) { + ASSERT(in.channels() == 1); + DVector64F kernel; + ComputeGaussianKernel(sigma, kernel); + TImage tmp; + // Compute convolved image. + blurred.create(in.size()); + ConvolveVertical(in, kernel, tmp); + ConvolveHorizontal(tmp, kernel, blurred); +} +// Compute the gaussian blur of an image and the derivatives of the blurred +// image, and store the results in three channels. +template +void BlurredImageAndDerivatives(const TImage& in, TImage& blurred, TImage grad[2], double sigma=0.32) { + ASSERT(in.channels() == 1); + DVector64F kernel, derivative; + ComputeGaussianKernel(sigma, kernel, derivative); + TImage tmp, dir[2]; + // Compute convolved image. + blurred.create(in.size()); + ConvolveVertical(in, kernel, tmp); + ConvolveHorizontal(tmp, kernel, blurred); + // Compute first derivative in x. + ConvolveHorizontal(tmp, derivative, grad[0]); + // Compute first derivative in y. + ConvolveHorizontal(in, kernel, tmp); + ConvolveVertical(tmp, derivative, grad[1]); +} +template +void BlurredImageAndDerivatives(const TImage& in, TImage& blurred, TImage< TPoint2 >& grad, double sigma=0.32) { + TImage dir[2]; + BlurredImageAndDerivatives(in, blurred, grad, sigma); + cv::merge(dir, 2, grad); +} +/*----------------------------------------------------------------*/ + + +// C L A S S ////////////////////////////////////////////////////// + +// returns determinant, note that if it's zero no inversion done; +// use Mi == M to store result into M +template +TYPE InvertMatrix3x3(const TYPE* m, TYPE* mi) { + const TYPE d(m[0]*(m[4]*m[8]-m[5]*m[7])+m[1]*(m[5]*m[6]-m[3]*m[8])+m[2]*(m[3]*m[7]-m[4]*m[6])); + if (d == TYPE(0)) + return TYPE(0); + const TYPE invd = TYPE(1)/d; + + TYPE mc[9]; + const TYPE* mm = m; + if (mi == m) { + memcpy(mc, m, sizeof(TYPE)*9); + mm = mc; + } + + mi[0] = (mm[4]*mm[8]-mm[5]*mm[7])*invd; + mi[1] = (mm[2]*mm[7]-mm[1]*mm[8])*invd; + mi[2] = (mm[1]*mm[5]-mm[2]*mm[4])*invd; + mi[3] = (mm[5]*mm[6]-mm[3]*mm[8])*invd; + mi[4] = (mm[0]*mm[8]-mm[2]*mm[6])*invd; + mi[5] = (mm[2]*mm[3]-mm[0]*mm[5])*invd; + mi[6] = (mm[3]*mm[7]-mm[4]*mm[6])*invd; + mi[7] = (mm[1]*mm[6]-mm[0]*mm[7])*invd; + mi[8] = (mm[0]*mm[4]-mm[1]*mm[3])*invd; + + return d; +} +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + + +// C L A S S ////////////////////////////////////////////////////// + +#ifdef _USE_EIGEN + +///Compute a rotation exponential using the Rodrigues Formula. +///The rotation axis is given by \f$\vec{w}\f$, and the rotation angle must +///be computed using \f$ \theta = |\vec{w}|\f$. This is provided as a separate +///function primarily to allow fast and rough matrix exponentials using fast +///and rough approximations to \e A and \e B. +/// +///@param w Vector about which to rotate. +///@param A \f$\frac{\sin \theta}{\theta}\f$ +///@param B \f$\frac{1 - \cos \theta}{\theta^2}\f$ +///@param R Matrix to hold the return value. +///@relates SO3 +template +inline void eigen_SO3_exp(const typename Eigen::SO3::Vec3& w, typename Eigen::SO3::Mat3& R) { + static const Precision one_6th(1.0/6.0); + static const Precision one_20th(1.0/20.0); + //Use a Taylor series expansion near zero. This is required for + //accuracy, since sin t / t and (1-cos t)/t^2 are both 0/0. + Precision A, B; + const Precision theta_sq(w.squaredNorm()); + if (theta_sq < Precision(1e-8)) { + A = Precision(1) - one_6th * theta_sq; + B = Precision(0.5); + } else { + if (theta_sq < Precision(1e-6)) { + B = Precision(0.5) - Precision(0.25) * one_6th * theta_sq; + A = Precision(1) - theta_sq * one_6th*(Precision(1) - one_20th * theta_sq); + } else { + const Precision theta(sqrt(theta_sq)); + const Precision inv_theta(Precision(1)/theta); + A = sin(theta) * inv_theta; + B = (Precision(1) - cos(theta)) * (inv_theta * inv_theta); + } + } + { + const Precision wx2(w(0)*w(0)); + const Precision wy2(w(1)*w(1)); + const Precision wz2(w(2)*w(2)); + R(0,0) = Precision(1) - B*(wy2 + wz2); + R(1,1) = Precision(1) - B*(wx2 + wz2); + R(2,2) = Precision(1) - B*(wx2 + wy2); + } + { + const Precision a(A*w[2]); + const Precision b(B*(w[0]*w[1])); + R(0,1) = b - a; + R(1,0) = b + a; + } + { + const Precision a(A*w[1]); + const Precision b(B*(w[0]*w[2])); + R(0,2) = b + a; + R(2,0) = b - a; + } + { + const Precision a(A*w[0]); + const Precision b(B*(w[1]*w[2])); + R(1,2) = b - a; + R(2,1) = b + a; + } +} +template +inline typename Eigen::SO3::Mat3 Eigen::SO3::exp(const Vec3& w) const { + Mat3 result; + eigen_SO3_exp(w, result); + return result; +} + +/// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra. +/// See the Detailed Description for details of this vector. +template +inline void eigen_SO3_ln(const typename Eigen::SO3::Mat3& R, typename Eigen::SO3::Vec3& w) { + const Precision cos_angle((R(0,0) + R(1,1) + R(2,2) - Precision(1)) * Precision(0.5)); + w(0) = (R(2,1)-R(1,2))*Precision(0.5); + w(1) = (R(0,2)-R(2,0))*Precision(0.5); + w(2) = (R(1,0)-R(0,1))*Precision(0.5); + + const Precision sin_angle_abs(sqrt(w.squaredNorm())); + if (cos_angle > Precision(M_SQRT1_2)) { // [0 - Pi/4] use asin + if (sin_angle_abs > Precision(0)) + w *= asin(sin_angle_abs) / sin_angle_abs; + } else if (cos_angle > Precision(-M_SQRT1_2)) { // [Pi/4 - 3Pi/4] use acos, but antisymmetric part + if (sin_angle_abs > Precision(0)) + w *= acos(cos_angle) / sin_angle_abs; + } else { // rest use symmetric part + // antisymmetric part vanishes, but still large rotation, need information from symmetric part + const Precision angle(Precision(M_PI) - asin(sin_angle_abs)); + const Precision d0(R(0,0) - cos_angle); + const Precision d1(R(1,1) - cos_angle); + const Precision d2(R(2,2) - cos_angle); + typename Eigen::SO3::Vec3 r2; + if (d0*d0 > d1*d1 && d0*d0 > d2*d2) { // first is largest, fill with first column + r2(0) = d0; + r2(1) = (R(1,0)+R(0,1))*Precision(0.5); + r2(2) = (R(0,2)+R(2,0))*Precision(0.5); + } else if (d1*d1 > d2*d2) { // second is largest, fill with second column + r2(0) = (R(1,0)+R(0,1))*Precision(0.5); + r2(1) = d1; + r2(2) = (R(2,1)+R(1,2))*Precision(0.5); + } else { // third is largest, fill with third column + r2(0) = (R(0,2)+R(2,0))*Precision(0.5); + r2(1) = (R(2,1)+R(1,2))*Precision(0.5); + r2(2) = d2; + } + // flip, if we point in the wrong direction! + if (r2.dot(w) < Precision(0)) + r2 *= Precision(-1); + w = r2 * (angle/r2.norm()); + } +} +template +inline typename Eigen::SO3::Vec3 Eigen::SO3::ln() const { + Vec3 result; + eigen_SO3_ln(mat, result); + return result; +} + +/// Write an SO3 to a stream +/// @relates SO3 +template +inline std::ostream& operator<<(std::ostream& os, const Eigen::SO3& rhs) { + return os << rhs.get_matrix(); +} +/// Read from SO3 to a stream +/// @relates SO3 +template +inline std::istream& operator>>(std::istream& is, Eigen::SO3& rhs) { + is >> rhs.mat; + rhs.coerce(); + return is; +} + +/// Right-multiply by a Vector +/// @relates SO3 +template +inline Eigen::Matrix operator*(const Eigen::SO3

& lhs, const Eigen::Matrix& rhs) { + return lhs.get_matrix() * rhs; +} +/// Left-multiply by a Vector +/// @relates SO3 +template +inline Eigen::Matrix operator*(const Eigen::Matrix& lhs, const Eigen::SO3

& rhs) { + return lhs * rhs.get_matrix(); +} +/// Right-multiply by a matrix +/// @relates SO3 +template +inline Eigen::Matrix operator*(const Eigen::SO3

& lhs, const Eigen::Matrix& rhs) { + return lhs.get_matrix() * rhs; +} +/// Left-multiply by a matrix +/// @relates SO3 +template +inline Eigen::Matrix operator*(const Eigen::Matrix& lhs, const Eigen::SO3

& rhs) { + return lhs * rhs.get_matrix(); +} +/*----------------------------------------------------------------*/ + + +/// Exponentiate an angle in the Lie algebra to generate a new SO2. +template +inline void eigen_SO2_exp(const Precision& d, typename Eigen::SO2::Mat2& R) { + R(0,0) = R(1,1) = cos(d); + R(1,0) = sin(d); + R(0,1) = -R(1,0); +} +template +inline typename Eigen::SO2::Mat2 Eigen::SO2::exp(const Precision& d) const { + Mat2 result; + eigen_SO2_exp(d, result); + return result; +} + +/// Extracts the rotation angle from the SO2 +template +inline void eigen_SO2_ln(const typename Eigen::SO2::Mat2& R, Precision& d) { + d = atan2(R(1,0), R(0,0)); +} +template +inline Precision Eigen::SO2::ln() const { + Precision d; + eigen_SO2_ln(mat, d); + return d; +} + +/// Write an SO2 to a stream +/// @relates SO2 +template +inline std::ostream& operator<<(std::ostream& os, const Eigen::SO2 & rhs) { + return os << rhs.get_matrix(); +} +/// Read from SO2 to a stream +/// @relates SO2 +template +inline std::istream& operator>>(std::istream& is, Eigen::SO2& rhs) { + is >> rhs.mat; + rhs.coerce(); + return is; +} + +/// Right-multiply by a Vector +/// @relates SO2 +template +inline Eigen::Matrix operator*(const Eigen::SO2

& lhs, const Eigen::Matrix& rhs) { + return lhs.get_matrix() * rhs; +} +/// Left-multiply by a Vector +/// @relates SO2 +template +inline Eigen::Matrix operator*(const Eigen::Matrix& lhs, const Eigen::SO2

& rhs) { + return lhs * rhs.get_matrix(); +} +/// Right-multiply by a Matrix +/// @relates SO2 +template +inline Eigen::Matrix operator*(const Eigen::SO2

& lhs, const Eigen::Matrix& rhs) { + return lhs.get_matrix() * rhs; +} +/// Left-multiply by a Matrix +/// @relates SO2 +template +inline Eigen::Matrix operator*(const Eigen::Matrix& lhs, const Eigen::SO2

& rhs) { + return lhs * rhs.get_matrix(); +} +/*----------------------------------------------------------------*/ + +namespace Eigen { + +template +std::istream& operator >> (std::istream& st, MatrixBase& m) { + for (int i = 0; i < m.rows(); ++i) + for (int j = 0; j < m.cols(); ++j) + st >> m(i, j); + return st; +} + +} // namespace Eigen +/*----------------------------------------------------------------*/ + +#endif // _USE_EIGEN + + +// C L A S S ////////////////////////////////////////////////////// + +#ifdef _USE_BOOST + +namespace boost { + namespace serialization { + + // Serialization support for cv::Mat + template + void save(Archive& ar, const cv::Mat& m, const unsigned int /*version*/) { + const int elem_type = m.type(); + const size_t elem_size = m.elemSize(); + + ar & m.cols; + ar & m.rows; + ar & elem_type; + ar & elem_size; + + const size_t data_size = elem_size * m.cols * m.rows; + if (m.isContinuous()) { + ar & boost::serialization::make_array(m.ptr(), data_size); + } else { + cv::Mat m_cont; + m.copyTo(m_cont); + ar & boost::serialization::make_array(m_cont.ptr(), data_size); + } + } + template + void load(Archive& ar, cv::Mat& m, const unsigned int /*version*/) { + int cols, rows, elem_type; + size_t elem_size; + + ar & cols; + ar & rows; + ar & elem_type; + ar & elem_size; + + m.create(rows, cols, elem_type); + + const size_t data_size = elem_size * m.cols * m.rows; + ar & boost::serialization::make_array(m.ptr(), data_size); + } + template + inline void serialize(Archive& ar, cv::Mat& m, const unsigned int version) { + split_free(ar, m, version); + } + + // Serialization support for cv::Mat_ + template + void save(Archive& ar, const cv::Mat_<_Tp>& m, const unsigned int /*version*/) { + ar & m.cols; + ar & m.rows; + + const size_t data_size = m.cols * m.rows; + if (m.isContinuous()) { + ar & boost::serialization::make_array((const _Tp*)m.ptr(), data_size); + } else { + cv::Mat_<_Tp> m_cont; + m.copyTo(m_cont); + ar & boost::serialization::make_array((const _Tp*)m_cont.ptr(), data_size); + } + } + template + void load(Archive& ar, cv::Mat_<_Tp>& m, const unsigned int /*version*/) { + int cols, rows; + ar & cols; + ar & rows; + + m.create(rows, cols); + + const size_t data_size = m.cols * m.rows; + for (size_t n=0; n + inline void serialize(Archive& ar, cv::Mat_<_Tp>& m, const unsigned int version) { + split_free(ar, m, version); + } + + // Serialization support for cv::Matx + template + void serialize(Archive& ar, cv::Matx<_Tp, m, n>& _m, const unsigned int /*version*/) { + ar & _m.val; + } + + // Serialization support for cv::Vec + template + void serialize(Archive& ar, cv::Vec<_Tp, cn>& v, const unsigned int /*version*/) { + ar & boost::serialization::base_object >(v); + } + + // Serialization support for cv::Point_ + template + void serialize(Archive& ar, cv::Point_<_Tp>& pt, const unsigned int /*version*/) { + ar & pt.x & pt.y; + } + + // Serialization support for cv::Point3_ + template + void serialize(Archive& ar, cv::Point3_<_Tp>& pt, const unsigned int /*version*/) { + ar & pt.x & pt.y & pt.z; + } + + // Serialization support for cv::Size_ + template + void serialize(Archive& ar, cv::Size_<_Tp>& sz, const unsigned int /*version*/) { + ar & sz.width & sz.height; + } + + // Serialization support for cv::Rect_ + template + void serialize(Archive& ar, cv::Rect_<_Tp>& rc, const unsigned int /*version*/) { + ar & rc.x & rc.y & rc.width & rc.height; + } + + // Serialization support for cv::KeyPoint + template + void serialize(Archive& ar, cv::KeyPoint& k, const unsigned int /*version*/) { + ar & k.pt; + ar & k.size; + ar & k.angle; + ar & k.response; + ar & k.octave; + ar & k.class_id; + } + + // Serialization support for cv::DMatch + template + void serialize(Archive& ar, cv::DMatch& m, const unsigned int /*version*/) { + ar & m.queryIdx; + ar & m.trainIdx; + ar & m.imgIdx; + ar & m.distance; + } + + #ifdef _USE_EIGEN + // Serialization support for Eigen::Matrix + // specialization handling fixed sized matrices + template + inline void save(Archive& ar, const Eigen::Matrix& M, const unsigned int /*version*/) { + ar << make_nvp("data", make_array(M.data(), _Rows*_Cols)); + } + template + inline void load(Archive& ar, Eigen::Matrix& M, const unsigned int /*version*/) { + ar >> make_nvp("data", make_array(M.data(), _Rows*_Cols)); + } + // The function that causes boost::serialization to look for separate + // save() and load() functions when serializing and Eigen matrix. + template + inline void serialize(Archive& ar, Eigen::Matrix& M, const unsigned int version) { + split_free(ar, M, version); + } + #endif // _USE_EIGEN + + } // namespace serialization +} // namespace boost +/*----------------------------------------------------------------*/ + +// include headers that implement a archive in simple text and binary format or XML format +#if defined(_MSC_VER) +#pragma warning (push) +#pragma warning (disable : 4275) // non dll-interface class +#pragma warning (disable : 4715) // not all control paths return a value +#endif +#include +#include +#include +#include +//#include +//#include +// include headers that implement compressed serialization support +#include +#include +#if BOOST_VERSION >= 106900 +#include +#endif +#if defined(_MSC_VER) +#pragma warning (pop) +#endif + +enum ARCHIVE_TYPE { + ARCHIVE_MVS = -1, + ARCHIVE_TEXT = 0, + ARCHIVE_BINARY, + ARCHIVE_BINARY_ZIP, + ARCHIVE_BINARY_ZSTD, + ARCHIVE_LAST, + #if BOOST_VERSION >= 106900 + ARCHIVE_DEFAULT = ARCHIVE_BINARY_ZSTD + #else + ARCHIVE_DEFAULT = ARCHIVE_BINARY_ZIP + #endif +}; + +// export the current state of the given reconstruction object +template +bool SerializeSave(const TYPE& obj, std::ofstream& fs, ARCHIVE_TYPE type, unsigned flags=boost::archive::no_header) +{ + // serialize out the current state + switch (type) { + case ARCHIVE_TEXT: { + boost::archive::text_oarchive ar(fs, flags); + ar << obj; + break; } + case ARCHIVE_BINARY: { + boost::archive::binary_oarchive ar(fs, flags); + ar << obj; + break; } + case ARCHIVE_BINARY_ZIP: { + namespace io = boost::iostreams; + io::filtering_streambuf ffs; + ffs.push(io::zlib_compressor(io::zlib::best_speed)); + ffs.push(fs); + boost::archive::binary_oarchive ar(ffs, flags); + ar << obj; + break; } + #if BOOST_VERSION >= 106900 + case ARCHIVE_BINARY_ZSTD: { + namespace io = boost::iostreams; + io::filtering_streambuf ffs; + ffs.push(io::zstd_compressor(io::zstd::best_speed)); + ffs.push(fs); + boost::archive::binary_oarchive ar(ffs, flags); + ar << obj; + break; } + #endif + default: + VERBOSE("error: Can not save the object, invalid archive type"); + return false; + } + return true; +} // SerializeSave +template +bool SerializeSave(const TYPE& obj, const SEACAVE::String& fileName, ARCHIVE_TYPE type, unsigned flags=boost::archive::no_header) +{ + // open the output stream + std::ofstream fs(fileName, std::ios::out | std::ios::binary); + if (!fs.is_open()) + return false; + // serialize out the current state + return SerializeSave(obj, fs, type, flags); +} // SerializeSave + +// import the state to the given reconstruction object +template +bool SerializeLoad(TYPE& obj, std::ifstream& fs, ARCHIVE_TYPE type, unsigned flags=boost::archive::no_header) +{ + try { + // serialize in the saved state + switch (type) { + case ARCHIVE_TEXT: { + boost::archive::text_iarchive ar(fs, flags); + ar >> obj; + break; } + case ARCHIVE_BINARY: { + boost::archive::binary_iarchive ar(fs, flags); + ar >> obj; + break; } + case ARCHIVE_BINARY_ZIP: { + namespace io = boost::iostreams; + io::filtering_streambuf ffs; + ffs.push(io::zlib_decompressor()); + ffs.push(fs); + boost::archive::binary_iarchive ar(ffs, flags); + ar >> obj; + break; } + #if BOOST_VERSION >= 106900 + case ARCHIVE_BINARY_ZSTD: { + namespace io = boost::iostreams; + io::filtering_streambuf ffs; + ffs.push(io::zstd_decompressor()); + ffs.push(fs); + boost::archive::binary_iarchive ar(ffs, flags); + ar >> obj; + break; } + #endif + default: + VERBOSE("error: Can not load the object, invalid archive type"); + return false; + } + } + catch (const std::exception& e) { + VERBOSE("error: invalid stream (%s)", e.what()); + return false; + } + return true; +} // SerializeLoad +template +bool SerializeLoad(TYPE& obj, const SEACAVE::String& fileName, ARCHIVE_TYPE type, unsigned flags=boost::archive::no_header) +{ + // open the input stream + std::ifstream fs(fileName, std::ios::in | std::ios::binary); + if (!fs.is_open()) + return false; + // serialize in the saved state + return SerializeLoad(obj, fs, type, flags); +} // SerializeLoad +/*----------------------------------------------------------------*/ + +#endif // _USE_BOOST + + +#ifdef _USE_BREAKPAD + +#include +#include +#include +#include + +class MiniDumper { +public: + static inline void Create(const SEACAVE::String& strAppNameANSI, SEACAVE::String strDumpPathANSI, MINIDUMP_TYPE dumpType=MiniDumpNormal, bool bOutOfProcess=true) { + static MiniDumper oMiniDumper(strAppNameANSI, strDumpPathANSI, dumpType, bOutOfProcess); + } + static inline void Create(const SEACAVE::String& strAppNameANSI, SEACAVE::String strDumpPathANSI, int verbosity, bool bOutOfProcess=true) { + MINIDUMP_TYPE dumpType; + switch (verbosity) { + case 1: + dumpType = MiniDumpWithDataSegs; + break; + case 2: + dumpType = MiniDumpWithFullMemory; + break; + default: + dumpType = MiniDumpNormal; + } + Create(strAppNameANSI, strDumpPathANSI, dumpType, bOutOfProcess); + } + MiniDumper(const SEACAVE::String& strAppNameANSI, SEACAVE::String strDumpPathANSI, MINIDUMP_TYPE dumpType=MiniDumpNormal, bool bOutOfProcess=true) + : + crash_server(NULL), + handler(NULL) + { + if (strDumpPathANSI.IsEmpty()) + strDumpPathANSI = SEACAVE::Util::getCurrentFolder(); + const std::wstring strDumpPath(strDumpPathANSI.begin(), strDumpPathANSI.end()); + const std::wstring strAppName(strAppNameANSI.begin(), strAppNameANSI.end()); + std::wstring strPipeName; + if (bOutOfProcess) { + // try to create an out-of-process minidumper + const SEACAVE::String strUUIDANSI(SEACAVE::Util::getUniqueName(0)); + const std::wstring strUUID(strUUIDANSI.begin(), strUUIDANSI.end()); + strPipeName = L"\\\\.\\pipe\\BreakpadCrashServices\\"+strAppName+L"-"+strUUID; + crash_server = new google_breakpad::CrashGenerationServer( + strPipeName, + NULL, + NULL, + NULL, + NULL, + NULL, + NULL, + NULL, + NULL, + NULL, + true, + &strDumpPath); + if (!crash_server->Start()) { + VERBOSE("error: unable to start minidump server"); + delete crash_server; + crash_server = NULL; + } + } + // create the minidumper + const int kCustomInfoCount = 2; + const google_breakpad::CustomInfoEntry kCustomInfoEntries[] = { + google_breakpad::CustomInfoEntry(L"prod", strAppName.c_str()), + google_breakpad::CustomInfoEntry(L"ver", L"1.0"), + }; + google_breakpad::CustomClientInfo custom_info = {kCustomInfoEntries, kCustomInfoCount}; + handler = new google_breakpad::ExceptionHandler( + strDumpPath, + NULL, + NULL, + NULL, + google_breakpad::ExceptionHandler::HANDLER_ALL, + dumpType, + (crash_server ? strPipeName.c_str() : NULL), + &custom_info); + } + ~MiniDumper() { + delete handler; + delete crash_server; + } + google_breakpad::CrashGenerationServer* crash_server; + google_breakpad::ExceptionHandler* handler; +}; + +#endif diff --git a/libs/Common/Util.cpp b/libs/Common/Util.cpp new file mode 100644 index 0000000..1ae22cb --- /dev/null +++ b/libs/Common/Util.cpp @@ -0,0 +1,805 @@ +//////////////////////////////////////////////////////////////////// +// Util.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "Util.h" +#ifdef _MSC_VER +#include +#ifndef _USE_WINSDKOS +#define _USE_WINSDKOS +#include +#endif +#else +#include +#ifdef __APPLE__ +#include +#else +#include +#endif +#include +#endif + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +typedef struct CPUINFO_TYP { + bool bSSE; // Streaming SIMD Extensions + bool bSSE2; // Streaming SIMD Extensions 2 + bool bSSE3; // Streaming SIMD Extensions 3 + bool bSSE41; // Streaming SIMD Extensions 4.1 + bool bSSE42; // Streaming SIMD Extensions 4.2 + bool bAVX; // Advanced Vector Extensions + bool bFMA; // Fused Multiply�Add + bool b3DNOW; // 3DNow! (vendor independent) + bool b3DNOWEX; // 3DNow! (AMD specific extensions) + bool bMMX; // MMX support + bool bMMXEX; // MMX (AMD specific extensions) + bool bEXT; // extended features available + char vendor[13]; // vendor name + char name[49]; // CPU name +} CPUINFO; + + +// F U N C T I O N S /////////////////////////////////////////////// + +Flags InitCPU(); +CPUINFO GetCPUInfo(); +bool OSSupportsSSE(); +bool OSSupportsAVX(); + + +// G L O B A L S /////////////////////////////////////////////////// + +const Flags Util::ms_CPUFNC(InitCPU()); + +/** + * Lookup table (precomputed CRC64 values for each 8 bit string) computation + * takes into account the fact that the reverse polynom has zeros in lower 8 bits: + * + * @code + * for (i = 0; i < 256; i++) + * { + * shiftRegister = i; + * for (j = 0; j < 8; j++) + * { + * if (shiftRegister & 1) + * shiftRegister = (shiftRegister >> 1) ^ Reverse_polynom; + * else + * shiftRegister >>= 1; + * } + * CRCTable[i] = shiftRegister; + * } + * @endcode + * + * Generic code would look as follows: + * + * @code + * for (i = 0; i < 256; i++) + * { + * shiftRegister = 0; + * bitString = i; + * for (j = 0; j < 8; j++) + * { + * if ((shiftRegister ^ (bitString >> j)) & 1) + * shiftRegister = (shiftRegister >> 1) ^ Reverse_polynom; + * else + * shiftRegister >>= 1; + * } + * CRCTable[i] = shiftRegister; + * } + * @endcode + * + * @remark Since the lookup table elements have 0 in the lower 32 bit word, + * the 32 bit assembler implementation of CRC64Process can be optimized, + * avoiding at least one 'xor' operation. + */ +static const uint64_t gs_au64CRC64[256] = +{ + 0x0000000000000000ULL, 0x01B0000000000000ULL, 0x0360000000000000ULL, 0x02D0000000000000ULL, + 0x06C0000000000000ULL, 0x0770000000000000ULL, 0x05A0000000000000ULL, 0x0410000000000000ULL, + 0x0D80000000000000ULL, 0x0C30000000000000ULL, 0x0EE0000000000000ULL, 0x0F50000000000000ULL, + 0x0B40000000000000ULL, 0x0AF0000000000000ULL, 0x0820000000000000ULL, 0x0990000000000000ULL, + 0x1B00000000000000ULL, 0x1AB0000000000000ULL, 0x1860000000000000ULL, 0x19D0000000000000ULL, + 0x1DC0000000000000ULL, 0x1C70000000000000ULL, 0x1EA0000000000000ULL, 0x1F10000000000000ULL, + 0x1680000000000000ULL, 0x1730000000000000ULL, 0x15E0000000000000ULL, 0x1450000000000000ULL, + 0x1040000000000000ULL, 0x11F0000000000000ULL, 0x1320000000000000ULL, 0x1290000000000000ULL, + 0x3600000000000000ULL, 0x37B0000000000000ULL, 0x3560000000000000ULL, 0x34D0000000000000ULL, + 0x30C0000000000000ULL, 0x3170000000000000ULL, 0x33A0000000000000ULL, 0x3210000000000000ULL, + 0x3B80000000000000ULL, 0x3A30000000000000ULL, 0x38E0000000000000ULL, 0x3950000000000000ULL, + 0x3D40000000000000ULL, 0x3CF0000000000000ULL, 0x3E20000000000000ULL, 0x3F90000000000000ULL, + 0x2D00000000000000ULL, 0x2CB0000000000000ULL, 0x2E60000000000000ULL, 0x2FD0000000000000ULL, + 0x2BC0000000000000ULL, 0x2A70000000000000ULL, 0x28A0000000000000ULL, 0x2910000000000000ULL, + 0x2080000000000000ULL, 0x2130000000000000ULL, 0x23E0000000000000ULL, 0x2250000000000000ULL, + 0x2640000000000000ULL, 0x27F0000000000000ULL, 0x2520000000000000ULL, 0x2490000000000000ULL, + 0x6C00000000000000ULL, 0x6DB0000000000000ULL, 0x6F60000000000000ULL, 0x6ED0000000000000ULL, + 0x6AC0000000000000ULL, 0x6B70000000000000ULL, 0x69A0000000000000ULL, 0x6810000000000000ULL, + 0x6180000000000000ULL, 0x6030000000000000ULL, 0x62E0000000000000ULL, 0x6350000000000000ULL, + 0x6740000000000000ULL, 0x66F0000000000000ULL, 0x6420000000000000ULL, 0x6590000000000000ULL, + 0x7700000000000000ULL, 0x76B0000000000000ULL, 0x7460000000000000ULL, 0x75D0000000000000ULL, + 0x71C0000000000000ULL, 0x7070000000000000ULL, 0x72A0000000000000ULL, 0x7310000000000000ULL, + 0x7A80000000000000ULL, 0x7B30000000000000ULL, 0x79E0000000000000ULL, 0x7850000000000000ULL, + 0x7C40000000000000ULL, 0x7DF0000000000000ULL, 0x7F20000000000000ULL, 0x7E90000000000000ULL, + 0x5A00000000000000ULL, 0x5BB0000000000000ULL, 0x5960000000000000ULL, 0x58D0000000000000ULL, + 0x5CC0000000000000ULL, 0x5D70000000000000ULL, 0x5FA0000000000000ULL, 0x5E10000000000000ULL, + 0x5780000000000000ULL, 0x5630000000000000ULL, 0x54E0000000000000ULL, 0x5550000000000000ULL, + 0x5140000000000000ULL, 0x50F0000000000000ULL, 0x5220000000000000ULL, 0x5390000000000000ULL, + 0x4100000000000000ULL, 0x40B0000000000000ULL, 0x4260000000000000ULL, 0x43D0000000000000ULL, + 0x47C0000000000000ULL, 0x4670000000000000ULL, 0x44A0000000000000ULL, 0x4510000000000000ULL, + 0x4C80000000000000ULL, 0x4D30000000000000ULL, 0x4FE0000000000000ULL, 0x4E50000000000000ULL, + 0x4A40000000000000ULL, 0x4BF0000000000000ULL, 0x4920000000000000ULL, 0x4890000000000000ULL, + 0xD800000000000000ULL, 0xD9B0000000000000ULL, 0xDB60000000000000ULL, 0xDAD0000000000000ULL, + 0xDEC0000000000000ULL, 0xDF70000000000000ULL, 0xDDA0000000000000ULL, 0xDC10000000000000ULL, + 0xD580000000000000ULL, 0xD430000000000000ULL, 0xD6E0000000000000ULL, 0xD750000000000000ULL, + 0xD340000000000000ULL, 0xD2F0000000000000ULL, 0xD020000000000000ULL, 0xD190000000000000ULL, + 0xC300000000000000ULL, 0xC2B0000000000000ULL, 0xC060000000000000ULL, 0xC1D0000000000000ULL, + 0xC5C0000000000000ULL, 0xC470000000000000ULL, 0xC6A0000000000000ULL, 0xC710000000000000ULL, + 0xCE80000000000000ULL, 0xCF30000000000000ULL, 0xCDE0000000000000ULL, 0xCC50000000000000ULL, + 0xC840000000000000ULL, 0xC9F0000000000000ULL, 0xCB20000000000000ULL, 0xCA90000000000000ULL, + 0xEE00000000000000ULL, 0xEFB0000000000000ULL, 0xED60000000000000ULL, 0xECD0000000000000ULL, + 0xE8C0000000000000ULL, 0xE970000000000000ULL, 0xEBA0000000000000ULL, 0xEA10000000000000ULL, + 0xE380000000000000ULL, 0xE230000000000000ULL, 0xE0E0000000000000ULL, 0xE150000000000000ULL, + 0xE540000000000000ULL, 0xE4F0000000000000ULL, 0xE620000000000000ULL, 0xE790000000000000ULL, + 0xF500000000000000ULL, 0xF4B0000000000000ULL, 0xF660000000000000ULL, 0xF7D0000000000000ULL, + 0xF3C0000000000000ULL, 0xF270000000000000ULL, 0xF0A0000000000000ULL, 0xF110000000000000ULL, + 0xF880000000000000ULL, 0xF930000000000000ULL, 0xFBE0000000000000ULL, 0xFA50000000000000ULL, + 0xFE40000000000000ULL, 0xFFF0000000000000ULL, 0xFD20000000000000ULL, 0xFC90000000000000ULL, + 0xB400000000000000ULL, 0xB5B0000000000000ULL, 0xB760000000000000ULL, 0xB6D0000000000000ULL, + 0xB2C0000000000000ULL, 0xB370000000000000ULL, 0xB1A0000000000000ULL, 0xB010000000000000ULL, + 0xB980000000000000ULL, 0xB830000000000000ULL, 0xBAE0000000000000ULL, 0xBB50000000000000ULL, + 0xBF40000000000000ULL, 0xBEF0000000000000ULL, 0xBC20000000000000ULL, 0xBD90000000000000ULL, + 0xAF00000000000000ULL, 0xAEB0000000000000ULL, 0xAC60000000000000ULL, 0xADD0000000000000ULL, + 0xA9C0000000000000ULL, 0xA870000000000000ULL, 0xAAA0000000000000ULL, 0xAB10000000000000ULL, + 0xA280000000000000ULL, 0xA330000000000000ULL, 0xA1E0000000000000ULL, 0xA050000000000000ULL, + 0xA440000000000000ULL, 0xA5F0000000000000ULL, 0xA720000000000000ULL, 0xA690000000000000ULL, + 0x8200000000000000ULL, 0x83B0000000000000ULL, 0x8160000000000000ULL, 0x80D0000000000000ULL, + 0x84C0000000000000ULL, 0x8570000000000000ULL, 0x87A0000000000000ULL, 0x8610000000000000ULL, + 0x8F80000000000000ULL, 0x8E30000000000000ULL, 0x8CE0000000000000ULL, 0x8D50000000000000ULL, + 0x8940000000000000ULL, 0x88F0000000000000ULL, 0x8A20000000000000ULL, 0x8B90000000000000ULL, + 0x9900000000000000ULL, 0x98B0000000000000ULL, 0x9A60000000000000ULL, 0x9BD0000000000000ULL, + 0x9FC0000000000000ULL, 0x9E70000000000000ULL, 0x9CA0000000000000ULL, 0x9D10000000000000ULL, + 0x9480000000000000ULL, 0x9530000000000000ULL, 0x97E0000000000000ULL, 0x9650000000000000ULL, + 0x9240000000000000ULL, 0x93F0000000000000ULL, 0x9120000000000000ULL, 0x9090000000000000ULL +}; + + +// F U N C T I O N S /////////////////////////////////////////////// + +String Util::getHomeFolder() +{ + #ifdef _MSC_VER + TCHAR homedir[MAX_PATH]; + if (SHGetSpecialFolderPath(0, homedir, CSIDL_PROFILE, TRUE) != TRUE) + return String(); + #else + const char *homedir; + if ((homedir = getenv("HOME")) == NULL) + homedir = getpwuid(getuid())->pw_dir; + #endif // _MSC_VER + String dir(String(homedir) + PATH_SEPARATOR); + return ensureUnifySlash(dir); +} + +String Util::getApplicationFolder() +{ + #ifdef _MSC_VER + TCHAR appdir[MAX_PATH]; + if (SHGetSpecialFolderPath(0, appdir, CSIDL_APPDATA, TRUE) != TRUE) + return String(); + String dir(String(appdir) + PATH_SEPARATOR); + #else + const char *homedir; + if ((homedir = getenv("HOME")) == NULL) + homedir = getpwuid(getuid())->pw_dir; + String dir(String(homedir) + PATH_SEPARATOR + String(_T(".config")) + PATH_SEPARATOR); + #endif // _MSC_VER + return ensureUnifySlash(dir); +} + +String Util::getCurrentFolder() +{ + TCHAR pathname[MAX_PATH+1]; + #ifdef _MSC_VER + if (!GetCurrentDirectory(MAX_PATH, pathname)) + #else // _MSC_VER + if (!getcwd(pathname, MAX_PATH)) + #endif // _MSC_VER + return String(); + String dir(String(pathname) + PATH_SEPARATOR); + return ensureUnifySlash(dir); +} +/*----------------------------------------------------------------*/ + + +uint64_t Util::CRC64(const void *pv, size_t cb) +{ + const uint8_t* pu8 = (const uint8_t *)pv; + uint64_t uCRC64 = 0ULL; + while (cb--) + uCRC64 = gs_au64CRC64[(uCRC64 ^ *pu8++) & 0xff] ^ (uCRC64 >> 8); + return uCRC64; +} + +uint64_t Util::CRC64Process(uint64_t uCRC64, const void *pv, size_t cb) +{ + const uint8_t *pu8 = (const uint8_t *)pv; + while (cb--) + uCRC64 = gs_au64CRC64[(uCRC64 ^ *pu8++) & 0xff] ^ (uCRC64 >> 8); + return uCRC64; +} +/*----------------------------------------------------------------*/ + + +String Util::GetCPUInfo() +{ + const CPUINFO info(::GetCPUInfo()); + String cpu(info.name[0] == 0 ? info.vendor : info.name); + #if 0 + if (info.bFMA) + cpu += _T(" FMA"); + else if (info.bAVX) + cpu += _T(" AVX"); + else if (info.bSSE42) + cpu += _T(" SSE4.2"); + else if (info.bSSE41) + cpu += _T(" SSE4.1"); + else if (info.bSSE3) + cpu += _T(" SSE3"); + else if (info.bSSE2) + cpu += _T(" SSE2"); + else if (info.bSSE) + cpu += _T(" SSE"); + if (info.b3DNOWEX) + cpu += _T(" 3DNOWEX"); + else if (info.b3DNOW) + cpu += _T(" 3DNOW"); + #endif + return cpu; +} +/*----------------------------------------------------------------*/ + +String Util::GetRAMInfo() +{ + #if defined(_MSC_VER) + + #ifdef _WIN64 + MEMORYSTATUSEX memoryStatus; + memset(&memoryStatus, sizeof(MEMORYSTATUSEX), 0); + memoryStatus.dwLength = sizeof(memoryStatus); + ::GlobalMemoryStatusEx(&memoryStatus); + const size_t nTotalPhys((size_t)memoryStatus.ullTotalPhys); + const size_t nTotalVirtual((size_t)memoryStatus.ullTotalVirtual); + #else + MEMORYSTATUS memoryStatus; + memset(&memoryStatus, sizeof(MEMORYSTATUS), 0); + memoryStatus.dwLength = sizeof(MEMORYSTATUS); + ::GlobalMemoryStatus(&memoryStatus); + const size_t nTotalPhys((size_t)memoryStatus.dwTotalPhys); + const size_t nTotalVirtual((size_t)memoryStatus.dwTotalVirtual); + #endif + + #elif defined(__APPLE__) + + int mib[2] = {CTL_HW, HW_MEMSIZE}; + const unsigned namelen = sizeof(mib) / sizeof(mib[0]); + size_t len = sizeof(size_t); + size_t nTotalPhys; + sysctl(mib, namelen, &nTotalPhys, &len, NULL, 0); + const size_t nTotalVirtual(nTotalPhys); + + #else // __GNUC__ + + struct sysinfo info; + sysinfo(&info); + const size_t nTotalPhys((size_t)info.totalram); + const size_t nTotalVirtual((size_t)info.totalswap); + + #endif // _MSC_VER + return formatBytes(nTotalPhys) + _T(" Physical Memory ") + formatBytes(nTotalVirtual) + _T(" Virtual Memory"); +} +/*----------------------------------------------------------------*/ + +String Util::GetOSInfo() +{ + #ifdef _MSC_VER + + String os; + #ifdef _USE_WINSDKOS + #ifndef _WIN32_WINNT_WIN10 + #define _WIN32_WINNT_WIN10 0x0A00 + if (IsWindowsVersionOrGreater(HIBYTE(_WIN32_WINNT_WIN10), LOBYTE(_WIN32_WINNT_WIN10), 0)) + #else + if (IsWindows10OrGreater()) + #endif + os = _T("Windows 10+"); + else if (IsWindows8Point1OrGreater()) + os = _T("Windows 8.1"); + else if (IsWindows8OrGreater()) + os = _T("Windows 8"); + else if (IsWindows7SP1OrGreater()) + os = _T("Windows 7 (SP1)"); + else if (IsWindows7OrGreater()) + os = _T("Windows 7"); + else if (IsWindowsVistaSP2OrGreater()) + os = _T("Windows Vista (SP2)"); + else if (IsWindowsVistaSP1OrGreater()) + os = _T("Windows Vista (SP1)"); + else if (IsWindowsVistaOrGreater()) + os = _T("Windows Vista"); + else if (IsWindowsXPSP3OrGreater()) + os = _T("Windows XP (SP3)"); + else if (IsWindowsXPSP2OrGreater()) + os = _T("Windows XP (SP2)"); + else if (IsWindowsXPSP1OrGreater()) + os = _T("Windows XP (SP1)"); + else if (IsWindowsXPOrGreater()) + os = _T("Windows XP"); + else + os = _T("Windows (unknown version)"); + #else + OSVERSIONINFOEX ver; + memset(&ver, 0, sizeof(OSVERSIONINFOEX)); + ver.dwOSVersionInfoSize = sizeof(OSVERSIONINFOEX); + + if (!GetVersionEx((OSVERSIONINFO*)&ver)) { + ver.dwOSVersionInfoSize = sizeof(OSVERSIONINFO); + if (!GetVersionEx((OSVERSIONINFO*)&ver)) { + return "Windows (unknown version)"; + } + } + + if (ver.dwPlatformId != VER_PLATFORM_WIN32_NT) { + os = "Win9x/ME"; + } else { + switch (ver.dwMajorVersion) + { + case 4: + os = "WinNT4"; + break; + + case 5: + switch (ver.dwMinorVersion) + { + case 0: os = "Win2000"; break; + case 1: os = "WinXP"; break; + case 2: os = "Win2003"; break; + default:os = "Unknown WinNT5"; + } + break; + + case 6: + switch (ver.dwMinorVersion) + { + case 0: os = (ver.wProductType == VER_NT_WORKSTATION ? "WinVista" : "Win2008"); break; + case 1: os = (ver.wProductType == VER_NT_WORKSTATION ? "Win7" : "Win2008R2"); break; + case 2: os = (ver.wProductType == VER_NT_WORKSTATION ? "Win8" : "Win2012"); break; + case 3: os = (ver.wProductType == VER_NT_WORKSTATION ? "Win8.1" : "Win2012R2"); break; + case 4: os = "Win10"; break; + default:os = "Unknown WinNT6"; + } + break; + + default: + os = "Windows (version unknown)"; + } + if (ver.wProductType & VER_NT_WORKSTATION) + os += " Pro"; + else if (ver.wProductType & VER_NT_SERVER) + os += " Server"; + else if (ver.wProductType & VER_NT_DOMAIN_CONTROLLER) + os += " DC"; + } + + if (ver.wServicePackMajor != 0) { + os += " (SP"; + os += String::ToString(ver.wServicePackMajor); + if (ver.wServicePackMinor != 0) { + os += '.'; + os += String::ToString(ver.wServicePackMinor); + } + os += ")"; + } + #endif + + #ifdef _WIN64 + os += " x64"; + #else + typedef BOOL (WINAPI *LPFN_ISWOW64PROCESS) (HANDLE, PBOOL); + const LPFN_ISWOW64PROCESS fnIsWow64Process = (LPFN_ISWOW64PROCESS)GetProcAddress(GetModuleHandle("kernel32"),"IsWow64Process"); + BOOL bIsWow64 = FALSE; + if (fnIsWow64Process && fnIsWow64Process(GetCurrentProcess(),&bIsWow64) && bIsWow64) + os += " x64"; + #endif + + return os; + + #else // _MSC_VER + + utsname n; + if (uname(&n) != 0) + return "linux (unknown version)"; + return String(n.sysname) + " " + String(n.release) + " (" + String(n.machine) + ")"; + + #endif // _MSC_VER +} +/*----------------------------------------------------------------*/ + +String Util::GetDiskInfo(const String& path) +{ + #if defined(_SUPPORT_CPP17) && (!defined(__GNUC__) || (__GNUC__ > 7)) + + const std::filesystem::space_info si = std::filesystem::space(path.c_str()); + return String::FormatString("%s (%s) space", formatBytes(si.available).c_str(), formatBytes(si.capacity).c_str()); + + #else + + return String(); + + #endif // _SUPPORT_CPP17 +} +/*----------------------------------------------------------------*/ + + +// Initialize various global variables (ex: random-number-generator state). +void Util::Init() +{ + #ifdef _RELEASE + const time_t t(Util::getTime()); + std::srand((unsigned)t); + #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >= 4) + cv::setRNGSeed((int)t); + #endif + #else + std::srand((unsigned)0); + #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >= 4) + cv::setRNGSeed((int)0); + #endif + #endif +} +/*----------------------------------------------------------------*/ + + +/** + * Set global variable for availability of SSE instructions. + */ +Flags InitCPU() +{ + const CPUINFO info(GetCPUInfo()); + Flags cpufncs(0); + if (info.bSSE) { + #if defined(_MSC_VER) && !defined(_WIN64) + _set_SSE2_enable(1); + #endif + if (OSSupportsSSE()) + cpufncs.set(Util::SSE); + } + if (info.bAVX && OSSupportsAVX()) + cpufncs.set(Util::AVX); + return (cpufncs); +} +/*----------------------------------------------------------------*/ + + +#if _PLATFORM_X86 +#ifdef _MSC_VER +#include +inline void CPUID(int CPUInfo[4], int level) { + __cpuid(CPUInfo, level); +} +#else +#include +inline void CPUID(int CPUInfo[4], int level) { + unsigned* p((unsigned*)CPUInfo); + __get_cpuid((unsigned&)level, p+0, p+1, p+2, p+3); +} +#endif +#else // _PLATFORM_X86 +inline void CPUID(int CPUInfo[4], int level) { + memset(CPUInfo, 0, sizeof(int)*4); +} +#endif // _PLATFORM_X86 + +/** + * Function to detect SSE availability in CPU. + */ +CPUINFO GetCPUInfo() +{ + CPUINFO info; + + // set all values to 0 (false) + memset(&info, 0, sizeof(CPUINFO)); + + int CPUInfo[4]; + + // CPUID with an InfoType argument of 0 returns the number of + // valid Ids in CPUInfo[0] and the CPU identification string in + // the other three array elements. The CPU identification string is + // not in linear order. The code below arranges the information + // in a human readable form. + CPUID(CPUInfo, 0); + *((int*)info.vendor) = CPUInfo[1]; + *((int*)(info.vendor+4)) = CPUInfo[3]; + *((int*)(info.vendor+8)) = CPUInfo[2]; + + // Interpret CPU feature information. + CPUID(CPUInfo, 1); + info.bMMX = (CPUInfo[3] & 0x800000) != 0; // test bit 23 for MMX + info.bSSE = (CPUInfo[3] & 0x2000000) != 0; // test bit 25 for SSE + info.bSSE2 = (CPUInfo[3] & 0x4000000) != 0; // test bit 26 for SSE2 + info.bSSE3 = (CPUInfo[2] & 0x1) != 0; // test bit 0 for SSE3 + info.bSSE41 = (CPUInfo[2] & 0x80000) != 0; // test bit 19 for SSE4.1 + info.bSSE42 = (CPUInfo[2] & 0x100000) != 0; // test bit 20 for SSE4.2 + info.bAVX = (CPUInfo[2] & 0x18000000) == 0x18000000; // test bits 28,27 for AVX + info.bFMA = (CPUInfo[2] & 0x18001000) == 0x18001000; // test bits 28,27,12 for FMA + + // EAX=0x80000000 => CPUID returns extended features + CPUID(CPUInfo, 0x80000000); + const unsigned nExIds = CPUInfo[0]; + info.bEXT = (nExIds >= 0x80000000); + + // must be greater than 0x80000004 to support CPU name + if (nExIds > 0x80000004) { + size_t idx(0); + CPUID(CPUInfo, 0x80000002); // CPUID returns CPU name part1 + while (((uint8_t*)CPUInfo)[idx] == ' ') + ++idx; + memcpy(info.name, (uint8_t*)CPUInfo + idx, sizeof(CPUInfo) - idx); + idx = sizeof(CPUInfo) - idx; + + CPUID(CPUInfo, 0x80000003); // CPUID returns CPU name part2 + memcpy(info.name+idx, CPUInfo, sizeof(CPUInfo)); + idx += 16; + + CPUID(CPUInfo, 0x80000004); // CPUID returns CPU name part3 + memcpy(info.name+idx, CPUInfo, sizeof(CPUInfo)); + } + + if ((strncmp(info.vendor, "AuthenticAMD", 12)==0) && info.bEXT) { // AMD + CPUID(CPUInfo, 0x80000001); // CPUID will copy ext. feat. bits to EDX and cpu type to EAX + info.b3DNOWEX = (CPUInfo[3] & 0x40000000) != 0; // indicates AMD extended 3DNow+! + info.bMMXEX = (CPUInfo[3] & 0x400000) != 0; // indicates AMD extended MMX + } + + return info; +} +/*----------------------------------------------------------------*/ + +#if _PLATFORM_X86 +#ifdef _MSC_VER +// Function to detect SSE availability in operating system. +bool OSSupportsSSE() +{ + #ifndef _WIN64 + // try SSE instruction and look for crash + __try + { + _asm xorps xmm0, xmm0 + } + __except(EXCEPTION_EXECUTE_HANDLER) { + if (_exception_code() == STATUS_ILLEGAL_INSTRUCTION) + return false; // sse not supported by os + return false; // unknown exception occurred + } + #endif // _WIN64 + + return true; +} +// Function to detect AVX availability in operating system. +bool OSSupportsAVX() +{ + #ifndef _WIN64 + // try AVX instruction + unsigned flag; + _asm { + mov ecx, 0; //specify 0 for XFEATURE_ENABLED_MASK register + XGETBV; //result in EDX:EAX + and eax, 06H; + cmp eax, 06H; // check OS has enabled both XMM and YMM state support + jne not_supported + mov eax, 1; // mark as supported + jmp done + not_supported: + mov eax, 0; // mark as not supported + done: + mov esi, flag + mov [esi], eax + } + return flag != 0; + #else + // check if the OS will save the YMM registers + unsigned long long xcrFeatureMask(_xgetbv(_XCR_XFEATURE_ENABLED_MASK)); + return (xcrFeatureMask & 0x6) == 0x6; + #endif // _WIN64 +} +/*----------------------------------------------------------------*/ + +#else // _MSC_VER + +// Function to detect SSE availability in operating system. +bool OSSupportsSSE() +{ + // try SSE instruction and look for crash + try { + asm("xorps %xmm0, %xmm0"); + } + catch(int e) { + return false; // unknown exception occurred + } + return true; +} +// Function to detect AVX availability in operating system. +bool OSSupportsAVX() +{ + // check if the OS will save the YMM registers + unsigned int index(0); //specify 0 for XFEATURE_ENABLED_MASK register + unsigned int eax, edx; + __asm__ __volatile__("xgetbv" : "=a"(eax), "=d"(edx) : "c"(index)); + unsigned long long xcrFeatureMask(((unsigned long long)edx << 32) | eax); + return (xcrFeatureMask & 0x6) == 0x6; +} +/*----------------------------------------------------------------*/ +#endif // _MSC_VER + +#else // _PLATFORM_X86 + +// Function to detect SSE availability in operating system. +bool OSSupportsSSE() +{ + return false; +} +// Function to detect AVX availability in operating system. +bool OSSupportsAVX() +{ + return false; +} +/*----------------------------------------------------------------*/ +#endif // _PLATFORM_X86 + + +// print details about the current build and PC +void Util::LogBuild() +{ + LOG(_T("OpenMVS %s v%u.%u.%u"), + #ifdef _ENVIRONMENT64 + _T("x64"), + #else + _T("x32"), + #endif + OpenMVS_MAJOR_VERSION, OpenMVS_MINOR_VERSION, OpenMVS_PATCH_VERSION); + #if TD_VERBOSE == TD_VERBOSE_OFF + LOG(_T("Build date: ") __DATE__); + #else + LOG(_T("Build date: ") __DATE__ _T(", ") __TIME__); + #endif + LOG(_T("CPU: %s (%u cores)"), Util::GetCPUInfo().c_str(), Thread::hardwareConcurrency()); + LOG((_T("RAM: ") + Util::GetRAMInfo()).c_str()); + LOG((_T("OS: ") + Util::GetOSInfo()).c_str()); + #ifdef _SUPPORT_CPP17 + LOG((_T("Disk: ") + Util::GetDiskInfo(WORKING_FOLDER_FULL)).c_str()); + #endif + if (!SIMD_ENABLED.isSet(Util::SSE)) LOG(_T("warning: no SSE compatible CPU or OS detected")); + else if (!SIMD_ENABLED.isSet(Util::AVX)) LOG(_T("warning: no AVX compatible CPU or OS detected")); + else LOG(_T("SSE & AVX compatible CPU & OS detected")); +} + +// print information about the memory usage +#if _PLATFORM_X86 +#ifdef _MSC_VER +#include +#pragma comment(lib, "Psapi.lib") +void Util::LogMemoryInfo() +{ + PROCESS_MEMORY_COUNTERS pmc; + if (!GetProcessMemoryInfo(GetCurrentProcess(), &pmc, sizeof(pmc))) + return; + LOG(_T("MEMORYINFO: {")); + LOG(_T("\tPageFaultCount %d"), pmc.PageFaultCount); + LOG(_T("\tPeakWorkingSetSize %s"), SEACAVE::Util::formatBytes(pmc.PeakWorkingSetSize).c_str()); + LOG(_T("\tWorkingSetSize %s"), SEACAVE::Util::formatBytes(pmc.WorkingSetSize).c_str()); + LOG(_T("\tQuotaPeakPagedPoolUsage %s"), SEACAVE::Util::formatBytes(pmc.QuotaPeakPagedPoolUsage).c_str()); + LOG(_T("\tQuotaPagedPoolUsage %s"), SEACAVE::Util::formatBytes(pmc.QuotaPagedPoolUsage).c_str()); + LOG(_T("\tQuotaPeakNonPagedPoolUsage %s"), SEACAVE::Util::formatBytes(pmc.QuotaPeakNonPagedPoolUsage).c_str()); + LOG(_T("\tQuotaNonPagedPoolUsage %s"), SEACAVE::Util::formatBytes(pmc.QuotaNonPagedPoolUsage).c_str()); + LOG(_T("\tPagefileUsage %s"), SEACAVE::Util::formatBytes(pmc.PagefileUsage).c_str()); + LOG(_T("\tPeakPagefileUsage %s"), SEACAVE::Util::formatBytes(pmc.PeakPagefileUsage).c_str()); + LOG(_T("} ENDINFO")); +} +#else // _MSC_VER +void Util::LogMemoryInfo() +{ + std::ifstream proc("/proc/self/status"); + if (!proc.is_open()) + return; + String s; + LOG(_T("MEMORYINFO: {")); + while (std::getline(proc, s), !proc.fail()) { + if (s.substr(0, 6) == "VmPeak" || s.substr(0, 6) == "VmSize") + LOG(_T("\t%s"), s.c_str()); + } + LOG(_T("} ENDINFO")); +} +#endif // _MSC_VER +#else // _PLATFORM_X86 +void Util::LogMemoryInfo() +{ +} +#endif // _PLATFORM_X86 + + +// Parses a ASCII command line string and returns an array of pointers to the command line arguments, +// along with a count of such arguments, in a way that is similar to the standard C run-time +// argv and argc values. +LPSTR* Util::CommandLineToArgvA(LPCSTR CmdLine, size_t& _argc) +{ + bool in_QM(false); + bool in_TEXT(false); + bool in_SPACE(true); + + size_t argc(0); + size_t len = strlen(CmdLine); + size_t i = ((len+2)/2)*sizeof(void*) + sizeof(void*); + LPSTR* argv = (LPSTR*)(new uint8_t[i + (len+2)*sizeof(CHAR)]); + LPSTR _argv = (LPSTR)(((CHAR*)argv)+i); + argv[argc] = _argv; + size_t j(0); i = 0; + + CHAR a; + while ((a = CmdLine[i]) != 0) { + if (in_QM) { + if (a == '\"') { + in_QM = false; + } else { + _argv[j] = a; + j++; + } + } else { + switch (a) { + case '\"': + in_QM = true; + in_TEXT = true; + if (in_SPACE) { + argv[argc] = _argv+j; + argc++; + } + in_SPACE = false; + break; + case ' ': + case '\t': + case '\n': + case '\r': + if (in_TEXT) { + _argv[j] = '\0'; + j++; + } + in_TEXT = false; + in_SPACE = true; + break; + default: + in_TEXT = true; + if (in_SPACE) { + argv[argc] = _argv+j; + argc++; + } + _argv[j] = a; + j++; + in_SPACE = false; + break; + } + } + i++; + } + _argv[j] = '\0'; + argv[argc] = NULL; + + _argc = argc; + return argv; +} +/*----------------------------------------------------------------*/ diff --git a/libs/Common/Util.h b/libs/Common/Util.h new file mode 100644 index 0000000..fd313f9 --- /dev/null +++ b/libs/Common/Util.h @@ -0,0 +1,835 @@ +//////////////////////////////////////////////////////////////////// +// Util.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_UTIL_H__ +#define __SEACAVE_UTIL_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#ifdef _MSC_VER +#include +#else +#include +#include +#endif + + +// D E F I N E S /////////////////////////////////////////////////// + +#define PATH_SEPARATOR _T('/') +#define PATH_SEPARATOR_STR _T("/") +#define REVERSE_PATH_SEPARATOR _T('\\') + +#ifdef _MSC_VER +#define LINE_SEPARATOR_STR _T("\r\n") +#define LINE_SEPARATOR_LEN 2 +#else +#define LINE_SEPARATOR_STR _T("\n") +#define LINE_SEPARATOR_LEN 1 +#endif + +#ifdef _MSC_VER +#define SETTINGS_PATH _T("%APPDATA%") +#else +#define SETTINGS_PATH _T("~/.config") +#endif + +#define ensureUnifySlashWin ensureUnifyReverseSlash +#define ensureUnifySlashUnix ensureUnifySlash + +#define GET_TICK() Util::getTick() +#define GET_TIME() Util::getTime() +#define SIMD_ENABLED Util::ms_CPUFNC + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// Manage setting/removing bit flags +template +class TFlags +{ +public: + typedef TYPE Type; + +public: + inline TFlags() : flags(0) { } + inline TFlags(const TFlags& rhs) : flags(rhs.flags) { } + inline TFlags(Type f) : flags(f) { } + inline bool isSet(Type aFlag) const { return (flags & aFlag) == aFlag; } + inline bool isSet(Type aFlag, Type nF) const { return (flags & (aFlag|nF)) == aFlag; } + inline bool isSetExclusive(Type aFlag) const { return flags == aFlag; } + inline bool isAnySet(Type aFlag) const { return (flags & aFlag) != 0; } + inline bool isAnySet(Type aFlag, Type nF) const { const Type m(flags & (aFlag|nF)); return m != 0 && (m & nF) == 0; } + inline bool isAnySetExclusive(Type aFlag) const { return (flags & aFlag) != 0 && (flags & ~aFlag) == 0; } + inline void set(Type aFlag, bool bSet) { if (bSet) set(aFlag); else unset(aFlag); } + inline void set(Type aFlag) { flags |= aFlag; } + inline void unset(Type aFlag) { flags &= ~aFlag; } + inline void flip(Type aFlag) { flags ^= aFlag; } + inline void operator=(TFlags rhs) { flags = rhs.flags; } + inline operator Type() const { return flags; } + inline operator Type&() { return flags; } +protected: + Type flags; + #ifdef _USE_BOOST + // implement BOOST serialization + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar & flags; + } + #endif +}; +typedef class GENERAL_API TFlags Flags; +/*----------------------------------------------------------------*/ + + +// A histogram class, that computes the distribution function (df) +// of unique sended value or iterable data inside the provided range. +// The Histogram object can keep a tally of values within a range, +// the range is arranged into some number of bins specified during +// construction. +// Jansson Consulting +// 2009-06-30, updated 2011-06-17 and 2011-08-03 +// 2011-12-17 Modified by Pierre Moulon +// - use vector array to avoid memory management +// - add value by sequence with iterator +// 2015-04-04 Modified by cDc +// - rewrite +// - add GetApproximatePermille() +// Dedicated to the public domain. +template +class THistogram +{ +public: + typedef TYPE Type; + +public: + // Construct a histogram that can count within a range of values. + // All bins of the histogram are set to zero. + THistogram(const std::pair& range, size_t bins=10) : + Start(range.first), + End(range.second), + BinInterval(Type(bins)/(End-Start)), + Freq(bins, 0), + Overflow(0), + Underflow(0) {} + + // Construct a histogram from a sequence of data + template + void Add(DataInputIterator begin, DataInputIterator end) { + for (DataInputIterator iter = begin; iter != end; ++iter) + Add(static_cast(*iter)); + } + // Increase the count for the bin that holds a value that is in range + // for this histogram or the under-/overflow count if it is not in range + void Add(const Type& x) { + if (x < Start) { + ++Underflow; + } else { + const size_t i(static_cast((x-Start)*BinInterval)); + if (i < Freq.size()) ++Freq[i]; + else ++Overflow; + } + } + + // Get the sum of all counts in the histogram + inline size_t GetTotalCount() const { return std::accumulate(Freq.begin(), Freq.end(), 0); } + // Get the overflow count + inline size_t GetOverflow() const { return Overflow; } + // Get the underflow count + inline size_t GetUnderflow() const { return Underflow; } + // Get frequencies + inline const std::vector& GetHist() const { return Freq; } + // Get XbinsValue + std::vector GetXbinsValue() const { + const size_t NBins(Freq.size()); + std::vector vec_XbinValue(NBins); + const Type val((End-Start)/static_cast(NBins-1)); + for (size_t i = 0; i < NBins; ++i) + vec_XbinValue[i] = (val*static_cast(i) + Start); + return vec_XbinValue; + } + // Get start + inline Type GetStart() const { return Start; } + // Get End + inline Type GetEnd() const { return End; } + + // Returns the approximate permille + Type GetApproximatePermille(float permille) const { + ASSERT(permille >= 0.f && permille <= 1.f); + size_t NumValues(0); + for (size_t n: Freq) + NumValues += n; + size_t Num(0); + Type UpperBound(Start); + for (size_t i = 0; i < Freq.size(); ++i) { + if (static_cast(Num)/NumValues > permille) + return UpperBound; + Num += Freq[i]; + UpperBound = (static_cast(i)*End)/(Freq.size()-1)+Start; + } + return End; + } + + // Text display of the histogram + std::string ToString(const std::string& sTitle = "") const { + std::ostringstream os; + os.precision(3); + os << sTitle << "\n"; + const size_t n(Freq.size()); + for (size_t i = 0; i < n; ++i) + os << static_cast(End-Start)/n*static_cast(i) << "\t|\t" << Freq[i] << "\n"; + os << End << "\n"; + return os.str(); + } + +protected: + const Type Start, End, BinInterval; // min/max/step of values + std::vector Freq; // histogram + size_t Overflow, Underflow; // count under/over flow +}; +typedef class GENERAL_API THistogram Histogram32F; +typedef class GENERAL_API THistogram Histogram64F; +/*----------------------------------------------------------------*/ + + +class GENERAL_API Util +{ +public: + static String getAppName() { + #ifdef _MSC_VER + String buf(MAX_PATH+1, '\0'); + GetModuleFileName(NULL, &buf.front(), MAX_PATH); + return ensureUnifySlash(buf); + #else // _MSC_VER + LPTSTR home = getenv("HOME"); + if (home == NULL) + return String(); + String name(String(home) + "/app"); + return ensureUnifySlash(name); + #endif // _MSC_VER + } + + // generate a unique name based on process ID and time + static String getUniqueName(TCHAR dash='-') + { + TCHAR szDate[256]; + #ifdef _MSC_VER + SYSTEMTIME st; + GetLocalTime(&st); + LPTSTR szTime = szDate+ + GetDateFormat(LOCALE_USER_DEFAULT,0,&st,_T("yy''MM''dd"),szDate,80); + GetTimeFormat(LOCALE_USER_DEFAULT,0,&st,_T("HH''mm''ss"),szTime,80); + #else // _MSC_VER + const time_t t = time(NULL); + const struct tm *tmp = localtime(&t); + LPTSTR szTime = szDate+1+ + strftime(szDate, 80, "%y%m%d", tmp); + strftime(szTime, 80, "%H%M%S", tmp); + #endif // _MSC_VER + const uint32_t ID((uint32_t(__PROCESS__) + RAND())&0x00FFFFFF); + if (dash) + return String::FormatString("%s%c%s%c%06X", szDate, dash, szTime, dash, ID); + return String::FormatString("%s%s%06X", szDate, szTime, ID); + } + + static String translateError(int aError) { + #ifdef _MSC_VER + LPVOID lpMsgBuf; + FormatMessage( + FORMAT_MESSAGE_ALLOCATE_BUFFER | + FORMAT_MESSAGE_FROM_SYSTEM | + FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, + aError, + MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), // Default language + (LPTSTR) &lpMsgBuf, + 0, + NULL + ); + String tmp((LPCTSTR)lpMsgBuf); + // Free the buffer. + LocalFree(lpMsgBuf); + String::size_type i; + + while ((i = tmp.find_last_of(LINE_SEPARATOR_STR)) != String::npos) + tmp.erase(i, LINE_SEPARATOR_LEN); + return tmp; + #else // _MSC_VER + return strerror(aError); + #endif // _MSC_VER + } + + static String& trimUnifySlash(String& path) + { + String::size_type start = 1; + while ((start = path.find(PATH_SEPARATOR, start)) != String::npos) + if (path[start-1] == PATH_SEPARATOR) + path.erase(start, 1); + else + ++start; + return path; + } + static String& ensureUnifySlash(String& path) + { + String::size_type start = 0; + while ((start = path.find(REVERSE_PATH_SEPARATOR, start)) != String::npos) + path[start] = PATH_SEPARATOR; + return trimUnifySlash(path); + } + static String& ensureUnifyReverseSlash(String& path) + { + String::size_type start = 0; + while ((start = path.find(PATH_SEPARATOR, start)) != String::npos) + path[start] = REVERSE_PATH_SEPARATOR; + return path; + } + + static String& ensureFolderSlash(String& path) + { + if (path.empty()) + return path; + String::size_type nEnd = path.size()-1; + if (path[nEnd] != PATH_SEPARATOR) + path += PATH_SEPARATOR; + return path; + } + + static void ensureFolder(const String& path) + { + String::size_type start = 0; + while ((start = path.find(PATH_SEPARATOR, start)) != String::npos) + #ifdef _MSC_VER + CreateDirectory(path.substr(0, ++start).c_str(), NULL); + #else + mkdir(path.substr(0, ++start).c_str(), 0755); + #endif + } + + static String& ensureValidPath(String& path) { + return simplifyPath(ensureUnifySlash(strTrim(path, _T("\"")))); + } + static String& ensureValidFolderPath(String& path) { + return simplifyPath(ensureFolderSlash(ensureUnifySlash(strTrim(path, _T("\""))))); + } + + static inline bool isFullPath(LPCTSTR path) { + // returns true if local drive full path or network path + return (path && ( + #ifdef _MSC_VER + (path[1]==_T(':') && path[0]!=_T('\0')) || + #else // _MSC_VER + path[0]==_T('/') || + #endif // _MSC_VER + #ifdef UNICODE + *reinterpret_cast(path)==0x5C005C00/*"\\\\"*/)); + #else + *reinterpret_cast(path)==0x5C5C/*"\\\\"*/)); + #endif // UNICODE + } + static String getFullPath(const String& str) { + if (isFullPath(str)) + return str; + return getCurrentFolder()+str; + } + + static inline bool isParentFolder(LPCTSTR path, int off=0) { + // returns true if the folder starting at the given position in path is the parent folder ".." + if (off < 0 || path[off] != _T('.')) + return false; + if (off > 0 && path[off-1] != PATH_SEPARATOR) + return false; + if (path[off+1] != _T('.')) + return false; + return path[off+2] == _T('\0') || path[off+2] == PATH_SEPARATOR; + } + + static String getHomeFolder(); + static String getApplicationFolder(); + static String getCurrentFolder(); + static String getProcessFolder() { + return getFilePath(getAppName()); + } + + static String ensureUnitPath(const String& path) + { + if (path.find(_T(" ")) == String::npos) + return path; + return String(_T("\"")+path+_T("\"")); + } + + static String& simplifyPath(String& path) { + // compress path by removing all "./" and "folder/../" occurrences + // (if path only, it should end in path-separator) + { + // removes all "./" occurrences + String::size_type i(0); + while ((i = path.find(_T(".") PATH_SEPARATOR_STR, i)) != String::npos) { + if (i > 0 && path[i-1] != PATH_SEPARATOR) + i += 2; + else + path.erase(i, 2); + }} + { + // removes all "folder/../" occurrences + String::size_type i(0); + while ((i = path.find(_T("..") PATH_SEPARATOR_STR, i)) != String::npos) { + if (i > 1 && path[i-1] == PATH_SEPARATOR) { + String::size_type prev = path.rfind(PATH_SEPARATOR, i-2); + if (prev == String::npos) prev = 0; else ++prev; + if (!isParentFolder(path, (int)prev)) { + path.erase(prev, i+3-prev); + i = prev; + continue; + } + } + i += 3; + }} + return path; + } + static String getSimplifiedPath(String path) { + return simplifyPath(path); + } + + static String getRelativePath(const String& currentPath, const String& targetPath) { + // returns the path to the target relative to the current path; + // both current and target paths must be full paths; + // current path is assumed to be a folder, while target path can be also a file + CLISTDEF2(String) currentPathValues, targetPathValues; + Util::strSplit(currentPath, PATH_SEPARATOR, currentPathValues); + if (currentPathValues.back().empty()) + currentPathValues.pop_back(); + Util::strSplit(targetPath, PATH_SEPARATOR, targetPathValues); + size_t idxCurrentPath(0), idxTargetPath(0); + while ( + idxCurrentPath < currentPathValues.size() && + idxTargetPath < targetPathValues.size() && + #ifdef _MSC_VER + _tcsicmp(currentPathValues[idxCurrentPath], targetPathValues[idxTargetPath]) == 0 + #else + _tcscmp(currentPathValues[idxCurrentPath], targetPathValues[idxTargetPath]) == 0 + #endif + ) + ++idxCurrentPath, ++idxTargetPath; + if (idxCurrentPath == 0) + return targetPath; + String relativePath; + relativePath.reserve(targetPath.size()); + while (idxCurrentPath < currentPathValues.size()) { + relativePath += _T("..") PATH_SEPARATOR_STR; + ++idxCurrentPath; + } + const size_t idxFirstTarget(idxTargetPath); + while (idxTargetPath < targetPathValues.size()) { + if (idxTargetPath > idxFirstTarget) + relativePath += PATH_SEPARATOR; + relativePath += targetPathValues[idxTargetPath++]; + } + return relativePath; + } + + static String& getCommonPath(String& commonPath, const String& path) { + // returns the path shared by the given paths + #ifdef _MSC_VER + while (_tcsnicmp(commonPath, path, commonPath.length()) != 0) { + #else + while (_tcsncmp(commonPath, path, commonPath.length()) != 0) { + #endif + commonPath.pop_back(); + commonPath = getFilePath(commonPath); + } + return commonPath; + } + static String getCommonPath(const String* arrPaths, size_t numPaths) { + // returns the path shared by all given paths + ASSERT(numPaths > 0); + String commonPath(arrPaths[0]); + for (size_t i=1; !commonPath.empty() && i 3 && nrNumbers > 0) + return buf; + rez = (uint32_t)((sTime%((int64_t)24*3600*1000)) / (3600*1000)); + if (rez) { + ++nrNumbers; + len += _sntprintf(buf+len, 128, "%uh", rez); + } + if (nAproximate > 2 && nrNumbers > 0) + return buf; + rez = (uint32_t)((sTime%((int64_t)3600*1000)) / (60*1000)); + if (rez) { + ++nrNumbers; + len += _sntprintf(buf+len, 128, "%um", rez); + } + if (nAproximate > 1 && nrNumbers > 0) + return buf; + rez = (uint32_t)((sTime%((int64_t)60*1000)) / (1*1000)); + if (rez) { + ++nrNumbers; + len += _sntprintf(buf+len, 128, "%us", rez); + } + if (nAproximate > 0 && nrNumbers > 0) + return buf; + rez = (uint32_t)(sTime%((int64_t)1*1000)); + if (rez || !nrNumbers) + len += _sntprintf(buf+len, 128, "%ums", rez); + + return String(buf, len); + } + + static String toString(const wchar_t* wsz) { + if (wsz == NULL) + return String(); + #if 1 + return std::wstring_convert, wchar_t>().to_bytes(wsz); + #elif 1 + std::mbstate_t state = std::mbstate_t(); + const size_t len(std::wcsrtombs(NULL, &wsz, 0, &state)); + if (len == static_cast(-1)) + return String(); + std::vector mbstr(len+1); + if (std::wcsrtombs(&mbstr[0], &wsz, mbstr.size(), &state) == static_cast(-1)) + return String(); + return String(&mbstr[0]); + #else + const std::wstring ws(wsz); + const std::locale locale(""); + typedef std::codecvt converter_type; + const converter_type& converter = std::use_facet(locale); + std::vector to(ws.length() * converter.max_length()); + std::mbstate_t state; + const wchar_t* from_next; + char* to_next; + if (converter.out(state, ws.data(), ws.data() + ws.length(), from_next, &to[0], &to[0] + to.size(), to_next) != converter_type::ok) + return String(); + return std::string(&to[0], to_next); + #endif + } + + static int64_t toInt64(LPCTSTR aString) { + #ifdef _MSC_VER + return _atoi64(aString); + #else + return atoll(aString); + #endif + } + static int toInt(LPCTSTR aString) { + return atoi(aString); + } + static uint32_t toUInt32Hex(LPCTSTR aString) { + uint32_t val; + sscanf(aString, "%x", &val); + return val; + } + static uint32_t toUInt32(LPCTSTR aString) { + return (uint32_t)atoi(aString); + } + static double toDouble(LPCTSTR aString) { + return atof(aString); + } + static float toFloat(LPCTSTR aString) { + return (float)atof(aString); + } + + static time_t getTime() { + return (time_t)time(NULL); + } + + static uint32_t getTick() { + #ifdef _MSC_VER + return GetTickCount(); + #else + timeval tv; + gettimeofday(&tv, NULL); + return (uint32_t)(tv.tv_sec * 1000 ) + (tv.tv_usec / 1000); + #endif + } + + + /** + * IPRT - CRC64. + * + * The method to compute the CRC64 is referred to as CRC-64-ISO: + * http://en.wikipedia.org/wiki/Cyclic_redundancy_check + * The generator polynomial is x^64 + x^4 + x^3 + x + 1. + * Reverse polynom: 0xd800000000000000ULL + * + * As in: http://www.virtualbox.org/svn/vbox/trunk/src/VBox/Runtime/common/checksum/crc64.cpp + */ + + /** + * Calculate CRC64 for a memory block. + * + * @returns CRC64 for the memory block. + * @param pv Pointer to the memory block. + * @param cb Size of the memory block in bytes. + */ + static uint64_t CRC64(const void *pv, size_t cb); + + /** + * Start a multiblock CRC64 calculation. + * + * @returns Start CRC64. + */ + static uint64_t CRC64Start() { + return 0ULL; + } + /** + * Processes a multiblock of a CRC64 calculation. + * + * @returns Intermediate CRC64 value. + * @param uCRC64 Current CRC64 intermediate value. + * @param pv The data block to process. + * @param cb The size of the data block in bytes. + */ + static uint64_t CRC64Process(uint64_t uCRC64, const void *pv, size_t cb); + /** + * Complete a multiblock CRC64 calculation. + * + * @returns CRC64 value. + * @param uCRC64 Current CRC64 intermediate value. + */ + static uint64_t CRC64Finish(uint64_t uCRC64) { + return uCRC64; + } + + + static void Init(); + + static String GetCPUInfo(); + static String GetRAMInfo(); + static String GetOSInfo(); + static String GetDiskInfo(const String&); + enum CPUFNC {NA=0, SSE, AVX}; + static const Flags ms_CPUFNC; + + static void LogBuild(); + static void LogMemoryInfo(); + + static LPSTR* CommandLineToArgvA(LPCSTR CmdLine, size_t& _argc); + static String CommandLineToString(size_t argc, LPCTSTR* argv) { + String strCmdLine; + for (size_t i=1; i msgLen) + std::cout << String(lastMsgLen-msgLen, _T(' ')); + std::cout << std::flush; + lastMsgLen = msgLen; + } + }; +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_UTIL_H__ diff --git a/libs/Common/Util.inl b/libs/Common/Util.inl new file mode 100644 index 0000000..b0ffa0f --- /dev/null +++ b/libs/Common/Util.inl @@ -0,0 +1,1289 @@ +//////////////////////////////////////////////////////////////////// +// Util.inl +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// I N L I N E ///////////////////////////////////////////////////// + +// normalize inhomogeneous 2D point by the given camera intrinsics K +// K is assumed to be the [3,3] triangular matrix with: fx, fy, s, cx, cy and scale 1 +template +inline void NormalizeProjection(const TYPE1* K, const TYPE2* x, TYPE3* n) { + ASSERT(ISZERO(K[3]) && ISZERO(K[6]) && ISZERO(K[7]) && ISEQUAL(K[8], TYPE1(1))); + n[0] = TYPE3(K[0]*x[0] + K[1]*x[1] + K[2]); + n[1] = TYPE3( K[4]*x[1] + K[5]); +} // NormalizeProjection +// same as above, but using K inverse +template +inline void NormalizeProjectionInv(const TYPE1* K, const TYPE2* x, TYPE3* n) { + ASSERT(ISZERO(K[3]) && ISZERO(K[6]) && ISZERO(K[7]) && ISEQUAL(K[8], TYPE1(1))); + n[0] = TYPE3((K[4]*x[0] - K[1]*x[1] + (K[5]*K[1]-K[2]*K[4])) / (K[0]*K[4])); + n[1] = TYPE3((x[1] - K[5]) / K[4]); +} // NormalizeProjectionInv +/*----------------------------------------------------------------*/ + +// compute relative pose of the given two cameras +template +inline void ComputeRelativeRotation(const TMatrix& Ri, const TMatrix& Rj, TMatrix& Rij) { + Rij = Rj * Ri.t(); +} // ComputeRelativeRotation +template +inline void ComputeRelativePose(const TMatrix& Ri, const TPoint3& Ci, const TMatrix& Rj, const TPoint3& Cj, TMatrix& Rij, TPoint3& Cij) { + Rij = Rj * Ri.t(); + Cij = Ri * (Cj - Ci); +} // ComputeRelativePose +/*----------------------------------------------------------------*/ + +// Triangulate the position of a 3D point +// given two corresponding normalized projections and the pose of the second camera relative to the first one; +// returns the triangulated 3D point from the point of view of the first camera +template +bool TriangulatePoint3D( + const TMatrix& R, const TPoint3& C, + const TPoint2& pt1, const TPoint2& pt2, + TPoint3& X +) { + // convert image points to 3-vectors (of unit length) + // used to describe landmark observations/bearings in camera frames + const TPoint3 f1(pt1.x,pt1.y,1); + const TPoint3 f2(pt2.x,pt2.y,1); + const TPoint3 f2_unrotated = R.t() * f2; + const TPoint2 b(C.dot(f1), C.dot(f2_unrotated)); + // optimized inversion of A + const TYPE1 a = normSq(f1); + const TYPE1 c = f1.dot(f2_unrotated); + const TYPE1 d = -normSq(f2_unrotated); + const TYPE1 det = a*d+c*c; + if (ABS(det) < EPSILONTOLERANCE()) + return false; + const TYPE1 invDet = TYPE1(1)/det; + const TPoint2 lambda((d*b.x+c*b.y)*invDet, (a*b.y-c*b.x)*invDet); + const TPoint3 xm = lambda.x * f1; + const TPoint3 xn = C + lambda.y * f2_unrotated; + X = (xm + xn)*TYPE1(0.5); + return true; +} // TriangulatePoint3D +// same as above, but using the two camera poses; +// returns the 3D point in world coordinates +template +bool TriangulatePoint3D( + const TMatrix& K1, const TMatrix& K2, + const TMatrix& R1, const TMatrix& R2, + const TPoint3& C1, const TPoint3& C2, + const TPoint2& x1, const TPoint2& x2, + TPoint3& X +) { + TPoint2 pt1, pt2; + NormalizeProjectionInv(K1.val, x1.ptr(), pt1.ptr()); + NormalizeProjectionInv(K2.val, x2.ptr(), pt2.ptr()); + TMatrix R; TPoint3 C; + ComputeRelativePose(R1, C1, R2, C2, R, C); + if (!TriangulatePoint3D(R, C, pt1, pt2, X)) + return false; + X = R1.t() * X + C1; + return true; +} // TriangulatePoint3D +/*----------------------------------------------------------------*/ + +// compute the homography matrix transforming points from image A to image B, +// given the relative pose of B with respect to A, and the plane +// (see R. Hartley, "Multiple View Geometry," 2004, pp. 234) +template +TMatrix HomographyMatrixComposition(const TMatrix& R, const TPoint3& C, const TMatrix& n, const TYPE& d) { + const TMatrix t(R*(-C)); + return TMatrix(R - t*(n.t()*INVERT(d))); +} +template +TMatrix HomographyMatrixComposition(const TMatrix& R, const TPoint3& C, const TMatrix& n, const TMatrix& X) { + return HomographyMatrixComposition(R, C, n, -n.dot(X)); +} +template +TMatrix HomographyMatrixComposition(const TMatrix& Ri, const TPoint3& Ci, const TMatrix& Rj, const TPoint3& Cj, const TMatrix& n, const TYPE& d) { + #if 0 + Matrix3x3 R; Point3 C; + ComputeRelativePose(Ri, Ci, Rj, Cj, R, C); + return HomographyMatrixComposition(R, C, n, d); + #else + const TMatrix t((Ci - Cj)*INVERT(d)); + return TMatrix(Rj * (Ri.t() - t*n.t())); + #endif +} +template +TMatrix HomographyMatrixComposition(const TMatrix& Ri, const TPoint3& Ci, const TMatrix& Rj, const TPoint3& Cj, const TMatrix& n, const TMatrix& X) { + #if 0 + Matrix3x3 R; Point3 C; + ComputeRelativePose(Ri, Ci, Rj, Cj, R, C); + return HomographyMatrixComposition(R, C, n, X); + #else + const TMatrix t((Ci - Cj)*INVERT(n.dot(X))); + return TMatrix(Rj * (Ri.t() + t*n.t())); + #endif +} +/*----------------------------------------------------------------*/ + +// transform essential matrix to fundamental matrix +template +inline TMatrix TransformE2F(const TMatrix& E, const TMatrix& K1, const TMatrix& K2) { + return K2.inv().t() * E * K1.inv(); +} // TransformE2F +// transform fundamental matrix to essential matrix +template +inline TMatrix TransformF2E(const TMatrix& F, const TMatrix& K1, const TMatrix& K2) { + return K2.t() * F * K1; +} // TransformF2E +/*----------------------------------------------------------------*/ + +// Creates a skew symmetric cross product matrix from the provided tuple. +// compose the cross-product matrix from the given vector: axb=[a]xb +template +inline TMatrix CreateCrossProductMatrix3(const TMatrix& x) { + TMatrix X; + X(0, 0) = TYPE(0); X(1, 0) = -x(2); X(2, 0) = x(1); + X(0, 1) = x(2); X(1, 1) = TYPE(0); X(2, 1) = -x(0); + X(0, 2) = -x(1); X(1, 2) = x(0); X(2, 2) = TYPE(0); + return X; +} // CreateCrossProductMatrix3 +// same as above, but transposed: axb=[b]xTa +template +inline TMatrix CreateCrossProductMatrix3T(const TMatrix& x) { + TMatrix X; + X(0, 0) = TYPE(0); X(1, 0) = x(2); X(2, 0) = -x(1); + X(0, 1) = -x(2); X(1, 1) = TYPE(0); X(2, 1) = x(0); + X(0, 2) = x(1); X(1, 2) = -x(0); X(2, 2) = TYPE(0); + return X; +} // CreateCrossProductMatrix3T +/*----------------------------------------------------------------*/ + +// compose essential matrix from the given rotation and direction +template +inline TMatrix CreateE(const TMatrix& R, const TMatrix& C) { + const TMatrix t = -(R * C); + return CreateCrossProductMatrix3(t) * R; +} // CreateE +// compose fundamental matrix from the given rotation, direction and camera matrix +template +inline TMatrix CreateF(const TMatrix& R, const TMatrix& C, const TMatrix& K1, const TMatrix& K2) { + const TMatrix E = CreateE(R, C); + return TransformE2F(E, K1, K2); +} // CreateF +/*----------------------------------------------------------------*/ + + +// computes an RQ decomposition of 3x3 matrices as in: +// "Computing euler angles from a rotation matrix", Gregory G Slabaugh, 1999 +template +inline void RQDecomp3x3(Eigen::Matrix M, Eigen::Matrix& R, Eigen::Matrix& Q) { + // find Givens rotation for x axis: + // | 1 0 0 | + // Qx = | 0 c s |, c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) + // | 0 -s c | + Eigen::Matrix cs = Eigen::Matrix(M(2,2), M(2,1)).normalized(); + Eigen::Matrix Qx{ {1, 0, 0}, {0, cs.x(), cs.y()}, {0, -cs.y(), cs.x()} }; + R.noalias() = M * Qx; + ASSERT(std::abs(R(2,1)) < FLT_EPSILON); + R(2,1) = 0; + // find Givens rotation for y axis: + // | c 0 -s | + // Qy = | 0 1 0 |, c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2) + // | s 0 c | + cs = Eigen::Matrix(R(2,2), -R(2,0)).normalized(); + Eigen::Matrix Qy{ {cs.x(), 0, -cs.y()}, {0, 1, 0}, {cs.y(), 0, cs.x()} }; + M.noalias() = R * Qy; + ASSERT(std::abs(M(2,0)) < FLT_EPSILON); + M(2,0) = 0; + // find Givens rotation for z axis: + // | c s 0 | + // Qz = |-s c 0 |, c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2) + // | 0 0 1 | + cs = Eigen::Matrix(M(1,1), M(1,0)).normalized(); + Eigen::Matrix Qz{ {cs.x(), cs.y(), 0}, {-cs.y(), cs.x(), 0}, {0, 0, 1} }; + R.noalias() = M * Qz; + ASSERT(std::abs(R(1,0)) < FLT_EPSILON); + R(1,0) = 0; + // solve the decomposition ambiguity: + // - diagonal entries of R, except the last one, shall be positive + // - rotate R by 180 degree if necessary + if (R(0,0) < 0) { + if (R(1,1) < 0) { + // rotate around z for 180 degree: + // |-1, 0, 0| + // | 0, -1, 0| + // | 0, 0, 1| + R(0,0) *= -1; + R(0,1) *= -1; + R(1,1) *= -1; + Qz(0,0) *= -1; + Qz(0,1) *= -1; + Qz(1,0) *= -1; + Qz(1,1) *= -1; + } else { + // rotate around y for 180 degree: + // |-1, 0, 0| + // | 0, 1, 0| + // | 0, 0, -1| + R(0,0) *= -1; + R(0,2) *= -1; + R(1,2) *= -1; + R(2,2) *= -1; + Qz.transposeInPlace(); + Qy(0,0) *= -1; + Qy(0,2) *= -1; + Qy(2,0) *= -1; + Qy(2,2) *= -1; + } + } else if (R(1,1) < 0) { + // rotate around x for 180 degree: + // | 1, 0, 0| + // | 0, -1, 0| + // | 0, 0, -1| + R(0,1) *= -1; + R(0,2) *= -1; + R(1,1) *= -1; + R(1,2) *= -1; + R(2,2) *= -1; + Qz.transposeInPlace(); + Qy.transposeInPlace(); + Qx(1,1) *= -1; + Qx(1,2) *= -1; + Qx(2,1) *= -1; + Qx(2,2) *= -1; + } + // calculate orthogonal matrix + Q.noalias() = Qz.transpose() * Qy.transpose() * Qx.transpose(); +} +template +inline void RQDecomp3x3(const TMatrix& M, TMatrix& R, TMatrix& Q) { + const Eigen::Matrix _M((typename TMatrix::CEMatMap)M); + Eigen::Matrix _R, _Q; + RQDecomp3x3(_M, _R, _Q); + R = _R; Q = _Q; +} // RQDecomp3x3 +/*----------------------------------------------------------------*/ + + +// compute the angle between the two rotations given +// as in: "Disambiguating Visual Relations Using Loop Constraints", 2010 +// returns cos(angle) (same as cos(ComputeAngleSO3(I)) +template +inline TYPE ComputeAngle(const TMatrix& I) { + return CLAMP(((TYPE)cv::trace(I)-TYPE(1))*TYPE(0.5), TYPE(-1), TYPE(1)); +} // ComputeAngle +template +FORCEINLINE TYPE ComputeAngle(const TMatrix& R1, const TMatrix& R2) { + return ComputeAngle(TMatrix(R1*R2.t())); +} // ComputeAngle +/*----------------------------------------------------------------*/ + + +// compute the Frobenius (or Euclidean) norm of the distance between the two matrices +template +inline TYPE FrobeniusNorm(const TMatrix& M) { + return SQRT(((typename TMatrix::EMatMap)M).cwiseAbs2().sum()); +} // FrobeniusNorm +template +FORCEINLINE TYPE FrobeniusNorm(const TMatrix& M1, const TMatrix& M2) { + return FrobeniusNorm(TMatrix(M1-M2)); +} // FrobeniusNorm +/*----------------------------------------------------------------*/ + + +// check if any three of the given 2D points are on the same line +template +bool CheckCollinearity(const TPoint2* ptr, int count, bool checkPartialSubsets=false) { + for (int i = (checkPartialSubsets?count-1:2); i < count; ++i) { + // check that the i-th selected point does not belong + // to a line connecting some previously selected points + for (int j = 1; j < i; ++j) { + const TPoint2 d1 = ptr[j] - ptr[i]; + for (int k = 0; k < j; ++k) { + const TPoint2 d2 = ptr[k] - ptr[i]; + if (ABS(d2.x*d1.y - d2.y*d1.x) <= FLT_EPSILON*(ABS(d1.x) + ABS(d1.y) + ABS(d2.x) + ABS(d2.y))) + return true; + } + } + } + return false; +} +// check if any three of the given 3D points are on the same line +template +bool CheckCollinearity(const TPoint3* ptr, int count, bool checkPartialSubsets=false) { + for (int i = (checkPartialSubsets?count-1:2); i < count; ++i) { + // check that the i-th selected point does not belong + // to a line connecting some previously selected points + for (int j = 1; j < i; ++j) { + const TPoint3 d1 = ptr[j] - ptr[i]; + for (int k = 0; k < j; ++k) { + const TPoint3 d2 = ptr[k] - ptr[i]; + if (normSq(d1.cross(d2)) < 1.e-16) + return true; + } + } + } + return false; +} +/*----------------------------------------------------------------*/ + + +// compute the corresponding ray for a given projection matrix P[3,4] and image point pt[2,1] +// output ray[3,1] +template +inline void RayPoint_3x4_2_3(const TYPE1* P, const TYPE2* pt, TYPE1* ray) { + Eigen::Map< const Eigen::Matrix > mP(P); + const Eigen::Matrix mM(mP.template topLeftCorner<3,3>()); + TYPE1 M[9]; + InvertMatrix3x3(mM.data(), M); + ray[0] = M[0*3+0]*pt[0] + M[0*3+1]*pt[1] + M[0*3+2]; + ray[1] = M[1*3+0]*pt[0] + M[1*3+1]*pt[1] + M[1*3+2]; + ray[2] = M[2*3+0]*pt[0] + M[2*3+1]*pt[1] + M[2*3+2]; +} // RayPoint_3x4_2_3 +/*----------------------------------------------------------------*/ + +// project column vertex - used only for visualization purposes +// (optimized ProjectVertex for P[3,4] and X[3,1], output pt[3,1]) +template +inline void ProjectVertex_3x4_3_3(const TYPE1* P, const TYPE1* X, TYPE2* pt) { + pt[0] = (TYPE2)(P[0*4+0]*X[0] + P[0*4+1]*X[1] + P[0*4+2]*X[2] + P[0*4+3]); + pt[1] = (TYPE2)(P[1*4+0]*X[0] + P[1*4+1]*X[1] + P[1*4+2]*X[2] + P[1*4+3]); + pt[2] = (TYPE2)(P[2*4+0]*X[0] + P[2*4+1]*X[1] + P[2*4+2]*X[2] + P[2*4+3]); +} // ProjectVertex_3x4_3_3 +// (optimized ProjectVertex for P[3,4] and X[4,1], output pt[3,1]) +template +inline void ProjectVertex_3x4_4_3(const TYPE1* P, const TYPE1* X, TYPE2* pt) { + pt[0] = (TYPE2)(P[0*4+0]*X[0] + P[0*4+1]*X[1] + P[0*4+2]*X[2] + P[0*4+3]*X[3]); + pt[1] = (TYPE2)(P[1*4+0]*X[0] + P[1*4+1]*X[1] + P[1*4+2]*X[2] + P[1*4+3]*X[3]); + pt[2] = (TYPE2)(P[2*4+0]*X[0] + P[2*4+1]*X[1] + P[2*4+2]*X[2] + P[2*4+3]*X[3]); +} // ProjectVertex_3x4_4_3 +// (optimized ProjectVertex for R[3,3], C[3,1] and X[3,1], output pt[2,1]) +template +inline void ProjectVertex_3x3_3_3_2(const TYPE1* R, const TYPE1* C, const TYPE1* X, TYPE2* pt) { + const TYPE1 T[3] = {X[0]-C[0], X[1]-C[1], X[2]-C[2]}; + const TYPE1 invW(INVERT(R[2*3+0]*T[0] + R[2*3+1]*T[1] + R[2*3+2]*T[2])); + pt[0] = (TYPE2)((R[0*3+0]*T[0] + R[0*3+1]*T[1] + R[0*3+2]*T[2]) * invW); + pt[1] = (TYPE2)((R[1*3+0]*T[0] + R[1*3+1]*T[1] + R[1*3+2]*T[2]) * invW); +} // ProjectVertex_3x3_3_3_2 +// (optimized ProjectVertex for R[3,3], C[3,1] and X[3,1], output pt[3,1]) +template +inline void ProjectVertex_3x3_3_3_3(const TYPE1* R, const TYPE1* C, const TYPE1* X, TYPE2* pt) { + const TYPE1 T[3] = {X[0]-C[0], X[1]-C[1], X[2]-C[2]}; + pt[0] = (TYPE2)(R[0*3+0]*T[0] + R[0*3+1]*T[1] + R[0*3+2]*T[2]); + pt[1] = (TYPE2)(R[1*3+0]*T[0] + R[1*3+1]*T[1] + R[1*3+2]*T[2]); + pt[2] = (TYPE2)(R[2*3+0]*T[0] + R[2*3+1]*T[1] + R[2*3+2]*T[2]); +} // ProjectVertex_3x3_3_3_3 +// (optimized ProjectVertex for H[3,3] and X[2,1], output pt[3,1]) +template +inline void ProjectVertex_3x3_2_3(const TYPE1* H, const TYPE2* X, TYPE3* pt) { + pt[0] = (TYPE3)(H[0*3+0]*X[0] + H[0*3+1]*X[1] + H[0*3+2]); + pt[1] = (TYPE3)(H[1*3+0]*X[0] + H[1*3+1]*X[1] + H[1*3+2]); + pt[2] = (TYPE3)(H[2*3+0]*X[0] + H[2*3+1]*X[1] + H[2*3+2]); +} // ProjectVertex_3x3_2_3 +// (optimized ProjectVertex for H[3,3] and X[2,1], output pt[2,1]) +template +inline void ProjectVertex_3x3_2_2(const TYPE1* H, const TYPE2* X, TYPE3* pt) { + const TYPE1 invZ(INVERT(H[2*3+0]*X[0] + H[2*3+1]*X[1] + H[2*3+2])); + pt[0] = (TYPE3)((H[0*3+0]*X[0] + H[0*3+1]*X[1] + H[0*3+2])*invZ); + pt[1] = (TYPE3)((H[1*3+0]*X[0] + H[1*3+1]*X[1] + H[1*3+2])*invZ); +} // ProjectVertex_3x3_2_2 +/*----------------------------------------------------------------*/ + +// project column vertex - used only for visualization purposes +// (optimized ProjectVertex for P[3,4] and X[3,1], output pt[2,1]) +template +inline void ProjectVertex_3x4_3_2(const TYPE1* P, const TYPE1* X, TYPE2* pt) { + const TYPE1 invW(INVERT(P[2*4+0]*X[0] + P[2*4+1]*X[1] + P[2*4+2]*X[2] + P[2*4+3])); + pt[0] = (TYPE2)((P[0*4+0]*X[0] + P[0*4+1]*X[1] + P[0*4+2]*X[2] + P[0*4+3]) * invW); + pt[1] = (TYPE2)((P[1*4+0]*X[0] + P[1*4+1]*X[1] + P[1*4+2]*X[2] + P[1*4+3]) * invW); +} // ProjectVertex_3x4_3_2 +// (optimized ProjectVertex for P[3,4] and X[4,1], output pt[2,1]) +template +inline void ProjectVertex_3x4_4_2(const TYPE1* P, const TYPE1* X, TYPE2* pt) { + const TYPE1 invW(INVERT(P[2*4+0]*X[0] + P[2*4+1]*X[1] + P[2*4+2]*X[2] + P[2*4+3]*X[3])); + pt[0] = (TYPE2)((P[0*4+0]*X[0] + P[0*4+1]*X[1] + P[0*4+2]*X[2] + P[0*4+3]*X[3]) * invW); + pt[1] = (TYPE2)((P[1*4+0]*X[0] + P[1*4+1]*X[1] + P[1*4+2]*X[2] + P[1*4+3]*X[3]) * invW); +} // ProjectVertex_3x4_4_2 +/*----------------------------------------------------------------*/ + +// project column vertex using the transpose of the given projective matrix +// (optimized ProjectVertex for P[3,4].t() and X[3,1], output pt[4,1]) +template +inline void ProjectVertex_3x4t_3_4(const TYPE1* P, const TYPE2* X, TYPE3* pt) { + pt[0] = (TYPE3)(P[0*4+0]*X[0] + P[1*4+0]*X[1] + P[2*4+0]*X[2]); + pt[1] = (TYPE3)(P[0*4+1]*X[0] + P[1*4+1]*X[1] + P[2*4+1]*X[2]); + pt[2] = (TYPE3)(P[0*4+2]*X[0] + P[1*4+2]*X[1] + P[2*4+2]*X[2]); + pt[3] = (TYPE3)(P[0*4+3]*X[0] + P[1*4+3]*X[1] + P[2*4+3]*X[2]); +} // ProjectVertex_3x4t_3_4 +// (optimized ProjectVertex for P[3,4].t() and X[3,1], output pt[3,1] - the first 3 coordinates) +template +inline void ProjectVertex_3x4t_3_3(const TYPE1* P, const TYPE2* X, TYPE3* pt) { + pt[0] = (TYPE3)(P[0*4+0]*X[0] + P[1*4+0]*X[1] + P[2*4+0]*X[2]); + pt[1] = (TYPE3)(P[0*4+1]*X[0] + P[1*4+1]*X[1] + P[2*4+1]*X[2]); + pt[2] = (TYPE3)(P[0*4+2]*X[0] + P[1*4+2]*X[1] + P[2*4+2]*X[2]); +} // ProjectVertex_3x4t_3_3 +/*----------------------------------------------------------------*/ + + +// given the camera matrix, compute the error between the given 2D point and the reprojected 3D point; +// note that here we're using "dirty" projection method which is suitable only for visualization; on the upside, it shouldn't matter because +// if a point is projected as infinite, there's something wrong with it anyway; +// (optimized ComputeReprojectionError for P[3,4] and X[3,1], output pt[2,1]) +template +inline TYPE2 ComputeReprojectionError_3x4_3_2(const TYPE1* P, const TYPE1* X, const TYPE2* pt) { + TYPE2 reprojection[2]; + ProjectVertex_3x4_3_2(P, X, reprojection); + return SQUARE(reprojection[0]-pt[0])+SQUARE(reprojection[1]-pt[1]); +} // ComputeReprojectionError_3x4_3_2 +// (optimized ComputeReprojectionError for P[3,4] and X[4,1], output pt[2,1]) +template +inline TYPE2 ComputeReprojectionError_3x4_4_2(const TYPE1* P, const TYPE1* X, const TYPE2* pt) { + TYPE2 reprojection[2]; + ProjectVertex_3x4_4_2(P, X, reprojection); + return SQUARE(reprojection[0]-pt[0])+SQUARE(reprojection[1]-pt[1]); +} // ComputeReprojectionError_3x4_4_2 +// (optimized ComputeReprojectionError for R[3,3], C[3,1] and X[3,1], output pt[2,1]) +template +inline TYPE2 ComputeReprojectionError_3x3_3_3_2(const TYPE1* R, const TYPE1* C, const TYPE1* X, const TYPE2* pt) { + TYPE2 reprojection[2]; + ProjectVertex_3x3_3_3_2(R, C, X, reprojection); + return SQUARE(reprojection[0]-pt[0])+SQUARE(reprojection[1]-pt[1]); +} // ComputeReprojectionError_3x3_3_3_2 +/*----------------------------------------------------------------*/ + + +// computes the depth of the given point +// from the point of view of the given camera pose +template +inline TYPE PointDepth(const TYPE* R, const TYPE* C, const TYPE* X) { + return R[2*3+0]*X[0] + R[2*3+1]*X[1] + R[2*3+2]*X[2] + C[2]; +} // PointDepth +template +inline TYPE PointDepth(const TYPE* P, const TYPE* X) { + return P[2*4+0]*X[0] + P[2*4+1]*X[1] + P[2*4+2]*X[2] + P[2*4+3]; +} // PointDepth +/*----------------------------------------------------------------*/ + + +// reproject the given 3D point with the given view; +// return squared reprojection error +// or FLT_MAX if the point is behind the camera +// reproj return the homogeneous reprojection +template +inline TYPE2 ProjectPoint(const TYPE1* P, const TYPE1* X, const TYPE2* proj, TYPE2* reproj) { + // reproject the 3D point on this view + ProjectVertex_3x4_3_3(P, X, reproj); + // filter out points behind + if (reproj[2] <= TYPE2(0)) + return FLT_MAX; + // return the reprojection error + return SQUARE(reproj[0]/reproj[2]-proj[0])+SQUARE(reproj[1]/reproj[2]-proj[1]); +} // ProjectPoint +template +inline TYPE2 ProjectPoint(const TYPE1* R, const TYPE1* C, const TYPE1* X, const TYPE2* proj, TYPE2* reproj) { + // reproject the 3D point on this view + ProjectVertex_3x3_3_3_3(R, C, X, reproj); + // filter out points behind + if (reproj[2] <= TYPE2(0)) + return FLT_MAX; + // return the reprojection error + return SQUARE(reproj[0]/reproj[2]-proj[0])+SQUARE(reproj[1]/reproj[2]-proj[1]); +} // ProjectPoint +/*----------------------------------------------------------------*/ + +// reproject the given 3D point with the given view; +// return true if the point is not behind the camera +// and the reprojection error is small enough; +// returns false otherwise +// reproj returns the homogeneous reprojection (first 3) and errSq (last 1) +template +inline bool IsPointInlier(const TYPE1* P, const TYPE1* X, const TYPE2* proj, TYPE2* reproj, TYPE2 thresholdSq) { + // reproject the 3D point on this view + ProjectVertex_3x4_3_3(P, X, reproj); + // filter out points behind the camera or + // having the reprojection error bigger than the given threshold + return (reproj[2] > TYPE2(0) && + (reproj[3]=(SQUARE(reproj[0]/reproj[2]-proj[0])+SQUARE(reproj[1]/reproj[2]-proj[1]))) <= thresholdSq); +} // IsPointInlier +template +inline bool IsPointInlier(const TYPE1* R, const TYPE1* C, const TYPE1* X, const TYPE2* proj, TYPE2* reproj, TYPE2 thresholdSq) { + // reproject the 3D point on this view + ProjectVertex_3x3_3_3_3(R, C, X, reproj); + // filter out points behind the camera or + // having the reprojection error bigger than the given threshold + return (reproj[2] > TYPE2(0) && + (reproj[3]=(SQUARE(reproj[0]/reproj[2]-proj[0])+SQUARE(reproj[1]/reproj[2]-proj[1]))) <= thresholdSq); +} // IsPointInlier +/*----------------------------------------------------------------*/ + + +// given a rotation matrix, return the angle in radians corresponding to the axis/angle decomposition +template +inline TYPE AngleFromRotationMatrix(const TYPE* R) { + const TYPE a = (R[0*3+0]+R[1*3+1]+R[2*3+2]-TYPE(1))/TYPE(2); + if (a < TYPE(-1)) + return acos(TYPE(-1)); + if (a > TYPE(1)) + return acos(TYPE(1)); + return acos(a); +} +template +FORCEINLINE TYPE AngleFromRotationMatrix(const TMatrix& R) { + return AngleFromRotationMatrix(R.val); +} +/*----------------------------------------------------------------*/ + +// given two 3D vectors, +// compute the angle between them +// returns cos(angle) +template +inline TYPE2 ComputeAngle(const TYPE1* V1, const TYPE1* V2) { + return CLAMP(TYPE2((V1[0]*V2[0]+V1[1]*V2[1]+V1[2]*V2[2])/SQRT((V1[0]*V1[0]+V1[1]*V1[1]+V1[2]*V1[2])*(V2[0]*V2[0]+V2[1]*V2[1]+V2[2]*V2[2]))), TYPE2(-1), TYPE2(1)); +} // ComputeAngle +// same as above, but with the vectors normalized +template +inline TYPE2 ComputeAngleN(const TYPE1* V1, const TYPE1* V2) { + return CLAMP(TYPE2(V1[0]*V2[0]+V1[1]*V2[1]+V1[2]*V2[2]), TYPE2(-1), TYPE2(1)); +} // ComputeAngleN +// given three 3D points, +// compute the angle between the vectors formed by the first point with the other two +// returns cos(angle) +template +inline TYPE2 ComputeAngle(const TYPE1* B, const TYPE1* X1, const TYPE1* X2) { + // create the two vectors + const TYPE1 V1[] = {X1[0]-B[0], X1[1]-B[1], X1[2]-B[2]}; + const TYPE1 V2[] = {X2[0]-B[0], X2[1]-B[1], X2[2]-B[2]}; + return ComputeAngle(V1, V2); +} // ComputeAngle +// given four 3D points, +// compute the angle between the two vectors formed by the first and second pair of points +// returns cos(angle) +template +inline TYPE2 ComputeAngle(const TYPE1* X1, const TYPE1* C1, const TYPE1* X2, const TYPE1* C2) { + // subtract out the camera center + const TYPE1 V1[] = {X1[0]-C1[0], X1[1]-C1[1], X1[2]-C1[2]}; + const TYPE1 V2[] = {X2[0]-C2[0], X2[1]-C2[1], X2[2]-C2[2]}; + return ComputeAngle(V1, V2); +} // ComputeAngle +/*----------------------------------------------------------------*/ + +// given a triangle defined by three 3D points, +// compute its normal (plane's normal oriented according to the given points order) +template +inline TPoint3 ComputeTriangleNormal(const TPoint3& v0, const TPoint3& v1, const TPoint3& v2) { + return (v1-v0).cross(v2-v0); +} // ComputeTriangleNormal +/*----------------------------------------------------------------*/ + +// compute the area of a triangle using Heron's formula +template +TYPE ComputeTriangleAreaSqLen(TYPE lena, TYPE lenb, TYPE lenc) { + const TYPE s((lena+lenb+lenc)/TYPE(2)); + return s*(s-lena)*(s-lenb)*(s-lenc); +} +template +TYPE ComputeTriangleAreaLen(TYPE lena, TYPE lenb, TYPE lenc) { + return (TYPE)sqrt(ComputeTriangleAreaSqLen(lena, lenb, lenc)); +} +template +TYPE ComputeTriangleAreaSqLenSq(TYPE lenaSq, TYPE lenbSq, TYPE lencSq) { + return SQUARE(lenaSq+lenbSq+lencSq)/TYPE(2)-(lenaSq*lenaSq+lenbSq*lenbSq+lencSq*lencSq); +} +template +TYPE ComputeTriangleAreaLenSq(TYPE lenaSq, TYPE lenbSq, TYPE lencSq) { + return (TYPE)sqrt(ComputeTriangleAreaSqLenSq(lenaSq, lenbSq, lencSq)); +} +// compute area for a triangle defined by three 2D points +template +TYPE EdgeFunction(const TPoint2& x0, const TPoint2& x1, const TPoint2& x2) { + return TYPE((x2-x0).cross(x1-x0)); +} +template +TYPE ComputeTriangleArea(const TPoint2& x0, const TPoint2& x1, const TPoint2& x2) { + return (TYPE)abs(EdgeFunction(x0, x1, x2)/TYPE(2)); +} +// compute area for a triangle defined by three 3D points +template +TYPE ComputeTriangleAreaSq(const TPoint3& x0, const TPoint3& x1, const TPoint3& x2) { + return (TYPE)normSq((x1-x0).cross(x2-x0))/TYPE(4); +} +template +TYPE ComputeTriangleArea(const TPoint3& x0, const TPoint3& x1, const TPoint3& x2) { + return (TYPE)sqrt(ComputeTriangleAreaSq(x0, x1, x2)); +} // ComputeTriangleArea +/*----------------------------------------------------------------*/ + +// compute signed volume of the tetrahedron defined by the given triangle connected to origin +template +TYPE ComputeTriangleVolume(const TPoint3& x0, const TPoint3& x1, const TPoint3& x2) { + return (TYPE)(x0.dot(x1.cross(x2)) / TYPE(6)); +} // ComputeTriangleVolume +/*----------------------------------------------------------------*/ + +// compute a shape quality measure of the triangle composed by vertices (v0,v1,v2) +// returns 2*AreaTri/(MaxEdge^2) in range [0, 0.866] +// (ex: equilateral sqrt(3)/2, half-square 1/2, up to a line that has zero quality) +template +inline TYPE ComputeTriangleQuality(const TPoint3& v0, const TPoint3& v1, const TPoint3& v2) { + const TPoint3 d10(v1-v0); + const TPoint3 d20(v2-v0); + const TPoint3 d12(v1-v2); + const TYPE a((TYPE)norm(d10.cross(d20))); + if (a == 0) return 0; // area zero triangles have surely zero quality + const TYPE nd10(normSq(d10)); + if (nd10 == 0) return 0; // area zero triangles have surely zero quality + const TYPE b(MAXF3(nd10, normSq(d20), normSq(d12))); + return a/b; +} // ComputeTriangleQuality +/*----------------------------------------------------------------*/ + +// given a triangle defined by 3 vertex positions and a point, +// compute the barycentric coordinates corresponding to that point +// (only the first two values: alpha and beta) +template +inline TPoint2 BarycentricCoordinatesUV(const TPoint3& A, const TPoint3& B, const TPoint3& C, const TPoint3& P) { + #if 0 + // the triangle normal + const TPoint3 normalVec((B-A).cross(C-A)); + //const TYPE normalLen(norm(normalVec)); + // the area of the triangles + const TYPE invAreaABC(INVERT(normSq(normalVec)/*/(2*normalLen)*/)); + const TPoint3 CP(C-P); + const TYPE areaPBC(normalVec.dot((B-P).cross(CP))/*/(2*normalLen)*/); + const TYPE areaPCA(normalVec.dot(CP.cross(A-P))/*/(2*normalLen)*/); + // the barycentric coordinates + return TPoint2( + areaPBC * invAreaABC, // alpha + areaPCA * invAreaABC // beta + ); + #else + // using the Cramer's rule for solving a linear system + const TPoint3 v0(A-C), v1(B-C), v2(P-C); + const TYPE d00(normSq(v0)); + const TYPE d01(v0.dot(v1)); + const TYPE d11(normSq(v1)); + const TYPE d20(v2.dot(v0)); + const TYPE d21(v2.dot(v1)); + const TYPE invDenom(INVERT(d00 * d11 - d01 * d01)); + return TPoint2( + (d11 * d20 - d01 * d21) * invDenom, // alpha + (d00 * d21 - d01 * d20) * invDenom // beta + ); + #endif +} +// same as above, but returns all three barycentric coordinates +template +inline TPoint3 BarycentricCoordinates(const TPoint3& A, const TPoint3& B, const TPoint3& C, const TPoint3& P) { + TPoint2 b(BarycentricCoordinatesUV(A, B, C, P)); + return TPoint3( + b.x, // alpha + b.y, // beta + TYPE(1)-b.x-b.y // gamma + ); +} +// same as above, but for 2D triangle case +template +inline TPoint2 BarycentricCoordinatesUV(const TPoint2& A, const TPoint2& B, const TPoint2& C, const TPoint2& P) { + const TPoint2 D(P - C); + const TYPE d00(A.x - C.x); + const TYPE d01(B.x - C.x); + const TYPE d10(A.y - C.y); + const TYPE d11(B.y - C.y); + const TYPE invDet(INVERT(d00 * d11 - d10 * d01)); + return TPoint2( + (d11 * D.x - d01 * D.y) * invDet, // alpha + (d00 * D.y - d10 * D.x) * invDet // beta + ); +} +// same as above, but returns all three barycentric coordinates +template +inline TPoint3 BarycentricCoordinates(const TPoint2& A, const TPoint2& B, const TPoint2& C, const TPoint2& P) { + TPoint2 b(BarycentricCoordinatesUV(A, B, C, P)); + return TPoint3( + b.x, // alpha + b.y, // beta + TYPE(1)-b.x-b.y // gamma + ); +} +// correct the barycentric coordinates in case of numerical errors +// (the corresponding point is assumed to be inside the triangle) +template +inline TPoint3 CorrectBarycentricCoordinates(TPoint2 b) { + if (b.x < TYPE(0)) // alpha + b.x = TYPE(0); + else if (b.x > TYPE(1)) + b.x = TYPE(1); + if (b.y < TYPE(0)) // beta + b.y = TYPE(0); + else if (b.y > TYPE(1)) + b.y = TYPE(1); + TYPE z(TYPE(1) - b.x - b.y); // gamma + if (z < 0) { + // equally distribute the error + const TYPE half(-z/TYPE(2)); + if (half > b.x) { + b.x = TYPE(0); + b.y = TYPE(1); + } else + if (half > b.y) { + b.x = TYPE(1); + b.y = TYPE(0); + } else { + b.x -= half; + b.y -= half; + } + z = TYPE(0); + } + // check that the given point is inside the triangle + ASSERT((b.x >= 0) && (b.y >= 0) && (b.x+b.y <= 1) && ISEQUAL(b.x+b.y+z, TYPE(1))); + return TPoint3(b.x, b.y, z); +} +template +inline TPoint3 PerspectiveCorrectBarycentricCoordinates(const TPoint3& b, TYPE z0, TYPE z1, TYPE z2) { + const TPoint3 pb(b.x * z1 * z2, b.y * z0 * z2, b.z * z0 * z1); + return pb / (pb.x + pb.y + pb.z); +} +/*----------------------------------------------------------------*/ + +// Encodes/decodes a normalized 3D vector in two parameters for the direction +template +inline void Normal2Dir(const TPoint3& d, TPoint2& p) { + ASSERT(ISEQUAL(norm(d), T(1))); + p.x = TR(atan2(d.y, d.x)); + p.y = TR(acos(d.z)); +} +template +inline void Dir2Normal(const TPoint2& p, TPoint3& d) { + const T siny(sin(p.y)); + d.x = TR(cos(p.x)*siny); + d.y = TR(sin(p.x)*siny); + d.z = TR(cos(p.y)); + ASSERT(ISEQUAL(norm(d), TR(1))); +} +// Encodes/decodes a 3D vector in two parameters for the direction and one parameter for the scale +template +inline void Vector2DirScale(const T vect[3], TR dir[2], TR* scale) +{ + const T scl(sqrt(SQUARE(vect[0])+SQUARE(vect[1])+SQUARE(vect[2]))); + ASSERT(!ISZERO(scl)); + scale[0] = TR(scl); + dir[0] = TR(atan2(vect[1], vect[0])); + dir[1] = TR(acos(vect[2] / scl)); +} +template +inline void DirScale2Vector(const T dir[2], const T* scale, TR vect[3]) +{ + ASSERT(!ISZERO(*scale)); + const T siny(*scale*sin(dir[1])); + vect[0] = TR(cos(dir[0])*siny); + vect[1] = TR(sin(dir[0])*siny); + vect[2] = TR(*scale*cos(dir[1])); +} +/*----------------------------------------------------------------*/ + + +template +inline T MaxDepthDifference(T d, T threshold) { + #if 0 + return (d*threshold*T(2))/(threshold+T(2)); + #else + return d*threshold; + #endif +} +template +inline T DepthSimilarity(T d0, T d1) { + ASSERT(d0 > 0); + #if 0 + return ABS(d0-d1)*T(2)/(d0+d1); + #else + return ABS(d0-d1)/d0; + #endif +} +template +inline bool IsDepthSimilar(T d0, T d1, T threshold=T(0.01)) { + return DepthSimilarity(d0, d1) < threshold; +} +template +inline bool IsNormalSimilar(const TPoint3& n0, const TPoint3& n1, T threshold=T(0.996194698)/*COS(FD2R(5.f))*/) { + return ComputeAngle(n0.ptr(), n1.ptr()) > threshold; +} +/*----------------------------------------------------------------*/ + + +// searches min/max value +template +inline TYPE FindMinElement(const TYPE* values, size_t n) { + TYPE m = std::numeric_limits::max(); + while (n) if (values[--n] < m) m = values[n]; + return m; +} +template +inline TYPE FindMaxElement(const TYPE* values, size_t n) { + TYPE m = -std::numeric_limits::max(); + while (n) if (values[--n] > m) m = values[n]; + return m; +} +// like above, but considers absolute value only +template +inline TYPE FindAbsMinElement(const TYPE* values, size_t n) { + TYPE m = std::numeric_limits::max(); + while (n) if (ABS(values[--n]) < m) m = ABS(values[n]); + return m; +} +template +inline TYPE FindAbsMaxElement(const TYPE* values, size_t n) { + TYPE m = -std::numeric_limits::max(); + while (n) if (ABS(values[--n]) > m) m = ABS(values[n]); + return m; +} +/*----------------------------------------------------------------*/ + + +// given an array of values and their bound, approximate the area covered, in percentage +template +inline TYPE ComputeCoveredArea(const TYPE* values, size_t size, const TYPE* bound, int stride=n) { + ASSERT(size > 0); + typedef Eigen::Matrix Vector; + typedef Eigen::Map MapVector; + typedef Eigen::Matrix Matrix; + typedef Eigen::Map > MapMatrix; + typedef Eigen::Matrix MatrixSurface; + const MapMatrix points(values, size, n, Eigen::OuterStride<>(stride)); + const Vector norm = MapVector(bound); + const Vector offset(Vector::Constant(bCentered ? TYPE(0.5) : TYPE(0))); + MatrixSurface surface; + surface.setZero(); + for (size_t i=0; i=0 && point(0)=0 && point(1) +inline void ComputeCovarianceMatrix(const TYPE* values, size_t size, TYPE* cov, int stride=n) { + ASSERT(size > 0); + typedef Eigen::Matrix Vector; + typedef Eigen::Matrix Matrix; + typedef Eigen::Map > MapMatrix; + typedef Eigen::Matrix MatrixOut; + typedef Eigen::Map MapMatrixOut; + const MapMatrix points(values, size, n, Eigen::OuterStride<>(stride)); + const Vector mean(points.colwise().sum() * (TYPE(1)/size)); + const Matrix transPoints(points.rowwise() - mean); + MapMatrixOut mapCov(cov); + mapCov = transPoints.transpose() * transPoints * (TYPE(1)/(size-1)); +} // ComputeCovarianceMatrix +/*----------------------------------------------------------------*/ + +// given an array of values, compute the mean +template +inline void ComputeMean(const TYPE* values, size_t size, TYPEW& mean) { + ASSERT(size > 0); + TYPEW sum(0); + for (size_t i=0; i +inline void ComputeGeometricMean(const TYPE* values, size_t size, TYPEW& gmean) { + ASSERT(size > 0); + TYPEW prod(1); + for (size_t i=0; i +inline void ComputeMeanStdOffline(const TYPE* values, size_t size, TYPEW& mean, TYPEW& stddev) { + ASSERT(size > 0); + TYPEW sum(0); + FOREACHRAWPTR(pVal, values, size) + sum += TYPEW(*pVal); + mean = sum / (float)size; + TYPEW sumSq(0); + FOREACHRAWPTR(pVal, values, size) + sumSq += SQUARE(TYPEW(*pVal)-mean); + const TYPEW variance(sumSq / (size - 1)); + stddev = SQRT(variance); +} // ComputeMeanStdOffline +// same as above, but uses one pass only (online) +template +inline void ComputeMeanStdOnline(const TYPE* values, size_t size, TYPEW& mean, TYPEW& stddev) { + ASSERT(size > 0); + TYPEW sum(0), sumSq(0); + FOREACHRAWPTR(pVal, values, size) { + const TYPEW val(*pVal); + sum += val; + sumSq += SQUARE(val); + } + const TYPEW invSize(TYPEW(1)/(float)size); + mean = sum * invSize; + const TYPEW variance((sumSq - SQUARE(sum) * invSize) / (float)(size - 1)); + stddev = SQRT(variance); +} // ComputeMeanStdOnlineFast +#define ComputeMeanStd ComputeMeanStdOnline +// same as above, but an interactive version +template +struct MeanStd { + typedef TYPE Type; + typedef TYPEW TypeW; + typedef TYPER TypeR; + typedef ARGTYPE ArgType; + TYPEW sum, sumSq; + size_t size; + MeanStd() : sum(0), sumSq(0), size(0) {} + MeanStd(const Type* values, size_t _size) : MeanStd() { Compute(values, _size); } + void Update(ArgType v) { + const TYPEW val(static_cast(v)); + sum += val; + sumSq += SQUARE(val); + ++size; + } + void Compute(const Type* values, size_t _size) { + for (size_t i=0; i<_size; ++i) + Update(values[i]); + } + TYPEW GetSum() const { return sum; } + TYPEW GetMean() const { return static_cast(sum / static_cast(size)); } + TYPEW GetRMS() const { return static_cast(SQRT(sumSq / static_cast(size))); } + TYPEW GetVarianceN() const { return static_cast(sumSq - SQUARE(sum) / static_cast(size)); } + TYPEW GetVariance() const { return static_cast(GetVarianceN() / static_cast(size)); } + TYPEW GetStdDev() const { return SQRT(GetVariance()); } + void Clear() { sum = sumSq = TYPEW(0); size = 0; } +}; +// same as above, but records also min/max values +template +struct MeanStdMinMax : MeanStd { + typedef MeanStd Base; + typedef TYPE Type; + typedef TYPEW TypeW; + typedef TYPER TypeR; + typedef ARGTYPE ArgType; + Type minVal, maxVal; + MeanStdMinMax() : minVal(std::numeric_limits::max()), maxVal(std::numeric_limits::lowest()) {} + MeanStdMinMax(const Type* values, size_t _size) : MeanStdMinMax() { Compute(values, _size); } + void Update(ArgType v) { + if (minVal > v) + minVal = v; + if (maxVal < v) + maxVal = v; + Base::Update(v); + } + void Compute(const Type* values, size_t _size) { + for (size_t i=0; i<_size; ++i) + Update(values[i]); + } +}; +/*----------------------------------------------------------------*/ + +// given an array of values, compute the X84 threshold as in: +// Hampel FR, Rousseeuw PJ, Ronchetti EM, Stahel WA +// "Robust Statistics: the Approach Based on Influence Functions" +// Wiley Series in Probability and Mathematical Statistics, John Wiley & Sons, 1986 +// returns the pair(median,trust_region) +// upper-bound threshold = median+trust_region +// lower-bound threshold = median-trust_region +template +inline std::pair ComputeX84Threshold(const TYPE* const values, size_t size, TYPEW mul=TYPEW(5.2), const uint8_t* mask=NULL) { + ASSERT(size > 0); + // median = MEDIAN(values); + cList data; + if (mask) { + // use only masked data + data.Reserve(size); + for (size_t i=0; i::type; + for (TYPE& val: data) + val = TYPE(ABS(TYPEI(val)-TYPEI(median))); + std::nth_element(data.Begin(), mid, data.End()); + return std::make_pair(median, mul*TYPEW(*mid)); +} // ComputeX84Threshold +/*----------------------------------------------------------------*/ + +// given an array of values, compute the upper/lower threshold using quartiles +template +inline std::tuple ComputeQuartileThreshold(const TYPE* const values, size_t size, TYPEW mul=TYPEW(1.5), const uint8_t* mask=NULL) { + ASSERT(size > 0); + cList data(size); + if (mask) { + // use only masked data + data.Reserve(size); + for (size_t i=0; i +inline REAL ComputeSNR(const TDMatrix& x0, const TDMatrix& x) { + ASSERT(x0.area() == x.area()); + const REAL err(norm(TDMatrix(x0 - x))); + const REAL x0Norm(norm(x0)); + REAL ret(std::numeric_limits::infinity()); + if (ISZERO(x0Norm) && err > 0) ret = 0; + else if (err > 0) ret = 20.0 * std::log10(x0Norm / err); + return ret; +} // ComputeSNR +template +inline REAL ComputeSNR(const TMatrix& x0, const TMatrix& x) { + return ComputeSNR(TDMatrix(N,1,(TYPE*)x0.val), TDMatrix(N,1,(TYPE*)x.val)); +} // ComputeSNR +template +inline REAL ComputePSNR(const TDMatrix& x0, const TDMatrix& x) { + ASSERT(x0.area() == x.area()); + const size_t N(x0.area()); + const REAL err(normSq(TDMatrix(x0 - x)) / N); + TYPE max1(0), max2(0); + for (unsigned i=0; i::infinity()); + if (ISZERO(maxBoth) && err > 0) ret = 0; + else if (err > 0) ret = 10.0 * std::log10(static_cast(maxBoth*maxBoth) / err); + return ret; +} // ComputePSNR +template +inline REAL ComputePSNR(const TMatrix& x0, const TMatrix& x) { + return ComputePSNR(TDMatrix(N,1,(TYPE*)x0.val), TDMatrix(N,1,(TYPE*)x.val)); +} // ComputePSNR +/*----------------------------------------------------------------*/ + + +// Builds histograms using "Kernel Density Estimation" and Gaussian kernels; +// see: L. Wassermann: "All of Statistics" for example. +// Samples should be composed of two point: value and weight +template +inline TYPE BandwidthGaussianKDE(const TMatrix* samples, size_t numSamples, TYPE sigma=0, TYPE* pSigma=NULL) { + if (sigma == 0) { + TYPE avg(0); + FOREACHRAWPTR(pSample, samples, numSamples) { + const TYPE& sample((*pSample)(0)); + avg += sample; + sigma += SQUARE(sample); + } + avg /= numSamples; + sigma = SQRT(sigma/numSamples - avg*avg); // Standard Deviation + if (pSigma) *pSigma = sigma; + } + // This is the optimal bandwidth if the point distribution is Gaussian. + // (see "Applied Smoothing Techniques for Data Analysis" by Adrian W, Bowman & Adelchi Azzalini (1997)) + return POW(TYPE(4)/(TYPE(3)*numSamples), TYPE(1)/TYPE(5))*sigma; +} +// estimate density at the given value +template +inline TYPE KernelDensityEstimation(const TMatrix* samples, size_t numSamples, TYPE x, TYPE bandwidth) { + TYPE y(0); + const TYPE invBandwidth(TYPE(1)/bandwidth); + FOREACHRAWPTR(pSample, samples, numSamples) { + const TMatrix& sample(*pSample); + const TYPE val((x - sample(0))*invBandwidth); + y += sample(1) * EXP(-TYPE(0.5)*val*val)*invBandwidth; + } + return (y / numSamples) * INV_SQRT_2PI; +} +// estimate density at the given range of values; +// INSERTER should have defined the operator(TYPE,TYPE) that receives a pair of value and density +template +void KernelDensityEstimation(const TMatrix* samples, size_t numSamples, TYPE bandwidth, TYPE xMin, TYPE xMax, size_t steps, INSERTER& inserter) { + const TYPE step((xMax-xMin)/steps); + for (size_t i=0; i +void KernelDensityEstimation(const TMatrix* samples, size_t numSamples, TYPE bandwidth, TYPE xMin, TYPE xMax, TYPE stepMax, INSERTER& inserter) { + size_t smallSteps(0); + TYPE step(stepMax); + TYPE x(xMin), last_x; + TYPE last_y(KernelDensityEstimation(samples, numSamples, x, bandwidth)); + TYPE last_delta(FLT_EPSILON); + inserter(x, last_y); + x += step; + do { + const TYPE y(KernelDensityEstimation(samples, numSamples, x, bandwidth)); + inserter(x, y); + if (smallSteps) { + if (--smallSteps == 0) { + // continue from the last coarse step + step = stepMax; + inserter.EndPeak(); + x = last_x; + } + } else { + const TYPE delta(y-last_y); + if (ABS(SIGN(last_delta)+SIGN(delta)) < 2 && inserter.SamplePeak(last_delta>0)) { + // last step contains a local peak (minimum/maximum), + // so go back and sample more often; + // assuming the curve is locally symmetric and the step small enough, + // the peak is inside the first or second step behind + last_x = x; + x -= (ABS(delta) < ABS(last_delta) ? step : step*TYPE(2)); + smallSteps = 5; + step /= TYPE(smallSteps+1); + } + last_y = y; + last_delta = delta; + } + } while ((x+=step) < xMax); +} +#if TD_VERBOSE == TD_VERBOSE_DEBUG +// example from http://en.wikipedia.org/wiki/Kernel_density_estimation +inline void ExampleKDE() { + typedef REAL Real; + const Real values[6] ={-2.1, -1.3, -0.4, 1.9, 5.1, 6.2}; + TMatrix samples[6]; + for (int i=0; i<6; ++i) { + samples[i](0) = values[i]; + samples[i](1) = 1; + } + struct SamplesInserter { + File f; + SamplesInserter(const String& fileName) + : f(fileName, File::WRITE, File::CREATE | File::TRUNCATE) {} + inline void operator() (Real x, Real y) { + f.print("%g\t%g\n", x, y); + } + inline bool SamplePeak(bool bMaxRegion) const { + return bMaxRegion; + } + inline void EndPeak() { + } + }; + const size_t steps(30); + const Real bandwidth = BandwidthGaussianKDE(samples, 6)/2; + const Real xMin(-15), xMax(25), stepMax((xMax-xMin)/steps); + SamplesInserter inserter("kde.txt"); + KernelDensityEstimation(samples, 6, bandwidth, xMin, xMax, stepMax, inserter); +} +#endif +/*----------------------------------------------------------------*/ + + +// normalize image points (inhomogeneous 2D) so that their centroid and typical magnitude of the vector is (1,1) +template +void NormalizePoints(const CLISTDEF0(TPoint2)& pointsIn, CLISTDEF0(TPoint2)& pointsOut, TMatrix* pH=NULL) { + // find centroid + TPoint2 ptAvg(0,0); + FOREACHPTR(pPt, pointsIn) + ptAvg.x += TPoint2(*pPt); + ptAvg *= HTYPE(1)/HTYPE(pointsIn.GetSize()); + // move centroid to origin + if (pointsOut.GetSize() != pointsIn.GetSize()) + pointsOut.Resize(pointsIn.GetSize()); + HTYPE var(0); + const TPoint2 ptAvgF(ptAvg); + FOREACH(i, pointsIn) { + const TPoint2& ptIn = pointsIn[i]; + TPoint2& ptOut = pointsOut[i]; + ptOut = ptIn - ptAvgF; + var += norm(ptOut); + } + // calculate variance + var /= HTYPE(pointsIn.GetSize()); + // scale points + HTYPE scale(1); + if (!ISZERO(var)) + scale = HTYPE(SQRT_2) / var; + FOREACHPTR(pPt, pointsOut) + *pPt *= scale; + // initialize normalizing homography matrix from Scale and Translation (H = S * T); + if (pH != NULL) { + TMatrix& H = *pH; + H = TMatrix::IDENTITY; + H(0,2) = -scale * ptAvg.x; + H(1,2) = -scale * ptAvg.y; + H(0,0) = scale; + H(1,1) = scale; + } +} +// normalize scene points (inhomogeneous 3D) so that their centroid and typical magnitude of the vector is (1,1,1) +// This normalization algorithm is suitable only for compact distribution of points (see Hartley04 p180) +template +void NormalizePoints(const CLISTDEF0(TPoint3)& pointsIn, CLISTDEF0(TPoint3)& pointsOut, TMatrix* pH=NULL) { + // find centroid + TPoint3 ptAvg(0, 0, 0); + FOREACHPTR(pPt, pointsIn) + ptAvg += TPoint3(*pPt); + ptAvg *= HTYPE(1)/HTYPE(pointsIn.GetSize()); + // move centroid to origin + if (pointsOut.GetSize() != pointsIn.GetSize()) + pointsOut.Resize(pointsIn.GetSize()); + HTYPE var(0); + FOREACH(i, pointsIn) { + const TPoint3& ptIn = pointsIn[i]; + TPoint3& ptOut = pointsOut[i]; + ptOut = ptIn - ptAvg; + var += norm(ptOut); + } + // calculate variance + var /= HTYPE(pointsIn.GetSize()); + // scale points + HTYPE scale(1); + if (!ISZERO(var)) + scale = HTYPE(SQRT_3) / var; + FOREACHPTR(pPt, pointsOut) + *pPt *= scale; + // initialize normalizing homography matrix + if (pH != NULL) { + TMatrix& H = *pH; + H = TMatrix::IDENTITY; + H(0,3) = -scale * ptAvg.x; + H(1,3) = -scale * ptAvg.y; + H(2,3) = -scale * ptAvg.z; + H(0,0) = scale; + H(1,1) = scale; + H(2,2) = scale; + } +} +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE diff --git a/libs/Common/UtilCUDA.cpp b/libs/Common/UtilCUDA.cpp new file mode 100644 index 0000000..dddc6d5 --- /dev/null +++ b/libs/Common/UtilCUDA.cpp @@ -0,0 +1,436 @@ +//////////////////////////////////////////////////////////////////// +// UtilCUDA.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "UtilCUDA.h" + +#ifdef _USE_CUDA + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +namespace CUDA { + +// S T R U C T S /////////////////////////////////////////////////// + +int desiredDeviceID = -1; +Devices devices; + +// GPU Architecture definitions +int _convertSMVer2Cores(int major, int minor) +{ + if (major == 9999 && minor == 9999) + return 1; + + // Defines for GPU Architecture types (using the SM version to determine the # of cores per SM + struct sSMtoCores { + int SM; // 0xMm (hexadecimal notation), M = SM Major version, and m = SM minor version + int Cores; + }; + const sSMtoCores nGpuArchCoresPerSM[] = { + {0x20, 32}, // Fermi Generation (SM 2.0) GF100 class + {0x21, 48}, // Fermi Generation (SM 2.1) GF10x class + {0x30, 192}, // Kepler Generation (SM 3.0) GK10x class + {0x32, 192}, // Kepler Generation (SM 3.2) GK10x class + {0x35, 192}, // Kepler Generation (SM 3.5) GK11x class + {0x37, 192}, // Kepler Generation (SM 3.7) GK21x class + {0x50, 128}, // Maxwell Generation (SM 5.0) GM10x class + {0x52, 128}, // Maxwell Generation (SM 5.2) GM20x class + {0x53, 128}, // Maxwell Generation (SM 5.3) GM20x class + {0x60, 64 }, // Pascal Generation (SM 6.0) GP100 class + {0x61, 128}, // Pascal Generation (SM 6.1) GP10x class + {0x62, 128}, // Pascal Generation (SM 6.2) GP10x class + {0x70, 64 }, // Volta Generation (SM 7.0) GV100 class + {0x72, 64 }, // Volta Generation (SM 7.2) GV10B class + {0x75, 64 }, // Turing Generation (SM 7.5) TU1xx class + {0x80, 64 }, // Ampere Generation (SM 8.0) GA100 class + {-1, -1} + }; + + int index(0); + while (nGpuArchCoresPerSM[index].SM != -1) { + if (nGpuArchCoresPerSM[index].SM == ((major << 4) + minor)) + return nGpuArchCoresPerSM[index].Cores; + index++; + } + + // If we don't find the values, we default use the previous one to run properly + VERBOSE("MapSMtoCores for SM %d.%d is undefined; default to use %d cores/SM", major, minor, nGpuArchCoresPerSM[index-1].Cores); + return nGpuArchCoresPerSM[index-1].Cores; +} + +// checks that the given device ID is valid; +// if successful returns the device info in the given structure +CUresult _gpuCheckDeviceId(int devID, Device& device) +{ + int device_count; + checkCudaError(cuDeviceGetCount(&device_count)); + if (device_count == 0) { + VERBOSE("CUDA error: no devices supporting CUDA"); + return CUDA_ERROR_NO_DEVICE; + } + if (devID < 0) + devID = 0; + if (devID >= device_count) { + VERBOSE("CUDA error: device [%d] is not a valid GPU device (%d CUDA capable GPU device(s) detected)", devID, device_count); + return CUDA_ERROR_INVALID_DEVICE; + } + checkCudaError(cuDeviceGetAttribute(&device.computeMode, CU_DEVICE_ATTRIBUTE_COMPUTE_MODE, devID)); + if (device.computeMode == CU_COMPUTEMODE_PROHIBITED) { + VERBOSE("CUDA error: device is running in "); + return CUDA_ERROR_PROFILER_DISABLED; + } + checkCudaError(cuDeviceComputeCapability(&device.major, &device.minor, devID)); + if (device.major < 1) { + VERBOSE("CUDA error: GPU device does not support CUDA"); + return CUDA_ERROR_INVALID_DEVICE; + } + checkCudaError(cuDeviceGetProperties(&device.prop, devID)); + device.ID = (CUdevice)devID; + return CUDA_SUCCESS; +} + +// finds the best GPU (with maximum GFLOPS); +// if successful returns the device info in the given structure +CUresult _gpuGetMaxGflopsDeviceId(Device& bestDevice) +{ + int device_count = 0; + checkCudaError(cuDeviceGetCount(&device_count)); + if (device_count == 0) { + VERBOSE("CUDA error: no devices supporting CUDA"); + return CUDA_ERROR_NO_DEVICE; + } + + // Find the best major SM Architecture GPU device + Devices devices; + int best_SM_arch = 0; + for (int current_device = 0; current_device < device_count; ++current_device) { + Device device; + if (reportCudaError(cuDeviceGetAttribute(&device.computeMode, CU_DEVICE_ATTRIBUTE_COMPUTE_MODE, current_device)) != CUDA_SUCCESS) + continue; + // If this GPU is not running on Compute Mode prohibited, then we can add it to the list + if (device.computeMode == CU_COMPUTEMODE_PROHIBITED) + continue; + if (reportCudaError(cuDeviceComputeCapability(&device.major, &device.minor, current_device)) != CUDA_SUCCESS) + continue; + if (device.major > 0 && device.major < 9999) { + best_SM_arch = MAXF(best_SM_arch, device.major); + device.ID = (CUdevice)current_device; + devices.Insert(device); + } + } + if (devices.IsEmpty()) { + VERBOSE("CUDA error: all devices have compute mode prohibited"); + return CUDA_ERROR_PROFILER_DISABLED; + } + + // Find the best CUDA capable GPU device + Device* max_perf_device = NULL; + size_t max_compute_perf = 0; + FOREACHPTR(pDevice, devices) { + ASSERT(pDevice->computeMode != CU_COMPUTEMODE_PROHIBITED); + int sm_per_multiproc = _convertSMVer2Cores(pDevice->major, pDevice->minor); + int multiProcessorCount; + if (reportCudaError(cuDeviceGetAttribute(&multiProcessorCount, CU_DEVICE_ATTRIBUTE_MULTIPROCESSOR_COUNT, (CUdevice)pDevice->ID)) != CUDA_SUCCESS) + continue; + int clockRate; + if (reportCudaError(cuDeviceGetAttribute(&clockRate, CU_DEVICE_ATTRIBUTE_CLOCK_RATE, pDevice->ID)) != CUDA_SUCCESS) + continue; + size_t compute_perf = (size_t)multiProcessorCount * sm_per_multiproc * clockRate; + if (compute_perf > max_compute_perf && + (best_SM_arch < 3 || // if we find GPU with SM major > 2, search only these + pDevice->major == best_SM_arch) ) // if our device==dest_SM_arch, choose this, or else pass + { + max_compute_perf = compute_perf; + max_perf_device = pDevice; + } + } + if (max_perf_device == NULL) + return CUDA_ERROR_INVALID_DEVICE; + + bestDevice = *max_perf_device; + checkCudaError(cuDeviceGetProperties(&bestDevice.prop, bestDevice.ID)); + return CUDA_SUCCESS; +} + +// initialize the given CUDA device and add it to the array of initialized devices; +// if the given device is -1, the best available device is selected +CUresult initDevice(int deviceID) +{ + if (deviceID < -1) + return CUDA_ERROR_INVALID_DEVICE; + + checkCudaError(cuInit(0)); + + Device device; + if (deviceID >= 0) { + checkCudaError(_gpuCheckDeviceId(deviceID, device)); + } else { + // Otherwise pick the device with the highest Gflops/s + checkCudaError(_gpuGetMaxGflopsDeviceId(device)); + } + if (device.major < 3) { + VERBOSE("CUDA error: compute capability 3.2 or greater required (available %d.%d for device[%d])", device.ID, device.major, device.minor); + return CUDA_ERROR_INVALID_DEVICE; + } + devices.Insert(device); + checkCudaError(cuCtxCreate(&devices.Last().ctx, CU_CTX_SCHED_AUTO, device.ID)); + + #if TD_VERBOSE != TD_VERBOSE_OFF + char name[2048]; + checkCudaError(cuDeviceGetName(name, 2048, device.ID)); + size_t memSize; + checkCudaError(cuDeviceTotalMem(&memSize, device.ID)); + DEBUG("CUDA device %d initialized: %s (compute capability %d.%d; memory %s)", device.ID, name, device.major, device.minor, Util::formatBytes(memSize).c_str()); + #endif + return CUDA_SUCCESS; +} + +// load/read module (program) from file/string and compile it +CUresult ptxJIT(LPCSTR program, CUmodule& hModule, int mode) +{ + CUlinkState lState; + CUjit_option options[6]; + void *optionVals[6]; + float walltime(0); + const unsigned logSize(8192); + char error_log[logSize], info_log[logSize]; + void *cuOut; + size_t outSize; + + // Setup linker options + // Return walltime from JIT compilation + options[0] = CU_JIT_WALL_TIME; + optionVals[0] = (void*)&walltime; + // Pass a buffer for info messages + options[1] = CU_JIT_INFO_LOG_BUFFER; + optionVals[1] = (void*)info_log; + // Pass the size of the info buffer + options[2] = CU_JIT_INFO_LOG_BUFFER_SIZE_BYTES; + optionVals[2] = (void*)(long)logSize; + // Pass a buffer for error message + options[3] = CU_JIT_ERROR_LOG_BUFFER; + optionVals[3] = (void*)error_log; + // Pass the size of the error buffer + options[4] = CU_JIT_ERROR_LOG_BUFFER_SIZE_BYTES; + optionVals[4] = (void*)(long)logSize; + // Make the linker verbose + options[5] = CU_JIT_LOG_VERBOSE; + optionVals[5] = (void*)1; + + // Create a pending linker invocation + checkCudaError(cuLinkCreate(6, options, optionVals, &lState)); + + const size_t programLen(strlen(program)); + CUresult myErr; + if (mode == JIT::FILE || (mode == JIT::AUTO && programLen < 256)) { + // Load the PTX from the file + myErr = cuLinkAddFile(lState, CU_JIT_INPUT_PTX, program, 0, 0, 0); + } else { + // Load the PTX from the string + myErr = cuLinkAddData(lState, CU_JIT_INPUT_PTX, (void*)program, programLen+1, 0, 0, 0, 0); + } + if (myErr != CUDA_SUCCESS) { + // Errors will be put in error_log, per CU_JIT_ERROR_LOG_BUFFER option above + VERBOSE("PTX Linker Error: %s", error_log); + return myErr; + } + + // Complete the linker step + checkCudaError(cuLinkComplete(lState, &cuOut, &outSize)); + + // Linker walltime and info_log were requested in options above + DEBUG_LEVEL(3, "CUDA link completed (%gms):\n%s", walltime, info_log); + + // Load resulting cuBin into module + checkCudaError(cuModuleLoadData(&hModule, cuOut)); + + // Destroy the linker invocation + return reportCudaError(cuLinkDestroy(lState)); +} + +// requested function (kernel) from module (program) +CUresult ptxGetFunc(const CUmodule& hModule, LPCSTR functionName, CUfunction& hKernel) +{ + // Locate the kernel entry point + checkCudaError(cuModuleGetFunction(&hKernel, hModule, functionName)); + DEBUG_LEVEL(3, "Kernel '%s' loaded", functionName); + return CUDA_SUCCESS; +} +/*----------------------------------------------------------------*/ + + +void MemDevice::Release() { + if (pData) { + reportCudaError(cuMemFree(pData)); + pData = 0; + } +} +CUresult MemDevice::Reset(size_t size) { + if (pData) { + if (nSize == size) + return CUDA_SUCCESS; + Release(); + } + if (cuMemAlloc(&pData, size) != CUDA_SUCCESS) { + pData = 0; + return CUDA_ERROR_OUT_OF_MEMORY; + } + nSize = size; + return CUDA_SUCCESS; +} +CUresult MemDevice::Reset(const void* pDataHost, size_t size) { + if (pData && nSize != size) + Release(); + if (!pData && cuMemAlloc(&pData, size) != CUDA_SUCCESS) { + pData = 0; + return CUDA_ERROR_OUT_OF_MEMORY; + } + nSize = size; + return cuMemcpyHtoD(pData, pDataHost, size); +} + +CUresult MemDevice::SetData(const void* pDataHost, size_t size) { + ASSERT(IsValid()); + return cuMemcpyHtoD(pData, pDataHost, size); +} + +CUresult MemDevice::GetData(void* pDataHost, size_t size) const { + ASSERT(IsValid()); + return cuMemcpyDtoH(pDataHost, pData, size); +} +/*----------------------------------------------------------------*/ + + +void EventRT::Release() { + if (hEvent) { + reportCudaError(cuEventDestroy(hEvent)); + hEvent = NULL; + } +} +CUresult EventRT::Reset(unsigned flags) { + CUresult ret(cuEventCreate(&hEvent, flags)); + if (ret != CUDA_SUCCESS) + hEvent = NULL; + return ret; +} +/*----------------------------------------------------------------*/ + + +void StreamRT::Release() { + if (hStream) { + reportCudaError(cuStreamDestroy(hStream)); + hStream = NULL; + } +} +CUresult StreamRT::Reset(unsigned flags) { + CUresult ret(cuStreamCreate(&hStream, flags)); + if (ret != CUDA_SUCCESS) + hStream = NULL; + return ret; +} + +CUresult StreamRT::Wait(CUevent hEvent) { + ASSERT(IsValid()); + return cuStreamWaitEvent(hStream, hEvent, 0); +} +/*----------------------------------------------------------------*/ + + +void ModuleRT::Release() { + if (hModule) { + reportCudaError(cuModuleUnload(hModule)); + hModule = NULL; + } +} +CUresult ModuleRT::Reset(LPCSTR program, int mode) { + // compile the module (program) from PTX and get its handle (Driver API) + CUresult result(ptxJIT(program, hModule, mode)); + if (result != CUDA_SUCCESS) + hModule = NULL; + return result; +} +/*----------------------------------------------------------------*/ + + +void KernelRT::Release() { + inDatas.Release(); + outDatas.Release(); + ptrModule.Release(); + hKernel = NULL; +} +void KernelRT::Reset() { + paramOffset = 0; + inDatas.Empty(); + outDatas.Empty(); +} +CUresult KernelRT::Reset(LPCSTR functionName) { + // get the function handle (Driver API) + ASSERT(ptrModule != NULL && ptrModule->IsValid()); + CUresult result(ptxGetFunc(*ptrModule, functionName, hKernel)); + if (result != CUDA_SUCCESS) { + ptrModule.Release(); + hKernel = NULL; + } + return result; +} +CUresult KernelRT::Reset(const ModuleRTPtr& _ptrModule, LPCSTR functionName) { + // set module + ptrModule = _ptrModule; + // set function + return Reset(functionName); +} +CUresult KernelRT::Reset(LPCSTR program, LPCSTR functionName, int mode) { + // compile the module (program) from PTX and get the function handle (Driver API) + ptrModule = new ModuleRT(program, mode); + if (!ptrModule->IsValid()) { + ptrModule.Release(); + hKernel = NULL; + return CUDA_ERROR_INVALID_HANDLE; + } + return Reset(functionName); +} + + +// append a generic input parameter (allocate© input buffer) +CUresult KernelRT::_AddParam(const InputParam& param) { + MemDevice& data = inDatas.AddEmpty(); + if (data.Reset(param.data, param.size) != CUDA_SUCCESS) + return CUDA_ERROR_OUT_OF_MEMORY; + return addKernelParam(hKernel, paramOffset, (CUdeviceptr)data); +} +// append a generic output parameter (allocate output buffer) +CUresult KernelRT::_AddParam(const OutputParam& param) { + MemDevice& data = outDatas.AddEmpty(); + if (data.Reset(param.size) != CUDA_SUCCESS) + return CUDA_ERROR_OUT_OF_MEMORY; + return addKernelParam(hKernel, paramOffset, (CUdeviceptr)data); +} + + +// copy result from the given output parameter index back to the host +CUresult KernelRT::GetResult(const CUdeviceptr data, const ReturnParam& param) const { + return fetchMemDevice(param.data, param.size, data); +} +// read from the device the variadic output parameters +CUresult KernelRT::GetResult(const std::initializer_list& params) const { + MemDeviceArr::IDX idx(0); + for (auto param : params) + if (outDatas[idx++].GetData(param.data, param.size) != CUDA_SUCCESS) + return CUDA_ERROR_INVALID_VALUE; + return CUDA_SUCCESS; +} +/*----------------------------------------------------------------*/ + +} // namespace CUDA + +} // namespace SEACAVE + +#endif // _USE_CUDA diff --git a/libs/Common/UtilCUDA.h b/libs/Common/UtilCUDA.h new file mode 100644 index 0000000..44d869d --- /dev/null +++ b/libs/Common/UtilCUDA.h @@ -0,0 +1,719 @@ +//////////////////////////////////////////////////////////////////// +// UtilCUDA.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_CUDA_H__ +#define __SEACAVE_CUDA_H__ + +#ifdef _USE_CUDA + + +// I N C L U D E S ///////////////////////////////////////////////// + +// CUDA driver +#include + +// CUDA toolkit +#include +#include +#include +#include +#include + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +namespace SEACAVE { + +namespace CUDA { + +extern int desiredDeviceID; + +// global list of initialized devices +struct Device { + CUdevice ID; + int major, minor; + int computeMode; + CUdevprop prop; + CUcontext ctx; + + inline Device() : ctx(NULL) {} + inline ~Device() { if (ctx != NULL) cuCtxDestroy(ctx); } +}; +typedef CLISTDEF0(Device) Devices; +extern Devices devices; + +// outputs the proper CUDA error code in the event that a CUDA host call returns an error +inline CUresult __reportCudaError(CUresult result, LPCSTR errorMessage) { + if (result == CUDA_SUCCESS) + return CUDA_SUCCESS; + LPCSTR szName; + cuGetErrorName(result, &szName); + LPCSTR szError; + cuGetErrorString(result, &szError); + #ifdef _DEBUG + VERBOSE("CUDA error at %s:%d: %s (%s (code %d) - %s)", __FILE__, __LINE__, errorMessage, szName, static_cast(result), szError); + #else + DEBUG("CUDA error: %s (%s (code %d) - %s)", errorMessage, szName, static_cast(result), szError); + #endif + ASSERT("CudaError" == NULL); + return result; +} +#define reportCudaError(val) CUDA::__reportCudaError(val, #val) + +#define checkCudaError(val) { const CUresult ret(CUDA::__reportCudaError(val, #val)); if (ret != CUDA_SUCCESS) return ret; } + +// outputs the proper CUDA error code and abort in the event that a CUDA host call returns an error +inline void __ensureCudaResult(CUresult result, LPCSTR errorMessage) { + if (__reportCudaError(result, errorMessage)) + return; + ASSERT("CudaAbort" == NULL); + exit(EXIT_FAILURE); +} +#define ensureCudaResult(val) CUDA::__ensureCudaResult(val, #val) + +inline void checkCudaCall(const cudaError_t error) { + if (error == cudaSuccess) + return; + #ifdef _DEBUG + VERBOSE("CUDA error at %s:%d: %s (code %d)", __FILE__, __LINE__, cudaGetErrorString(error), error); + #else + DEBUG("CUDA error: %s (code %d)", cudaGetErrorString(error), error); + #endif + ASSERT("CudaError" == NULL); + exit(EXIT_FAILURE); +} + +// rounds up addr to the align boundary +template +inline T align(T o, T a) { + a -= T(1); + return (o + a)&~a; +} + +// initialize the given CUDA device and add it to the array of initialized devices; +// if the given device is -1, the best available device is selected +CUresult initDevice(int deviceID=-1); + +// load/read module (program) from file/string and compile it +enum JIT { AUTO=0, STRING=1, FILE=2 }; +CUresult ptxJIT(LPCSTR program, CUmodule& hModule, int mode=JIT::AUTO); + +// requested function (kernel) from module (program) +CUresult ptxGetFunc(const CUmodule& hModule, LPCSTR functionName, CUfunction& hKernel); + +// add a new parameter to the given kernel +template +inline CUresult addKernelParam(CUfunction& hKernel, int& paramOffset, const T& param) { + paramOffset = align(paramOffset, (int)alignof(T)); + const CUresult result(cuParamSetv(hKernel, paramOffset, (void*)¶m, sizeof(T))); + paramOffset += sizeof(T); + return result; +} + +// allocate on the CUDA device a chunk of memory of the given size +inline CUresult allocMemDevice(size_t size, CUdeviceptr& dataDevice) { + return cuMemAlloc(&dataDevice, size); +} +// copy on the CUDA device the given chunk of memory +inline CUresult copyMemDevice(const void* data, size_t size, CUdeviceptr dataDevice) { + return cuMemcpyHtoD(dataDevice, data, size); +} +// allocate and copy on the CUDA device the given chunk of memory +inline CUresult createReplicaDevice(const void* data, size_t size, CUdeviceptr& dataDevice) { + if (cuMemAlloc(&dataDevice, size) != CUDA_SUCCESS) + return CUDA_ERROR_OUT_OF_MEMORY; + return cuMemcpyHtoD(dataDevice, data, size); +} +// copy from the CUDA device the given chunk of memory +inline CUresult fetchMemDevice(void* data, size_t size, const CUdeviceptr dataDevice) { + return cuMemcpyDtoH(data, dataDevice, size); +} +// free the given memory on the CUDA device +inline CUresult freeMemDevice(CUdeviceptr& dataDevice) { + if (cuMemFree(dataDevice) != CUDA_SUCCESS) + return CUDA_ERROR_NOT_INITIALIZED; + dataDevice = 0; + return CUDA_SUCCESS; +} +/*----------------------------------------------------------------*/ + + +class MemDevice +{ +protected: + CUdeviceptr pData; + size_t nSize; + +public: + inline MemDevice() : pData(0) {} + inline MemDevice(size_t size) : pData(0) { reportCudaError(Reset(size)); } + inline MemDevice(const void* pDataHost, size_t size) : pData(0) { reportCudaError(Reset(pDataHost, size)); } + template + inline MemDevice(const TImage& param) : pData(0) { reportCudaError(Reset(param)); } + template + inline MemDevice(const cList& param) : pData(0) { reportCudaError(Reset(param)); } + inline ~MemDevice() { Release(); } + + MemDevice(MemDevice& rhs) : pData(rhs.pData) { rhs.pData = 0; } + MemDevice& operator=(MemDevice& rhs) { pData = rhs.pData; rhs.pData = 0; return *this; } + + inline bool IsValid() const { + return (pData != 0); + } + void Release(); + CUresult Reset(size_t size); + CUresult Reset(const void* pDataHost, size_t size); + template + inline CUresult Reset(const TImage& param) { + ASSERT(!param.empty() && param.isContinuous()); + return Reset(param.getData(), sizeof(TYPE)*param.area()); + } + template + inline CUresult Reset(const cList& param) { + ASSERT(!param.IsEmpty()); + return Reset(param.GetData(), param.GetDataSize()); + } + + CUresult SetData(const void* pDataHost, size_t size); + template + inline CUresult SetData(const TImage& param) { + ASSERT(!param.empty() && param.isContinuous()); + return SetData(param.getData(), sizeof(TYPE)*param.area()); + } + template + inline CUresult SetData(const cList& param) { + ASSERT(!param.IsEmpty()); + return SetData(param.GetData(), param.GetDataSize()); + } + + CUresult GetData(void* pDataHost, size_t size) const; + template + inline CUresult GetData(TImage& param) const { + ASSERT(!param.empty() && param.isContinuous()); + return GetData(param.getData(), sizeof(TYPE)*param.area()); + } + template + inline CUresult GetData(cList& param) const { + ASSERT(!param.IsEmpty()); + return GetData(param.GetData(), param.GetDataSize()); + } + + inline operator CUdeviceptr() const { + return pData; + } +}; +typedef CSharedPtr MemDevicePtr; +typedef CLISTDEFIDX(MemDevice,int) MemDeviceArr; +/*----------------------------------------------------------------*/ + + +class EventRT +{ +protected: + CUevent hEvent; + +protected: + EventRT(const EventRT&); + EventRT& operator=(const EventRT&); + +public: + inline EventRT(unsigned flags = CU_EVENT_DEFAULT) { reportCudaError(Reset(flags)); } + inline ~EventRT() { Release(); } + + inline bool IsValid() const { + return (hEvent != NULL); + } + void Release(); + CUresult Reset(unsigned flags = CU_EVENT_DEFAULT); + + inline operator CUevent() const { + return hEvent; + } +}; +typedef CSharedPtr EventRTPtr; +/*----------------------------------------------------------------*/ + + +class StreamRT +{ +protected: + CUstream hStream; + +protected: + StreamRT(const StreamRT&); + StreamRT& operator=(const StreamRT&); + +public: + inline StreamRT(unsigned flags = CU_STREAM_DEFAULT) { reportCudaError(Reset(flags)); } + inline ~StreamRT() { Release(); } + + inline bool IsValid() const { + return (hStream != NULL); + } + void Release(); + CUresult Reset(unsigned flags = CU_STREAM_DEFAULT); + + inline operator CUstream() const { + return hStream; + } + + CUresult Wait(CUevent hEvent); +}; +typedef CSharedPtr StreamRTPtr; +/*----------------------------------------------------------------*/ + + +class ModuleRT +{ +protected: + CUmodule hModule; + +protected: + ModuleRT(const ModuleRT&); + ModuleRT& operator=(const ModuleRT&); + +public: + inline ModuleRT() : hModule(NULL) {} + inline ModuleRT(LPCSTR program, int mode=JIT::AUTO) { Reset(program, mode); } + inline ~ModuleRT() { Release(); } + + inline bool IsValid() const { + return (hModule != NULL); + } + void Release(); + CUresult Reset(LPCSTR program, int mode=JIT::AUTO); + + inline operator CUmodule() const { + return hModule; + } +}; +typedef CSharedPtr ModuleRTPtr; +/*----------------------------------------------------------------*/ + + +class KernelRT +{ +public: + ModuleRTPtr ptrModule; + StreamRTPtr ptrStream; + CUfunction hKernel; + MemDeviceArr inDatas; // array of pointers to the allocated memory read by the program + MemDeviceArr outDatas; // array of pointers to the allocated memory written by the program + int paramOffset; // used during parameter insertion to remember current parameter position + +protected: + KernelRT(const KernelRT&); + KernelRT& operator=(const KernelRT&); + +public: + inline KernelRT() : hKernel(NULL) {} + inline KernelRT(const ModuleRTPtr& _ptrModule, LPCSTR functionName) : ptrModule(_ptrModule) { Reset(functionName); } + inline KernelRT(LPCSTR program, LPCSTR functionName, int mode=JIT::AUTO) { Reset(program, functionName, mode); } + + inline bool IsValid() const { + ASSERT(hKernel == NULL || (ptrModule != NULL && ptrModule->IsValid())); + return (hKernel != NULL); + } + void Release(); + void Reset(); + CUresult Reset(LPCSTR functionName); + CUresult Reset(const ModuleRTPtr& _ptrModule, LPCSTR functionName); + CUresult Reset(LPCSTR program, LPCSTR functionName, int mode=JIT::AUTO); + + struct InputParam { + const void* data; // pointer to host data to be allocated and copied to the CUDA device + size_t size; // size in bytes of the data + inline InputParam() {} + inline InputParam(const void* _data, size_t _size) : data(_data), size(_size) {} + }; + struct OutputParam { + size_t size; // size in bytes of the data to be allocated on the CUDA device + inline OutputParam() {} + inline OutputParam(size_t _size) : size(_size) {} + }; + // lunch the program with the given parameters; + // numThreads - total number of threads to run + // args - variadic parameters to be passed to the kernel + #ifdef _SUPPORT_CPP11 + template + CUresult operator()(int numThreads, Args&&... args) { + ASSERT(IsValid()); + Reset(); + CUresult result; + // set the kernel parameters (Driver API) + if ((result=AddParam(std::forward(args)...)) != CUDA_SUCCESS) + return result; + if ((result=cuParamSetSize(hKernel, paramOffset)) != CUDA_SUCCESS) + return result; + // launch the kernel (Driver API) + const CUdevprop& deviceProp = CUDA::devices.back().prop; + const int numBlockThreads(MINF(numThreads, deviceProp.maxThreadsPerBlock)); + const int nBlocks(MAXF((numThreads+numBlockThreads-1)/numBlockThreads, 1)); + if ((result=cuFuncSetBlockShape(hKernel, numBlockThreads, 1, 1)) != CUDA_SUCCESS) + return result; + if (ptrStream != NULL) + return cuLaunchGridAsync(hKernel, nBlocks, 1, *ptrStream); + return cuLaunchGrid(hKernel, nBlocks, 1); + } + // same for 2D data + template + CUresult operator()(const TPoint2& numThreads, Args&&... args) { + ASSERT(IsValid()); + Reset(); + CUresult result; + // set the kernel parameters (Driver API) + if ((result=AddParam(std::forward(args)...)) != CUDA_SUCCESS) + return result; + if ((result=cuParamSetSize(hKernel, paramOffset)) != CUDA_SUCCESS) + return result; + // launch the kernel (Driver API) + const CUdevprop& deviceProp = CUDA::devices.back().prop; + const REAL scale(MINF(REAL(1), SQRT((REAL)deviceProp.maxThreadsPerBlock/(REAL)(numThreads.x*numThreads.y)))); + const SEACAVE::TPoint2 numBlockThreads(FLOOR2INT(SEACAVE::TPoint2(numThreads)*scale)); + const TPoint2 nBlocks( + MAXF((numThreads.x+numBlockThreads.x-1)/numBlockThreads.x, 1), + MAXF((numThreads.y+numBlockThreads.y-1)/numBlockThreads.y, 1)); + if ((result=cuFuncSetBlockShape(hKernel, numBlockThreads.x, numBlockThreads.y, 1)) != CUDA_SUCCESS) + return result; + if (ptrStream != NULL) + return cuLaunchGridAsync(hKernel, nBlocks.x, nBlocks.y, *ptrStream); + return cuLaunchGrid(hKernel, nBlocks.x, nBlocks.y); + } + #endif // _SUPPORT_CPP11 + + struct ReturnParam { + void* data; // pointer to host data to be written with the output data from the CUDA device + size_t size; // size in bytes of the data + inline ReturnParam() {} + inline ReturnParam(void* _data, size_t _size) : data(_data), size(_size) {} + }; + CUresult GetResult(const CUdeviceptr data, const ReturnParam& param) const; + inline CUresult GetResult(const MemDevice& memDev, const ReturnParam& param) const { + return memDev.GetData(param.data, param.size); + } + inline CUresult GetResult(int idx, const ReturnParam& param) const { + return GetResult(outDatas[idx], param); + } + template + inline CUresult GetResult(int idx, const TImage& param) const { + ASSERT(!param.empty() && param.isContinuous()); + return GetResult(idx, ReturnParam(param.getData(), sizeof(TYPE)*param.area())); + } + template + inline CUresult GetResult(int idx, const cList& param) const { + ASSERT(!param.IsEmpty()); + return GetResult(idx, ReturnParam(param.GetData(), param.GetDataSize())); + } + CUresult GetResult(const std::initializer_list& params) const; + +protected: + CUresult _AddParam(const InputParam& param); + CUresult _AddParam(const OutputParam& param); + template + inline CUresult _AddParam(const T& param) { + return addKernelParam(hKernel, paramOffset, param); + } + inline CUresult _AddParam(const MemDevice& param) { + ASSERT(param.IsValid()); + return addKernelParam(hKernel, paramOffset, (CUdeviceptr)param); + } + template + inline CUresult _AddParam(const TImage& param) { + ASSERT(!param.empty() && param.isContinuous()); + return _AddParam(InputParam(param.getData(), sizeof(TYPE)*param.area())); + } + template + inline CUresult _AddParam(const cList& param) { + ASSERT(!param.IsEmpty()); + return _AddParam(InputParam(param.GetData(), param.GetDataSize())); + } + #ifdef _SUPPORT_CPP11 + template + inline CUresult AddParam(T&& param) { + return _AddParam(std::forward(param)); + } + template + inline CUresult AddParam(T&& param, Args&&... args) { + CUresult result(AddParam(std::forward(param))); + if (result != CUDA_SUCCESS) + return result; + if ((result=AddParam(std::forward(args)...)) != CUDA_SUCCESS) + return result; + return CUDA_SUCCESS; + } + #endif // _SUPPORT_CPP11 +}; +typedef CSharedPtr KernelRTPtr; +/*----------------------------------------------------------------*/ + + +namespace ARRAY { +template struct traits { static const CUarray_format format; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_UNSIGNED_INT8; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_UNSIGNED_INT16; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_UNSIGNED_INT32; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_SIGNED_INT8; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_SIGNED_INT16; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_SIGNED_INT32; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_HALF; }; +template<> struct traits { static const CUarray_format format = CU_AD_FORMAT_FLOAT; }; +} // namespace ARRAY + +template +class TArrayRT +{ +public: + typedef TYPE Type; + typedef TImage ImageType; + +protected: + CUarray hArray; + +public: + inline TArrayRT() : hArray(NULL) {} + inline TArrayRT(const Image8U::Size& size, unsigned flags=0) : hArray(NULL) { reportCudaError(Reset(size, flags)); } + inline TArrayRT(unsigned width, unsigned height, unsigned depth=0, unsigned flags=0) : hArray(NULL) { reportCudaError(Reset(width, height, depth, flags)); } + inline ~TArrayRT() { Release(); } + + TArrayRT(TArrayRT& rhs) : hArray(rhs.hArray) { rhs.hArray = NULL; } + TArrayRT& operator=(TArrayRT& rhs) { + hArray = rhs.hArray; + rhs.hArray = NULL; + return *this; + } + + inline bool IsValid() const { + return (hArray != NULL); + } + void Release() { + if (hArray) { + reportCudaError(cuArrayDestroy(hArray)); + hArray = NULL; + } + } + inline CUresult Reset(const Image8U::Size& size, unsigned flags=0) { + return Reset((unsigned)size.width, (unsigned)size.height, 0, flags); + } + CUresult Reset(unsigned width, unsigned height, unsigned depth=0, unsigned flags=0) { + Release(); + CUDA_ARRAY3D_DESCRIPTOR prop; + prop.Width = width; + prop.Height = height; + prop.Depth = depth; + prop.Format = ARRAY::traits::format; + prop.NumChannels = cv::DataType::channels; + prop.Flags = flags; + CUresult ret(cuArray3DCreate(&hArray, &prop)); + if (ret != CUDA_SUCCESS) + hArray = NULL; + return ret; + } + + operator CUarray() const { + return hArray; + } + operator CUarray&() { + return hArray; + } + CUDA_ARRAY3D_DESCRIPTOR GetDescriptor() const { + CUDA_ARRAY3D_DESCRIPTOR prop; + cuArray3DGetDescriptor(&prop, hArray); + return prop; + } + unsigned Width() const { + return (unsigned)GetDescriptor().Width; + } + unsigned Height() const { + return (unsigned)GetDescriptor().Height; + } + unsigned Depth() const { + return (unsigned)GetDescriptor().Depth; + } + unsigned NumChannels() const { + return GetDescriptor().NumChannels; + } + CUarray_format Format() const { + return GetDescriptor().Format; + } + unsigned Flags() const { + return GetDescriptor().Flags; + } + size_t Size() const { + return sizeof(Type)*Width()*Height()*(Depth()>0?Depth():1)*NumChannels(); + } + + // copy some data from host memory to device memory + CUresult SetData(const ImageType& image) { + ASSERT(IsValid() && !image.empty()); + CUDA_MEMCPY2D param; + memset(¶m, 0, sizeof(CUDA_MEMCPY2D)); + param.dstMemoryType = CU_MEMORYTYPE_ARRAY; + param.dstArray = hArray; + param.srcMemoryType = CU_MEMORYTYPE_HOST; + param.srcHost = image.getData(); + param.srcPitch = image.row_stride(); + param.WidthInBytes = image.row_stride(); + param.Height = image.height(); + return cuMemcpy2D(¶m); + } + + // copy data from device memory to host memory + CUresult GetData(ImageType& image) const { + ASSERT(IsValid() && !image.empty()); + CUDA_MEMCPY2D param; + memset(¶m, 0, sizeof(CUDA_MEMCPY2D)); + param.dstMemoryType = CU_MEMORYTYPE_HOST; + param.dstHost = image.getData(); + param.dstPitch = image.row_stride(); + param.srcMemoryType = CU_MEMORYTYPE_ARRAY; + param.srcArray = hArray; + param.WidthInBytes = image.row_stride(); + param.Height = image.height(); + return cuMemcpy2D(¶m); + } +}; +typedef TArrayRT ArrayRT8U; +typedef TArrayRT ArrayRT32U; +typedef TArrayRT ArrayRT16F; +typedef TArrayRT ArrayRT32F; +/*----------------------------------------------------------------*/ + + +template +class TTextureRT +{ +public: + typedef TArrayRT ArrayType; + typedef typename ArrayType::Type Type; + typedef typename ArrayType::ImageType ImageType; + +public: + ModuleRTPtr ptrModule; + CUtexref hTexref; + +public: + inline TTextureRT() : hTexref(NULL) {} + inline TTextureRT(const ModuleRTPtr& _ptrModule, LPCSTR texrefName, CUfilter_mode filtermode=CU_TR_FILTER_MODE_POINT, CUaddress_mode addrmode=CU_TR_ADDRESS_MODE_CLAMP, bool bNormalizedCoords=false) : ptrModule(_ptrModule) { Reset(texrefName, filtermode, addrmode, bNormalizedCoords); } + inline ~TTextureRT() { Release(); } + + inline bool IsValid() const { + ASSERT(hTexref == NULL || (ptrModule != NULL && ptrModule->IsValid())); + return (hTexref != NULL); + } + void Release() { + ptrModule.Release(); + hTexref = NULL; + } + CUresult Reset(LPCSTR texrefName, CUfilter_mode filtermode=CU_TR_FILTER_MODE_POINT, CUaddress_mode addrmode=CU_TR_ADDRESS_MODE_CLAMP, bool bNormalizedCoords=false) { + // get the texture-reference handle (Driver API) + ASSERT(ptrModule != NULL && ptrModule->IsValid()); + CUresult result(cuModuleGetTexRef(&hTexref, *ptrModule, texrefName)); + if (result != CUDA_SUCCESS) + Release(); + // set texture parameters + checkCudaError(cuTexRefSetFilterMode(hTexref, filtermode)); + if (bNormalizedCoords) { + checkCudaError(cuTexRefSetFlags(hTexref, CU_TRSF_NORMALIZED_COORDINATES)); + for (int i=0; i<2; ++i) + checkCudaError(cuTexRefSetAddressMode(hTexref, i, addrmode)); + } else { + for (int i=0; i<2; ++i) + checkCudaError(cuTexRefSetAddressMode(hTexref, i, CU_TR_ADDRESS_MODE_CLAMP)); + } + cuTexRefSetFormat(hTexref, ARRAY::traits::format, cv::DataType::channels); + return result; + } + inline CUresult Reset(const ModuleRTPtr& _ptrModule, LPCSTR texrefName, CUfilter_mode filtermode=CU_TR_FILTER_MODE_POINT, CUaddress_mode addrmode=CU_TR_ADDRESS_MODE_CLAMP, bool bNormalizedCoords=false) { + // set module + ptrModule = _ptrModule; + // set texture + return Reset(texrefName, filtermode, addrmode, bNormalizedCoords); + } + + // bind the given array to the texture + CUresult Bind(ArrayType& array) { + return cuTexRefSetArray(hTexref, array, CU_TRSA_OVERRIDE_FORMAT); + } + + // fetch the array bind to the texture + CUresult Fetch(ArrayType& array) { + return cuTexRefGetArray(hTexref, array); + } +}; +typedef TTextureRT TextureRT8U; +typedef TTextureRT TextureRT32U; +typedef TTextureRT TextureRT16F; +typedef TTextureRT TextureRT32F; +/*----------------------------------------------------------------*/ + + +template +class TSurfaceRT +{ +public: + typedef TArrayRT ArrayType; + typedef typename ArrayType::Type Type; + typedef typename ArrayType::ImageType ImageType; + +public: + ModuleRTPtr ptrModule; + CUsurfref hSurfref; + +public: + inline TSurfaceRT() : hSurfref(NULL) {} + inline TSurfaceRT(const ModuleRTPtr& _ptrModule, LPCSTR surfrefName) : ptrModule(_ptrModule) { Reset(surfrefName); } + inline ~TSurfaceRT() { Release(); } + + inline bool IsValid() const { + ASSERT(hSurfref == NULL || (ptrModule != NULL && ptrModule->IsValid())); + return (hSurfref != NULL); + } + void Release() { + ptrModule.Release(); + hSurfref = NULL; + } + CUresult Reset(LPCSTR surfrefName) { + // get the surface-reference handle (Driver API) + ASSERT(ptrModule != NULL && ptrModule->IsValid()); + const CUresult result(cuModuleGetSurfRef(&hSurfref, *ptrModule, surfrefName)); + if (result != CUDA_SUCCESS) + Release(); + return result; + } + inline CUresult Reset(const ModuleRTPtr& _ptrModule, LPCSTR texrefName) { + // set module + ptrModule = _ptrModule; + // set texture + return Reset(texrefName); + } + + // bind the given array to the surface + CUresult Bind(const ArrayType& array) { + return cuSurfRefSetArray(hSurfref, array, 0); + } + + // fetch the array bind to the surface + CUresult Fetch(const ArrayType& array) const { + return cuSurfRefGetArray(hSurfref, array); + } +}; +typedef TSurfaceRT SurfaceRT8U; +typedef TSurfaceRT SurfaceRT32U; +typedef TSurfaceRT SurfaceRT16F; +typedef TSurfaceRT SurfaceRT32F; +/*----------------------------------------------------------------*/ + +} // namespace CUDA + +} // namespace SEACAVE + +#endif // _USE_CUDA + +#endif // __SEACAVE_CUDA_H__ diff --git a/libs/IO/CMakeLists.txt b/libs/IO/CMakeLists.txt new file mode 100644 index 0000000..0f05951 --- /dev/null +++ b/libs/IO/CMakeLists.txt @@ -0,0 +1,51 @@ +# Find required packages +FIND_PACKAGE(PNG QUIET) +if(PNG_FOUND) + INCLUDE_DIRECTORIES(${PNG_INCLUDE_DIRS}) + ADD_DEFINITIONS(${PNG_DEFINITIONS}) + SET(_USE_PNG TRUE CACHE INTERNAL "") +else() + SET(PNG_LIBRARIES "") +endif() +FIND_PACKAGE(JPEG QUIET) +if(JPEG_FOUND) + INCLUDE_DIRECTORIES(${JPEG_INCLUDE_DIR}) + ADD_DEFINITIONS(${JPEG_DEFINITIONS}) + SET(_USE_JPG TRUE CACHE INTERNAL "") +else() + SET(JPEG_LIBRARIES "") +endif() +FIND_PACKAGE(TIFF QUIET) +if(TIFF_FOUND) + INCLUDE_DIRECTORIES(${TIFF_INCLUDE_DIR}) + ADD_DEFINITIONS(${TIFF_DEFINITIONS}) + SET(_USE_TIFF TRUE CACHE INTERNAL "") +else() + SET(TIFF_LIBRARIES "") +endif() + +# List sources files +FILE(GLOB LIBRARY_FILES_C "*.cpp") +FILE(GLOB LIBRARY_FILES_H "*.h" "*.inl") + +cxx_library_with_type(IO "Libs" "" "${cxx_default}" + ${LIBRARY_FILES_C} ${LIBRARY_FILES_H} +) + +# Manually set Common.h as the precompiled header +IF(CMAKE_VERSION VERSION_GREATER_EQUAL 3.16.0) + TARGET_PRECOMPILE_HEADERS(IO PRIVATE "Common.h") +endif() + +# Link its dependencies +TARGET_LINK_LIBRARIES(IO Common ${PNG_LIBRARIES} ${JPEG_LIBRARIES} ${TIFF_LIBRARIES} ${EXIV2_LIBS}) + +# Install +SET_TARGET_PROPERTIES(IO PROPERTIES + PUBLIC_HEADER "${LIBRARY_FILES_H}") +INSTALL(TARGETS IO + EXPORT OpenMVSTargets + LIBRARY DESTINATION "${INSTALL_LIB_DIR}" + ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" + PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/IO") diff --git a/libs/IO/Common.cpp b/libs/IO/Common.cpp new file mode 100644 index 0000000..b08fa42 --- /dev/null +++ b/libs/IO/Common.cpp @@ -0,0 +1,12 @@ +//////////////////////////////////////////////////////////////////// +// Common.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +// Source file that includes just the standard includes +// Common.pch will be the pre-compiled header +// Common.obj will contain the pre-compiled type information + +#include "Common.h" diff --git a/libs/IO/Common.h b/libs/IO/Common.h new file mode 100644 index 0000000..e43aaac --- /dev/null +++ b/libs/IO/Common.h @@ -0,0 +1,63 @@ +//////////////////////////////////////////////////////////////////// +// Common.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __IO_COMMON_H__ +#define __IO_COMMON_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#if defined(IO_EXPORTS) && !defined(Common_EXPORTS) +#define Common_EXPORTS +#endif + +#include "../Common/Common.h" + +#ifndef IO_API +#define IO_API GENERAL_API +#endif +#ifndef IO_TPL +#define IO_TPL GENERAL_TPL +#endif + +#define _IMAGE_BMP // add BMP support +#define _IMAGE_TGA // add TGA support +#define _IMAGE_DDS // add DDS support +#ifdef _USE_PNG +#define _IMAGE_PNG // add PNG support +#endif +#ifdef _USE_JPG +#define _IMAGE_JPG // add JPG support +#endif +#ifdef _USE_TIFF +#define _IMAGE_TIFF // add TIFF support +#endif + +#include "ImageSCI.h" +#ifdef _IMAGE_BMP +#include "ImageBMP.h" +#endif +#ifdef _IMAGE_TGA +#include "ImageTGA.h" +#endif +#ifdef _IMAGE_DDS +#include "ImageDDS.h" +#endif +#ifdef _IMAGE_PNG +#include "ImagePNG.h" +#endif +#ifdef _IMAGE_JPG +#include "ImageJPG.h" +#endif +#ifdef _IMAGE_TIFF +#include "ImageTIFF.h" +#endif +#include "PLY.h" +#include "OBJ.h" +/*----------------------------------------------------------------*/ + +#endif // __IO_COMMON_H__ diff --git a/libs/IO/Image.cpp b/libs/IO/Image.cpp new file mode 100644 index 0000000..3d3e1ce --- /dev/null +++ b/libs/IO/Image.cpp @@ -0,0 +1,935 @@ +//////////////////////////////////////////////////////////////////// +// Image.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "Image.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +DEFINE_LOG(CImage, _T("IO ")); + +// Set the image details; +// if image's data is not NULL, but its size is too small, +// data's buffer is not allocated and _BUFFERSIZE is returned. +HRESULT CImage::Reset(Size width, Size height, PIXELFORMAT pixFormat, Size levels, bool bAllocate) +{ + // reinitialize image with given params + const size_t oldDataSize = GetDataSize(); + m_dataWidth = m_width = width; + m_dataHeight= m_height = height; + m_format = pixFormat; + m_stride = GetStride(pixFormat); + m_lineWidth = m_width * m_stride; + m_numLevels = levels; + m_level = 0; + if (bAllocate) { + if (m_data != NULL) { + if (oldDataSize < GetDataSize()) + return _BUFFERSIZE; + } else { + m_data = new uint8_t[GetDataSize()]; + } + } + return _OK; +} // Reset +/*----------------------------------------------------------------*/ + +HRESULT CImage::Reset(LPCTSTR szFileName, IMCREATE mode) +{ + // open the new image stream + m_fileName = szFileName; + // try to directly access the file + File* f; + if (mode == READ) { + f = new File(szFileName, File::READ, File::OPEN); + } else { + Util::ensureFolder(szFileName); + f = new File(szFileName, File::WRITE, File::CREATE | File::TRUNCATE); + } + if (!f->isOpen()) { + delete f; + return _INVALIDFILE; + } + if ((m_pStream = f) == NULL) { + LOG(LT_IMAGE, _T("error: failed opening image '%s'"), szFileName); + return _INVALIDFILE; + } + return _OK; +} // Reset +/*----------------------------------------------------------------*/ + +HRESULT CImage::Reset(IOSTREAMPTR& pStream) +{ + // use the already opened image stream + m_fileName.clear(); + m_pStream = pStream; + return _OK; +} // Reset +/*----------------------------------------------------------------*/ + +void CImage::Close() +{ + // close the image stream + m_pStream.Release(); + m_data.Release(); +} // Close +/*----------------------------------------------------------------*/ + + +HRESULT CImage::ReadHeader() +{ + return _OK; +} // ReadHeader +/*----------------------------------------------------------------*/ + +HRESULT CImage::ReadData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + // read data + if (dataFormat == m_format && nStride == m_stride) { + // read image directly to the data buffer + if (lineWidth == m_lineWidth) { + const size_t nSize = m_dataHeight*m_lineWidth; + if (nSize != m_pStream->read(pData, nSize)) + return _INVALIDFILE; + } else { + for (Size j=0; jread(pData, m_lineWidth)) + return _INVALIDFILE; + } + } else { + // read image to a buffer and convert it + CAutoPtrArr const buffer(new uint8_t[m_lineWidth]); + for (Size j=0; jread(buffer, m_lineWidth)) + return _INVALIDFILE; + if (!FilterFormat(pData, dataFormat, nStride, buffer, m_format, m_stride, m_dataWidth)) + return _FAIL; + } + } + // prepare next level + if (m_level+1 < m_numLevels) + m_lineWidth = GetDataSizes(++m_level, m_dataWidth, m_dataHeight); + return _OK; +} // ReadData +/*----------------------------------------------------------------*/ + + +HRESULT CImage::WriteHeader(PIXELFORMAT imageFormat, Size width, Size height, BYTE numLevels) +{ + // write header + m_numLevels = numLevels; + m_level = 0; + m_format = imageFormat; + m_stride = GetStride(m_format); + m_width = width; + m_height = height; + m_lineWidth = GetDataSizes(0, m_dataWidth, m_dataHeight); + return _OK; +} // WriteHeader +/*----------------------------------------------------------------*/ + +HRESULT CImage::WriteData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + // write data + if (dataFormat == m_format && nStride == m_stride) { + // write data buffer directly to the image + if (lineWidth == m_lineWidth) { + const size_t nSize = m_dataHeight*m_lineWidth; + if (nSize != m_pStream->write(pData, nSize)) + return _INVALIDFILE; + } else { + for (Size j=0; jwrite(pData, m_lineWidth)) + return _INVALIDFILE; + } + } else { + // convert data to a buffer and write it + CAutoPtrArr const buffer(new uint8_t[m_lineWidth]); + for (Size j=0; jwrite(buffer, m_lineWidth)) + return _INVALIDFILE; + } + } + // prepare next level + if (m_level+1 < m_numLevels) + m_lineWidth = GetDataSizes(++m_level, m_dataWidth, m_dataHeight); + return _OK; +} // WriteData +/*----------------------------------------------------------------*/ + + +CImage::Size CImage::GetDataSizes(Size mipLevel, Size& width, Size& height) const +{ + // Bump by the mip level. + height = MAXF((Size)1, m_height >> mipLevel); + width = MAXF((Size)1, m_width >> mipLevel); + + // if compressed, divide dims by 4 + if (m_format >= PF_DXT1) + { + width = (width+3)>>2; + height = (height+3)>>2; + } + + return width * m_stride; +} // GetDataSizes +/*----------------------------------------------------------------*/ + + +CImage::Size CImage::GetStride(PIXELFORMAT pixFormat) +{ + switch (pixFormat) + { + case PF_A8: + case PF_GRAY8: + return 1; + case PF_R5G6B5: + return 2; + case PF_B8G8R8: + case PF_R8G8B8: + return 3; + case PF_R8G8B8A8: + case PF_A8R8G8B8: + case PF_B8G8R8A8: + case PF_A8B8G8R8: + return 4; + case PF_DXT1: + return 8; + case PF_DXT2: + case PF_DXT3: + case PF_DXT4: + case PF_DXT5: + case PF_3DC: + return 16; + default: + LOG(LT_IMAGE, "error: unsupported SCI pixel format"); + } + return 0; +} // GetStride +/*----------------------------------------------------------------*/ + + +// If the given image format has alpha channel, returns true. +bool CImage::FormatHasAlpha(PIXELFORMAT format) +{ + switch (format) + { + case PF_A8: + case PF_R8G8B8A8: + case PF_A8R8G8B8: + case PF_B8G8R8A8: + case PF_A8B8G8R8: + case PF_DXT2: + case PF_DXT3: + case PF_DXT4: + case PF_DXT5: + return true; + case PF_GRAY8: + case PF_R5G6B5: + case PF_B8G8R8: + case PF_R8G8B8: + case PF_DXT1: + case PF_3DC: + return false; + default: + ASSERT("Unknown format" == NULL); + } + return false; +} // FormatHasAlpha +/*----------------------------------------------------------------*/ + + +// Convert from one format to another. +// nSzize is the number of pixels to process. +bool CImage::FilterFormat(void* pDst, PIXELFORMAT formatDst, Size strideDst, const void* pSrc, PIXELFORMAT formatSrc, Size strideSrc, Size nSzize) +{ + //ASSERT(formatDst != formatSrc || strideDst != strideSrc) + switch (formatDst) + { + case PF_A8: + case PF_GRAY8: + switch (formatSrc) + { + case PF_R8G8B8A8: + case PF_A8R8G8B8: + case PF_B8G8R8A8: + case PF_A8B8G8R8: + // from PF_R8G8B8A8 to PF_A8 (just copy the alpha channel) + (uint8_t*&)pSrc += 3; //skip the first RGB values + case PF_A8: + case PF_GRAY8: + // from PF_A8 to PF_A8 (just copy) + ASSERT(strideDst != strideSrc); + for (Size i=0; iReset(szName, mode))) { + delete pImage; + return NULL; + } + + return pImage; + +UNKNOWN_FORMAT: + // Unknown format + LOG(LT_IMAGE, "error: unknown image format '%s'", szName); + return NULL; +} // Create +/*----------------------------------------------------------------*/ + + +#ifndef _RELEASE + +// Save image as raw data. +void CImage::Dump(LPCTSTR szFileName) +{ + const String strName = Util::getFileName(szFileName)+String::ToString(m_width)+_T("x")+String::ToString(m_height)+_T("x")+String::ToString(m_stride)+Util::getFileExt(szFileName); + File f(strName, File::WRITE, File::CREATE | File::TRUNCATE); + if (!f.isOpen()) + return; + f.write(GetData(), GetDataSize()); +} // FlipRB24 +/*----------------------------------------------------------------*/ + +#endif diff --git a/libs/IO/Image.h b/libs/IO/Image.h new file mode 100644 index 0000000..44acd56 --- /dev/null +++ b/libs/IO/Image.h @@ -0,0 +1,128 @@ +//////////////////////////////////////////////////////////////////// +// Image.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_IMAGE_H__ +#define __SEACAVE_IMAGE_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + +#define LT_IMAGE CImage::ms_nLogType + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// All formats are listed from left to right, most-significant bit to least-significant bit. +// For example, PF_A8R8G8B8 is ordered from the most-significant bit channel A (alpha), +// to the least-significant bit channel B (blue). When traversing image data, the data is +// stored in memory on a little-endian machine from least-significant bit to most-significant bit, +// which means that the channel order in memory is from least-significant bit (blue) to +// most-significant bit (alpha). +typedef enum PIXELFORMAT_TYPE { + PF_UNKNOWN = 0, + // gray + PF_A8, + PF_GRAY8, + // uncompressed RGB + PF_R5G6B5, + PF_R8G8B8, + PF_R8G8B8A8, + PF_A8R8G8B8, + // uncompressed BGR + PF_B8G8R8, + PF_B8G8R8A8, + PF_A8B8G8R8, + // compressed + PF_DXT1 = 128, + PF_DXT2, + PF_DXT3, + PF_DXT4, + PF_DXT5, + PF_3DC, +} PIXELFORMAT; + +class IO_API CImage +{ + DECLARE_LOG(); + +public: + enum IMCREATE { + READ, + WRITE + }; + typedef uint32_t Size; + + CImage() {} + virtual ~CImage() {} + + virtual HRESULT Reset(Size width, Size height, PIXELFORMAT pixFormat, Size levels = 1, bool bAllocate = false); + virtual HRESULT Reset(LPCTSTR szFileName, IMCREATE mode); + virtual HRESULT Reset(IOSTREAMPTR& pStream); + virtual void Close(); + + virtual HRESULT ReadHeader(); + virtual HRESULT ReadData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + + virtual HRESULT WriteHeader(PIXELFORMAT, Size width, Size height, BYTE numLevels); + virtual HRESULT WriteData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + + const IOSTREAMPTR& GetStream() const { return m_pStream; } + IOSTREAMPTR& GetStream() { return m_pStream; } + BYTE* GetData() const { return m_data; } + BYTE*& GetData() { return m_data; } + size_t GetDataSize() const { return m_lineWidth * m_dataHeight; } + Size GetWidth() const { return m_width; } + Size GetHeight() const { return m_height; } + Size GetDataWidth() const { return m_dataWidth; } + Size GetDataHeight() const { return m_dataHeight; } + Size GetStride() const { return m_stride; } + Size GetLineWidth() const { return m_lineWidth; } + BYTE GetNumLevels() const { return m_numLevels; } + PIXELFORMAT GetFormat() const { return m_format; } + bool FormatHasAlpha() const { return FormatHasAlpha(m_format); } + const String& GetFileName() const { return m_fileName; } + String& GetFileName() { return m_fileName; } + + Size GetDataSizes(Size mipLevel, Size& width, Size& height) const; + + static Size GetStride(PIXELFORMAT); //in bits + static bool FormatHasAlpha(PIXELFORMAT); + static bool FilterFormat(void*, PIXELFORMAT, Size, const void*, PIXELFORMAT, Size, Size nSzize); + static void FlipRB24(uint8_t* data, Size size, Size stride); + static void CopyFlipRB24(uint8_t* pDst, const uint8_t* pSrc, Size size, Size strideDst, Size strideSrc); + + static CImage* Create(LPCTSTR szName, IMCREATE mode); + + #ifndef _RELEASE + void Dump(LPCTSTR szFileName); + #endif + +protected: + IOSTREAMPTR m_pStream; // stream used to read/write the image data + CAutoPtrArr m_data; // image's data buffer + Size m_width; // image width in pixels + Size m_height; // image height in pixels + Size m_dataWidth; // image's data width including mipmaps + Size m_dataHeight; // image's data height + Size m_stride; // bytes per pixel + Size m_lineWidth; // image canvas width in bytes + PIXELFORMAT m_format; // image format (pixel type) + BYTE m_numLevels; // number of mipmap levels (0 = auto-generate) + BYTE m_level; // index of the mipmap level currently reading + String m_fileName; // image's file name +}; // class CImage +typedef CSharedPtr IMAGEPTR; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_IMAGE_H__ diff --git a/libs/IO/ImageBMP.cpp b/libs/IO/ImageBMP.cpp new file mode 100644 index 0000000..e67c205 --- /dev/null +++ b/libs/IO/ImageBMP.cpp @@ -0,0 +1,261 @@ +//////////////////////////////////////////////////////////////////// +// ImageBMP.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" + +#ifdef _IMAGE_BMP +#include "ImageBMP.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// S T R U C T S /////////////////////////////////////////////////// + +#ifndef _MSC_VER +typedef long LONG; +typedef struct tagBITMAPFILEHEADER { + WORD bfType; + DWORD bfSize; + WORD bfReserved1; + WORD bfReserved2; + DWORD bfOffBits; +} BITMAPFILEHEADER; +typedef struct tagBITMAPINFOHEADER { + DWORD biSize; + LONG biWidth; + LONG biHeight; + WORD biPlanes; + WORD biBitCount; + DWORD biCompression; + DWORD biSizeImage; + LONG biXPelsPerMeter; + LONG biYPelsPerMeter; + DWORD biClrUsed; + DWORD biClrImportant; +} BITMAPINFOHEADER; +/* constants for the biCompression field */ +#define BI_RGB 0L +#define BI_RLE8 1L +#define BI_RLE4 2L +#define BI_BITFIELDS 3L +#define BI_JPEG 4L +#define BI_PNG 5L +#endif + + +CImageBMP::CImageBMP() +{ +} // Constructor + +CImageBMP::~CImageBMP() +{ +} // Destructor +/*----------------------------------------------------------------*/ + + +HRESULT CImageBMP::ReadHeader() +{ + // Jump to the beginning of the file + ((ISTREAM*)m_pStream)->setPos(0); + + // Read the BITMAPFILEHEADER + // and check the type field to make sure we have a .bmp file + // bfType should contain the letters "BM" + BITMAPFILEHEADER bmp_fileheader; + if (sizeof(BITMAPFILEHEADER) != m_pStream->read(&bmp_fileheader, sizeof(BITMAPFILEHEADER)) || + memcmp(&bmp_fileheader.bfType, "BM", 2)) + { + LOG(LT_IMAGE, _T("error: invalid BMP image")); + return _INVALIDFILE; + } + + // Read the BITMAPINFOHEADER. + // and double Check to make sure we got a correct BITMAPINFOHEADER + BITMAPINFOHEADER bmp_infoheader; + if (sizeof(BITMAPINFOHEADER) != m_pStream->read(&bmp_infoheader, sizeof(BITMAPINFOHEADER)) || + bmp_infoheader.biSize != sizeof(bmp_infoheader)) + { + LOG(LT_IMAGE, _T("error: invalid BMP image")); + return _INVALIDFILE; + } + + // Check for unsupported format: biPlanes MUST equal 1 + // and we can only handle uncompressed .bmps + if (bmp_infoheader.biPlanes != 1 || + (bmp_infoheader.biCompression != BI_RGB && bmp_infoheader.biCompression != BI_BITFIELDS)) + { + LOG(LT_IMAGE, "error: unsupported BMP image"); + return _INVALIDFILE; + } + + // Initililize our width, height and format as the .bmp we are loading + m_dataWidth = m_width = bmp_infoheader.biWidth; + m_dataHeight= m_height = bmp_infoheader.biHeight; + m_numLevels = 0; + m_level = 0; + m_lineWidth = (bmp_fileheader.bfSize-bmp_fileheader.bfOffBits)/bmp_infoheader.biHeight; //bmp_infoheader.biSizeImage may be set to zero for BI_RGB bitmaps + m_stride = bmp_infoheader.biBitCount>>3; + switch (m_stride) + { + case 1: + m_format = PF_GRAY8; + break; + case 2: + m_format = PF_R5G6B5; + break; + case 3: + m_format = PF_R8G8B8; + break; + case 4: + m_format = PF_R8G8B8A8; + break; + default: + LOG(LT_IMAGE, "error: unsupported BMP image"); + return _INVALIDFILE; + } + // Ensure m_lineWidth is DWORD aligned + while ((m_lineWidth%4) != 0) ++m_lineWidth; + + // Jump to the location where the bitmap data is stored + ((ISTREAM*)m_pStream)->setPos(bmp_fileheader.bfOffBits); + + return _OK; +} // ReadHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImageBMP::ReadData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + // read data + const size_t nSize = m_width*m_stride; + const size_t nPad = m_lineWidth-nSize; + CAutoPtrArr const bufferPad(nPad ? new uint8_t[nPad] : NULL); + if (dataFormat == m_format && nStride == m_stride) { + // read image directly to the data buffer + (BYTE*&)pData += (m_height-1)*lineWidth; + for (Size j=0; jread(pData, nSize) || + (nPad && nPad != m_pStream->read(bufferPad, nPad))) + return _INVALIDFILE; + } else { + // read image to a buffer and convert it + CAutoPtrArr const buffer(new uint8_t[m_lineWidth]); + for (Size j=0; jread(buffer, m_lineWidth)) + return _INVALIDFILE; + if (!FilterFormat((uint8_t*)pData+(m_height-j-1)*lineWidth, dataFormat, nStride, buffer, m_format, m_stride, m_width)) + return _FAIL; + } + } + return _OK; +} // ReadData +/*----------------------------------------------------------------*/ + + +HRESULT CImageBMP::WriteHeader(PIXELFORMAT imageFormat, Size width, Size height, BYTE /*numLevels*/) +{ + // write header + m_numLevels = 0; + m_level = 0; + DWORD dwCompression; + switch (imageFormat) + { + case PF_A8: + case PF_GRAY8: + m_stride = 1; + dwCompression = BI_RGB; + m_format = PF_GRAY8; + break; + case PF_R5G6B5: + m_stride = 2; + dwCompression = BI_BITFIELDS; + m_format = PF_R5G6B5; + break; + case PF_B8G8R8: + case PF_R8G8B8: + m_stride = 3; + dwCompression = BI_RGB; + m_format = PF_R8G8B8; + break; + case PF_R8G8B8A8: + case PF_A8R8G8B8: + case PF_B8G8R8A8: + case PF_A8B8G8R8: + m_stride = 4; + dwCompression = BI_BITFIELDS; + m_format = PF_R8G8B8A8; + break; + default: + LOG(LT_IMAGE, "error: unsupported BMP image format"); + return _INVALIDFILE; + } + m_dataWidth = m_width = width; + m_dataHeight= m_height = height; + m_lineWidth = m_width*m_stride; + // Ensure m_lineWidth is DWORD aligned + while ((m_lineWidth%4) != 0) ++m_lineWidth; + ((OSTREAM*)m_pStream)->setPos(0); + // Write the BITMAPFILEHEADER and the BITMAPINFOHEADER. + const BITMAPFILEHEADER bmp_fileheader = { + 0x4d42, //BM + (DWORD)sizeof(BITMAPFILEHEADER)+(DWORD)sizeof(BITMAPINFOHEADER)+m_lineWidth*m_height, //total file size + 0, 0, //reserved + (DWORD)sizeof(BITMAPFILEHEADER)+(DWORD)sizeof(BITMAPINFOHEADER) //offset to the bitmap bits + }; + const BITMAPINFOHEADER bmp_infoheader = { + (DWORD)sizeof(BITMAPINFOHEADER), //size of this structure + (LONG)m_width, (LONG)m_height, //dim + (WORD)1, //number of color planes being used (must be set to 1) + (WORD)(m_stride<<3), //number of bits per pixel + dwCompression, //compression method being used + m_lineWidth*m_height, //raw image size + 0, 0, //pixels-per-meter + 0, //number of colors in the color palette + 0 //number of important colors used + }; + if (sizeof(BITMAPFILEHEADER) != m_pStream->write(&bmp_fileheader, sizeof(BITMAPFILEHEADER)) || + sizeof(BITMAPINFOHEADER) != m_pStream->write(&bmp_infoheader, sizeof(BITMAPINFOHEADER))) + { + LOG(LT_IMAGE, "error: failed writing the BMP image"); + return _INVALIDFILE; + } + return _OK; +} // WriteHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImageBMP::WriteData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + // write data + const size_t nSize = m_width*m_stride; + const size_t nPad = m_lineWidth-nSize; + CAutoPtrArr const bufferPad(nPad ? new uint8_t[nPad] : NULL); + if (nPad) memset(bufferPad, 0, nPad); + if (dataFormat == m_format && nStride == m_stride) { + // write data buffer directly to the image + for (Size j=0; jwrite((uint8_t*)pData+(m_height-j-1)*lineWidth, nSize) || + (nPad && nPad != m_pStream->write(bufferPad, nPad))) + return _INVALIDFILE; + } else { + // convert data to a buffer and write it + CAutoPtrArr const buffer(new uint8_t[m_lineWidth]); + for (Size j=0; jwrite(buffer, m_lineWidth)) + return _INVALIDFILE; + } + } + return _OK; +} // WriteData +/*----------------------------------------------------------------*/ + +#endif // _IMAGE_BMP diff --git a/libs/IO/ImageBMP.h b/libs/IO/ImageBMP.h new file mode 100644 index 0000000..43ae475 --- /dev/null +++ b/libs/IO/ImageBMP.h @@ -0,0 +1,36 @@ +//////////////////////////////////////////////////////////////////// +// ImageBMP.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_IMAGEBMP_H__ +#define __SEACAVE_IMAGEBMP_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Image.h" + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class IO_API CImageBMP : public CImage +{ +public: + CImageBMP(); + virtual ~CImageBMP(); + + HRESULT ReadHeader(); + HRESULT ReadData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + HRESULT WriteHeader(PIXELFORMAT, Size width, Size height, BYTE numLevels); + HRESULT WriteData(void*, PIXELFORMAT, Size nStride, Size lineWidth); +}; // class CImageBMP +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_IMAGEBMP_H__ diff --git a/libs/IO/ImageDDS.cpp b/libs/IO/ImageDDS.cpp new file mode 100644 index 0000000..09e1e26 --- /dev/null +++ b/libs/IO/ImageDDS.cpp @@ -0,0 +1,341 @@ +//////////////////////////////////////////////////////////////////// +// ImageDDS.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" + +#ifdef _IMAGE_DDS +#include "ImageDDS.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + +// These were copied from the DX9 docs. The names are changed +// from the "real" defines since not all platforms have them. + +#ifndef MakeFourCC +#define MakeFourCC(ch0, ch1, ch2, ch3) \ + ((uint32_t)(uint8_t)(ch0) | ((uint32_t)(uint8_t)(ch1) << 8) | \ + ((uint32_t)(uint8_t)(ch2) << 16) | ((uint32_t)(uint8_t)(ch3) << 24 )) +#endif + +#define IMAGE_DDS_TYPE MakeFourCC('D','D','S',' ') //"DDS " = 4 bytes + +#define FOURCC_DXT1 MakeFourCC('D','X','T','1') +#define FOURCC_DXT2 MakeFourCC('D','X','T','2') +#define FOURCC_DXT3 MakeFourCC('D','X','T','3') +#define FOURCC_DXT4 MakeFourCC('D','X','T','4') +#define FOURCC_DXT5 MakeFourCC('D','X','T','5') +#define FOURCC_3DC MakeFourCC('A','T','I','2') + +enum DDSSurfaceDescFlags +{ + DDSDCaps = 0x00000001l, + DDSDHeight = 0x00000002l, + DDSDWidth = 0x00000004l, + DDSDPitch = 0x00000008l, + DDSDPixelFormat = 0x00001000l, + DDSDMipMapCount = 0x00020000l, + DDSDLinearSize = 0x00080000l, + DDSDDepth = 0x00800000l, +}; + +enum DDSPixelFormatFlags +{ + DDPFAlphaPixels = 0x00000001, + DDPFFourCC = 0x00000004, + DDPFRGB = 0x00000040, +}; + +enum DDSCapFlags +{ + DDSCAPSComplex = 0x00000008, + DDSCAPSTexture = 0x00001000, + DDSCAPSMipMap = 0x00400000, + + DDSCAPS2Cubemap = 0x00000200, + DDSCAPS2Cubemap_POSITIVEX = 0x00000400, + DDSCAPS2Cubemap_NEGATIVEX = 0x00000800, + DDSCAPS2Cubemap_POSITIVEY = 0x00001000, + DDSCAPS2Cubemap_NEGATIVEY = 0x00002000, + DDSCAPS2Cubemap_POSITIVEZ = 0x00004000, + DDSCAPS2Cubemap_NEGATIVEZ = 0x00008000, + DDSCAPS2Volume = 0x00200000, +}; + +typedef struct DDPIXELFORMAT_TYPE +{ + DWORD dwSize; // size of structure + DWORD dwFlags; // pixel format flags + DWORD dwFourCC; // (FOURCC code) + union + { + DWORD dwRGBBitCount; // how many bits per pixel + DWORD dwYUVBitCount; // how many bits per pixel + DWORD dwZBufferBitDepth; // how many total bits/pixel in z buffer (including any stencil bits) + DWORD dwAlphaBitDepth; // how many bits for alpha channels + DWORD dwLuminanceBitCount; // how many bits per pixel + DWORD dwBumpBitCount; // how many bits per "buxel", total + DWORD dwPrivateFormatBitCount;// Bits per pixel of private driver formats. Only valid in texture + // format list and if DDPF_D3DFORMAT is set + }; + union + { + DWORD dwRBitMask; // mask for red bit + DWORD dwYBitMask; // mask for Y bits + DWORD dwStencilBitDepth; // how many stencil bits (note: dwZBufferBitDepth-dwStencilBitDepth is total Z-only bits) + DWORD dwLuminanceBitMask; // mask for luminance bits + DWORD dwBumpDuBitMask; // mask for bump map U delta bits + DWORD dwOperations; // DDPF_D3DFORMAT Operations + }; + union + { + DWORD dwGBitMask; // mask for green bits + DWORD dwUBitMask; // mask for U bits + DWORD dwZBitMask; // mask for Z bits + DWORD dwBumpDvBitMask; // mask for bump map V delta bits + struct + { + WORD wFlipMSTypes; // Multisample methods supported via flip for this D3DFORMAT + WORD wBltMSTypes; // Multisample methods supported via blt for this D3DFORMAT + }; + + }; + union + { + DWORD dwBBitMask; // mask for blue bits + DWORD dwVBitMask; // mask for V bits + DWORD dwStencilBitMask; // mask for stencil bits + DWORD dwBumpLuminanceBitMask; // mask for luminance in bump map + }; + union + { + DWORD dwRGBAlphaBitMask; // mask for alpha channel + DWORD dwYUVAlphaBitMask; // mask for alpha channel + DWORD dwLuminanceAlphaBitMask;// mask for alpha channel + DWORD dwRGBZBitMask; // mask for Z channel + DWORD dwYUVZBitMask; // mask for Z channel + }; +} DDSPF; + +typedef struct DDSCAPS_TYPE +{ + DWORD dwCaps; // capabilities of surface wanted + DWORD dwCaps2; + DWORD dwCaps3; + union + { + DWORD dwCaps4; + DWORD dwVolumeDepth; + }; +} DDSCAPS; + +typedef struct DDSINFOHEADER_TYPE { + DWORD dwHeader; // magic number "DDS " + DWORD dwSize; // size of the DDSURFACEDESC structure + DWORD dwFlags; // determines what fields are valid + DWORD dwHeight; // height of surface to be created + DWORD dwWidth; // width of input surface + DWORD dwPitchOrLinearSize; // distance to start of next line or formless late-allocated optimized surface size + DWORD dwDepth; // the depth if this is a volume texture + DWORD dwMipMapCount; // number of mip-map levels requestde + DWORD dwReserved1[11]; + DDSPF ddsPixelFormat; // pixel format description of the surface + DDSCAPS ddsCaps; // direct draw surface capabilities + DWORD dwReserved2; +} DDSINFOHEADER; + +#define IMAGE_DDS_DDSINFOHEADERSIZE (sizeof(DDSINFOHEADER)-sizeof(DWORD)/*dwHeader*/) // should be 124 + +#define ISBITMASK(r,g,b,a) (ddsInfo.ddsPixelFormat.dwRBitMask == r && ddsInfo.ddsPixelFormat.dwGBitMask == g && ddsInfo.ddsPixelFormat.dwBBitMask == b && ddsInfo.ddsPixelFormat.dwRGBAlphaBitMask == a ) + + + +// S T R U C T S /////////////////////////////////////////////////// + +CImageDDS::CImageDDS() +{ +} // Constructor + +CImageDDS::~CImageDDS() +{ +} // Destructor +/*----------------------------------------------------------------*/ + + +HRESULT CImageDDS::ReadHeader() +{ + // read header + ((ISTREAM*)m_pStream)->setPos(0); + DDSINFOHEADER ddsInfo; + m_pStream->read(&ddsInfo, sizeof(DDSINFOHEADER)); + if (ddsInfo.dwHeader != IMAGE_DDS_TYPE && + ddsInfo.dwSize != IMAGE_DDS_DDSINFOHEADERSIZE && + !(ddsInfo.dwFlags & (DDSDCaps | DDSDPixelFormat | DDSDWidth | DDSDHeight))) { // always include DDSD_CAPS, DDSD_PIXELFORMAT, DDSD_WIDTH, DDSD_HEIGHT + LOG(LT_IMAGE, "error: invalid DDS image"); + return _INVALIDFILE; + } + + m_width = ddsInfo.dwWidth; + m_height = ddsInfo.dwHeight; + m_numLevels = ddsInfo.dwFlags & DDSDMipMapCount ? (BYTE)ddsInfo.dwMipMapCount : 0; + m_level = 0; + + // get the format + m_stride = ddsInfo.ddsPixelFormat.dwRGBBitCount>>3; + m_format = PF_UNKNOWN; + if (ddsInfo.ddsPixelFormat.dwFlags & DDPFFourCC) + { + switch (ddsInfo.ddsPixelFormat.dwFourCC) + { + case FOURCC_DXT1: + m_format = PF_DXT1; + m_stride = 8; + break; + case FOURCC_DXT2: + m_format = PF_DXT2; + m_stride = 16; + break; + case FOURCC_DXT3: + m_format = PF_DXT3; + m_stride = 16; + break; + case FOURCC_DXT4: + m_format = PF_DXT4; + m_stride = 16; + break; + case FOURCC_DXT5: + m_format = PF_DXT5; + m_stride = 16; + break; + case FOURCC_3DC: + m_format = PF_3DC; + m_stride = 16; + break; + //case 0x74: + // m_format=FORMAT_R32G32B32A32F; + // break; + //case 0x71: + // m_format=FORMAT_R16G16B16A16F; + // break; + //case 0x70: + // m_format=FORMAT_G16R16F; + // break; + //case 0x73: + // m_format=FORMAT_G32R32F; + // break; + //case 0x6F: + // m_format=FORMAT_R16F; + // break; + //case 0x72: + // m_format=FORMAT_R32F; + // break; + } + } + else if (ddsInfo.ddsPixelFormat.dwFlags & DDPFRGB) + { + if (ddsInfo.ddsPixelFormat.dwFlags & DDPFAlphaPixels) + { + if (ISBITMASK(0xff, 0xff00, 0xff0000, 0xff000000)) + m_format = PF_B8G8R8A8; + else if (ISBITMASK(0xff0000, 0xff00, 0xff, 0xff000000)) + m_format = PF_R8G8B8A8; + //else if (ISBITMASK(0xff000000, 0xff0000, 0xff00, 0xff)) + // m_format = FORMAT_ABGR; + //else if (ISBITMASK(0x7C00, 0x3E0, 0x1F, 0x8000)) + // m_format = FORMAT_A1R5G5B5; + } + else //no DDPFAlphaPixels + { + if (ISBITMASK(0xff, 0xff00, 0xff0000, 0x00)) + m_format = PF_B8G8R8; + else if (ISBITMASK(0xff0000, 0xff00, 0xff, 0x00)) + m_format = PF_R8G8B8; + else if (ISBITMASK(0xF800, 0x7E0, 0x1F, 0x00)) + m_format = PF_R5G6B5; + //else if (ISBITMASK(0xffFF, 0xffFF0000, 0x00, 0x00)) + // m_format = FORMAT_G16R16; + //else if (ISBITMASK(0x7C00, 0x3E0, 0x1F, 0x00)) + // m_format = FORMAT_X1R5G5B5; + } + } + else //no DDPFRGB + { + if (ISBITMASK(0x00, 0x00, 0x00, 0xff)) + m_format = PF_A8; + else if (ISBITMASK(0xff, 0x00, 0x00, 0x00)) + m_format = PF_GRAY8; + //else if (ISBITMASK(0xffff, 0x00, 0x00, 0x00)) + // m_format = FORMAT_L16; + //else if (ISBITMASK(0xff, 0x00, 0x00, 0xff00)) + // m_format = FORMAT_A8L8; + //else if (ISBITMASK(0xff, 0xff00, 0x00, 0x00)) + // m_format = FORMAT_V8U8; + //else if (ISBITMASK(0xFF, 0xFF00, 0xFF0000, 0xFF000000)) + // m_format = FORMAT_Q8W8V8U8; + //else if (ISBITMASK(0xFFFF, 0xFFFF0000, 0x00, 0x00)) + // m_format = FORMAT_V16U16; + } + ASSERT(m_format != PF_UNKNOWN); + if (m_format == PF_UNKNOWN) + { + LOG(LT_IMAGE, "error: unsupported DDS image"); + return _INVALIDFILE; + } + + m_lineWidth = GetDataSizes(0, m_dataWidth, m_dataHeight); + return _OK; +} // ReadHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImageDDS::ReadData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + // read data + if (dataFormat == m_format && nStride == m_stride) { + // read image directly to the data buffer + if (lineWidth == m_lineWidth) { + const size_t nSize = m_dataHeight*m_lineWidth; + if (nSize != m_pStream->read(pData, nSize)) + return _INVALIDFILE; + } else { + for (Size j=0; jread(pData, m_lineWidth)) + return _INVALIDFILE; + } + } else { + // read image to a buffer and convert it + CAutoPtrArr const buffer(new uint8_t[m_lineWidth]); + for (Size j=0; jread(buffer, m_lineWidth)) + return _INVALIDFILE; + if (!FilterFormat(pData, dataFormat, nStride, buffer, m_format, m_stride, m_dataWidth)) + return _FAIL; + } + } + // prepare next level + m_lineWidth = GetDataSizes(++m_level, m_dataWidth, m_dataHeight); + return _OK; +} // ReadData +/*----------------------------------------------------------------*/ + + +HRESULT CImageDDS::WriteHeader(PIXELFORMAT imageFormat, Size width, Size height, BYTE numLevels) +{ + return _FAIL; +} // WriteHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImageDDS::WriteData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + return _FAIL; +} // WriteData +/*----------------------------------------------------------------*/ + +#endif // _IMAGE_DDS \ No newline at end of file diff --git a/libs/IO/ImageDDS.h b/libs/IO/ImageDDS.h new file mode 100644 index 0000000..268d963 --- /dev/null +++ b/libs/IO/ImageDDS.h @@ -0,0 +1,36 @@ +//////////////////////////////////////////////////////////////////// +// ImageDDS.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_IMAGEDDS_H__ +#define __SEACAVE_IMAGEDDS_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Image.h" + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class IO_API CImageDDS : public CImage +{ +public: + CImageDDS(); + virtual ~CImageDDS(); + + HRESULT ReadHeader(); + HRESULT ReadData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + HRESULT WriteHeader(PIXELFORMAT, Size width, Size height, BYTE numLevels); + HRESULT WriteData(void*, PIXELFORMAT, Size nStride, Size lineWidth); +}; // class CImageDDS +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_IMAGEDDS_H__ diff --git a/libs/IO/ImageJPG.cpp b/libs/IO/ImageJPG.cpp new file mode 100644 index 0000000..7ed958e --- /dev/null +++ b/libs/IO/ImageJPG.cpp @@ -0,0 +1,281 @@ +//////////////////////////////////////////////////////////////////// +// ImageJPG.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" + +#ifdef _IMAGE_JPG +#include "ImageJPG.h" + +#ifdef _MSC_VER +#define XMD_H // prevent redefinition of INT32 +#undef FAR // prevent FAR redefinition +#endif + +extern "C" { +#include +} +#include + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define JPG_BUFFER_SIZE (16*1024) + +struct JpegErrorMgr +{ + struct jpeg_error_mgr pub; + jmp_buf setjmp_buffer; +}; + +struct JpegSource +{ + struct jpeg_source_mgr pub; + ISTREAM* pStream; + JOCTET* buffer; +}; + +struct JpegState +{ + jpeg_decompress_struct cinfo; // IJG JPEG codec structure + JpegErrorMgr jerr;// error processing manager state + JpegSource source;// memory buffer source +}; + + +// F U N C T I O N S /////////////////////////////////////////////// + +METHODDEF(void) +stub(j_decompress_ptr cinfo) +{ + JpegSource* source = (JpegSource*)cinfo->src; + source->pStream->setPos(0); +} + +METHODDEF(boolean) +fill_input_buffer(j_decompress_ptr cinfo) +{ + JpegSource* source = (JpegSource*)cinfo->src; + const size_t size = source->pStream->read(source->buffer, JPG_BUFFER_SIZE); + if (size == STREAM_ERROR || size == 0) + return FALSE; + source->pub.next_input_byte = source->buffer; + source->pub.bytes_in_buffer = size; + return TRUE; +} + +METHODDEF(void) +skip_input_data(j_decompress_ptr cinfo, long num_bytes) +{ + JpegSource* source = (JpegSource*)cinfo->src; + + if (num_bytes > (long)source->pub.bytes_in_buffer) + { + // We need to skip more data than we have in the buffer. + source->pStream->setPos(source->pStream->getPos() + (num_bytes - source->pub.bytes_in_buffer)); + source->pub.next_input_byte += source->pub.bytes_in_buffer; + source->pub.bytes_in_buffer = 0; + } + else + { + // Skip portion of the buffer + source->pub.bytes_in_buffer -= num_bytes; + source->pub.next_input_byte += num_bytes; + } +} + +METHODDEF(void) +error_exit(j_common_ptr cinfo) +{ + JpegErrorMgr* err_mgr = (JpegErrorMgr*)(cinfo->err); + + /* Return control to the setjmp point */ + longjmp( err_mgr->setjmp_buffer, 1 ); +} + + +// S T R U C T S /////////////////////////////////////////////////// + +CImageJPG::CImageJPG() : m_state(NULL) +{ +} // Constructor + +CImageJPG::~CImageJPG() +{ + //clean up + Close(); +} // Destructor +/*----------------------------------------------------------------*/ + +void CImageJPG::Close() +{ + if (m_state) + { + JpegState* state = (JpegState*)m_state; + jpeg_destroy_decompress( &state->cinfo ); + delete state; + m_state = NULL; + } + m_width = m_height = 0; + CImage::Close(); +} +/*----------------------------------------------------------------*/ + +HRESULT CImageJPG::ReadHeader() +{ + JpegState* state = new JpegState; + m_state = state; + state->cinfo.err = jpeg_std_error(&state->jerr.pub); + state->jerr.pub.error_exit = error_exit; + + if (setjmp(state->jerr.setjmp_buffer ) == 0) + { + jpeg_create_decompress( &state->cinfo ); + + // Prepare for suspending reader + state->source.pub.init_source = stub; + state->source.pub.fill_input_buffer = fill_input_buffer; + state->source.pub.skip_input_data = skip_input_data; + state->source.pub.resync_to_restart = jpeg_resync_to_restart; + state->source.pub.term_source = stub; + state->source.pub.bytes_in_buffer = 0;// forces fill_input_buffer on first read + state->source.pub.next_input_byte = NULL; + state->source.pStream = (IOSTREAM*)m_pStream; + state->source.buffer = (JOCTET*)(*state->cinfo.mem->alloc_small)((j_common_ptr)&state->cinfo, JPOOL_PERMANENT, JPG_BUFFER_SIZE * sizeof(JOCTET)); + state->cinfo.src = &state->source.pub; + + jpeg_read_header(&state->cinfo, TRUE); + + m_dataWidth = m_width = state->cinfo.image_width; + m_dataHeight= m_height = state->cinfo.image_height; + m_numLevels = 0; + m_level = 0; + m_stride = state->cinfo.num_components; + m_lineWidth = m_width * m_stride; + switch (m_stride) + { + case 1: + m_format = PF_GRAY8; + state->cinfo.out_color_space = JCS_GRAYSCALE; + state->cinfo.out_color_components = 1; + break; + case 3: + m_format = PF_B8G8R8; + state->cinfo.out_color_space = JCS_RGB; + state->cinfo.out_color_components = 3; + break; + default: + LOG(LT_IMAGE, "error: unsupported JPG image"); + return _INVALIDFILE; + } + return _OK; + } + + Close(); + return _FAIL; +} // ReadHeader +/*----------------------------------------------------------------*/ + +HRESULT CImageJPG::ReadData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + JpegState* state = (JpegState*)m_state; + + if (state && m_width && m_height) + { + jpeg_decompress_struct* cinfo = &state->cinfo; + JpegErrorMgr* jerr = &state->jerr; + + if (setjmp(jerr->setjmp_buffer) == 0) + { + jpeg_start_decompress(cinfo); + + // read data + if (dataFormat == m_format && nStride == m_stride) { + // read image directly to the data buffer + JSAMPLE* buffer[1] = {(JSAMPLE*)pData}; + uint8_t*& data = (uint8_t*&)buffer[0]; + for (Size j=0; jmem->alloc_sarray)((j_common_ptr)cinfo, JPOOL_IMAGE, m_lineWidth, 1); + uint8_t* dst = (uint8_t*)pData; + uint8_t* src = (uint8_t*)buffer[0]; + for (Size j=0; j +#include + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + + +// F U N C T I O N S /////////////////////////////////////////////// + +void custom_png_read_file(png_structp png_ptr, png_bytep buffer, png_size_t size) +{ + png_voidp const ptrIO = png_get_io_ptr(png_ptr); + ((InputStream*)ptrIO)->read(buffer, size); +} + +void custom_png_write_file(png_structp png_ptr, png_bytep buffer, png_size_t size) +{ + png_voidp const ptrIO = png_get_io_ptr(png_ptr); + ((IOStream*)ptrIO)->write(buffer, size); +} + +void custom_png_flush_file(png_structp png_ptr) +{ + png_voidp const ptrIO = png_get_io_ptr(png_ptr); + ((IOStream*)ptrIO)->flush(); +} + + + +// S T R U C T S /////////////////////////////////////////////////// + +CImagePNG::CImagePNG() : m_png_ptr(NULL), m_info_ptr(NULL) +{ +} // Constructor + +CImagePNG::~CImagePNG() +{ + Close(); +} // Destructor +/*----------------------------------------------------------------*/ + +void CImagePNG::Close() +{ + if (m_png_ptr) { + png_structp png_ptr = (png_structp)m_png_ptr; + png_infop info_ptr = (png_infop)m_info_ptr; + if (bRead) + png_destroy_read_struct(&png_ptr, &info_ptr, (png_infopp)NULL); + else + png_destroy_write_struct(&png_ptr, &info_ptr); + m_png_ptr = m_info_ptr = NULL; + } + CImage::Close(); +} +/*----------------------------------------------------------------*/ + + +HRESULT CImagePNG::ReadHeader() +{ + // initialize stuff + ASSERT(m_png_ptr == NULL); + m_png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); + if (!m_png_ptr) { + LOG(LT_IMAGE, "error: invalid PNG image (png_create_read_struct)"); + return _INVALIDFILE; + } + bRead = true; + png_structp png_ptr = (png_structp)m_png_ptr; + + m_info_ptr = png_create_info_struct(png_ptr); + if (!m_info_ptr) { + LOG(LT_IMAGE, "error: invalid PNG image (png_create_info_struct)"); + return _INVALIDFILE; + } + png_infop info_ptr = (png_infop)m_info_ptr; + + if (setjmp(png_jmpbuf(png_ptr))) + return _INVALIDFILE; + + ((ISTREAM*)m_pStream)->setPos(0); + png_set_read_fn(png_ptr, m_pStream, custom_png_read_file); + + png_read_info(png_ptr, info_ptr); + + png_uint_32 width, height; int bitdepth, colortype; + png_get_IHDR(png_ptr, info_ptr, &width, &height, &bitdepth, &colortype, NULL, NULL, NULL); + + if (png_get_valid(png_ptr, info_ptr, PNG_INFO_tRNS)) { + png_set_tRNS_to_alpha(png_ptr); + colortype = PNG_COLOR_TYPE_RGB_ALPHA; + } + + #if 0 + // if it doesn't have a file gamma, don't do any correction ("do no harm") + if (png_get_valid(png_ptr, info_ptr, PNG_INFO_gAMA)) { + double gamma = 0; + const double screen_gamma = 2.2; + if (png_get_gAMA(png_ptr, info_ptr, &gamma)) + png_set_gamma(png_ptr, screen_gamma, gamma); + } + #endif + + switch (colortype) + { + case PNG_COLOR_TYPE_GRAY: + if (bitdepth < 8) + png_set_expand_gray_1_2_4_to_8(png_ptr); + m_format = PF_GRAY8; + m_stride = 1; + break; + case PNG_COLOR_TYPE_PALETTE: + png_set_palette_to_rgb(png_ptr); + m_format = PF_B8G8R8; + m_stride = 3; + break; + case PNG_COLOR_TYPE_GRAY_ALPHA: + png_set_gray_to_rgb(png_ptr); + m_format = PF_B8G8R8; + m_stride = 3; + break; + case PNG_COLOR_TYPE_RGB: + m_format = PF_B8G8R8; + m_stride = 3; + break; + case PNG_COLOR_TYPE_RGB_ALPHA: + m_format = PF_B8G8R8A8; + m_stride = 4; + break; + default: + LOG(LT_IMAGE, "error: unsupported PNG image"); + return _INVALIDFILE; + } + + if (bitdepth == 16) + png_set_strip_16(png_ptr); + + png_set_interlace_handling(png_ptr); + png_read_update_info(png_ptr, info_ptr); + + m_dataWidth = m_width = (Size)width; + m_dataHeight= m_height = (Size)height; + m_numLevels = 0; + m_level = 0; + m_lineWidth = (Size)png_get_rowbytes(png_ptr, info_ptr); + + return _OK; +} // ReadHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImagePNG::ReadData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + png_structp png_ptr = (png_structp)m_png_ptr; + + // read data + if (nStride == m_stride && (m_format != PF_B8G8R8A8 || (dataFormat != PF_A8R8G8B8 && dataFormat != PF_A8B8G8R8))) { + if ((m_format == PF_B8G8R8 && dataFormat == PF_R8G8B8) || + (m_format == PF_B8G8R8A8 && dataFormat == PF_R8G8B8A8)) { + // flip R and B from the PNG library + png_set_bgr(png_ptr); + } else { + ASSERT(m_format == dataFormat); + } + // read image directly to the data buffer + for (Size j=0; j const buffer(new png_byte[m_lineWidth]); + for (Size j=0; j 0) { + png_set_compression_mem_level(png_ptr, compression_level); + } + else { + // tune parameters for speed + // (see http://wiki.linuxquestions.org/wiki/Libpng) + png_set_filter(png_ptr, PNG_FILTER_TYPE_BASE, PNG_FILTER_SUB); + png_set_compression_level(png_ptr, Z_BEST_SPEED); + } + png_set_compression_strategy(png_ptr, compression_strategy); + + png_set_IHDR(png_ptr, info_ptr, m_width, m_height, 8/*depth*/, + m_stride == 1 ? PNG_COLOR_TYPE_GRAY : + m_stride == 3 ? PNG_COLOR_TYPE_RGB : PNG_COLOR_TYPE_RGBA, + PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, + PNG_FILTER_TYPE_DEFAULT); + + png_write_info(png_ptr, info_ptr); + + return _OK; +} // WriteHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImagePNG::WriteData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + png_structp png_ptr = (png_structp)m_png_ptr; + + // write data + if (nStride == m_stride && (m_format != PF_B8G8R8A8 || (dataFormat != PF_A8R8G8B8 && dataFormat != PF_A8B8G8R8))) { + if ((m_format == PF_B8G8R8 && dataFormat == PF_R8G8B8) || + (m_format == PF_B8G8R8A8 && dataFormat == PF_R8G8B8A8)) { + // flip R and B from the PNG library + png_set_bgr(png_ptr); + } else { + ASSERT(m_format == dataFormat); + } + // write image directly to the data buffer + for (Size j=0; j const buffer(new png_byte[m_lineWidth]); + for (Size j=0; jsetPos(0); + SCIINFOHEADER sciInfo; + m_pStream->read(&sciInfo, sizeof(SCIINFOHEADER)); + if (sciInfo.dwHeader != IMAGE_SCI_HEADER) { + LOG(LT_IMAGE, "error: invalid SCI image"); + return _INVALIDFILE; + } + m_width = sciInfo.shWidth; + m_height = sciInfo.shHeight; + m_numLevels = sciInfo.byLevels; + m_level = 0; + m_format = (PIXELFORMAT)sciInfo.byFormat; + m_stride = GetStride(m_format); + m_lineWidth = GetDataSizes(0, m_dataWidth, m_dataHeight); + return _OK; +} // ReadHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImageSCI::WriteHeader(PIXELFORMAT imageFormat, Size width, Size height, BYTE numLevels) +{ + // write header + CImage::WriteHeader(imageFormat, width, height, numLevels); + ((OSTREAM*)m_pStream)->setPos(0); + const SCIINFOHEADER sciInfo = {IMAGE_SCI_HEADER, (uint16_t)m_width, (uint16_t)m_height, (uint8_t)m_format, m_numLevels, 0, 0}; + if (sizeof(SCIINFOHEADER) != m_pStream->write(&sciInfo, sizeof(SCIINFOHEADER))) { + LOG(LT_IMAGE, "error: failed writing the SCI image"); + return _INVALIDFILE; + } + return _OK; +} // WriteHeader +/*----------------------------------------------------------------*/ diff --git a/libs/IO/ImageSCI.h b/libs/IO/ImageSCI.h new file mode 100644 index 0000000..fb9bb28 --- /dev/null +++ b/libs/IO/ImageSCI.h @@ -0,0 +1,34 @@ +//////////////////////////////////////////////////////////////////// +// ImageSCI.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_IMAGESCI_H__ +#define __SEACAVE_IMAGESCI_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Image.h" + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class IO_API CImageSCI : public CImage +{ +public: + CImageSCI(); + virtual ~CImageSCI(); + + HRESULT ReadHeader(); + HRESULT WriteHeader(PIXELFORMAT, Size width, Size height, BYTE numLevels); +}; // class CImageSCI +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_IMAGESCI_H__ diff --git a/libs/IO/ImageTGA.cpp b/libs/IO/ImageTGA.cpp new file mode 100644 index 0000000..deefa12 --- /dev/null +++ b/libs/IO/ImageTGA.cpp @@ -0,0 +1,146 @@ +//////////////////////////////////////////////////////////////////// +// ImageTGA.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" + +#ifdef _IMAGE_TGA +#include "ImageTGA.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + +#pragma pack(push, 1) +typedef struct TGAINFOHEADER_TYPE { + BYTE byIDLength; // ID length + BYTE byCMType; // Color map type + BYTE byType; // Image type + struct { // Color map specifications + uint16_t shCMIdx; // First entry index, offset into the color map table + uint16_t shCMLength; // Color map length, number of entries + BYTE byCMBPP; // Color map entry size, number of bits per pixel + }; + struct { // Image specifications + uint16_t shXOrigin; // X-origin, absolute coordinate of lower-left corner for displays where origin is at the lower left + uint16_t shYOrigin; // Y-origin, as for X-origin + uint16_t shWidth; // Image width, width in pixels + uint16_t shHeight; // Image height, width in pixels + BYTE byBPP; // Pixel depth, bits per pixel + union { + BYTE byAlphaInfo; // Image descriptor, bits 3-0 give the alpha channel depth, bits 4-5 give direction + struct { + BYTE byAlphaBPP : 3;// alpha channel depth + BYTE bMirrored : 1;// alpha channel depth + BYTE bFlipped : 1;// alpha channel depth + BYTE : 0;// Force alignment to next boundary. + }; + }; + }; +} TGAINFOHEADER; +#pragma pack(pop) + + + +// S T R U C T S /////////////////////////////////////////////////// + +CImageTGA::CImageTGA() +{ +} // Constructor + +CImageTGA::~CImageTGA() +{ +} // Destructor +/*----------------------------------------------------------------*/ + + +HRESULT CImageTGA::ReadHeader() +{ + // read header + ((ISTREAM*)m_pStream)->setPos(0); + TGAINFOHEADER tgaInfo; + m_pStream->read(&tgaInfo, sizeof(TGAINFOHEADER)); + if (tgaInfo.byCMType != 0) { // paletted images not supported + LOG(LT_IMAGE, "error: invalid TGA image"); + return _INVALIDFILE; + } + + m_dataWidth = m_width = tgaInfo.shWidth; + m_dataHeight = m_height = tgaInfo.shHeight; + m_numLevels = 0; + m_level = 0; + + // get the format + m_stride = (tgaInfo.byBPP>>3); + switch (tgaInfo.byType) + { + case 2: //uncompressed, true-color image + case 3: //uncompressed, black-and-white image + m_format = (m_stride == 3 ? PF_R8G8B8 : PF_R8G8B8A8); + m_bRLE = false; + break; + //case 10://run-length encoded, true-color image and + // m_format = IF_3DC; + // m_bRLE = true; + // break; + default: + ASSERT(0); + LOG(LT_IMAGE, "error: unsupported TGA image"); + return _INVALIDFILE; + } + + m_lineWidth = m_width * m_stride; + + // read the id stuff, not using seeking to be able to use non seekable sources + if (tgaInfo.byIDLength) { + CAutoPtrArr const buffer(new uint8_t[tgaInfo.byIDLength]); + m_pStream->read(buffer, tgaInfo.byIDLength); + } + + return _OK; +} // ReadHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImageTGA::ReadData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + // read data + if (dataFormat == m_format && nStride == m_stride) { + // read image directly to the data buffer + (BYTE*&)pData += (m_height-1)*lineWidth; + for (Size j=0; jread(pData, m_lineWidth)) + return _INVALIDFILE; + } else { + // read image to a buffer and convert it + CAutoPtrArr const buffer(new uint8_t[m_lineWidth]); + for (Size j=0; jread(buffer, m_lineWidth)) + return _INVALIDFILE; + if (!FilterFormat((BYTE*)pData+(m_height-j-1)*lineWidth, dataFormat, nStride, buffer, m_format, m_stride, m_width)) + return _FAIL; + } + } + return _OK; +} // ReadData +/*----------------------------------------------------------------*/ + + +HRESULT CImageTGA::WriteHeader(PIXELFORMAT imageFormat, Size width, Size height, BYTE numLevels) +{ + return _FAIL; +} // WriteHeader +/*----------------------------------------------------------------*/ + + +HRESULT CImageTGA::WriteData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + return _FAIL; +} // WriteData +/*----------------------------------------------------------------*/ + +#endif // _IMAGE_TGA diff --git a/libs/IO/ImageTGA.h b/libs/IO/ImageTGA.h new file mode 100644 index 0000000..517814e --- /dev/null +++ b/libs/IO/ImageTGA.h @@ -0,0 +1,39 @@ +//////////////////////////////////////////////////////////////////// +// ImageTGA.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_IMAGETGA_H__ +#define __SEACAVE_IMAGETGA_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Image.h" + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class IO_API CImageTGA : public CImage +{ +public: + CImageTGA(); + virtual ~CImageTGA(); + + HRESULT ReadHeader(); + HRESULT ReadData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + HRESULT WriteHeader(PIXELFORMAT, Size width, Size height, BYTE numLevels); + HRESULT WriteData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + +protected: + bool m_bRLE; +}; // class CImageTGA +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_IMAGETGA_H__ diff --git a/libs/IO/ImageTIFF.cpp b/libs/IO/ImageTIFF.cpp new file mode 100644 index 0000000..bfdd7e1 --- /dev/null +++ b/libs/IO/ImageTIFF.cpp @@ -0,0 +1,567 @@ +//////////////////////////////////////////////////////////////////// +// ImageTIFF.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" + +#ifdef _IMAGE_TIFF +#include "ImageTIFF.h" + +extern "C" { +#if !defined(_MSC_VER) && !defined(__BORLANDC__) +#include +#undef TIFF_INT64_T +#define TIFF_INT64_T int64_t +#undef TIFF_UINT64_T +#define TIFF_UINT64_T uint64_t +#endif +#include +} + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + +/* + ISO C++ uses a 'std::streamsize' type to define counts. This makes + it similar to, (but perhaps not the same as) size_t. + + The std::ios::pos_type is used to represent stream positions as used + by tellg(), tellp(), seekg(), and seekp(). This makes it similar to + (but perhaps not the same as) 'off_t'. The std::ios::streampos type + is used for character streams, but is documented to not be an + integral type anymore, so it should *not* be assigned to an integral + type. + + The std::ios::off_type is used to specify relative offsets needed by + the variants of seekg() and seekp() which accept a relative offset + argument. + + Useful prototype knowledge: + + Obtain read position + ios::pos_type basic_istream::tellg() + + Set read position + basic_istream& basic_istream::seekg(ios::pos_type) + basic_istream& basic_istream::seekg(ios::off_type, ios_base::seekdir) + + Read data + basic_istream& istream::read(char *str, streamsize count) + + Number of characters read in last unformatted read + streamsize istream::gcount(); + + Obtain write position + ios::pos_type basic_ostream::tellp() + + Set write position + basic_ostream& basic_ostream::seekp(ios::pos_type) + basic_ostream& basic_ostream::seekp(ios::off_type, ios_base::seekdir) + + Write data + basic_ostream& ostream::write(const char *str, streamsize count) +*/ + +struct tiffis_data; +struct tiffos_data; + +extern "C" { + + static tmsize_t _tiffosReadProc(thandle_t, void*, tmsize_t); + static tmsize_t _tiffisReadProc(thandle_t fd, void* buf, tmsize_t size); + static tmsize_t _tiffosWriteProc(thandle_t fd, void* buf, tmsize_t size); + static tmsize_t _tiffisWriteProc(thandle_t, void*, tmsize_t); + static uint64_t _tiffosSeekProc(thandle_t fd, uint64_t off, int whence); + static uint64_t _tiffisSeekProc(thandle_t fd, uint64_t off, int whence); + static uint64_t _tiffosSizeProc(thandle_t fd); + static uint64_t _tiffisSizeProc(thandle_t fd); + static int _tiffosCloseProc(thandle_t fd); + static int _tiffisCloseProc(thandle_t fd); + static int _tiffDummyMapProc(thandle_t, void** base, toff_t* size); + static void _tiffDummyUnmapProc(thandle_t, void* base, toff_t size); + static TIFF* _tiffStreamOpen(const char* name, const char* mode, void *fd); + + struct tiffis_data + { + ISTREAM* stream; + size_f_t start_pos; + }; + + struct tiffos_data + { + OSTREAM* stream; + size_f_t start_pos; + }; + + static tmsize_t _tiffosReadProc(thandle_t, void*, tmsize_t) + { + return 0; + } + + static tmsize_t _tiffisReadProc(thandle_t fd, void* buf, tmsize_t size) + { + tiffis_data *data = reinterpret_cast(fd); + + // Verify that type does not overflow. + size_t request_size = size; + if (static_cast(request_size) != size) + return static_cast(-1); + + return static_cast(data->stream->read(buf, request_size)); + } + + static tmsize_t _tiffosWriteProc(thandle_t fd, void* buf, tmsize_t size) + { + tiffos_data *data = reinterpret_cast(fd); + + // Verify that type does not overflow. + size_t request_size = size; + if (static_cast(request_size) != size) + return static_cast(-1); + + return static_cast(data->stream->write(buf, request_size)); + } + + static tmsize_t _tiffisWriteProc(thandle_t, void*, tmsize_t) + { + return 0; + } + + static uint64_t _tiffosSeekProc(thandle_t fd, uint64_t off, int whence) + { + tiffos_data *data = reinterpret_cast(fd); + OSTREAM* os = data->stream; + + // if the stream has already failed, don't do anything + if (os == NULL) + return static_cast(-1); + + bool bSucceeded(true); + switch (whence) { + case SEEK_SET: + { + // Compute 64-bit offset + uint64_t new_offset = static_cast(data->start_pos) + off; + + // Verify that value does not overflow + size_f_t offset = static_cast(new_offset); + if (static_cast(offset) != new_offset) + return static_cast(-1); + + bSucceeded = os->setPos(offset); + break; + } + case SEEK_CUR: + { + // Verify that value does not overflow + size_f_t offset = static_cast(off); + if (static_cast(offset) != off) + return static_cast(-1); + + bSucceeded = os->setPos(os->getPos()+offset); + break; + } + case SEEK_END: + { + // Verify that value does not overflow + size_f_t offset = static_cast(off); + if (static_cast(offset) != off) + return static_cast(-1); + + bSucceeded = os->setPos(os->getSize()-offset); + break; + } + } + + // Attempt to workaround problems with seeking past the end of the + // stream. ofstream doesn't have a problem with this but + // ostrstream/ostringstream does. In that situation, add intermediate + // '\0' characters. + if (!bSucceeded) { + size_f_t origin; + switch (whence) { + case SEEK_SET: + default: + origin = data->start_pos; + break; + case SEEK_CUR: + origin = os->getPos(); + break; + case SEEK_END: + os->setPos(os->getSize()); + origin = os->getPos(); + break; + } + + // only do something if desired seek position is valid + if ((static_cast(origin) + off) > static_cast(data->start_pos)) { + uint64_t num_fill; + // extend the stream to the expected size + os->setPos(os->getSize()); + num_fill = (static_cast(origin)) + off - os->getPos(); + const char dummy = '\0'; + for (uint64_t i = 0; i < num_fill; i++) + os->write(&dummy, 1); + // retry the seek + os->setPos(static_cast(static_cast(origin) + off)); + } + } + + return static_cast(os->getPos()); + } + + static uint64_t _tiffisSeekProc(thandle_t fd, uint64_t off, int whence) + { + tiffis_data *data = reinterpret_cast(fd); + ISTREAM* is = data->stream; + + switch (whence) { + case SEEK_SET: + { + // Compute 64-bit offset + uint64_t new_offset = static_cast(data->start_pos) + off; + + // Verify that value does not overflow + size_f_t offset = static_cast(new_offset); + if (static_cast(offset) != new_offset) + return static_cast(-1); + + is->setPos(offset); + break; + } + case SEEK_CUR: + { + // Verify that value does not overflow + size_f_t offset = static_cast(off); + if (static_cast(offset) != off) + return static_cast(-1); + + is->setPos(is->getPos()+offset); + break; + } + case SEEK_END: + { + // Verify that value does not overflow + size_f_t offset = static_cast(off); + if (static_cast(offset) != off) + return static_cast(-1); + + is->setPos(is->getSize()-offset); + break; + } + } + + return (uint64_t)(is->getPos() - data->start_pos); + } + + static uint64_t _tiffosSizeProc(thandle_t fd) + { + tiffos_data *data = reinterpret_cast(fd); + return (uint64_t)data->stream->getSize(); + } + + static uint64_t _tiffisSizeProc(thandle_t fd) + { + tiffis_data *data = reinterpret_cast(fd); + return (uint64_t)data->stream->getSize(); + } + + static int _tiffosCloseProc(thandle_t fd) + { + // Our stream was not allocated by us, so it shouldn't be closed by us. + delete reinterpret_cast(fd); + return 0; + } + + static int _tiffisCloseProc(thandle_t fd) + { + // Our stream was not allocated by us, so it shouldn't be closed by us. + delete reinterpret_cast(fd); + return 0; + } + + static int _tiffDummyMapProc(thandle_t, void** /*base*/, toff_t* /*size*/) + { + return (0); + } + + static void _tiffDummyUnmapProc(thandle_t, void* /*base*/, toff_t /*size*/) + { + } + + /* + * Open a TIFF file descriptor for read/writing. + */ + static TIFF* _tiffStreamOpen(const char* name, const char* mode, void *fd) + { + TIFF* tif; + + if (strchr(mode, 'w')) { + tiffos_data *data = new tiffos_data; + data->stream = reinterpret_cast(fd); + data->start_pos = data->stream->getPos(); + + // Open for writing. + tif = TIFFClientOpen(name, mode, + reinterpret_cast(data), + _tiffosReadProc, + _tiffosWriteProc, + _tiffosSeekProc, + _tiffosCloseProc, + _tiffosSizeProc, + _tiffDummyMapProc, + _tiffDummyUnmapProc); + } else { + tiffis_data *data = new tiffis_data; + data->stream = reinterpret_cast(fd); + data->start_pos = data->stream->getPos(); + // Open for reading. + tif = TIFFClientOpen(name, mode, + reinterpret_cast(data), + _tiffisReadProc, + _tiffisWriteProc, + _tiffisSeekProc, + _tiffisCloseProc, + _tiffisSizeProc, + _tiffDummyMapProc, + _tiffDummyUnmapProc); + } + + return (tif); + } + +} /* extern "C" */ + +// TIFFOpen() mode flags are different to fopen(). A 'b' in mode "rb" has no effect when reading. +// http://www.remotesensing.org/libtiff/man/TIFFOpen.3tiff.html +// NB: We don't support mapped files with streams so add 'm' +TIFF* TIFFStreamOpen(const char* name, OSTREAM* os) +{ + return _tiffStreamOpen(name, "wm", os); +} +TIFF* TIFFStreamOpen(const char* name, ISTREAM* is) +{ + return _tiffStreamOpen(name, "rm", is); +} + + +// S T R U C T S /////////////////////////////////////////////////// + +CImageTIFF::CImageTIFF() : m_state(NULL) +{ +} // Constructor + +CImageTIFF::~CImageTIFF() +{ + //clean up + Close(); +} // Destructor +/*----------------------------------------------------------------*/ + +void CImageTIFF::Close() +{ + if (m_state) + { + TIFF* tif = static_cast(m_state); + TIFFClose(tif); + m_state = NULL; + } + m_width = m_height = 0; + CImage::Close(); +} +/*----------------------------------------------------------------*/ + +HRESULT CImageTIFF::ReadHeader() +{ + TIFF* tif = static_cast(m_state); + if (!tif) { + tif = TIFFStreamOpen("ReadTIFF", (ISTREAM*)m_pStream); + if (!tif) { + LOG(LT_IMAGE, "error: unsupported TIFF image"); + return _INVALIDFILE; + } + } + m_state = tif; + + uint16 photometric = 0; + if (TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &m_width) && + TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &m_height) && + TIFFGetField(tif, TIFFTAG_PHOTOMETRIC, &photometric)) + { + uint16 bpp=8, ncn = photometric > 1 ? 3 : 1; + TIFFGetField(tif, TIFFTAG_BITSPERSAMPLE, &bpp); + TIFFGetField(tif, TIFFTAG_SAMPLESPERPIXEL, &ncn); + + m_dataWidth = m_width; + m_dataHeight= m_height; + m_numLevels = 0; + m_level = 0; + + if ((bpp == 32 && ncn == 3) || photometric == PHOTOMETRIC_LOGLUV) { + // this is HDR format with 3 floats per pixel + //TODO: implement + ASSERT("error: not implemented" == NULL); + Close(); + return _FAIL; + } + if (bpp > 8 && + ((photometric != 2 && photometric != 1) || + (ncn != 1 && ncn != 3 && ncn != 4))) + bpp = 8; + switch (bpp) { + case 8: + m_stride = 4; + m_format = PF_B8G8R8A8; + break; + //case 16: + // m_type = CV_MAKETYPE(CV_16U, photometric > 1 ? 3 : 1); + // break; + //case 32: + // m_type = CV_MAKETYPE(CV_32F, photometric > 1 ? 3 : 1); + // break; + //case 64: + // m_type = CV_MAKETYPE(CV_64F, photometric > 1 ? 3 : 1); + // break; + default: + //TODO: implement + ASSERT("error: not implemented" == NULL); + LOG(LT_IMAGE, "error: unsupported TIFF image"); + Close(); + return _INVALIDFILE; + } + m_lineWidth = m_width * m_stride; + + return _OK; + } + + Close(); + return _FAIL; +} // ReadHeader +/*----------------------------------------------------------------*/ + +HRESULT CImageTIFF::ReadData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + if (m_state && m_width && m_height) { + TIFF* tif = (TIFF*)m_state; + uint32_t tile_width0 = m_width, tile_height0 = 0; + int is_tiled = TIFFIsTiled(tif); + uint16 photometric; + TIFFGetField(tif, TIFFTAG_PHOTOMETRIC, &photometric); + uint16 bpp = 8, ncn = photometric > 1 ? 3 : 1; + TIFFGetField(tif, TIFFTAG_BITSPERSAMPLE, &bpp); + TIFFGetField(tif, TIFFTAG_SAMPLESPERPIXEL, &ncn); + const int bitsPerByte = 8; + int dst_bpp = (int)(1 * bitsPerByte); + if (dst_bpp == 8) { + char errmsg[1024]; + if (!TIFFRGBAImageOK(tif, errmsg)) { + Close(); + return _INVALIDFILE; + } + } + + if ((!is_tiled) || + (is_tiled && + TIFFGetField(tif, TIFFTAG_TILEWIDTH, &tile_width0) && + TIFFGetField(tif, TIFFTAG_TILELENGTH, &tile_height0))) + { + if (!is_tiled) + TIFFGetField(tif, TIFFTAG_ROWSPERSTRIP, &tile_height0); + + if (tile_width0 <= 0) + tile_width0 = m_width; + + if (tile_height0 <= 0 || + (!is_tiled && tile_height0 == std::numeric_limits::max())) + tile_height0 = m_height; + + uint8_t* data = (uint8_t*)pData; + if (!is_tiled && tile_height0 == 1 && dataFormat == m_format && nStride == m_stride) { + // read image directly to the data buffer + for (Size j=0; j m_height) + tile_height = m_height - y; + + for (uint32_t x = 0; x < m_width; x += tile_width0) { + uint32_t tile_width = tile_width0; + if (x + tile_width > m_width) + tile_width = m_width - x; + + int ok; + switch (dst_bpp) { + case 8: + { + uint8_t* bstart = buffer; + if (!is_tiled) + ok = TIFFReadRGBAStrip(tif, y, (uint32_t*)buffer); + else { + ok = TIFFReadRGBATile(tif, x, y, (uint32_t*)buffer); + //Tiles fill the buffer from the bottom up + bstart += (tile_height0 - tile_height) * tile_width0 * 4; + } + if (!ok) { + Close(); + return _INVALIDFILE; + } + + for (uint32_t i = 0; i < tile_height; ++i) { + uint8_t* dst = data + x*3 + lineWidth*(tile_height - i - 1); + uint8_t* src = bstart + i*tile_width0*4; + if (!FilterFormat(dst, dataFormat, nStride, src, m_format, m_stride, tile_width)) { + Close(); + return _FAIL; + } + } + break; + } + default: + { + Close(); + return _INVALIDFILE; + } + } + } + } + } + + return _OK; + } + } + + Close(); + return _FAIL; +} // Read +/*----------------------------------------------------------------*/ + +HRESULT CImageTIFF::WriteHeader(PIXELFORMAT imageFormat, Size width, Size height, BYTE numLevels) +{ + //TODO: to implement the TIFF encoder + return _OK; +} // WriteHeader +/*----------------------------------------------------------------*/ + +HRESULT CImageTIFF::WriteData(void* pData, PIXELFORMAT dataFormat, Size nStride, Size lineWidth) +{ + //TODO: to implement the TIFF encoder + return _OK; +} // WriteData +/*----------------------------------------------------------------*/ + +#endif // _IMAGE_TIFF diff --git a/libs/IO/ImageTIFF.h b/libs/IO/ImageTIFF.h new file mode 100644 index 0000000..f3eb743 --- /dev/null +++ b/libs/IO/ImageTIFF.h @@ -0,0 +1,44 @@ +//////////////////////////////////////////////////////////////////// +// ImageTIFF.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_IMAGETIFF_H__ +#define __SEACAVE_IMAGETIFF_H__ + + +// D E F I N E S /////////////////////////////////////////////////// + + +// I N C L U D E S ///////////////////////////////////////////////// + +#include "Image.h" + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +class IO_API CImageTIFF : public CImage +{ +public: + CImageTIFF(); + virtual ~CImageTIFF(); + + void Close(); + + HRESULT ReadHeader(); + HRESULT ReadData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + HRESULT WriteHeader(PIXELFORMAT, Size width, Size height, BYTE numLevels); + HRESULT WriteData(void*, PIXELFORMAT, Size nStride, Size lineWidth); + +protected: + void* m_state; +}; // class CImageTIFF +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_IMAGETIFF_H__ diff --git a/libs/IO/OBJ.cpp b/libs/IO/OBJ.cpp new file mode 100644 index 0000000..0c1f530 --- /dev/null +++ b/libs/IO/OBJ.cpp @@ -0,0 +1,290 @@ +//////////////////////////////////////////////////////////////////// +// OBJ.cpp +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "OBJ.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + +// uncomment to enable multi-threading based on OpenMP +#ifdef _USE_OPENMP +#define OBJ_USE_OPENMP +#endif + +#define OBJ_INDEX_OFFSET 1 + + +// S T R U C T S /////////////////////////////////////////////////// + +ObjModel::MaterialLib::Material::Material(const Image8U3& _diffuse_map, const Color& _Kd) + : + diffuse_map(_diffuse_map), + Kd(_Kd) +{ +} + +bool ObjModel::MaterialLib::Material::LoadDiffuseMap() +{ + if (diffuse_map.empty()) + return diffuse_map.Load(diffuse_name); + return true; +} +/*----------------------------------------------------------------*/ + + + +// S T R U C T S /////////////////////////////////////////////////// + +ObjModel::MaterialLib::MaterialLib() +{ +} + +bool ObjModel::MaterialLib::Save(const String& prefix, bool texLossless) const +{ + std::ofstream out((prefix+".mtl").c_str()); + if (!out.good()) + return false; + + const String pathName(Util::getFilePath(prefix)); + const String name(Util::getFileNameExt(prefix)); + #ifdef OBJ_USE_OPENMP + bool bSuccess(true); + #pragma omp parallel for + #endif + for (int_t i = 0; i < (int_t)materials.size(); ++i) { + const Material& mat = materials[i]; + // save material description + std::stringstream ss; + ss << "newmtl " << mat.name << "\n" + << "Ka 1.000000 1.000000 1.000000" << "\n" + << "Kd " << mat.Kd.r << " " << mat.Kd.g << " " << mat.Kd.b << "\n" + << "Ks 0.000000 0.000000 0.000000" << "\n" + << "Tr 1.000000" << "\n" + << "illum 1" << "\n" + << "Ns 1.000000" << "\n"; + // save material maps + if (mat.diffuse_map.empty()) { + #ifdef OBJ_USE_OPENMP + #pragma omp critical + #endif + out << ss.str(); + continue; + } + if (mat.diffuse_name.IsEmpty()) + const_cast(mat.diffuse_name) = name+"_"+mat.name+"_map_Kd."+(texLossless?"png":"jpg"); + ss << "map_Kd " << mat.diffuse_name << "\n"; + #ifdef OBJ_USE_OPENMP + #pragma omp critical + #endif + out << ss.str(); + const bool bRet(mat.diffuse_map.Save(pathName+mat.diffuse_name)); + #ifdef OBJ_USE_OPENMP + #pragma omp critical + if (!bRet) + bSuccess = false; + #else + if (!bRet) + return false; + #endif + } + #ifdef OBJ_USE_OPENMP + return bSuccess; + #else + return true; + #endif +} + +bool ObjModel::MaterialLib::Load(const String& fileName) +{ + const size_t numMaterials(materials.size()); + std::ifstream in(fileName.c_str()); + String keyword; + while (in.good() && in >> keyword) { + if (keyword == "newmtl") { + in >> keyword; + materials.push_back(Material(keyword)); + } else if (keyword == "Kd") { + ASSERT(numMaterials < materials.size()); + Color c; + in >> c.r >> c.g >> c.b; + materials.back().Kd = c; + } else if (keyword == "map_Kd") { + ASSERT(numMaterials < materials.size()); + String& diffuse_name = materials.back().diffuse_name; + in >> diffuse_name; + diffuse_name = Util::getFilePath(fileName) + diffuse_name; + } + } + return numMaterials < materials.size(); +} +/*----------------------------------------------------------------*/ + + + +// S T R U C T S /////////////////////////////////////////////////// + +bool ObjModel::Save(const String& fileName, unsigned precision, bool texLossless) const +{ + if (vertices.empty()) + return false; + const String prefix(Util::getFileFullName(fileName)); + const String name(Util::getFileNameExt(prefix)); + + if (!material_lib.Save(prefix, texLossless)) + return false; + + std::ofstream out((prefix + ".obj").c_str()); + if (!out.good()) + return false; + + out << "mtllib " << name << ".mtl" << "\n"; + + out << std::fixed << std::setprecision(precision); + for (size_t i = 0; i < vertices.size(); ++i) { + out << "v " + << vertices[i][0] << " " + << vertices[i][1] << " " + << vertices[i][2] << "\n"; + } + + for (size_t i = 0; i < texcoords.size(); ++i) { + out << "vt " + << texcoords[i][0] << " " + << texcoords[i][1] << "\n"; + } + + for (size_t i = 0; i < normals.size(); ++i) { + out << "vn " + << normals[i][0] << " " + << normals[i][1] << " " + << normals[i][2] << "\n"; + } + + for (size_t i = 0; i < groups.size(); ++i) { + out << "usemtl " << groups[i].material_name << "\n"; + for (size_t j = 0; j < groups[i].faces.size(); ++j) { + const Face& face = groups[i].faces[j]; + out << "f"; + for (size_t k = 0; k < 3; ++k) { + out << " " << face.vertices[k] + OBJ_INDEX_OFFSET; + if (!texcoords.empty()) { + out << "/" << face.texcoords[k] + OBJ_INDEX_OFFSET; + if (!normals.empty()) + out << "/" << face.normals[k] + OBJ_INDEX_OFFSET; + } else + if (!normals.empty()) + out << "//" << face.normals[k] + OBJ_INDEX_OFFSET; + } + out << "\n"; + } + } + return true; +} + +bool ObjModel::Load(const String& fileName) +{ + ASSERT(vertices.empty() && groups.empty() && material_lib.materials.empty()); + std::ifstream fin(fileName.c_str()); + String line, keyword; + std::istringstream in; + while (fin.good()) { + std::getline(fin, line); + if (line.empty() || line[0u] == '#') + continue; + in.str(line); + in >> keyword; + if (keyword == "v") { + Vertex v; + in >> v[0] >> v[1] >> v[2]; + if (in.fail()) + return false; + vertices.push_back(v); + } else + if (keyword == "vt") { + TexCoord vt; + in >> vt[0] >> vt[1]; + if (in.fail()) + return false; + texcoords.push_back(vt); + } else + if (keyword == "vn") { + Normal vn; + in >> vn[0] >> vn[1] >> vn[2]; + if (in.fail()) + return false; + normals.push_back(vn); + } else + if (keyword == "f") { + Face f; + memset(&f, 0xFF, sizeof(Face)); + for (size_t k = 0; k < 3; ++k) { + in >> keyword; + switch (sscanf(keyword, "%u/%u/%u", f.vertices+k, f.texcoords+k, f.normals+k)) { + case 1: + f.vertices[k] -= OBJ_INDEX_OFFSET; + break; + case 2: + f.vertices[k] -= OBJ_INDEX_OFFSET; + if (f.texcoords[k] != NO_ID) + f.texcoords[k] -= OBJ_INDEX_OFFSET; + if (f.normals[k] != NO_ID) + f.normals[k] -= OBJ_INDEX_OFFSET; + break; + case 3: + f.vertices[k] -= OBJ_INDEX_OFFSET; + f.texcoords[k] -= OBJ_INDEX_OFFSET; + f.normals[k] -= OBJ_INDEX_OFFSET; + break; + default: + return false; + } + } + if (in.fail()) + return false; + if (groups.empty()) + AddGroup(""); + groups.back().faces.push_back(f); + } else + if (keyword == "mtllib") { + in >> keyword; + if (!material_lib.Load(keyword)) + return false; + } else + if (keyword == "usemtl") { + Group group; + in >> group.material_name; + if (in.fail()) + return false; + groups.push_back(group); + } + in.clear(); + } + return !vertices.empty(); +} + + +ObjModel::Group& ObjModel::AddGroup(const String& material_name) +{ + groups.push_back(Group()); + Group& group = groups.back(); + group.material_name = material_name; + if (!GetMaterial(material_name)) + material_lib.materials.push_back(MaterialLib::Material(material_name)); + return group; +} + +ObjModel::MaterialLib::Material* ObjModel::GetMaterial(const String& name) +{ + MaterialLib::Materials::iterator it(std::find_if(material_lib.materials.begin(), material_lib.materials.end(), [&name](const MaterialLib::Material& mat) { return mat.name == name; })); + if (it == material_lib.materials.end()) + return NULL; + return &(*it); +} +/*----------------------------------------------------------------*/ diff --git a/libs/IO/OBJ.h b/libs/IO/OBJ.h new file mode 100644 index 0000000..d32c356 --- /dev/null +++ b/libs/IO/OBJ.h @@ -0,0 +1,115 @@ +//////////////////////////////////////////////////////////////////// +// OBJ.h +// +// Copyright 2007 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#ifndef __SEACAVE_OBJ_H__ +#define __SEACAVE_OBJ_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + + +namespace SEACAVE { + +// S T R U C T S /////////////////////////////////////////////////// + +// OBJ model files parser. +// +// The OBJ file format is a simple data-format that represents 3D geometry alone — +// namely, the position of each vertex, the UV position of each texture coordinate +// vertex, vertex normals, and the faces that make each polygon defined as a list of +// vertices, and texture vertices. Vertices are stored in a counter-clockwise order +// by default, making explicit declaration of face normals unnecessary. + +class IO_API ObjModel { +public: + typedef Pixel32F Color; + + // represents a material lib of an OBJ model + struct MaterialLib { + // represents a Lambertian material + struct Material { + String name; + String diffuse_name; + Image8U3 diffuse_map; + Color Kd; + + Material() : Kd(Color::WHITE) {} + Material(const String& _name) : name(_name), Kd(Color::WHITE) {} + Material(const Image8U3& _diffuse_map, const Color& _Kd=Color::WHITE); + + // Makes sure the image is loaded for the diffuse map + bool LoadDiffuseMap(); + }; + + typedef std::vector Materials; + + Materials materials; + + MaterialLib(); + + // Saves the material lib to a .mtl file and all textures of its materials with the given prefix name + bool Save(const String& prefix, bool texLossless=false) const; + // Loads the material lib from a .mtl file and all textures of its materials with the given file name + bool Load(const String& fileName); + }; + + typedef Point3f Vertex; + typedef Point2f TexCoord; + typedef Point3f Normal; + + typedef uint32_t Index; + + struct Face { + Index vertices[3]; + Index texcoords[3]; + Index normals[3]; + }; + + struct Group { + String material_name; + std::vector faces; + }; + + typedef std::vector Vertices; + typedef std::vector TexCoords; + typedef std::vector Normals; + typedef std::vector Groups; + +protected: + Vertices vertices; + TexCoords texcoords; + Normals normals; + Groups groups; + MaterialLib material_lib; + +public: + ObjModel() {} + + // Saves the obj model to an .obj file, its material lib and the materials with the given file name + bool Save(const String& fileName, unsigned precision=6, bool texLossless=false) const; + // Loads the obj model from an .obj file, its material lib and the materials with the given file name + bool Load(const String& fileName); + + // Creates a new group with the given material name + Group& AddGroup(const String& material_name); + // Retrieves a material from the library based on its name + MaterialLib::Material* GetMaterial(const String& name); + + MaterialLib& get_material_lib() { return material_lib; } + Vertices& get_vertices() { return vertices; } + TexCoords& get_texcoords() { return texcoords; } + Normals& get_normals() { return normals; } + Groups& get_groups() { return groups; } +}; +/*----------------------------------------------------------------*/ + +} // namespace SEACAVE + +#endif // __SEACAVE_OBJ_H__ diff --git a/libs/IO/PLY.cpp b/libs/IO/PLY.cpp new file mode 100644 index 0000000..4dbfa5e --- /dev/null +++ b/libs/IO/PLY.cpp @@ -0,0 +1,2386 @@ +//////////////////////////////////////////////////////////////////// +// PLY.cpp +// +// Copyright 2023 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) + +#include "Common.h" +#include "PLY.h" + +using namespace SEACAVE; + + +// D E F I N E S /////////////////////////////////////////////////// + +#define NO_OTHER_PROPS -1 + +#define DONT_STORE_PROP 0 +#define STORE_PROP 1 + +#define OTHER_PROP 0 +#define NAMED_PROP 1 + +#define abort_ply(...) { VERBOSE(__VA_ARGS__); exit(-1); } + + +// S T R U C T S /////////////////////////////////////////////////// + +const char* const PLY::type_names[] = { + "invalid", "int8", "int16", "int32", "uint8", "uint16", "uint32", "float32", "float64", +}; + +const char* const PLY::old_type_names[] = { + "invalid", "char", "short", "int", "uchar", "ushort", "uint", "float", "double", +}; + +const int PLY::ply_type_size[] = { + 0, 1, 2, 4, 1, 2, 4, 4, 8 +}; + +const PLY::RuleName PLY::rule_name_list[] = { + {AVERAGE_RULE, "avg"}, + {RANDOM_RULE, "rnd"}, + {MINIMUM_RULE, "max"}, + {MAXIMUM_RULE, "min"}, + {MAJORITY_RULE, "major"}, + {SAME_RULE, "same"}, + {-1, "end_marker"} +}; + + + +/******************/ +/* Construction */ +/******************/ + + +/****************************************************************************** +Init PLY data as empty. + +Entry: + +Exit: +******************************************************************************/ + +PLY::PLY() + : + which_elem(NULL), other_elems(NULL), current_rules(NULL), rule_list(NULL), + istream(NULL), mfp(NULL), write_type_names(type_names) +{ +} + +PLY::~PLY() +{ + release(); +} + + +/****************************************************************************** +Free the memory used by a PLY file. +******************************************************************************/ + +void PLY::release() +{ + if (mfp) { + delete mfp; + mfp = NULL; + ostream = NULL; + } else + if (istream) { + if (static_cast(istream)->getInputStream(ISTREAM::LAYER_ID_IN)) { + IOSTREAM* const iostream(static_cast(istream)->getIOStream(ISTREAM::LAYER_ID_IN, OSTREAM::LAYER_ID_OUT)); + if (iostream) + delete iostream; + else + delete istream; + } else { + ASSERT(static_cast(ostream)->getOutputStream(OSTREAM::LAYER_ID_OUT)); + IOSTREAM* const iostream(static_cast(ostream)->getIOStream(ISTREAM::LAYER_ID_IN, OSTREAM::LAYER_ID_OUT)); + if (iostream) + delete iostream; + else + delete ostream; + } + istream = NULL; + } + if (!elems.empty()) { + for (size_t i=0; iprops.empty()) { + for (size_t j=0; jprops.size(); ++j) + delete elem->props[j]; + elem->props.clear(); + elem->store_prop.clear(); + } + delete elem; + } + elems.clear(); + } + if (other_elems) { + for (size_t i=0; iother_list.size(); ++i) { + OtherElem& elem = other_elems->other_list[i]; + delete[] elem.other_data; + delete elem.other_props; + } + delete other_elems; other_elems = NULL; + } + comments.clear(); + obj_info.clear(); + vals.clear(); +} + + + +/*************/ +/* Writing */ +/*************/ + + +/****************************************************************************** +Given a file pointer, get ready to write PLY data to the file. + +Entry: +fp - the given file pointer +nelems - number of elements in object +elem_names - list of element names +file_type - file type, either ascii or binary +memBufferSize - memory file initial size (useful if the ply size is unknown) + +Exit: +returns a pointer to a PlyFile, used to refer to this file, or NULL if error +******************************************************************************/ + +bool PLY::write(LPCSTR _filename, int nelems, LPCSTR* elem_names, int _file_type, size_t memBufferSize) +{ + filename = _filename; + if (memBufferSize == 0) { + // create output file now + File* const pf(new File(filename.c_str(), File::WRITE, File::CREATE | File::TRUNCATE)); + if (!pf->isOpen()) + return false; + return write(new BufferedOutputStream(pf, 64*1024), nelems, elem_names, _file_type, memBufferSize); + } + return write((OSTREAM*)NULL, nelems, elem_names, _file_type, memBufferSize); +} + +bool PLY::write(OSTREAM* fp, int nelems, LPCSTR* elem_names, int _file_type, size_t memBufferSize) +{ + // create a record for this object + file_type = _file_type; + version = 1.0; + other_elems = NULL; + + if (memBufferSize > 0) { + // write ply into a memory buffer, and save it to disk at the end; + // useful in case the number of ply elements is not know from the start + // in order to avoid an additional file copy operation + ASSERT(fp == NULL && !filename.empty()); + mfp = new MemFile(memBufferSize); + ostream = mfp; + } else { + // directly write ply data to disk; + // in case the number of ply elements is unknown from the start, + // they are written into a temporary file and the header is written + // at the end using a file copy operation + ASSERT(fp != NULL); + ostream = fp; + mfp = NULL; + } + + // tuck aside the names of the elements + elems.resize(nelems); + for (int i = 0; i < nelems; ++i) { + PlyElement* elem = new PlyElement; + elem->name = elem_names[i]; + elem->num = 0; + elems[i] = elem; + } + return true; +} + + +/****************************************************************************** +Describe an element, including its properties and how many will be written +to the file. + +Entry: +elem_name - name of element that information is being specified about +nelems - number of elements of this type to be written +nprops - number of properties contained in the element +prop_list - list of properties +******************************************************************************/ + +void PLY::element_layout( + const char* elem_name, + int nelems, + int nprops, + PlyProperty* prop_list + ) +{ + // look for appropriate element + PlyElement *elem = find_element(elem_name); + if (elem == NULL) + abort_ply("error: element_layout: can't find element '%s'", elem_name); + elem->num = nelems; + + // copy the list of properties + elem->props.resize(nprops); + elem->store_prop.resize(nprops); + + for (int i = 0; i < nprops; ++i) { + PlyProperty *prop = new PlyProperty; + elem->props[i] = prop; + elem->store_prop[i] = NAMED_PROP; + copy_property(*prop, prop_list[i]); + } +} + + +/****************************************************************************** +Describe a property of an element. + +Entry: +elem_name - name of element that information is being specified about +prop - the new property +******************************************************************************/ + +void PLY::describe_property(const char* elem_name, const PlyProperty& prop) +{ + // look for appropriate element + put_element_setup(elem_name); + + // describe property + describe_property(prop); +} + +void PLY::describe_property(const char* elem_name, int nprops, const PlyProperty* props) +{ + // look for appropriate element + put_element_setup(elem_name); + + // describe properties + for (int i=0; inum = nelems; +} + + +/****************************************************************************** +Signal that we've described everything a PLY file's header and that the +header should be written to the file. + +This function can be also called at the end, in which case the file is close, +the header is written in a new file and the already written content is copied +from the temporary file. +******************************************************************************/ + +bool PLY::header_complete() +{ + std::string filenameTmp; + if (mfp != NULL) { + // ply data was written into a memory buffer, + // now write the header to disk and append the data in memory + ostream = NULL; + } else if (!filename.empty() && ostream->getPos() > 0) { + // close this file, rename it, and open a new file to write the header + delete ostream; ostream = NULL; + filenameTmp = filename+".tmp"; + if (!File::renameFile(filename.c_str(), filenameTmp.c_str())) + return false; + } + if (ostream == NULL) { + File* const pf(new File(filename.c_str(), File::WRITE, File::CREATE | File::TRUNCATE)); + if (!pf->isOpen()) + return false; + ostream = new BufferedOutputStream(pf, 64*1024); + } + + // write header + ostream->print("ply\n"); + + switch (file_type) { + case ASCII: + ostream->print("format ascii 1.0\n"); + break; + case BINARY_BE: + ostream->print("format binary_big_endian 1.0\n"); + break; + case BINARY_LE: + ostream->print("format binary_little_endian 1.0\n"); + break; + default: + abort_ply("error: ply_header_complete: bad file type = %d\n", file_type); + } + + // write out the comments + for (size_t i = 0; i < comments.size(); ++i) + ostream->print("comment %s\n", comments[i].c_str()); + + // write out object information + for (size_t i = 0; i < obj_info.size(); ++i) + ostream->print("obj_info %s\n", obj_info[i].c_str()); + + // write out information about each element + for (size_t i = 0; i < elems.size(); ++i) { + PlyElement *elem = elems[i]; + ASSERT(elem->num > 0); + ostream->print("element %s %d\n", elem->name.c_str(), elem->num); + + // write out each property + for (size_t j = 0; j < elem->props.size(); ++j) { + PlyProperty *prop = elem->props[j]; + if (prop->is_list == LIST) { + ostream->print("property list "); + write_scalar_type(prop->count_external); + ostream->print(" "); + write_scalar_type(prop->external_type); + ostream->print(" %s\n", prop->name.c_str()); + } else if (prop->is_list == STRING) { + ostream->print("property string"); + ostream->print(" %s\n", prop->name.c_str()); + } else { + ostream->print("property "); + write_scalar_type(prop->external_type); + ostream->print(" %s\n", prop->name.c_str()); + } + } + } + + ostream->print("end_header\n"); + + if (mfp != NULL) { + // now write the ply data from memory to disk + ostream->write(mfp->getBuffer(), mfp->getSize()); + delete mfp; mfp = NULL; + } else if (!filenameTmp.empty()) { + // append the body of the ply from the temp file, and delete it + File ftmp(filenameTmp.c_str(), File::READ, File::OPEN); + if (!ftmp.isOpen()) + return false; + Unsigned8Arr buffer(256 * 1024); + size_t len; + while ((len = ftmp.read(buffer.data(), buffer.size())) > 0) + ostream->write(buffer.data(), len); + ftmp.close(); + File::deleteFile(filenameTmp.c_str()); + } else { + // element writing is starting next, reset counters + for (size_t i = 0; i < elems.size(); ++i) + elems[i]->num = 0; + } + return true; +} + + +/****************************************************************************** +Specify which elements are going to be written. This should be called +before a call to the routine ply_put_element(). + +Entry: +elem_name - name of element we're talking about +******************************************************************************/ + +void PLY::put_element_setup(const char* elem_name) +{ + PlyElement *elem = find_element(elem_name); + if (elem == NULL) + abort_ply("error: put_element_setup: can't find element '%s'", elem_name); + which_elem = elem; +} + + +/****************************************************************************** +Write an element to the file. This routine assumes that we're +writing the type of element specified in the last call to the routine +put_element_setup(). + +Entry: +elem_ptr - pointer to the element +******************************************************************************/ + +void PLY::put_element(const void* elem_ptr) +{ + char *item; + char *elem_data; + char **item_ptr; + ValueType val; + char **other_ptr; + + PlyElement *elem = which_elem; + elem_data = (char*)elem_ptr; + other_ptr = (char**)(elem_data + elem->other_offset); + + // write out either to an ascii or binary file + if (file_type == ASCII) { + + // write an ascii file + + // write out each property of the element + for (size_t j = 0; j < elem->props.size(); ++j) { + + PlyProperty *prop = elem->props[j]; + + if (elem->store_prop[j] == OTHER_PROP) + elem_data = *other_ptr; + else + elem_data = (char*)elem_ptr; + + switch (prop->is_list) { + case SCALAR: { // scalar + item = elem_data + prop->offset; + get_stored_item((void*)item, prop->internal_type, val); + write_ascii_item(val, prop->internal_type, prop->external_type); + break; + } + case LIST: { // list + item = elem_data + prop->count_offset; + get_stored_item((void*)item, prop->count_internal, val); + write_ascii_item(val, prop->count_internal, prop->count_external); + const int list_count(ValueType2Type(val, prop->count_external)); + item_ptr = (char**)(elem_data + prop->offset); + item = item_ptr[0]; + const int item_size = ply_type_size[prop->internal_type]; + for (int k = 0; k < list_count; k++) { + get_stored_item((void*)item, prop->internal_type, val); + write_ascii_item(val, prop->internal_type, prop->external_type); + item += item_size; + } + break; + } + case STRING: { // string + item = elem_data + prop->offset; + char** str = (char**)item; + ostream->print("\"%s\"", *str); + break; + } + default: + abort_ply("error: put_element: bad type = %d", prop->is_list); + } + } + + ostream->print("\n"); + } else { + + // write a binary file + + // write out each property of the element + for (size_t j = 0; j < elem->props.size(); ++j) { + PlyProperty *prop = elem->props[j]; + if (elem->store_prop[j] == OTHER_PROP) + elem_data = *other_ptr; + else + elem_data = (char*)elem_ptr; + switch (prop->is_list) { + case SCALAR: { // scalar + item = elem_data + prop->offset; + get_stored_item((void*)item, prop->internal_type, val); + write_binary_item(val, prop->internal_type, prop->external_type); + break; + } + case LIST: { // list + item = elem_data + prop->count_offset; + int item_size = ply_type_size[prop->count_internal]; + get_stored_item((void*)item, prop->count_internal, val); + write_binary_item(val, prop->count_internal, prop->count_external); + const int list_count(ValueType2Type(val, prop->count_external)); + item_ptr = (char**)(elem_data + prop->offset); + item = item_ptr[0]; + item_size = ply_type_size[prop->internal_type]; + for (int k = 0; k < list_count; k++) { + get_stored_item((void*)item, prop->internal_type, val); + write_binary_item(val, prop->internal_type, prop->external_type); + item += item_size; + } + break; + } + case STRING: { // string + item = elem_data + prop->offset; + char** str = (char**)item; + + // write the length + const int len = (int)_tcslen(*str) + 1; + ostream->write(&len, sizeof(int)); + + // write the string, including the null character + ostream->write(*str, len); + break; + } + default: + abort_ply("error: put_element: bad type = %d", prop->is_list); + } + } + } + + // count element items + elem->num++; +} + + + +/*************/ +/* Reading */ +/*************/ + + +/****************************************************************************** +Given a file pointer, get ready to read PLY data from the file. + +Entry: +fp - the given file pointer + +Exit: +nelems - number of elements in object +elem_names - list of element names +returns a pointer to a PlyFile, used to refer to this file, or NULL if error +******************************************************************************/ + +bool PLY::read(LPCSTR _filename) +{ + filename = _filename; + File* const pf(new File(_filename, File::READ, File::OPEN)); + if (!pf->isOpen()) + return false; + return read(new BufferedInputStream(pf, 64*1024)); +} + +bool PLY::read(ISTREAM* fp) +{ + // create record for this object + ASSERT(elems.empty()); + other_elems = NULL; + rule_list = NULL; + istream = fp; + + // read and parse the file's header + int nwords; + char *orig_line; + STRISTREAM sfp(istream); + char **words = get_words(sfp, &nwords, &orig_line); + if (words == NULL) + return false; + if (!equal_strings(words[0], "ply")) { + free(words); + return false; + } + free(words); + + // parse words + while ((words = get_words(sfp, &nwords, &orig_line)) != NULL) { + if (equal_strings(words[0], "format")) { + if (nwords != 3) + return false; + if (equal_strings(words[1], "ascii")) + file_type = ASCII; + else if (equal_strings(words[1], "binary_big_endian")) + file_type = BINARY_BE; + else if (equal_strings(words[1], "binary_little_endian")) + file_type = BINARY_LE; + else + return false; + version = (float)atof(words[2]); + } else if (equal_strings(words[0], "element")) + add_element((const char**)words, nwords); + else if (equal_strings(words[0], "property")) + add_property((const char**)words, nwords); + else if (equal_strings(words[0], "comment")) + add_comment(orig_line); + else if (equal_strings(words[0], "obj_info")) + add_obj_info(orig_line); + else if (equal_strings(words[0], "end_header")) { + free(words); + break; + } + free(words); + } + sfp.emptyBuffer(); + + // create tags for each property of each element, to be used + // later to say whether or not to store each property for the user + for (size_t i = 0; i < elems.size(); ++i) { + PlyElement *elem = elems[i]; + elem->store_prop.resize(elem->props.size()); + for (size_t j = 0; j < elem->props.size(); ++j) + elem->store_prop[j] = DONT_STORE_PROP; + elem->other_offset = NO_OTHER_PROPS; // no "other" props by default + } + return true; +} + + +/****************************************************************************** +Get information about a particular element. + +Entry: +elem_name - name of element to get information about + +Exit: +props - the list of properties returned +returns number of elements of this type in the file +******************************************************************************/ + +int PLY::get_element_description(const char* elem_name, std::vector& prop_list) const +{ + // find information about the element + PlyElement *elem = find_element(elem_name); + if (elem == NULL) + return 0; + + // make a copy of the element's property list + prop_list.resize(elem->props.size()); + for (size_t i = 0; i < elem->props.size(); ++i) { + PlyProperty *prop = new PlyProperty; + copy_property(*prop, *elem->props[i]); + prop_list[i] = prop; + } + + return elem->num; +} + + +/****************************************************************************** +Specify which properties of an element are to be returned. This should be +called before a call to the routine get_element(). + +Entry: +elem_name - which element we're talking about +nprops - number of properties +prop_list - list of properties +******************************************************************************/ + +void PLY::get_element_setup( + const char* elem_name, + int nprops, + PlyProperty* prop_list + ) +{ + // find information about the element + PlyElement *elem = find_element(elem_name); + which_elem = elem; + + // deposit the property information into the element's description + for (int i = 0; i < nprops; ++i) { + // look for actual property + int index = find_property(elem, prop_list[i].name.c_str()); + if (index == -1) { + DEBUG("warning: Can't find property '%s' in element '%s'", prop_list[i].name.c_str(), elem_name); + continue; + } + + // store its description + PlyProperty *prop = elem->props[index]; + prop->internal_type = prop_list[i].internal_type; + prop->offset = prop_list[i].offset; + prop->count_internal = prop_list[i].count_internal; + prop->count_offset = prop_list[i].count_offset; + + // specify that the user wants this property + elem->store_prop[index] = STORE_PROP; + } +} + + +/****************************************************************************** +Specify a property of an element that is to be returned. This should be +called (usually multiple times) before a call to the routine get_element(). +This routine should be used in preference to the less flexible old routine +called ply_get_element_setup(). + +Entry: +elem_name - which element we're talking about +prop - property to add to those that will be returned +******************************************************************************/ + +void PLY::get_property(const char* elem_name, PlyProperty* prop) +{ + // find information about the element + PlyElement *elem = find_element(elem_name); + which_elem = elem; + + // deposit the property information into the element's description + int index = find_property(elem, prop->name.c_str()); + if (index == -1) { + DEBUG("warning: Can't find property '%s' in element '%s'", prop->name.c_str(), elem_name); + return; + } + PlyProperty *prop_ptr = elem->props[index]; + prop_ptr->internal_type = prop->internal_type; + prop_ptr->offset = prop->offset; + prop_ptr->count_internal = prop->count_internal; + prop_ptr->count_offset = prop->count_offset; + + // specify that the user wants this property + elem->store_prop[index] = STORE_PROP; +} + + +/****************************************************************************** +Read one element from the file. This routine assumes that we're reading +the type of element specified in the last call to the routine +ply_get_element_setup(). + +Entry: +elem_ptr - pointer to location where the element information should be put +******************************************************************************/ + +void PLY::get_element(void* elem_ptr) +{ + if (file_type == ASCII) + ascii_get_element((uint8_t*)elem_ptr); + else + binary_get_element((uint8_t*)elem_ptr); +} + + +/****************************************************************************** +Extract the comments from the header information of a PLY file. + +Entry: + +Exit: +returns the list of comments +******************************************************************************/ + +std::vector& PLY::get_comments() +{ + return comments; +} + + +/****************************************************************************** +Extract the object information (arbitrary text) from the header information +of a PLY file. + +Entry: + +Exit: +returns the list of object info lines +******************************************************************************/ + +std::vector& PLY::get_obj_info() +{ + return obj_info; +} + + +/****************************************************************************** +Make ready for "other" properties of an element-- those properties that +the user has not explicitly asked for, but that are to be stashed away +in a special structure to be carried along with the element's other +information. + +Entry: +elem - element for which we want to save away other properties +******************************************************************************/ + +void PLY::setup_other_props(PlyElement* elem) +{ + int size = 0; + + // Examine each property in decreasing order of size. + // We do this so that all data types will be aligned by + // word, half-word, or whatever within the structure. + for (int type_size = 8; type_size > 0; type_size /= 2) { + + // add up the space taken by each property, and save this information + // away in the property descriptor + for (size_t i = 0; i < elem->props.size(); ++i) { + + // don't bother with properties we've been asked to store explicitly + if (elem->store_prop[i]) + continue; + + PlyProperty *prop = elem->props[i]; + + // internal types will be same as external + prop->internal_type = prop->external_type; + prop->count_internal = prop->count_external; + + // list case + if (prop->is_list == LIST) { + + // pointer to list + if (type_size == sizeof(void *)) { + prop->offset = size; + size += sizeof(void *); // always use size of a pointer here + } + + // count of number of list elements + if (type_size == ply_type_size[prop->count_external]) { + prop->count_offset = size; + size += ply_type_size[prop->count_external]; + } + } + // string + else if (prop->is_list == STRING) { + // pointer to string + if (type_size == sizeof(char*)) { + prop->offset = size; + size += sizeof(char*); + } + } + // scalar + else if (type_size == ply_type_size[prop->external_type]) { + prop->offset = size; + size += ply_type_size[prop->external_type]; + } + } + + } + + // save the size for the other_props structure + elem->other_size = size; +} + + +/****************************************************************************** +Specify that we want the "other" properties of an element to be tucked +away within the user's structure. + +Entry: +elem - the element that we want to store other_props in +offset - offset to where other_props will be stored inside user's structure + +Exit: +returns pointer to structure containing description of other_props +******************************************************************************/ + +PLY::PlyOtherProp* PLY::get_other_properties(PlyElement* elem, int offset) +{ + // remember that this is the "current" element + which_elem = elem; + + // save the offset to where to store the other_props + elem->other_offset = offset; + + // place the appropriate pointers, etc. in the element's property list + setup_other_props(elem); + + // create structure for describing other_props + PlyOtherProp *other = new PlyOtherProp; + other->name = elem->name; + #if 0 + if (elem->other_offset == NO_OTHER_PROPS) { + other->size = 0; + other->props = NULL; + other->nprops = 0; + return other; + } + #endif + other->size = elem->other_size; + other->props.reserve(elem->props.size()); + + // save descriptions of each "other" property + for (size_t i = 0; i < elem->props.size(); ++i) { + if (elem->store_prop[i]) + continue; + PlyProperty *prop = new PlyProperty; + copy_property(*prop, *elem->props[i]); + other->props.push_back(prop); + } + + // set other_offset pointer appropriately if there are NO other properties + if (other->props.empty()) + elem->other_offset = NO_OTHER_PROPS; + + // return structure + return other; +} + + +/****************************************************************************** +Specify that we want the "other" properties of an element to be tucked +away within the user's structure. The user needn't be concerned for how +these properties are stored. + +Entry: +elem_name - name of element that we want to store other_props in +offset - offset to where other_props will be stored inside user's structure + +Exit: +returns pointer to structure containing description of other_props +******************************************************************************/ + +PLY::PlyOtherProp* PLY::get_other_properties(const char* elem_name, int offset) +{ + // find information about the element + PlyElement *elem = find_element(elem_name); + if (elem == NULL) { + DEBUG("warning: get_other_properties: Can't find element '%s'", elem_name); + return NULL; + } + + PlyOtherProp *other = get_other_properties(elem, offset); + return other; +} + + + + +/*************************/ +/* Other Element Stuff */ +/*************************/ + + + + + +/****************************************************************************** +Grab all the data for the current element that a user does not want to +explicitly read in. Stores this in the PLY object's data structure. + +Entry: + +Exit: +returns pointer to ALL the "other" element data for this PLY file +******************************************************************************/ + +PLY::PlyOtherElems* PLY::get_other_element() +{ + PlyElement *elem = which_elem; + + // create room for the new "other" element, initializing the + // other data structure if necessary + OtherElem other; + + // count of element instances in file + other.elem_count = elem->num; + + // save name of element + other.elem_name = elem->name; + + // create a list to hold all the current elements + other.other_data = new OtherData*[other.elem_count]; + + // set up for getting elements + other.other_props = get_other_properties(elem->name.c_str(), offsetof(OtherData,other_props)); + + // grab all these elements + for (int i = 0; i < other.elem_count; ++i) { + // grab and element from the file + other.other_data[i] = new OtherData; + get_element((uint8_t*)other.other_data[i]); + } + + // return pointer to the other elements data + if (other_elems == NULL) + other_elems = new PlyOtherElems; + other_elems->other_list.push_back(other); + return other_elems; +} + + +/****************************************************************************** +Write out the "other" elements specified for this PLY file. + +Entry: +******************************************************************************/ + +void PLY::put_other_elements() +{ + // make sure we have other elements to write + if (other_elems == NULL) + return; + + // write out the data for each "other" element + for (size_t i = 0; i < other_elems->other_list.size(); ++i) { + OtherElem *other = &(other_elems->other_list[i]); + put_element_setup(other->elem_name.c_str()); + + // write out each instance of the current element + for (int j = 0; j < other->elem_count; ++j) + put_element(other->other_data[j]); + } +} + + + +/*******************/ +/* Miscellaneous */ +/*******************/ + + +/****************************************************************************** +Use old PLY type names during writing for backward compatibility. +******************************************************************************/ + +void PLY::set_legacy_type_names() +{ + write_type_names = old_type_names; +} + + +/****************************************************************************** +Get version number and file type of a PlyFile. + +Entry: + +Exit: +version - version of the file +file_type - PLY_ASCII, PLY_BINARY_BE, or PLY_BINARY_LE +******************************************************************************/ + +void PLY::get_info(float* _version, int* _file_type) +{ + *_version = version; + *_file_type = file_type; +} + + +/****************************************************************************** +Find an element from the element list of a given PLY object. + +Entry: +element - name of element we're looking for + +Exit: +returns the element, or NULL if not found +******************************************************************************/ + +PLY::PlyElement* PLY::find_element(const char* element) const +{ + for (size_t i=0; iname.c_str())) + return elems[i]; + return NULL; +} + + +/****************************************************************************** +Find a property in the list of properties of a given element. + +Entry: +elem - pointer to element in which we want to find the property +prop_name - name of property to find + +Exit: +returns the index to position in list +******************************************************************************/ + +int PLY::find_property(PlyElement* elem, const char* prop_name) const +{ + for (size_t i=0; iprops.size(); ++i) + if (equal_strings(prop_name, elem->props[i]->name.c_str())) + return (int)i; + return -1; +} + + +/****************************************************************************** +Read an element from an ascii file. + +Entry: +elem_ptr - pointer to element +******************************************************************************/ + +void PLY::ascii_get_element(uint8_t* elem_ptr) +{ + char *elem_data, *item; + char *item_ptr; + ValueType val; + char *orig_line; + char *other_data(NULL); + int other_flag(0); + + // the kind of element we're reading currently + PlyElement *elem = which_elem; + + // do we need to setup for other_props? + if (elem->other_offset != NO_OTHER_PROPS) { + other_flag = 1; + // make room for other_props + other_data = new char[elem->other_size]; + // store pointer in user's structure to the other_props + *((char**)(elem_ptr + elem->other_offset)) = other_data; + } + + // read in the element + int nwords; + char **words; + { + STRISTREAM sfp(istream); + words = get_words(sfp, &nwords, &orig_line); + if (words == NULL) + abort_ply("error: get_element: unexpected end of file"); + sfp.emptyBuffer(); + } + int which_word = 0; + + for (size_t j = 0; j < elem->props.size(); ++j) { + PlyProperty *prop = elem->props[j]; + const int store_it(elem->store_prop[j] | other_flag); + + // store either in the user's structure or in other_props + if (elem->store_prop[j]) + elem_data = (char*)elem_ptr; + else + elem_data = other_data; + + if (prop->is_list == LIST) { // a list + // get and store the number of items in the list + get_ascii_item(words[which_word++], prop->count_external, val); + if (store_it) { + item = elem_data + prop->count_offset; + store_item(item, prop->count_internal, val, prop->count_external); + } + + // allocate space for an array of items and store a ptr to the array + const int list_count(ValueType2Type(val, prop->count_external)); + char** store_array = (char**)(elem_data + prop->offset); + if (list_count == 0) { + if (store_it) + *store_array = NULL; + } else { + const int item_size(ply_type_size[prop->internal_type]); + + if (store_it) { + item_ptr = new char[item_size * list_count]; + item = item_ptr; + *store_array = item_ptr; + } + + // read items and store them into the array + for (int k = 0; k < list_count; k++) { + get_ascii_item(words[which_word++], prop->external_type, val); + if (store_it) { + store_item(item, prop->internal_type, val, prop->external_type); + item += item_size; + } + } + } + } else if (prop->is_list == STRING) { // a string + if (store_it) { + item = elem_data + prop->offset; + *((char**)item) = strdup(words[which_word++]); + } else { + which_word++; + } + } else { // a scalar + get_ascii_item(words[which_word++], prop->external_type, val); + if (store_it) { + item = elem_data + prop->offset; + store_item(item, prop->internal_type, val, prop->external_type); + } + } + } + + free(words); +} + + +/****************************************************************************** +Read an element from a binary file. + +Entry: +elem_ptr - pointer to an element +******************************************************************************/ + +void PLY::binary_get_element(uint8_t* elem_ptr) +{ + char *elem_data; + char *item; + char *item_ptr; + ValueType val; + char *other_data(NULL); + int other_flag(0); + + // the kind of element we're reading currently + PlyElement* elem = which_elem; + + // do we need to setup for other_props? + if (elem->other_offset != NO_OTHER_PROPS) { + other_flag = 1; + // make room for other_props + other_data = new char[elem->other_size]; + // store pointer in user's structure to the other_props + *((char**)(elem_ptr + elem->other_offset)) = other_data; + } + + // read in a number of elements + for (size_t j = 0; j < elem->props.size(); ++j) { + PlyProperty *prop = elem->props[j]; + const int store_it(elem->store_prop[j] | other_flag); + + // store either in the user's structure or in other_props + if (elem->store_prop[j]) + elem_data = (char*)elem_ptr; + else + elem_data = other_data; + + if (prop->is_list == LIST) { // list + // get and store the number of items in the list + get_binary_item(prop->count_external, val); + if (store_it) { + item = elem_data + prop->count_offset; + store_item(item, prop->count_internal, val, prop->count_external); + } + + // allocate space for an array of items and store a ptr to the array + const int list_count(ValueType2Type(val, prop->count_external)); + const int item_size(ply_type_size[prop->internal_type]); + char** store_array = (char**)(elem_data + prop->offset); + if (list_count == 0) { + if (store_it) + *store_array = NULL; + } else { + if (store_it) { + item_ptr = new char[item_size * list_count]; + item = item_ptr; + *store_array = item_ptr; + } + + // read items and store them into the array + for (int k = 0; k < list_count; k++) { + get_binary_item(prop->external_type, val); + if (store_it) { + store_item(item, prop->internal_type, val, prop->external_type); + item += item_size; + } + } + } + } else if (prop->is_list == STRING) { // string + int len; + istream->read(&len, sizeof(int)); + char *str = new char[len]; + istream->read(str, len); + if (store_it) { + item = elem_data + prop->offset; + *((char**)item) = str; + } + } else { // scalar + get_binary_item(prop->external_type, val); + if (store_it) { + item = elem_data + prop->offset; + store_item(item, prop->internal_type, val, prop->external_type); + } + } + } +} + + +/****************************************************************************** +Write to a file the word that represents a PLY data type. + +Entry: +code - code for type +******************************************************************************/ + +void PLY::write_scalar_type(int code) +{ + // make sure this is a valid code + if (code <= StartType || code >= EndType) + abort_ply("error: write_scalar_type: bad data code = %d", code); + + // write the code to a file + ostream->print("%s", write_type_names[code]); +} + + +/****************************************************************************** +Get a text line from a file and break it up into words. + +IMPORTANT: The calling routine should call "free" on the returned pointer once +finished with it. + +Entry: +sfp - string file to read from + +Exit: +nwords - number of words returned +orig_line - the original line of characters +returns a list of words from the line, or NULL if end-of-file +******************************************************************************/ + +char** PLY::get_words(STRISTREAM& sfp, int* nwords, char** orig_line) +{ + const int BIG_STRING = 4096; + char str[BIG_STRING]; + char str_copy[BIG_STRING]; + + int max_words = 10; + int num_words = 0; + char *ptr, *ptr2; + + char** words = (char**)malloc(sizeof(char*) * max_words); + + // read in a line + size_t len(sfp.readLine(str, BIG_STRING-2)); + if (len == 0 || len == STREAM_ERROR) { + *nwords = 0; + *orig_line = NULL; + free(words); + return NULL; + } + + // convert line-feed and tabs into spaces + // (this guarantees that there will be a space before the + // null character at the end of the string) + if (str[len-1] == '\r') + --len; + str[len] = '\n'; + + for (ptr = str, ptr2 = str_copy; ; ptr++, ptr2++) { + switch (*ptr) { + case '\t': + *ptr = ' '; + *ptr2 = ' '; + break; + case '\n': + *ptr = ' '; + *(ptr+1) = *ptr2 = '\0'; + goto EXIT_LOOP; + default: + *ptr2 = *ptr; + } + } + EXIT_LOOP: + + // find the words in the line + ptr = str; + while (*ptr != '\0') { + + // jump over leading spaces + while (*ptr == ' ') + ptr++; + + // break if we reach the end + if (*ptr == '\0') + break; + + // allocate more room for words if necessary + if (num_words >= max_words) { + max_words += 10; + words = (char**)realloc(words, sizeof(char*) * max_words); + } + + if (*ptr == '\"') { // a quote indicates that we have a string + // skip over leading quote + ptr++; + + // save pointer to beginning of word + words[num_words++] = ptr; + + // find trailing quote or end of line + while (*ptr != '\"' && *ptr != '\0') + ptr++; + + // replace quote with a null character to mark the end of the word + // if we are not already at the end of the line + if (*ptr != '\0') + *ptr++ = '\0'; + } else { // non-string + // save pointer to beginning of word + words[num_words++] = ptr; + + // jump over non-spaces + while (*ptr != ' ') + ptr++; + + // place a null character here to mark the end of the word + *ptr++ = '\0'; + } + } + + // return the list of words + *nwords = num_words; + *orig_line = str_copy; + return words; +} + + +/****************************************************************************** +Write out an item to a file as raw binary bytes. + +Entry: +val - item value to be written +double_val - value type +type - data type to write out +******************************************************************************/ + +void PLY::write_binary_item( + const ValueType& val, + int from_type, + int to_type + ) +{ + switch (to_type) { + case Int8: { + const int8_t v(ValueType2Type(val, from_type)); + ostream->write(&v, 1); + break; } + case Int16: { + const int16_t v(ValueType2Type(val, from_type)); + ostream->write(&v, 2); + break; } + case Int32: { + const int32_t v(ValueType2Type(val, from_type)); + ostream->write(&v, 4); + break; } + case Uint8: { + const uint8_t v(ValueType2Type(val, from_type)); + ostream->write(&v, 1); + break; } + case Uint16: { + const uint16_t v(ValueType2Type(val, from_type)); + ostream->write(&v, 2); + break; } + case Uint32: { + const uint32_t v(ValueType2Type(val, from_type)); + ostream->write(&v, 4); + break; } + case Float32: { + const float v(ValueType2Type(val, from_type)); + ostream->write(&v, 4); + break; } + case Float64: { + const double v(ValueType2Type(val, from_type)); + ostream->write(&v, 8); + break; } + default: + abort_ply("error: write_binary_item: bad type = %d", to_type); + } +} + + +/****************************************************************************** +Write out an item to a file as ascii characters. + +Entry: +val - item value to be written +double_val - value type +type - data type to write out +******************************************************************************/ + +void PLY::write_ascii_item( + const ValueType& val, + int from_type, + int to_type + ) +{ + switch (to_type) { + case Int8: + case Int16: + case Int32: + ostream->print("%d ", ValueType2Type(val, from_type)); + break; + case Uint8: + case Uint16: + case Uint32: + ostream->print("%u ", ValueType2Type(val, from_type)); + break; + case Float32: + case Float64: + ostream->print("%g ", ValueType2Type(val, from_type)); + break; + default: + abort_ply("error: write_ascii_item: bad type = %d", to_type); + } +} + + +/****************************************************************************** +Get the value of an item that is in memory, and place the result +into an integer, an unsigned integer and a double. + +Entry: +ptr - pointer to the item +type - data type supposedly in the item + +Exit: +val - extracted value +******************************************************************************/ + +void PLY::get_stored_item( + const void* ptr, + int type, + ValueType& val + ) +{ + switch (type) { + case Int8: + val.i8 = *((const int8_t*)ptr); + break; + case Uint8: + val.u8 = *((const uint8_t*)ptr); + break; + case Int16: + val.i16 = *((const int16_t*)ptr); + break; + case Uint16: + val.u16 = *((const uint16_t*)ptr); + break; + case Int32: + val.i32 = *((const int32_t*)ptr); + break; + case Uint32: + val.u32 = *((const uint32_t*)ptr); + break; + case Float32: + val.f = *((const float*)ptr); + break; + case Float64: + val.d = *((const double*)ptr); + break; + default: + abort_ply("error: get_stored_item: bad type = %d", type); + } +} + + +/****************************************************************************** +Get the value of an item from a binary file, and place the result +into an integer, an unsigned integer and a double. + +Entry: +type - data type supposedly in the word + +Exit: +val - store value +******************************************************************************/ + +void PLY::get_binary_item(int type, ValueType& val) +{ + switch (type) { + case Int8: + istream->read(&val.i8, 1); + break; + case Uint8: + istream->read(&val.u8, 1); + break; + case Int16: + istream->read(&val.i16, 2); + break; + case Uint16: + istream->read(&val.u16, 2); + break; + case Int32: + istream->read(&val.i32, 4); + break; + case Uint32: + istream->read(&val.u32, 4); + break; + case Float32: + istream->read(&val.f, 4); + break; + case Float64: + istream->read(&val.d, 8); + break; + default: + abort_ply("error: get_binary_item: bad type = %d", type); + } +} + + +/****************************************************************************** +Extract the value of an item from an ascii word, and place the result +into an integer, an unsigned integer and a double. + +Entry: +word - word to extract value from +type - data type supposedly in the word + +Exit: +val - store value +******************************************************************************/ + +void PLY::get_ascii_item(const char* word, int type, ValueType& val) +{ + switch (type) { + case Int8: + val.i8 = (int8_t)atoi(word); + break; + case Uint8: + val.u8 = (uint8_t)atoi(word); + break; + case Int16: + val.i16 = (int16_t)atoi(word); + break; + case Uint16: + val.u16 = (uint16_t)atoi(word); + break; + case Int32: + val.i32 = atoi(word); + break; + case Uint32: + val.u32 = strtoul(word, (char**)NULL, 10); + break; + case Float32: + val.f = (float)atof(word); + break; + case Float64: + val.d = atof(word); + break; + default: + abort_ply("error: get_ascii_item: bad type = %d", type); + } +} + + +/****************************************************************************** +Store a value into a place being pointed to, guided by a data type. + +Entry: +to_type - data type +val - value to be stored +from_type - value type + +Exit: +ptr - data pointer to stored value +******************************************************************************/ + +void PLY::store_item( + void* ptr, + int to_type, + const ValueType& val, + int from_type + ) +{ + switch (to_type) { + case Int8: + *((int8_t*)ptr) = ValueType2Type(val, from_type); + break; + case Uint8: + *((uint8_t*)ptr) = ValueType2Type(val, from_type); + break; + case Int16: + *((int16_t*)ptr) = ValueType2Type(val, from_type); + break; + case Uint16: + *((uint16_t*)ptr) = ValueType2Type(val, from_type); + break; + case Int32: + *((int32_t*)ptr) = ValueType2Type(val, from_type); + break; + case Uint32: + *((uint32_t*)ptr) = ValueType2Type(val, from_type); + break; + case Float32: + *((float*)ptr) = ValueType2Type(val, from_type); + break; + case Float64: + *((double*)ptr) = ValueType2Type(val, from_type); + break; + default: + abort_ply("error: store_item: bad type = %d", to_type); + } +} + + +/****************************************************************************** +Add an element to a PLY file descriptor. + +Entry: +words - list of words describing the element +nwords - number of words in the list +******************************************************************************/ + +void PLY::add_element(const char** words, int /*nwords*/) +{ + // create the new element + PlyElement *elem = new PlyElement; + elem->name = words[1]; + elem->num = atoi(words[2]); + + // add the new element to the object's list + elems.push_back(elem); +} + + +/****************************************************************************** +Return the type of a property, given the name of the property. + +Entry: +name - name of property type + +Exit: +returns integer code for property, or 0 if not found +******************************************************************************/ + +int PLY::get_prop_type(const char* type_name) +{ + // try to match the type name + for (int i = StartType + 1; i < EndType; ++i) + if (equal_strings (type_name, type_names[i])) + return i; + + // see if we can match an old type name + for (int i = StartType + 1; i < EndType; ++i) + if (equal_strings (type_name, old_type_names[i])) + return i; + + // if we get here, we didn't find the type + return 0; +} + + +/****************************************************************************** +Add a property to a PLY file descriptor. + +Entry: +words - list of words describing the property +nwords - number of words in the list +******************************************************************************/ + +void PLY::add_property(const char** words, int /*nwords*/) +{ + // create the new property + PlyProperty *prop = new PlyProperty; + + if (equal_strings(words[1], "list")) { // list + prop->count_external = get_prop_type (words[2]); + prop->external_type = get_prop_type (words[3]); + prop->name = words[4]; + prop->is_list = LIST; + } else if (equal_strings(words[1], "string")) { // string + prop->count_external = Int8; + prop->external_type = Int8; + prop->name = words[2]; + prop->is_list = STRING; + } else { // scalar + prop->external_type = get_prop_type (words[1]); + prop->name = words[2]; + prop->is_list = SCALAR; + } + + // internal types are the same as external by default + prop->internal_type = prop->external_type; + prop->count_internal = prop->count_external; + + // add this property to the list of properties of the current element + PlyElement *elem = elems.back(); + elem->props.push_back(prop); +} + + +/****************************************************************************** +Add a comment to a PLY file descriptor. + +Entry: +line - line containing comment +******************************************************************************/ + +void PLY::add_comment(const char* line) +{ + // skip over "comment" and leading spaces and tabs + int i = 7; + while (line[i] == ' ' || line[i] == '\t') + i++; + append_comment(&line[i]); +} + + +/****************************************************************************** +Add a some object information to a PLY file descriptor. + +Entry: +line - line containing text info +******************************************************************************/ + +void PLY::add_obj_info(const char* line) +{ + // skip over "obj_info" and leading spaces and tabs + int i = 8; + while (line[i] == ' ' || line[i] == '\t') + i++; + append_obj_info(&line[i]); +} + + +/****************************************************************************** +Copy a property. +******************************************************************************/ + +void PLY::copy_property(PlyProperty& dest, const PlyProperty& src) +{ + dest.name = src.name; + dest.external_type = src.external_type; + dest.internal_type = src.internal_type; + dest.offset = src.offset; + + dest.is_list = src.is_list; + dest.count_external = src.count_external; + dest.count_internal = src.count_internal; + dest.count_offset = src.count_offset; +} + + + +/****************************************************************************** +Return a list of the names of the elements in a particular PLY file. + +Entry: + +Exit: +elem_names - the list of element names +returns the number of elements +******************************************************************************/ + +int PLY::get_element_list(std::vector& elem_names) const +{ + // create the list of element names + elem_names.resize(elems.size()); + for (size_t i = 0; i < elems.size(); ++i) + elem_names[i] = elems[i]->name; + // return the number of elements and the list of element names + return (int)elems.size(); +} + + +/****************************************************************************** +Append a comment to a PLY file. + +Entry: +comment - the comment to append +******************************************************************************/ + +void PLY::append_comment(const char* comment) +{ + // add comment to list + comments.push_back(comment); +} + + +/****************************************************************************** +Copy the comments from one PLY file to another. + +Entry: +out_ply - destination file to copy comments to +in_ply - the source of the comments +******************************************************************************/ + +void PLY::copy_comments(const PLY& in_ply) +{ + for (size_t i = 0; i < in_ply.comments.size(); ++i) + append_comment(in_ply.comments[i].c_str()); +} + + +/****************************************************************************** +Append object information (arbitrary text) to a PLY file. + +Entry: +obj_info - the object info to append +******************************************************************************/ + +void PLY::append_obj_info(const char* _obj_info) +{ + // add info to list + obj_info.push_back(_obj_info); +} + + +/****************************************************************************** +Copy the object information from one PLY file to another. + +Entry: +out_ply - destination file to copy object information to +in_ply - the source of the object information +******************************************************************************/ + +void PLY::copy_obj_info(const PLY& in_ply) +{ + for (size_t i = 0; i < in_ply.obj_info.size(); ++i) + append_obj_info(in_ply.obj_info[i].c_str()); +} + + +/****************************************************************************** +Specify the index of the next element to be read in from a PLY file. + +Entry: +index - index of the element to be read + +Exit: +elem_count - the number of elements in the file +returns pointer to the name of this next element +******************************************************************************/ + +LPCSTR PLY::setup_element_read(int index, int* elem_count) +{ + if ((size_t)index > elems.size()) { + DEBUG("warning: No element with index %d", index); + return 0; + } + + PlyElement* elem = elems[index]; + + // set this to be the current element + which_elem = elem; + + // return the number of such elements in the file and the element's name + *elem_count = elem->num; + return elem->name.c_str(); +} + + +/****************************************************************************** +Specify one of several properties of the current element that is to be +read from a file. This should be called (usually multiple times) before a +call to the routine get_element(). + +Entry: +prop - property to add to those that will be returned +******************************************************************************/ + +void PLY::setup_property(const PlyProperty& prop) +{ + PlyElement *elem = which_elem; + + // deposit the property information into the element's description + int index = find_property(elem, prop.name.c_str()); + if (index == -1) { + DEBUG("warning: Can't find property '%s' in element '%s'", prop.name.c_str(), elem->name.c_str()); + return; + } + PlyProperty *prop_ptr = elem->props[index]; + prop_ptr->internal_type = prop.internal_type; + prop_ptr->offset = prop.offset; + prop_ptr->count_internal = prop.count_internal; + prop_ptr->count_offset = prop.count_offset; + + // specify that the user wants this property + elem->store_prop[index] = STORE_PROP; +} + + +/****************************************************************************** +Specify that we want the "other" properties of the current element to be tucked +away within the user's structure. + +Entry: +offset - offset to where other_props will be stored inside user's structure + +Exit: +returns pointer to structure containing description of other_props +******************************************************************************/ + +PLY::PlyOtherProp* PLY::get_other_properties(int offset) +{ + return get_other_properties(which_elem, offset); +} + + +/****************************************************************************** +Describe which element is to be written next and state how many of them will +be written. + +Entry: +elem_name - name of element that information is being described +nelems - number of elements of this type to be written +******************************************************************************/ + +void PLY::describe_element(const char* elem_name, int nelems) +{ + // look for appropriate element + PlyElement *elem = find_element(elem_name); + if (elem == NULL) + abort_ply("error: describe_element: can't find element '%s'",elem_name); + + elem->num = nelems; + + // now this element is the current element + which_elem = elem; +} + + +/****************************************************************************** +Describe a property of an element. + +Entry: +prop - the new property +******************************************************************************/ + +void PLY::describe_property(const PlyProperty& prop) +{ + PlyElement *elem = which_elem; + + // copy the new property + PlyProperty *elem_prop = new PlyProperty; + copy_property(*elem_prop, prop); + elem->props.push_back(elem_prop); + elem->store_prop.push_back(NAMED_PROP); +} + + +/****************************************************************************** +Describe what the "other" properties are that are to be stored, and where +they are in an element. +******************************************************************************/ + +void PLY::describe_other_properties( + PlyOtherProp *other, + int offset + ) +{ + // look for appropriate element + PlyElement *elem = find_element(other->name.c_str()); + if (elem == NULL) { + DEBUG("warning: describe_other_properties: can't find element '%s'", other->name.c_str()); + return; + } + + // copy the other properties + for (size_t i = 0; i < other->props.size(); ++i) { + PlyProperty *prop = new PlyProperty; + copy_property(*prop, *other->props[i]); + elem->props.push_back(prop); + elem->store_prop.push_back(OTHER_PROP); + } + + // save other info about other properties + elem->other_size = other->size; + elem->other_offset = offset; +} + + +/****************************************************************************** +Pass along a pointer to "other" elements that we want to save in a given +PLY file. These other elements were presumably read from another PLY file. + +Entry: +other_elems - info about other elements that we want to store +******************************************************************************/ + +void PLY::describe_other_elements(PlyOtherElems* _other_elems) +{ + // ignore this call if there is no other element + if (_other_elems == NULL) + return; + + // save pointer to this information + other_elems = _other_elems; + + // describe the other properties of this element + for (size_t i = 0; i < other_elems->other_list.size(); ++i) { + OtherElem *other = &(other_elems->other_list[i]); + element_count(other->elem_name.c_str(), other->elem_count); + describe_other_properties(other->other_props, offsetof(OtherData,other_props)); + } +} + + + +/****************************************************************************** +Initialize the property propagation rules for an element. Default is to +use averaging (AVERAGE_RULE) for creating all new properties. + +Entry: +elem_name - name of the element that we're making the rules for + +Exit: +returns pointer to the default rules +******************************************************************************/ + +PLY::PlyPropRules* PLY::init_rule(const char* elem_name) +{ + PlyElement *elem = find_element(elem_name); + if (elem == NULL) + abort_ply("error: init_rule: Can't find element '%s'", elem_name); + + PlyPropRules *rules = new PlyPropRules; + rules->elem = elem; + rules->max_props = 0; + rules->rule_list = NULL; + + // see if there are other rules we should use + if (elem->props.empty()) + return rules; + + // default is to use averaging rule + rules->rule_list = new int[elem->props.size()]; + for (size_t i = 0; i < elem->props.size(); ++i) + rules->rule_list[i] = AVERAGE_RULE; + + // try to match the element, property and rule name + for (PlyRuleList *list = rule_list; list != NULL; list = list->next) { + + if (!equal_strings(list->element, elem->name.c_str())) + continue; + + int found_prop = 0; + for (size_t i = 0; i < elem->props.size(); ++i) + if (equal_strings(list->property, elem->props[i]->name.c_str())) { + + found_prop = 1; + + // look for matching rule name + for (int j = 0; rule_name_list[j].code != -1; ++j) + if (equal_strings(list->name, rule_name_list[j].name.c_str())) { + rules->rule_list[i] = rule_name_list[j].code; + break; + } + } + + if (!found_prop) { + DEBUG("warning: Can't find property '%s' for rule '%s'", list->property, list->name); + continue; + } + } + + return rules; +} + + +/****************************************************************************** +Modify a property propagation rule. + +Entry: +rules - rules for the element +prop_name - name of the property whose rule we're modifying +rule_type - type of rule (MAXIMUM_RULE, MINIMUM_RULE, MAJORITY_RULE, etc.) +******************************************************************************/ + +void PLY::modify_rule(PlyPropRules* rules, const char* prop_name, int rule_type) +{ + PlyElement *elem = rules->elem; + + // find the property and modify its rule type + for (size_t i = 0; i < elem->props.size(); ++i) + if (equal_strings(elem->props[i]->name.c_str(), prop_name)) { + rules->rule_list[i] = rule_type; + return; + } + + // we didn't find the property if we get here + abort_ply("error: modify_rule: Can't find property '%s'", prop_name); +} + + +/****************************************************************************** +Begin to create a set of properties from a set of propagation rules. + +Entry: +rules - rules for the element +******************************************************************************/ + +void PLY::start_props(PlyPropRules* rules) +{ + // save pointer to the rules in the PLY object + current_rules = rules; +} + + +/****************************************************************************** +Remember a set of properties and their weights for creating a new set of +properties. + +Entry: +weight - weights for this set of properties +other_props - the properties to use +******************************************************************************/ + +void PLY::weight_props(float weight, void* other_props) +{ + PlyPropRules *rules = current_rules; + + // allocate space for properties and weights, if necessary + if (rules->max_props == 0) { + rules->max_props = 6; + } + if (rules->props.size() == rules->max_props) { + rules->max_props *= 2; + } + rules->props.reserve(rules->max_props); + rules->weights.reserve(rules->max_props); + + // remember these new properties and their weights + rules->props.push_back(other_props); + rules->weights.push_back(weight); +} + + +/****************************************************************************** +Return a pointer to a new set of properties that have been created using +a specified set of property combination rules and a given collection of +"other" properties. + +Exit: +returns a pointer to the new properties +******************************************************************************/ + +void* PLY::get_new_props() +{ + PlyPropRules *rules = current_rules; + PlyElement *elem = rules->elem; + PlyProperty *prop; + int offset; + int type; + ValueType val; + + // return NULL if we've got no "other" properties + if (elem->other_size == 0) + return NULL; + + // create room for combined other properties + char *new_data = new char[elem->other_size]; + + // make sure there is enough room to store values we're to combine + vals.resize(rules->props.size()); + + // calculate the combination for each "other" property of the element + for (size_t i = 0; i < elem->props.size(); ++i) { + + // don't bother with properties we've been asked to store explicitly + if (elem->store_prop[i]) + continue; + + prop = elem->props[i]; + offset = prop->offset; + type = prop->external_type; + + // collect together all the values we're to combine + for (size_t j = 0; j < rules->props.size(); ++j) { + char* data = (char*)rules->props[j]; + void* ptr = (void *)(data + offset); + get_stored_item((void*)ptr, type, val); + vals[j] = ValueType2Type(val, type); + } + + // calculate the combined value + switch (rules->rule_list[i]) { + case AVERAGE_RULE: { + double sum = 0; + double weight_sum = 0; + for (size_t j = 0; j < rules->props.size(); ++j) { + sum += vals[j] * rules->weights[j]; + weight_sum += rules->weights[j]; + } + val.d = sum / weight_sum; + break; + } + case MINIMUM_RULE: { + val.d = vals[0]; + for (size_t j = 1; j < rules->props.size(); ++j) + if (val.d > vals[j]) + val.d = vals[j]; + break; + } + case MAXIMUM_RULE: { + val.d = vals[0]; + for (size_t j = 1; j < rules->props.size(); ++j) + if (val.d < vals[j]) + val.d = vals[j]; + break; + } + case RANDOM_RULE: { + val.d = vals[FLOOR2INT(SEACAVE::random() * rules->props.size())]; + break; + } + case SAME_RULE: { + val.d = vals[0]; + for (size_t j = 1; j < rules->props.size(); ++j) + if (val.d != vals[j]) + abort_ply("error: get_new_props: Error combining properties that should be the same"); + break; + } + default: + abort_ply("error: get_new_props: Bad rule = %d", rules->rule_list[i]); + } + + // store the combined value + store_item(new_data + offset, type, val, Float64); + } + + return new_data; +} + + +/****************************************************************************** +Set the list of user-specified property combination rules. +******************************************************************************/ + +void PLY::set_prop_rules(PlyRuleList* prop_rules) +{ + rule_list = prop_rules; +} + + +/****************************************************************************** +Append a property rule to a growing list of user-specified rules. + +Entry: +rule_list - current rule list +name - name of property combination rule +property - "element.property" says which property the rule affects + +Exit: +returns pointer to the new rule list +******************************************************************************/ + +PLY::PlyRuleList* PLY::append_prop_rule( + PlyRuleList* rule_list, + const char* name, + const char* property + ) +{ + char *str2; + char *ptr; + + // find . + char *str = strdup(property); + for (ptr = str; *ptr != '\0' && *ptr != '.'; ptr++) ; + + // split string at . + if (*ptr == '.') { + *ptr = '\0'; + str2 = ptr + 1; + } else { + DEBUG("warning: Can't find property '%s' for rule '%s'", property, name); + return rule_list; + } + + PlyRuleList *rule = new PlyRuleList; + rule->name = name; + rule->element = str; + rule->property = str2; + rule->next = NULL; + + // either start rule list or append to it + if (rule_list == NULL) + rule_list = rule; + else { // append new rule to current list + PlyRuleList *rule_ptr = rule_list; + while (rule_ptr->next != NULL) + rule_ptr = rule_ptr->next; + rule_ptr->next = rule; + } + + // return pointer to list + return rule_list; +} + + +/****************************************************************************** +See if a name matches the name of any property combination rules. + +Entry: +name - name of rule we're trying to match + +Exit: +returns 1 if we find a match, 0 if not +******************************************************************************/ + +int PLY::matches_rule_name(const char* name) +{ + for (int i = 0; rule_name_list[i].code != -1; ++i) + if (equal_strings(rule_name_list[i].name.c_str(), name)) + return 1; + return 0; +} diff --git a/libs/IO/PLY.h b/libs/IO/PLY.h new file mode 100644 index 0000000..44a0f25 --- /dev/null +++ b/libs/IO/PLY.h @@ -0,0 +1,315 @@ +//////////////////////////////////////////////////////////////////// +// PLY.h +// +// Copyright 2023 cDc@seacave +// Distributed under the Boost Software License, Version 1.0 +// (See http://www.boost.org/LICENSE_1_0.txt) +// +// PLY polygon files parser. +// (originally by Greg Turk, heavily modified by cDc@seacave) +// +// A PLY file contains a single polygonal _object_. +// +// An object is composed of lists of _elements_. Typical elements are +// vertices, faces, edges and materials. +// +// Each type of element for a given object has one or more _properties_ +// associated with the element type. For instance, a vertex element may +// have as properties three floating-point values x,y,z and three unsigned +// chars for red, green and blue. + + +#ifndef __SEACAVE_PLY_H__ +#define __SEACAVE_PLY_H__ + + +// I N C L U D E S ///////////////////////////////////////////////// + + +// D E F I N E S /////////////////////////////////////////////////// + +#define PLY_OKAY 0 // ply routine worked okay +#define PLY_ERROR -1 // error in ply routine + + +// S T R U C T S /////////////////////////////////////////////////// + +class IO_API PLY +{ +public: + // scalar data types supported by PLY format + enum FileType { + ASCII = 1, // ascii PLY file + BINARY_BE = 2, // binary PLY file, big endian + BINARY_LE = 3, // binary PLY file, little endian + }; + enum DataType { + StartType = 0, + Int8 = 1, + Int16 = 2, + Int32 = 3, + Uint8 = 4, + Uint16 = 5, + Uint32 = 6, + Float32 = 7, + Float64 = 8, + EndType = 9, + }; + enum ListType { + SCALAR = 0, + LIST = 1, + STRING = 2, + }; + enum RuleType { + AVERAGE_RULE = 1, + MAJORITY_RULE = 2, + MINIMUM_RULE = 3, + MAXIMUM_RULE = 4, + SAME_RULE = 5, + RANDOM_RULE = 6, + }; + + // description of a property + struct PlyProperty { + std::string name; // property name + int external_type; // file's data type + int internal_type; // program's data type + int offset; // offset bytes of prop in a struct + + int is_list; // 0 = scalar, 1 = list, 2 = char string + int count_external; // file's count type + int count_internal; // program's count type + int count_offset; // offset byte for list count + }; + + // description of an element + struct PlyElement { + std::string name; // element name + int num; // number of elements in this object + int size; // size of element (bytes) or -1 if variable + std::vector props;// list of properties in the file + std::vector store_prop; // flags: property wanted by user? + int other_offset; // offset to un-asked-for props, or -1 if none + int other_size; // size of other_props structure + }; + + // describes other properties in an element + struct PlyOtherProp { + std::string name; // element name + int size; // size of other_props + std::vector props;// list of properties in other_props + }; + + // for storing other_props for an other element + struct OtherData { + void* other_props; + }; + + // data for one "other" element + struct OtherElem { + std::string elem_name; // names of other elements + int elem_count; // count of instances of each element + OtherData** other_data; // actual property data for the elements + PlyOtherProp* other_props; // description of the property data + }; + + // "other" elements, not interpreted by user + struct PlyOtherElems { + std::vector other_list;// list of data for other elements + }; + + // rules for combining "other" properties + struct PlyPropRules { + PlyElement* elem; // element whose rules we are making + int* rule_list; // types of rules (AVERAGE_PLY, MAJORITY_PLY, etc.) + uint32_t max_props; // maximum number of properties we have room for now + std::vector props; // list of properties we're combining + std::vector weights; // list of weights of the properties + }; + + struct PlyRuleList { + LPCSTR name; // name of the rule + char* element; // name of element that rule applies to + char* property; // name of property that rule applies to + struct PlyRuleList* next; // pointer for linked list of rules + }; + + // property propagation rules + struct RuleName { + int code; + std::string name; + }; + +protected: + struct ValueType { + union { + int8_t i8; + int16_t i16; + int32_t i32; + uint8_t u8; + uint16_t u16; + uint32_t u32; + float f; + double d; + }; + }; + +public: + PLY(); + ~PLY(); + + bool read(LPCSTR); + bool read(SEACAVE::ISTREAM*); + bool write(LPCSTR, int, LPCSTR*, int, size_t memBufferSize=0); + bool write(SEACAVE::OSTREAM*, int, LPCSTR*, int, size_t memBufferSize=0); + void release(); + + void set_legacy_type_names(); + + void get_info(float*, int*); + + void append_comment(const char*); + void append_obj_info(const char*); + void copy_comments(const PLY&); + void copy_obj_info(const PLY&); + std::vector& get_comments(); + std::vector& get_obj_info(); + + void describe_property(const char*, const PlyProperty&); + void describe_property(const char*, int nprops, const PlyProperty*); + void get_property(const char*, PlyProperty*); + void get_element(void*); + + PlyOtherElems* get_other_element(); + + int get_element_list(std::vector&) const; + void setup_property(const PlyProperty&); + LPCSTR setup_element_read(int, int*); + PlyOtherProp* get_other_properties(int); + + int get_elements_count() const { return (int)elems.size(); } + int get_current_element_count() const { return which_elem->num; } + void element_count(const char*, int); + void describe_element(const char*, int); + void describe_property(const PlyProperty&); + void describe_other_properties(PlyOtherProp*, int); + void describe_other_elements( PlyOtherElems*); + void get_element_setup(const char*, int, PlyProperty*); + int get_element_description(const char*, std::vector&) const; + void element_layout(const char*, int, int, PlyProperty*); + + bool header_complete(); + void put_element_setup(const char*); + void put_element(const void*); + void put_other_elements(); + + PlyPropRules* init_rule(const char*); + void modify_rule(PlyPropRules*, const char*, int); + void start_props(PlyPropRules*); + void weight_props(float, void*); + void* get_new_props(); + void set_prop_rules(PlyRuleList*); + PlyRuleList* append_prop_rule(PlyRuleList*, const char*, const char*); + + // find an element in a ply's list + PlyElement* find_element(const char*) const; + // find a property in an element's list + int find_property(PlyElement*, const char*) const; + + static inline bool equal_strings(const char* s1, const char* s2) { return _tcscmp(s1, s2) == 0; } + +protected: + // write to a file the word describing a PLY file data type + void write_scalar_type(int); + + // read a line from a file and break it up into separate words + typedef SEACAVE::TokenInputStream STRISTREAM; + char** get_words(STRISTREAM&, int*, char**); + + // write an item to a file + void write_binary_item(const ValueType&, int, int); + void write_ascii_item(const ValueType&, int, int); + + // return the value of a stored item + static void get_stored_item(const void*, int, ValueType&); + + // get binary or ascii item and store it according to ptr and type + void get_binary_item(int, ValueType&); + static void get_ascii_item(const char*, int, ValueType&); + + // store a value into where a pointer and a type specify + static void store_item(void*, int, const ValueType&, int); + + // add information to a PLY file descriptor + void add_element(const char**, int); + void add_property(const char**, int); + void add_comment(const char*); + void add_obj_info(const char*); + + // get a bunch of elements from a file + void ascii_get_element(uint8_t*); + void binary_get_element(uint8_t*); + + void setup_other_props(PlyElement*); + PlyOtherProp* get_other_properties(PlyElement*, int); + PlyOtherProp* get_other_properties(const char*, int); + + int matches_rule_name(const char*); + int get_prop_type(const char*); + + // copy a property + static void copy_property(PlyProperty&, const PlyProperty&); + + // return the value as T stored in val as type + template + static inline T ValueType2Type(const ValueType& val, int type) { + switch (type) { + case Int8: + return (T)val.i8; + case Int16: + return (T)val.i16; + case Int32: + return (T)val.i32; + case Uint8: + return (T)val.u8; + case Uint16: + return (T)val.u16; + case Uint32: + return (T)val.u32; + case Float32: + return (T)val.f; + case Float64: + return (T)val.d; + } + ASSERT("error: bad type" == NULL); + return T(0); + } + +protected: + // description of PLY file + std::string filename; // file name + int file_type; // ascii or binary + float version; // version number of file + std::vector elems; // list of elements + std::vector comments;// list of comments + std::vector obj_info;// list of object info items + PlyElement* which_elem; // element we're currently reading or writing + PlyOtherElems* other_elems; // "other" elements from a PLY file + PlyPropRules* current_rules; // current propagation rules + PlyRuleList* rule_list; // rule list from user + std::vector vals; // rule list from user + + union { + SEACAVE::ISTREAM* istream; // input file pointer + SEACAVE::OSTREAM* ostream; // output file pointer + }; + SEACAVE::MemFile* mfp; // mem-file pointer (optional) + const char* const* write_type_names; // names of scalar types to be used for writing (new types by default) + + static const char* const type_names[9]; // names of scalar types + static const char* const old_type_names[9]; // old names of types for backward compatibility + static const int ply_type_size[9]; + static const RuleName rule_name_list[7]; +}; + +#endif // __SEACAVE_PLY_H__ diff --git a/libs/IO/TinyXML2.cpp b/libs/IO/TinyXML2.cpp new file mode 100644 index 0000000..bea3414 --- /dev/null +++ b/libs/IO/TinyXML2.cpp @@ -0,0 +1,2987 @@ +/* +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#include "Common.h" +#include "TinyXML2.h" + +#include // yes, this one new style header, is in the Android SDK. +#if defined(ANDROID_NDK) || defined(__BORLANDC__) || defined(__QNXNTO__) +# include +# include +#else +# include +# include +#endif + +#if defined(_MSC_VER) && (_MSC_VER >= 1400 ) && (!defined WINCE) + // Microsoft Visual Studio, version 2005 and higher. Not WinCE. + /*int _snprintf_s( + char *buffer, + size_t sizeOfBuffer, + size_t count, + const char *format [, + argument] ... + );*/ + static inline int TIXML_SNPRINTF( char* buffer, size_t size, const char* format, ... ) + { + va_list va; + va_start( va, format ); + const int result = vsnprintf_s( buffer, size, _TRUNCATE, format, va ); + va_end( va ); + return result; + } + + static inline int TIXML_VSNPRINTF( char* buffer, size_t size, const char* format, va_list va ) + { + const int result = vsnprintf_s( buffer, size, _TRUNCATE, format, va ); + return result; + } + + #define TIXML_VSCPRINTF _vscprintf + #define TIXML_SSCANF sscanf_s +#elif defined _MSC_VER + // Microsoft Visual Studio 2003 and earlier or WinCE + #define TIXML_SNPRINTF _snprintf + #define TIXML_VSNPRINTF _vsnprintf + #define TIXML_SSCANF sscanf + #if (_MSC_VER < 1400 ) && (!defined WINCE) + // Microsoft Visual Studio 2003 and not WinCE. + #define TIXML_VSCPRINTF _vscprintf // VS2003's C runtime has this, but VC6 C runtime or WinCE SDK doesn't have. + #else + // Microsoft Visual Studio 2003 and earlier or WinCE. + static inline int TIXML_VSCPRINTF( const char* format, va_list va ) + { + int len = 512; + for (;;) { + len = len*2; + char* str = new char[len](); + const int required = _vsnprintf(str, len, format, va); + delete[] str; + if ( required != -1 ) { + TIXMLASSERT( required >= 0 ); + len = required; + break; + } + } + TIXMLASSERT( len >= 0 ); + return len; + } + #endif +#else + // GCC version 3 and higher + //#warning( "Using sn* functions." ) + #define TIXML_SNPRINTF snprintf + #define TIXML_VSNPRINTF vsnprintf + static inline int TIXML_VSCPRINTF( const char* format, va_list va ) + { + int len = vsnprintf( 0, 0, format, va ); + TIXMLASSERT( len >= 0 ); + return len; + } + #define TIXML_SSCANF sscanf +#endif + +#if defined(_WIN64) + #define TIXML_FSEEK _fseeki64 + #define TIXML_FTELL _ftelli64 +#elif defined(__APPLE__) || defined(__FreeBSD__) || defined(__ANDROID__) + #define TIXML_FSEEK fseeko + #define TIXML_FTELL ftello +#elif defined(__unix__) && defined(__x86_64__) + #define TIXML_FSEEK fseeko64 + #define TIXML_FTELL ftello64 +#else + #define TIXML_FSEEK fseek + #define TIXML_FTELL ftell +#endif + + +static const char LINE_FEED = static_cast(0x0a); // all line endings are normalized to LF +static const char LF = LINE_FEED; +static const char CARRIAGE_RETURN = static_cast(0x0d); // CR gets filtered out +static const char CR = CARRIAGE_RETURN; +static const char SINGLE_QUOTE = '\''; +static const char DOUBLE_QUOTE = '\"'; + +// Bunch of unicode info at: +// http://www.unicode.org/faq/utf_bom.html +// ef bb bf (Microsoft "lead bytes") - designates UTF-8 + +static const unsigned char TIXML_UTF_LEAD_0 = 0xefU; +static const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; +static const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + +namespace tinyxml2 +{ + +struct Entity { + const char* pattern; + int length; + char value; +}; + +static const int NUM_ENTITIES = 5; +static const Entity entities[NUM_ENTITIES] = { + { "quot", 4, DOUBLE_QUOTE }, + { "amp", 3, '&' }, + { "apos", 4, SINGLE_QUOTE }, + { "lt", 2, '<' }, + { "gt", 2, '>' } +}; + + +StrPair::~StrPair() +{ + Reset(); +} + + +void StrPair::TransferTo( StrPair* other ) +{ + if ( this == other ) { + return; + } + // This in effect implements the assignment operator by "moving" + // ownership (as in auto_ptr). + + TIXMLASSERT( other != 0 ); + TIXMLASSERT( other->_flags == 0 ); + TIXMLASSERT( other->_start == 0 ); + TIXMLASSERT( other->_end == 0 ); + + other->Reset(); + + other->_flags = _flags; + other->_start = _start; + other->_end = _end; + + _flags = 0; + _start = 0; + _end = 0; +} + + +void StrPair::Reset() +{ + if ( _flags & NEEDS_DELETE ) { + delete [] _start; + } + _flags = 0; + _start = 0; + _end = 0; +} + + +void StrPair::SetStr( const char* str, int flags ) +{ + TIXMLASSERT( str ); + Reset(); + size_t len = strlen( str ); + TIXMLASSERT( _start == 0 ); + _start = new char[ len+1 ]; + memcpy( _start, str, len+1 ); + _end = _start + len; + _flags = flags | NEEDS_DELETE; +} + + +char* StrPair::ParseText( char* p, const char* endTag, int strFlags, int* curLineNumPtr ) +{ + TIXMLASSERT( p ); + TIXMLASSERT( endTag && *endTag ); + TIXMLASSERT(curLineNumPtr); + + char* start = p; + const char endChar = *endTag; + size_t length = strlen( endTag ); + + // Inner loop of text parsing. + while ( *p ) { + if ( *p == endChar && strncmp( p, endTag, length ) == 0 ) { + Set( start, p, strFlags ); + return p + length; + } else if (*p == '\n') { + ++(*curLineNumPtr); + } + ++p; + TIXMLASSERT( p ); + } + return 0; +} + + +char* StrPair::ParseName( char* p ) +{ + if ( !p || !(*p) ) { + return 0; + } + if ( !XMLUtil::IsNameStartChar( (unsigned char) *p ) ) { + return 0; + } + + char* const start = p; + ++p; + while ( *p && XMLUtil::IsNameChar( (unsigned char) *p ) ) { + ++p; + } + + Set( start, p, 0 ); + return p; +} + + +void StrPair::CollapseWhitespace() +{ + // Adjusting _start would cause undefined behavior on delete[] + TIXMLASSERT( ( _flags & NEEDS_DELETE ) == 0 ); + // Trim leading space. + _start = XMLUtil::SkipWhiteSpace( _start, 0 ); + + if ( *_start ) { + const char* p = _start; // the read pointer + char* q = _start; // the write pointer + + while( *p ) { + if ( XMLUtil::IsWhiteSpace( *p )) { + p = XMLUtil::SkipWhiteSpace( p, 0 ); + if ( *p == 0 ) { + break; // don't write to q; this trims the trailing space. + } + *q = ' '; + ++q; + } + *q = *p; + ++q; + ++p; + } + *q = 0; + } +} + + +const char* StrPair::GetStr() +{ + TIXMLASSERT( _start ); + TIXMLASSERT( _end ); + if ( _flags & NEEDS_FLUSH ) { + *_end = 0; + _flags ^= NEEDS_FLUSH; + + if ( _flags ) { + const char* p = _start; // the read pointer + char* q = _start; // the write pointer + + while( p < _end ) { + if ( (_flags & NEEDS_NEWLINE_NORMALIZATION) && *p == CR ) { + // CR-LF pair becomes LF + // CR alone becomes LF + // LF-CR becomes LF + if ( *(p+1) == LF ) { + p += 2; + } + else { + ++p; + } + *q = LF; + ++q; + } + else if ( (_flags & NEEDS_NEWLINE_NORMALIZATION) && *p == LF ) { + if ( *(p+1) == CR ) { + p += 2; + } + else { + ++p; + } + *q = LF; + ++q; + } + else if ( (_flags & NEEDS_ENTITY_PROCESSING) && *p == '&' ) { + // Entities handled by tinyXML2: + // - special entities in the entity table [in/out] + // - numeric character reference [in] + // 中 or 中 + + if ( *(p+1) == '#' ) { + const int buflen = 10; + char buf[buflen] = { 0 }; + int len = 0; + const char* adjusted = const_cast( XMLUtil::GetCharacterRef( p, buf, &len ) ); + if ( adjusted == 0 ) { + *q = *p; + ++p; + ++q; + } + else { + TIXMLASSERT( 0 <= len && len <= buflen ); + TIXMLASSERT( q + len <= adjusted ); + p = adjusted; + memcpy( q, buf, len ); + q += len; + } + } + else { + bool entityFound = false; + for( int i = 0; i < NUM_ENTITIES; ++i ) { + const Entity& entity = entities[i]; + if ( strncmp( p + 1, entity.pattern, entity.length ) == 0 + && *( p + entity.length + 1 ) == ';' ) { + // Found an entity - convert. + *q = entity.value; + ++q; + p += entity.length + 2; + entityFound = true; + break; + } + } + if ( !entityFound ) { + // fixme: treat as error? + ++p; + ++q; + } + } + } + else { + *q = *p; + ++p; + ++q; + } + } + *q = 0; + } + // The loop below has plenty going on, and this + // is a less useful mode. Break it out. + if ( _flags & NEEDS_WHITESPACE_COLLAPSING ) { + CollapseWhitespace(); + } + _flags = (_flags & NEEDS_DELETE); + } + TIXMLASSERT( _start ); + return _start; +} + + + + +// --------- XMLUtil ----------- // + +const char* XMLUtil::writeBoolTrue = "true"; +const char* XMLUtil::writeBoolFalse = "false"; + +void XMLUtil::SetBoolSerialization(const char* writeTrue, const char* writeFalse) +{ + static const char* defTrue = "true"; + static const char* defFalse = "false"; + + writeBoolTrue = (writeTrue) ? writeTrue : defTrue; + writeBoolFalse = (writeFalse) ? writeFalse : defFalse; +} + + +const char* XMLUtil::ReadBOM( const char* p, bool* bom ) +{ + TIXMLASSERT( p ); + TIXMLASSERT( bom ); + *bom = false; + const unsigned char* pu = reinterpret_cast(p); + // Check for BOM: + if ( *(pu+0) == TIXML_UTF_LEAD_0 + && *(pu+1) == TIXML_UTF_LEAD_1 + && *(pu+2) == TIXML_UTF_LEAD_2 ) { + *bom = true; + p += 3; + } + TIXMLASSERT( p ); + return p; +} + + +void XMLUtil::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) +{ + const unsigned long BYTE_MASK = 0xBF; + const unsigned long BYTE_MARK = 0x80; + const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; + + if (input < 0x80) { + *length = 1; + } + else if ( input < 0x800 ) { + *length = 2; + } + else if ( input < 0x10000 ) { + *length = 3; + } + else if ( input < 0x200000 ) { + *length = 4; + } + else { + *length = 0; // This code won't convert this correctly anyway. + return; + } + + output += *length; + + // Scary scary fall throughs are annotated with carefully designed comments + // to suppress compiler warnings such as -Wimplicit-fallthrough in gcc + switch (*length) { + case 4: + --output; + *output = static_cast((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + //fall through + case 3: + --output; + *output = static_cast((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + //fall through + case 2: + --output; + *output = static_cast((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + //fall through + case 1: + --output; + *output = static_cast(input | FIRST_BYTE_MARK[*length]); + break; + default: + TIXMLASSERT( false ); + } +} + + +const char* XMLUtil::GetCharacterRef( const char* p, char* value, int* length ) +{ + // Presume an entity, and pull it out. + *length = 0; + + if ( *(p+1) == '#' && *(p+2) ) { + unsigned long ucs = 0; + TIXMLASSERT( sizeof( ucs ) >= 4 ); + ptrdiff_t delta = 0; + unsigned mult = 1; + static const char SEMICOLON = ';'; + + if ( *(p+2) == 'x' ) { + // Hexadecimal. + const char* q = p+3; + if ( !(*q) ) { + return 0; + } + + q = strchr( q, SEMICOLON ); + + if ( !q ) { + return 0; + } + TIXMLASSERT( *q == SEMICOLON ); + + delta = q-p; + --q; + + while ( *q != 'x' ) { + unsigned int digit = 0; + + if ( *q >= '0' && *q <= '9' ) { + digit = *q - '0'; + } + else if ( *q >= 'a' && *q <= 'f' ) { + digit = *q - 'a' + 10; + } + else if ( *q >= 'A' && *q <= 'F' ) { + digit = *q - 'A' + 10; + } + else { + return 0; + } + TIXMLASSERT( digit < 16 ); + TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit ); + const unsigned int digitScaled = mult * digit; + TIXMLASSERT( ucs <= ULONG_MAX - digitScaled ); + ucs += digitScaled; + TIXMLASSERT( mult <= UINT_MAX / 16 ); + mult *= 16; + --q; + } + } + else { + // Decimal. + const char* q = p+2; + if ( !(*q) ) { + return 0; + } + + q = strchr( q, SEMICOLON ); + + if ( !q ) { + return 0; + } + TIXMLASSERT( *q == SEMICOLON ); + + delta = q-p; + --q; + + while ( *q != '#' ) { + if ( *q >= '0' && *q <= '9' ) { + const unsigned int digit = *q - '0'; + TIXMLASSERT( digit < 10 ); + TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit ); + const unsigned int digitScaled = mult * digit; + TIXMLASSERT( ucs <= ULONG_MAX - digitScaled ); + ucs += digitScaled; + } + else { + return 0; + } + TIXMLASSERT( mult <= UINT_MAX / 10 ); + mult *= 10; + --q; + } + } + // convert the UCS to UTF-8 + ConvertUTF32ToUTF8( ucs, value, length ); + return p + delta + 1; + } + return p+1; +} + + +void XMLUtil::ToStr( int v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%d", v ); +} + + +void XMLUtil::ToStr( unsigned v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%u", v ); +} + + +void XMLUtil::ToStr( bool v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%s", v ? writeBoolTrue : writeBoolFalse); +} + +/* + ToStr() of a number is a very tricky topic. + https://github.com/leethomason/tinyxml2/issues/106 +*/ +void XMLUtil::ToStr( float v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%.8g", v ); +} + + +void XMLUtil::ToStr( double v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%.17g", v ); +} + + +void XMLUtil::ToStr( int64_t v, char* buffer, int bufferSize ) +{ + // horrible syntax trick to make the compiler happy about %lld + TIXML_SNPRINTF(buffer, bufferSize, "%lld", static_cast(v)); +} + +void XMLUtil::ToStr( uint64_t v, char* buffer, int bufferSize ) +{ + // horrible syntax trick to make the compiler happy about %llu + TIXML_SNPRINTF(buffer, bufferSize, "%llu", (long long)v); +} + +bool XMLUtil::ToInt(const char* str, int* value) +{ + if (IsPrefixHex(str)) { + unsigned v; + if (TIXML_SSCANF(str, "%x", &v) == 1) { + *value = static_cast(v); + return true; + } + } + else { + if (TIXML_SSCANF(str, "%d", value) == 1) { + return true; + } + } + return false; +} + +bool XMLUtil::ToUnsigned(const char* str, unsigned* value) +{ + if (TIXML_SSCANF(str, IsPrefixHex(str) ? "%x" : "%u", value) == 1) { + return true; + } + return false; +} + +bool XMLUtil::ToBool( const char* str, bool* value ) +{ + int ival = 0; + if ( ToInt( str, &ival )) { + *value = (ival==0) ? false : true; + return true; + } + static const char* TRUE_VALS[] = { "true", "True", "TRUE", 0 }; + static const char* FALSE_VALS[] = { "false", "False", "FALSE", 0 }; + + for (int i = 0; TRUE_VALS[i]; ++i) { + if (StringEqual(str, TRUE_VALS[i])) { + *value = true; + return true; + } + } + for (int i = 0; FALSE_VALS[i]; ++i) { + if (StringEqual(str, FALSE_VALS[i])) { + *value = false; + return true; + } + } + return false; +} + + +bool XMLUtil::ToFloat( const char* str, float* value ) +{ + if ( TIXML_SSCANF( str, "%f", value ) == 1 ) { + return true; + } + return false; +} + + +bool XMLUtil::ToDouble( const char* str, double* value ) +{ + if ( TIXML_SSCANF( str, "%lf", value ) == 1 ) { + return true; + } + return false; +} + + +bool XMLUtil::ToInt64(const char* str, int64_t* value) +{ + if (IsPrefixHex(str)) { + unsigned long long v = 0; // horrible syntax trick to make the compiler happy about %llx + if (TIXML_SSCANF(str, "%llx", &v) == 1) { + *value = static_cast(v); + return true; + } + } + else { + long long v = 0; // horrible syntax trick to make the compiler happy about %lld + if (TIXML_SSCANF(str, "%lld", &v) == 1) { + *value = static_cast(v); + return true; + } + } + return false; +} + + +bool XMLUtil::ToUnsigned64(const char* str, uint64_t* value) { + unsigned long long v = 0; // horrible syntax trick to make the compiler happy about %llu + if(TIXML_SSCANF(str, IsPrefixHex(str) ? "%llx" : "%llu", &v) == 1) { + *value = (uint64_t)v; + return true; + } + return false; +} + + +char* XMLDocument::Identify( char* p, XMLNode** node ) +{ + TIXMLASSERT( node ); + TIXMLASSERT( p ); + char* const start = p; + int const startLine = _parseCurLineNum; + p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum ); + if( !*p ) { + *node = 0; + TIXMLASSERT( p ); + return p; + } + + // These strings define the matching patterns: + static const char* xmlHeader = { "( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += xmlHeaderLen; + } + else if ( XMLUtil::StringEqual( p, commentHeader, commentHeaderLen ) ) { + returnNode = CreateUnlinkedNode( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += commentHeaderLen; + } + else if ( XMLUtil::StringEqual( p, cdataHeader, cdataHeaderLen ) ) { + XMLText* text = CreateUnlinkedNode( _textPool ); + returnNode = text; + returnNode->_parseLineNum = _parseCurLineNum; + p += cdataHeaderLen; + text->SetCData( true ); + } + else if ( XMLUtil::StringEqual( p, dtdHeader, dtdHeaderLen ) ) { + returnNode = CreateUnlinkedNode( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += dtdHeaderLen; + } + else if ( XMLUtil::StringEqual( p, elementHeader, elementHeaderLen ) ) { + returnNode = CreateUnlinkedNode( _elementPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += elementHeaderLen; + } + else { + returnNode = CreateUnlinkedNode( _textPool ); + returnNode->_parseLineNum = _parseCurLineNum; // Report line of first non-whitespace character + p = start; // Back it up, all the text counts. + _parseCurLineNum = startLine; + } + + TIXMLASSERT( returnNode ); + TIXMLASSERT( p ); + *node = returnNode; + return p; +} + + +bool XMLDocument::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + if ( visitor->VisitEnter( *this ) ) { + for ( const XMLNode* node=FirstChild(); node; node=node->NextSibling() ) { + if ( !node->Accept( visitor ) ) { + break; + } + } + } + return visitor->VisitExit( *this ); +} + + +// --------- XMLNode ----------- // + +XMLNode::XMLNode( XMLDocument* doc ) : + _document( doc ), + _parent( 0 ), + _value(), + _parseLineNum( 0 ), + _firstChild( 0 ), _lastChild( 0 ), + _prev( 0 ), _next( 0 ), + _userData( 0 ), + _memPool( 0 ) +{ +} + + +XMLNode::~XMLNode() +{ + DeleteChildren(); + if ( _parent ) { + _parent->Unlink( this ); + } +} + +const char* XMLNode::Value() const +{ + // Edge case: XMLDocuments don't have a Value. Return null. + if ( this->ToDocument() ) + return 0; + return _value.GetStr(); +} + +void XMLNode::SetValue( const char* str, bool staticMem ) +{ + if ( staticMem ) { + _value.SetInternedStr( str ); + } + else { + _value.SetStr( str ); + } +} + +XMLNode* XMLNode::DeepClone(XMLDocument* target) const +{ + XMLNode* clone = this->ShallowClone(target); + if (!clone) return 0; + + for (const XMLNode* child = this->FirstChild(); child; child = child->NextSibling()) { + XMLNode* childClone = child->DeepClone(target); + TIXMLASSERT(childClone); + clone->InsertEndChild(childClone); + } + return clone; +} + +void XMLNode::DeleteChildren() +{ + while( _firstChild ) { + TIXMLASSERT( _lastChild ); + DeleteChild( _firstChild ); + } + _firstChild = _lastChild = 0; +} + + +void XMLNode::Unlink( XMLNode* child ) +{ + TIXMLASSERT( child ); + TIXMLASSERT( child->_document == _document ); + TIXMLASSERT( child->_parent == this ); + if ( child == _firstChild ) { + _firstChild = _firstChild->_next; + } + if ( child == _lastChild ) { + _lastChild = _lastChild->_prev; + } + + if ( child->_prev ) { + child->_prev->_next = child->_next; + } + if ( child->_next ) { + child->_next->_prev = child->_prev; + } + child->_next = 0; + child->_prev = 0; + child->_parent = 0; +} + + +void XMLNode::DeleteChild( XMLNode* node ) +{ + TIXMLASSERT( node ); + TIXMLASSERT( node->_document == _document ); + TIXMLASSERT( node->_parent == this ); + Unlink( node ); + TIXMLASSERT(node->_prev == 0); + TIXMLASSERT(node->_next == 0); + TIXMLASSERT(node->_parent == 0); + DeleteNode( node ); +} + + +XMLNode* XMLNode::InsertEndChild( XMLNode* addThis ) +{ + TIXMLASSERT( addThis ); + if ( addThis->_document != _document ) { + TIXMLASSERT( false ); + return 0; + } + InsertChildPreamble( addThis ); + + if ( _lastChild ) { + TIXMLASSERT( _firstChild ); + TIXMLASSERT( _lastChild->_next == 0 ); + _lastChild->_next = addThis; + addThis->_prev = _lastChild; + _lastChild = addThis; + + addThis->_next = 0; + } + else { + TIXMLASSERT( _firstChild == 0 ); + _firstChild = _lastChild = addThis; + + addThis->_prev = 0; + addThis->_next = 0; + } + addThis->_parent = this; + return addThis; +} + + +XMLNode* XMLNode::InsertFirstChild( XMLNode* addThis ) +{ + TIXMLASSERT( addThis ); + if ( addThis->_document != _document ) { + TIXMLASSERT( false ); + return 0; + } + InsertChildPreamble( addThis ); + + if ( _firstChild ) { + TIXMLASSERT( _lastChild ); + TIXMLASSERT( _firstChild->_prev == 0 ); + + _firstChild->_prev = addThis; + addThis->_next = _firstChild; + _firstChild = addThis; + + addThis->_prev = 0; + } + else { + TIXMLASSERT( _lastChild == 0 ); + _firstChild = _lastChild = addThis; + + addThis->_prev = 0; + addThis->_next = 0; + } + addThis->_parent = this; + return addThis; +} + + +XMLNode* XMLNode::InsertAfterChild( XMLNode* afterThis, XMLNode* addThis ) +{ + TIXMLASSERT( addThis ); + if ( addThis->_document != _document ) { + TIXMLASSERT( false ); + return 0; + } + + TIXMLASSERT( afterThis ); + + if ( afterThis->_parent != this ) { + TIXMLASSERT( false ); + return 0; + } + if ( afterThis == addThis ) { + // Current state: BeforeThis -> AddThis -> OneAfterAddThis + // Now AddThis must disappear from it's location and then + // reappear between BeforeThis and OneAfterAddThis. + // So just leave it where it is. + return addThis; + } + + if ( afterThis->_next == 0 ) { + // The last node or the only node. + return InsertEndChild( addThis ); + } + InsertChildPreamble( addThis ); + addThis->_prev = afterThis; + addThis->_next = afterThis->_next; + afterThis->_next->_prev = addThis; + afterThis->_next = addThis; + addThis->_parent = this; + return addThis; +} + + + + +const XMLElement* XMLNode::FirstChildElement( const char* name ) const +{ + for( const XMLNode* node = _firstChild; node; node = node->_next ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return 0; +} + + +const XMLElement* XMLNode::LastChildElement( const char* name ) const +{ + for( const XMLNode* node = _lastChild; node; node = node->_prev ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return 0; +} + + +const XMLElement* XMLNode::NextSiblingElement( const char* name ) const +{ + for( const XMLNode* node = _next; node; node = node->_next ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return 0; +} + + +const XMLElement* XMLNode::PreviousSiblingElement( const char* name ) const +{ + for( const XMLNode* node = _prev; node; node = node->_prev ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return 0; +} + + +char* XMLNode::ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) +{ + // This is a recursive method, but thinking about it "at the current level" + // it is a pretty simple flat list: + // + // + // + // With a special case: + // + // + // + // + // Where the closing element (/foo) *must* be the next thing after the opening + // element, and the names must match. BUT the tricky bit is that the closing + // element will be read by the child. + // + // 'endTag' is the end tag for this node, it is returned by a call to a child. + // 'parentEnd' is the end tag for the parent, which is filled in and returned. + + XMLDocument::DepthTracker tracker(_document); + if (_document->Error()) + return 0; + + while( p && *p ) { + XMLNode* node = 0; + + p = _document->Identify( p, &node ); + TIXMLASSERT( p ); + if ( node == 0 ) { + break; + } + + const int initialLineNum = node->_parseLineNum; + + StrPair endTag; + p = node->ParseDeep( p, &endTag, curLineNumPtr ); + if ( !p ) { + DeleteNode( node ); + if ( !_document->Error() ) { + _document->SetError( XML_ERROR_PARSING, initialLineNum, 0); + } + break; + } + + const XMLDeclaration* const decl = node->ToDeclaration(); + if ( decl ) { + // Declarations are only allowed at document level + // + // Multiple declarations are allowed but all declarations + // must occur before anything else. + // + // Optimized due to a security test case. If the first node is + // a declaration, and the last node is a declaration, then only + // declarations have so far been added. + bool wellLocated = false; + + if (ToDocument()) { + if (FirstChild()) { + wellLocated = + FirstChild() && + FirstChild()->ToDeclaration() && + LastChild() && + LastChild()->ToDeclaration(); + } + else { + wellLocated = true; + } + } + if ( !wellLocated ) { + _document->SetError( XML_ERROR_PARSING_DECLARATION, initialLineNum, "XMLDeclaration value=%s", decl->Value()); + DeleteNode( node ); + break; + } + } + + XMLElement* ele = node->ToElement(); + if ( ele ) { + // We read the end tag. Return it to the parent. + if ( ele->ClosingType() == XMLElement::CLOSING ) { + if ( parentEndTag ) { + ele->_value.TransferTo( parentEndTag ); + } + node->_memPool->SetTracked(); // created and then immediately deleted. + DeleteNode( node ); + return p; + } + + // Handle an end tag returned to this level. + // And handle a bunch of annoying errors. + bool mismatch = false; + if ( endTag.Empty() ) { + if ( ele->ClosingType() == XMLElement::OPEN ) { + mismatch = true; + } + } + else { + if ( ele->ClosingType() != XMLElement::OPEN ) { + mismatch = true; + } + else if ( !XMLUtil::StringEqual( endTag.GetStr(), ele->Name() ) ) { + mismatch = true; + } + } + if ( mismatch ) { + _document->SetError( XML_ERROR_MISMATCHED_ELEMENT, initialLineNum, "XMLElement name=%s", ele->Name()); + DeleteNode( node ); + break; + } + } + InsertEndChild( node ); + } + return 0; +} + +/*static*/ void XMLNode::DeleteNode( XMLNode* node ) +{ + if ( node == 0 ) { + return; + } + TIXMLASSERT(node->_document); + if (!node->ToDocument()) { + node->_document->MarkInUse(node); + } + + MemPool* pool = node->_memPool; + node->~XMLNode(); + pool->Free( node ); +} + +void XMLNode::InsertChildPreamble( XMLNode* insertThis ) const +{ + TIXMLASSERT( insertThis ); + TIXMLASSERT( insertThis->_document == _document ); + + if (insertThis->_parent) { + insertThis->_parent->Unlink( insertThis ); + } + else { + insertThis->_document->MarkInUse(insertThis); + insertThis->_memPool->SetTracked(); + } +} + +const XMLElement* XMLNode::ToElementWithName( const char* name ) const +{ + const XMLElement* element = this->ToElement(); + if ( element == 0 ) { + return 0; + } + if ( name == 0 ) { + return element; + } + if ( XMLUtil::StringEqual( element->Name(), name ) ) { + return element; + } + return 0; +} + +// --------- XMLText ---------- // +char* XMLText::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + if ( this->CData() ) { + p = _value.ParseText( p, "]]>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); + if ( !p ) { + _document->SetError( XML_ERROR_PARSING_CDATA, _parseLineNum, 0 ); + } + return p; + } + else { + int flags = _document->ProcessEntities() ? StrPair::TEXT_ELEMENT : StrPair::TEXT_ELEMENT_LEAVE_ENTITIES; + if ( _document->WhitespaceMode() == COLLAPSE_WHITESPACE ) { + flags |= StrPair::NEEDS_WHITESPACE_COLLAPSING; + } + + p = _value.ParseText( p, "<", flags, curLineNumPtr ); + if ( p && *p ) { + return p-1; + } + if ( !p ) { + _document->SetError( XML_ERROR_PARSING_TEXT, _parseLineNum, 0 ); + } + } + return 0; +} + + +XMLNode* XMLText::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLText* text = doc->NewText( Value() ); // fixme: this will always allocate memory. Intern? + text->SetCData( this->CData() ); + return text; +} + + +bool XMLText::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLText* text = compare->ToText(); + return ( text && XMLUtil::StringEqual( text->Value(), Value() ) ); +} + + +bool XMLText::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + + +// --------- XMLComment ---------- // + +XMLComment::XMLComment( XMLDocument* doc ) : XMLNode( doc ) +{ +} + + +XMLComment::~XMLComment() +{ +} + + +char* XMLComment::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + // Comment parses as text. + p = _value.ParseText( p, "-->", StrPair::COMMENT, curLineNumPtr ); + if ( p == 0 ) { + _document->SetError( XML_ERROR_PARSING_COMMENT, _parseLineNum, 0 ); + } + return p; +} + + +XMLNode* XMLComment::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLComment* comment = doc->NewComment( Value() ); // fixme: this will always allocate memory. Intern? + return comment; +} + + +bool XMLComment::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLComment* comment = compare->ToComment(); + return ( comment && XMLUtil::StringEqual( comment->Value(), Value() )); +} + + +bool XMLComment::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + + +// --------- XMLDeclaration ---------- // + +XMLDeclaration::XMLDeclaration( XMLDocument* doc ) : XMLNode( doc ) +{ +} + + +XMLDeclaration::~XMLDeclaration() +{ + //printf( "~XMLDeclaration\n" ); +} + + +char* XMLDeclaration::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + // Declaration parses as text. + p = _value.ParseText( p, "?>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); + if ( p == 0 ) { + _document->SetError( XML_ERROR_PARSING_DECLARATION, _parseLineNum, 0 ); + } + return p; +} + + +XMLNode* XMLDeclaration::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLDeclaration* dec = doc->NewDeclaration( Value() ); // fixme: this will always allocate memory. Intern? + return dec; +} + + +bool XMLDeclaration::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLDeclaration* declaration = compare->ToDeclaration(); + return ( declaration && XMLUtil::StringEqual( declaration->Value(), Value() )); +} + + + +bool XMLDeclaration::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + +// --------- XMLUnknown ---------- // + +XMLUnknown::XMLUnknown( XMLDocument* doc ) : XMLNode( doc ) +{ +} + + +XMLUnknown::~XMLUnknown() +{ +} + + +char* XMLUnknown::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + // Unknown parses as text. + p = _value.ParseText( p, ">", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); + if ( !p ) { + _document->SetError( XML_ERROR_PARSING_UNKNOWN, _parseLineNum, 0 ); + } + return p; +} + + +XMLNode* XMLUnknown::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLUnknown* text = doc->NewUnknown( Value() ); // fixme: this will always allocate memory. Intern? + return text; +} + + +bool XMLUnknown::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLUnknown* unknown = compare->ToUnknown(); + return ( unknown && XMLUtil::StringEqual( unknown->Value(), Value() )); +} + + +bool XMLUnknown::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + +// --------- XMLAttribute ---------- // + +const char* XMLAttribute::Name() const +{ + return _name.GetStr(); +} + +const char* XMLAttribute::Value() const +{ + return _value.GetStr(); +} + +char* XMLAttribute::ParseDeep( char* p, bool processEntities, int* curLineNumPtr ) +{ + // Parse using the name rules: bug fix, was using ParseText before + p = _name.ParseName( p ); + if ( !p || !*p ) { + return 0; + } + + // Skip white space before = + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + if ( *p != '=' ) { + return 0; + } + + ++p; // move up to opening quote + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + if ( *p != '\"' && *p != '\'' ) { + return 0; + } + + const char endTag[2] = { *p, 0 }; + ++p; // move past opening quote + + p = _value.ParseText( p, endTag, processEntities ? StrPair::ATTRIBUTE_VALUE : StrPair::ATTRIBUTE_VALUE_LEAVE_ENTITIES, curLineNumPtr ); + return p; +} + + +void XMLAttribute::SetName( const char* n ) +{ + _name.SetStr( n ); +} + + +XMLError XMLAttribute::QueryIntValue( int* value ) const +{ + if ( XMLUtil::ToInt( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryUnsignedValue( unsigned int* value ) const +{ + if ( XMLUtil::ToUnsigned( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryInt64Value(int64_t* value) const +{ + if (XMLUtil::ToInt64(Value(), value)) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryUnsigned64Value(uint64_t* value) const +{ + if(XMLUtil::ToUnsigned64(Value(), value)) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryBoolValue( bool* value ) const +{ + if ( XMLUtil::ToBool( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryFloatValue( float* value ) const +{ + if ( XMLUtil::ToFloat( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryDoubleValue( double* value ) const +{ + if ( XMLUtil::ToDouble( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +void XMLAttribute::SetAttribute( const char* v ) +{ + _value.SetStr( v ); +} + + +void XMLAttribute::SetAttribute( int v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + + +void XMLAttribute::SetAttribute( unsigned v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + + +void XMLAttribute::SetAttribute(int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + _value.SetStr(buf); +} + +void XMLAttribute::SetAttribute(uint64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + _value.SetStr(buf); +} + + +void XMLAttribute::SetAttribute( bool v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + +void XMLAttribute::SetAttribute( double v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + +void XMLAttribute::SetAttribute( float v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + + +// --------- XMLElement ---------- // +XMLElement::XMLElement( XMLDocument* doc ) : XMLNode( doc ), + _closingType( OPEN ), + _rootAttribute( 0 ) +{ +} + + +XMLElement::~XMLElement() +{ + while( _rootAttribute ) { + XMLAttribute* next = _rootAttribute->_next; + DeleteAttribute( _rootAttribute ); + _rootAttribute = next; + } +} + + +const XMLAttribute* XMLElement::FindAttribute( const char* name ) const +{ + for( XMLAttribute* a = _rootAttribute; a; a = a->_next ) { + if ( XMLUtil::StringEqual( a->Name(), name ) ) { + return a; + } + } + return 0; +} + + +const char* XMLElement::Attribute( const char* name, const char* value ) const +{ + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return 0; + } + if ( !value || XMLUtil::StringEqual( a->Value(), value )) { + return a->Value(); + } + return 0; +} + +int XMLElement::IntAttribute(const char* name, int defaultValue) const +{ + int i = defaultValue; + QueryIntAttribute(name, &i); + return i; +} + +unsigned XMLElement::UnsignedAttribute(const char* name, unsigned defaultValue) const +{ + unsigned i = defaultValue; + QueryUnsignedAttribute(name, &i); + return i; +} + +int64_t XMLElement::Int64Attribute(const char* name, int64_t defaultValue) const +{ + int64_t i = defaultValue; + QueryInt64Attribute(name, &i); + return i; +} + +uint64_t XMLElement::Unsigned64Attribute(const char* name, uint64_t defaultValue) const +{ + uint64_t i = defaultValue; + QueryUnsigned64Attribute(name, &i); + return i; +} + +bool XMLElement::BoolAttribute(const char* name, bool defaultValue) const +{ + bool b = defaultValue; + QueryBoolAttribute(name, &b); + return b; +} + +double XMLElement::DoubleAttribute(const char* name, double defaultValue) const +{ + double d = defaultValue; + QueryDoubleAttribute(name, &d); + return d; +} + +float XMLElement::FloatAttribute(const char* name, float defaultValue) const +{ + float f = defaultValue; + QueryFloatAttribute(name, &f); + return f; +} + +const char* XMLElement::GetText() const +{ + /* skip comment node */ + const XMLNode* node = FirstChild(); + while (node) { + if (node->ToComment()) { + node = node->NextSibling(); + continue; + } + break; + } + + if ( node && node->ToText() ) { + return node->Value(); + } + return 0; +} + + +void XMLElement::SetText( const char* inText ) +{ + if ( FirstChild() && FirstChild()->ToText() ) + FirstChild()->SetValue( inText ); + else { + XMLText* theText = GetDocument()->NewText( inText ); + InsertFirstChild( theText ); + } +} + + +void XMLElement::SetText( int v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText( unsigned v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText(int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + SetText(buf); +} + +void XMLElement::SetText(uint64_t v) { + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + SetText(buf); +} + + +void XMLElement::SetText( bool v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText( float v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText( double v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +XMLError XMLElement::QueryIntText( int* ival ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToInt( t, ival ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryUnsignedText( unsigned* uval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToUnsigned( t, uval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryInt64Text(int64_t* ival) const +{ + if (FirstChild() && FirstChild()->ToText()) { + const char* t = FirstChild()->Value(); + if (XMLUtil::ToInt64(t, ival)) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryUnsigned64Text(uint64_t* ival) const +{ + if(FirstChild() && FirstChild()->ToText()) { + const char* t = FirstChild()->Value(); + if(XMLUtil::ToUnsigned64(t, ival)) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryBoolText( bool* bval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToBool( t, bval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryDoubleText( double* dval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToDouble( t, dval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryFloatText( float* fval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToFloat( t, fval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + +int XMLElement::IntText(int defaultValue) const +{ + int i = defaultValue; + QueryIntText(&i); + return i; +} + +unsigned XMLElement::UnsignedText(unsigned defaultValue) const +{ + unsigned i = defaultValue; + QueryUnsignedText(&i); + return i; +} + +int64_t XMLElement::Int64Text(int64_t defaultValue) const +{ + int64_t i = defaultValue; + QueryInt64Text(&i); + return i; +} + +uint64_t XMLElement::Unsigned64Text(uint64_t defaultValue) const +{ + uint64_t i = defaultValue; + QueryUnsigned64Text(&i); + return i; +} + +bool XMLElement::BoolText(bool defaultValue) const +{ + bool b = defaultValue; + QueryBoolText(&b); + return b; +} + +double XMLElement::DoubleText(double defaultValue) const +{ + double d = defaultValue; + QueryDoubleText(&d); + return d; +} + +float XMLElement::FloatText(float defaultValue) const +{ + float f = defaultValue; + QueryFloatText(&f); + return f; +} + + +XMLAttribute* XMLElement::FindOrCreateAttribute( const char* name ) +{ + XMLAttribute* last = 0; + XMLAttribute* attrib = 0; + for( attrib = _rootAttribute; + attrib; + last = attrib, attrib = attrib->_next ) { + if ( XMLUtil::StringEqual( attrib->Name(), name ) ) { + break; + } + } + if ( !attrib ) { + attrib = CreateAttribute(); + TIXMLASSERT( attrib ); + if ( last ) { + TIXMLASSERT( last->_next == 0 ); + last->_next = attrib; + } + else { + TIXMLASSERT( _rootAttribute == 0 ); + _rootAttribute = attrib; + } + attrib->SetName( name ); + } + return attrib; +} + + +void XMLElement::DeleteAttribute( const char* name ) +{ + XMLAttribute* prev = 0; + for( XMLAttribute* a=_rootAttribute; a; a=a->_next ) { + if ( XMLUtil::StringEqual( name, a->Name() ) ) { + if ( prev ) { + prev->_next = a->_next; + } + else { + _rootAttribute = a->_next; + } + DeleteAttribute( a ); + break; + } + prev = a; + } +} + + +char* XMLElement::ParseAttributes( char* p, int* curLineNumPtr ) +{ + XMLAttribute* prevAttribute = 0; + + // Read the attributes. + while( p ) { + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + if ( !(*p) ) { + _document->SetError( XML_ERROR_PARSING_ELEMENT, _parseLineNum, "XMLElement name=%s", Name() ); + return 0; + } + + // attribute. + if (XMLUtil::IsNameStartChar( (unsigned char) *p ) ) { + XMLAttribute* attrib = CreateAttribute(); + TIXMLASSERT( attrib ); + attrib->_parseLineNum = _document->_parseCurLineNum; + + const int attrLineNum = attrib->_parseLineNum; + + p = attrib->ParseDeep( p, _document->ProcessEntities(), curLineNumPtr ); + if ( !p || Attribute( attrib->Name() ) ) { + DeleteAttribute( attrib ); + _document->SetError( XML_ERROR_PARSING_ATTRIBUTE, attrLineNum, "XMLElement name=%s", Name() ); + return 0; + } + // There is a minor bug here: if the attribute in the source xml + // document is duplicated, it will not be detected and the + // attribute will be doubly added. However, tracking the 'prevAttribute' + // avoids re-scanning the attribute list. Preferring performance for + // now, may reconsider in the future. + if ( prevAttribute ) { + TIXMLASSERT( prevAttribute->_next == 0 ); + prevAttribute->_next = attrib; + } + else { + TIXMLASSERT( _rootAttribute == 0 ); + _rootAttribute = attrib; + } + prevAttribute = attrib; + } + // end of the tag + else if ( *p == '>' ) { + ++p; + break; + } + // end of the tag + else if ( *p == '/' && *(p+1) == '>' ) { + _closingType = CLOSED; + return p+2; // done; sealed element. + } + else { + _document->SetError( XML_ERROR_PARSING_ELEMENT, _parseLineNum, 0 ); + return 0; + } + } + return p; +} + +void XMLElement::DeleteAttribute( XMLAttribute* attribute ) +{ + if ( attribute == 0 ) { + return; + } + MemPool* pool = attribute->_memPool; + attribute->~XMLAttribute(); + pool->Free( attribute ); +} + +XMLAttribute* XMLElement::CreateAttribute() +{ + TIXMLASSERT( sizeof( XMLAttribute ) == _document->_attributePool.ItemSize() ); + XMLAttribute* attrib = new (_document->_attributePool.Alloc() ) XMLAttribute(); + TIXMLASSERT( attrib ); + attrib->_memPool = &_document->_attributePool; + attrib->_memPool->SetTracked(); + return attrib; +} + + +XMLElement* XMLElement::InsertNewChildElement(const char* name) +{ + XMLElement* node = _document->NewElement(name); + return InsertEndChild(node) ? node : 0; +} + +XMLComment* XMLElement::InsertNewComment(const char* comment) +{ + XMLComment* node = _document->NewComment(comment); + return InsertEndChild(node) ? node : 0; +} + +XMLText* XMLElement::InsertNewText(const char* text) +{ + XMLText* node = _document->NewText(text); + return InsertEndChild(node) ? node : 0; +} + +XMLDeclaration* XMLElement::InsertNewDeclaration(const char* text) +{ + XMLDeclaration* node = _document->NewDeclaration(text); + return InsertEndChild(node) ? node : 0; +} + +XMLUnknown* XMLElement::InsertNewUnknown(const char* text) +{ + XMLUnknown* node = _document->NewUnknown(text); + return InsertEndChild(node) ? node : 0; +} + + + +// +// +// foobar +// +char* XMLElement::ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) +{ + // Read the element name. + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + + // The closing element is the form. It is + // parsed just like a regular element then deleted from + // the DOM. + if ( *p == '/' ) { + _closingType = CLOSING; + ++p; + } + + p = _value.ParseName( p ); + if ( _value.Empty() ) { + return 0; + } + + p = ParseAttributes( p, curLineNumPtr ); + if ( !p || !*p || _closingType != OPEN ) { + return p; + } + + p = XMLNode::ParseDeep( p, parentEndTag, curLineNumPtr ); + return p; +} + + + +XMLNode* XMLElement::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLElement* element = doc->NewElement( Value() ); // fixme: this will always allocate memory. Intern? + for( const XMLAttribute* a=FirstAttribute(); a; a=a->Next() ) { + element->SetAttribute( a->Name(), a->Value() ); // fixme: this will always allocate memory. Intern? + } + return element; +} + + +bool XMLElement::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLElement* other = compare->ToElement(); + if ( other && XMLUtil::StringEqual( other->Name(), Name() )) { + + const XMLAttribute* a=FirstAttribute(); + const XMLAttribute* b=other->FirstAttribute(); + + while ( a && b ) { + if ( !XMLUtil::StringEqual( a->Value(), b->Value() ) ) { + return false; + } + a = a->Next(); + b = b->Next(); + } + if ( a || b ) { + // different count + return false; + } + return true; + } + return false; +} + + +bool XMLElement::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + if ( visitor->VisitEnter( *this, _rootAttribute ) ) { + for ( const XMLNode* node=FirstChild(); node; node=node->NextSibling() ) { + if ( !node->Accept( visitor ) ) { + break; + } + } + } + return visitor->VisitExit( *this ); +} + + +// --------- XMLDocument ----------- // + +// Warning: List must match 'enum XMLError' +const char* XMLDocument::_errorNames[XML_ERROR_COUNT] = { + "XML_SUCCESS", + "XML_NO_ATTRIBUTE", + "XML_WRONG_ATTRIBUTE_TYPE", + "XML_ERROR_FILE_NOT_FOUND", + "XML_ERROR_FILE_COULD_NOT_BE_OPENED", + "XML_ERROR_FILE_READ_ERROR", + "XML_ERROR_PARSING_ELEMENT", + "XML_ERROR_PARSING_ATTRIBUTE", + "XML_ERROR_PARSING_TEXT", + "XML_ERROR_PARSING_CDATA", + "XML_ERROR_PARSING_COMMENT", + "XML_ERROR_PARSING_DECLARATION", + "XML_ERROR_PARSING_UNKNOWN", + "XML_ERROR_EMPTY_DOCUMENT", + "XML_ERROR_MISMATCHED_ELEMENT", + "XML_ERROR_PARSING", + "XML_CAN_NOT_CONVERT_TEXT", + "XML_NO_TEXT_NODE", + "XML_ELEMENT_DEPTH_EXCEEDED" +}; + + +XMLDocument::XMLDocument( bool processEntities, Whitespace whitespaceMode ) : + XMLNode( 0 ), + _writeBOM( false ), + _processEntities( processEntities ), + _errorID(XML_SUCCESS), + _whitespaceMode( whitespaceMode ), + _errorStr(), + _errorLineNum( 0 ), + _charBuffer( 0 ), + _parseCurLineNum( 0 ), + _parsingDepth(0), + _unlinked(), + _elementPool(), + _attributePool(), + _textPool(), + _commentPool() +{ + // avoid VC++ C4355 warning about 'this' in initializer list (C4355 is off by default in VS2012+) + _document = this; +} + + +XMLDocument::~XMLDocument() +{ + Clear(); +} + + +void XMLDocument::MarkInUse(const XMLNode* const node) +{ + TIXMLASSERT(node); + TIXMLASSERT(node->_parent == 0); + + for (int i = 0; i < _unlinked.Size(); ++i) { + if (node == _unlinked[i]) { + _unlinked.SwapRemove(i); + break; + } + } +} + +void XMLDocument::Clear() +{ + DeleteChildren(); + while( _unlinked.Size()) { + DeleteNode(_unlinked[0]); // Will remove from _unlinked as part of delete. + } + +#ifdef TINYXML2_DEBUG + const bool hadError = Error(); +#endif + ClearError(); + + delete [] _charBuffer; + _charBuffer = 0; + _parsingDepth = 0; + +#if 0 + _textPool.Trace( "text" ); + _elementPool.Trace( "element" ); + _commentPool.Trace( "comment" ); + _attributePool.Trace( "attribute" ); +#endif + +#ifdef TINYXML2_DEBUG + if ( !hadError ) { + TIXMLASSERT( _elementPool.CurrentAllocs() == _elementPool.Untracked() ); + TIXMLASSERT( _attributePool.CurrentAllocs() == _attributePool.Untracked() ); + TIXMLASSERT( _textPool.CurrentAllocs() == _textPool.Untracked() ); + TIXMLASSERT( _commentPool.CurrentAllocs() == _commentPool.Untracked() ); + } +#endif +} + + +void XMLDocument::DeepCopy(XMLDocument* target) const +{ + TIXMLASSERT(target); + if (target == this) { + return; // technically success - a no-op. + } + + target->Clear(); + for (const XMLNode* node = this->FirstChild(); node; node = node->NextSibling()) { + target->InsertEndChild(node->DeepClone(target)); + } +} + +XMLElement* XMLDocument::NewElement( const char* name ) +{ + XMLElement* ele = CreateUnlinkedNode( _elementPool ); + ele->SetName( name ); + return ele; +} + + +XMLComment* XMLDocument::NewComment( const char* str ) +{ + XMLComment* comment = CreateUnlinkedNode( _commentPool ); + comment->SetValue( str ); + return comment; +} + + +XMLText* XMLDocument::NewText( const char* str ) +{ + XMLText* text = CreateUnlinkedNode( _textPool ); + text->SetValue( str ); + return text; +} + + +XMLDeclaration* XMLDocument::NewDeclaration( const char* str ) +{ + XMLDeclaration* dec = CreateUnlinkedNode( _commentPool ); + dec->SetValue( str ? str : "xml version=\"1.0\" encoding=\"UTF-8\"" ); + return dec; +} + + +XMLUnknown* XMLDocument::NewUnknown( const char* str ) +{ + XMLUnknown* unk = CreateUnlinkedNode( _commentPool ); + unk->SetValue( str ); + return unk; +} + +static FILE* callfopen( const char* filepath, const char* mode ) +{ + TIXMLASSERT( filepath ); + TIXMLASSERT( mode ); +#if defined(_MSC_VER) && (_MSC_VER >= 1400 ) && (!defined WINCE) + FILE* fp = 0; + const errno_t err = fopen_s( &fp, filepath, mode ); + if ( err ) { + return 0; + } +#else + FILE* fp = fopen( filepath, mode ); +#endif + return fp; +} + +void XMLDocument::DeleteNode( XMLNode* node ) { + TIXMLASSERT( node ); + TIXMLASSERT(node->_document == this ); + if (node->_parent) { + node->_parent->DeleteChild( node ); + } + else { + // Isn't in the tree. + // Use the parent delete. + // Also, we need to mark it tracked: we 'know' + // it was never used. + node->_memPool->SetTracked(); + // Call the static XMLNode version: + XMLNode::DeleteNode(node); + } +} + + +XMLError XMLDocument::LoadFile( const char* filename ) +{ + if ( !filename ) { + TIXMLASSERT( false ); + SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=" ); + return _errorID; + } + + Clear(); + FILE* fp = callfopen( filename, "rb" ); + if ( !fp ) { + SetError( XML_ERROR_FILE_NOT_FOUND, 0, "filename=%s", filename ); + return _errorID; + } + LoadFile( fp ); + fclose( fp ); + return _errorID; +} + +XMLError XMLDocument::LoadFile( FILE* fp ) +{ + Clear(); + + TIXML_FSEEK( fp, 0, SEEK_SET ); + if ( fgetc( fp ) == EOF && ferror( fp ) != 0 ) { + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + return _errorID; + } + + TIXML_FSEEK( fp, 0, SEEK_END ); + + unsigned long long filelength; + { + const long long fileLengthSigned = TIXML_FTELL( fp ); + TIXML_FSEEK( fp, 0, SEEK_SET ); + if ( fileLengthSigned == -1L ) { + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + return _errorID; + } + TIXMLASSERT( fileLengthSigned >= 0 ); + filelength = static_cast(fileLengthSigned); + } + + const size_t maxSizeT = static_cast(-1); + // We'll do the comparison as an unsigned long long, because that's guaranteed to be at + // least 8 bytes, even on a 32-bit platform. + if ( filelength >= static_cast(maxSizeT) ) { + // Cannot handle files which won't fit in buffer together with null terminator + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + return _errorID; + } + + if ( filelength == 0 ) { + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 ); + return _errorID; + } + + const size_t size = static_cast(filelength); + TIXMLASSERT( _charBuffer == 0 ); + _charBuffer = new char[size+1]; + const size_t read = fread( _charBuffer, 1, size, fp ); + if ( read != size ) { + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + return _errorID; + } + + _charBuffer[size] = 0; + + Parse(); + return _errorID; +} + + +XMLError XMLDocument::SaveFile( const char* filename, bool compact ) +{ + if ( !filename ) { + TIXMLASSERT( false ); + SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=" ); + return _errorID; + } + + FILE* fp = callfopen( filename, "w" ); + if ( !fp ) { + SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=%s", filename ); + return _errorID; + } + SaveFile(fp, compact); + fclose( fp ); + return _errorID; +} + + +XMLError XMLDocument::SaveFile( FILE* fp, bool compact ) +{ + // Clear any error from the last save, otherwise it will get reported + // for *this* call. + ClearError(); + XMLPrinter stream( fp, compact ); + Print( &stream ); + return _errorID; +} + + +XMLError XMLDocument::Parse( const char* p, size_t len ) +{ + Clear(); + + if ( len == 0 || !p || !*p ) { + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 ); + return _errorID; + } + if ( len == static_cast(-1) ) { + len = strlen( p ); + } + TIXMLASSERT( _charBuffer == 0 ); + _charBuffer = new char[ len+1 ]; + memcpy( _charBuffer, p, len ); + _charBuffer[len] = 0; + + Parse(); + if ( Error() ) { + // clean up now essentially dangling memory. + // and the parse fail can put objects in the + // pools that are dead and inaccessible. + DeleteChildren(); + _elementPool.Clear(); + _attributePool.Clear(); + _textPool.Clear(); + _commentPool.Clear(); + } + return _errorID; +} + + +void XMLDocument::Print( XMLPrinter* streamer ) const +{ + if ( streamer ) { + Accept( streamer ); + } + else { + XMLPrinter stdoutStreamer( stdout ); + Accept( &stdoutStreamer ); + } +} + + +void XMLDocument::ClearError() { + _errorID = XML_SUCCESS; + _errorLineNum = 0; + _errorStr.Reset(); +} + + +void XMLDocument::SetError( XMLError error, int lineNum, const char* format, ... ) +{ + TIXMLASSERT( error >= 0 && error < XML_ERROR_COUNT ); + _errorID = error; + _errorLineNum = lineNum; + _errorStr.Reset(); + + const size_t BUFFER_SIZE = 1000; + char* buffer = new char[BUFFER_SIZE]; + + TIXMLASSERT(sizeof(error) <= sizeof(int)); + TIXML_SNPRINTF(buffer, BUFFER_SIZE, "Error=%s ErrorID=%d (0x%x) Line number=%d", ErrorIDToName(error), int(error), int(error), lineNum); + + if (format) { + size_t len = strlen(buffer); + TIXML_SNPRINTF(buffer + len, BUFFER_SIZE - len, ": "); + len = strlen(buffer); + + va_list va; + va_start(va, format); + TIXML_VSNPRINTF(buffer + len, BUFFER_SIZE - len, format, va); + va_end(va); + } + _errorStr.SetStr(buffer); + delete[] buffer; +} + + +/*static*/ const char* XMLDocument::ErrorIDToName(XMLError errorID) +{ + TIXMLASSERT( errorID >= 0 && errorID < XML_ERROR_COUNT ); + const char* errorName = _errorNames[errorID]; + TIXMLASSERT( errorName && errorName[0] ); + return errorName; +} + +const char* XMLDocument::ErrorStr() const +{ + return _errorStr.Empty() ? "" : _errorStr.GetStr(); +} + + +void XMLDocument::PrintError() const +{ + printf("%s\n", ErrorStr()); +} + +const char* XMLDocument::ErrorName() const +{ + return ErrorIDToName(_errorID); +} + +void XMLDocument::Parse() +{ + TIXMLASSERT( NoChildren() ); // Clear() must have been called previously + TIXMLASSERT( _charBuffer ); + _parseCurLineNum = 1; + _parseLineNum = 1; + char* p = _charBuffer; + p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum ); + p = const_cast( XMLUtil::ReadBOM( p, &_writeBOM ) ); + if ( !*p ) { + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 ); + return; + } + ParseDeep(p, 0, &_parseCurLineNum ); +} + +void XMLDocument::PushDepth() +{ + _parsingDepth++; + if (_parsingDepth == TINYXML2_MAX_ELEMENT_DEPTH) { + SetError(XML_ELEMENT_DEPTH_EXCEEDED, _parseCurLineNum, "Element nesting is too deep." ); + } +} + +void XMLDocument::PopDepth() +{ + TIXMLASSERT(_parsingDepth > 0); + --_parsingDepth; +} + +XMLPrinter::XMLPrinter( FILE* file, bool compact, int depth ) : + _elementJustOpened( false ), + _stack(), + _firstElement( true ), + _fp( file ), + _depth( depth ), + _textDepth( -1 ), + _processEntities( true ), + _compactMode( compact ), + _buffer() +{ + for( int i=0; i(entityValue); + TIXMLASSERT( flagIndex < ENTITY_RANGE ); + _entityFlag[flagIndex] = true; + } + _restrictedEntityFlag[static_cast('&')] = true; + _restrictedEntityFlag[static_cast('<')] = true; + _restrictedEntityFlag[static_cast('>')] = true; // not required, but consistency is nice + _buffer.Push( 0 ); +} + + +void XMLPrinter::Print( const char* format, ... ) +{ + va_list va; + va_start( va, format ); + + if ( _fp ) { + vfprintf( _fp, format, va ); + } + else { + const int len = TIXML_VSCPRINTF( format, va ); + // Close out and re-start the va-args + va_end( va ); + TIXMLASSERT( len >= 0 ); + va_start( va, format ); + TIXMLASSERT( _buffer.Size() > 0 && _buffer[_buffer.Size() - 1] == 0 ); + char* p = _buffer.PushArr( len ) - 1; // back up over the null terminator. + TIXML_VSNPRINTF( p, len+1, format, va ); + } + va_end( va ); +} + + +void XMLPrinter::Write( const char* data, size_t size ) +{ + if ( _fp ) { + fwrite ( data , sizeof(char), size, _fp); + } + else { + char* p = _buffer.PushArr( static_cast(size) ) - 1; // back up over the null terminator. + memcpy( p, data, size ); + p[size] = 0; + } +} + + +void XMLPrinter::Putc( char ch ) +{ + if ( _fp ) { + fputc ( ch, _fp); + } + else { + char* p = _buffer.PushArr( sizeof(char) ) - 1; // back up over the null terminator. + p[0] = ch; + p[1] = 0; + } +} + + +void XMLPrinter::PrintSpace( int depth ) +{ + for( int i=0; i 0 && *q < ENTITY_RANGE ) { + // Check for entities. If one is found, flush + // the stream up until the entity, write the + // entity, and keep looking. + if ( flag[static_cast(*q)] ) { + while ( p < q ) { + const size_t delta = q - p; + const int toPrint = ( INT_MAX < delta ) ? INT_MAX : static_cast(delta); + Write( p, toPrint ); + p += toPrint; + } + bool entityPatternPrinted = false; + for( int i=0; i(delta); + Write( p, toPrint ); + } + } + else { + Write( p ); + } +} + + +void XMLPrinter::PushHeader( bool writeBOM, bool writeDec ) +{ + if ( writeBOM ) { + static const unsigned char bom[] = { TIXML_UTF_LEAD_0, TIXML_UTF_LEAD_1, TIXML_UTF_LEAD_2, 0 }; + Write( reinterpret_cast< const char* >( bom ) ); + } + if ( writeDec ) { + PushDeclaration( "xml version=\"1.0\"" ); + } +} + +void XMLPrinter::PrepareForNewNode( bool compactMode ) +{ + SealElementIfJustOpened(); + + if ( compactMode ) { + return; + } + + if ( _firstElement ) { + PrintSpace (_depth); + } else if ( _textDepth < 0) { + Putc( '\n' ); + PrintSpace( _depth ); + } + + _firstElement = false; +} + +void XMLPrinter::OpenElement( const char* name, bool compactMode ) +{ + PrepareForNewNode( compactMode ); + _stack.Push( name ); + + Write ( "<" ); + Write ( name ); + + _elementJustOpened = true; + ++_depth; +} + + +void XMLPrinter::PushAttribute( const char* name, const char* value ) +{ + TIXMLASSERT( _elementJustOpened ); + Putc ( ' ' ); + Write( name ); + Write( "=\"" ); + PrintString( value, false ); + Putc ( '\"' ); +} + + +void XMLPrinter::PushAttribute( const char* name, int v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::PushAttribute( const char* name, unsigned v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::PushAttribute(const char* name, int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + PushAttribute(name, buf); +} + + +void XMLPrinter::PushAttribute(const char* name, uint64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + PushAttribute(name, buf); +} + + +void XMLPrinter::PushAttribute( const char* name, bool v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::PushAttribute( const char* name, double v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::CloseElement( bool compactMode ) +{ + --_depth; + const char* name = _stack.Pop(); + + if ( _elementJustOpened ) { + Write( "/>" ); + } + else { + if ( _textDepth < 0 && !compactMode) { + Putc( '\n' ); + PrintSpace( _depth ); + } + Write ( "" ); + } + + if ( _textDepth == _depth ) { + _textDepth = -1; + } + if ( _depth == 0 && !compactMode) { + Putc( '\n' ); + } + _elementJustOpened = false; +} + + +void XMLPrinter::SealElementIfJustOpened() +{ + if ( !_elementJustOpened ) { + return; + } + _elementJustOpened = false; + Putc( '>' ); +} + + +void XMLPrinter::PushText( const char* text, bool cdata ) +{ + _textDepth = _depth-1; + + SealElementIfJustOpened(); + if ( cdata ) { + Write( "" ); + } + else { + PrintString( text, true ); + } +} + + +void XMLPrinter::PushText( int64_t value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( uint64_t value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(value, buf, BUF_SIZE); + PushText(buf, false); +} + + +void XMLPrinter::PushText( int value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( unsigned value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( bool value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( float value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( double value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushComment( const char* comment ) +{ + PrepareForNewNode( _compactMode ); + + Write( "" ); +} + + +void XMLPrinter::PushDeclaration( const char* value ) +{ + PrepareForNewNode( _compactMode ); + + Write( "" ); +} + + +void XMLPrinter::PushUnknown( const char* value ) +{ + PrepareForNewNode( _compactMode ); + + Write( "' ); +} + + +bool XMLPrinter::VisitEnter( const XMLDocument& doc ) +{ + _processEntities = doc.ProcessEntities(); + if ( doc.HasBOM() ) { + PushHeader( true, false ); + } + return true; +} + + +bool XMLPrinter::VisitEnter( const XMLElement& element, const XMLAttribute* attribute ) +{ + const XMLElement* parentElem = 0; + if ( element.Parent() ) { + parentElem = element.Parent()->ToElement(); + } + const bool compactMode = parentElem ? CompactMode( *parentElem ) : _compactMode; + OpenElement( element.Name(), compactMode ); + while ( attribute ) { + PushAttribute( attribute->Name(), attribute->Value() ); + attribute = attribute->Next(); + } + return true; +} + + +bool XMLPrinter::VisitExit( const XMLElement& element ) +{ + CloseElement( CompactMode(element) ); + return true; +} + + +bool XMLPrinter::Visit( const XMLText& text ) +{ + PushText( text.Value(), text.CData() ); + return true; +} + + +bool XMLPrinter::Visit( const XMLComment& comment ) +{ + PushComment( comment.Value() ); + return true; +} + +bool XMLPrinter::Visit( const XMLDeclaration& declaration ) +{ + PushDeclaration( declaration.Value() ); + return true; +} + + +bool XMLPrinter::Visit( const XMLUnknown& unknown ) +{ + PushUnknown( unknown.Value() ); + return true; +} + +} // namespace tinyxml2 diff --git a/libs/IO/TinyXML2.h b/libs/IO/TinyXML2.h new file mode 100644 index 0000000..ae237ad --- /dev/null +++ b/libs/IO/TinyXML2.h @@ -0,0 +1,2368 @@ +/* +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#ifndef TINYXML2_INCLUDED +#define TINYXML2_INCLUDED + +#if defined(ANDROID_NDK) || defined(__BORLANDC__) || defined(__QNXNTO__) +# include +# include +# include +# include +# include +# if defined(__PS3__) +# include +# endif +#else +# include +# include +# include +# include +# include +#endif +#include + +/* + TODO: intern strings instead of allocation. +*/ +/* + gcc: + g++ -Wall -DTINYXML2_DEBUG tinyxml2.cpp xmltest.cpp -o gccxmltest.exe + + Formatting, Artistic Style: + AStyle.exe --style=1tbs --indent-switches --break-closing-brackets --indent-preprocessor tinyxml2.cpp tinyxml2.h +*/ + +#if defined( _DEBUG ) || defined (__DEBUG__) +# ifndef TINYXML2_DEBUG +# define TINYXML2_DEBUG +# endif +#endif + +#ifdef _MSC_VER +# pragma warning(push) +# pragma warning(disable: 4251) +#endif + +#ifndef TINYXML2_LIB +#define TINYXML2_LIB IO_API +#endif + +#if !defined(TIXMLASSERT) +#if defined(TINYXML2_DEBUG) +# if defined(_MSC_VER) +# define TIXMLASSERT ASSERT +# elif defined (ANDROID_NDK) +# include +# define TIXMLASSERT( x ) if ( !(x)) { __android_log_assert( "assert", "grinliz", "ASSERT in '%s' at %d.", __FILE__, __LINE__ ); } +# else +# include +# define TIXMLASSERT ASSERT +# endif +#else +# define TIXMLASSERT( x ) {} +#endif +#endif + +/* Versioning, past 1.0.14: + http://semver.org/ +*/ +static const int TIXML2_MAJOR_VERSION = 9; +static const int TIXML2_MINOR_VERSION = 0; +static const int TIXML2_PATCH_VERSION = 0; + +#define TINYXML2_MAJOR_VERSION 9 +#define TINYXML2_MINOR_VERSION 0 +#define TINYXML2_PATCH_VERSION 0 + +// A fixed element depth limit is problematic. There needs to be a +// limit to avoid a stack overflow. However, that limit varies per +// system, and the capacity of the stack. On the other hand, it's a trivial +// attack that can result from ill, malicious, or even correctly formed XML, +// so there needs to be a limit in place. +static const int TINYXML2_MAX_ELEMENT_DEPTH = 100; + +namespace tinyxml2 +{ +class XMLDocument; +class XMLElement; +class XMLAttribute; +class XMLComment; +class XMLText; +class XMLDeclaration; +class XMLUnknown; +class XMLPrinter; + +/* + A class that wraps strings. Normally stores the start and end + pointers into the XML file itself, and will apply normalization + and entity translation if actually read. Can also store (and memory + manage) a traditional char[] + + Isn't clear why TINYXML2_LIB is needed; but seems to fix #719 +*/ +class TINYXML2_LIB StrPair +{ +public: + enum Mode { + NEEDS_ENTITY_PROCESSING = 0x01, + NEEDS_NEWLINE_NORMALIZATION = 0x02, + NEEDS_WHITESPACE_COLLAPSING = 0x04, + + TEXT_ELEMENT = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, + TEXT_ELEMENT_LEAVE_ENTITIES = NEEDS_NEWLINE_NORMALIZATION, + ATTRIBUTE_NAME = 0, + ATTRIBUTE_VALUE = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, + ATTRIBUTE_VALUE_LEAVE_ENTITIES = NEEDS_NEWLINE_NORMALIZATION, + COMMENT = NEEDS_NEWLINE_NORMALIZATION + }; + + StrPair() : _flags( 0 ), _start( 0 ), _end( 0 ) {} + ~StrPair(); + + void Set( char* start, char* end, int flags ) { + TIXMLASSERT( start ); + TIXMLASSERT( end ); + Reset(); + _start = start; + _end = end; + _flags = flags | NEEDS_FLUSH; + } + + const char* GetStr(); + + bool Empty() const { + return _start == _end; + } + + void SetInternedStr( const char* str ) { + Reset(); + _start = const_cast(str); + } + + void SetStr( const char* str, int flags=0 ); + + char* ParseText( char* in, const char* endTag, int strFlags, int* curLineNumPtr ); + char* ParseName( char* in ); + + void TransferTo( StrPair* other ); + void Reset(); + +private: + void CollapseWhitespace(); + + enum { + NEEDS_FLUSH = 0x100, + NEEDS_DELETE = 0x200 + }; + + int _flags; + char* _start; + char* _end; + + StrPair( const StrPair& other ); // not supported + void operator=( const StrPair& other ); // not supported, use TransferTo() +}; + + +/* + A dynamic array of Plain Old Data. Doesn't support constructors, etc. + Has a small initial memory pool, so that low or no usage will not + cause a call to new/delete +*/ +template +class DynArray +{ +public: + DynArray() : + _mem( _pool ), + _allocated( INITIAL_SIZE ), + _size( 0 ) + { + } + + ~DynArray() { + if ( _mem != _pool ) { + delete [] _mem; + } + } + + void Clear() { + _size = 0; + } + + void Push( T t ) { + TIXMLASSERT( _size < INT_MAX ); + EnsureCapacity( _size+1 ); + _mem[_size] = t; + ++_size; + } + + T* PushArr( int count ) { + TIXMLASSERT( count >= 0 ); + TIXMLASSERT( _size <= INT_MAX - count ); + EnsureCapacity( _size+count ); + T* ret = &_mem[_size]; + _size += count; + return ret; + } + + T Pop() { + TIXMLASSERT( _size > 0 ); + --_size; + return _mem[_size]; + } + + void PopArr( int count ) { + TIXMLASSERT( _size >= count ); + _size -= count; + } + + bool Empty() const { + return _size == 0; + } + + T& operator[](int i) { + TIXMLASSERT( i>= 0 && i < _size ); + return _mem[i]; + } + + const T& operator[](int i) const { + TIXMLASSERT( i>= 0 && i < _size ); + return _mem[i]; + } + + const T& PeekTop() const { + TIXMLASSERT( _size > 0 ); + return _mem[ _size - 1]; + } + + int Size() const { + TIXMLASSERT( _size >= 0 ); + return _size; + } + + int Capacity() const { + TIXMLASSERT( _allocated >= INITIAL_SIZE ); + return _allocated; + } + + void SwapRemove(int i) { + TIXMLASSERT(i >= 0 && i < _size); + TIXMLASSERT(_size > 0); + _mem[i] = _mem[_size - 1]; + --_size; + } + + const T* Mem() const { + TIXMLASSERT( _mem ); + return _mem; + } + + T* Mem() { + TIXMLASSERT( _mem ); + return _mem; + } + +private: + DynArray( const DynArray& ); // not supported + void operator=( const DynArray& ); // not supported + + void EnsureCapacity( int cap ) { + TIXMLASSERT( cap > 0 ); + if ( cap > _allocated ) { + TIXMLASSERT( cap <= INT_MAX / 2 ); + const int newAllocated = cap * 2; + T* newMem = new T[newAllocated]; + TIXMLASSERT( newAllocated >= _size ); + memcpy( newMem, _mem, sizeof(T)*_size ); // warning: not using constructors, only works for PODs + if ( _mem != _pool ) { + delete [] _mem; + } + _mem = newMem; + _allocated = newAllocated; + } + } + + T* _mem; + T _pool[INITIAL_SIZE]; + int _allocated; // objects allocated + int _size; // number objects in use +}; + + +/* + Parent virtual class of a pool for fast allocation + and deallocation of objects. +*/ +class MemPool +{ +public: + MemPool() {} + virtual ~MemPool() {} + + virtual int ItemSize() const = 0; + virtual void* Alloc() = 0; + virtual void Free( void* ) = 0; + virtual void SetTracked() = 0; +}; + + +/* + Template child class to create pools of the correct type. +*/ +template< int ITEM_SIZE > +class MemPoolT : public MemPool +{ +public: + MemPoolT() : _blockPtrs(), _root(0), _currentAllocs(0), _nAllocs(0), _maxAllocs(0), _nUntracked(0) {} + ~MemPoolT() { + MemPoolT< ITEM_SIZE >::Clear(); + } + + void Clear() { + // Delete the blocks. + while( !_blockPtrs.Empty()) { + Block* lastBlock = _blockPtrs.Pop(); + delete lastBlock; + } + _root = 0; + _currentAllocs = 0; + _nAllocs = 0; + _maxAllocs = 0; + _nUntracked = 0; + } + + virtual int ItemSize() const { + return ITEM_SIZE; + } + int CurrentAllocs() const { + return _currentAllocs; + } + + virtual void* Alloc() { + if ( !_root ) { + // Need a new block. + Block* block = new Block(); + _blockPtrs.Push( block ); + + Item* blockItems = block->items; + for( int i = 0; i < ITEMS_PER_BLOCK - 1; ++i ) { + blockItems[i].next = &(blockItems[i + 1]); + } + blockItems[ITEMS_PER_BLOCK - 1].next = 0; + _root = blockItems; + } + Item* const result = _root; + TIXMLASSERT( result != 0 ); + _root = _root->next; + + ++_currentAllocs; + if ( _currentAllocs > _maxAllocs ) { + _maxAllocs = _currentAllocs; + } + ++_nAllocs; + ++_nUntracked; + return result; + } + + virtual void Free( void* mem ) { + if ( !mem ) { + return; + } + --_currentAllocs; + Item* item = static_cast( mem ); +#ifdef TINYXML2_DEBUG + memset( item, 0xfe, sizeof( *item ) ); +#endif + item->next = _root; + _root = item; + } + void Trace( const char* name ) { + printf( "Mempool %s watermark=%d [%dk] current=%d size=%d nAlloc=%d blocks=%d\n", + name, _maxAllocs, _maxAllocs * ITEM_SIZE / 1024, _currentAllocs, + ITEM_SIZE, _nAllocs, _blockPtrs.Size() ); + } + + void SetTracked() { + --_nUntracked; + } + + int Untracked() const { + return _nUntracked; + } + + // This number is perf sensitive. 4k seems like a good tradeoff on my machine. + // The test file is large, 170k. + // Release: VS2010 gcc(no opt) + // 1k: 4000 + // 2k: 4000 + // 4k: 3900 21000 + // 16k: 5200 + // 32k: 4300 + // 64k: 4000 21000 + // Declared public because some compilers do not accept to use ITEMS_PER_BLOCK + // in private part if ITEMS_PER_BLOCK is private + enum { ITEMS_PER_BLOCK = (4 * 1024) / ITEM_SIZE }; + +private: + MemPoolT( const MemPoolT& ); // not supported + void operator=( const MemPoolT& ); // not supported + + union Item { + Item* next; + char itemData[ITEM_SIZE]; + }; + struct Block { + Item items[ITEMS_PER_BLOCK]; + }; + DynArray< Block*, 10 > _blockPtrs; + Item* _root; + + int _currentAllocs; + int _nAllocs; + int _maxAllocs; + int _nUntracked; +}; + + + +/** + Implements the interface to the "Visitor pattern" (see the Accept() method.) + If you call the Accept() method, it requires being passed a XMLVisitor + class to handle callbacks. For nodes that contain other nodes (Document, Element) + you will get called with a VisitEnter/VisitExit pair. Nodes that are always leafs + are simply called with Visit(). + + If you return 'true' from a Visit method, recursive parsing will continue. If you return + false, no children of this node or its siblings will be visited. + + All flavors of Visit methods have a default implementation that returns 'true' (continue + visiting). You need to only override methods that are interesting to you. + + Generally Accept() is called on the XMLDocument, although all nodes support visiting. + + You should never change the document from a callback. + + @sa XMLNode::Accept() +*/ +class TINYXML2_LIB XMLVisitor +{ +public: + virtual ~XMLVisitor() {} + + /// Visit a document. + virtual bool VisitEnter( const XMLDocument& /*doc*/ ) { + return true; + } + /// Visit a document. + virtual bool VisitExit( const XMLDocument& /*doc*/ ) { + return true; + } + + /// Visit an element. + virtual bool VisitEnter( const XMLElement& /*element*/, const XMLAttribute* /*firstAttribute*/ ) { + return true; + } + /// Visit an element. + virtual bool VisitExit( const XMLElement& /*element*/ ) { + return true; + } + + /// Visit a declaration. + virtual bool Visit( const XMLDeclaration& /*declaration*/ ) { + return true; + } + /// Visit a text node. + virtual bool Visit( const XMLText& /*text*/ ) { + return true; + } + /// Visit a comment node. + virtual bool Visit( const XMLComment& /*comment*/ ) { + return true; + } + /// Visit an unknown node. + virtual bool Visit( const XMLUnknown& /*unknown*/ ) { + return true; + } +}; + +// WARNING: must match XMLDocument::_errorNames[] +enum XMLError { + XML_SUCCESS = 0, + XML_NO_ATTRIBUTE, + XML_WRONG_ATTRIBUTE_TYPE, + XML_ERROR_FILE_NOT_FOUND, + XML_ERROR_FILE_COULD_NOT_BE_OPENED, + XML_ERROR_FILE_READ_ERROR, + XML_ERROR_PARSING_ELEMENT, + XML_ERROR_PARSING_ATTRIBUTE, + XML_ERROR_PARSING_TEXT, + XML_ERROR_PARSING_CDATA, + XML_ERROR_PARSING_COMMENT, + XML_ERROR_PARSING_DECLARATION, + XML_ERROR_PARSING_UNKNOWN, + XML_ERROR_EMPTY_DOCUMENT, + XML_ERROR_MISMATCHED_ELEMENT, + XML_ERROR_PARSING, + XML_CAN_NOT_CONVERT_TEXT, + XML_NO_TEXT_NODE, + XML_ELEMENT_DEPTH_EXCEEDED, + + XML_ERROR_COUNT +}; + + +/* + Utility functionality. +*/ +class TINYXML2_LIB XMLUtil +{ +public: + static const char* SkipWhiteSpace( const char* p, int* curLineNumPtr ) { + TIXMLASSERT( p ); + + while( IsWhiteSpace(*p) ) { + if (curLineNumPtr && *p == '\n') { + ++(*curLineNumPtr); + } + ++p; + } + TIXMLASSERT( p ); + return p; + } + static char* SkipWhiteSpace( char* const p, int* curLineNumPtr ) { + return const_cast( SkipWhiteSpace( const_cast(p), curLineNumPtr ) ); + } + + // Anything in the high order range of UTF-8 is assumed to not be whitespace. This isn't + // correct, but simple, and usually works. + static bool IsWhiteSpace( char p ) { + return !IsUTF8Continuation(p) && isspace( static_cast(p) ); + } + + inline static bool IsNameStartChar( unsigned char ch ) { + if ( ch >= 128 ) { + // This is a heuristic guess in attempt to not implement Unicode-aware isalpha() + return true; + } + if ( isalpha( ch ) ) { + return true; + } + return ch == ':' || ch == '_'; + } + + inline static bool IsNameChar( unsigned char ch ) { + return IsNameStartChar( ch ) + || isdigit( ch ) + || ch == '.' + || ch == '-'; + } + + inline static bool IsPrefixHex( const char* p) { + p = SkipWhiteSpace(p, 0); + return p && *p == '0' && ( *(p + 1) == 'x' || *(p + 1) == 'X'); + } + + inline static bool StringEqual( const char* p, const char* q, int nChar=INT_MAX ) { + if ( p == q ) { + return true; + } + TIXMLASSERT( p ); + TIXMLASSERT( q ); + TIXMLASSERT( nChar >= 0 ); + return strncmp( p, q, nChar ) == 0; + } + + inline static bool IsUTF8Continuation( const char p ) { + return ( p & 0x80 ) != 0; + } + + static const char* ReadBOM( const char* p, bool* hasBOM ); + // p is the starting location, + // the UTF-8 value of the entity will be placed in value, and length filled in. + static const char* GetCharacterRef( const char* p, char* value, int* length ); + static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); + + // converts primitive types to strings + static void ToStr( int v, char* buffer, int bufferSize ); + static void ToStr( unsigned v, char* buffer, int bufferSize ); + static void ToStr( bool v, char* buffer, int bufferSize ); + static void ToStr( float v, char* buffer, int bufferSize ); + static void ToStr( double v, char* buffer, int bufferSize ); + static void ToStr(int64_t v, char* buffer, int bufferSize); + static void ToStr(uint64_t v, char* buffer, int bufferSize); + + // converts strings to primitive types + static bool ToInt( const char* str, int* value ); + static bool ToUnsigned( const char* str, unsigned* value ); + static bool ToBool( const char* str, bool* value ); + static bool ToFloat( const char* str, float* value ); + static bool ToDouble( const char* str, double* value ); + static bool ToInt64(const char* str, int64_t* value); + static bool ToUnsigned64(const char* str, uint64_t* value); + // Changes what is serialized for a boolean value. + // Default to "true" and "false". Shouldn't be changed + // unless you have a special testing or compatibility need. + // Be careful: static, global, & not thread safe. + // Be sure to set static const memory as parameters. + static void SetBoolSerialization(const char* writeTrue, const char* writeFalse); + +private: + static const char* writeBoolTrue; + static const char* writeBoolFalse; +}; + + +/** XMLNode is a base class for every object that is in the + XML Document Object Model (DOM), except XMLAttributes. + Nodes have siblings, a parent, and children which can + be navigated. A node is always in a XMLDocument. + The type of a XMLNode can be queried, and it can + be cast to its more defined type. + + A XMLDocument allocates memory for all its Nodes. + When the XMLDocument gets deleted, all its Nodes + will also be deleted. + + @verbatim + A Document can contain: Element (container or leaf) + Comment (leaf) + Unknown (leaf) + Declaration( leaf ) + + An Element can contain: Element (container or leaf) + Text (leaf) + Attributes (not on tree) + Comment (leaf) + Unknown (leaf) + + @endverbatim +*/ +class TINYXML2_LIB XMLNode +{ + friend class XMLDocument; + friend class XMLElement; +public: + + /// Get the XMLDocument that owns this XMLNode. + const XMLDocument* GetDocument() const { + TIXMLASSERT( _document ); + return _document; + } + /// Get the XMLDocument that owns this XMLNode. + XMLDocument* GetDocument() { + TIXMLASSERT( _document ); + return _document; + } + + /// Safely cast to an Element, or null. + virtual XMLElement* ToElement() { + return 0; + } + /// Safely cast to Text, or null. + virtual XMLText* ToText() { + return 0; + } + /// Safely cast to a Comment, or null. + virtual XMLComment* ToComment() { + return 0; + } + /// Safely cast to a Document, or null. + virtual XMLDocument* ToDocument() { + return 0; + } + /// Safely cast to a Declaration, or null. + virtual XMLDeclaration* ToDeclaration() { + return 0; + } + /// Safely cast to an Unknown, or null. + virtual XMLUnknown* ToUnknown() { + return 0; + } + + virtual const XMLElement* ToElement() const { + return 0; + } + virtual const XMLText* ToText() const { + return 0; + } + virtual const XMLComment* ToComment() const { + return 0; + } + virtual const XMLDocument* ToDocument() const { + return 0; + } + virtual const XMLDeclaration* ToDeclaration() const { + return 0; + } + virtual const XMLUnknown* ToUnknown() const { + return 0; + } + + /** The meaning of 'value' changes for the specific type. + @verbatim + Document: empty (NULL is returned, not an empty string) + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + */ + const char* Value() const; + + /** Set the Value of an XML node. + @sa Value() + */ + void SetValue( const char* val, bool staticMem=false ); + + /// Gets the line number the node is in, if the document was parsed from a file. + int GetLineNum() const { return _parseLineNum; } + + /// Get the parent of this node on the DOM. + const XMLNode* Parent() const { + return _parent; + } + + XMLNode* Parent() { + return _parent; + } + + /// Returns true if this node has no children. + bool NoChildren() const { + return !_firstChild; + } + + /// Get the first child node, or null if none exists. + const XMLNode* FirstChild() const { + return _firstChild; + } + + XMLNode* FirstChild() { + return _firstChild; + } + + /** Get the first child element, or optionally the first child + element with the specified name. + */ + const XMLElement* FirstChildElement( const char* name = 0 ) const; + + XMLElement* FirstChildElement( const char* name = 0 ) { + return const_cast(const_cast(this)->FirstChildElement( name )); + } + + /// Get the last child node, or null if none exists. + const XMLNode* LastChild() const { + return _lastChild; + } + + XMLNode* LastChild() { + return _lastChild; + } + + /** Get the last child element or optionally the last child + element with the specified name. + */ + const XMLElement* LastChildElement( const char* name = 0 ) const; + + XMLElement* LastChildElement( const char* name = 0 ) { + return const_cast(const_cast(this)->LastChildElement(name) ); + } + + /// Get the previous (left) sibling node of this node. + const XMLNode* PreviousSibling() const { + return _prev; + } + + XMLNode* PreviousSibling() { + return _prev; + } + + /// Get the previous (left) sibling element of this node, with an optionally supplied name. + const XMLElement* PreviousSiblingElement( const char* name = 0 ) const ; + + XMLElement* PreviousSiblingElement( const char* name = 0 ) { + return const_cast(const_cast(this)->PreviousSiblingElement( name ) ); + } + + /// Get the next (right) sibling node of this node. + const XMLNode* NextSibling() const { + return _next; + } + + XMLNode* NextSibling() { + return _next; + } + + /// Get the next (right) sibling element of this node, with an optionally supplied name. + const XMLElement* NextSiblingElement( const char* name = 0 ) const; + + XMLElement* NextSiblingElement( const char* name = 0 ) { + return const_cast(const_cast(this)->NextSiblingElement( name ) ); + } + + /** + Add a child node as the last (right) child. + If the child node is already part of the document, + it is moved from its old location to the new location. + Returns the addThis argument or 0 if the node does not + belong to the same document. + */ + XMLNode* InsertEndChild( XMLNode* addThis ); + + XMLNode* LinkEndChild( XMLNode* addThis ) { + return InsertEndChild( addThis ); + } + /** + Add a child node as the first (left) child. + If the child node is already part of the document, + it is moved from its old location to the new location. + Returns the addThis argument or 0 if the node does not + belong to the same document. + */ + XMLNode* InsertFirstChild( XMLNode* addThis ); + /** + Add a node after the specified child node. + If the child node is already part of the document, + it is moved from its old location to the new location. + Returns the addThis argument or 0 if the afterThis node + is not a child of this node, or if the node does not + belong to the same document. + */ + XMLNode* InsertAfterChild( XMLNode* afterThis, XMLNode* addThis ); + + /** + Delete all the children of this node. + */ + void DeleteChildren(); + + /** + Delete a child of this node. + */ + void DeleteChild( XMLNode* node ); + + /** + Make a copy of this node, but not its children. + You may pass in a Document pointer that will be + the owner of the new Node. If the 'document' is + null, then the node returned will be allocated + from the current Document. (this->GetDocument()) + + Note: if called on a XMLDocument, this will return null. + */ + virtual XMLNode* ShallowClone( XMLDocument* document ) const = 0; + + /** + Make a copy of this node and all its children. + + If the 'target' is null, then the nodes will + be allocated in the current document. If 'target' + is specified, the memory will be allocated is the + specified XMLDocument. + + NOTE: This is probably not the correct tool to + copy a document, since XMLDocuments can have multiple + top level XMLNodes. You probably want to use + XMLDocument::DeepCopy() + */ + XMLNode* DeepClone( XMLDocument* target ) const; + + /** + Test if 2 nodes are the same, but don't test children. + The 2 nodes do not need to be in the same Document. + + Note: if called on a XMLDocument, this will return false. + */ + virtual bool ShallowEqual( const XMLNode* compare ) const = 0; + + /** Accept a hierarchical visit of the nodes in the TinyXML-2 DOM. Every node in the + XML tree will be conditionally visited and the host will be called back + via the XMLVisitor interface. + + This is essentially a SAX interface for TinyXML-2. (Note however it doesn't re-parse + the XML for the callbacks, so the performance of TinyXML-2 is unchanged by using this + interface versus any other.) + + The interface has been based on ideas from: + + - http://www.saxproject.org/ + - http://c2.com/cgi/wiki?HierarchicalVisitorPattern + + Which are both good references for "visiting". + + An example of using Accept(): + @verbatim + XMLPrinter printer; + tinyxmlDoc.Accept( &printer ); + const char* xmlcstr = printer.CStr(); + @endverbatim + */ + virtual bool Accept( XMLVisitor* visitor ) const = 0; + + /** + Set user data into the XMLNode. TinyXML-2 in + no way processes or interprets user data. + It is initially 0. + */ + void SetUserData(void* userData) { _userData = userData; } + + /** + Get user data set into the XMLNode. TinyXML-2 in + no way processes or interprets user data. + It is initially 0. + */ + void* GetUserData() const { return _userData; } + +protected: + explicit XMLNode( XMLDocument* ); + virtual ~XMLNode(); + + virtual char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr); + + XMLDocument* _document; + XMLNode* _parent; + mutable StrPair _value; + int _parseLineNum; + + XMLNode* _firstChild; + XMLNode* _lastChild; + + XMLNode* _prev; + XMLNode* _next; + + void* _userData; + +private: + MemPool* _memPool; + void Unlink( XMLNode* child ); + static void DeleteNode( XMLNode* node ); + void InsertChildPreamble( XMLNode* insertThis ) const; + const XMLElement* ToElementWithName( const char* name ) const; + + XMLNode( const XMLNode& ); // not supported + XMLNode& operator=( const XMLNode& ); // not supported +}; + + +/** XML text. + + Note that a text node can have child element nodes, for example: + @verbatim + This is bold + @endverbatim + + A text node can have 2 ways to output the next. "normal" output + and CDATA. It will default to the mode it was parsed from the XML file and + you generally want to leave it alone, but you can change the output mode with + SetCData() and query it with CData(). +*/ +class TINYXML2_LIB XMLText : public XMLNode +{ + friend class XMLDocument; +public: + virtual bool Accept( XMLVisitor* visitor ) const; + + virtual XMLText* ToText() { + return this; + } + virtual const XMLText* ToText() const { + return this; + } + + /// Declare whether this should be CDATA or standard text. + void SetCData( bool isCData ) { + _isCData = isCData; + } + /// Returns true if this is a CDATA text element. + bool CData() const { + return _isCData; + } + + virtual XMLNode* ShallowClone( XMLDocument* document ) const; + virtual bool ShallowEqual( const XMLNode* compare ) const; + +protected: + explicit XMLText( XMLDocument* doc ) : XMLNode( doc ), _isCData( false ) {} + virtual ~XMLText() {} + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ); + +private: + bool _isCData; + + XMLText( const XMLText& ); // not supported + XMLText& operator=( const XMLText& ); // not supported +}; + + +/** An XML Comment. */ +class TINYXML2_LIB XMLComment : public XMLNode +{ + friend class XMLDocument; +public: + virtual XMLComment* ToComment() { + return this; + } + virtual const XMLComment* ToComment() const { + return this; + } + + virtual bool Accept( XMLVisitor* visitor ) const; + + virtual XMLNode* ShallowClone( XMLDocument* document ) const; + virtual bool ShallowEqual( const XMLNode* compare ) const; + +protected: + explicit XMLComment( XMLDocument* doc ); + virtual ~XMLComment(); + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr); + +private: + XMLComment( const XMLComment& ); // not supported + XMLComment& operator=( const XMLComment& ); // not supported +}; + + +/** In correct XML the declaration is the first entry in the file. + @verbatim + + @endverbatim + + TinyXML-2 will happily read or write files without a declaration, + however. + + The text of the declaration isn't interpreted. It is parsed + and written as a string. +*/ +class TINYXML2_LIB XMLDeclaration : public XMLNode +{ + friend class XMLDocument; +public: + virtual XMLDeclaration* ToDeclaration() { + return this; + } + virtual const XMLDeclaration* ToDeclaration() const { + return this; + } + + virtual bool Accept( XMLVisitor* visitor ) const; + + virtual XMLNode* ShallowClone( XMLDocument* document ) const; + virtual bool ShallowEqual( const XMLNode* compare ) const; + +protected: + explicit XMLDeclaration( XMLDocument* doc ); + virtual ~XMLDeclaration(); + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ); + +private: + XMLDeclaration( const XMLDeclaration& ); // not supported + XMLDeclaration& operator=( const XMLDeclaration& ); // not supported +}; + + +/** Any tag that TinyXML-2 doesn't recognize is saved as an + unknown. It is a tag of text, but should not be modified. + It will be written back to the XML, unchanged, when the file + is saved. + + DTD tags get thrown into XMLUnknowns. +*/ +class TINYXML2_LIB XMLUnknown : public XMLNode +{ + friend class XMLDocument; +public: + virtual XMLUnknown* ToUnknown() { + return this; + } + virtual const XMLUnknown* ToUnknown() const { + return this; + } + + virtual bool Accept( XMLVisitor* visitor ) const; + + virtual XMLNode* ShallowClone( XMLDocument* document ) const; + virtual bool ShallowEqual( const XMLNode* compare ) const; + +protected: + explicit XMLUnknown( XMLDocument* doc ); + virtual ~XMLUnknown(); + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ); + +private: + XMLUnknown( const XMLUnknown& ); // not supported + XMLUnknown& operator=( const XMLUnknown& ); // not supported +}; + + + +/** An attribute is a name-value pair. Elements have an arbitrary + number of attributes, each with a unique name. + + @note The attributes are not XMLNodes. You may only query the + Next() attribute in a list. +*/ +class TINYXML2_LIB XMLAttribute +{ + friend class XMLElement; +public: + /// The name of the attribute. + const char* Name() const; + + /// The value of the attribute. + const char* Value() const; + + /// Gets the line number the attribute is in, if the document was parsed from a file. + int GetLineNum() const { return _parseLineNum; } + + /// The next attribute in the list. + const XMLAttribute* Next() const { + return _next; + } + + /** IntValue interprets the attribute as an integer, and returns the value. + If the value isn't an integer, 0 will be returned. There is no error checking; + use QueryIntValue() if you need error checking. + */ + int IntValue() const { + int i = 0; + QueryIntValue(&i); + return i; + } + + int64_t Int64Value() const { + int64_t i = 0; + QueryInt64Value(&i); + return i; + } + + uint64_t Unsigned64Value() const { + uint64_t i = 0; + QueryUnsigned64Value(&i); + return i; + } + + /// Query as an unsigned integer. See IntValue() + unsigned UnsignedValue() const { + unsigned i=0; + QueryUnsignedValue( &i ); + return i; + } + /// Query as a boolean. See IntValue() + bool BoolValue() const { + bool b=false; + QueryBoolValue( &b ); + return b; + } + /// Query as a double. See IntValue() + double DoubleValue() const { + double d=0; + QueryDoubleValue( &d ); + return d; + } + /// Query as a float. See IntValue() + float FloatValue() const { + float f=0; + QueryFloatValue( &f ); + return f; + } + + /** QueryIntValue interprets the attribute as an integer, and returns the value + in the provided parameter. The function will return XML_SUCCESS on success, + and XML_WRONG_ATTRIBUTE_TYPE if the conversion is not successful. + */ + XMLError QueryIntValue( int* value ) const; + /// See QueryIntValue + XMLError QueryUnsignedValue( unsigned int* value ) const; + /// See QueryIntValue + XMLError QueryInt64Value(int64_t* value) const; + /// See QueryIntValue + XMLError QueryUnsigned64Value(uint64_t* value) const; + /// See QueryIntValue + XMLError QueryBoolValue( bool* value ) const; + /// See QueryIntValue + XMLError QueryDoubleValue( double* value ) const; + /// See QueryIntValue + XMLError QueryFloatValue( float* value ) const; + + /// Set the attribute to a string value. + void SetAttribute( const char* value ); + /// Set the attribute to value. + void SetAttribute( int value ); + /// Set the attribute to value. + void SetAttribute( unsigned value ); + /// Set the attribute to value. + void SetAttribute(int64_t value); + /// Set the attribute to value. + void SetAttribute(uint64_t value); + /// Set the attribute to value. + void SetAttribute( bool value ); + /// Set the attribute to value. + void SetAttribute( double value ); + /// Set the attribute to value. + void SetAttribute( float value ); + +private: + enum { BUF_SIZE = 200 }; + + XMLAttribute() : _name(), _value(),_parseLineNum( 0 ), _next( 0 ), _memPool( 0 ) {} + virtual ~XMLAttribute() {} + + XMLAttribute( const XMLAttribute& ); // not supported + void operator=( const XMLAttribute& ); // not supported + void SetName( const char* name ); + + char* ParseDeep( char* p, bool processEntities, int* curLineNumPtr ); + + mutable StrPair _name; + mutable StrPair _value; + int _parseLineNum; + XMLAttribute* _next; + MemPool* _memPool; +}; + + +/** The element is a container class. It has a value, the element name, + and can contain other elements, text, comments, and unknowns. + Elements also contain an arbitrary number of attributes. +*/ +class TINYXML2_LIB XMLElement : public XMLNode +{ + friend class XMLDocument; +public: + /// Get the name of an element (which is the Value() of the node.) + const char* Name() const { + return Value(); + } + /// Set the name of the element. + void SetName( const char* str, bool staticMem=false ) { + SetValue( str, staticMem ); + } + + virtual XMLElement* ToElement() { + return this; + } + virtual const XMLElement* ToElement() const { + return this; + } + virtual bool Accept( XMLVisitor* visitor ) const; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none + exists. For example: + + @verbatim + const char* value = ele->Attribute( "foo" ); + @endverbatim + + The 'value' parameter is normally null. However, if specified, + the attribute will only be returned if the 'name' and 'value' + match. This allow you to write code: + + @verbatim + if ( ele->Attribute( "foo", "bar" ) ) callFooIsBar(); + @endverbatim + + rather than: + @verbatim + if ( ele->Attribute( "foo" ) ) { + if ( strcmp( ele->Attribute( "foo" ), "bar" ) == 0 ) callFooIsBar(); + } + @endverbatim + */ + const char* Attribute( const char* name, const char* value=0 ) const; + + /** Given an attribute name, IntAttribute() returns the value + of the attribute interpreted as an integer. The default + value will be returned if the attribute isn't present, + or if there is an error. (For a method with error + checking, see QueryIntAttribute()). + */ + int IntAttribute(const char* name, int defaultValue = 0) const; + /// See IntAttribute() + unsigned UnsignedAttribute(const char* name, unsigned defaultValue = 0) const; + /// See IntAttribute() + int64_t Int64Attribute(const char* name, int64_t defaultValue = 0) const; + /// See IntAttribute() + uint64_t Unsigned64Attribute(const char* name, uint64_t defaultValue = 0) const; + /// See IntAttribute() + bool BoolAttribute(const char* name, bool defaultValue = false) const; + /// See IntAttribute() + double DoubleAttribute(const char* name, double defaultValue = 0) const; + /// See IntAttribute() + float FloatAttribute(const char* name, float defaultValue = 0) const; + + /** Given an attribute name, QueryIntAttribute() returns + XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion + can't be performed, or XML_NO_ATTRIBUTE if the attribute + doesn't exist. If successful, the result of the conversion + will be written to 'value'. If not successful, nothing will + be written to 'value'. This allows you to provide default + value: + + @verbatim + int value = 10; + QueryIntAttribute( "foo", &value ); // if "foo" isn't found, value will still be 10 + @endverbatim + */ + XMLError QueryIntAttribute( const char* name, int* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryIntValue( value ); + } + + /// See QueryIntAttribute() + XMLError QueryUnsignedAttribute( const char* name, unsigned int* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryUnsignedValue( value ); + } + + /// See QueryIntAttribute() + XMLError QueryInt64Attribute(const char* name, int64_t* value) const { + const XMLAttribute* a = FindAttribute(name); + if (!a) { + return XML_NO_ATTRIBUTE; + } + return a->QueryInt64Value(value); + } + + /// See QueryIntAttribute() + XMLError QueryUnsigned64Attribute(const char* name, uint64_t* value) const { + const XMLAttribute* a = FindAttribute(name); + if(!a) { + return XML_NO_ATTRIBUTE; + } + return a->QueryUnsigned64Value(value); + } + + /// See QueryIntAttribute() + XMLError QueryBoolAttribute( const char* name, bool* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryBoolValue( value ); + } + /// See QueryIntAttribute() + XMLError QueryDoubleAttribute( const char* name, double* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryDoubleValue( value ); + } + /// See QueryIntAttribute() + XMLError QueryFloatAttribute( const char* name, float* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryFloatValue( value ); + } + + /// See QueryIntAttribute() + XMLError QueryStringAttribute(const char* name, const char** value) const { + const XMLAttribute* a = FindAttribute(name); + if (!a) { + return XML_NO_ATTRIBUTE; + } + *value = a->Value(); + return XML_SUCCESS; + } + + + + /** Given an attribute name, QueryAttribute() returns + XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion + can't be performed, or XML_NO_ATTRIBUTE if the attribute + doesn't exist. It is overloaded for the primitive types, + and is a generally more convenient replacement of + QueryIntAttribute() and related functions. + + If successful, the result of the conversion + will be written to 'value'. If not successful, nothing will + be written to 'value'. This allows you to provide default + value: + + @verbatim + int value = 10; + QueryAttribute( "foo", &value ); // if "foo" isn't found, value will still be 10 + @endverbatim + */ + XMLError QueryAttribute( const char* name, int* value ) const { + return QueryIntAttribute( name, value ); + } + + XMLError QueryAttribute( const char* name, unsigned int* value ) const { + return QueryUnsignedAttribute( name, value ); + } + + XMLError QueryAttribute(const char* name, int64_t* value) const { + return QueryInt64Attribute(name, value); + } + + XMLError QueryAttribute(const char* name, uint64_t* value) const { + return QueryUnsigned64Attribute(name, value); + } + + XMLError QueryAttribute( const char* name, bool* value ) const { + return QueryBoolAttribute( name, value ); + } + + XMLError QueryAttribute( const char* name, double* value ) const { + return QueryDoubleAttribute( name, value ); + } + + XMLError QueryAttribute( const char* name, float* value ) const { + return QueryFloatAttribute( name, value ); + } + + XMLError QueryAttribute(const char* name, const char** value) const { + return QueryStringAttribute(name, value); + } + + /// Sets the named attribute to value. + void SetAttribute( const char* name, const char* value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, int value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, unsigned value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + + /// Sets the named attribute to value. + void SetAttribute(const char* name, int64_t value) { + XMLAttribute* a = FindOrCreateAttribute(name); + a->SetAttribute(value); + } + + /// Sets the named attribute to value. + void SetAttribute(const char* name, uint64_t value) { + XMLAttribute* a = FindOrCreateAttribute(name); + a->SetAttribute(value); + } + + /// Sets the named attribute to value. + void SetAttribute( const char* name, bool value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, double value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, float value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + + /** + Delete an attribute. + */ + void DeleteAttribute( const char* name ); + + /// Return the first attribute in the list. + const XMLAttribute* FirstAttribute() const { + return _rootAttribute; + } + /// Query a specific attribute in the list. + const XMLAttribute* FindAttribute( const char* name ) const; + + /** Convenience function for easy access to the text inside an element. Although easy + and concise, GetText() is limited compared to getting the XMLText child + and accessing it directly. + + If the first child of 'this' is a XMLText, the GetText() + returns the character string of the Text node, else null is returned. + + This is a convenient method for getting the text of simple contained text: + @verbatim + This is text + const char* str = fooElement->GetText(); + @endverbatim + + 'str' will be a pointer to "This is text". + + Note that this function can be misleading. If the element foo was created from + this XML: + @verbatim + This is text + @endverbatim + + then the value of str would be null. The first child node isn't a text node, it is + another element. From this XML: + @verbatim + This is text + @endverbatim + GetText() will return "This is ". + */ + const char* GetText() const; + + /** Convenience function for easy access to the text inside an element. Although easy + and concise, SetText() is limited compared to creating an XMLText child + and mutating it directly. + + If the first child of 'this' is a XMLText, SetText() sets its value to + the given string, otherwise it will create a first child that is an XMLText. + + This is a convenient method for setting the text of simple contained text: + @verbatim + This is text + fooElement->SetText( "Hullaballoo!" ); + Hullaballoo! + @endverbatim + + Note that this function can be misleading. If the element foo was created from + this XML: + @verbatim + This is text + @endverbatim + + then it will not change "This is text", but rather prefix it with a text element: + @verbatim + Hullaballoo!This is text + @endverbatim + + For this XML: + @verbatim + + @endverbatim + SetText() will generate + @verbatim + Hullaballoo! + @endverbatim + */ + void SetText( const char* inText ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( int value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( unsigned value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText(int64_t value); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText(uint64_t value); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( bool value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( double value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( float value ); + + /** + Convenience method to query the value of a child text node. This is probably best + shown by example. Given you have a document is this form: + @verbatim + + 1 + 1.4 + + @endverbatim + + The QueryIntText() and similar functions provide a safe and easier way to get to the + "value" of x and y. + + @verbatim + int x = 0; + float y = 0; // types of x and y are contrived for example + const XMLElement* xElement = pointElement->FirstChildElement( "x" ); + const XMLElement* yElement = pointElement->FirstChildElement( "y" ); + xElement->QueryIntText( &x ); + yElement->QueryFloatText( &y ); + @endverbatim + + @returns XML_SUCCESS (0) on success, XML_CAN_NOT_CONVERT_TEXT if the text cannot be converted + to the requested type, and XML_NO_TEXT_NODE if there is no child text to query. + + */ + XMLError QueryIntText( int* ival ) const; + /// See QueryIntText() + XMLError QueryUnsignedText( unsigned* uval ) const; + /// See QueryIntText() + XMLError QueryInt64Text(int64_t* uval) const; + /// See QueryIntText() + XMLError QueryUnsigned64Text(uint64_t* uval) const; + /// See QueryIntText() + XMLError QueryBoolText( bool* bval ) const; + /// See QueryIntText() + XMLError QueryDoubleText( double* dval ) const; + /// See QueryIntText() + XMLError QueryFloatText( float* fval ) const; + + int IntText(int defaultValue = 0) const; + + /// See QueryIntText() + unsigned UnsignedText(unsigned defaultValue = 0) const; + /// See QueryIntText() + int64_t Int64Text(int64_t defaultValue = 0) const; + /// See QueryIntText() + uint64_t Unsigned64Text(uint64_t defaultValue = 0) const; + /// See QueryIntText() + bool BoolText(bool defaultValue = false) const; + /// See QueryIntText() + double DoubleText(double defaultValue = 0) const; + /// See QueryIntText() + float FloatText(float defaultValue = 0) const; + + /** + Convenience method to create a new XMLElement and add it as last (right) + child of this node. Returns the created and inserted element. + */ + XMLElement* InsertNewChildElement(const char* name); + /// See InsertNewChildElement() + XMLComment* InsertNewComment(const char* comment); + /// See InsertNewChildElement() + XMLText* InsertNewText(const char* text); + /// See InsertNewChildElement() + XMLDeclaration* InsertNewDeclaration(const char* text); + /// See InsertNewChildElement() + XMLUnknown* InsertNewUnknown(const char* text); + + + // internal: + enum ElementClosingType { + OPEN, // + CLOSED, // + CLOSING // + }; + ElementClosingType ClosingType() const { + return _closingType; + } + virtual XMLNode* ShallowClone( XMLDocument* document ) const; + virtual bool ShallowEqual( const XMLNode* compare ) const; + +protected: + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ); + +private: + XMLElement( XMLDocument* doc ); + virtual ~XMLElement(); + XMLElement( const XMLElement& ); // not supported + void operator=( const XMLElement& ); // not supported + + XMLAttribute* FindOrCreateAttribute( const char* name ); + char* ParseAttributes( char* p, int* curLineNumPtr ); + static void DeleteAttribute( XMLAttribute* attribute ); + XMLAttribute* CreateAttribute(); + + enum { BUF_SIZE = 200 }; + ElementClosingType _closingType; + // The attribute list is ordered; there is no 'lastAttribute' + // because the list needs to be scanned for dupes before adding + // a new attribute. + XMLAttribute* _rootAttribute; +}; + + +enum Whitespace { + PRESERVE_WHITESPACE, + COLLAPSE_WHITESPACE +}; + + +/** A Document binds together all the functionality. + It can be saved, loaded, and printed to the screen. + All Nodes are connected and allocated to a Document. + If the Document is deleted, all its Nodes are also deleted. +*/ +class TINYXML2_LIB XMLDocument : public XMLNode +{ + friend class XMLElement; + // Gives access to SetError and Push/PopDepth, but over-access for everything else. + // Wishing C++ had "internal" scope. + friend class XMLNode; + friend class XMLText; + friend class XMLComment; + friend class XMLDeclaration; + friend class XMLUnknown; +public: + /// constructor + XMLDocument( bool processEntities = true, Whitespace whitespaceMode = PRESERVE_WHITESPACE ); + ~XMLDocument(); + + virtual XMLDocument* ToDocument() { + TIXMLASSERT( this == _document ); + return this; + } + virtual const XMLDocument* ToDocument() const { + TIXMLASSERT( this == _document ); + return this; + } + + /** + Parse an XML file from a character string. + Returns XML_SUCCESS (0) on success, or + an errorID. + + You may optionally pass in the 'nBytes', which is + the number of bytes which will be parsed. If not + specified, TinyXML-2 will assume 'xml' points to a + null terminated string. + */ + XMLError Parse( const char* xml, size_t nBytes=static_cast(-1) ); + + /** + Load an XML file from disk. + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError LoadFile( const char* filename ); + + /** + Load an XML file from disk. You are responsible + for providing and closing the FILE*. + + NOTE: The file should be opened as binary ("rb") + not text in order for TinyXML-2 to correctly + do newline normalization. + + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError LoadFile( FILE* ); + + /** + Save the XML file to disk. + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError SaveFile( const char* filename, bool compact = false ); + + /** + Save the XML file to disk. You are responsible + for providing and closing the FILE*. + + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError SaveFile( FILE* fp, bool compact = false ); + + bool ProcessEntities() const { + return _processEntities; + } + Whitespace WhitespaceMode() const { + return _whitespaceMode; + } + + /** + Returns true if this document has a leading Byte Order Mark of UTF8. + */ + bool HasBOM() const { + return _writeBOM; + } + /** Sets whether to write the BOM when writing the file. + */ + void SetBOM( bool useBOM ) { + _writeBOM = useBOM; + } + + /** Return the root element of DOM. Equivalent to FirstChildElement(). + To get the first node, use FirstChild(). + */ + XMLElement* RootElement() { + return FirstChildElement(); + } + const XMLElement* RootElement() const { + return FirstChildElement(); + } + + /** Print the Document. If the Printer is not provided, it will + print to stdout. If you provide Printer, this can print to a file: + @verbatim + XMLPrinter printer( fp ); + doc.Print( &printer ); + @endverbatim + + Or you can use a printer to print to memory: + @verbatim + XMLPrinter printer; + doc.Print( &printer ); + // printer.CStr() has a const char* to the XML + @endverbatim + */ + void Print( XMLPrinter* streamer=0 ) const; + virtual bool Accept( XMLVisitor* visitor ) const; + + /** + Create a new Element associated with + this Document. The memory for the Element + is managed by the Document. + */ + XMLElement* NewElement( const char* name ); + /** + Create a new Comment associated with + this Document. The memory for the Comment + is managed by the Document. + */ + XMLComment* NewComment( const char* comment ); + /** + Create a new Text associated with + this Document. The memory for the Text + is managed by the Document. + */ + XMLText* NewText( const char* text ); + /** + Create a new Declaration associated with + this Document. The memory for the object + is managed by the Document. + + If the 'text' param is null, the standard + declaration is used.: + @verbatim + + @endverbatim + */ + XMLDeclaration* NewDeclaration( const char* text=0 ); + /** + Create a new Unknown associated with + this Document. The memory for the object + is managed by the Document. + */ + XMLUnknown* NewUnknown( const char* text ); + + /** + Delete a node associated with this document. + It will be unlinked from the DOM. + */ + void DeleteNode( XMLNode* node ); + + /// Clears the error flags. + void ClearError(); + + /// Return true if there was an error parsing the document. + bool Error() const { + return _errorID != XML_SUCCESS; + } + /// Return the errorID. + XMLError ErrorID() const { + return _errorID; + } + const char* ErrorName() const; + static const char* ErrorIDToName(XMLError errorID); + + /** Returns a "long form" error description. A hopefully helpful + diagnostic with location, line number, and/or additional info. + */ + const char* ErrorStr() const; + + /// A (trivial) utility function that prints the ErrorStr() to stdout. + void PrintError() const; + + /// Return the line where the error occurred, or zero if unknown. + int ErrorLineNum() const + { + return _errorLineNum; + } + + /// Clear the document, resetting it to the initial state. + void Clear(); + + /** + Copies this document to a target document. + The target will be completely cleared before the copy. + If you want to copy a sub-tree, see XMLNode::DeepClone(). + + NOTE: that the 'target' must be non-null. + */ + void DeepCopy(XMLDocument* target) const; + + // internal + char* Identify( char* p, XMLNode** node ); + + // internal + void MarkInUse(const XMLNode* const); + + virtual XMLNode* ShallowClone( XMLDocument* /*document*/ ) const { + return 0; + } + virtual bool ShallowEqual( const XMLNode* /*compare*/ ) const { + return false; + } + +private: + XMLDocument( const XMLDocument& ); // not supported + void operator=( const XMLDocument& ); // not supported + + bool _writeBOM; + bool _processEntities; + XMLError _errorID; + Whitespace _whitespaceMode; + mutable StrPair _errorStr; + int _errorLineNum; + char* _charBuffer; + int _parseCurLineNum; + int _parsingDepth; + // Memory tracking does add some overhead. + // However, the code assumes that you don't + // have a bunch of unlinked nodes around. + // Therefore it takes less memory to track + // in the document vs. a linked list in the XMLNode, + // and the performance is the same. + DynArray _unlinked; + + MemPoolT< sizeof(XMLElement) > _elementPool; + MemPoolT< sizeof(XMLAttribute) > _attributePool; + MemPoolT< sizeof(XMLText) > _textPool; + MemPoolT< sizeof(XMLComment) > _commentPool; + + static const char* _errorNames[XML_ERROR_COUNT]; + + void Parse(); + + void SetError( XMLError error, int lineNum, const char* format, ... ); + + // Something of an obvious security hole, once it was discovered. + // Either an ill-formed XML or an excessively deep one can overflow + // the stack. Track stack depth, and error out if needed. + class DepthTracker { + public: + explicit DepthTracker(XMLDocument * document) { + this->_document = document; + document->PushDepth(); + } + ~DepthTracker() { + _document->PopDepth(); + } + private: + XMLDocument * _document; + }; + void PushDepth(); + void PopDepth(); + + template + NodeType* CreateUnlinkedNode( MemPoolT& pool ); +}; + +template +inline NodeType* XMLDocument::CreateUnlinkedNode( MemPoolT& pool ) +{ + TIXMLASSERT( sizeof( NodeType ) == PoolElementSize ); + TIXMLASSERT( sizeof( NodeType ) == pool.ItemSize() ); + NodeType* returnNode = new (pool.Alloc()) NodeType( this ); + TIXMLASSERT( returnNode ); + returnNode->_memPool = &pool; + + _unlinked.Push(returnNode); + return returnNode; +} + +/** + A XMLHandle is a class that wraps a node pointer with null checks; this is + an incredibly useful thing. Note that XMLHandle is not part of the TinyXML-2 + DOM structure. It is a separate utility class. + + Take an example: + @verbatim + + + + + + + @endverbatim + + Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very + easy to write a *lot* of code that looks like: + + @verbatim + XMLElement* root = document.FirstChildElement( "Document" ); + if ( root ) + { + XMLElement* element = root->FirstChildElement( "Element" ); + if ( element ) + { + XMLElement* child = element->FirstChildElement( "Child" ); + if ( child ) + { + XMLElement* child2 = child->NextSiblingElement( "Child" ); + if ( child2 ) + { + // Finally do something useful. + @endverbatim + + And that doesn't even cover "else" cases. XMLHandle addresses the verbosity + of such code. A XMLHandle checks for null pointers so it is perfectly safe + and correct to use: + + @verbatim + XMLHandle docHandle( &document ); + XMLElement* child2 = docHandle.FirstChildElement( "Document" ).FirstChildElement( "Element" ).FirstChildElement().NextSiblingElement(); + if ( child2 ) + { + // do something useful + @endverbatim + + Which is MUCH more concise and useful. + + It is also safe to copy handles - internally they are nothing more than node pointers. + @verbatim + XMLHandle handleCopy = handle; + @endverbatim + + See also XMLConstHandle, which is the same as XMLHandle, but operates on const objects. +*/ +class TINYXML2_LIB XMLHandle +{ +public: + /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. + explicit XMLHandle( XMLNode* node ) : _node( node ) { + } + /// Create a handle from a node. + explicit XMLHandle( XMLNode& node ) : _node( &node ) { + } + /// Copy constructor + XMLHandle( const XMLHandle& ref ) : _node( ref._node ) { + } + /// Assignment + XMLHandle& operator=( const XMLHandle& ref ) { + _node = ref._node; + return *this; + } + + /// Get the first child of this handle. + XMLHandle FirstChild() { + return XMLHandle( _node ? _node->FirstChild() : 0 ); + } + /// Get the first child element of this handle. + XMLHandle FirstChildElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->FirstChildElement( name ) : 0 ); + } + /// Get the last child of this handle. + XMLHandle LastChild() { + return XMLHandle( _node ? _node->LastChild() : 0 ); + } + /// Get the last child element of this handle. + XMLHandle LastChildElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->LastChildElement( name ) : 0 ); + } + /// Get the previous sibling of this handle. + XMLHandle PreviousSibling() { + return XMLHandle( _node ? _node->PreviousSibling() : 0 ); + } + /// Get the previous sibling element of this handle. + XMLHandle PreviousSiblingElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->PreviousSiblingElement( name ) : 0 ); + } + /// Get the next sibling of this handle. + XMLHandle NextSibling() { + return XMLHandle( _node ? _node->NextSibling() : 0 ); + } + /// Get the next sibling element of this handle. + XMLHandle NextSiblingElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->NextSiblingElement( name ) : 0 ); + } + + /// Safe cast to XMLNode. This can return null. + XMLNode* ToNode() { + return _node; + } + /// Safe cast to XMLElement. This can return null. + XMLElement* ToElement() { + return ( _node ? _node->ToElement() : 0 ); + } + /// Safe cast to XMLText. This can return null. + XMLText* ToText() { + return ( _node ? _node->ToText() : 0 ); + } + /// Safe cast to XMLUnknown. This can return null. + XMLUnknown* ToUnknown() { + return ( _node ? _node->ToUnknown() : 0 ); + } + /// Safe cast to XMLDeclaration. This can return null. + XMLDeclaration* ToDeclaration() { + return ( _node ? _node->ToDeclaration() : 0 ); + } + +private: + XMLNode* _node; +}; + + +/** + A variant of the XMLHandle class for working with const XMLNodes and Documents. It is the + same in all regards, except for the 'const' qualifiers. See XMLHandle for API. +*/ +class TINYXML2_LIB XMLConstHandle +{ +public: + explicit XMLConstHandle( const XMLNode* node ) : _node( node ) { + } + explicit XMLConstHandle( const XMLNode& node ) : _node( &node ) { + } + XMLConstHandle( const XMLConstHandle& ref ) : _node( ref._node ) { + } + + XMLConstHandle& operator=( const XMLConstHandle& ref ) { + _node = ref._node; + return *this; + } + + const XMLConstHandle FirstChild() const { + return XMLConstHandle( _node ? _node->FirstChild() : 0 ); + } + const XMLConstHandle FirstChildElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->FirstChildElement( name ) : 0 ); + } + const XMLConstHandle LastChild() const { + return XMLConstHandle( _node ? _node->LastChild() : 0 ); + } + const XMLConstHandle LastChildElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->LastChildElement( name ) : 0 ); + } + const XMLConstHandle PreviousSibling() const { + return XMLConstHandle( _node ? _node->PreviousSibling() : 0 ); + } + const XMLConstHandle PreviousSiblingElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->PreviousSiblingElement( name ) : 0 ); + } + const XMLConstHandle NextSibling() const { + return XMLConstHandle( _node ? _node->NextSibling() : 0 ); + } + const XMLConstHandle NextSiblingElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->NextSiblingElement( name ) : 0 ); + } + + + const XMLNode* ToNode() const { + return _node; + } + const XMLElement* ToElement() const { + return ( _node ? _node->ToElement() : 0 ); + } + const XMLText* ToText() const { + return ( _node ? _node->ToText() : 0 ); + } + const XMLUnknown* ToUnknown() const { + return ( _node ? _node->ToUnknown() : 0 ); + } + const XMLDeclaration* ToDeclaration() const { + return ( _node ? _node->ToDeclaration() : 0 ); + } + +private: + const XMLNode* _node; +}; + + +/** + Printing functionality. The XMLPrinter gives you more + options than the XMLDocument::Print() method. + + It can: + -# Print to memory. + -# Print to a file you provide. + -# Print XML without a XMLDocument. + + Print to Memory + + @verbatim + XMLPrinter printer; + doc.Print( &printer ); + SomeFunction( printer.CStr() ); + @endverbatim + + Print to a File + + You provide the file pointer. + @verbatim + XMLPrinter printer( fp ); + doc.Print( &printer ); + @endverbatim + + Print without a XMLDocument + + When loading, an XML parser is very useful. However, sometimes + when saving, it just gets in the way. The code is often set up + for streaming, and constructing the DOM is just overhead. + + The Printer supports the streaming case. The following code + prints out a trivially simple XML file without ever creating + an XML document. + + @verbatim + XMLPrinter printer( fp ); + printer.OpenElement( "foo" ); + printer.PushAttribute( "foo", "bar" ); + printer.CloseElement(); + @endverbatim +*/ +class TINYXML2_LIB XMLPrinter : public XMLVisitor +{ +public: + /** Construct the printer. If the FILE* is specified, + this will print to the FILE. Else it will print + to memory, and the result is available in CStr(). + If 'compact' is set to true, then output is created + with only required whitespace and newlines. + */ + XMLPrinter( FILE* file=0, bool compact = false, int depth = 0 ); + virtual ~XMLPrinter() {} + + /** If streaming, write the BOM and declaration. */ + void PushHeader( bool writeBOM, bool writeDeclaration ); + /** If streaming, start writing an element. + The element must be closed with CloseElement() + */ + void OpenElement( const char* name, bool compactMode=false ); + /// If streaming, add an attribute to an open element. + void PushAttribute( const char* name, const char* value ); + void PushAttribute( const char* name, int value ); + void PushAttribute( const char* name, unsigned value ); + void PushAttribute( const char* name, int64_t value ); + void PushAttribute( const char* name, uint64_t value ); + void PushAttribute( const char* name, bool value ); + void PushAttribute( const char* name, double value ); + /// If streaming, close the Element. + virtual void CloseElement( bool compactMode=false ); + + /// Add a text node. + void PushText( const char* text, bool cdata=false ); + /// Add a text node from an integer. + void PushText( int value ); + /// Add a text node from an unsigned. + void PushText( unsigned value ); + /// Add a text node from a signed 64bit integer. + void PushText( int64_t value ); + /// Add a text node from an unsigned 64bit integer. + void PushText( uint64_t value ); + /// Add a text node from a bool. + void PushText( bool value ); + /// Add a text node from a float. + void PushText( float value ); + /// Add a text node from a double. + void PushText( double value ); + + /// Add a comment + void PushComment( const char* comment ); + + void PushDeclaration( const char* value ); + void PushUnknown( const char* value ); + + virtual bool VisitEnter( const XMLDocument& /*doc*/ ); + virtual bool VisitExit( const XMLDocument& /*doc*/ ) { + return true; + } + + virtual bool VisitEnter( const XMLElement& element, const XMLAttribute* attribute ); + virtual bool VisitExit( const XMLElement& element ); + + virtual bool Visit( const XMLText& text ); + virtual bool Visit( const XMLComment& comment ); + virtual bool Visit( const XMLDeclaration& declaration ); + virtual bool Visit( const XMLUnknown& unknown ); + + /** + If in print to memory mode, return a pointer to + the XML file in memory. + */ + const char* CStr() const { + return _buffer.Mem(); + } + /** + If in print to memory mode, return the size + of the XML file in memory. (Note the size returned + includes the terminating null.) + */ + int CStrSize() const { + return _buffer.Size(); + } + /** + If in print to memory mode, reset the buffer to the + beginning. + */ + void ClearBuffer( bool resetToFirstElement = true ) { + _buffer.Clear(); + _buffer.Push(0); + _firstElement = resetToFirstElement; + } + +protected: + virtual bool CompactMode( const XMLElement& ) { return _compactMode; } + + /** Prints out the space before an element. You may override to change + the space and tabs used. A PrintSpace() override should call Print(). + */ + virtual void PrintSpace( int depth ); + virtual void Print( const char* format, ... ); + virtual void Write( const char* data, size_t size ); + virtual void Putc( char ch ); + + inline void Write(const char* data) { Write(data, strlen(data)); } + + void SealElementIfJustOpened(); + bool _elementJustOpened; + DynArray< const char*, 10 > _stack; + +private: + /** + Prepares to write a new node. This includes sealing an element that was + just opened, and writing any whitespace necessary if not in compact mode. + */ + void PrepareForNewNode( bool compactMode ); + void PrintString( const char*, bool restrictedEntitySet ); // prints out, after detecting entities. + + bool _firstElement; + FILE* _fp; + int _depth; + int _textDepth; + bool _processEntities; + bool _compactMode; + + enum { + ENTITY_RANGE = 64, + BUF_SIZE = 200 + }; + bool _entityFlag[ENTITY_RANGE]; + bool _restrictedEntityFlag[ENTITY_RANGE]; + + DynArray< char, 20 > _buffer; + + // Prohibit cloning, intentionally not implemented + XMLPrinter( const XMLPrinter& ); + XMLPrinter& operator=( const XMLPrinter& ); +}; + + +} // tinyxml2 + +#if defined(_MSC_VER) +# pragma warning(pop) +#endif + +#endif // TINYXML2_INCLUDED diff --git a/libs/IO/json.hpp b/libs/IO/json.hpp new file mode 100644 index 0000000..b6d30f8 --- /dev/null +++ b/libs/IO/json.hpp @@ -0,0 +1,20406 @@ +/* + __ _____ _____ _____ + __| | __| | | | JSON for Modern C++ +| | |__ | | | | | | version 3.5.0 +|_____|_____|_____|_|___| https://github.com/nlohmann/json + +Licensed under the MIT License . +SPDX-License-Identifier: MIT +Copyright (c) 2013-2018 Niels Lohmann . + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#ifndef NLOHMANN_JSON_HPP +#define NLOHMANN_JSON_HPP + +#define NLOHMANN_JSON_VERSION_MAJOR 3 +#define NLOHMANN_JSON_VERSION_MINOR 5 +#define NLOHMANN_JSON_VERSION_PATCH 0 + +#include // all_of, find, for_each +#include // assert +#include // and, not, or +#include // nullptr_t, ptrdiff_t, size_t +#include // hash, less +#include // initializer_list +#include // istream, ostream +#include // random_access_iterator_tag +#include // accumulate +#include // string, stoi, to_string +#include // declval, forward, move, pair, swap + +// #include +#ifndef NLOHMANN_JSON_FWD_HPP +#define NLOHMANN_JSON_FWD_HPP + +#include // int64_t, uint64_t +#include // map +#include // allocator +#include // string +#include // vector + +/*! +@brief namespace for Niels Lohmann +@see https://github.com/nlohmann +@since version 1.0.0 +*/ +namespace nlohmann +{ +/*! +@brief default JSONSerializer template argument + +This serializer ignores the template arguments and uses ADL +([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl)) +for serialization. +*/ +template +struct adl_serializer; + +template class ObjectType = + std::map, + template class ArrayType = std::vector, + class StringType = std::string, class BooleanType = bool, + class NumberIntegerType = std::int64_t, + class NumberUnsignedType = std::uint64_t, + class NumberFloatType = double, + template class AllocatorType = std::allocator, + template class JSONSerializer = + adl_serializer> +class basic_json; + +/*! +@brief JSON Pointer + +A JSON pointer defines a string syntax for identifying a specific value +within a JSON document. It can be used with functions `at` and +`operator[]`. Furthermore, JSON pointers are the base for JSON patches. + +@sa [RFC 6901](https://tools.ietf.org/html/rfc6901) + +@since version 2.0.0 +*/ +template +class json_pointer; + +/*! +@brief default JSON class + +This type is the default specialization of the @ref basic_json class which +uses the standard template types. + +@since version 1.0.0 +*/ +using json = basic_json<>; +} // namespace nlohmann + +#endif + +// #include + + +// This file contains all internal macro definitions +// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them + +// exclude unsupported compilers +#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK) + #if defined(__clang__) + #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400 + #error "unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER)) + #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800 + #error "unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #endif +#endif + +// disable float-equal warnings on GCC/clang +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + +// disable documentation warnings on clang +#if defined(__clang__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdocumentation" +#endif + +// allow for portable deprecation warnings +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #define JSON_DEPRECATED __attribute__((deprecated)) +#elif defined(_MSC_VER) + #define JSON_DEPRECATED __declspec(deprecated) +#else + #define JSON_DEPRECATED +#endif + +// allow to disable exceptions +#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || defined(_CPPUNWIND)) && !defined(JSON_NOEXCEPTION) + #define JSON_THROW(exception) throw exception + #define JSON_TRY try + #define JSON_CATCH(exception) catch(exception) + #define JSON_INTERNAL_CATCH(exception) catch(exception) +#else + #define JSON_THROW(exception) std::abort() + #define JSON_TRY if(true) + #define JSON_CATCH(exception) if(false) + #define JSON_INTERNAL_CATCH(exception) if(false) +#endif + +// override exception macros +#if defined(JSON_THROW_USER) + #undef JSON_THROW + #define JSON_THROW JSON_THROW_USER +#endif +#if defined(JSON_TRY_USER) + #undef JSON_TRY + #define JSON_TRY JSON_TRY_USER +#endif +#if defined(JSON_CATCH_USER) + #undef JSON_CATCH + #define JSON_CATCH JSON_CATCH_USER + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_CATCH_USER +#endif +#if defined(JSON_INTERNAL_CATCH_USER) + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_INTERNAL_CATCH_USER +#endif + +// manual branch prediction +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #define JSON_LIKELY(x) __builtin_expect(!!(x), 1) + #define JSON_UNLIKELY(x) __builtin_expect(!!(x), 0) +#else + #define JSON_LIKELY(x) x + #define JSON_UNLIKELY(x) x +#endif + +// C++ language standard detection +#if (defined(__cplusplus) && __cplusplus >= 201703L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 +#elif (defined(__cplusplus) && __cplusplus >= 201402L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1) + #define JSON_HAS_CPP_14 +#endif + +/*! +@brief macro to briefly define a mapping between an enum and JSON +@def NLOHMANN_JSON_SERIALIZE_ENUM +@since version 3.4.0 +*/ +#define NLOHMANN_JSON_SERIALIZE_ENUM(ENUM_TYPE, ...) \ + template \ + inline void to_json(BasicJsonType& j, const ENUM_TYPE& e) \ + { \ + static_assert(std::is_enum::value, #ENUM_TYPE " must be an enum!"); \ + static const std::pair m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [e](const std::pair& ej_pair) -> bool \ + { \ + return ej_pair.first == e; \ + }); \ + j = ((it != std::end(m)) ? it : std::begin(m))->second; \ + } \ + template \ + inline void from_json(const BasicJsonType& j, ENUM_TYPE& e) \ + { \ + static_assert(std::is_enum::value, #ENUM_TYPE " must be an enum!"); \ + static const std::pair m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [j](const std::pair& ej_pair) -> bool \ + { \ + return ej_pair.second == j; \ + }); \ + e = ((it != std::end(m)) ? it : std::begin(m))->first; \ + } + +// Ugly macros to avoid uglier copy-paste when specializing basic_json. They +// may be removed in the future once the class is split. + +#define NLOHMANN_BASIC_JSON_TPL_DECLARATION \ + template class ObjectType, \ + template class ArrayType, \ + class StringType, class BooleanType, class NumberIntegerType, \ + class NumberUnsignedType, class NumberFloatType, \ + template class AllocatorType, \ + template class JSONSerializer> + +#define NLOHMANN_BASIC_JSON_TPL \ + basic_json + +// #include + + +#include // not +#include // size_t +#include // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type + +namespace nlohmann +{ +namespace detail +{ +// alias templates to reduce boilerplate +template +using enable_if_t = typename std::enable_if::type; + +template +using uncvref_t = typename std::remove_cv::type>::type; + +// implementation of C++14 index_sequence and affiliates +// source: https://stackoverflow.com/a/32223343 +template +struct index_sequence +{ + using type = index_sequence; + using value_type = std::size_t; + static constexpr std::size_t size() noexcept + { + return sizeof...(Ints); + } +}; + +template +struct merge_and_renumber; + +template +struct merge_and_renumber, index_sequence> + : index_sequence < I1..., (sizeof...(I1) + I2)... > {}; + +template +struct make_index_sequence + : merge_and_renumber < typename make_index_sequence < N / 2 >::type, + typename make_index_sequence < N - N / 2 >::type > {}; + +template<> struct make_index_sequence<0> : index_sequence<> {}; +template<> struct make_index_sequence<1> : index_sequence<0> {}; + +template +using index_sequence_for = make_index_sequence; + +// dispatch utility (taken from ranges-v3) +template struct priority_tag : priority_tag < N - 1 > {}; +template<> struct priority_tag<0> {}; + +// taken from ranges-v3 +template +struct static_const +{ + static constexpr T value{}; +}; + +template +constexpr T static_const::value; +} // namespace detail +} // namespace nlohmann + +// #include + + +#include // not +#include // numeric_limits +#include // false_type, is_constructible, is_integral, is_same, true_type +#include // declval + +// #include + +// #include + + +#include // random_access_iterator_tag + +// #include + + +namespace nlohmann +{ +namespace detail +{ +template struct make_void +{ + using type = void; +}; +template using void_t = typename make_void::type; +} // namespace detail +} // namespace nlohmann + +// #include + + +namespace nlohmann +{ +namespace detail +{ +template +struct iterator_types {}; + +template +struct iterator_types < + It, + void_t> +{ + using difference_type = typename It::difference_type; + using value_type = typename It::value_type; + using pointer = typename It::pointer; + using reference = typename It::reference; + using iterator_category = typename It::iterator_category; +}; + +// This is required as some compilers implement std::iterator_traits in a way that +// doesn't work with SFINAE. See https://github.com/nlohmann/json/issues/1341. +template +struct iterator_traits +{ +}; + +template +struct iterator_traits < T, enable_if_t < !std::is_pointer::value >> + : iterator_types +{ +}; + +template +struct iterator_traits::value>> +{ + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = ptrdiff_t; + using pointer = T*; + using reference = T&; +}; +} +} + +// #include + +// #include + + +#include + +// #include + + +// http://en.cppreference.com/w/cpp/experimental/is_detected +namespace nlohmann +{ +namespace detail +{ +struct nonesuch +{ + nonesuch() = delete; + ~nonesuch() = delete; + nonesuch(nonesuch const&) = delete; + void operator=(nonesuch const&) = delete; +}; + +template class Op, + class... Args> +struct detector +{ + using value_t = std::false_type; + using type = Default; +}; + +template class Op, class... Args> +struct detector>, Op, Args...> +{ + using value_t = std::true_type; + using type = Op; +}; + +template