diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 6b053a2e686f36693295c847420318ebaf241299..1a2be0d55785081587eeda5b4b183d4d392e4f1a 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -5,7 +5,7 @@ windows job:
- 'call "C:/Program Files (x86)/Microsoft Visual Studio/2017/Community/VC/Auxiliary/Build/vcvars64.bat"'
- mkdir build
- cd build
- - 'cmake -DWITH_PCL=FALSE -DCMAKE_GENERATOR_PLATFORM=x64 -DOpenCV_DIR="D:/opencv-4.0.1/build/install" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1" ..'
+ - 'cmake -DWITH_PCL=FALSE -DCMAKE_GENERATOR_PLATFORM=x64 -DEigen3_DIR="C:/Program Files (x86)/Eigen3/share/eigen3/cmake" -DOpenCV_DIR="D:/opencv-4.0.1/build/install" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1" ..'
- devenv ftl.utu.fi.sln /build Release
linux job:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6eaf7b6d5304fca33e7009e98e14653de458d535..7984483ed69f705ae3b46be9b72b85420356409f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,9 +26,13 @@ find_package( OpenCV REQUIRED )
find_package( Threads REQUIRED )
find_package( URIParser REQUIRED )
find_package( MsgPack REQUIRED )
+find_package( Eigen3 REQUIRED )
if (WITH_FIXSTARS)
find_package( LibSGM )
+ if (LibSGM_FOUND)
+ set(HAVE_LIBSGM true)
+ endif()
endif()
if(${CMAKE_VERSION} VERSION_GREATER "3.12.0")
@@ -137,31 +141,33 @@ else()
endif()
SET(CMAKE_USE_RELATIVE_PATHS ON)
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
-add_subdirectory(common/cpp)
-add_subdirectory(net)
+add_subdirectory(components/common/cpp)
+add_subdirectory(components/net)
+add_subdirectory(components/rgbd-sources)
if (BUILD_RENDERER)
- add_subdirectory(renderer)
+ add_subdirectory(components/renderers)
set(HAVE_RENDER)
endif()
if (BUILD_VISION)
- add_subdirectory(vision)
+ add_subdirectory(applications/vision)
endif()
-if (BUILD_RECONSTRUCT)
- add_subdirectory(reconstruct)
+if (BUILD_RECONSTRUCT AND HAVE_PCL)
+ add_subdirectory(applications/reconstruct)
endif()
### Generate Build Configuration Files =========================================
-configure_file(${CMAKE_SOURCE_DIR}/common/cpp/include/ftl/config.h.in
- ${CMAKE_SOURCE_DIR}/common/cpp/include/ftl/config.h
+configure_file(${CMAKE_SOURCE_DIR}/components/common/cpp/include/ftl/config.h.in
+ ${CMAKE_SOURCE_DIR}/components/common/cpp/include/ftl/config.h
)
-configure_file(${CMAKE_SOURCE_DIR}/common/cpp/src/config.cpp.in
- ${CMAKE_SOURCE_DIR}/common/cpp/src/config.cpp
+configure_file(${CMAKE_SOURCE_DIR}/components/common/cpp/src/config.cpp.in
+ ${CMAKE_SOURCE_DIR}/components/common/cpp/src/config.cpp
)
# For issue #17
diff --git a/README.md b/README.md
index b979ca3105e6b184ecd1f3e09b753c5125e24490..caddf61ebab500e0d844f7ad07a40c70a98db65f 100644
--- a/README.md
+++ b/README.md
@@ -2,17 +2,32 @@
This monorepo contains all elements of the FTL software system.
-* net : A p2p messaging library for C++ and JavaScript
-* vision : Stereo vision node in p2p network
-* reconstruct : Performs scene reconstruction from vision nodes
-* renderer : Produce video or live feeds from scene representation
+* Components : Modular components compiled as static libs
+ * net : A p2p messaging library for C++ and JavaScript
+ * rgbd-sources : Abstraction and implementations of different RGB-Depth data sources
+ * renderers : A collection of visualisation tools, including for RGB-D and point clouds
+ * scene-sources : Abstraction and implementations of 3D scene data sources
+ * common : Utility and configuration tools
+* Applications : Executable apps for the different node machines
+ * vision : Stereo vision node in p2p network, generates an RGB-Depth net stream
+ * reconstruct : Performs scene reconstruction from synchronised RGB-Depth sources
* front-end : Client side FTL code, both web and native
* web-service : A web backend service provider acting as a form of proxy
* www : FTL Website
-The architecture is a mix of C++ and NodeJS largely communicating over a p2p
+The architecture is a mix of C++ and NodeJS largely communicating over a p2p RPC
network protocol.
+Overall, initial vision machines capture video sources and generate colour and
+depth images. These images are then streamed from all such sources to a single
+machine to reconstruct the scene in 3D. The reconstructed scene is then further
+compressed and streamed to other machines for additional analysis, processing or
+visualisation.
+
+The whole system is configured using JSON config files, examples of which can be
+found in the `config` folder. The intention is that a web interface will
+eventually enable this configuration to take place online.
+
## Build
Use the following commands in the root repository directory to build:
@@ -29,5 +44,5 @@ to configure the project.
You will need to have OpenCV and glog installed. CUDA and LibSGM are optional
but recommended also. OpenCV should have cuda stereo modules compiled, along
with the viz module if local point cloud display is required. These are contrib
-modules.
+modules. PCL is also required to build the reconstruction components and app.
diff --git a/reconstruct/CMakeLists.txt b/applications/reconstruct/CMakeLists.txt
similarity index 63%
rename from reconstruct/CMakeLists.txt
rename to applications/reconstruct/CMakeLists.txt
index 2bbdfa4976d21e4323847580347d7ba87e8d7318..58d9312bd9ce8d5c1247b81e2a5525f7aa904002 100644
--- a/reconstruct/CMakeLists.txt
+++ b/applications/reconstruct/CMakeLists.txt
@@ -1,5 +1,5 @@
# Need to include staged files and libs
-include_directories(${PROJECT_SOURCE_DIR}/reconstruct/include)
+include_directories(${PROJECT_SOURCE_DIR}/applications/reconstruct/include)
#include_directories(${PROJECT_BINARY_DIR})
set(REPSRC
@@ -12,6 +12,6 @@ add_dependencies(ftl-reconstruct ftlnet)
add_dependencies(ftl-reconstruct ftlcommon)
#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(ftl-reconstruct ftlcommon Threads::Threads ${OpenCV_LIBS} glog::glog ftlnet ftlrender)
+target_link_libraries(ftl-reconstruct ftlcommon ftlrgbd Threads::Threads ${OpenCV_LIBS} glog::glog ftlnet ftlrender)
diff --git a/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
similarity index 81%
rename from reconstruct/include/ftl/registration.hpp
rename to applications/reconstruct/include/ftl/registration.hpp
index 58893623e483152618af8eab88fc7d5bd0619769..32bdc86ea69a4e45824ba9fa26923534d48de3eb 100644
--- a/reconstruct/include/ftl/registration.hpp
+++ b/applications/reconstruct/include/ftl/registration.hpp
@@ -2,6 +2,7 @@
#define _FTL_RECONSTRUCT_REGISTRATION_HPP_
#include <ftl/config.h>
+#include <ftl/rgbd.hpp>
#include <opencv2/opencv.hpp>
#ifdef HAVE_PCL
@@ -21,11 +22,11 @@ Eigen::Matrix4f findTransformation( std::vector<pcl::PointCloud<pcl::PointXYZ>::
/* Convert chessboard corners found with OpenCV's findChessboardCorners to PCL point cloud.
*/
-pcl::PointCloud<pcl::PointXYZ>::Ptr cornersToPointCloud(const std::vector<cv::Point2f> &corners, const cv::Mat &disp, const cv::Mat &Q);
+pcl::PointCloud<pcl::PointXYZ>::Ptr cornersToPointCloud(const std::vector<cv::Point2f> &corners, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p);
/* Find chessboard corners from image.
*/
-bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &disp, const cv::Mat &Q, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out);
+bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out);
};
};
diff --git a/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
similarity index 64%
rename from reconstruct/src/main.cpp
rename to applications/reconstruct/src/main.cpp
index 9c9400e3a2ea05f3d88b1142de444ee3d4a071d4..af23dcdf2f6c1ae18b2497803687b971d7dc10b3 100644
--- a/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -7,6 +7,7 @@
#include <glog/logging.h>
#include <ftl/config.h>
#include <ftl/configuration.hpp>
+#include <ftl/rgbd.hpp>
// #include <zlib.h>
// #include <lz4.h>
@@ -45,6 +46,7 @@ using ftl::Display;
using ftl::config;
using std::string;
using std::vector;
+using ftl::rgbd::RGBDSource;
using json = nlohmann::json;
using std::this_thread::sleep_for;
@@ -62,44 +64,50 @@ using pcl::PointXYZRGB;
using cv::Mat;
-class InputStereo {
-private:
- string uri_;
- Mat rgb_, disp_;
- Mat q_;
- std::mutex m_;
+namespace ftl {
+namespace rgbd {
+PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src);
+}
+}
-#ifdef HAVE_PCL
- static PointCloud<PointXYZRGB>::Ptr _getPointCloud(Mat rgb, Mat disp, Mat q) {
- cv::Mat_<cv::Vec3f> XYZ(disp.rows, disp.cols);
- reprojectImageTo3D(disp, XYZ, q, false);
- return ftl::utility::matToPointXYZ(XYZ, rgb);
- }
-#endif
+PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src) {
+ const double CX = src->getParameters().cx;
+ const double CY = src->getParameters().cy;
+ const double F = src->getParameters().f;
-public:
- InputStereo(string uri, const Mat Q): uri_(uri), q_(Q) {};
-
- void set(Mat &rgb, Mat &disp) {
- unique_lock<mutex> lk(m_);
- rgb_ = rgb;
- disp_ = disp;
+ cv::Mat rgb;
+ cv::Mat depth;
+ src->getRGBD(rgb, depth);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
+ point_cloud_ptr->width = rgb.cols * rgb.rows;
+ point_cloud_ptr->height = 1;
+
+ for(int i=0;i<rgb.rows;i++) {
+ const float *sptr = depth.ptr<float>(i);
+ for(int j=0;j<rgb.cols;j++) {
+ float d = sptr[j] * 1000.0f;
+
+ pcl::PointXYZRGB point;
+ point.x = (((double)j + CX) / F) * d;
+ point.y = (((double)i + CY) / F) * d;
+ point.z = d;
+
+ if (point.x == INFINITY || point.y == INFINITY || point.z == INFINITY || point.z < 6.0f) {
+ point.x = 0; point.y = 0; point.z = 0;
+ }
+
+ cv::Point3_<uchar> prgb = rgb.at<cv::Point3_<uchar>>(i, j);
+ uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
+ point.rgb = *reinterpret_cast<float*>(&rgb);
+
+ point_cloud_ptr -> points.push_back(point);
+ }
}
-
- string getURI() { return uri_; }
- Mat getQ() { return q_; }
- /* thread unsafe, use lock() */
-#ifdef HAVE_PCL
- PointCloud<PointXYZRGB>::Ptr getPointCloud() { return _getPointCloud(rgb_, disp_, q_); }
-#endif
- Mat getRGB() { return rgb_; }
- Mat getDisparity() { return disp_; }
- /* Mat getDepth() {} */
- /* Mat getLeftRGB() {} */
- /* Mat getRightRGB() {} */
- unique_lock<mutex> lock() { return unique_lock<mutex>(m_); } // use recursive mutex instead (and move locking to methods)?
-};
+ return point_cloud_ptr;
+}
+
#ifdef HAVE_PCL
#include <pcl/filters/uniform_sampling.h>
@@ -173,7 +181,7 @@ void saveRegistration(std::map<string, Eigen::Matrix4f> registration) {
}
template <template<class> class Container>
-std::map<string, Eigen::Matrix4f> runRegistration(Container<InputStereo> &inputs) {
+std::map<string, Eigen::Matrix4f> runRegistration(ftl::net::Universe &net, Container<RGBDSource*> &inputs) {
std::map<string, Eigen::Matrix4f> registration;
@@ -195,7 +203,7 @@ std::map<string, Eigen::Matrix4f> runRegistration(Container<InputStereo> &inputs
size_t ref_i;
bool found = false;
for (size_t i = 0; i < inputs.size(); i++) {
- if (inputs[i].getURI() == ref_uri) {
+ if (inputs[i]->getConfig()["uri"] == ref_uri) {
ref_i = i;
found = true;
break;
@@ -205,35 +213,28 @@ std::map<string, Eigen::Matrix4f> runRegistration(Container<InputStereo> &inputs
if (!found) { LOG(ERROR) << "Reference input not found!"; return registration; }
for (auto &input : inputs) {
- cv::namedWindow("Registration: " + input.getURI(), cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
+ cv::namedWindow("Registration: " + (string)input->getConfig()["uri"], cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
}
// vector for every input: vector of point clouds of patterns
vector<vector<PointCloud<PointXYZ>::Ptr>> patterns(inputs.size());
while (iter > 0) {
+ net.broadcast("grab");
bool retval = true; // set to false if pattern not found in one of the sources
vector<PointCloud<PointXYZ>::Ptr> patterns_iter(inputs.size());
for (size_t i = 0; i < inputs.size(); i++) {
- InputStereo &input = inputs[i];
- Mat rgb, disp, Q;
+ RGBDSource *input = inputs[i];
+ Mat rgb, depth;
- while(true) {
- auto lk = input.lock();
- rgb = input.getRGB();
- disp = input.getDisparity();
- Q = input.getQ();
- lk.unlock();
-
- // todo solve this somewhere else
- if ((rgb.cols > 0) && (disp.cols > 0)) { break; }
- else { sleep_for(milliseconds(500)); }
- }
+ input->getRGBD(rgb, depth);
+
+ if ((rgb.cols == 0) || (depth.cols == 0)) { retval = false; break; }
- retval &= ftl::registration::findChessboardCorners(rgb, disp, Q, pattern_size, patterns_iter[i]);
+ retval &= ftl::registration::findChessboardCorners(rgb, depth, input->getParameters(), pattern_size, patterns_iter[i]);
- cv::imshow("Registration: " + input.getURI(), rgb);
+ cv::imshow("Registration: " + (string)input->getConfig()["uri"], rgb);
}
cv::waitKey(delay);
@@ -247,7 +248,7 @@ std::map<string, Eigen::Matrix4f> runRegistration(Container<InputStereo> &inputs
else { LOG(WARNING) << "Pattern not detected on all inputs";}
}
- for (auto &input : inputs) { cv::destroyWindow("Registration: " + input.getURI()); }
+ for (auto &input : inputs) { cv::destroyWindow("Registration: " + (string)input->getConfig()["uri"]); }
for (size_t i = 0; i < inputs.size(); i++) {
Eigen::Matrix4f T;
@@ -257,34 +258,13 @@ std::map<string, Eigen::Matrix4f> runRegistration(Container<InputStereo> &inputs
else {
T = ftl::registration::findTransformation(patterns[i], patterns[ref_i]);
}
- registration[inputs[i].getURI()] = T;
+ registration[(string)inputs[i]->getConfig()["uri"]] = T;
}
saveRegistration(registration);
return registration;
}
#endif
-bool getCalibration(Universe &net, string src, Mat &Q) {
- Q = Mat(cv::Size(4,4), CV_32F);
- while(true) {
- auto buf = net.findOne<vector<unsigned char>>((string) src +"/calibration");
- if (buf) {
- memcpy(Q.data, (*buf).data(), (*buf).size());
-
- if (Q.step*Q.rows != (*buf).size()) {
- LOG(ERROR) << "Corrupted calibration";
- return false;
- }
-
- return true;
- }
- else {
- LOG(INFO) << "Could not get calibration, retrying";
- sleep_for(milliseconds(500));
- }
- }
-}
-
static void run() {
Universe net(config["net"]);
@@ -296,30 +276,19 @@ static void run() {
return;
}
- std::deque<InputStereo> inputs;
+ std::deque<RGBDSource*> inputs;
std::vector<Display> displays;
- // todo networking and initilization for input should possibly be implemented in their own class
- // with error handling etc.
- //
-
- for (auto &src : config["sources"]) {
- Mat Q;
- if (!getCalibration(net, src, Q)) { continue; } // skips input if calibration bad
-
- InputStereo &in = inputs.emplace_back(src, Q);
- displays.emplace_back(config["display"], src);
-
- LOG(INFO) << src << " loaded";
-
- net.subscribe(src, [&in](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
- Mat rgb, disp;
- cv::imdecode(jpg, cv::IMREAD_COLOR, &rgb);
- Mat(rgb.size(), CV_16UC1);
- cv::imdecode(d, cv::IMREAD_UNCHANGED, &disp);
- disp.convertTo(disp, CV_32FC1, 1.0f/16.0f);
- in.set(rgb, disp);
- });
+ // TODO Allow for non-net source types
+ for (auto &src : config["sources"]) {
+ RGBDSource *in = ftl::rgbd::RGBDSource::create(src, &net); //new ftl::rgbd::NetSource(src, &net);
+ if (!in) {
+ LOG(ERROR) << "Unrecognised source: " << src;
+ } else {
+ inputs.push_back(in);
+ displays.emplace_back(config["display"], src["uri"]);
+ LOG(INFO) << (string)src["uri"] << " loaded";
+ }
}
// Displays and Inputs configured
@@ -329,14 +298,14 @@ static void run() {
#ifdef HAVE_PCL
std::map<string, Eigen::Matrix4f> registration;
if (config["registration"]["calibration"]["run"]) {
- registration = runRegistration(inputs);
+ registration = runRegistration(net, inputs);
}
else {
registration = loadRegistration();
}
vector<Eigen::Matrix4f> T;
for (auto &input : inputs) {
- Eigen::Matrix4f RT = (registration.count(input.getURI()) > 0) ? registration[input.getURI()] : registration["default"];
+ Eigen::Matrix4f RT = (registration.count((string)input->getConfig()["uri"]) > 0) ? registration[(string)input->getConfig()["uri"]] : registration["default"];
T.push_back(RT);
}
@@ -345,7 +314,7 @@ static void run() {
Display display_merged(config["display"], "Merged"); // todo
int active = displays.size();
- while (active > 0) {
+ while (active > 0 && display_merged.active()) {
active = 0;
net.broadcast("grab"); // To sync cameras
@@ -353,21 +322,15 @@ static void run() {
PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
for (size_t i = 0; i < inputs.size(); i++) {
- Display &display = displays[i];
- InputStereo &input = inputs[i];
+ //Display &display = displays[i];
+ RGBDSource *input = inputs[i];
- if (!display.active()) continue;
+ //if (!display.active()) continue;
active += 1;
+
+ clouds[i] = ftl::rgbd::createPointCloud(input);
- auto lk = input.lock();
- //Mat rgb = input.getRGB();
- //Mat disparity = input.getDisparity();
- clouds[i] = input.getPointCloud();
- lk.unlock();
-
- display.render(clouds[i]);
- //display.render(rgb, disparity);
- display.wait(50);
+ //display.render(clouds[i]);
}
for (size_t i = 0; i < clouds.size(); i++) {
@@ -388,6 +351,6 @@ static void run() {
}
int main(int argc, char **argv) {
- ftl::configure(argc, argv, "representation"); // TODO(nick) change to "reconstruction"
+ ftl::configure(argc, argv, "reconstruction");
run();
}
diff --git a/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp
similarity index 87%
rename from reconstruct/src/registration.cpp
rename to applications/reconstruct/src/registration.cpp
index 7020db090142332364769ea3c1d899f95c220d82..900a5a4aa44d34f6dc0359277cdb9ebefb62a367 100644
--- a/reconstruct/src/registration.cpp
+++ b/applications/reconstruct/src/registration.cpp
@@ -24,6 +24,7 @@
namespace ftl {
namespace registration {
+using ftl::rgbd::CameraParameters;
using std::vector;
using pcl::PointCloud;
using pcl::PointXYZ;
@@ -60,13 +61,14 @@ void fitPlane(PointCloud<PointXYZ>::Ptr cloud_in, PointCloud<PointXYZ>::Ptr clou
}
//template<typename T = PointXYZ> typename
-PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners, const Mat &disp, const Mat &Q) {
-
- cv::Matx44d Q_;
- Q.convertTo(Q_, CV_64F);
+PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners, const Mat &disp, const CameraParameters &p) {
int corners_len = corners.size();
vector<cv::Vec3f> points(corners_len);
+
+ const double CX = p.cx;
+ const double CY = p.cy;
+ const double F = p.f;
// Output point cloud
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
@@ -80,31 +82,32 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
for (int i = 0; i < corners_len; i++) {
double x = corners[i].x;
double y = corners[i].y;
- double d = disp.at<float>((int) y, (int) x); // todo: better estimation
+ double d = disp.at<float>((int) y, (int) x) * 1000.0f; // todo: better estimation
- cv::Vec4d homg_pt = Q_ * cv::Vec4d(x, y, d, 1.0);
- cv::Vec3d p = cv::Vec3d(homg_pt.val) / homg_pt[3];
+ //cv::Vec4d homg_pt = Q_ * cv::Vec4d(x, y, d, 1.0);
+ //cv::Vec3d p = cv::Vec3d(homg_pt.val) / homg_pt[3];
+
+ PointXYZ point;
+ point.x = (((double)x + CX) / F) * d; // / 1000.0f;
+ point.y = (((double)y + CY) / F) * d; // / 1000.0f;
+ point.z = d;
// no check since disparities assumed to be good in the calibration pattern
// if (fabs(d-minDisparity) <= FLT_EPSILON)
- PointXYZ point;
- point.x = p[0];
- point.y = p[1];
- point.z = p[2];
cloud->push_back(point);
}
return cloud;
}
-bool findChessboardCorners(Mat &rgb, const Mat &disp, const Mat &Q, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out) {
+bool findChessboardCorners(Mat &rgb, const Mat &disp, const CameraParameters &p, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out) {
vector<cv::Point2f> corners(pattern_size.width * pattern_size.height);
bool retval = cv::findChessboardCorners(rgb, pattern_size, corners); // (todo) change to findChessboardCornersSB (OpenCV > 4)
// todo: verify that disparities are good
cv::drawChessboardCorners(rgb, pattern_size, Mat(corners), retval);
if (!retval) { return false; }
- auto corners_cloud = cornersToPointCloud(corners, disp, Q);
+ auto corners_cloud = cornersToPointCloud(corners, disp, p);
if (out) { *out += *corners_cloud; } // if cloud is valid, add the points
else { out = corners_cloud; }
return true;
diff --git a/applications/vision/CMakeLists.txt b/applications/vision/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..00cf2abafb0bbe91327823ffb81bf73b39e5d7cc
--- /dev/null
+++ b/applications/vision/CMakeLists.txt
@@ -0,0 +1,27 @@
+# Need to include staged files and libs
+#include_directories(${PROJECT_SOURCE_DIR}/vision/include)
+#include_directories(${PROJECT_BINARY_DIR})
+
+#add_subdirectory(lib)
+
+set(CVNODESRC
+ src/main.cpp
+ src/streamer.cpp
+ src/middlebury.cpp
+)
+
+add_executable(ftl-vision ${CVNODESRC})
+
+target_include_directories(ftl-vision PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>
+ PRIVATE src)
+
+if (CUDA_FOUND)
+set_property(TARGET ftl-vision PROPERTY CUDA_SEPARABLE_COMPILATION OFF)
+endif()
+
+#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
+target_link_libraries(ftl-vision ftlrgbd ftlcommon ftlrender Threads::Threads ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} glog::glog ftlnet)
+
+
diff --git a/vision/README.md b/applications/vision/README.md
similarity index 100%
rename from vision/README.md
rename to applications/vision/README.md
diff --git a/vision/include/elas.h b/applications/vision/include/elas.h
similarity index 100%
rename from vision/include/elas.h
rename to applications/vision/include/elas.h
diff --git a/vision/include/ftl/middlebury.hpp b/applications/vision/include/ftl/middlebury.hpp
similarity index 100%
rename from vision/include/ftl/middlebury.hpp
rename to applications/vision/include/ftl/middlebury.hpp
diff --git a/vision/include/ftl/streamer.hpp b/applications/vision/include/ftl/streamer.hpp
similarity index 100%
rename from vision/include/ftl/streamer.hpp
rename to applications/vision/include/ftl/streamer.hpp
diff --git a/vision/include/ftl/synched.hpp b/applications/vision/include/ftl/synched.hpp
similarity index 100%
rename from vision/include/ftl/synched.hpp
rename to applications/vision/include/ftl/synched.hpp
diff --git a/vision/src/main.cpp b/applications/vision/src/main.cpp
similarity index 64%
rename from vision/src/main.cpp
rename to applications/vision/src/main.cpp
index 49d3f43dbf0fa1bceda2b64b42d1b67d7f88bb83..ed63b5a18d651341f3b36e27d82db430c4ceaf58 100644
--- a/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -18,10 +18,7 @@
#include <condition_variable>
#include <opencv2/opencv.hpp>
-#include <ftl/local.hpp>
-#include <ftl/synched.hpp>
-#include <ftl/calibrate.hpp>
-#include <ftl/disparity.hpp>
+#include <ftl/rgbd.hpp>
#include <ftl/middlebury.hpp>
#include <ftl/display.hpp>
#include <ftl/streamer.hpp>
@@ -37,12 +34,11 @@
#pragma comment(lib, "Rpcrt4.lib")
#endif
-using ftl::Calibrate;
-using ftl::LocalSource;
+using ftl::rgbd::RGBDSource;
+using ftl::rgbd::CameraParameters;
+using ftl::rgbd::StereoVideoSource;
using ftl::Display;
using ftl::Streamer;
-using ftl::Disparity;
-using ftl::SyncSource;
using ftl::net::Universe;
using std::string;
using std::vector;
@@ -60,73 +56,35 @@ using ftl::config;
static void run(const string &file) {
ctpl::thread_pool pool(2);
Universe net(config["net"]);
-
LOG(INFO) << "Net started.";
- LocalSource *lsrc;
- if (ftl::is_video(file)) {
- // Load video file
- LOG(INFO) << "Using video file...";
- lsrc = new LocalSource(file, config["source"]);
- } else if (file != "") {
- auto vid = ftl::locateFile("video.mp4");
- if (!vid) {
- LOG(FATAL) << "No video.mp4 file found in provided paths";
- } else {
- LOG(INFO) << "Using test directory...";
- lsrc = new LocalSource(*vid, config["source"]);
- }
- } else {
- // Use cameras
- LOG(INFO) << "Using cameras...";
- lsrc = new LocalSource(config["source"]);
- }
-
- //auto sync = new SyncSource(); // TODO(nick) Pass protocol object
- // Add any remote channels
- /* for (auto c : OPTION_channels) {
- sync->addChannel(c);
- } */
-
- LOG(INFO) << "Locating calibration...";
-
- // Perform or load calibration intrinsics + extrinsics
- Calibrate calibrate(lsrc, config["calibration"]);
- if (config["calibrate"]) calibrate.recalibrate();
- if (!calibrate.isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
- else LOG(INFO) << "Calibration initiated.";
-
- cv::Mat Q_32F;
- calibrate.getQ().convertTo(Q_32F, CV_32F);
+ RGBDSource *source = nullptr;
+ source = new StereoVideoSource(config, file);
// Allow remote users to access camera calibration matrix
- net.bind(string("ftl://utu.fi/")+(string)config["stream"]["name"]+string("/rgb-d/calibration"), [Q_32F]() -> vector<unsigned char> {
+ net.bind(string("ftl://utu.fi/")+(string)config["stream"]["name"]+string("/rgb-d/calibration"), [source]() -> vector<unsigned char> {
vector<unsigned char> buf;
- buf.resize(Q_32F.step*Q_32F.rows);
+ buf.resize(sizeof(CameraParameters));
LOG(INFO) << "Calib buf size = " << buf.size();
- memcpy(buf.data(), Q_32F.data, buf.size());
+ memcpy(buf.data(), &source->getParameters(), buf.size());
return buf;
});
- Mat l, r, disp;
+ Mat rgb, depth;
bool grabbed = false;
mutex datam;
condition_variable datacv;
// Wait for grab message to sync camera capture
- net.bind("grab", [&calibrate,&l,&r,&datam,&datacv,&grabbed]() -> void {
+ net.bind("grab", [&source,&datam,&datacv,&grabbed]() -> void {
unique_lock<mutex> datalk(datam);
if (grabbed) return;
- calibrate.rectified(l, r);
+ source->grab();
grabbed = true;
datacv.notify_one();
});
- // Choose and configure disparity algorithm
- auto disparity = Disparity::create(config["disparity"]);
- if (!disparity) LOG(FATAL) << "Unknown disparity algorithm : " << config["disparity"];
-
- Mat pl, pdisp;
+ Mat prgb, pdepth;
vector<unsigned char> rgb_buf;
vector<unsigned char> d_buf;
string uri = string("ftl://utu.fi/")+(string)config["stream"]["name"]+string("/rgb-d");
@@ -145,7 +103,7 @@ static void run(const string &file) {
// Fake grab if no peers to allow visualisation locally
if (net.numberOfPeers() == 0) {
grabbed = true;
- calibrate.rectified(l, r);
+ source->grab();
}
// Pipeline for disparity
@@ -156,7 +114,7 @@ static void run(const string &file) {
grabbed = false;
auto start = std::chrono::high_resolution_clock::now();
- disparity->compute(l, r, disp);
+ source->getRGBD(rgb, depth);
datalk.unlock();
unique_lock<mutex> lk(m);
@@ -172,7 +130,7 @@ static void run(const string &file) {
// Pipeline for stream compression
pool.push([&](int id) {
auto start = std::chrono::high_resolution_clock::now();
- if (pl.rows != 0) stream.send(pl, pdisp);
+ if (prgb.rows != 0) stream.send(prgb, pdepth);
unique_lock<mutex> lk(m);
jobs++;
lk.unlock();
@@ -184,7 +142,7 @@ static void run(const string &file) {
});
// Send RGB+Depth images for local rendering
- if (pl.rows > 0) display.render(pl, r, pdisp, Q_32F);
+ if (prgb.rows > 0) display.render(prgb, pdepth, source->getParameters());
display.wait(1);
// Wait for both pipelines to complete
@@ -192,8 +150,8 @@ static void run(const string &file) {
cv.wait(lk, [&jobs]{return jobs == 2;});
// Store previous frame for next stage compression
- l.copyTo(pl);
- disp.copyTo(pdisp);
+ rgb.copyTo(prgb);
+ depth.copyTo(pdepth);
//net.publish(uri, rgb_buf, d_buf);
}
diff --git a/vision/src/middlebury.cpp b/applications/vision/src/middlebury.cpp
similarity index 98%
rename from vision/src/middlebury.cpp
rename to applications/vision/src/middlebury.cpp
index 85ec4a52b53e994b2c3dcbfef849663eabcb60dd..dfe9c8c88919845569c23f0606ee0e275a4ca274 100644
--- a/vision/src/middlebury.cpp
+++ b/applications/vision/src/middlebury.cpp
@@ -1,6 +1,6 @@
#include <ftl/middlebury.hpp>
#include <glog/logging.h>
-#include <ftl/disparity.hpp>
+#include <ftl/rgbd.hpp>
#include <string>
#include <algorithm>
@@ -243,11 +243,12 @@ void ftl::middlebury::test(nlohmann::json &config) {
cv::resize(r, r, cv::Size(r.cols * scale,r.rows * scale), 0, 0, cv::INTER_LINEAR);
}
+ // TODO(Nick) Update to use an RGBD Image source
// Run algorithm
- auto disparity = ftl::Disparity::create(config["disparity"]);
+ //auto disparity = ftl::Disparity::create(config["disparity"]);
Mat disp;
- disparity->compute(l,r,disp);
+ // disparity->compute(l,r,disp);
//disp.convertTo(disp, CV_32F);
// Display results
diff --git a/vision/src/streamer.cpp b/applications/vision/src/streamer.cpp
similarity index 97%
rename from vision/src/streamer.cpp
rename to applications/vision/src/streamer.cpp
index d9bef41d43f1b9c9c273eb03a6c2e987ecb46432..b9a3f78bb905383b94f002ccb431a863e897346d 100644
--- a/vision/src/streamer.cpp
+++ b/applications/vision/src/streamer.cpp
@@ -26,7 +26,7 @@ void Streamer::send(const Mat &rgb, const Mat &depth) {
cv::imencode(".jpg", rgb, rgb_buf);
Mat d2;
- depth.convertTo(d2, CV_16UC1, 16);
+ depth.convertTo(d2, CV_16UC1, 16*100);
vector<unsigned char> d_buf;
/*d_buf.resize(d2.step*d2.rows);
diff --git a/vision/src/sync.cpp b/applications/vision/src/sync.cpp
similarity index 100%
rename from vision/src/sync.cpp
rename to applications/vision/src/sync.cpp
diff --git a/common/config/calibration.xml b/common/config/calibration.xml
deleted file mode 100644
index ebc780ce8d68fe9980adecfe822a6e12e9fc0257..0000000000000000000000000000000000000000
--- a/common/config/calibration.xml
+++ /dev/null
@@ -1,59 +0,0 @@
-<?xml version="1.0"?>
-<opencv_storage>
-<Settings>
- <!-- Number of inner corners per a item row and column. (square, circle) -->
- <BoardSize_Width>9</BoardSize_Width>
- <BoardSize_Height>6</BoardSize_Height>
-
- <!-- The size of a square in some user defined metric system (pixel, millimeter)-->
- <Square_Size>50</Square_Size>
-
- <!-- The type of input used for camera calibration. One of: CHESSBOARD CIRCLES_GRID ASYMMETRIC_CIRCLES_GRID -->
- <Calibrate_Pattern>"CHESSBOARD"</Calibrate_Pattern>
-
- <!-- The input to use for calibration.
- To use an input camera -> give the ID of the camera, like "1"
- To use an input video -> give the path of the input video, like "/tmp/x.avi"
- To use an image list -> give the path to the XML or YAML file containing the list of the images, like "/tmp/circles_list.xml"
- -->
- <!--Input>"images/CameraCalibration/VID5/VID5.xml"</Input-->
- <!-- If true (non-zero) we flip the input images around the horizontal axis.-->
- <Input_FlipAroundHorizontalAxis>0</Input_FlipAroundHorizontalAxis>
-
- <!-- Time delay between frames in case of camera. -->
- <Input_Delay>100</Input_Delay>
-
- <!-- How many frames to use, for calibration. -->
- <Calibrate_NrOfFrameToUse>25</Calibrate_NrOfFrameToUse>
- <!-- Consider only fy as a free parameter, the ratio fx/fy stays the same as in the input cameraMatrix.
- Use or not setting. 0 - False Non-Zero - True-->
- <Calibrate_FixAspectRatio> 1 </Calibrate_FixAspectRatio>
- <!-- If true (non-zero) tangential distortion coefficients are set to zeros and stay zero.-->
- <Calibrate_AssumeZeroTangentialDistortion>1</Calibrate_AssumeZeroTangentialDistortion>
- <!-- If true (non-zero) the principal point is not changed during the global optimization.-->
- <Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>
-
- <!-- The name of the output log file. -->
- <!--Write_outputFileName>"out_camera_data.xml"</Write_outputFileName-->
- <!-- If true (non-zero) we write to the output file the feature points.-->
- <Write_DetectedFeaturePoints>1</Write_DetectedFeaturePoints>
- <!-- If true (non-zero) we write to the output file the extrinsic camera parameters.-->
- <Write_extrinsicParameters>1</Write_extrinsicParameters>
- <!-- If true (non-zero) we write to the output file the refined 3D target grid points.-->
- <Write_gridPoints>1</Write_gridPoints>
- <!-- If true (non-zero) we show after calibration the undistorted images.-->
- <!--Show_UndistortedImage>1</Show_UndistortedImage-->
- <!-- If true (non-zero) will be used fisheye camera model.-->
- <Calibrate_UseFisheyeModel>0</Calibrate_UseFisheyeModel>
- <!-- If true (non-zero) distortion coefficient k1 will be equals to zero.-->
- <Fix_K1>0</Fix_K1>
- <!-- If true (non-zero) distortion coefficient k2 will be equals to zero.-->
- <Fix_K2>0</Fix_K2>
- <!-- If true (non-zero) distortion coefficient k3 will be equals to zero.-->
- <Fix_K3>0</Fix_K3>
- <!-- If true (non-zero) distortion coefficient k4 will be equals to zero.-->
- <Fix_K4>1</Fix_K4>
- <!-- If true (non-zero) distortion coefficient k5 will be equals to zero.-->
- <Fix_K5>1</Fix_K5>
-</Settings>
-</opencv_storage>
diff --git a/common/cpp/CMakeLists.txt b/components/common/cpp/CMakeLists.txt
similarity index 82%
rename from common/cpp/CMakeLists.txt
rename to components/common/cpp/CMakeLists.txt
index c8248564d8629021ce24b40f7952a20a4d7337bc..2b99f94b23d2eb130b66a6af9134661b1e9bded9 100644
--- a/common/cpp/CMakeLists.txt
+++ b/components/common/cpp/CMakeLists.txt
@@ -6,6 +6,8 @@ set(COMMONSRC
add_library(ftlcommon ${COMMONSRC})
+target_compile_options(ftlcommon PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-fPIC>)
+
target_include_directories(ftlcommon PUBLIC
${PCL_INCLUDE_DIRS}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
diff --git a/common/cpp/include/ctpl_stl.h b/components/common/cpp/include/ctpl_stl.h
similarity index 100%
rename from common/cpp/include/ctpl_stl.h
rename to components/common/cpp/include/ctpl_stl.h
diff --git a/common/cpp/include/ftl/config.h.in b/components/common/cpp/include/ftl/config.h.in
similarity index 81%
rename from common/cpp/include/ftl/config.h.in
rename to components/common/cpp/include/ftl/config.h.in
index ca51cebb894ed858580b823201f859aaa5e1f675..3d2b7d296440a20bb5515b078d9358d90e0e88d7 100644
--- a/common/cpp/include/ftl/config.h.in
+++ b/components/common/cpp/include/ftl/config.h.in
@@ -1,6 +1,16 @@
#ifndef __FTL_CONFIG_H__
#define __FTL_CONFIG_H__
+#ifdef WIN32
+#ifdef BUILD_SHARED
+#define FTLEXPORT __declspec(dllexport)
+#else
+#define FTLEXPORT __declspec(dllimport)
+#endif
+#else // WIN32
+#define FTLEXPORT
+#endif
+
#cmakedefine HAVE_VIZ
#cmakedefine HAVE_OPENCVCUDA
#cmakedefine HAVE_URIPARSESINGLE
@@ -8,6 +18,7 @@
#cmakedefine HAVE_OPENCV
#cmakedefine HAVE_PCL
#cmakedefine HAVE_RENDER
+#cmakedefine HAVE_LIBSGM
extern const char *FTL_VERSION_LONG;
extern const char *FTL_VERSION;
diff --git a/components/common/cpp/include/ftl/configurable.hpp b/components/common/cpp/include/ftl/configurable.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..71c8a556401fd568e0808e762fe5d851b9fb522a
--- /dev/null
+++ b/components/common/cpp/include/ftl/configurable.hpp
@@ -0,0 +1,42 @@
+#pragma once
+#ifndef _FTL_CONFIGURABLE_HPP_
+#define _FTL_CONFIGURABLE_HPP_
+
+#include <glog/logging.h>
+#include <nlohmann/json.hpp>
+#include <string>
+#include <tuple>
+
+#define REQUIRED(...) required(__func__, __VA_ARGS__)
+
+namespace ftl {
+
+class Configurable {
+ public:
+ Configurable() {}
+ explicit Configurable(nlohmann::json &config) : config_(config) {
+
+ }
+
+ void required(const char *f, const std::vector<std::tuple<std::string, std::string, std::string>> &r) {
+ bool diderror = false;
+ for (auto i : r) {
+ auto [name, desc, type] = i;
+ if (config_[name].type_name() != type) {
+ LOG(ERROR) << "Missing required option in \"" << f << "\": \"" << name << "\" - " << desc;
+ LOG(ERROR) << " Got type " << config_[name].type_name() << " but expected " << type;
+ diderror = true;
+ }
+ }
+ if (diderror) LOG(FATAL) << "Cannot continue without required option";
+ }
+
+ nlohmann::json &getConfig() { return config_; }
+
+ protected:
+ nlohmann::json config_;
+};
+
+}
+
+#endif // _FTL_CONFIGURABLE_HPP_
diff --git a/common/cpp/include/ftl/configuration.hpp b/components/common/cpp/include/ftl/configuration.hpp
similarity index 100%
rename from common/cpp/include/ftl/configuration.hpp
rename to components/common/cpp/include/ftl/configuration.hpp
diff --git a/vision/include/ftl/cuda_common.hpp b/components/common/cpp/include/ftl/cuda_common.hpp
similarity index 100%
rename from vision/include/ftl/cuda_common.hpp
rename to components/common/cpp/include/ftl/cuda_common.hpp
diff --git a/common/cpp/include/ftl/utility/opencv_to_pcl.hpp b/components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
similarity index 100%
rename from common/cpp/include/ftl/utility/opencv_to_pcl.hpp
rename to components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
diff --git a/common/cpp/include/nlohmann/json.hpp b/components/common/cpp/include/nlohmann/json.hpp
similarity index 100%
rename from common/cpp/include/nlohmann/json.hpp
rename to components/common/cpp/include/nlohmann/json.hpp
diff --git a/common/cpp/src/config.cpp.in b/components/common/cpp/src/config.cpp.in
similarity index 100%
rename from common/cpp/src/config.cpp.in
rename to components/common/cpp/src/config.cpp.in
diff --git a/common/cpp/src/configuration.cpp b/components/common/cpp/src/configuration.cpp
similarity index 100%
rename from common/cpp/src/configuration.cpp
rename to components/common/cpp/src/configuration.cpp
diff --git a/common/cpp/src/opencv_to_pcl.cpp b/components/common/cpp/src/opencv_to_pcl.cpp
similarity index 100%
rename from common/cpp/src/opencv_to_pcl.cpp
rename to components/common/cpp/src/opencv_to_pcl.cpp
diff --git a/net/CMakeLists.txt b/components/net/CMakeLists.txt
similarity index 100%
rename from net/CMakeLists.txt
rename to components/net/CMakeLists.txt
diff --git a/net/README.md b/components/net/README.md
similarity index 100%
rename from net/README.md
rename to components/net/README.md
diff --git a/net/cpp/CMakeLists.txt b/components/net/cpp/CMakeLists.txt
similarity index 67%
rename from net/cpp/CMakeLists.txt
rename to components/net/cpp/CMakeLists.txt
index d246cec8f0840d01be5ae5263f603452e2a8f666..d1e06de139d3bc5604cc68b95dc3d53b6a2d6937 100644
--- a/net/cpp/CMakeLists.txt
+++ b/components/net/cpp/CMakeLists.txt
@@ -1,5 +1,5 @@
# Need to include staged files and libs
-include_directories(${PROJECT_SOURCE_DIR}/net/cpp/include)
+#include_directories(${PROJECT_SOURCE_DIR}/net/cpp/include)
#include_directories(${PROJECT_BINARY_DIR})
@@ -18,7 +18,7 @@ target_include_directories(ftlnet PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlnet Threads::Threads glog::glog ${UUID_LIBRARIES} ${URIPARSER_LIBRARIES})
+target_link_libraries(ftlnet ftlcommon Threads::Threads glog::glog ${UUID_LIBRARIES} ${URIPARSER_LIBRARIES})
install(TARGETS ftlnet EXPORT ftlnet-config
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
@@ -26,8 +26,8 @@ install(TARGETS ftlnet EXPORT ftlnet-config
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
-add_executable(net-cli src/main.cpp ../../common/cpp/src/config.cpp)
-target_link_libraries(net-cli ftlnet glog::glog ${URIPARSER_LIBRARIES} Threads::Threads ${READLINE_LIBRARY} ${UUID_LIBRARIES})
+add_executable(net-cli src/main.cpp)
+target_link_libraries(net-cli ftlnet ftlcommon glog::glog ${URIPARSER_LIBRARIES} Threads::Threads ${READLINE_LIBRARY} ${UUID_LIBRARIES})
add_dependencies(net-cli ftlnet)
ADD_SUBDIRECTORY(test)
diff --git a/net/cpp/include/ftl/net.hpp b/components/net/cpp/include/ftl/net.hpp
similarity index 100%
rename from net/cpp/include/ftl/net.hpp
rename to components/net/cpp/include/ftl/net.hpp
diff --git a/net/cpp/include/ftl/net/dispatcher.hpp b/components/net/cpp/include/ftl/net/dispatcher.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/dispatcher.hpp
rename to components/net/cpp/include/ftl/net/dispatcher.hpp
diff --git a/net/cpp/include/ftl/net/func_traits.hpp b/components/net/cpp/include/ftl/net/func_traits.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/func_traits.hpp
rename to components/net/cpp/include/ftl/net/func_traits.hpp
diff --git a/net/cpp/include/ftl/net/handlers.hpp b/components/net/cpp/include/ftl/net/handlers.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/handlers.hpp
rename to components/net/cpp/include/ftl/net/handlers.hpp
diff --git a/net/cpp/include/ftl/net/ice.hpp b/components/net/cpp/include/ftl/net/ice.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/ice.hpp
rename to components/net/cpp/include/ftl/net/ice.hpp
diff --git a/net/cpp/include/ftl/net/listener.hpp b/components/net/cpp/include/ftl/net/listener.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/listener.hpp
rename to components/net/cpp/include/ftl/net/listener.hpp
diff --git a/net/cpp/include/ftl/net/p2p.hpp b/components/net/cpp/include/ftl/net/p2p.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/p2p.hpp
rename to components/net/cpp/include/ftl/net/p2p.hpp
diff --git a/net/cpp/include/ftl/net/peer.hpp b/components/net/cpp/include/ftl/net/peer.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/peer.hpp
rename to components/net/cpp/include/ftl/net/peer.hpp
diff --git a/net/cpp/include/ftl/net/protocol.hpp b/components/net/cpp/include/ftl/net/protocol.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/protocol.hpp
rename to components/net/cpp/include/ftl/net/protocol.hpp
diff --git a/net/cpp/include/ftl/net/stun.hpp b/components/net/cpp/include/ftl/net/stun.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/stun.hpp
rename to components/net/cpp/include/ftl/net/stun.hpp
diff --git a/net/cpp/include/ftl/net/universe.hpp b/components/net/cpp/include/ftl/net/universe.hpp
similarity index 98%
rename from net/cpp/include/ftl/net/universe.hpp
rename to components/net/cpp/include/ftl/net/universe.hpp
index c8b10b7651a9cc9cf64ec8237156b0fcf0d0596b..ccb6e12156fd02a7aece1ed4c84d44edf12f7719 100644
--- a/net/cpp/include/ftl/net/universe.hpp
+++ b/components/net/cpp/include/ftl/net/universe.hpp
@@ -5,6 +5,7 @@
#include <msgpack_optional.hpp>
#endif
+#include <ftl/configurable.hpp>
#include <ftl/net/peer.hpp>
#include <ftl/net/listener.hpp>
#include <ftl/net/dispatcher.hpp>
@@ -28,7 +29,7 @@ namespace net {
* callbacks will execute in a different thread so must also be threadsafe in
* their actions.
*/
-class Universe {
+class Universe : public ftl::Configurable {
public:
Universe();
/**
@@ -143,7 +144,6 @@ class Universe {
private:
bool active_;
ftl::UUID this_peer;
- nlohmann::json config_;
std::mutex net_mutex_;
fd_set sfderror_;
fd_set sfdread_;
diff --git a/net/cpp/include/ftl/net/ws_internal.hpp b/components/net/cpp/include/ftl/net/ws_internal.hpp
similarity index 100%
rename from net/cpp/include/ftl/net/ws_internal.hpp
rename to components/net/cpp/include/ftl/net/ws_internal.hpp
diff --git a/net/cpp/include/ftl/uri.hpp b/components/net/cpp/include/ftl/uri.hpp
similarity index 100%
rename from net/cpp/include/ftl/uri.hpp
rename to components/net/cpp/include/ftl/uri.hpp
diff --git a/net/cpp/include/ftl/uuid.hpp b/components/net/cpp/include/ftl/uuid.hpp
similarity index 100%
rename from net/cpp/include/ftl/uuid.hpp
rename to components/net/cpp/include/ftl/uuid.hpp
diff --git a/net/cpp/include/msgpack_optional.hpp b/components/net/cpp/include/msgpack_optional.hpp
similarity index 100%
rename from net/cpp/include/msgpack_optional.hpp
rename to components/net/cpp/include/msgpack_optional.hpp
diff --git a/net/cpp/src/dispatcher.cpp b/components/net/cpp/src/dispatcher.cpp
similarity index 100%
rename from net/cpp/src/dispatcher.cpp
rename to components/net/cpp/src/dispatcher.cpp
diff --git a/net/cpp/src/ice.cpp b/components/net/cpp/src/ice.cpp
similarity index 100%
rename from net/cpp/src/ice.cpp
rename to components/net/cpp/src/ice.cpp
diff --git a/net/cpp/src/listener.cpp b/components/net/cpp/src/listener.cpp
similarity index 100%
rename from net/cpp/src/listener.cpp
rename to components/net/cpp/src/listener.cpp
diff --git a/net/cpp/src/main.cpp b/components/net/cpp/src/main.cpp
similarity index 100%
rename from net/cpp/src/main.cpp
rename to components/net/cpp/src/main.cpp
diff --git a/net/cpp/src/net.cpp b/components/net/cpp/src/net.cpp
similarity index 100%
rename from net/cpp/src/net.cpp
rename to components/net/cpp/src/net.cpp
diff --git a/net/cpp/src/net_internal.hpp b/components/net/cpp/src/net_internal.hpp
similarity index 100%
rename from net/cpp/src/net_internal.hpp
rename to components/net/cpp/src/net_internal.hpp
diff --git a/net/cpp/src/p2p.cpp b/components/net/cpp/src/p2p.cpp
similarity index 100%
rename from net/cpp/src/p2p.cpp
rename to components/net/cpp/src/p2p.cpp
diff --git a/net/cpp/src/peer.cpp b/components/net/cpp/src/peer.cpp
similarity index 100%
rename from net/cpp/src/peer.cpp
rename to components/net/cpp/src/peer.cpp
diff --git a/net/cpp/src/universe.cpp b/components/net/cpp/src/universe.cpp
similarity index 96%
rename from net/cpp/src/universe.cpp
rename to components/net/cpp/src/universe.cpp
index 194b9d32687bf224fdefaf891b6dff90d4c03fa4..b78af39903a14d71a24cbc6aafc8820314efca43 100644
--- a/net/cpp/src/universe.cpp
+++ b/components/net/cpp/src/universe.cpp
@@ -18,12 +18,12 @@ using std::optional;
using std::unique_lock;
using std::mutex;
-Universe::Universe() : active_(true), this_peer(ftl::net::this_peer), thread_(Universe::__start, this) {
+Universe::Universe() : Configurable(), active_(true), this_peer(ftl::net::this_peer), thread_(Universe::__start, this) {
_installBindings();
}
Universe::Universe(nlohmann::json &config) :
- active_(true), this_peer(ftl::net::this_peer), config_(config), thread_(Universe::__start, this) {
+ Configurable(config), active_(true), this_peer(ftl::net::this_peer), thread_(Universe::__start, this) {
if (config["listen"].is_array()) {
for (auto &l : config["listen"]) {
listen(l);
diff --git a/net/cpp/src/uri.cpp b/components/net/cpp/src/uri.cpp
similarity index 100%
rename from net/cpp/src/uri.cpp
rename to components/net/cpp/src/uri.cpp
diff --git a/net/cpp/src/ws_internal.cpp b/components/net/cpp/src/ws_internal.cpp
similarity index 100%
rename from net/cpp/src/ws_internal.cpp
rename to components/net/cpp/src/ws_internal.cpp
diff --git a/net/cpp/test/CMakeLists.txt b/components/net/cpp/test/CMakeLists.txt
similarity index 82%
rename from net/cpp/test/CMakeLists.txt
rename to components/net/cpp/test/CMakeLists.txt
index b641391a363b21f66342c65d75c09bf7401f8439..e81a89721fb6022c7f2fb7c286a588efd5fc0d92 100644
--- a/net/cpp/test/CMakeLists.txt
+++ b/components/net/cpp/test/CMakeLists.txt
@@ -7,6 +7,7 @@ add_executable(peer_unit
./peer_unit.cpp
../../../common/cpp/src/config.cpp
)
+target_include_directories(peer_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include" "${CMAKE_CURRENT_SOURCE_DIR}/../../../common/cpp/include")
target_link_libraries(peer_unit
${URIPARSER_LIBRARIES}
glog::glog
@@ -18,6 +19,7 @@ add_executable(uri_unit
./tests.cpp
../src/uri.cpp
./uri_unit.cpp)
+target_include_directories(uri_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include")
target_link_libraries(uri_unit
${URIPARSER_LIBRARIES})
@@ -40,6 +42,7 @@ add_executable(net_integration
../../../common/cpp/src/config.cpp
./net_integration.cpp)
add_dependencies(net_integration ftlnet)
+target_include_directories(net_integration PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}../include")
target_link_libraries(net_integration
ftlnet
${URIPARSER_LIBRARIES}
diff --git a/net/cpp/test/catch.hpp b/components/net/cpp/test/catch.hpp
similarity index 100%
rename from net/cpp/test/catch.hpp
rename to components/net/cpp/test/catch.hpp
diff --git a/net/cpp/test/ice.cpp b/components/net/cpp/test/ice.cpp
similarity index 100%
rename from net/cpp/test/ice.cpp
rename to components/net/cpp/test/ice.cpp
diff --git a/net/cpp/test/net_integration.cpp b/components/net/cpp/test/net_integration.cpp
similarity index 100%
rename from net/cpp/test/net_integration.cpp
rename to components/net/cpp/test/net_integration.cpp
diff --git a/net/cpp/test/p2p_base_unit.cpp b/components/net/cpp/test/p2p_base_unit.cpp
similarity index 100%
rename from net/cpp/test/p2p_base_unit.cpp
rename to components/net/cpp/test/p2p_base_unit.cpp
diff --git a/net/cpp/test/peer_unit.cpp b/components/net/cpp/test/peer_unit.cpp
similarity index 100%
rename from net/cpp/test/peer_unit.cpp
rename to components/net/cpp/test/peer_unit.cpp
diff --git a/net/cpp/test/protocol_unit.cpp b/components/net/cpp/test/protocol_unit.cpp
similarity index 100%
rename from net/cpp/test/protocol_unit.cpp
rename to components/net/cpp/test/protocol_unit.cpp
diff --git a/net/cpp/test/tests.cpp b/components/net/cpp/test/tests.cpp
similarity index 100%
rename from net/cpp/test/tests.cpp
rename to components/net/cpp/test/tests.cpp
diff --git a/net/cpp/test/uri_unit.cpp b/components/net/cpp/test/uri_unit.cpp
similarity index 100%
rename from net/cpp/test/uri_unit.cpp
rename to components/net/cpp/test/uri_unit.cpp
diff --git a/net/js/package.json b/components/net/js/package.json
similarity index 100%
rename from net/js/package.json
rename to components/net/js/package.json
diff --git a/net/js/src/index.js b/components/net/js/src/index.js
similarity index 100%
rename from net/js/src/index.js
rename to components/net/js/src/index.js
diff --git a/net/js/src/listener.js b/components/net/js/src/listener.js
similarity index 100%
rename from net/js/src/listener.js
rename to components/net/js/src/listener.js
diff --git a/net/js/src/peer.js b/components/net/js/src/peer.js
similarity index 100%
rename from net/js/src/peer.js
rename to components/net/js/src/peer.js
diff --git a/net/js/src/universe.js b/components/net/js/src/universe.js
similarity index 100%
rename from net/js/src/universe.js
rename to components/net/js/src/universe.js
diff --git a/net/js/test/peer_unit.js b/components/net/js/test/peer_unit.js
similarity index 100%
rename from net/js/test/peer_unit.js
rename to components/net/js/test/peer_unit.js
diff --git a/renderer/CMakeLists.txt b/components/renderers/CMakeLists.txt
similarity index 100%
rename from renderer/CMakeLists.txt
rename to components/renderers/CMakeLists.txt
diff --git a/renderer/cpp/CMakeLists.txt b/components/renderers/cpp/CMakeLists.txt
similarity index 75%
rename from renderer/cpp/CMakeLists.txt
rename to components/renderers/cpp/CMakeLists.txt
index 6ef0af73b9e9f21de44ab64c6e9ecbe30b6ee6a8..e603f6740f6313f2f699e3e0bf9ef78774ab464d 100644
--- a/renderer/cpp/CMakeLists.txt
+++ b/components/renderers/cpp/CMakeLists.txt
@@ -10,6 +10,6 @@ target_include_directories(ftlrender PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlrender ftlcommon Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
+target_link_libraries(ftlrender ftlcommon ftlnet Eigen3::Eigen Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
#ADD_SUBDIRECTORY(test)
diff --git a/renderer/cpp/include/ftl/display.hpp b/components/renderers/cpp/include/ftl/display.hpp
similarity index 83%
rename from renderer/cpp/include/ftl/display.hpp
rename to components/renderers/cpp/include/ftl/display.hpp
index 04ff003de8b723f0af5d0475a1946fc6f7a4d302..75d0f785843392be10d3722b89728645dfd30d0a 100644
--- a/renderer/cpp/include/ftl/display.hpp
+++ b/components/renderers/cpp/include/ftl/display.hpp
@@ -6,6 +6,7 @@
#define _FTL_DISPLAY_HPP_
#include <ftl/config.h>
+#include "../../../rgbd-sources/include/ftl/rgbd_source.hpp"
#include <nlohmann/json.hpp>
#include <opencv2/opencv.hpp>
@@ -33,8 +34,7 @@ class Display {
explicit Display(nlohmann::json &config, std::string name);
~Display();
- inline bool render(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &q) { return render(rgb, cv::Mat(), q); }
- bool render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &depth, const cv::Mat &q);
+ bool render(const cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::CameraParameters &p);
#if defined HAVE_PCL
bool render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr);
diff --git a/renderer/cpp/src/display.cpp b/components/renderers/cpp/src/display.cpp
similarity index 77%
rename from renderer/cpp/src/display.cpp
rename to components/renderers/cpp/src/display.cpp
index 4a64fbb85988d8584b023f4ff65e73b5e3f5fcda..32f99719250c9d2abca5a8cce0ebbaef8bedf8bf 100644
--- a/renderer/cpp/src/display.cpp
+++ b/components/renderers/cpp/src/display.cpp
@@ -85,14 +85,51 @@ Display::~Display() {
#endif // HAVE_VIZ
}
-bool Display::render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &depth, const cv::Mat &q) {
+#ifdef HAVE_PCL
+/**
+ * Convert an OpenCV RGB and Depth Mats to a PCL XYZRGB point cloud.
+ */
+static pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgbdToPointXYZ(const cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::CameraParameters &p) {
+ const double CX = p.cx;
+ const double CY = p.cy;
+ const double F = p.f;
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
+ point_cloud_ptr->width = rgb.cols * rgb.rows;
+ point_cloud_ptr->height = 1;
+
+ for(int i=0;i<rgb.rows;i++) {
+ const float *sptr = depth.ptr<float>(i);
+ for(int j=0;j<rgb.cols;j++) {
+ float d = sptr[j] * 1000.0f;
+
+ pcl::PointXYZRGB point;
+ point.x = (((double)j + CX) / F) * d;
+ point.y = (((double)i + CY) / F) * d;
+ point.z = d;
+
+ if (point.x == INFINITY || point.y == INFINITY || point.z > 20000.0f || point.z < 0.04f) {
+ point.x = 0.0f; point.y = 0.0f; point.z = 0.0f;
+ }
+
+ cv::Point3_<uchar> prgb = rgb.at<cv::Point3_<uchar>>(i, j);
+ uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
+ point.rgb = *reinterpret_cast<float*>(&rgb);
+
+ point_cloud_ptr -> points.push_back(point);
+ }
+ }
+
+ return point_cloud_ptr;
+}
+#endif // HAVE_PCL
+
+bool Display::render(const cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::CameraParameters &p) {
Mat idepth;
- if (config_["points"] && q.rows != 0) {
+ if (config_["points"] && rgb.rows != 0) {
#if defined HAVE_PCL
- cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud
- reprojectImageTo3D(depth, XYZ, q, false);
- auto pc = ftl::utility::matToPointXYZ(XYZ, rgb);
+ auto pc = rgbdToPointXYZ(rgb, depth, p);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
if (!pclviz_->updatePointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction")) {
@@ -103,7 +140,7 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &dep
#elif defined HAVE_VIZ
//cv::Mat Q_32F;
//calibrate_.getQ().convertTo(Q_32F, CV_32F);
- cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud
+ /*cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud
reprojectImageTo3D(depth+20.0f, XYZ, q, true);
// Remove all invalid pixels from point cloud
@@ -115,7 +152,7 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &dep
window_->showWidget("coosys", cv::viz::WCoordinateSystem());
window_->showWidget("Depth", cloud_widget);
- //window_->spinOnce(40, true);
+ //window_->spinOnce(40, true);*/
#else // HAVE_VIZ
@@ -133,15 +170,15 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &dep
cv::imshow("Left: " + name_, rgb);
}
if (config_["right"]) {
- if (config_["crosshair"]) {
+ /*if (config_["crosshair"]) {
cv::line(rgbr, cv::Point(0, rgbr.rows/2), cv::Point(rgbr.cols-1, rgbr.rows/2), cv::Scalar(0,0,255), 1);
cv::line(rgbr, cv::Point(rgbr.cols/2, 0), cv::Point(rgbr.cols/2, rgbr.rows-1), cv::Scalar(0,0,255), 1);
}
cv::namedWindow("Right: " + name_, cv::WINDOW_KEEPRATIO);
- cv::imshow("Right: " + name_, rgbr);
+ cv::imshow("Right: " + name_, rgbr);*/
}
- if (config_["depth"]) {
+ if (config_["disparity"]) {
/*Mat depth32F = (focal * (float)l.cols * base_line) / depth;
normalize(depth32F, depth32F, 0, 255, NORM_MINMAX, CV_8U);
cv::imshow("Depth", depth32F);
@@ -149,17 +186,17 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &dep
//exit if ESC is pressed
active_ = false;
}*/
- } else if (config_["disparity"]) {
+ } else if (config_["depth"]) {
if ((bool)config_["flip_vert"]) {
cv::flip(depth, idepth, 0);
} else {
idepth = depth;
}
- idepth.convertTo(idepth, CV_8U, 255.0f / 256.0f); // TODO(nick)
+ idepth.convertTo(idepth, CV_8U, 255.0f / 10.0f); // TODO(nick)
applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- cv::imshow("Disparity:" + name_, idepth);
+ cv::imshow("Depth: " + name_, idepth);
//if(cv::waitKey(40) == 27) {
// exit if ESC is pressed
// active_ = false;
@@ -210,7 +247,7 @@ void Display::wait(int ms) {
#endif // HAVE_VIZ
}
- if (config_["disparity"] || config_["left"] || config_["right"]) {
+ if (config_["depth"] || config_["left"] || config_["right"]) {
if(cv::waitKey(ms) == 27) {
// exit if ESC is pressed
active_ = false;
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0c4cafc742eac4a254f6fbf38f3813972a33126f
--- /dev/null
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -0,0 +1,48 @@
+set(RGBDSRC
+ src/calibrate.cpp
+ src/local.cpp
+ src/disparity.cpp
+ src/rgbd_source.cpp
+ src/stereovideo_source.cpp
+ src/net_source.cpp
+ src/algorithms/rtcensus.cpp
+ src/algorithms/rtcensus_sgm.cpp
+ src/algorithms/opencv_sgbm.cpp
+ src/algorithms/opencv_bm.cpp
+)
+
+if (LIBSGM_FOUND)
+ list(APPEND RGBDSRC "src/algorithms/fixstars_sgm.cpp")
+endif (LIBSGM_FOUND)
+
+if (CUDA_FOUND)
+ list(APPEND RGBDSRC
+ "src/algorithms/opencv_cuda_bm.cpp"
+ "src/algorithms/opencv_cuda_bp.cpp"
+ "src/algorithms/rtcensus.cu"
+ "src/algorithms/rtcensus_sgm.cu"
+ "src/algorithms/consistency.cu"
+ "src/algorithms/sparse_census.cu"
+ "src/algorithms/tex_filter.cu"
+ "src/algorithms/nick1.cu"
+ "src/algorithms/nick.cpp")
+endif (CUDA_FOUND)
+
+add_library(ftlrgbd ${RGBDSRC})
+
+# target_compile_options(ftlrgbd PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-fPIC>)
+# target_compile_options(ftlrgbd PUBLIC "-DMAKE_SHARED")
+
+target_include_directories(ftlrgbd PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>
+ PRIVATE src)
+
+if (CUDA_FOUND)
+set_property(TARGET ftlrgbd PROPERTY CUDA_SEPARABLE_COMPILATION OFF)
+endif()
+
+#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
+target_link_libraries(ftlrgbd ftlcommon Threads::Threads ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen glog::glog ftlnet)
+
+
diff --git a/vision/include/ftl/algorithms/opencv_cuda_csbp.hpp b/components/rgbd-sources/README.md
similarity index 100%
rename from vision/include/ftl/algorithms/opencv_cuda_csbp.hpp
rename to components/rgbd-sources/README.md
diff --git a/components/rgbd-sources/include/ftl/net_source.hpp b/components/rgbd-sources/include/ftl/net_source.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..327e95319145252dc7417f7dc9bbf6b4511fc34c
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/net_source.hpp
@@ -0,0 +1,43 @@
+#pragma once
+#ifndef _FTL_RGBD_NET_HPP_
+#define _FTL_RGBD_NET_HPP_
+
+#include <ftl/net/universe.hpp>
+#include <ftl/rgbd_source.hpp>
+#include <string>
+#include <mutex>
+
+namespace ftl {
+namespace rgbd {
+
+/**
+ * RGBD source from either a stereo video file with left + right images, or
+ * direct from two camera devices. A variety of algorithms are included for
+ * calculating disparity, before converting to depth. Calibration of the images
+ * is also performed.
+ */
+class NetSource : public RGBDSource {
+ public:
+ explicit NetSource(nlohmann::json &config);
+ NetSource(nlohmann::json &config, ftl::net::Universe *net);
+ ~NetSource();
+
+ void grab();
+ void getRGBD(cv::Mat &rgb, cv::Mat &depth);
+ bool isReady();
+
+ static inline RGBDSource *create(nlohmann::json &config, ftl::net::Universe *net) {
+ return new NetSource(config, net);
+ }
+
+ private:
+ bool has_calibration_;
+ cv::Mat rgb_;
+ cv::Mat depth_;
+ std::mutex m_;
+};
+
+}
+}
+
+#endif // _FTL_RGBD_NET_HPP_
diff --git a/components/rgbd-sources/include/ftl/rgbd.hpp b/components/rgbd-sources/include/ftl/rgbd.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c85524e9c344fe9e885f707804d814d737fb93e3
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/rgbd.hpp
@@ -0,0 +1,11 @@
+#pragma once
+#ifndef _FTL_RGBD_HPP_
+#define _FTL_RGBD_HPP_
+
+#include <ftl/rgbd_source.hpp>
+#include <ftl/rgbdimage_source.hpp>
+#include <ftl/stereoimage_source.hpp>
+#include <ftl/stereovideo_source.hpp>
+#include <ftl/net_source.hpp>
+
+#endif // _FTL_RGBD_HPP_
diff --git a/components/rgbd-sources/include/ftl/rgbd_source.hpp b/components/rgbd-sources/include/ftl/rgbd_source.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c43a1aaf2b4fe98b335c6654c4074ff7c4da69f6
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/rgbd_source.hpp
@@ -0,0 +1,87 @@
+#pragma once
+#ifndef _FTL_RGBD_SOURCE_HPP_
+#define _FTL_RGBD_SOURCE_HPP_
+
+#include <ftl/config.h>
+#include <ftl/configurable.hpp>
+#include <ftl/net/universe.hpp>
+#include <opencv2/opencv.hpp>
+#include <Eigen/Eigen>
+
+namespace ftl {
+namespace rgbd {
+
+struct CameraParameters {
+ double f;
+ double cx;
+ double cy;
+ unsigned int width;
+ unsigned int height;
+ double minDepth;
+ double maxDepth;
+};
+
+/**
+ * Abstract class for any generic RGB-Depth data source. It can also store pose
+ * information, although this must be provided by an external source.
+ */
+class RGBDSource : public ftl::Configurable {
+ public:
+ RGBDSource(nlohmann::json &config);
+ RGBDSource(nlohmann::json &config, ftl::net::Universe *net);
+ virtual ~RGBDSource();
+
+ virtual void grab()=0;
+ virtual void getRGBD(cv::Mat &rgb, cv::Mat &depth)=0;
+ virtual bool isReady();
+
+ const CameraParameters &getParameters() { return params_; };
+
+ void setPose(const Eigen::Matrix4f &pose) { pose_ = pose; };
+ const Eigen::Matrix4f &getPose() { return pose_; };
+
+ /**
+ * Save the current RGB and Depth images to image files (jpg and png) with
+ * the specified file prefix (excluding file extension).
+ */
+ bool snapshot(const std::string &fileprefix);
+
+ /**
+ * Generate a video of this RGB-D source.
+ */
+ //bool record(const std::string &filename);
+
+ /**
+ * Factory registration class.
+ */
+ class Register {
+ public:
+ Register(const std::string &n, std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)> f) {
+ RGBDSource::_register(n,f);
+ };
+ };
+
+ /**
+ * Factory instance creator where config contains a "type" property
+ * used as the instance name to construct.
+ */
+ static RGBDSource *create(nlohmann::json &config, ftl::net::Universe *net);
+
+ protected:
+ static void _register(const std::string &n, std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)> f);
+
+ protected:
+ CameraParameters params_;
+ ftl::net::Universe *net_;
+
+ private:
+ Eigen::Matrix4f pose_;
+
+ private:
+ static std::map<std::string,std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)>> sources__;
+};
+
+};
+};
+
+#endif // _FTL_RGBD_SOURCE_HPP_
diff --git a/vision/src/algorithms/opencv_cuda_csbp.cpp b/components/rgbd-sources/include/ftl/rgbdimage_source.hpp
similarity index 100%
rename from vision/src/algorithms/opencv_cuda_csbp.cpp
rename to components/rgbd-sources/include/ftl/rgbdimage_source.hpp
diff --git a/components/rgbd-sources/include/ftl/rgbdvideo_source.hpp b/components/rgbd-sources/include/ftl/rgbdvideo_source.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/components/rgbd-sources/include/ftl/stereoimage_source.hpp b/components/rgbd-sources/include/ftl/stereoimage_source.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/components/rgbd-sources/include/ftl/stereovideo_source.hpp b/components/rgbd-sources/include/ftl/stereovideo_source.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d59908722779bf7d5c513c8f870db44dcfb60ed0
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/stereovideo_source.hpp
@@ -0,0 +1,48 @@
+#pragma once
+#ifndef _FTL_RGBD_STEREOVIDEO_HPP_
+#define _FTL_RGBD_STEREOVIDEO_HPP_
+
+#include <ftl/rgbd_source.hpp>
+#include <string>
+
+namespace ftl {
+
+class LocalSource;
+class Calibrate;
+class Disparity;
+
+namespace rgbd {
+
+/**
+ * RGBD source from either a stereo video file with left + right images, or
+ * direct from two camera devices. A variety of algorithms are included for
+ * calculating disparity, before converting to depth. Calibration of the images
+ * is also performed.
+ */
+class StereoVideoSource : public RGBDSource {
+ public:
+ StereoVideoSource(nlohmann::json &config, ftl::net::Universe *net);
+ StereoVideoSource(nlohmann::json &config, const std::string &file);
+ ~StereoVideoSource();
+
+ void grab();
+ void getRGBD(cv::Mat &rgb, cv::Mat &depth);
+ bool isReady();
+
+ static inline RGBDSource *create(nlohmann::json &config, ftl::net::Universe *net) {
+ return new StereoVideoSource(config, net);
+ }
+
+ private:
+ ftl::LocalSource *lsrc_;
+ ftl::Calibrate *calib_;
+ ftl::Disparity *disp_;
+ bool ready_;
+ cv::Mat left_;
+ cv::Mat right_;
+};
+
+}
+}
+
+#endif // _FTL_RGBD_STEREOVIDEO_HPP_
diff --git a/vision/src/algorithms/consistency.cu b/components/rgbd-sources/src/algorithms/consistency.cu
similarity index 100%
rename from vision/src/algorithms/consistency.cu
rename to components/rgbd-sources/src/algorithms/consistency.cu
diff --git a/vision/src/algorithms/elas.cpp b/components/rgbd-sources/src/algorithms/elas.cpp
similarity index 100%
rename from vision/src/algorithms/elas.cpp
rename to components/rgbd-sources/src/algorithms/elas.cpp
diff --git a/vision/include/ftl/algorithms/elas.hpp b/components/rgbd-sources/src/algorithms/elas.hpp
similarity index 100%
rename from vision/include/ftl/algorithms/elas.hpp
rename to components/rgbd-sources/src/algorithms/elas.hpp
diff --git a/vision/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
similarity index 88%
rename from vision/src/algorithms/fixstars_sgm.cpp
rename to components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index 1c4b4decffe7beeb977145090300b18b2f08961f..4274af03342491dd51100d8ebc0e8e27f0ce6986 100644
--- a/vision/src/algorithms/fixstars_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
@@ -1,12 +1,12 @@
/* Copyright 2019 Nicolas Pope */
-#include <ftl/algorithms/fixstars_sgm.hpp>
+#include "fixstars_sgm.hpp"
#include <glog/logging.h>
using ftl::algorithms::FixstarsSGM;
using cv::Mat;
-static ftl::Disparity::Register fixstarssgm("libsgm", FixstarsSGM::create);
+//static ftl::Disparity::Register fixstarssgm("libsgm", FixstarsSGM::create);
FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
ssgm_ = nullptr;
diff --git a/vision/include/ftl/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
similarity index 96%
rename from vision/include/ftl/algorithms/fixstars_sgm.hpp
rename to components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
index 9d71c1b939f6c43769fcd83c32f80ba60d2545b1..76ca631855eb47955a7695747e04b5045995bf6c 100644
--- a/vision/include/ftl/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -8,7 +8,7 @@
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <libsgm.h>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
namespace ftl {
namespace algorithms {
diff --git a/vision/src/algorithms/nick.cpp b/components/rgbd-sources/src/algorithms/nick.cpp
similarity index 96%
rename from vision/src/algorithms/nick.cpp
rename to components/rgbd-sources/src/algorithms/nick.cpp
index 5699023e7683b1f8c57aeebd02d6c99a8f71559a..55ccd1db439e61bc13114239461da44d53ffc7f6 100644
--- a/vision/src/algorithms/nick.cpp
+++ b/components/rgbd-sources/src/algorithms/nick.cpp
@@ -1,4 +1,4 @@
-#include <ftl/algorithms/nick.hpp>
+#include "nick.hpp"
#include <vector_types.h>
using ftl::algorithms::NickCuda;
diff --git a/vision/include/ftl/algorithms/nick.hpp b/components/rgbd-sources/src/algorithms/nick.hpp
similarity index 95%
rename from vision/include/ftl/algorithms/nick.hpp
rename to components/rgbd-sources/src/algorithms/nick.hpp
index 685882ba062f49b0b22f0f709ad8d1f9cb4ddb1e..ee18f77019ce600d745023084f1fd059258ca9e9 100644
--- a/vision/include/ftl/algorithms/nick.hpp
+++ b/components/rgbd-sources/src/algorithms/nick.hpp
@@ -4,7 +4,7 @@
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/cudastereo.hpp>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
namespace ftl {
namespace algorithms {
diff --git a/vision/src/algorithms/nick1.cu b/components/rgbd-sources/src/algorithms/nick1.cu
similarity index 99%
rename from vision/src/algorithms/nick1.cu
rename to components/rgbd-sources/src/algorithms/nick1.cu
index 034ae81afd20efd16e49d2b8418c5539a211be36..ab6c0d4bbb60b51c16b95a4bd42b3e67ddedb46d 100644
--- a/vision/src/algorithms/nick1.cu
+++ b/components/rgbd-sources/src/algorithms/nick1.cu
@@ -5,7 +5,7 @@
*/
#include <ftl/cuda_common.hpp>
-#include <ftl/cuda_algorithms.hpp>
+#include "../cuda_algorithms.hpp"
using namespace cv::cuda;
using namespace cv;
diff --git a/vision/src/algorithms/opencv_bm.cpp b/components/rgbd-sources/src/algorithms/opencv_bm.cpp
similarity index 96%
rename from vision/src/algorithms/opencv_bm.cpp
rename to components/rgbd-sources/src/algorithms/opencv_bm.cpp
index 203ac37e221e5b086fcf32295dfbc9e09d1e1655..e4a840e46b9f9045e1e0ef4ce206780440f1a2e9 100644
--- a/vision/src/algorithms/opencv_bm.cpp
+++ b/components/rgbd-sources/src/algorithms/opencv_bm.cpp
@@ -1,4 +1,4 @@
-#include <ftl/algorithms/opencv_bm.hpp>
+#include "opencv_bm.hpp"
using ftl::algorithms::OpenCVBM;
using namespace cv::ximgproc;
diff --git a/vision/include/ftl/algorithms/opencv_bm.hpp b/components/rgbd-sources/src/algorithms/opencv_bm.hpp
similarity index 96%
rename from vision/include/ftl/algorithms/opencv_bm.hpp
rename to components/rgbd-sources/src/algorithms/opencv_bm.hpp
index 3a83ae6d8b5d775132d211a5810be5c1c95dbaa4..b890c1b450ebb8263a5f821cdec21c9d707152cb 100644
--- a/vision/include/ftl/algorithms/opencv_bm.hpp
+++ b/components/rgbd-sources/src/algorithms/opencv_bm.hpp
@@ -9,7 +9,7 @@
#include <opencv2/opencv.hpp>
#include "opencv2/ximgproc.hpp"
#include <opencv2/calib3d.hpp>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
namespace ftl {
namespace algorithms {
diff --git a/vision/src/algorithms/opencv_cuda_bm.cpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bm.cpp
similarity index 94%
rename from vision/src/algorithms/opencv_cuda_bm.cpp
rename to components/rgbd-sources/src/algorithms/opencv_cuda_bm.cpp
index 5957792fe530e10e9b5c8213887a253ecb85de98..6261049f07d53bfbcd1152857ab77dc630d6f726 100644
--- a/vision/src/algorithms/opencv_cuda_bm.cpp
+++ b/components/rgbd-sources/src/algorithms/opencv_cuda_bm.cpp
@@ -1,4 +1,4 @@
-#include <ftl/algorithms/opencv_cuda_bm.hpp>
+#include "opencv_cuda_bm.hpp"
using ftl::algorithms::OpenCVCudaBM;
using namespace cv;
diff --git a/vision/include/ftl/algorithms/opencv_cuda_bm.hpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bm.hpp
similarity index 96%
rename from vision/include/ftl/algorithms/opencv_cuda_bm.hpp
rename to components/rgbd-sources/src/algorithms/opencv_cuda_bm.hpp
index 405cf97cd44423327b93498093f33caef0fefa2f..3542c8dd5f3da6f8a8e0f39cf34e7ada4dd05d04 100644
--- a/vision/include/ftl/algorithms/opencv_cuda_bm.hpp
+++ b/components/rgbd-sources/src/algorithms/opencv_cuda_bm.hpp
@@ -8,7 +8,7 @@
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/cudastereo.hpp>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
namespace ftl {
namespace algorithms {
diff --git a/vision/src/algorithms/opencv_cuda_bp.cpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bp.cpp
similarity index 93%
rename from vision/src/algorithms/opencv_cuda_bp.cpp
rename to components/rgbd-sources/src/algorithms/opencv_cuda_bp.cpp
index 51f1e58d0a1f836d4b6e412060fbdf7d06e3c9b8..ac0eafc50010af9062243564873599062d722420 100644
--- a/vision/src/algorithms/opencv_cuda_bp.cpp
+++ b/components/rgbd-sources/src/algorithms/opencv_cuda_bp.cpp
@@ -1,4 +1,4 @@
-#include <ftl/algorithms/opencv_cuda_bp.hpp>
+#include "opencv_cuda_bp.hpp"
using ftl::algorithms::OpenCVCudaBP;
using namespace cv;
diff --git a/vision/include/ftl/algorithms/opencv_cuda_bp.hpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bp.hpp
similarity index 96%
rename from vision/include/ftl/algorithms/opencv_cuda_bp.hpp
rename to components/rgbd-sources/src/algorithms/opencv_cuda_bp.hpp
index a89d56df40c65488f4fca09222bbae0f960857e9..4a096ed30bf753835dd3587b827d5204ce5a4652 100644
--- a/vision/include/ftl/algorithms/opencv_cuda_bp.hpp
+++ b/components/rgbd-sources/src/algorithms/opencv_cuda_bp.hpp
@@ -8,7 +8,7 @@
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/cudastereo.hpp>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
namespace ftl {
namespace algorithms {
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.cpp b/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.hpp b/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/vision/src/algorithms/opencv_sgbm.cpp b/components/rgbd-sources/src/algorithms/opencv_sgbm.cpp
similarity index 97%
rename from vision/src/algorithms/opencv_sgbm.cpp
rename to components/rgbd-sources/src/algorithms/opencv_sgbm.cpp
index 1f9a5aa1e8d5a3f03f669c0672c3244a76e61fb2..0d5f0f9e9034c7b456639751d80221a6b0184610 100644
--- a/vision/src/algorithms/opencv_sgbm.cpp
+++ b/components/rgbd-sources/src/algorithms/opencv_sgbm.cpp
@@ -2,7 +2,7 @@
* Copyright 2019 Nicolas Pope
*/
-#include <ftl/algorithms/opencv_sgbm.hpp>
+#include "opencv_sgbm.hpp"
using ftl::algorithms::OpenCVSGBM;
using cv::Mat;
diff --git a/vision/include/ftl/algorithms/opencv_sgbm.hpp b/components/rgbd-sources/src/algorithms/opencv_sgbm.hpp
similarity index 96%
rename from vision/include/ftl/algorithms/opencv_sgbm.hpp
rename to components/rgbd-sources/src/algorithms/opencv_sgbm.hpp
index d36966a1d660b6d7d0b4d21f96d25602de63ab6b..052b331aebd73cf5042223f92b0b489c5c5d1319 100644
--- a/vision/include/ftl/algorithms/opencv_sgbm.hpp
+++ b/components/rgbd-sources/src/algorithms/opencv_sgbm.hpp
@@ -9,7 +9,7 @@
#include <opencv2/opencv.hpp>
#include "opencv2/ximgproc.hpp"
#include <opencv2/calib3d.hpp>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
namespace ftl {
namespace algorithms {
diff --git a/vision/src/algorithms/rtcensus.cpp b/components/rgbd-sources/src/algorithms/rtcensus.cpp
similarity index 98%
rename from vision/src/algorithms/rtcensus.cpp
rename to components/rgbd-sources/src/algorithms/rtcensus.cpp
index d1aeba173fabb8764b3d78eddf0b761bd74aa1b3..cd0e92ea706fea7fcaa8469b52cae31983170ca8 100644
--- a/vision/src/algorithms/rtcensus.cpp
+++ b/components/rgbd-sources/src/algorithms/rtcensus.cpp
@@ -22,7 +22,7 @@
#include <bitset>
#include <chrono>
-#include <ftl/algorithms/rtcensus.hpp>
+#include "rtcensus.hpp"
using ftl::algorithms::RTCensus;
using std::vector;
@@ -34,8 +34,6 @@ using std::get;
using std::make_tuple;
using std::bitset;
-static ftl::Disparity::Register rtcensus("rtcensus", RTCensus::create);
-
/* (8) and (16) */
#define XHI(P1, P2) ((P1 <= P2) ? 0 : 1)
diff --git a/vision/src/algorithms/rtcensus.cu b/components/rgbd-sources/src/algorithms/rtcensus.cu
similarity index 99%
rename from vision/src/algorithms/rtcensus.cu
rename to components/rgbd-sources/src/algorithms/rtcensus.cu
index d2aa073eb8db0730e1e108b381f8fe6b322571ff..6ff8e331e6d7feba47522175c9243a7ea60e8aff 100644
--- a/vision/src/algorithms/rtcensus.cu
+++ b/components/rgbd-sources/src/algorithms/rtcensus.cu
@@ -14,7 +14,7 @@
*/
#include <ftl/cuda_common.hpp>
-#include <ftl/cuda_algorithms.hpp>
+#include "../cuda_algorithms.hpp"
using namespace cv::cuda;
using namespace cv;
diff --git a/vision/include/ftl/algorithms/rtcensus.hpp b/components/rgbd-sources/src/algorithms/rtcensus.hpp
similarity index 97%
rename from vision/include/ftl/algorithms/rtcensus.hpp
rename to components/rgbd-sources/src/algorithms/rtcensus.hpp
index 2623e18bd7b72a1bbdd537696d3b004348ae789a..f4a7d9c26b29c98c831d386ca71c9512d2ad1bfc 100644
--- a/vision/include/ftl/algorithms/rtcensus.hpp
+++ b/components/rgbd-sources/src/algorithms/rtcensus.hpp
@@ -7,7 +7,7 @@
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
#include <nlohmann/json.hpp>
#include <ftl/config.h>
diff --git a/vision/src/algorithms/rtcensus_sgm.cpp b/components/rgbd-sources/src/algorithms/rtcensus_sgm.cpp
similarity index 98%
rename from vision/src/algorithms/rtcensus_sgm.cpp
rename to components/rgbd-sources/src/algorithms/rtcensus_sgm.cpp
index d6b93a08b397637a4871f029b1a8041ca1085517..26c673a03a971b82d489a9a4e56830439b690386 100644
--- a/vision/src/algorithms/rtcensus_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/rtcensus_sgm.cpp
@@ -13,7 +13,7 @@
*/
-#include <ftl/algorithms/rtcensus_sgm.hpp>
+#include "rtcensus_sgm.hpp"
#include <vector>
#include <tuple>
#include <bitset>
diff --git a/vision/src/algorithms/rtcensus_sgm.cu b/components/rgbd-sources/src/algorithms/rtcensus_sgm.cu
similarity index 99%
rename from vision/src/algorithms/rtcensus_sgm.cu
rename to components/rgbd-sources/src/algorithms/rtcensus_sgm.cu
index 94b36ed1ce9ae91fa06f94d14e0ad2b7bea0dd21..d59a77bc0b8fe3f0538f8974350144bf1c3b386e 100644
--- a/vision/src/algorithms/rtcensus_sgm.cu
+++ b/components/rgbd-sources/src/algorithms/rtcensus_sgm.cu
@@ -14,7 +14,7 @@
*/
#include <ftl/cuda_common.hpp>
-#include <ftl/cuda_algorithms.hpp>
+#include "../cuda_algorithms.hpp"
using namespace cv::cuda;
using namespace cv;
diff --git a/vision/include/ftl/algorithms/rtcensus_sgm.hpp b/components/rgbd-sources/src/algorithms/rtcensus_sgm.hpp
similarity index 97%
rename from vision/include/ftl/algorithms/rtcensus_sgm.hpp
rename to components/rgbd-sources/src/algorithms/rtcensus_sgm.hpp
index e2da5d8d5d2c357054c3b7a2a9654e72511c72d0..c62c21ca8549333c53f836b4090b3b736b125cc3 100644
--- a/vision/include/ftl/algorithms/rtcensus_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/rtcensus_sgm.hpp
@@ -7,7 +7,7 @@
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
-#include <ftl/disparity.hpp>
+#include "../disparity.hpp"
#include <nlohmann/json.hpp>
#if defined HAVE_CUDA
diff --git a/vision/src/algorithms/sparse_census.cu b/components/rgbd-sources/src/algorithms/sparse_census.cu
similarity index 100%
rename from vision/src/algorithms/sparse_census.cu
rename to components/rgbd-sources/src/algorithms/sparse_census.cu
diff --git a/vision/src/algorithms/tex_filter.cu b/components/rgbd-sources/src/algorithms/tex_filter.cu
similarity index 100%
rename from vision/src/algorithms/tex_filter.cu
rename to components/rgbd-sources/src/algorithms/tex_filter.cu
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index becacbf57317bc96a19d2d4904f0107160ff3324..ea6cd7d276c9d43bf39fd0b360921d3fb0ff83dc 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -272,6 +272,11 @@ bool Calibrate::_loadCalibration() {
map1_[0], map2_[0]);
initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2,
map1_[1], map2_[1]);
+
+ // Re-distort
+ initUndistortRectifyMap(M1, Mat(), R1.t(), P1,
+ img_size, CV_16SC2, imap1_, imap2_);
+ r1_ = R1.t();
return true;
}
@@ -474,6 +479,11 @@ bool Calibrate::undistort(cv::Mat &l, cv::Mat &r) {
return true;
}
+void Calibrate::distort(cv::Mat &rgb, cv::Mat &depth) {
+ remap(rgb, rgb, imap1_, imap2_, INTER_LINEAR);
+ remap(depth, depth, imap1_, imap2_, INTER_LINEAR);
+}
+
bool Calibrate::rectified(cv::Mat &l, cv::Mat &r) {
return undistort(l, r);
}
diff --git a/vision/include/ftl/calibrate.hpp b/components/rgbd-sources/src/calibrate.hpp
similarity index 97%
rename from vision/include/ftl/calibrate.hpp
rename to components/rgbd-sources/src/calibrate.hpp
index 83a2bd9c139bee3a1920545ff0c9244bae43a2bc..78e8fde6af0dc28d327f0034e5eb915f1bdad0fc 100644
--- a/vision/include/ftl/calibrate.hpp
+++ b/components/rgbd-sources/src/calibrate.hpp
@@ -6,7 +6,7 @@
#define _FTL_CALIBRATION_HPP_
#include <opencv2/opencv.hpp>
-#include <ftl/local.hpp>
+#include "local.hpp"
#include <string>
#include <vector>
#include <nlohmann/json.hpp>
@@ -92,6 +92,8 @@ class Calibrate {
bool recalibrate();
bool undistort(cv::Mat &l, cv::Mat &r);
+
+ void distort(cv::Mat &rgb, cv::Mat &depth);
/**
* Get both left and right images from local source, but with intrinsic
@@ -119,6 +121,9 @@ class Calibrate {
bool calibrated_;
std::vector<cv::Mat> map1_;
std::vector<cv::Mat> map2_;
+ cv::Mat imap1_;
+ cv::Mat imap2_;
+ cv::Mat r1_;
cv::Mat Q_;
};
};
diff --git a/vision/include/ftl/cuda_algorithms.hpp b/components/rgbd-sources/src/cuda_algorithms.hpp
similarity index 100%
rename from vision/include/ftl/cuda_algorithms.hpp
rename to components/rgbd-sources/src/cuda_algorithms.hpp
diff --git a/vision/src/disparity.cpp b/components/rgbd-sources/src/disparity.cpp
similarity index 55%
rename from vision/src/disparity.cpp
rename to components/rgbd-sources/src/disparity.cpp
index a29d6dc3c4987da4f9c2bf819b17f763b71846b1..76558a133eef9388d48a6e08643e9130a30e2deb 100644
--- a/vision/src/disparity.cpp
+++ b/components/rgbd-sources/src/disparity.cpp
@@ -2,7 +2,9 @@
* Copyright 2019 Nicolas Pope
*/
-#include <ftl/disparity.hpp>
+#include "disparity.hpp"
+#include <glog/logging.h>
+#include <ftl/config.h>
using ftl::Disparity;
@@ -21,6 +23,17 @@ Disparity *Disparity::create(nlohmann::json &config) {
void Disparity::_register(const std::string &n,
std::function<Disparity*(nlohmann::json&)> f) {
+ LOG(INFO) << "Register disparity algorithm: " << n;
algorithms__[n] = f;
}
+// TODO(Nick) Add remaining algorithms
+
+#include "algorithms/rtcensus.hpp"
+static ftl::Disparity::Register rtcensus("rtcensus", ftl::algorithms::RTCensus::create);
+
+#ifdef HAVE_LIBSGM
+#include "algorithms/fixstars_sgm.hpp"
+static ftl::Disparity::Register fixstarssgm("libsgm", ftl::algorithms::FixstarsSGM::create);
+#endif // HAVE_LIBSGM
+
diff --git a/vision/include/ftl/disparity.hpp b/components/rgbd-sources/src/disparity.hpp
similarity index 100%
rename from vision/include/ftl/disparity.hpp
rename to components/rgbd-sources/src/disparity.hpp
diff --git a/vision/src/local.cpp b/components/rgbd-sources/src/local.cpp
similarity index 82%
rename from vision/src/local.cpp
rename to components/rgbd-sources/src/local.cpp
index f26bea1d31c571dbb9b3fc133add138e0e59df3e..6faa4649bff2e49368ea508a77c2e212e49f866e 100644
--- a/vision/src/local.cpp
+++ b/components/rgbd-sources/src/local.cpp
@@ -8,7 +8,7 @@
#include <chrono>
#include <thread>
-#include <ftl/local.hpp>
+#include "local.hpp"
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
@@ -24,19 +24,28 @@ using std::chrono::milliseconds;
using std::this_thread::sleep_for;
LocalSource::LocalSource(nlohmann::json &config)
- : timestamp_(0.0),
- flip_(config["flip"]),
- flip_v_(config["flip_vert"]),
- nostereo_(config["nostereo"]),
- downsize_(config.value("scale", 1.0f)) {
+ : Configurable(config), timestamp_(0.0) {
+
+ REQUIRED({
+ {"flip","Switch left and right views","boolean"},
+ {"flip_vert","Rotate image 180 degrees","boolean"},
+ {"nostereo","Force single camera mode","boolean"},
+ {"width","Pixel width of camera source","number"},
+ {"height","Pixel height of camera source","number"},
+ {"max_fps","Maximum frames per second","number"},
+ {"scale","Change the input image or video scaling","number"}
+ });
+
+ flip_ = config["flip"].get<bool>();
+ flip_v_ = config["flip_vert"].get<bool>();
+ nostereo_ = config["nostereo"].get<bool>();
+ downsize_ = config["scale"].get<float>();
// Use cameras
camera_a_ = new VideoCapture;
LOG(INFO) << "Cameras check... ";
camera_a_->open((flip_) ? 1 : 0);
- LOG(INFO) << "Cameras open? " << flip_;
-
if (!nostereo_) {
camera_b_ = new VideoCapture((flip_) ? 0 : 1);
} else {
@@ -74,11 +83,23 @@ LocalSource::LocalSource(nlohmann::json &config)
}
LocalSource::LocalSource(const string &vid, nlohmann::json &config)
- : timestamp_(0.0),
- flip_(config["flip"]),
- flip_v_(config["flip_vert"]),
- nostereo_(config["nostereo"]),
- downsize_(config.value("scale", 1.0f)) {
+ : Configurable(config), timestamp_(0.0) {
+
+ REQUIRED({
+ {"flip","Switch left and right views","boolean"},
+ {"flip_vert","Rotate image 180 degrees","boolean"},
+ {"nostereo","Force single camera mode","boolean"},
+ {"width","Pixel width of camera source","number"},
+ {"height","Pixel height of camera source","number"},
+ {"max_fps","Maximum frames per second","number"},
+ {"scale","Change the input image or video scaling","number"}
+ });
+
+ flip_ = config["flip"].get<bool>();
+ flip_v_ = config["flip_vert"].get<bool>();
+ nostereo_ = config["nostereo"].get<bool>();
+ downsize_ = config["scale"].get<float>();
+
if (vid == "") {
LOG(FATAL) << "No video file specified";
camera_a_ = nullptr;
diff --git a/vision/include/ftl/local.hpp b/components/rgbd-sources/src/local.hpp
similarity index 90%
rename from vision/include/ftl/local.hpp
rename to components/rgbd-sources/src/local.hpp
index 9a4204e65bab87eb71af8768decb12d116116b20..ad79b9bf68dd4d5f5ae5589b2b9f6eed76b6a08a 100644
--- a/vision/include/ftl/local.hpp
+++ b/components/rgbd-sources/src/local.hpp
@@ -1,6 +1,7 @@
#ifndef _FTL_LOCAL_HPP_
#define _FTL_LOCAL_HPP_
+#include <ftl/configurable.hpp>
#include <string>
#include <nlohmann/json.hpp>
@@ -10,7 +11,7 @@ namespace cv {
};
namespace ftl {
-class LocalSource {
+class LocalSource : public Configurable {
public:
explicit LocalSource(nlohmann::json &config);
LocalSource(const std::string &vid, nlohmann::json &config);
diff --git a/components/rgbd-sources/src/net_source.cpp b/components/rgbd-sources/src/net_source.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1a8b8517703ca61f325e0e35a53fe977a3f1766d
--- /dev/null
+++ b/components/rgbd-sources/src/net_source.cpp
@@ -0,0 +1,69 @@
+#include <ftl/net_source.hpp>
+#include <vector>
+#include <thread>
+#include <chrono>
+
+using ftl::rgbd::NetSource;
+using ftl::net::Universe;
+using std::string;
+using ftl::rgbd::CameraParameters;
+using std::mutex;
+using std::unique_lock;
+using std::vector;
+using std::this_thread::sleep_for;
+using std::chrono::milliseconds;
+
+static bool getCalibration(Universe &net, string src, ftl::rgbd::CameraParameters &p) {
+ while(true) {
+ auto buf = net.findOne<vector<unsigned char>>((string) src +"/calibration");
+ if (buf) {
+ memcpy((char*)&p, (*buf).data(), (*buf).size());
+
+ if (sizeof(p) != (*buf).size()) {
+ LOG(ERROR) << "Corrupted calibration";
+ return false;
+ }
+
+ return true;
+ } else {
+ LOG(INFO) << "Could not get calibration, retrying";
+ sleep_for(milliseconds(500));
+ }
+ }
+}
+
+NetSource::NetSource(nlohmann::json &config) : RGBDSource(config) {
+
+}
+
+NetSource::NetSource(nlohmann::json &config, ftl::net::Universe *net)
+ : RGBDSource(config, net) {
+
+ has_calibration_ = getCalibration(*net, config["uri"].get<string>(), params_);
+
+ net->subscribe(config["uri"].get<string>(), [this](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
+ unique_lock<mutex> lk(m_);
+ cv::imdecode(jpg, cv::IMREAD_COLOR, &rgb_);
+ //Mat(rgb_.size(), CV_16UC1);
+ cv::imdecode(d, cv::IMREAD_UNCHANGED, &depth_);
+ depth_.convertTo(depth_, CV_32FC1, 1.0f/(16.0f*100.0f));
+ });
+}
+
+NetSource::~NetSource() {
+ // TODO Unsubscribe
+}
+
+void NetSource::grab() {
+ // net_.broadcast("grab");
+}
+
+bool NetSource::isReady() {
+ return has_calibration_ && !rgb_.empty();
+}
+
+void NetSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
+ unique_lock<mutex> lk(m_);
+ rgb_.copyTo(rgb);
+ depth_.copyTo(depth);
+}
diff --git a/components/rgbd-sources/src/rgbd_source.cpp b/components/rgbd-sources/src/rgbd_source.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5423600283ab201f3edeb6092f58d32305aa45e3
--- /dev/null
+++ b/components/rgbd-sources/src/rgbd_source.cpp
@@ -0,0 +1,52 @@
+#include <ftl/rgbd_source.hpp>
+
+using ftl::rgbd::RGBDSource;
+using ftl::Configurable;
+
+std::map<std::string, std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)>>
+ RGBDSource::sources__;
+
+RGBDSource::RGBDSource(nlohmann::json &config) : Configurable(config), net_(nullptr) {
+
+}
+
+RGBDSource::RGBDSource(nlohmann::json &config, ftl::net::Universe *net) : Configurable(config), net_(net) {
+
+}
+
+RGBDSource::~RGBDSource() {
+
+}
+
+bool RGBDSource::isReady() {
+ return false;
+}
+
+bool RGBDSource::snapshot(const std::string &fileprefix) {
+ cv::Mat rgb;
+ cv::Mat depth;
+ getRGBD(rgb, depth);
+
+ cv::Mat d2;
+ depth.convertTo(d2, CV_16UC1, 16*100);
+
+ cv::imwrite(fileprefix+"-RGB.jpg", rgb);
+ cv::imwrite(fileprefix+"-DEPTH.png",depth);
+}
+
+RGBDSource *RGBDSource::create(nlohmann::json &config, ftl::net::Universe *net) {
+ if (config["type"].type_name() != "string") return nullptr;
+ if (sources__.count(config["type"]) != 1) return nullptr;
+ return sources__[config["type"]](config, net);
+}
+
+void RGBDSource::_register(const std::string &n,
+ std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)> f) {
+ LOG(INFO) << "Register RGB-D Source: " << n;
+ sources__[n] = f;
+}
+
+#include <ftl/net_source.hpp>
+static RGBDSource::Register netsource("net", ftl::rgbd::NetSource::create);
+#include <ftl/stereovideo_source.hpp>
+static RGBDSource::Register svsource("stereovideo", ftl::rgbd::StereoVideoSource::create);
diff --git a/components/rgbd-sources/src/stereovideo_source.cpp b/components/rgbd-sources/src/stereovideo_source.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..457b7727c35bb98ec8655c18a8fa2185bf9c5259
--- /dev/null
+++ b/components/rgbd-sources/src/stereovideo_source.cpp
@@ -0,0 +1,110 @@
+#include <glog/logging.h>
+#include <ftl/stereovideo_source.hpp>
+#include <ftl/configuration.hpp>
+#include "calibrate.hpp"
+#include "local.hpp"
+#include "disparity.hpp"
+
+using ftl::Calibrate;
+using ftl::LocalSource;
+using ftl::rgbd::StereoVideoSource;
+using std::string;
+
+StereoVideoSource::StereoVideoSource(nlohmann::json &config, ftl::net::Universe *net)
+ : RGBDSource(config, net) {
+
+}
+
+StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file)
+ : RGBDSource(config), ready_(false) {
+
+ REQUIRED({
+ {"source","Details on source video [object]","object"}
+ });
+
+ if (ftl::is_video(file)) {
+ // Load video file
+ LOG(INFO) << "Using video file...";
+ lsrc_ = new LocalSource(file, config["source"]);
+ } else if (file != "") {
+ auto vid = ftl::locateFile("video.mp4");
+ if (!vid) {
+ LOG(FATAL) << "No video.mp4 file found in provided paths";
+ } else {
+ LOG(INFO) << "Using test directory...";
+ lsrc_ = new LocalSource(*vid, config["source"]);
+ }
+ } else {
+ // Use cameras
+ LOG(INFO) << "Using cameras...";
+ lsrc_ = new LocalSource(config["source"]);
+ }
+
+ calib_ = new Calibrate(lsrc_, config["calibration"]);
+ if (config["calibrate"]) calib_->recalibrate();
+ if (!calib_->isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
+ else LOG(INFO) << "Calibration initiated.";
+
+ // Generate camera parameters from Q matrix
+ cv::Mat q = calib_->getQ();
+ params_ = {
+ q.at<double>(2,3), // F
+ q.at<double>(0,3), // Cx
+ q.at<double>(1,3), // Cy
+ (unsigned int)left_.cols, // TODO (Nick)
+ (unsigned int)left_.rows,
+ 0.0f, // 0m min
+ 15.0f // 15m max
+ };
+
+ disp_ = Disparity::create(config["disparity"]);
+ if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << config["disparity"];
+
+ LOG(INFO) << "StereoVideo source ready...";
+ ready_ = true;
+}
+
+StereoVideoSource::~StereoVideoSource() {
+ delete disp_;
+ delete calib_;
+ delete lsrc_;
+}
+
+void StereoVideoSource::grab() {
+ calib_->rectified(left_, right_);
+}
+
+bool StereoVideoSource::isReady() {
+ return ready_;
+}
+
+static void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv::Mat &q) {
+ cv::Matx44d _Q;
+ q.convertTo(_Q, CV_64F);
+
+ if (depth.empty()) depth = cv::Mat(disparity.size(), CV_32F);
+
+ for( int y = 0; y < disparity.rows; y++ ) {
+ const float *sptr = disparity.ptr<float>(y);
+ float *dptr = depth.ptr<float>(y);
+
+ for( int x = 0; x < disparity.cols; x++ ) {
+ double d = sptr[x];
+ cv::Vec4d homg_pt = _Q*cv::Vec4d(x, y, d, 1.0);
+ //dptr[x] = Vec3d(homg_pt.val);
+ //dptr[x] /= homg_pt[3];
+ dptr[x] = (homg_pt[2] / homg_pt[3]) / 1000.0f; // Depth in meters
+
+ if( fabs(d) <= FLT_EPSILON )
+ dptr[x] = 1000.0f;
+ }
+ }
+}
+
+void StereoVideoSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
+ cv::Mat disp;
+ disp_->compute(left_, right_, disp);
+ rgb = left_;
+ disparityToDepth(disp, depth, calib_->getQ());
+ calib_->distort(rgb,depth);
+}
diff --git a/common/config/config.json b/config/config.json
similarity index 93%
rename from common/config/config.json
rename to config/config.json
index 186f3821754d22a57367c832415d18263810d622..215a704c3c3bc0eb282df45d22a79dd3defb7099 100644
--- a/common/config/config.json
+++ b/config/config.json
@@ -1,5 +1,6 @@
{
"vision": {
+ "type": "stereovideo",
"middlebury": {
"dataset": "",
"threshold": 10.0,
@@ -69,11 +70,11 @@
"name": "dummy"
}
},
- "representation": {
+ "reconstruction": {
"net": {
"peers": ["tcp://localhost:9001"]
},
- "sources": ["ftl://utu.fi/node1/rgb-d"],
+ "sources": [{"type": "net", "uri": "ftl://utu.fi/node1/rgb-d"}],
"display": {
"flip_vert": false,
"disparity": false,
diff --git a/config/config_localhost.json b/config/config_localhost.json
new file mode 100644
index 0000000000000000000000000000000000000000..b67fe65b07f413ac1529609d57c9d000ba8076bf
--- /dev/null
+++ b/config/config_localhost.json
@@ -0,0 +1,97 @@
+{
+ "vision": {
+ "type": "stereovideo",
+ "middlebury": {
+ "dataset": "",
+ "threshold": 2.0,
+ "scale": 0.5
+ },
+ "source": {
+ "flip": false,
+ "nostereo": false,
+ "scale": 1.0,
+ "flip_vert": false,
+ "max_fps": 25,
+ "width": 640,
+ "height": 480,
+ "crosshair": false
+ },
+ "calibrate": false,
+ "calibration": {
+ "board_size": [9,6],
+ "square_size": 50,
+ "frame_delay": 1.0,
+ "num_frames": 35,
+ "assume_zero_tangential_distortion": true,
+ "fix_aspect_ratio": true,
+ "fix_principal_point_at_center": true,
+ "use_fisheye_model": false,
+ "fix_k1": false,
+ "fix_k2": false,
+ "fix_k3": false,
+ "fix_k4": true,
+ "fix_k5": true,
+ "save": true,
+ "use_intrinsics": true,
+ "use_extrinsics": true,
+ "flip_vertical": false
+ },
+ "camera": {
+ "name": "Panasonic Lumix DMC-FZ300",
+ "focal_length": 25,
+ "sensor_width": 6.17,
+ "base_line": 0.1
+ },
+ "disparity": {
+ "algorithm": "libsgm",
+ "use_cuda": true,
+ "minimum": 0,
+ "maximum": 256,
+ "tau": 0.0,
+ "gamma": 0.0,
+ "window_size": 5,
+ "sigma": 1.5,
+ "lambda": 8000.0,
+ "filter": true
+ },
+ "display": {
+ "flip_vert": false,
+ "disparity": false,
+ "points": false,
+ "depth": false,
+ "left": false,
+ "right": false
+ },
+ "net": {
+ "listen": "tcp://*:9001",
+ "peers": []
+ },
+ "stream": {
+ "name": "dummy"
+ }
+ },
+ "reconstruction": {
+ "net": {
+ "peers": ["tcp://localhost:9001"]
+ },
+ "sources": [{"type": "net", "uri": "ftl://utu.fi/dummy/rgb-d"}],
+ "display": {
+ "flip_vert": false,
+ "disparity": false,
+ "points": true,
+ "depth": false,
+ "left": false,
+ "right": false
+ },
+ "registration": {
+ "reference-source" : "ftl://utu.fi/dummy/rgb-d",
+ "calibration" : {
+ "run": false,
+ "iterations" : 10,
+ "delay" : 1000,
+ "patternsize" : [9, 6]
+ }
+ }
+ }
+}
+
diff --git a/vision/CMakeLists.txt b/vision/CMakeLists.txt
deleted file mode 100644
index 5320fd283d02d6468da3fb2d2192b9819b52a8e6..0000000000000000000000000000000000000000
--- a/vision/CMakeLists.txt
+++ /dev/null
@@ -1,52 +0,0 @@
-# Need to include staged files and libs
-include_directories(${PROJECT_SOURCE_DIR}/vision/include)
-#include_directories(${PROJECT_BINARY_DIR})
-
-add_subdirectory(lib)
-
-set(CVNODESRC
- src/main.cpp
- src/calibrate.cpp
- src/local.cpp
- src/sync.cpp
- src/disparity.cpp
- src/streamer.cpp
- src/middlebury.cpp
- src/algorithms/rtcensus.cpp
- src/algorithms/rtcensus_sgm.cpp
- src/algorithms/opencv_sgbm.cpp
- src/algorithms/opencv_bm.cpp
- src/algorithms/elas.cpp
-)
-
-if (LIBSGM_FOUND)
- list(APPEND CVNODESRC "src/algorithms/fixstars_sgm.cpp")
-endif (LIBSGM_FOUND)
-
-if (CUDA_FOUND)
- list(APPEND CVNODESRC
- "src/algorithms/opencv_cuda_bm.cpp"
- "src/algorithms/opencv_cuda_bp.cpp"
- "src/algorithms/rtcensus.cu"
- "src/algorithms/rtcensus_sgm.cu"
- "src/algorithms/consistency.cu"
- "src/algorithms/sparse_census.cu"
- "src/algorithms/tex_filter.cu"
- "src/algorithms/nick1.cu"
- "src/algorithms/nick.cpp")
-endif (CUDA_FOUND)
-
-add_executable(ftl-vision ${CVNODESRC})
-add_dependencies(ftl-vision ftlcommon)
-add_dependencies(ftl-vision ftlnet)
-add_dependencies(ftl-vision ftlrender)
-add_dependencies(ftl-vision libelas)
-
-if (CUDA_FOUND)
-set_property(TARGET ftl-vision PROPERTY CUDA_SEPARABLE_COMPILATION ON)
-endif()
-
-#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(ftl-vision ftlcommon ftlrender Threads::Threads libelas ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} glog::glog ftlnet)
-
-
diff --git a/vision/include/ftl/algorithms/np_attention.hpp b/vision/include/ftl/algorithms/np_attention.hpp
deleted file mode 100644
index 0b5111b3b072ce167137bea473cb76658c8953d2..0000000000000000000000000000000000000000
--- a/vision/include/ftl/algorithms/np_attention.hpp
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef _FTL_ALGORITHMS_NP_ATTENTION_HPP_
-#define _FTL_ALGORITHMS_NP_ATTENTION_HPP_
-
-namespace ftl {
-namespace gpu {
-
-struct AttentionItem {
- float factor;
- HisteresisTexture<float> source;
-}
-
-class Attention {
- public:
- Attention();
-
- void source(float factor, const HisteresisTexture<float> &ht);
-
- void update();
-
- cudaTextureObject_t cudaTexture();
- TextureObject<float> texture();
-
- private:
- std::vector<Component> components_;
-};
-
-}
-}
-
-#endif // _FTL_ALGORITHMS_NP_ATTENTION_HPP_
-
diff --git a/vision/lib/CMakeLists.txt b/vision/lib/CMakeLists.txt
deleted file mode 100644
index 6525db95dddf47fafa28434674119602f8f933ff..0000000000000000000000000000000000000000
--- a/vision/lib/CMakeLists.txt
+++ /dev/null
@@ -1,13 +0,0 @@
-
-### Lib ELAS ###################################################################
-
-# directories
-set (LIBELAS_SRC_DIR elas)
-# sources
-FILE(GLOB LIBELAS_SRC_FILES "elas/*.cpp")
-
-add_library(libelas ${LIBELAS_SRC_FILES})
-target_include_directories(libelas
- PRIVATE elas)
-
-
diff --git a/vision/lib/elas/descriptor.cpp b/vision/lib/elas/descriptor.cpp
deleted file mode 100644
index 6420afd43f10f217205232ec0653a0a903686e1a..0000000000000000000000000000000000000000
--- a/vision/lib/elas/descriptor.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
-Copyright 2011. All rights reserved.
-Institute of Measurement and Control Systems
-Karlsruhe Institute of Technology, Germany
-
-This file is part of libelas.
-Authors: Andreas Geiger
-
-libelas 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 any later version.
-
-libelas 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.
-
-You should have received a copy of the GNU General Public License along with
-libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
-Street, Fifth Floor, Boston, MA 02110-1301, USA
-*/
-
-#include "descriptor.h"
-#include "filter.h"
-#include <emmintrin.h>
-
-using namespace std;
-
-Descriptor::Descriptor(uint8_t* I,int32_t width,int32_t height,int32_t bpl,bool half_resolution) {
- I_desc = (uint8_t*)_mm_malloc(16*width*height*sizeof(uint8_t),16);
- uint8_t* I_du = (uint8_t*)_mm_malloc(bpl*height*sizeof(uint8_t),16);
- uint8_t* I_dv = (uint8_t*)_mm_malloc(bpl*height*sizeof(uint8_t),16);
- filter::sobel3x3(I,I_du,I_dv,bpl,height);
- createDescriptor(I_du,I_dv,width,height,bpl,half_resolution);
- _mm_free(I_du);
- _mm_free(I_dv);
-}
-
-Descriptor::~Descriptor() {
- _mm_free(I_desc);
-}
-
-void Descriptor::createDescriptor (uint8_t* I_du,uint8_t* I_dv,int32_t width,int32_t height,int32_t bpl,bool half_resolution) {
-
- uint8_t *I_desc_curr;
- uint32_t addr_v0,addr_v1,addr_v2,addr_v3,addr_v4;
-
- // do not compute every second line
- if (half_resolution) {
-
- // create filter strip
- for (int32_t v=4; v<height-3; v+=2) {
-
- addr_v2 = v*bpl;
- addr_v0 = addr_v2-2*bpl;
- addr_v1 = addr_v2-1*bpl;
- addr_v3 = addr_v2+1*bpl;
- addr_v4 = addr_v2+2*bpl;
-
- for (int32_t u=3; u<width-3; u++) {
- I_desc_curr = I_desc+(v*width+u)*16;
- *(I_desc_curr++) = *(I_du+addr_v0+u+0);
- *(I_desc_curr++) = *(I_du+addr_v1+u-2);
- *(I_desc_curr++) = *(I_du+addr_v1+u+0);
- *(I_desc_curr++) = *(I_du+addr_v1+u+2);
- *(I_desc_curr++) = *(I_du+addr_v2+u-1);
- *(I_desc_curr++) = *(I_du+addr_v2+u+0);
- *(I_desc_curr++) = *(I_du+addr_v2+u+0);
- *(I_desc_curr++) = *(I_du+addr_v2+u+1);
- *(I_desc_curr++) = *(I_du+addr_v3+u-2);
- *(I_desc_curr++) = *(I_du+addr_v3+u+0);
- *(I_desc_curr++) = *(I_du+addr_v3+u+2);
- *(I_desc_curr++) = *(I_du+addr_v4+u+0);
- *(I_desc_curr++) = *(I_dv+addr_v1+u+0);
- *(I_desc_curr++) = *(I_dv+addr_v2+u-1);
- *(I_desc_curr++) = *(I_dv+addr_v2+u+1);
- *(I_desc_curr++) = *(I_dv+addr_v3+u+0);
- }
- }
-
- // compute full descriptor images
- } else {
-
- // create filter strip
- for (int32_t v=3; v<height-3; v++) {
-
- addr_v2 = v*bpl;
- addr_v0 = addr_v2-2*bpl;
- addr_v1 = addr_v2-1*bpl;
- addr_v3 = addr_v2+1*bpl;
- addr_v4 = addr_v2+2*bpl;
-
- for (int32_t u=3; u<width-3; u++) {
- I_desc_curr = I_desc+(v*width+u)*16;
- *(I_desc_curr++) = *(I_du+addr_v0+u+0);
- *(I_desc_curr++) = *(I_du+addr_v1+u-2);
- *(I_desc_curr++) = *(I_du+addr_v1+u+0);
- *(I_desc_curr++) = *(I_du+addr_v1+u+2);
- *(I_desc_curr++) = *(I_du+addr_v2+u-1);
- *(I_desc_curr++) = *(I_du+addr_v2+u+0);
- *(I_desc_curr++) = *(I_du+addr_v2+u+0);
- *(I_desc_curr++) = *(I_du+addr_v2+u+1);
- *(I_desc_curr++) = *(I_du+addr_v3+u-2);
- *(I_desc_curr++) = *(I_du+addr_v3+u+0);
- *(I_desc_curr++) = *(I_du+addr_v3+u+2);
- *(I_desc_curr++) = *(I_du+addr_v4+u+0);
- *(I_desc_curr++) = *(I_dv+addr_v1+u+0);
- *(I_desc_curr++) = *(I_dv+addr_v2+u-1);
- *(I_desc_curr++) = *(I_dv+addr_v2+u+1);
- *(I_desc_curr++) = *(I_dv+addr_v3+u+0);
- }
- }
- }
-
-}
diff --git a/vision/lib/elas/descriptor.h b/vision/lib/elas/descriptor.h
deleted file mode 100644
index fe2c0ab1203036b574b161f7c34e81d39d1651ee..0000000000000000000000000000000000000000
--- a/vision/lib/elas/descriptor.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
-Copyright 2011. All rights reserved.
-Institute of Measurement and Control Systems
-Karlsruhe Institute of Technology, Germany
-
-This file is part of libelas.
-Authors: Andreas Geiger
-
-libelas 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 any later version.
-
-libelas 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.
-
-You should have received a copy of the GNU General Public License along with
-libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
-Street, Fifth Floor, Boston, MA 02110-1301, USA
-*/
-
-// NOTE: This descripter is a sparse approximation to the 50-dimensional
-// descriptor described in the paper. It produces similar results, but
-// is faster to compute.
-
-#ifndef __DESCRIPTOR_H__
-#define __DESCRIPTOR_H__
-
-#include <iostream>
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <math.h>
-
-// Define fixed-width datatypes for Visual Studio projects
-#ifndef _MSC_VER
- #include <stdint.h>
-#else
-// typedef __int8 int8_t;
-// typedef __int16 int16_t;
-// typedef __int32 int32_t;
-// typedef __int64 int64_t;
-// typedef unsigned __int8 uint8_t;
-// typedef unsigned __int16 uint16_t;
-// typedef unsigned __int32 uint32_t;
-// typedef unsigned __int64 uint64_t;
-#endif
-
-class Descriptor {
-
-public:
-
- // constructor creates filters
- Descriptor(uint8_t* I,int32_t width,int32_t height,int32_t bpl,bool half_resolution);
-
- // deconstructor releases memory
- ~Descriptor();
-
- // descriptors accessible from outside
- uint8_t* I_desc;
-
-private:
-
- // build descriptor I_desc from I_du and I_dv
- void createDescriptor(uint8_t* I_du,uint8_t* I_dv,int32_t width,int32_t height,int32_t bpl,bool half_resolution);
-
-};
-
-#endif
diff --git a/vision/lib/elas/elas.cpp b/vision/lib/elas/elas.cpp
deleted file mode 100644
index bddf239f142929337c7ab69680585af0a3951c1a..0000000000000000000000000000000000000000
--- a/vision/lib/elas/elas.cpp
+++ /dev/null
@@ -1,1560 +0,0 @@
-/*
-Copyright 2011. All rights reserved.
-Institute of Measurement and Control Systems
-Karlsruhe Institute of Technology, Germany
-
-This file is part of libelas.
-Authors: Andreas Geiger
-
-libelas 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 any later version.
-
-libelas 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.
-
-You should have received a copy of the GNU General Public License along with
-libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
-Street, Fifth Floor, Boston, MA 02110-1301, USA
-*/
-
-#include "elas.h"
-
-#include <algorithm>
-#include <math.h>
-#include "descriptor.h"
-#include "triangle.h"
-#include "matrix.h"
-
-using namespace std;
-
-void Elas::process (uint8_t* I1_,uint8_t* I2_,float* D1,float* D2,const int32_t* dims){
-
- // get width, height and bytes per line
- width = dims[0];
- height = dims[1];
- bpl = width + 15-(width-1)%16;
-
- // copy images to byte aligned memory
- I1 = (uint8_t*)_mm_malloc(bpl*height*sizeof(uint8_t),16);
- I2 = (uint8_t*)_mm_malloc(bpl*height*sizeof(uint8_t),16);
- memset (I1,0,bpl*height*sizeof(uint8_t));
- memset (I2,0,bpl*height*sizeof(uint8_t));
- if (bpl==dims[2]) {
- memcpy(I1,I1_,bpl*height*sizeof(uint8_t));
- memcpy(I2,I2_,bpl*height*sizeof(uint8_t));
- } else {
- for (int32_t v=0; v<height; v++) {
- memcpy(I1+v*bpl,I1_+v*dims[2],width*sizeof(uint8_t));
- memcpy(I2+v*bpl,I2_+v*dims[2],width*sizeof(uint8_t));
- }
- }
-
-#ifdef PROFILE
- timer.start("Descriptor");
-#endif
- Descriptor desc1(I1,width,height,bpl,param.subsampling);
- Descriptor desc2(I2,width,height,bpl,param.subsampling);
-
-#ifdef PROFILE
- timer.start("Support Matches");
-#endif
- vector<support_pt> p_support = computeSupportMatches(desc1.I_desc,desc2.I_desc);
-
- // if not enough support points for triangulation
- if (p_support.size()<3) {
- cout << "ERROR: Need at least 3 support points!" << endl;
- _mm_free(I1);
- _mm_free(I2);
- return;
- }
-
-#ifdef PROFILE
- timer.start("Delaunay Triangulation");
-#endif
- vector<triangle> tri_1 = computeDelaunayTriangulation(p_support,0);
- vector<triangle> tri_2 = computeDelaunayTriangulation(p_support,1);
-
-#ifdef PROFILE
- timer.start("Disparity Planes");
-#endif
- computeDisparityPlanes(p_support,tri_1,0);
- computeDisparityPlanes(p_support,tri_2,1);
-
-#ifdef PROFILE
- timer.start("Grid");
-#endif
-
- // allocate memory for disparity grid
- int32_t grid_width = (int32_t)ceil((float)width/(float)param.grid_size);
- int32_t grid_height = (int32_t)ceil((float)height/(float)param.grid_size);
- int32_t grid_dims[3] = {param.disp_max+2,grid_width,grid_height};
- int32_t* disparity_grid_1 = (int32_t*)calloc((param.disp_max+2)*grid_height*grid_width,sizeof(int32_t));
- int32_t* disparity_grid_2 = (int32_t*)calloc((param.disp_max+2)*grid_height*grid_width,sizeof(int32_t));
-
- createGrid(p_support,disparity_grid_1,grid_dims,0);
- createGrid(p_support,disparity_grid_2,grid_dims,1);
-
-#ifdef PROFILE
- timer.start("Matching");
-#endif
- computeDisparity(p_support,tri_1,disparity_grid_1,grid_dims,desc1.I_desc,desc2.I_desc,0,D1);
- computeDisparity(p_support,tri_2,disparity_grid_2,grid_dims,desc1.I_desc,desc2.I_desc,1,D2);
-
-#ifdef PROFILE
- timer.start("L/R Consistency Check");
-#endif
- leftRightConsistencyCheck(D1,D2);
-
-#ifdef PROFILE
- timer.start("Remove Small Segments");
-#endif
- removeSmallSegments(D1);
- if (!param.postprocess_only_left)
- removeSmallSegments(D2);
-
-#ifdef PROFILE
- timer.start("Gap Interpolation");
-#endif
- gapInterpolation(D1);
- if (!param.postprocess_only_left)
- gapInterpolation(D2);
-
- if (param.filter_adaptive_mean) {
-#ifdef PROFILE
- timer.start("Adaptive Mean");
-#endif
- adaptiveMean(D1);
- if (!param.postprocess_only_left)
- adaptiveMean(D2);
- }
-
- if (param.filter_median) {
-#ifdef PROFILE
- timer.start("Median");
-#endif
- median(D1);
- if (!param.postprocess_only_left)
- median(D2);
- }
-
-#ifdef PROFILE
- timer.plot();
-#endif
-
- // release memory
- free(disparity_grid_1);
- free(disparity_grid_2);
- _mm_free(I1);
- _mm_free(I2);
-}
-
-void Elas::removeInconsistentSupportPoints (int16_t* D_can,int32_t D_can_width,int32_t D_can_height) {
-
- // for all valid support points do
- for (int32_t u_can=0; u_can<D_can_width; u_can++) {
- for (int32_t v_can=0; v_can<D_can_height; v_can++) {
- int16_t d_can = *(D_can+getAddressOffsetImage(u_can,v_can,D_can_width));
- if (d_can>=0) {
-
- // compute number of other points supporting the current point
- int32_t support = 0;
- for (int32_t u_can_2=u_can-param.incon_window_size; u_can_2<=u_can+param.incon_window_size; u_can_2++) {
- for (int32_t v_can_2=v_can-param.incon_window_size; v_can_2<=v_can+param.incon_window_size; v_can_2++) {
- if (u_can_2>=0 && v_can_2>=0 && u_can_2<D_can_width && v_can_2<D_can_height) {
- int16_t d_can_2 = *(D_can+getAddressOffsetImage(u_can_2,v_can_2,D_can_width));
- if (d_can_2>=0 && abs(d_can-d_can_2)<=param.incon_threshold)
- support++;
- }
- }
- }
-
- // invalidate support point if number of supporting points is too low
- if (support<param.incon_min_support)
- *(D_can+getAddressOffsetImage(u_can,v_can,D_can_width)) = -1;
- }
- }
- }
-}
-
-void Elas::removeRedundantSupportPoints(int16_t* D_can,int32_t D_can_width,int32_t D_can_height,
- int32_t redun_max_dist, int32_t redun_threshold, bool vertical) {
-
- // parameters
- int32_t redun_dir_u[2] = {0,0};
- int32_t redun_dir_v[2] = {0,0};
- if (vertical) {
- redun_dir_v[0] = -1;
- redun_dir_v[1] = +1;
- } else {
- redun_dir_u[0] = -1;
- redun_dir_u[1] = +1;
- }
-
- // for all valid support points do
- for (int32_t u_can=0; u_can<D_can_width; u_can++) {
- for (int32_t v_can=0; v_can<D_can_height; v_can++) {
- int16_t d_can = *(D_can+getAddressOffsetImage(u_can,v_can,D_can_width));
- if (d_can>=0) {
-
- // check all directions for redundancy
- bool redundant = true;
- for (int32_t i=0; i<2; i++) {
-
- // search for support
- int32_t u_can_2 = u_can;
- int32_t v_can_2 = v_can;
- int16_t d_can_2;
- bool support = false;
- for (int32_t j=0; j<redun_max_dist; j++) {
- u_can_2 += redun_dir_u[i];
- v_can_2 += redun_dir_v[i];
- if (u_can_2<0 || v_can_2<0 || u_can_2>=D_can_width || v_can_2>=D_can_height)
- break;
- d_can_2 = *(D_can+getAddressOffsetImage(u_can_2,v_can_2,D_can_width));
- if (d_can_2>=0 && abs(d_can-d_can_2)<=redun_threshold) {
- support = true;
- break;
- }
- }
-
- // if we have no support => point is not redundant
- if (!support) {
- redundant = false;
- break;
- }
- }
-
- // invalidate support point if it is redundant
- if (redundant)
- *(D_can+getAddressOffsetImage(u_can,v_can,D_can_width)) = -1;
- }
- }
- }
-}
-
-void Elas::addCornerSupportPoints(vector<support_pt> &p_support) {
-
- // list of border points
- vector<support_pt> p_border;
- p_border.push_back(support_pt(0,0,0));
- p_border.push_back(support_pt(0,height-1,0));
- p_border.push_back(support_pt(width-1,0,0));
- p_border.push_back(support_pt(width-1,height-1,0));
-
- // find closest d
- for (int32_t i=0; i<p_border.size(); i++) {
- int32_t best_dist = 10000000;
- for (int32_t j=0; j<p_support.size(); j++) {
- int32_t du = p_border[i].u-p_support[j].u;
- int32_t dv = p_border[i].v-p_support[j].v;
- int32_t curr_dist = du*du+dv*dv;
- if (curr_dist<best_dist) {
- best_dist = curr_dist;
- p_border[i].d = p_support[j].d;
- }
- }
- }
-
- // for right image
- p_border.push_back(support_pt(p_border[2].u+p_border[2].d,p_border[2].v,p_border[2].d));
- p_border.push_back(support_pt(p_border[3].u+p_border[3].d,p_border[3].v,p_border[3].d));
-
- // add border points to support points
- for (int32_t i=0; i<p_border.size(); i++)
- p_support.push_back(p_border[i]);
-}
-
-inline int16_t Elas::computeMatchingDisparity (const int32_t &u,const int32_t &v,uint8_t* I1_desc,uint8_t* I2_desc,const bool &right_image) {
-
- const int32_t u_step = 2;
- const int32_t v_step = 2;
- const int32_t window_size = 3;
-
- int32_t desc_offset_1 = -16*u_step-16*width*v_step;
- int32_t desc_offset_2 = +16*u_step-16*width*v_step;
- int32_t desc_offset_3 = -16*u_step+16*width*v_step;
- int32_t desc_offset_4 = +16*u_step+16*width*v_step;
-
- __m128i xmm1,xmm2,xmm3,xmm4,xmm5,xmm6;
-
- // check if we are inside the image region
- if (u>=window_size+u_step && u<=width-window_size-1-u_step && v>=window_size+v_step && v<=height-window_size-1-v_step) {
-
- // compute desc and start addresses
- int32_t line_offset = 16*width*v;
- uint8_t *I1_line_addr,*I2_line_addr;
- if (!right_image) {
- I1_line_addr = I1_desc+line_offset;
- I2_line_addr = I2_desc+line_offset;
- } else {
- I1_line_addr = I2_desc+line_offset;
- I2_line_addr = I1_desc+line_offset;
- }
-
- // compute I1 block start addresses
- uint8_t* I1_block_addr = I1_line_addr+16*u;
- uint8_t* I2_block_addr;
-
- // we require at least some texture
- int32_t sum = 0;
- for (int32_t i=0; i<16; i++)
- sum += abs((int32_t)(*(I1_block_addr+i))-128);
- if (sum<param.support_texture)
- return -1;
-
- // load first blocks to xmm registers
- xmm1 = _mm_load_si128((__m128i*)(I1_block_addr+desc_offset_1));
- xmm2 = _mm_load_si128((__m128i*)(I1_block_addr+desc_offset_2));
- xmm3 = _mm_load_si128((__m128i*)(I1_block_addr+desc_offset_3));
- xmm4 = _mm_load_si128((__m128i*)(I1_block_addr+desc_offset_4));
-
- // declare match energy for each disparity
- int32_t u_warp;
-
- // best match
- int16_t min_1_E = 32767;
- int16_t min_1_d = -1;
- int16_t min_2_E = 32767;
- int16_t min_2_d = -1;
-
- // get valid disparity range
- int32_t disp_min_valid = max(param.disp_min,0);
- int32_t disp_max_valid = param.disp_max;
- if (!right_image) disp_max_valid = min(param.disp_max,u-window_size-u_step);
- else disp_max_valid = min(param.disp_max,width-u-window_size-u_step);
-
- // assume, that we can compute at least 10 disparities for this pixel
- if (disp_max_valid-disp_min_valid<10)
- return -1;
-
- // for all disparities do
- for (int16_t d=disp_min_valid; d<=disp_max_valid; d++) {
-
- // warp u coordinate
- if (!right_image) u_warp = u-d;
- else u_warp = u+d;
-
- // compute I2 block start addresses
- I2_block_addr = I2_line_addr+16*u_warp;
-
- // compute match energy at this disparity
- xmm6 = _mm_load_si128((__m128i*)(I2_block_addr+desc_offset_1));
- xmm6 = _mm_sad_epu8(xmm1,xmm6);
- xmm5 = _mm_load_si128((__m128i*)(I2_block_addr+desc_offset_2));
- xmm6 = _mm_add_epi16(_mm_sad_epu8(xmm2,xmm5),xmm6);
- xmm5 = _mm_load_si128((__m128i*)(I2_block_addr+desc_offset_3));
- xmm6 = _mm_add_epi16(_mm_sad_epu8(xmm3,xmm5),xmm6);
- xmm5 = _mm_load_si128((__m128i*)(I2_block_addr+desc_offset_4));
- xmm6 = _mm_add_epi16(_mm_sad_epu8(xmm4,xmm5),xmm6);
- sum = _mm_extract_epi16(xmm6,0)+_mm_extract_epi16(xmm6,4);
-
- // best + second best match
- if (sum<min_1_E) {
- min_2_E = min_1_E;
- min_2_d = min_1_d;
- min_1_E = sum;
- min_1_d = d;
- } else if (sum<min_2_E) {
- min_2_E = sum;
- min_2_d = d;
- }
- }
-
- // check if best and second best match are available and if matching ratio is sufficient
- if (min_1_d>=0 && min_2_d>=0 && (float)min_1_E<param.support_threshold*(float)min_2_E)
- return min_1_d;
- else
- return -1;
-
- } else
- return -1;
-}
-
-vector<Elas::support_pt> Elas::computeSupportMatches (uint8_t* I1_desc,uint8_t* I2_desc) {
-
- // be sure that at half resolution we only need data
- // from every second line!
- int32_t D_candidate_stepsize = param.candidate_stepsize;
- if (param.subsampling)
- D_candidate_stepsize += D_candidate_stepsize%2;
-
- // create matrix for saving disparity candidates
- int32_t D_can_width = 0;
- int32_t D_can_height = 0;
- for (int32_t u=0; u<width; u+=D_candidate_stepsize) D_can_width++;
- for (int32_t v=0; v<height; v+=D_candidate_stepsize) D_can_height++;
- int16_t* D_can = (int16_t*)calloc(D_can_width*D_can_height,sizeof(int16_t));
-
- // loop variables
- int32_t u,v;
- int16_t d,d2;
-
- // for all point candidates in image 1 do
- for (int32_t u_can=1; u_can<D_can_width; u_can++) {
- u = u_can*D_candidate_stepsize;
- for (int32_t v_can=1; v_can<D_can_height; v_can++) {
- v = v_can*D_candidate_stepsize;
-
- // initialize disparity candidate to invalid
- *(D_can+getAddressOffsetImage(u_can,v_can,D_can_width)) = -1;
-
- // find forwards
- d = computeMatchingDisparity(u,v,I1_desc,I2_desc,false);
- if (d>=0) {
-
- // find backwards
- d2 = computeMatchingDisparity(u-d,v,I1_desc,I2_desc,true);
- if (d2>=0 && abs(d-d2)<=param.lr_threshold)
- *(D_can+getAddressOffsetImage(u_can,v_can,D_can_width)) = d;
- }
- }
- }
-
- // remove inconsistent support points
- removeInconsistentSupportPoints(D_can,D_can_width,D_can_height);
-
- // remove support points on straight lines, since they are redundant
- // this reduces the number of triangles a little bit and hence speeds up
- // the triangulation process
- removeRedundantSupportPoints(D_can,D_can_width,D_can_height,5,1,true);
- removeRedundantSupportPoints(D_can,D_can_width,D_can_height,5,1,false);
-
- // move support points from image representation into a vector representation
- vector<support_pt> p_support;
- for (int32_t u_can=1; u_can<D_can_width; u_can++)
- for (int32_t v_can=1; v_can<D_can_height; v_can++)
- if (*(D_can+getAddressOffsetImage(u_can,v_can,D_can_width))>=0)
- p_support.push_back(support_pt(u_can*D_candidate_stepsize,
- v_can*D_candidate_stepsize,
- *(D_can+getAddressOffsetImage(u_can,v_can,D_can_width))));
-
- // if flag is set, add support points in image corners
- // with the same disparity as the nearest neighbor support point
- if (param.add_corners)
- addCornerSupportPoints(p_support);
-
- // free memory
- free(D_can);
-
- // return support point vector
- return p_support;
-}
-
-vector<Elas::triangle> Elas::computeDelaunayTriangulation (vector<support_pt> p_support,int32_t right_image) {
-
- // input/output structure for triangulation
- struct triangulateio in, out;
- int32_t k;
-
- // inputs
- in.numberofpoints = p_support.size();
- in.pointlist = (float*)malloc(in.numberofpoints*2*sizeof(float));
- k=0;
- if (!right_image) {
- for (int32_t i=0; i<p_support.size(); i++) {
- in.pointlist[k++] = p_support[i].u;
- in.pointlist[k++] = p_support[i].v;
- }
- } else {
- for (int32_t i=0; i<p_support.size(); i++) {
- in.pointlist[k++] = p_support[i].u-p_support[i].d;
- in.pointlist[k++] = p_support[i].v;
- }
- }
- in.numberofpointattributes = 0;
- in.pointattributelist = NULL;
- in.pointmarkerlist = NULL;
- in.numberofsegments = 0;
- in.numberofholes = 0;
- in.numberofregions = 0;
- in.regionlist = NULL;
-
- // outputs
- out.pointlist = NULL;
- out.pointattributelist = NULL;
- out.pointmarkerlist = NULL;
- out.trianglelist = NULL;
- out.triangleattributelist = NULL;
- out.neighborlist = NULL;
- out.segmentlist = NULL;
- out.segmentmarkerlist = NULL;
- out.edgelist = NULL;
- out.edgemarkerlist = NULL;
-
- // do triangulation (z=zero-based, n=neighbors, Q=quiet, B=no boundary markers)
- char parameters[] = "zQB";
- triangulate(parameters, &in, &out, NULL);
-
- // put resulting triangles into vector tri
- vector<triangle> tri;
- k=0;
- for (int32_t i=0; i<out.numberoftriangles; i++) {
- tri.push_back(triangle(out.trianglelist[k],out.trianglelist[k+1],out.trianglelist[k+2]));
- k+=3;
- }
-
- // free memory used for triangulation
- free(in.pointlist);
- free(out.pointlist);
- free(out.trianglelist);
-
- // return triangles
- return tri;
-}
-
-void Elas::computeDisparityPlanes (vector<support_pt> p_support,vector<triangle> &tri,int32_t right_image) {
-
- // init matrices
- Matrix A(3,3);
- Matrix b(3,1);
-
- // for all triangles do
- for (int32_t i=0; i<tri.size(); i++) {
-
- // get triangle corner indices
- int32_t c1 = tri[i].c1;
- int32_t c2 = tri[i].c2;
- int32_t c3 = tri[i].c3;
-
- // compute matrix A for linear system of left triangle
- A.val[0][0] = p_support[c1].u;
- A.val[1][0] = p_support[c2].u;
- A.val[2][0] = p_support[c3].u;
- A.val[0][1] = p_support[c1].v; A.val[0][2] = 1;
- A.val[1][1] = p_support[c2].v; A.val[1][2] = 1;
- A.val[2][1] = p_support[c3].v; A.val[2][2] = 1;
-
- // compute vector b for linear system (containing the disparities)
- b.val[0][0] = p_support[c1].d;
- b.val[1][0] = p_support[c2].d;
- b.val[2][0] = p_support[c3].d;
-
- // on success of gauss jordan elimination
- if (b.solve(A)) {
-
- // grab results from b
- tri[i].t1a = b.val[0][0];
- tri[i].t1b = b.val[1][0];
- tri[i].t1c = b.val[2][0];
-
- // otherwise: invalid
- } else {
- tri[i].t1a = 0;
- tri[i].t1b = 0;
- tri[i].t1c = 0;
- }
-
- // compute matrix A for linear system of right triangle
- A.val[0][0] = p_support[c1].u-p_support[c1].d;
- A.val[1][0] = p_support[c2].u-p_support[c2].d;
- A.val[2][0] = p_support[c3].u-p_support[c3].d;
- A.val[0][1] = p_support[c1].v; A.val[0][2] = 1;
- A.val[1][1] = p_support[c2].v; A.val[1][2] = 1;
- A.val[2][1] = p_support[c3].v; A.val[2][2] = 1;
-
- // compute vector b for linear system (containing the disparities)
- b.val[0][0] = p_support[c1].d;
- b.val[1][0] = p_support[c2].d;
- b.val[2][0] = p_support[c3].d;
-
- // on success of gauss jordan elimination
- if (b.solve(A)) {
-
- // grab results from b
- tri[i].t2a = b.val[0][0];
- tri[i].t2b = b.val[1][0];
- tri[i].t2c = b.val[2][0];
-
- // otherwise: invalid
- } else {
- tri[i].t2a = 0;
- tri[i].t2b = 0;
- tri[i].t2c = 0;
- }
- }
-}
-
-void Elas::createGrid(vector<support_pt> p_support,int32_t* disparity_grid,int32_t* grid_dims,bool right_image) {
-
- // get grid dimensions
- int32_t grid_width = grid_dims[1];
- int32_t grid_height = grid_dims[2];
-
- // allocate temporary memory
- int32_t* temp1 = (int32_t*)calloc((param.disp_max+1)*grid_height*grid_width,sizeof(int32_t));
- int32_t* temp2 = (int32_t*)calloc((param.disp_max+1)*grid_height*grid_width,sizeof(int32_t));
-
- // for all support points do
- for (int32_t i=0; i<p_support.size(); i++) {
-
- // compute disparity range to fill for this support point
- int32_t x_curr = p_support[i].u;
- int32_t y_curr = p_support[i].v;
- int32_t d_curr = p_support[i].d;
- int32_t d_min = max(d_curr-1,0);
- int32_t d_max = min(d_curr+1,param.disp_max);
-
- // fill disparity grid helper
- for (int32_t d=d_min; d<=d_max; d++) {
- int32_t x;
- if (!right_image)
- x = floor((float)(x_curr/param.grid_size));
- else
- x = floor((float)(x_curr-d_curr)/(float)param.grid_size);
- int32_t y = floor((float)y_curr/(float)param.grid_size);
-
- // point may potentially lay outside (corner points)
- if (x>=0 && x<grid_width &&y>=0 && y<grid_height) {
- int32_t addr = getAddressOffsetGrid(x,y,d,grid_width,param.disp_max+1);
- *(temp1+addr) = 1;
- }
- }
- }
-
- // diffusion pointers
- const int32_t* tl = temp1 + (0*grid_width+0)*(param.disp_max+1);
- const int32_t* tc = temp1 + (0*grid_width+1)*(param.disp_max+1);
- const int32_t* tr = temp1 + (0*grid_width+2)*(param.disp_max+1);
- const int32_t* cl = temp1 + (1*grid_width+0)*(param.disp_max+1);
- const int32_t* cc = temp1 + (1*grid_width+1)*(param.disp_max+1);
- const int32_t* cr = temp1 + (1*grid_width+2)*(param.disp_max+1);
- const int32_t* bl = temp1 + (2*grid_width+0)*(param.disp_max+1);
- const int32_t* bc = temp1 + (2*grid_width+1)*(param.disp_max+1);
- const int32_t* br = temp1 + (2*grid_width+2)*(param.disp_max+1);
-
- int32_t* result = temp2 + (1*grid_width+1)*(param.disp_max+1);
- int32_t* end_input = temp1 + grid_width*grid_height*(param.disp_max+1);
-
- // diffuse temporary grid
- for( ; br != end_input; tl++, tc++, tr++, cl++, cc++, cr++, bl++, bc++, br++, result++ )
- *result = *tl | *tc | *tr | *cl | *cc | *cr | *bl | *bc | *br;
-
- // for all grid positions create disparity grid
- for (int32_t x=0; x<grid_width; x++) {
- for (int32_t y=0; y<grid_height; y++) {
-
- // start with second value (first is reserved for count)
- int32_t curr_ind = 1;
-
- // for all disparities do
- for (int32_t d=0; d<=param.disp_max; d++) {
-
- // if yes => add this disparity to current cell
- if (*(temp2+getAddressOffsetGrid(x,y,d,grid_width,param.disp_max+1))>0) {
- *(disparity_grid+getAddressOffsetGrid(x,y,curr_ind,grid_width,param.disp_max+2))=d;
- curr_ind++;
- }
- }
-
- // finally set number of indices
- *(disparity_grid+getAddressOffsetGrid(x,y,0,grid_width,param.disp_max+2))=curr_ind-1;
- }
- }
-
- // release temporary memory
- free(temp1);
- free(temp2);
-}
-
-inline void Elas::updatePosteriorMinimum(__m128i* I2_block_addr,const int32_t &d,const int32_t &w,
- const __m128i &xmm1,__m128i &xmm2,int32_t &val,int32_t &min_val,int32_t &min_d) {
- xmm2 = _mm_load_si128(I2_block_addr);
- xmm2 = _mm_sad_epu8(xmm1,xmm2);
- val = _mm_extract_epi16(xmm2,0)+_mm_extract_epi16(xmm2,4)+w;
- if (val<min_val) {
- min_val = val;
- min_d = d;
- }
-}
-
-inline void Elas::updatePosteriorMinimum(__m128i* I2_block_addr,const int32_t &d,
- const __m128i &xmm1,__m128i &xmm2,int32_t &val,int32_t &min_val,int32_t &min_d) {
- xmm2 = _mm_load_si128(I2_block_addr);
- xmm2 = _mm_sad_epu8(xmm1,xmm2);
- val = _mm_extract_epi16(xmm2,0)+_mm_extract_epi16(xmm2,4);
- if (val<min_val) {
- min_val = val;
- min_d = d;
- }
-}
-
-inline void Elas::findMatch(int32_t &u,int32_t &v,float &plane_a,float &plane_b,float &plane_c,
- int32_t* disparity_grid,int32_t *grid_dims,uint8_t* I1_desc,uint8_t* I2_desc,
- int32_t *P,int32_t &plane_radius,bool &valid,bool &right_image,float* D){
-
- // get image width and height
- const int32_t disp_num = grid_dims[0]-1;
- const int32_t window_size = 2;
-
- // address of disparity we want to compute
- uint32_t d_addr;
- if (param.subsampling) d_addr = getAddressOffsetImage(u/2,v/2,width/2);
- else d_addr = getAddressOffsetImage(u,v,width);
-
- // check if u is ok
- if (u<window_size || u>=width-window_size)
- return;
-
- // compute line start address
- int32_t line_offset = 16*width*max(min(v,height-3),2);
- uint8_t *I1_line_addr,*I2_line_addr;
- if (!right_image) {
- I1_line_addr = I1_desc+line_offset;
- I2_line_addr = I2_desc+line_offset;
- } else {
- I1_line_addr = I2_desc+line_offset;
- I2_line_addr = I1_desc+line_offset;
- }
-
- // compute I1 block start address
- uint8_t* I1_block_addr = I1_line_addr+16*u;
-
- // does this patch have enough texture?
- int32_t sum = 0;
- for (int32_t i=0; i<16; i++)
- sum += abs((int32_t)(*(I1_block_addr+i))-128);
- if (sum<param.match_texture)
- return;
-
- // compute disparity, min disparity and max disparity of plane prior
- int32_t d_plane = (int32_t)(plane_a*(float)u+plane_b*(float)v+plane_c);
- int32_t d_plane_min = max(d_plane-plane_radius,0);
- int32_t d_plane_max = min(d_plane+plane_radius,disp_num-1);
-
- // get grid pointer
- int32_t grid_x = (int32_t)floor((float)u/(float)param.grid_size);
- int32_t grid_y = (int32_t)floor((float)v/(float)param.grid_size);
- uint32_t grid_addr = getAddressOffsetGrid(grid_x,grid_y,0,grid_dims[1],grid_dims[0]);
- int32_t num_grid = *(disparity_grid+grid_addr);
- int32_t* d_grid = disparity_grid+grid_addr+1;
-
- // loop variables
- int32_t d_curr, u_warp, val;
- int32_t min_val = 10000;
- int32_t min_d = -1;
- __m128i xmm1 = _mm_load_si128((__m128i*)I1_block_addr);
- __m128i xmm2;
-
- // left image
- if (!right_image) {
- for (int32_t i=0; i<num_grid; i++) {
- d_curr = d_grid[i];
- if (d_curr<d_plane_min || d_curr>d_plane_max) {
- u_warp = u-d_curr;
- if (u_warp<window_size || u_warp>=width-window_size)
- continue;
- updatePosteriorMinimum((__m128i*)(I2_line_addr+16*u_warp),d_curr,xmm1,xmm2,val,min_val,min_d);
- }
- }
- for (d_curr=d_plane_min; d_curr<=d_plane_max; d_curr++) {
- u_warp = u-d_curr;
- if (u_warp<window_size || u_warp>=width-window_size)
- continue;
- updatePosteriorMinimum((__m128i*)(I2_line_addr+16*u_warp),d_curr,valid?*(P+abs(d_curr-d_plane)):0,xmm1,xmm2,val,min_val,min_d);
- }
-
- // right image
- } else {
- for (int32_t i=0; i<num_grid; i++) {
- d_curr = d_grid[i];
- if (d_curr<d_plane_min || d_curr>d_plane_max) {
- u_warp = u+d_curr;
- if (u_warp<window_size || u_warp>=width-window_size)
- continue;
- updatePosteriorMinimum((__m128i*)(I2_line_addr+16*u_warp),d_curr,xmm1,xmm2,val,min_val,min_d);
- }
- }
- for (d_curr=d_plane_min; d_curr<=d_plane_max; d_curr++) {
- u_warp = u+d_curr;
- if (u_warp<window_size || u_warp>=width-window_size)
- continue;
- updatePosteriorMinimum((__m128i*)(I2_line_addr+16*u_warp),d_curr,valid?*(P+abs(d_curr-d_plane)):0,xmm1,xmm2,val,min_val,min_d);
- }
- }
-
- // set disparity value
- if (min_d>=0) *(D+d_addr) = min_d; // MAP value (min neg-Log probability)
- else *(D+d_addr) = -1; // invalid disparity
-}
-
-// TODO: %2 => more elegantly
-void Elas::computeDisparity(vector<support_pt> p_support,vector<triangle> tri,int32_t* disparity_grid,int32_t *grid_dims,
- uint8_t* I1_desc,uint8_t* I2_desc,bool right_image,float* D) {
-
- // number of disparities
- const int32_t disp_num = grid_dims[0]-1;
-
- // descriptor window_size
- int32_t window_size = 2;
-
- // init disparity image to -10
- if (param.subsampling) {
- for (int32_t i=0; i<(width/2)*(height/2); i++)
- *(D+i) = -10;
- } else {
- for (int32_t i=0; i<width*height; i++)
- *(D+i) = -10;
- }
-
- // pre-compute prior
- float two_sigma_squared = 2*param.sigma*param.sigma;
- int32_t* P = new int32_t[disp_num];
- for (int32_t delta_d=0; delta_d<disp_num; delta_d++)
- P[delta_d] = (int32_t)((-log(param.gamma+exp(-delta_d*delta_d/two_sigma_squared))+log(param.gamma))/param.beta);
- int32_t plane_radius = (int32_t)max((float)ceil(param.sigma*param.sradius),(float)2.0);
-
- // loop variables
- int32_t c1, c2, c3;
- float plane_a,plane_b,plane_c,plane_d;
-
- // for all triangles do
- for (uint32_t i=0; i<tri.size(); i++) {
-
- // get plane parameters
- uint32_t p_i = i*3;
- if (!right_image) {
- plane_a = tri[i].t1a;
- plane_b = tri[i].t1b;
- plane_c = tri[i].t1c;
- plane_d = tri[i].t2a;
- } else {
- plane_a = tri[i].t2a;
- plane_b = tri[i].t2b;
- plane_c = tri[i].t2c;
- plane_d = tri[i].t1a;
- }
-
- // triangle corners
- c1 = tri[i].c1;
- c2 = tri[i].c2;
- c3 = tri[i].c3;
-
- // sort triangle corners wrt. u (ascending)
- float tri_u[3];
- if (!right_image) {
- tri_u[0] = p_support[c1].u;
- tri_u[1] = p_support[c2].u;
- tri_u[2] = p_support[c3].u;
- } else {
- tri_u[0] = p_support[c1].u-p_support[c1].d;
- tri_u[1] = p_support[c2].u-p_support[c2].d;
- tri_u[2] = p_support[c3].u-p_support[c3].d;
- }
- float tri_v[3] = {p_support[c1].v,p_support[c2].v,p_support[c3].v};
-
- for (uint32_t j=0; j<3; j++) {
- for (uint32_t k=0; k<j; k++) {
- if (tri_u[k]>tri_u[j]) {
- float tri_u_temp = tri_u[j]; tri_u[j] = tri_u[k]; tri_u[k] = tri_u_temp;
- float tri_v_temp = tri_v[j]; tri_v[j] = tri_v[k]; tri_v[k] = tri_v_temp;
- }
- }
- }
-
- // rename corners
- float A_u = tri_u[0]; float A_v = tri_v[0];
- float B_u = tri_u[1]; float B_v = tri_v[1];
- float C_u = tri_u[2]; float C_v = tri_v[2];
-
- // compute straight lines connecting triangle corners
- float AB_a = 0; float AC_a = 0; float BC_a = 0;
- if ((int32_t)(A_u)!=(int32_t)(B_u)) AB_a = (A_v-B_v)/(A_u-B_u);
- if ((int32_t)(A_u)!=(int32_t)(C_u)) AC_a = (A_v-C_v)/(A_u-C_u);
- if ((int32_t)(B_u)!=(int32_t)(C_u)) BC_a = (B_v-C_v)/(B_u-C_u);
- float AB_b = A_v-AB_a*A_u;
- float AC_b = A_v-AC_a*A_u;
- float BC_b = B_v-BC_a*B_u;
-
- // a plane is only valid if itself and its projection
- // into the other image is not too much slanted
- bool valid = fabs(plane_a)<0.7 && fabs(plane_d)<0.7;
-
- // first part (triangle corner A->B)
- if ((int32_t)(A_u)!=(int32_t)(B_u)) {
- for (int32_t u=max((int32_t)A_u,0); u<min((int32_t)B_u,width); u++){
- if (!param.subsampling || u%2==0) {
- int32_t v_1 = (uint32_t)(AC_a*(float)u+AC_b);
- int32_t v_2 = (uint32_t)(AB_a*(float)u+AB_b);
- for (int32_t v=min(v_1,v_2); v<max(v_1,v_2); v++)
- if (!param.subsampling || v%2==0) {
- findMatch(u,v,plane_a,plane_b,plane_c,disparity_grid,grid_dims,
- I1_desc,I2_desc,P,plane_radius,valid,right_image,D);
- }
- }
- }
- }
-
- // second part (triangle corner B->C)
- if ((int32_t)(B_u)!=(int32_t)(C_u)) {
- for (int32_t u=max((int32_t)B_u,0); u<min((int32_t)C_u,width); u++){
- if (!param.subsampling || u%2==0) {
- int32_t v_1 = (uint32_t)(AC_a*(float)u+AC_b);
- int32_t v_2 = (uint32_t)(BC_a*(float)u+BC_b);
- for (int32_t v=min(v_1,v_2); v<max(v_1,v_2); v++)
- if (!param.subsampling || v%2==0) {
- findMatch(u,v,plane_a,plane_b,plane_c,disparity_grid,grid_dims,
- I1_desc,I2_desc,P,plane_radius,valid,right_image,D);
- }
- }
- }
- }
-
- }
-
- delete[] P;
-}
-
-void Elas::leftRightConsistencyCheck(float* D1,float* D2) {
-
- // get disparity image dimensions
- int32_t D_width = width;
- int32_t D_height = height;
- if (param.subsampling) {
- D_width = width/2;
- D_height = height/2;
- }
-
- // make a copy of both images
- float* D1_copy = (float*)malloc(D_width*D_height*sizeof(float));
- float* D2_copy = (float*)malloc(D_width*D_height*sizeof(float));
- memcpy(D1_copy,D1,D_width*D_height*sizeof(float));
- memcpy(D2_copy,D2,D_width*D_height*sizeof(float));
-
- // loop variables
- uint32_t addr,addr_warp;
- float u_warp_1,u_warp_2,d1,d2;
-
- // for all image points do
- for (int32_t u=0; u<D_width; u++) {
- for (int32_t v=0; v<D_height; v++) {
-
- // compute address (u,v) and disparity value
- addr = getAddressOffsetImage(u,v,D_width);
- d1 = *(D1_copy+addr);
- d2 = *(D2_copy+addr);
- if (param.subsampling) {
- u_warp_1 = (float)u-d1/2;
- u_warp_2 = (float)u+d2/2;
- } else {
- u_warp_1 = (float)u-d1;
- u_warp_2 = (float)u+d2;
- }
-
-
- // check if left disparity is valid
- if (d1>=0 && u_warp_1>=0 && u_warp_1<D_width) {
-
- // compute warped image address
- addr_warp = getAddressOffsetImage((int32_t)u_warp_1,v,D_width);
-
- // if check failed
- if (fabs(*(D2_copy+addr_warp)-d1)>param.lr_threshold)
- *(D1+addr) = -10;
-
- // set invalid
- } else
- *(D1+addr) = -10;
-
- // check if right disparity is valid
- if (d2>=0 && u_warp_2>=0 && u_warp_2<D_width) {
-
- // compute warped image address
- addr_warp = getAddressOffsetImage((int32_t)u_warp_2,v,D_width);
-
- // if check failed
- if (fabs(*(D1_copy+addr_warp)-d2)>param.lr_threshold)
- *(D2+addr) = -10;
-
- // set invalid
- } else
- *(D2+addr) = -10;
- }
- }
-
- // release memory
- free(D1_copy);
- free(D2_copy);
-}
-
-void Elas::removeSmallSegments (float* D) {
-
- // get disparity image dimensions
- int32_t D_width = width;
- int32_t D_height = height;
- int32_t D_speckle_size = param.speckle_size;
- if (param.subsampling) {
- D_width = width/2;
- D_height = height/2;
- D_speckle_size = sqrt((float)param.speckle_size)*2;
- }
-
- // allocate memory on heap for dynamic programming arrays
- int32_t *D_done = (int32_t*)calloc(D_width*D_height,sizeof(int32_t));
- int32_t *seg_list_u = (int32_t*)calloc(D_width*D_height,sizeof(int32_t));
- int32_t *seg_list_v = (int32_t*)calloc(D_width*D_height,sizeof(int32_t));
- int32_t seg_list_count;
- int32_t seg_list_curr;
- int32_t u_neighbor[4];
- int32_t v_neighbor[4];
- int32_t u_seg_curr;
- int32_t v_seg_curr;
-
- // declare loop variables
- int32_t addr_start, addr_curr, addr_neighbor;
-
- // for all pixels do
- for (int32_t u=0; u<D_width; u++) {
- for (int32_t v=0; v<D_height; v++) {
-
- // get address of first pixel in this segment
- addr_start = getAddressOffsetImage(u,v,D_width);
-
- // if this pixel has not already been processed
- if (*(D_done+addr_start)==0) {
-
- // init segment list (add first element
- // and set it to be the next element to check)
- *(seg_list_u+0) = u;
- *(seg_list_v+0) = v;
- seg_list_count = 1;
- seg_list_curr = 0;
-
- // add neighboring segments as long as there
- // are none-processed pixels in the seg_list;
- // none-processed means: seg_list_curr<seg_list_count
- while (seg_list_curr<seg_list_count) {
-
- // get current position from seg_list
- u_seg_curr = *(seg_list_u+seg_list_curr);
- v_seg_curr = *(seg_list_v+seg_list_curr);
-
- // get address of current pixel in this segment
- addr_curr = getAddressOffsetImage(u_seg_curr,v_seg_curr,D_width);
-
- // fill list with neighbor positions
- u_neighbor[0] = u_seg_curr-1; v_neighbor[0] = v_seg_curr;
- u_neighbor[1] = u_seg_curr+1; v_neighbor[1] = v_seg_curr;
- u_neighbor[2] = u_seg_curr; v_neighbor[2] = v_seg_curr-1;
- u_neighbor[3] = u_seg_curr; v_neighbor[3] = v_seg_curr+1;
-
- // for all neighbors do
- for (int32_t i=0; i<4; i++) {
-
- // check if neighbor is inside image
- if (u_neighbor[i]>=0 && v_neighbor[i]>=0 && u_neighbor[i]<D_width && v_neighbor[i]<D_height) {
-
- // get neighbor pixel address
- addr_neighbor = getAddressOffsetImage(u_neighbor[i],v_neighbor[i],D_width);
-
- // check if neighbor has not been added yet and if it is valid
- if (*(D_done+addr_neighbor)==0 && *(D+addr_neighbor)>=0) {
-
- // is the neighbor similar to the current pixel
- // (=belonging to the current segment)
- if (fabs(*(D+addr_curr)-*(D+addr_neighbor))<=param.speckle_sim_threshold) {
-
- // add neighbor coordinates to segment list
- *(seg_list_u+seg_list_count) = u_neighbor[i];
- *(seg_list_v+seg_list_count) = v_neighbor[i];
- seg_list_count++;
-
- // set neighbor pixel in I_done to "done"
- // (otherwise a pixel may be added 2 times to the list, as
- // neighbor of one pixel and as neighbor of another pixel)
- *(D_done+addr_neighbor) = 1;
- }
- }
-
- }
- }
-
- // set current pixel in seg_list to "done"
- seg_list_curr++;
-
- // set current pixel in I_done to "done"
- *(D_done+addr_curr) = 1;
-
- } // end: while (seg_list_curr<seg_list_count)
-
- // if segment NOT large enough => invalidate pixels
- if (seg_list_count<D_speckle_size) {
-
- // for all pixels in current segment invalidate pixels
- for (int32_t i=0; i<seg_list_count; i++) {
- addr_curr = getAddressOffsetImage(*(seg_list_u+i),*(seg_list_v+i),D_width);
- *(D+addr_curr) = -10;
- }
- }
- } // end: if (*(I_done+addr_start)==0)
-
- }
- }
-
- // free memory
- free(D_done);
- free(seg_list_u);
- free(seg_list_v);
-}
-
-void Elas::gapInterpolation(float* D) {
-
- // get disparity image dimensions
- int32_t D_width = width;
- int32_t D_height = height;
- int32_t D_ipol_gap_width = param.ipol_gap_width;
- if (param.subsampling) {
- D_width = width/2;
- D_height = height/2;
- D_ipol_gap_width = param.ipol_gap_width/2+1;
- }
-
- // discontinuity threshold
- float discon_threshold = 3.0;
-
- // declare loop variables
- int32_t count,addr,v_first,v_last,u_first,u_last;
- float d1,d2,d_ipol;
-
- // 1. Row-wise:
- // for each row do
- for (int32_t v=0; v<D_height; v++) {
-
- // init counter
- count = 0;
-
- // for each element of the row do
- for (int32_t u=0; u<D_width; u++) {
-
- // get address of this location
- addr = getAddressOffsetImage(u,v,D_width);
-
- // if disparity valid
- if (*(D+addr)>=0) {
-
- // check if speckle is small enough
- if (count>=1 && count<=D_ipol_gap_width) {
-
- // first and last value for interpolation
- u_first = u-count;
- u_last = u-1;
-
- // if value in range
- if (u_first>0 && u_last<D_width-1) {
-
- // compute mean disparity
- d1 = *(D+getAddressOffsetImage(u_first-1,v,D_width));
- d2 = *(D+getAddressOffsetImage(u_last+1,v,D_width));
- if (fabs(d1-d2)<discon_threshold) d_ipol = (d1+d2)/2;
- else d_ipol = min(d1,d2);
-
- // set all values to d_ipol
- for (int32_t u_curr=u_first; u_curr<=u_last; u_curr++)
- *(D+getAddressOffsetImage(u_curr,v,D_width)) = d_ipol;
- }
-
- }
-
- // reset counter
- count = 0;
-
- // otherwise increment counter
- } else {
- count++;
- }
- }
-
- // if full size disp map requested
- if (param.add_corners) {
-
- // extrapolate to the left
- for (int32_t u=0; u<D_width; u++) {
-
- // get address of this location
- addr = getAddressOffsetImage(u,v,D_width);
-
- // if disparity valid
- if (*(D+addr)>=0) {
- for (int32_t u2=max(u-D_ipol_gap_width,0); u2<u; u2++)
- *(D+getAddressOffsetImage(u2,v,D_width)) = *(D+addr);
- break;
- }
- }
-
- // extrapolate to the right
- for (int32_t u=D_width-1; u>=0; u--) {
-
- // get address of this location
- addr = getAddressOffsetImage(u,v,D_width);
-
- // if disparity valid
- if (*(D+addr)>=0) {
- for (int32_t u2=u; u2<=min(u+D_ipol_gap_width,D_width-1); u2++)
- *(D+getAddressOffsetImage(u2,v,D_width)) = *(D+addr);
- break;
- }
- }
- }
- }
-
- // 2. Column-wise:
- // for each column do
- for (int32_t u=0; u<D_width; u++) {
-
- // init counter
- count = 0;
-
- // for each element of the column do
- for (int32_t v=0; v<D_height; v++) {
-
- // get address of this location
- addr = getAddressOffsetImage(u,v,D_width);
-
- // if disparity valid
- if (*(D+addr)>=0) {
-
- // check if gap is small enough
- if (count>=1 && count<=D_ipol_gap_width) {
-
- // first and last value for interpolation
- v_first = v-count;
- v_last = v-1;
-
- // if value in range
- if (v_first>0 && v_last<D_height-1) {
-
- // compute mean disparity
- d1 = *(D+getAddressOffsetImage(u,v_first-1,D_width));
- d2 = *(D+getAddressOffsetImage(u,v_last+1,D_width));
- if (fabs(d1-d2)<discon_threshold) d_ipol = (d1+d2)/2;
- else d_ipol = min(d1,d2);
-
- // set all values to d_ipol
- for (int32_t v_curr=v_first; v_curr<=v_last; v_curr++)
- *(D+getAddressOffsetImage(u,v_curr,D_width)) = d_ipol;
- }
-
- }
-
- // reset counter
- count = 0;
-
- // otherwise increment counter
- } else {
- count++;
- }
- }
-
- // added extrapolation to top and bottom since bottom rows sometimes stay unlabeled...
- // DS 5/12/2014
-
- // if full size disp map requested
- if (param.add_corners) {
-
- // extrapolate towards top
- for (int32_t v=0; v<D_height; v++) {
-
- // get address of this location
- addr = getAddressOffsetImage(u,v,D_width);
-
- // if disparity valid
- if (*(D+addr)>=0) {
- for (int32_t v2=max(v-D_ipol_gap_width,0); v2<v; v2++)
- *(D+getAddressOffsetImage(u,v2,D_width)) = *(D+addr);
- break;
- }
- }
-
- // extrapolate towards the bottom
- for (int32_t v=D_height-1; v>=0; v--) {
-
- // get address of this location
- addr = getAddressOffsetImage(u,v,D_width);
-
- // if disparity valid
- if (*(D+addr)>=0) {
- for (int32_t v2=v; v2<=min(v+D_ipol_gap_width,D_height-1); v2++)
- *(D+getAddressOffsetImage(u,v2,D_width)) = *(D+addr);
- break;
- }
- }
- }
- }
-}
-
-// implements approximation to bilateral filtering
-void Elas::adaptiveMean (float* D) {
-
- // get disparity image dimensions
- int32_t D_width = width;
- int32_t D_height = height;
- if (param.subsampling) {
- D_width = width/2;
- D_height = height/2;
- }
-
- // allocate temporary memory
- float* D_copy = (float*)malloc(D_width*D_height*sizeof(float));
- float* D_tmp = (float*)malloc(D_width*D_height*sizeof(float));
- memcpy(D_copy,D,D_width*D_height*sizeof(float));
-
- // zero input disparity maps to -10 (this makes the bilateral
- // weights of all valid disparities to 0 in this region)
- for (int32_t i=0; i<D_width*D_height; i++) {
- if (*(D+i)<0) {
- *(D_copy+i) = -10;
- *(D_tmp+i) = -10;
- }
- }
-
- __m128 xconst0 = _mm_set1_ps(0);
- __m128 xconst4 = _mm_set1_ps(4);
- __m128 xval,xweight1,xweight2,xfactor1,xfactor2;
-
- float *val = (float *)_mm_malloc(8*sizeof(float),16);
- float *weight = (float*)_mm_malloc(4*sizeof(float),16);
- float *factor = (float*)_mm_malloc(4*sizeof(float),16);
-
- // set absolute mask
- __m128 xabsmask = _mm_set1_ps(0x7FFFFFFF);
-
- // when doing subsampling: 4 pixel bilateral filter width
- if (param.subsampling) {
-
- // horizontal filter
- for (int32_t v=3; v<D_height-3; v++) {
-
- // init
- for (int32_t u=0; u<3; u++)
- val[u] = *(D_copy+v*D_width+u);
-
- // loop
- for (int32_t u=3; u<D_width; u++) {
-
- // set
- float val_curr = *(D_copy+v*D_width+(u-1));
- val[u%4] = *(D_copy+v*D_width+u);
-
- xval = _mm_load_ps(val);
- xweight1 = _mm_sub_ps(xval,_mm_set1_ps(val_curr));
- xweight1 = _mm_and_ps(xweight1,xabsmask);
- xweight1 = _mm_sub_ps(xconst4,xweight1);
- xweight1 = _mm_max_ps(xconst0,xweight1);
- xfactor1 = _mm_mul_ps(xval,xweight1);
-
- _mm_store_ps(weight,xweight1);
- _mm_store_ps(factor,xfactor1);
-
- float weight_sum = weight[0]+weight[1]+weight[2]+weight[3];
- float factor_sum = factor[0]+factor[1]+factor[2]+factor[3];
-
- if (weight_sum>0) {
- float d = factor_sum/weight_sum;
- if (d>=0) *(D_tmp+v*D_width+(u-1)) = d;
- }
- }
- }
-
- // vertical filter
- for (int32_t u=3; u<D_width-3; u++) {
-
- // init
- for (int32_t v=0; v<3; v++)
- val[v] = *(D_tmp+v*D_width+u);
-
- // loop
- for (int32_t v=3; v<D_height; v++) {
-
- // set
- float val_curr = *(D_tmp+(v-1)*D_width+u);
- val[v%4] = *(D_tmp+v*D_width+u);
-
- xval = _mm_load_ps(val);
- xweight1 = _mm_sub_ps(xval,_mm_set1_ps(val_curr));
- xweight1 = _mm_and_ps(xweight1,xabsmask);
- xweight1 = _mm_sub_ps(xconst4,xweight1);
- xweight1 = _mm_max_ps(xconst0,xweight1);
- xfactor1 = _mm_mul_ps(xval,xweight1);
-
- _mm_store_ps(weight,xweight1);
- _mm_store_ps(factor,xfactor1);
-
- float weight_sum = weight[0]+weight[1]+weight[2]+weight[3];
- float factor_sum = factor[0]+factor[1]+factor[2]+factor[3];
-
- if (weight_sum>0) {
- float d = factor_sum/weight_sum;
- if (d>=0) *(D+(v-1)*D_width+u) = d;
- }
- }
- }
-
- // full resolution: 8 pixel bilateral filter width
- } else {
-
-
- // horizontal filter
- for (int32_t v=3; v<D_height-3; v++) {
-
- // init
- for (int32_t u=0; u<7; u++)
- val[u] = *(D_copy+v*D_width+u);
-
- // loop
- for (int32_t u=7; u<D_width; u++) {
-
- // set
- float val_curr = *(D_copy+v*D_width+(u-3));
- val[u%8] = *(D_copy+v*D_width+u);
-
- xval = _mm_load_ps(val);
- xweight1 = _mm_sub_ps(xval,_mm_set1_ps(val_curr));
- xweight1 = _mm_and_ps(xweight1,xabsmask);
- xweight1 = _mm_sub_ps(xconst4,xweight1);
- xweight1 = _mm_max_ps(xconst0,xweight1);
- xfactor1 = _mm_mul_ps(xval,xweight1);
-
- xval = _mm_load_ps(val+4);
- xweight2 = _mm_sub_ps(xval,_mm_set1_ps(val_curr));
- xweight2 = _mm_and_ps(xweight2,xabsmask);
- xweight2 = _mm_sub_ps(xconst4,xweight2);
- xweight2 = _mm_max_ps(xconst0,xweight2);
- xfactor2 = _mm_mul_ps(xval,xweight2);
-
- xweight1 = _mm_add_ps(xweight1,xweight2);
- xfactor1 = _mm_add_ps(xfactor1,xfactor2);
-
- _mm_store_ps(weight,xweight1);
- _mm_store_ps(factor,xfactor1);
-
- float weight_sum = weight[0]+weight[1]+weight[2]+weight[3];
- float factor_sum = factor[0]+factor[1]+factor[2]+factor[3];
-
- if (weight_sum>0) {
- float d = factor_sum/weight_sum;
- if (d>=0) *(D_tmp+v*D_width+(u-3)) = d;
- }
- }
- }
-
- // vertical filter
- for (int32_t u=3; u<D_width-3; u++) {
-
- // init
- for (int32_t v=0; v<7; v++)
- val[v] = *(D_tmp+v*D_width+u);
-
- // loop
- for (int32_t v=7; v<D_height; v++) {
-
- // set
- float val_curr = *(D_tmp+(v-3)*D_width+u);
- val[v%8] = *(D_tmp+v*D_width+u);
-
- xval = _mm_load_ps(val);
- xweight1 = _mm_sub_ps(xval,_mm_set1_ps(val_curr));
- xweight1 = _mm_and_ps(xweight1,xabsmask);
- xweight1 = _mm_sub_ps(xconst4,xweight1);
- xweight1 = _mm_max_ps(xconst0,xweight1);
- xfactor1 = _mm_mul_ps(xval,xweight1);
-
- xval = _mm_load_ps(val+4);
- xweight2 = _mm_sub_ps(xval,_mm_set1_ps(val_curr));
- xweight2 = _mm_and_ps(xweight2,xabsmask);
- xweight2 = _mm_sub_ps(xconst4,xweight2);
- xweight2 = _mm_max_ps(xconst0,xweight2);
- xfactor2 = _mm_mul_ps(xval,xweight2);
-
- xweight1 = _mm_add_ps(xweight1,xweight2);
- xfactor1 = _mm_add_ps(xfactor1,xfactor2);
-
- _mm_store_ps(weight,xweight1);
- _mm_store_ps(factor,xfactor1);
-
- float weight_sum = weight[0]+weight[1]+weight[2]+weight[3];
- float factor_sum = factor[0]+factor[1]+factor[2]+factor[3];
-
- if (weight_sum>0) {
- float d = factor_sum/weight_sum;
- if (d>=0) *(D+(v-3)*D_width+u) = d;
- }
- }
- }
- }
-
- // free memory
- _mm_free(val);
- _mm_free(weight);
- _mm_free(factor);
- free(D_copy);
- free(D_tmp);
-}
-
-void Elas::median (float* D) {
-
- // get disparity image dimensions
- int32_t D_width = width;
- int32_t D_height = height;
- if (param.subsampling) {
- D_width = width/2;
- D_height = height/2;
- }
-
- // temporary memory
- float *D_temp = (float*)calloc(D_width*D_height,sizeof(float));
-
- int32_t window_size = 3;
-
- float *vals = new float[window_size*2+1];
- int32_t i,j;
- float temp;
-
- // first step: horizontal median filter
- for (int32_t u=window_size; u<D_width-window_size; u++) {
- for (int32_t v=window_size; v<D_height-window_size; v++) {
- if (*(D+getAddressOffsetImage(u,v,D_width))>=0) {
- j = 0;
- for (int32_t u2=u-window_size; u2<=u+window_size; u2++) {
- temp = *(D+getAddressOffsetImage(u2,v,D_width));
- i = j-1;
- while (i>=0 && *(vals+i)>temp) {
- *(vals+i+1) = *(vals+i);
- i--;
- }
- *(vals+i+1) = temp;
- j++;
- }
- *(D_temp+getAddressOffsetImage(u,v,D_width)) = *(vals+window_size);
- } else {
- *(D_temp+getAddressOffsetImage(u,v,D_width)) = *(D+getAddressOffsetImage(u,v,D_width));
- }
-
- }
- }
-
- // second step: vertical median filter
- for (int32_t u=window_size; u<D_width-window_size; u++) {
- for (int32_t v=window_size; v<D_height-window_size; v++) {
- if (*(D+getAddressOffsetImage(u,v,D_width))>=0) {
- j = 0;
- for (int32_t v2=v-window_size; v2<=v+window_size; v2++) {
- temp = *(D_temp+getAddressOffsetImage(u,v2,D_width));
- i = j-1;
- while (i>=0 && *(vals+i)>temp) {
- *(vals+i+1) = *(vals+i);
- i--;
- }
- *(vals+i+1) = temp;
- j++;
- }
- *(D+getAddressOffsetImage(u,v,D_width)) = *(vals+window_size);
- } else {
- *(D+getAddressOffsetImage(u,v,D_width)) = *(D+getAddressOffsetImage(u,v,D_width));
- }
- }
- }
-
- free(D_temp);
- free(vals);
-}
diff --git a/vision/lib/elas/filter.cpp b/vision/lib/elas/filter.cpp
deleted file mode 100644
index f98a21a49a86a6be8172662ab37c78bca0f0ea71..0000000000000000000000000000000000000000
--- a/vision/lib/elas/filter.cpp
+++ /dev/null
@@ -1,468 +0,0 @@
-/*
-Copyright 2011. All rights reserved.
-Institute of Measurement and Control Systems
-Karlsruhe Institute of Technology, Germany
-
-This file is part of libelas.
-Authors: Julius Ziegler, Andreas Geiger
-
-libelas 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 any later version.
-
-libelas 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.
-
-You should have received a copy of the GNU General Public License along with
-libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
-Street, Fifth Floor, Boston, MA 02110-1301, USA
-*/
-
-#include <stdio.h>
-#include <string.h>
-#include <cassert>
-
-#include "filter.h"
-
-// define fixed-width datatypes for Visual Studio projects
-//#ifndef _MSC_VER
- #include <stdint.h>
-//#else
-// typedef __int8 int8_t;
-// typedef __int16 int16_t;
-// typedef __int32 int32_t;
-// typedef __int64 int64_t;
-// typedef unsigned __int8 uint8_t;
-// typedef unsigned __int16 uint16_t;
-// typedef unsigned __int32 uint32_t;
-// typedef unsigned __int64 uint64_t;
-//#endif
-
-// fast filters: implements 3x3 and 5x5 sobel filters and
-// 5x5 blob and corner filters based on SSE2/3 instructions
-namespace filter {
-
- // private namespace, public user functions at the bottom of this file
- namespace detail {
- void integral_image( const uint8_t* in, int32_t* out, int w, int h ) {
- int32_t* out_top = out;
- const uint8_t* line_end = in + w;
- const uint8_t* in_end = in + w*h;
- int32_t line_sum = 0;
- for( ; in != line_end; in++, out++ ) {
- line_sum += *in;
- *out = line_sum;
- }
- for( ; in != in_end; ) {
- int32_t line_sum = 0;
- const uint8_t* line_end = in + w;
- for( ; in != line_end; in++, out++, out_top++ ) {
- line_sum += *in;
- *out = *out_top + line_sum;
- }
- }
- }
-
- void unpack_8bit_to_16bit( const __m128i a, __m128i& b0, __m128i& b1 ) {
- __m128i zero = _mm_setzero_si128();
- b0 = _mm_unpacklo_epi8( a, zero );
- b1 = _mm_unpackhi_epi8( a, zero );
- }
-
- void pack_16bit_to_8bit_saturate( const __m128i a0, const __m128i a1, __m128i& b ) {
- b = _mm_packus_epi16( a0, a1 );
- }
-
- // convolve image with a (1,4,6,4,1) row vector. Result is accumulated into output.
- // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_14641_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ) {
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const __m128i* i0 = (const __m128i*)(in);
- const int16_t* i1 = in+1;
- const int16_t* i2 = in+2;
- const int16_t* i3 = in+3;
- const int16_t* i4 = in+4;
- uint8_t* result = out + 2;
- const int16_t* const end_input = in + w*h;
- __m128i offs = _mm_set1_epi16( 128 );
- for( ; i4 < end_input; i0 += 1, i1 += 8, i2 += 8, i3 += 8, i4 += 8, result += 16 ) {
- __m128i result_register_lo;
- __m128i result_register_hi;
- for( int i=0; i<2; i++ ) {
- __m128i* result_register;
- if( i==0 ) result_register = &result_register_lo;
- else result_register = &result_register_hi;
- __m128i i0_register = *i0;
- __m128i i1_register = _mm_loadu_si128( (__m128i*)( i1 ) );
- __m128i i2_register = _mm_loadu_si128( (__m128i*)( i2 ) );
- __m128i i3_register = _mm_loadu_si128( (__m128i*)( i3 ) );
- __m128i i4_register = _mm_loadu_si128( (__m128i*)( i4 ) );
- *result_register = _mm_setzero_si128();
- *result_register = _mm_add_epi16( i0_register, *result_register );
- i1_register = _mm_add_epi16( i1_register, i1_register );
- i1_register = _mm_add_epi16( i1_register, i1_register );
- *result_register = _mm_add_epi16( i1_register, *result_register );
- i2_register = _mm_add_epi16( i2_register, i2_register );
- *result_register = _mm_add_epi16( i2_register, *result_register );
- i2_register = _mm_add_epi16( i2_register, i2_register );
- *result_register = _mm_add_epi16( i2_register, *result_register );
- i3_register = _mm_add_epi16( i3_register, i3_register );
- i3_register = _mm_add_epi16( i3_register, i3_register );
- *result_register = _mm_add_epi16( i3_register, *result_register );
- *result_register = _mm_add_epi16( i4_register, *result_register );
- *result_register = _mm_srai_epi16( *result_register, 7 );
- *result_register = _mm_add_epi16( *result_register, offs );
- if( i==0 ) {
- i0 += 1;
- i1 += 8;
- i2 += 8;
- i3 += 8;
- i4 += 8;
- }
- }
- pack_16bit_to_8bit_saturate( result_register_lo, result_register_hi, result_register_lo );
- _mm_storeu_si128( ((__m128i*)( result )), result_register_lo );
- }
- }
-
- // convolve image with a (1,2,0,-2,-1) row vector. Result is accumulated into output.
- // This one works on 16bit input and 8bit output.
- // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_12021_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ) {
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const __m128i* i0 = (const __m128i*)(in);
- const int16_t* i1 = in+1;
- const int16_t* i3 = in+3;
- const int16_t* i4 = in+4;
- uint8_t* result = out + 2;
- const int16_t* const end_input = in + w*h;
- __m128i offs = _mm_set1_epi16( 128 );
- for( ; i4 < end_input; i0 += 1, i1 += 8, i3 += 8, i4 += 8, result += 16 ) {
- __m128i result_register_lo;
- __m128i result_register_hi;
- for( int i=0; i<2; i++ ) {
- __m128i* result_register;
- if( i==0 ) result_register = &result_register_lo;
- else result_register = &result_register_hi;
- __m128i i0_register = *i0;
- __m128i i1_register = _mm_loadu_si128( (__m128i*)( i1 ) );
- __m128i i3_register = _mm_loadu_si128( (__m128i*)( i3 ) );
- __m128i i4_register = _mm_loadu_si128( (__m128i*)( i4 ) );
- *result_register = _mm_setzero_si128();
- *result_register = _mm_add_epi16( i0_register, *result_register );
- i1_register = _mm_add_epi16( i1_register, i1_register );
- *result_register = _mm_add_epi16( i1_register, *result_register );
- i3_register = _mm_add_epi16( i3_register, i3_register );
- *result_register = _mm_sub_epi16( *result_register, i3_register );
- *result_register = _mm_sub_epi16( *result_register, i4_register );
- *result_register = _mm_srai_epi16( *result_register, 7 );
- *result_register = _mm_add_epi16( *result_register, offs );
- if( i==0 ) {
- i0 += 1;
- i1 += 8;
- i3 += 8;
- i4 += 8;
- }
- }
- pack_16bit_to_8bit_saturate( result_register_lo, result_register_hi, result_register_lo );
- _mm_storeu_si128( ((__m128i*)( result )), result_register_lo );
- }
- }
-
- // convolve image with a (1,2,1) row vector. Result is accumulated into output.
- // This one works on 16bit input and 8bit output.
- // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_121_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ) {
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const __m128i* i0 = (const __m128i*)(in);
- const int16_t* i1 = in+1;
- const int16_t* i2 = in+2;
- uint8_t* result = out + 1;
- const int16_t* const end_input = in + w*h;
- const size_t blocked_loops = (w*h-2)/16;
- __m128i offs = _mm_set1_epi16( 128 );
- for( size_t i=0; i != blocked_loops; i++ ) {
- __m128i result_register_lo;
- __m128i result_register_hi;
- __m128i i1_register;
- __m128i i2_register;
-
- i1_register = _mm_loadu_si128( (__m128i*)( i1 ) );
- i2_register = _mm_loadu_si128( (__m128i*)( i2 ) );
- result_register_lo = *i0;
- i1_register = _mm_add_epi16( i1_register, i1_register );
- result_register_lo = _mm_add_epi16( i1_register, result_register_lo );
- result_register_lo = _mm_add_epi16( i2_register, result_register_lo );
- result_register_lo = _mm_srai_epi16( result_register_lo, 2 );
- result_register_lo = _mm_add_epi16( result_register_lo, offs );
-
- i0++;
- i1+=8;
- i2+=8;
-
- i1_register = _mm_loadu_si128( (__m128i*)( i1 ) );
- i2_register = _mm_loadu_si128( (__m128i*)( i2 ) );
- result_register_hi = *i0;
- i1_register = _mm_add_epi16( i1_register, i1_register );
- result_register_hi = _mm_add_epi16( i1_register, result_register_hi );
- result_register_hi = _mm_add_epi16( i2_register, result_register_hi );
- result_register_hi = _mm_srai_epi16( result_register_hi, 2 );
- result_register_hi = _mm_add_epi16( result_register_hi, offs );
-
- i0++;
- i1+=8;
- i2+=8;
-
- pack_16bit_to_8bit_saturate( result_register_lo, result_register_hi, result_register_lo );
- _mm_storeu_si128( ((__m128i*)( result )), result_register_lo );
-
- result += 16;
- }
- }
-
- // convolve image with a (1,0,-1) row vector. Result is accumulated into output.
- // This one works on 16bit input and 8bit output.
- // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_101_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ) {
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const __m128i* i0 = (const __m128i*)(in);
- const int16_t* i2 = in+2;
- uint8_t* result = out + 1;
- const int16_t* const end_input = in + w*h;
- const size_t blocked_loops = (w*h-2)/16;
- __m128i offs = _mm_set1_epi16( 128 );
- for( size_t i=0; i != blocked_loops; i++ ) {
- __m128i result_register_lo;
- __m128i result_register_hi;
- __m128i i2_register;
-
- i2_register = _mm_loadu_si128( (__m128i*)( i2 ) );
- result_register_lo = *i0;
- result_register_lo = _mm_sub_epi16( result_register_lo, i2_register );
- result_register_lo = _mm_srai_epi16( result_register_lo, 2 );
- result_register_lo = _mm_add_epi16( result_register_lo, offs );
-
- i0 += 1;
- i2 += 8;
-
- i2_register = _mm_loadu_si128( (__m128i*)( i2 ) );
- result_register_hi = *i0;
- result_register_hi = _mm_sub_epi16( result_register_hi, i2_register );
- result_register_hi = _mm_srai_epi16( result_register_hi, 2 );
- result_register_hi = _mm_add_epi16( result_register_hi, offs );
-
- i0 += 1;
- i2 += 8;
-
- pack_16bit_to_8bit_saturate( result_register_lo, result_register_hi, result_register_lo );
- _mm_storeu_si128( ((__m128i*)( result )), result_register_lo );
-
- result += 16;
- }
-
- for( ; i2 < end_input; i2++, result++) {
- *result = ((*(i2-2) - *i2)>>2)+128;
- }
- }
-
- void convolve_cols_5x5( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ) {
- using namespace std;
- memset( out_h, 0, w*h*sizeof(int16_t) );
- memset( out_v, 0, w*h*sizeof(int16_t) );
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const int w_chunk = w/16;
- __m128i* i0 = (__m128i*)( in );
- __m128i* i1 = (__m128i*)( in ) + w_chunk*1;
- __m128i* i2 = (__m128i*)( in ) + w_chunk*2;
- __m128i* i3 = (__m128i*)( in ) + w_chunk*3;
- __m128i* i4 = (__m128i*)( in ) + w_chunk*4;
- __m128i* result_h = (__m128i*)( out_h ) + 4*w_chunk;
- __m128i* result_v = (__m128i*)( out_v ) + 4*w_chunk;
- __m128i* end_input = (__m128i*)( in ) + w_chunk*h;
- __m128i sixes = _mm_set1_epi16( 6 );
- __m128i fours = _mm_set1_epi16( 4 );
- for( ; i4 != end_input; i0++, i1++, i2++, i3++, i4++, result_v+=2, result_h+=2 ) {
- __m128i ilo, ihi;
- unpack_8bit_to_16bit( *i0, ihi, ilo );
- *result_h = _mm_add_epi16( ihi, *result_h );
- *(result_h+1) = _mm_add_epi16( ilo, *(result_h+1) );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- unpack_8bit_to_16bit( *i1, ihi, ilo );
- *result_h = _mm_add_epi16( ihi, *result_h );
- *result_h = _mm_add_epi16( ihi, *result_h );
- *(result_h+1) = _mm_add_epi16( ilo, *(result_h+1) );
- *(result_h+1) = _mm_add_epi16( ilo, *(result_h+1) );
- ihi = _mm_mullo_epi16( ihi, fours );
- ilo = _mm_mullo_epi16( ilo, fours );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- unpack_8bit_to_16bit( *i2, ihi, ilo );
- ihi = _mm_mullo_epi16( ihi, sixes );
- ilo = _mm_mullo_epi16( ilo, sixes );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- unpack_8bit_to_16bit( *i3, ihi, ilo );
- *result_h = _mm_sub_epi16( *result_h, ihi );
- *result_h = _mm_sub_epi16( *result_h, ihi );
- *(result_h+1) = _mm_sub_epi16( *(result_h+1), ilo );
- *(result_h+1) = _mm_sub_epi16( *(result_h+1), ilo );
- ihi = _mm_mullo_epi16( ihi, fours );
- ilo = _mm_mullo_epi16( ilo, fours );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- unpack_8bit_to_16bit( *i4, ihi, ilo );
- *result_h = _mm_sub_epi16( *result_h, ihi );
- *(result_h+1) = _mm_sub_epi16( *(result_h+1), ilo );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- }
- }
-
- void convolve_col_p1p1p0m1m1_5x5( const unsigned char* in, int16_t* out, int w, int h ) {
- memset( out, 0, w*h*sizeof(int16_t) );
- using namespace std;
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const int w_chunk = w/16;
- __m128i* i0 = (__m128i*)( in );
- __m128i* i1 = (__m128i*)( in ) + w_chunk*1;
- __m128i* i3 = (__m128i*)( in ) + w_chunk*3;
- __m128i* i4 = (__m128i*)( in ) + w_chunk*4;
- __m128i* result = (__m128i*)( out ) + 4*w_chunk;
- __m128i* end_input = (__m128i*)( in ) + w_chunk*h;
- for( ; i4 != end_input; i0++, i1++, i3++, i4++, result+=2 ) {
- __m128i ilo0, ihi0;
- unpack_8bit_to_16bit( *i0, ihi0, ilo0 );
- __m128i ilo1, ihi1;
- unpack_8bit_to_16bit( *i1, ihi1, ilo1 );
- *result = _mm_add_epi16( ihi0, ihi1 );
- *(result+1) = _mm_add_epi16( ilo0, ilo1 );
- __m128i ilo, ihi;
- unpack_8bit_to_16bit( *i3, ihi, ilo );
- *result = _mm_sub_epi16( *result, ihi );
- *(result+1) = _mm_sub_epi16( *(result+1), ilo );
- unpack_8bit_to_16bit( *i4, ihi, ilo );
- *result = _mm_sub_epi16( *result, ihi );
- *(result+1) = _mm_sub_epi16( *(result+1), ilo );
- }
- }
-
- void convolve_row_p1p1p0m1m1_5x5( const int16_t* in, int16_t* out, int w, int h ) {
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const __m128i* i0 = (const __m128i*)(in);
- const int16_t* i1 = in+1;
- const int16_t* i3 = in+3;
- const int16_t* i4 = in+4;
- int16_t* result = out + 2;
- const int16_t* const end_input = in + w*h;
- for( ; i4+8 < end_input; i0 += 1, i1 += 8, i3 += 8, i4 += 8, result += 8 ) {
- __m128i result_register;
- __m128i i0_register = *i0;
- __m128i i1_register = _mm_loadu_si128( (__m128i*)( i1 ) );
- __m128i i3_register = _mm_loadu_si128( (__m128i*)( i3 ) );
- __m128i i4_register = _mm_loadu_si128( (__m128i*)( i4 ) );
- result_register = _mm_add_epi16( i0_register, i1_register );
- result_register = _mm_sub_epi16( result_register, i3_register );
- result_register = _mm_sub_epi16( result_register, i4_register );
- _mm_storeu_si128( ((__m128i*)( result )), result_register );
- }
- }
-
- void convolve_cols_3x3( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ) {
- using namespace std;
- assert( w % 16 == 0 && "width must be multiple of 16!" );
- const int w_chunk = w/16;
- __m128i* i0 = (__m128i*)( in );
- __m128i* i1 = (__m128i*)( in ) + w_chunk*1;
- __m128i* i2 = (__m128i*)( in ) + w_chunk*2;
- __m128i* result_h = (__m128i*)( out_h ) + 2*w_chunk;
- __m128i* result_v = (__m128i*)( out_v ) + 2*w_chunk;
- __m128i* end_input = (__m128i*)( in ) + w_chunk*h;
- for( ; i2 != end_input; i0++, i1++, i2++, result_v+=2, result_h+=2 ) {
- *result_h = _mm_setzero_si128();
- *(result_h+1) = _mm_setzero_si128();
- *result_v = _mm_setzero_si128();
- *(result_v+1) = _mm_setzero_si128();
- __m128i ilo, ihi;
- unpack_8bit_to_16bit( *i0, ihi, ilo );
- unpack_8bit_to_16bit( *i0, ihi, ilo );
- *result_h = _mm_add_epi16( ihi, *result_h );
- *(result_h+1) = _mm_add_epi16( ilo, *(result_h+1) );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- unpack_8bit_to_16bit( *i1, ihi, ilo );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- unpack_8bit_to_16bit( *i2, ihi, ilo );
- *result_h = _mm_sub_epi16( *result_h, ihi );
- *(result_h+1) = _mm_sub_epi16( *(result_h+1), ilo );
- *result_v = _mm_add_epi16( *result_v, ihi );
- *(result_v+1) = _mm_add_epi16( *(result_v+1), ilo );
- }
- }
- };
-
- void sobel3x3( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ) {
- int16_t* temp_h = (int16_t*)( _mm_malloc( w*h*sizeof( int16_t ), 16 ) );
- int16_t* temp_v = (int16_t*)( _mm_malloc( w*h*sizeof( int16_t ), 16 ) );
- detail::convolve_cols_3x3( in, temp_v, temp_h, w, h );
- detail::convolve_101_row_3x3_16bit( temp_v, out_v, w, h );
- detail::convolve_121_row_3x3_16bit( temp_h, out_h, w, h );
- _mm_free( temp_h );
- _mm_free( temp_v );
- }
-
- void sobel5x5( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ) {
- int16_t* temp_h = (int16_t*)( _mm_malloc( w*h*sizeof( int16_t ), 16 ) );
- int16_t* temp_v = (int16_t*)( _mm_malloc( w*h*sizeof( int16_t ), 16 ) );
- detail::convolve_cols_5x5( in, temp_v, temp_h, w, h );
- detail::convolve_12021_row_5x5_16bit( temp_v, out_v, w, h );
- detail::convolve_14641_row_5x5_16bit( temp_h, out_h, w, h );
- _mm_free( temp_h );
- _mm_free( temp_v );
- }
-
- // -1 -1 0 1 1
- // -1 -1 0 1 1
- // 0 0 0 0 0
- // 1 1 0 -1 -1
- // 1 1 0 -1 -1
- void checkerboard5x5( const uint8_t* in, int16_t* out, int w, int h ) {
- int16_t* temp = (int16_t*)( _mm_malloc( w*h*sizeof( int16_t ), 16 ) );
- detail::convolve_col_p1p1p0m1m1_5x5( in, temp, w, h );
- detail::convolve_row_p1p1p0m1m1_5x5( temp, out, w, h );
- _mm_free( temp );
- }
-
- // -1 -1 -1 -1 -1
- // -1 1 1 1 -1
- // -1 1 8 1 -1
- // -1 1 1 1 -1
- // -1 -1 -1 -1 -1
- void blob5x5( const uint8_t* in, int16_t* out, int w, int h ) {
- int32_t* integral = (int32_t*)( _mm_malloc( w*h*sizeof( int32_t ), 16 ) );
- detail::integral_image( in, integral, w, h );
- int16_t* out_ptr = out + 3 + 3*w;
- int16_t* out_end = out + w * h - 2 - 2*w;
- const int32_t* i00 = integral;
- const int32_t* i50 = integral + 5;
- const int32_t* i05 = integral + 5*w;
- const int32_t* i55 = integral + 5 + 5*w;
- const int32_t* i11 = integral + 1 + 1*w;
- const int32_t* i41 = integral + 4 + 1*w;
- const int32_t* i14 = integral + 1 + 4*w;
- const int32_t* i44 = integral + 4 + 4*w;
- const uint8_t* im22 = in + 3 + 3*w;
- for( ; out_ptr != out_end; out_ptr++, i00++, i50++, i05++, i55++, i11++, i41++, i14++, i44++, im22++ ) {
- int32_t result = 0;
- result = -( *i55 - *i50 - *i05 + *i00 );
- result += 2*( *i44 - *i41 - *i14 + *i11 );
- result += 7* *im22;
- *out_ptr = result;
- }
- _mm_free( integral );
- }
-};
diff --git a/vision/lib/elas/filter.h b/vision/lib/elas/filter.h
deleted file mode 100644
index 4567bd9b7e7ed598cb0c9810f904fd912511ea59..0000000000000000000000000000000000000000
--- a/vision/lib/elas/filter.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
-Copyright 2011. All rights reserved.
-Institute of Measurement and Control Systems
-Karlsruhe Institute of Technology, Germany
-
-This file is part of libelas.
-Authors: Julius Ziegler, Andreas Geiger
-
-libelas 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 any later version.
-
-libelas 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.
-
-You should have received a copy of the GNU General Public License along with
-libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
-Street, Fifth Floor, Boston, MA 02110-1301, USA
-*/
-
-#ifndef __FILTER_H__
-#define __FILTER_H__
-
-#include <emmintrin.h>
-#include <pmmintrin.h>
-
-// define fixed-width datatypes for Visual Studio projects
-//#ifndef _MSC_VER
- #include <stdint.h>
-//#else
-// typedef __int8 int8_t;
-// typedef __int16 int16_t;
-// typedef __int32 int32_t;
-// typedef __int64 int64_t;
-// typedef unsigned __int8 uint8_t;
-// typedef unsigned __int16 uint16_t;
-// typedef unsigned __int32 uint32_t;
-// typedef unsigned __int64 uint64_t;
-//#endif
-
-// fast filters: implements 3x3 and 5x5 sobel filters and
-// 5x5 blob and corner filters based on SSE2/3 instructions
-namespace filter {
-
- // private namespace, public user functions at the bottom of this file
- namespace detail {
- void integral_image( const uint8_t* in, int32_t* out, int w, int h );
- void unpack_8bit_to_16bit( const __m128i a, __m128i& b0, __m128i& b1 );
- void pack_16bit_to_8bit_saturate( const __m128i a0, const __m128i a1, __m128i& b );
-
- // convolve image with a (1,4,6,4,1) row vector. Result is accumulated into output.
- // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_14641_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h );
-
- // convolve image with a (1,2,0,-2,-1) row vector. Result is accumulated into output.
- // This one works on 16bit input and 8bit output.
- // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_12021_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h );
-
- // convolve image with a (1,2,1) row vector. Result is accumulated into output.
- // This one works on 16bit input and 8bit output.
- // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_121_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h );
-
- // convolve image with a (1,0,-1) row vector. Result is accumulated into output.
- // This one works on 16bit input and 8bit output.
- // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255].
- void convolve_101_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h );
-
- void convolve_cols_5x5( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h );
-
- void convolve_col_p1p1p0m1m1_5x5( const unsigned char* in, int16_t* out, int w, int h );
-
- void convolve_row_p1p1p0m1m1_5x5( const int16_t* in, int16_t* out, int w, int h );
-
- void convolve_cols_3x3( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h );
- }
-
- void sobel3x3( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h );
-
- void sobel5x5( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h );
-
- // -1 -1 0 1 1
- // -1 -1 0 1 1
- // 0 0 0 0 0
- // 1 1 0 -1 -1
- // 1 1 0 -1 -1
- void checkerboard5x5( const uint8_t* in, int16_t* out, int w, int h );
-
- // -1 -1 -1 -1 -1
- // -1 1 1 1 -1
- // -1 1 8 1 -1
- // -1 1 1 1 -1
- // -1 -1 -1 -1 -1
- void blob5x5( const uint8_t* in, int16_t* out, int w, int h );
-};
-
-#endif
diff --git a/vision/lib/elas/matrix.cpp b/vision/lib/elas/matrix.cpp
deleted file mode 100644
index 3201dd5b5ecd74eafcbb155eabc426d247120e77..0000000000000000000000000000000000000000
--- a/vision/lib/elas/matrix.cpp
+++ /dev/null
@@ -1,851 +0,0 @@
-/*
-Copyright 2011. All rights reserved.
-Institute of Measurement and Control Systems
-Karlsruhe Institute of Technology, Germany
-
-This file is part of libviso2.
-Authors: Andreas Geiger
-
-libviso2 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 2 of the License, or any later version.
-
-libviso2 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.
-
-You should have received a copy of the GNU General Public License along with
-libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
-Street, Fifth Floor, Boston, MA 02110-1301, USA
-*/
-
-#include "matrix.h"
-#include <algorithm>
-#include <math.h>
-
-#define SWAP(a,b) {temp=a;a=b;b=temp;}
-#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
-static FLOAT sqrarg;
-#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
-static FLOAT maxarg1,maxarg2;
-#define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
-static int32_t iminarg1,iminarg2;
-#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ? (iminarg1) : (iminarg2))
-
-
-using namespace std;
-
-Matrix::Matrix () {
- m = 0;
- n = 0;
- val = 0;
-}
-
-Matrix::Matrix (const int32_t m_,const int32_t n_) {
- allocateMemory(m_,n_);
-}
-
-Matrix::Matrix (const int32_t m_,const int32_t n_,const FLOAT* val_) {
- allocateMemory(m_,n_);
- int32_t k=0;
- for (int32_t i=0; i<m_; i++)
- for (int32_t j=0; j<n_; j++)
- val[i][j] = val_[k++];
-}
-
-Matrix::Matrix (const Matrix &M) {
- allocateMemory(M.m,M.n);
- for (int32_t i=0; i<M.m; i++)
- memcpy(val[i],M.val[i],M.n*sizeof(FLOAT));
-}
-
-Matrix::~Matrix () {
- releaseMemory();
-}
-
-Matrix& Matrix::operator= (const Matrix &M) {
- if (this!=&M) {
- if (M.m!=m || M.n!=n) {
- releaseMemory();
- allocateMemory(M.m,M.n);
- }
- if (M.n>0)
- for (int32_t i=0; i<M.m; i++)
- memcpy(val[i],M.val[i],M.n*sizeof(FLOAT));
- }
- return *this;
-}
-
-void Matrix::getData(FLOAT* val_,int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
- if (i2==-1) i2 = m-1;
- if (j2==-1) j2 = n-1;
- int32_t k=0;
- for (int32_t i=i1; i<=i2; i++)
- for (int32_t j=j1; j<=j2; j++)
- val_[k++] = val[i][j];
-}
-
-Matrix Matrix::getMat(int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
- if (i2==-1) i2 = m-1;
- if (j2==-1) j2 = n-1;
- if (i1<0 || i2>=m || j1<0 || j2>=n || i2<i1 || j2<j1) {
- cerr << "ERROR: Cannot get submatrix [" << i1 << ".." << i2 <<
- "] x [" << j1 << ".." << j2 << "]" <<
- " of a (" << m << "x" << n << ") matrix." << endl;
- exit(0);
- }
- Matrix M(i2-i1+1,j2-j1+1);
- for (int32_t i=0; i<M.m; i++)
- for (int32_t j=0; j<M.n; j++)
- M.val[i][j] = val[i1+i][j1+j];
- return M;
-}
-
-void Matrix::setMat(const Matrix &M,const int32_t i1,const int32_t j1) {
- if (i1<0 || j1<0 || i1+M.m>m || j1+M.n>n) {
- cerr << "ERROR: Cannot set submatrix [" << i1 << ".." << i1+M.m-1 <<
- "] x [" << j1 << ".." << j1+M.n-1 << "]" <<
- " of a (" << m << "x" << n << ") matrix." << endl;
- exit(0);
- }
- for (int32_t i=0; i<M.m; i++)
- for (int32_t j=0; j<M.n; j++)
- val[i1+i][j1+j] = M.val[i][j];
-}
-
-void Matrix::setVal(FLOAT s,int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
- if (i2==-1) i2 = m-1;
- if (j2==-1) j2 = n-1;
- if (i2<i1 || j2<j1) {
- cerr << "ERROR in setVal: Indices must be ordered (i1<=i2, j1<=j2)." << endl;
- exit(0);
- }
- for (int32_t i=i1; i<=i2; i++)
- for (int32_t j=j1; j<=j2; j++)
- val[i][j] = s;
-}
-
-void Matrix::setDiag(FLOAT s,int32_t i1,int32_t i2) {
- if (i2==-1) i2 = min(m-1,n-1);
- for (int32_t i=i1; i<=i2; i++)
- val[i][i] = s;
-}
-
-void Matrix::zero() {
- setVal(0);
-}
-
-Matrix Matrix::extractCols (vector<int> idx) {
- Matrix M(m,idx.size());
- for (int32_t j=0; j<M.n; j++)
- if (idx[j]<n)
- for (int32_t i=0; i<m; i++)
- M.val[i][j] = val[i][idx[j]];
- return M;
-}
-
-Matrix Matrix::eye (const int32_t m) {
- Matrix M(m,m);
- for (int32_t i=0; i<m; i++)
- M.val[i][i] = 1;
- return M;
-}
-
-void Matrix::eye () {
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- val[i][j] = 0;
- for (int32_t i=0; i<min(m,n); i++)
- val[i][i] = 1;
-}
-
-Matrix Matrix::diag (const Matrix &M) {
- if (M.m>1 && M.n==1) {
- Matrix D(M.m,M.m);
- for (int32_t i=0; i<M.m; i++)
- D.val[i][i] = M.val[i][0];
- return D;
- } else if (M.m==1 && M.n>1) {
- Matrix D(M.n,M.n);
- for (int32_t i=0; i<M.n; i++)
- D.val[i][i] = M.val[0][i];
- return D;
- }
- cout << "ERROR: Trying to create diagonal matrix from vector of size (" << M.m << "x" << M.n << ")" << endl;
- exit(0);
-}
-
-Matrix Matrix::reshape(const Matrix &M,int32_t m_,int32_t n_) {
- if (M.m*M.n != m_*n_) {
- cerr << "ERROR: Trying to reshape a matrix of size (" << M.m << "x" << M.n <<
- ") to size (" << m_ << "x" << n_ << ")" << endl;
- exit(0);
- }
- Matrix M2(m_,n_);
- for (int32_t k=0; k<m_*n_; k++) {
- int32_t i1 = k/M.n;
- int32_t j1 = k%M.n;
- int32_t i2 = k/n_;
- int32_t j2 = k%n_;
- M2.val[i2][j2] = M.val[i1][j1];
- }
- return M2;
-}
-
-Matrix Matrix::rotMatX (const FLOAT &angle) {
- FLOAT s = sin(angle);
- FLOAT c = cos(angle);
- Matrix R(3,3);
- R.val[0][0] = +1;
- R.val[1][1] = +c;
- R.val[1][2] = -s;
- R.val[2][1] = +s;
- R.val[2][2] = +c;
- return R;
-}
-
-Matrix Matrix::rotMatY (const FLOAT &angle) {
- FLOAT s = sin(angle);
- FLOAT c = cos(angle);
- Matrix R(3,3);
- R.val[0][0] = +c;
- R.val[0][2] = +s;
- R.val[1][1] = +1;
- R.val[2][0] = -s;
- R.val[2][2] = +c;
- return R;
-}
-
-Matrix Matrix::rotMatZ (const FLOAT &angle) {
- FLOAT s = sin(angle);
- FLOAT c = cos(angle);
- Matrix R(3,3);
- R.val[0][0] = +c;
- R.val[0][1] = -s;
- R.val[1][0] = +s;
- R.val[1][1] = +c;
- R.val[2][2] = +1;
- return R;
-}
-
-Matrix Matrix::operator+ (const Matrix &M) {
- const Matrix &A = *this;
- const Matrix &B = M;
- if (A.m!=B.m || A.n!=B.n) {
- cerr << "ERROR: Trying to add matrices of size (" << A.m << "x" << A.n <<
- ") and (" << B.m << "x" << B.n << ")" << endl;
- exit(0);
- }
- Matrix C(A.m,A.n);
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- C.val[i][j] = A.val[i][j]+B.val[i][j];
- return C;
-}
-
-Matrix Matrix::operator- (const Matrix &M) {
- const Matrix &A = *this;
- const Matrix &B = M;
- if (A.m!=B.m || A.n!=B.n) {
- cerr << "ERROR: Trying to subtract matrices of size (" << A.m << "x" << A.n <<
- ") and (" << B.m << "x" << B.n << ")" << endl;
- exit(0);
- }
- Matrix C(A.m,A.n);
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- C.val[i][j] = A.val[i][j]-B.val[i][j];
- return C;
-}
-
-Matrix Matrix::operator* (const Matrix &M) {
- const Matrix &A = *this;
- const Matrix &B = M;
- if (A.n!=B.m) {
- cerr << "ERROR: Trying to multiply matrices of size (" << A.m << "x" << A.n <<
- ") and (" << B.m << "x" << B.n << ")" << endl;
- exit(0);
- }
- Matrix C(A.m,B.n);
- for (int32_t i=0; i<A.m; i++)
- for (int32_t j=0; j<B.n; j++)
- for (int32_t k=0; k<A.n; k++)
- C.val[i][j] += A.val[i][k]*B.val[k][j];
- return C;
-}
-
-Matrix Matrix::operator* (const FLOAT &s) {
- Matrix C(m,n);
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- C.val[i][j] = val[i][j]*s;
- return C;
-}
-
-Matrix Matrix::operator/ (const Matrix &M) {
- const Matrix &A = *this;
- const Matrix &B = M;
-
- if (A.m==B.m && A.n==B.n) {
- Matrix C(A.m,A.n);
- for (int32_t i=0; i<A.m; i++)
- for (int32_t j=0; j<A.n; j++)
- if (B.val[i][j]!=0)
- C.val[i][j] = A.val[i][j]/B.val[i][j];
- return C;
-
- } else if (A.m==B.m && B.n==1) {
- Matrix C(A.m,A.n);
- for (int32_t i=0; i<A.m; i++)
- for (int32_t j=0; j<A.n; j++)
- if (B.val[i][0]!=0)
- C.val[i][j] = A.val[i][j]/B.val[i][0];
- return C;
-
- } else if (A.n==B.n && B.m==1) {
- Matrix C(A.m,A.n);
- for (int32_t i=0; i<A.m; i++)
- for (int32_t j=0; j<A.n; j++)
- if (B.val[0][j]!=0)
- C.val[i][j] = A.val[i][j]/B.val[0][j];
- return C;
-
- } else {
- cerr << "ERROR: Trying to divide matrices of size (" << A.m << "x" << A.n <<
- ") and (" << B.m << "x" << B.n << ")" << endl;
- exit(0);
- }
-}
-
-Matrix Matrix::operator/ (const FLOAT &s) {
- if (fabs(s)<1e-20) {
- cerr << "ERROR: Trying to divide by zero!" << endl;
- exit(0);
- }
- Matrix C(m,n);
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- C.val[i][j] = val[i][j]/s;
- return C;
-}
-
-Matrix Matrix::operator- () {
- Matrix C(m,n);
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- C.val[i][j] = -val[i][j];
- return C;
-}
-
-Matrix Matrix::operator~ () {
- Matrix C(n,m);
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- C.val[j][i] = val[i][j];
- return C;
-}
-
-FLOAT Matrix::l2norm () {
- FLOAT norm = 0;
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- norm += val[i][j]*val[i][j];
- return sqrt(norm);
-}
-
-FLOAT Matrix::mean () {
- FLOAT mean = 0;
- for (int32_t i=0; i<m; i++)
- for (int32_t j=0; j<n; j++)
- mean += val[i][j];
- return mean/(FLOAT)(m*n);
-}
-
-Matrix Matrix::cross (const Matrix &a, const Matrix &b) {
- if (a.m!=3 || a.n!=1 || b.m!=3 || b.n!=1) {
- cerr << "ERROR: Cross product vectors must be of size (3x1)" << endl;
- exit(0);
- }
- Matrix c(3,1);
- c.val[0][0] = a.val[1][0]*b.val[2][0]-a.val[2][0]*b.val[1][0];
- c.val[1][0] = a.val[2][0]*b.val[0][0]-a.val[0][0]*b.val[2][0];
- c.val[2][0] = a.val[0][0]*b.val[1][0]-a.val[1][0]*b.val[0][0];
- return c;
-}
-
-Matrix Matrix::inv (const Matrix &M) {
- if (M.m!=M.n) {
- cerr << "ERROR: Trying to invert matrix of size (" << M.m << "x" << M.n << ")" << endl;
- exit(0);
- }
- Matrix A(M);
- Matrix B = eye(M.m);
- B.solve(A);
- return B;
-}
-
-bool Matrix::inv () {
- if (m!=n) {
- cerr << "ERROR: Trying to invert matrix of size (" << m << "x" << n << ")" << endl;
- exit(0);
- }
- Matrix A(*this);
- eye();
- solve(A);
- return true;
-}
-
-FLOAT Matrix::det () {
-
- if (m != n) {
- cerr << "ERROR: Trying to compute determinant of a matrix of size (" << m << "x" << n << ")" << endl;
- exit(0);
- }
-
- Matrix A(*this);
- int32_t *idx = (int32_t*)malloc(m*sizeof(int32_t));
- FLOAT d;
- A.lu(idx,d);
- for( int32_t i=0; i<m; i++)
- d *= A.val[i][i];
- free(idx);
-}
-
-bool Matrix::solve (const Matrix &M, FLOAT eps) {
-
- // substitutes
- const Matrix &A = M;
- Matrix &B = *this;
-
- if (A.m != A.n || A.m != B.m || A.m<1 || B.n<1) {
- cerr << "ERROR: Trying to eliminate matrices of size (" << A.m << "x" << A.n <<
- ") and (" << B.m << "x" << B.n << ")" << endl;
- exit(0);
- }
-
- // index vectors for bookkeeping on the pivoting
- int32_t* indxc = new int32_t[m];
- int32_t* indxr = new int32_t[m];
- int32_t* ipiv = new int32_t[m];
-
- // loop variables
- int32_t i, icol, irow, j, k, l, ll;
- FLOAT big, dum, pivinv, temp;
-
- // initialize pivots to zero
- for (j=0;j<m;j++) ipiv[j]=0;
-
- // main loop over the columns to be reduced
- for (i=0;i<m;i++) {
-
- big=0.0;
-
- // search for a pivot element
- for (j=0;j<m;j++)
- if (ipiv[j]!=1)
- for (k=0;k<m;k++)
- if (ipiv[k]==0)
- if (fabs(A.val[j][k])>=big) {
- big=fabs(A.val[j][k]);
- irow=j;
- icol=k;
- }
- ++(ipiv[icol]);
-
- // We now have the pivot element, so we interchange rows, if needed, to put the pivot
- // element on the diagonal. The columns are not physically interchanged, only relabeled.
- if (irow != icol) {
- for (l=0;l<m;l++) SWAP(A.val[irow][l], A.val[icol][l])
- for (l=0;l<n;l++) SWAP(B.val[irow][l], B.val[icol][l])
- }
-
- indxr[i]=irow; // We are now ready to divide the pivot row by the
- indxc[i]=icol; // pivot element, located at irow and icol.
-
- // check for singularity
- if (fabs(A.val[icol][icol]) < eps) {
- delete[] indxc;
- delete[] indxr;
- delete[] ipiv;
- return false;
- }
-
- pivinv=1.0/A.val[icol][icol];
- A.val[icol][icol]=1.0;
- for (l=0;l<m;l++) A.val[icol][l] *= pivinv;
- for (l=0;l<n;l++) B.val[icol][l] *= pivinv;
-
- // Next, we reduce the rows except for the pivot one
- for (ll=0;ll<m;ll++)
- if (ll!=icol) {
- dum = A.val[ll][icol];
- A.val[ll][icol] = 0.0;
- for (l=0;l<m;l++) A.val[ll][l] -= A.val[icol][l]*dum;
- for (l=0;l<n;l++) B.val[ll][l] -= B.val[icol][l]*dum;
- }
- }
-
- // This is the end of the main loop over columns of the reduction. It only remains to unscramble
- // the solution in view of the column interchanges. We do this by interchanging pairs of
- // columns in the reverse order that the permutation was built up.
- for (l=m-1;l>=0;l--) {
- if (indxr[l]!=indxc[l])
- for (k=0;k<m;k++)
- SWAP(A.val[k][indxr[l]], A.val[k][indxc[l]])
- }
-
- // success
- delete[] indxc;
- delete[] indxr;
- delete[] ipiv;
- return true;
-}
-
-// Given a matrix a[1..n][1..n], this routine replaces it by the LU decomposition of a rowwise
-// permutation of itself. a and n are input. a is output, arranged as in equation (2.3.14) above;
-// indx[1..n] is an output vector that records the row permutation effected by the partial
-// pivoting; d is output as ±1 depending on whether the number of row interchanges was even
-// or odd, respectively. This routine is used in combination with lubksb to solve linear equations
-// or invert a matrix.
-
-bool Matrix::lu(int32_t *idx, FLOAT &d, FLOAT eps) {
-
- if (m != n) {
- cerr << "ERROR: Trying to LU decompose a matrix of size (" << m << "x" << n << ")" << endl;
- exit(0);
- }
-
- int32_t i,imax,j,k;
- FLOAT big,dum,sum,temp;
- FLOAT* vv = (FLOAT*)malloc(n*sizeof(FLOAT)); // vv stores the implicit scaling of each row.
- d = 1.0;
- for (i=0; i<n; i++) { // Loop over rows to get the implicit scaling information.
- big = 0.0;
- for (j=0; j<n; j++)
- if ((temp=fabs(val[i][j]))>big)
- big = temp;
- if (big == 0.0) { // No nonzero largest element.
- free(vv);
- return false;
- }
- vv[i] = 1.0/big; // Save the scaling.
- }
- for (j=0; j<n; j++) { // This is the loop over columns of Crout’s method.
- for (i=0; i<j; i++) { // This is equation (2.3.12) except for i = j.
- sum = val[i][j];
- for (k=0; k<i; k++)
- sum -= val[i][k]*val[k][j];
- val[i][j] = sum;
- }
- big = 0.0; // Initialize the search for largest pivot element.
- for (i=j; i<n; i++) {
- sum = val[i][j];
- for (k=0; k<j; k++)
- sum -= val[i][k]*val[k][j];
- val[i][j] = sum;
- if ( (dum=vv[i]*fabs(sum))>=big) {
- big = dum;
- imax = i;
- }
- }
- if (j!=imax) { // Do we need to interchange rows?
- for (k=0; k<n; k++) { // Yes, do so...
- dum = val[imax][k];
- val[imax][k] = val[j][k];
- val[j][k] = dum;
- }
- d = -d; // ...and change the parity of d.
- vv[imax]=vv[j]; // Also interchange the scale factor.
- }
- idx[j] = imax;
- if (j!=n-1) { // Now, finally, divide by the pivot element.
- dum = 1.0/val[j][j];
- for (i=j+1; i<n; i++)
- val[i][j] *= dum;
- }
- } // Go back for the next column in the reduction.
-
- // success
- free(vv);
- return true;
-}
-
-// Given a matrix M/A[1..m][1..n], this routine computes its singular value decomposition, M/A =
-// U·W·V T. Thematrix U replaces a on output. The diagonal matrix of singular values W is output
-// as a vector w[1..n]. Thematrix V (not the transpose V T ) is output as v[1..n][1..n].
-void Matrix::svd(Matrix &U2,Matrix &W,Matrix &V) {
-
- Matrix U = Matrix(*this);
- U2 = Matrix(m,m);
- V = Matrix(n,n);
-
- FLOAT* w = (FLOAT*)malloc(n*sizeof(FLOAT));
- FLOAT* rv1 = (FLOAT*)malloc(n*sizeof(FLOAT));
-
- int32_t flag,i,its,j,jj,k,l,nm;
- FLOAT anorm,c,f,g,h,s,scale,x,y,z;
-
- g = scale = anorm = 0.0; // Householder reduction to bidiagonal form.
- for (i=0;i<n;i++) {
- l = i+1;
- rv1[i] = scale*g;
- g = s = scale = 0.0;
- if (i < m) {
- for (k=i;k<m;k++) scale += fabs(U.val[k][i]);
- if (scale) {
- for (k=i;k<m;k++) {
- U.val[k][i] /= scale;
- s += U.val[k][i]*U.val[k][i];
- }
- f = U.val[i][i];
- g = -SIGN(sqrt(s),f);
- h = f*g-s;
- U.val[i][i] = f-g;
- for (j=l;j<n;j++) {
- for (s=0.0,k=i;k<m;k++) s += U.val[k][i]*U.val[k][j];
- f = s/h;
- for (k=i;k<m;k++) U.val[k][j] += f*U.val[k][i];
- }
- for (k=i;k<m;k++) U.val[k][i] *= scale;
- }
- }
- w[i] = scale*g;
- g = s = scale = 0.0;
- if (i<m && i!=n-1) {
- for (k=l;k<n;k++) scale += fabs(U.val[i][k]);
- if (scale) {
- for (k=l;k<n;k++) {
- U.val[i][k] /= scale;
- s += U.val[i][k]*U.val[i][k];
- }
- f = U.val[i][l];
- g = -SIGN(sqrt(s),f);
- h = f*g-s;
- U.val[i][l] = f-g;
- for (k=l;k<n;k++) rv1[k] = U.val[i][k]/h;
- for (j=l;j<m;j++) {
- for (s=0.0,k=l;k<n;k++) s += U.val[j][k]*U.val[i][k];
- for (k=l;k<n;k++) U.val[j][k] += s*rv1[k];
- }
- for (k=l;k<n;k++) U.val[i][k] *= scale;
- }
- }
- anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
- }
- for (i=n-1;i>=0;i--) { // Accumulation of right-hand transformations.
- if (i<n-1) {
- if (g) {
- for (j=l;j<n;j++) // Double division to avoid possible underflow.
- V.val[j][i]=(U.val[i][j]/U.val[i][l])/g;
- for (j=l;j<n;j++) {
- for (s=0.0,k=l;k<n;k++) s += U.val[i][k]*V.val[k][j];
- for (k=l;k<n;k++) V.val[k][j] += s*V.val[k][i];
- }
- }
- for (j=l;j<n;j++) V.val[i][j] = V.val[j][i] = 0.0;
- }
- V.val[i][i] = 1.0;
- g = rv1[i];
- l = i;
- }
- for (i=IMIN(m,n)-1;i>=0;i--) { // Accumulation of left-hand transformations.
- l = i+1;
- g = w[i];
- for (j=l;j<n;j++) U.val[i][j] = 0.0;
- if (g) {
- g = 1.0/g;
- for (j=l;j<n;j++) {
- for (s=0.0,k=l;k<m;k++) s += U.val[k][i]*U.val[k][j];
- f = (s/U.val[i][i])*g;
- for (k=i;k<m;k++) U.val[k][j] += f*U.val[k][i];
- }
- for (j=i;j<m;j++) U.val[j][i] *= g;
- } else for (j=i;j<m;j++) U.val[j][i]=0.0;
- ++U.val[i][i];
- }
- for (k=n-1;k>=0;k--) { // Diagonalization of the bidiagonal form: Loop over singular values,
- for (its=0;its<30;its++) { // and over allowed iterations.
- flag = 1;
- for (l=k;l>=0;l--) { // Test for splitting.
- nm = l-1;
- if ((FLOAT)(fabs(rv1[l])+anorm) == anorm) { flag = 0; break; }
- if ((FLOAT)(fabs( w[nm])+anorm) == anorm) { break; }
- }
- if (flag) {
- c = 0.0; // Cancellation of rv1[l], if l > 1.
- s = 1.0;
- for (i=l;i<=k;i++) {
- f = s*rv1[i];
- rv1[i] = c*rv1[i];
- if ((FLOAT)(fabs(f)+anorm) == anorm) break;
- g = w[i];
- h = pythag(f,g);
- w[i] = h;
- h = 1.0/h;
- c = g*h;
- s = -f*h;
- for (j=0;j<m;j++) {
- y = U.val[j][nm];
- z = U.val[j][i];
- U.val[j][nm] = y*c+z*s;
- U.val[j][i] = z*c-y*s;
- }
- }
- }
- z = w[k];
- if (l==k) { // Convergence.
- if (z<0.0) { // Singular value is made nonnegative.
- w[k] = -z;
- for (j=0;j<n;j++) V.val[j][k] = -V.val[j][k];
- }
- break;
- }
- if (its == 29)
- cerr << "ERROR in SVD: No convergence in 30 iterations" << endl;
- x = w[l]; // Shift from bottom 2-by-2 minor.
- nm = k-1;
- y = w[nm];
- g = rv1[nm];
- h = rv1[k];
- f = ((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
- g = pythag(f,1.0);
- f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
- c = s = 1.0; // Next QR transformation:
- for (j=l;j<=nm;j++) {
- i = j+1;
- g = rv1[i];
- y = w[i];
- h = s*g;
- g = c*g;
- z = pythag(f,h);
- rv1[j] = z;
- c = f/z;
- s = h/z;
- f = x*c+g*s;
- g = g*c-x*s;
- h = y*s;
- y *= c;
- for (jj=0;jj<n;jj++) {
- x = V.val[jj][j];
- z = V.val[jj][i];
- V.val[jj][j] = x*c+z*s;
- V.val[jj][i] = z*c-x*s;
- }
- z = pythag(f,h);
- w[j] = z; // Rotation can be arbitrary if z = 0.
- if (z) {
- z = 1.0/z;
- c = f*z;
- s = h*z;
- }
- f = c*g+s*y;
- x = c*y-s*g;
- for (jj=0;jj<m;jj++) {
- y = U.val[jj][j];
- z = U.val[jj][i];
- U.val[jj][j] = y*c+z*s;
- U.val[jj][i] = z*c-y*s;
- }
- }
- rv1[l] = 0.0;
- rv1[k] = f;
- w[k] = x;
- }
- }
-
- // sort singular values and corresponding columns of u and v
- // by decreasing magnitude. Also, signs of corresponding columns are
- // flipped so as to maximize the number of positive elements.
- int32_t s2,inc=1;
- FLOAT sw;
- FLOAT* su = (FLOAT*)malloc(m*sizeof(FLOAT));
- FLOAT* sv = (FLOAT*)malloc(n*sizeof(FLOAT));
- do { inc *= 3; inc++; } while (inc <= n);
- do {
- inc /= 3;
- for (i=inc;i<n;i++) {
- sw = w[i];
- for (k=0;k<m;k++) su[k] = U.val[k][i];
- for (k=0;k<n;k++) sv[k] = V.val[k][i];
- j = i;
- while (w[j-inc] < sw) {
- w[j] = w[j-inc];
- for (k=0;k<m;k++) U.val[k][j] = U.val[k][j-inc];
- for (k=0;k<n;k++) V.val[k][j] = V.val[k][j-inc];
- j -= inc;
- if (j < inc) break;
- }
- w[j] = sw;
- for (k=0;k<m;k++) U.val[k][j] = su[k];
- for (k=0;k<n;k++) V.val[k][j] = sv[k];
- }
- } while (inc > 1);
- for (k=0;k<n;k++) { // flip signs
- s2=0;
- for (i=0;i<m;i++) if (U.val[i][k] < 0.0) s2++;
- for (j=0;j<n;j++) if (V.val[j][k] < 0.0) s2++;
- if (s2 > (m+n)/2) {
- for (i=0;i<m;i++) U.val[i][k] = -U.val[i][k];
- for (j=0;j<n;j++) V.val[j][k] = -V.val[j][k];
- }
- }
-
- // create vector and copy singular values
- W = Matrix(min(m,n),1,w);
-
- // extract mxm submatrix U
- U2.setMat(U.getMat(0,0,m-1,min(m-1,n-1)),0,0);
-
- // release temporary memory
- free(w);
- free(rv1);
- free(su);
- free(sv);
-}
-
-ostream& operator<< (ostream& out,const Matrix& M) {
- if (M.m==0 || M.n==0) {
- out << "[empty matrix]";
- } else {
- char buffer[1024];
- for (int32_t i=0; i<M.m; i++) {
- for (int32_t j=0; j<M.n; j++) {
- sprintf(buffer,"%12.7f ",M.val[i][j]);
- out << buffer;
- }
- if (i<M.m-1)
- out << endl;
- }
- }
- return out;
-}
-
-void Matrix::allocateMemory (const int32_t m_,const int32_t n_) {
- m = abs(m_); n = abs(n_);
- if (m==0 || n==0) {
- val = 0;
- return;
- }
- val = (FLOAT**)malloc(m*sizeof(FLOAT*));
- val[0] = (FLOAT*)calloc(m*n,sizeof(FLOAT));
- for(int32_t i=1; i<m; i++)
- val[i] = val[i-1]+n;
-}
-
-void Matrix::releaseMemory () {
- if (val!=0) {
- free(val[0]);
- free(val);
- }
-}
-
-FLOAT Matrix::pythag(FLOAT a,FLOAT b) {
- FLOAT absa,absb;
- absa = fabs(a);
- absb = fabs(b);
- if (absa > absb)
- return absa*sqrt(1.0+SQR(absb/absa));
- else
- return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
-}
-
diff --git a/vision/lib/elas/matrix.h b/vision/lib/elas/matrix.h
deleted file mode 100644
index c559de7e96b669a3616cf17060c2818d3b71381c..0000000000000000000000000000000000000000
--- a/vision/lib/elas/matrix.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/*
-Copyright 2011. All rights reserved.
-Institute of Measurement and Control Systems
-Karlsruhe Institute of Technology, Germany
-
-This file is part of libviso2.
-Authors: Andreas Geiger
-
-libviso2 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 2 of the License, or any later version.
-
-libviso2 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.
-
-You should have received a copy of the GNU General Public License along with
-libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
-Street, Fifth Floor, Boston, MA 02110-1301, USA
-*/
-
-#ifndef MATRIX_H
-#define MATRIX_H
-
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <iostream>
-#include <vector>
-
-#ifndef _MSC_VER
- #include <stdint.h>
-#else
-// typedef __int8 int8_t;
-// typedef __int16 int16_t;
-// typedef __int32 int32_t;
-// typedef __int64 int64_t;
-// typedef unsigned __int8 uint8_t;
-// typedef unsigned __int16 uint16_t;
-// typedef unsigned __int32 uint32_t;
-// typedef unsigned __int64 uint64_t;
-#endif
-
-#define endll endl << endl // double end line definition
-
-typedef double FLOAT; // double precision
-//typedef float FLOAT; // single precision
-
-class Matrix {
-
-public:
-
- // constructor / deconstructor
- Matrix (); // init empty 0x0 matrix
- Matrix (const int32_t m,const int32_t n); // init empty mxn matrix
- Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val'
- Matrix (const Matrix &M); // creates deepcopy of M
- ~Matrix ();
-
- // assignment operator, copies contents of M
- Matrix& operator= (const Matrix &M);
-
- // copies submatrix of M into array 'val', default values copy whole row/column/matrix
- void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);
-
- // set or get submatrices of current matrix
- Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1);
- void setMat(const Matrix &M,const int32_t i,const int32_t j);
-
- // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix
- void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);
-
- // set (part of) diagonal to scalar, -1 as end replaces whole diagonal
- void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1);
-
- // clear matrix
- void zero();
-
- // extract columns with given index
- Matrix extractCols (std::vector<int> idx);
-
- // create identity matrix
- static Matrix eye (const int32_t m);
- void eye ();
-
- // create diagonal matrix with nx1 or 1xn matrix M as elements
- static Matrix diag(const Matrix &M);
-
- // returns the m-by-n matrix whose elements are taken column-wise from M
- static Matrix reshape(const Matrix &M,int32_t m,int32_t n);
-
- // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix)
- static Matrix rotMatX(const FLOAT &angle);
- static Matrix rotMatY(const FLOAT &angle);
- static Matrix rotMatZ(const FLOAT &angle);
-
- // simple arithmetic operations
- Matrix operator+ (const Matrix &M); // add matrix
- Matrix operator- (const Matrix &M); // subtract matrix
- Matrix operator* (const Matrix &M); // multiply with matrix
- Matrix operator* (const FLOAT &s); // multiply with scalar
- Matrix operator/ (const Matrix &M); // divide elementwise by matrix (or vector)
- Matrix operator/ (const FLOAT &s); // divide by scalar
- Matrix operator- (); // negative matrix
- Matrix operator~ (); // transpose
- FLOAT l2norm (); // euclidean norm (vectors) / frobenius norm (matrices)
- FLOAT mean (); // mean of all elements in matrix
-
- // complex arithmetic operations
- static Matrix cross (const Matrix &a, const Matrix &b); // cross product of two vectors
- static Matrix inv (const Matrix &M); // invert matrix M
- bool inv (); // invert this matrix
- FLOAT det (); // returns determinant of matrix
- bool solve (const Matrix &M,FLOAT eps=1e-20); // solve linear system M*x=B, replaces *this and M
- bool lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20); // replace *this by lower upper decomposition
- void svd(Matrix &U,Matrix &W,Matrix &V); // singular value decomposition *this = U*diag(W)*V^T
-
- // print matrix to stream
- friend std::ostream& operator<< (std::ostream& out,const Matrix& M);
-
- // direct data access
- FLOAT **val;
- int32_t m,n;
-
-private:
-
- void allocateMemory (const int32_t m_,const int32_t n_);
- void releaseMemory ();
- inline FLOAT pythag(FLOAT a,FLOAT b);
-
-};
-
-#endif // MATRIX_H
diff --git a/vision/lib/elas/triangle.cpp b/vision/lib/elas/triangle.cpp
deleted file mode 100644
index 14d4a2d03c06537a24547bb8c6c44526964ab207..0000000000000000000000000000000000000000
--- a/vision/lib/elas/triangle.cpp
+++ /dev/null
@@ -1,8635 +0,0 @@
-/*****************************************************************************/
-/* */
-/* 888888888 ,o, / 888 */
-/* 888 88o88o " o8888o 88o8888o o88888o 888 o88888o */
-/* 888 888 888 88b 888 888 888 888 888 d888 88b */
-/* 888 888 888 o88^o888 888 888 "88888" 888 8888oo888 */
-/* 888 888 888 C888 888 888 888 / 888 q888 */
-/* 888 888 888 "88o^888 888 888 Cb 888 "88oooo" */
-/* "8oo8D */
-/* */
-/* A Two-Dimensional Quality Mesh Generator and Delaunay Triangulator. */
-/* (triangle.c) */
-/* */
-/* Version 1.6 */
-/* July 28, 2005 */
-/* */
-/* Copyright 1993, 1995, 1997, 1998, 2002, 2005 */
-/* Jonathan Richard Shewchuk */
-/* 2360 Woolsey #H */
-/* Berkeley, California 94705-1927 */
-/* jrs@cs.berkeley.edu */
-/* */
-/* Modified by Andreas Geiger, 2011 */
-/* */
-/* This program may be freely redistributed under the condition that the */
-/* copyright notices (including this entire header and the copyright */
-/* notice printed when the `-h' switch is selected) are not removed, and */
-/* no compensation is received. Private, research, and institutional */
-/* use is free. You may distribute modified versions of this code UNDER */
-/* THE CONDITION THAT THIS CODE AND ANY MODIFICATIONS MADE TO IT IN THE */
-/* SAME FILE REMAIN UNDER COPYRIGHT OF THE ORIGINAL AUTHOR, BOTH SOURCE */
-/* AND OBJECT CODE ARE MADE FREELY AVAILABLE WITHOUT CHARGE, AND CLEAR */
-/* NOTICE IS GIVEN OF THE MODIFICATIONS. Distribution of this code as */
-/* part of a commercial system is permissible ONLY BY DIRECT ARRANGEMENT */
-/* WITH THE AUTHOR. (If you are not directly supplying this code to a */
-/* customer, and you are instead telling them how they can obtain it for */
-/* free, then you are not required to make any arrangement with me.) */
-/* */
-/* Hypertext instructions for Triangle are available on the Web at */
-/* */
-/* http://www.cs.cmu.edu/~quake/triangle.html */
-/* */
-/* Disclaimer: Neither I nor Carnegie Mellon warrant this code in any way */
-/* whatsoever. This code is provided "as-is". Use at your own risk. */
-/* */
-/* Some of the references listed below are marked with an asterisk. [*] */
-/* These references are available for downloading from the Web page */
-/* */
-/* http://www.cs.cmu.edu/~quake/triangle.research.html */
-/* */
-/* Three papers discussing aspects of Triangle are available. A short */
-/* overview appears in "Triangle: Engineering a 2D Quality Mesh */
-/* Generator and Delaunay Triangulator," in Applied Computational */
-/* Geometry: Towards Geometric Engineering, Ming C. Lin and Dinesh */
-/* Manocha, editors, Lecture Notes in Computer Science volume 1148, */
-/* pages 203-222, Springer-Verlag, Berlin, May 1996 (from the First ACM */
-/* Workshop on Applied Computational Geometry). [*] */
-/* */
-/* The algorithms are discussed in the greatest detail in "Delaunay */
-/* Refinement Algorithms for Triangular Mesh Generation," Computational */
-/* Geometry: Theory and Applications 22(1-3):21-74, May 2002. [*] */
-/* */
-/* More detail about the data structures may be found in my dissertation: */
-/* "Delaunay Refinement Mesh Generation," Ph.D. thesis, Technical Report */
-/* CMU-CS-97-137, School of Computer Science, Carnegie Mellon University, */
-/* Pittsburgh, Pennsylvania, 18 May 1997. [*] */
-/* */
-/* Triangle was created as part of the Quake Project in the School of */
-/* Computer Science at Carnegie Mellon University. For further */
-/* information, see Hesheng Bao, Jacobo Bielak, Omar Ghattas, Loukas F. */
-/* Kallivokas, David R. O'Hallaron, Jonathan R. Shewchuk, and Jifeng Xu, */
-/* "Large-scale Simulation of Elastic Wave Propagation in Heterogeneous */
-/* Media on Parallel Computers," Computer Methods in Applied Mechanics */
-/* and Engineering 152(1-2):85-102, 22 January 1998. */
-/* */
-/* Triangle's Delaunay refinement algorithm for quality mesh generation is */
-/* a hybrid of one due to Jim Ruppert, "A Delaunay Refinement Algorithm */
-/* for Quality 2-Dimensional Mesh Generation," Journal of Algorithms */
-/* 18(3):548-585, May 1995 [*], and one due to L. Paul Chew, "Guaranteed- */
-/* Quality Mesh Generation for Curved Surfaces," Proceedings of the Ninth */
-/* Annual Symposium on Computational Geometry (San Diego, California), */
-/* pages 274-280, Association for Computing Machinery, May 1993, */
-/* http://portal.acm.org/citation.cfm?id=161150 . */
-/* */
-/* The Delaunay refinement algorithm has been modified so that it meshes */
-/* domains with small input angles well, as described in Gary L. Miller, */
-/* Steven E. Pav, and Noel J. Walkington, "When and Why Ruppert's */
-/* Algorithm Works," Twelfth International Meshing Roundtable, pages */
-/* 91-102, Sandia National Laboratories, September 2003. [*] */
-/* */
-/* My implementation of the divide-and-conquer and incremental Delaunay */
-/* triangulation algorithms follows closely the presentation of Guibas */
-/* and Stolfi, even though I use a triangle-based data structure instead */
-/* of their quad-edge data structure. (In fact, I originally implemented */
-/* Triangle using the quad-edge data structure, but the switch to a */
-/* triangle-based data structure sped Triangle by a factor of two.) The */
-/* mesh manipulation primitives and the two aforementioned Delaunay */
-/* triangulation algorithms are described by Leonidas J. Guibas and Jorge */
-/* Stolfi, "Primitives for the Manipulation of General Subdivisions and */
-/* the Computation of Voronoi Diagrams," ACM Transactions on Graphics */
-/* 4(2):74-123, April 1985, http://portal.acm.org/citation.cfm?id=282923 .*/
-/* */
-/* Their O(n log n) divide-and-conquer algorithm is adapted from Der-Tsai */
-/* Lee and Bruce J. Schachter, "Two Algorithms for Constructing the */
-/* Delaunay Triangulation," International Journal of Computer and */
-/* Information Science 9(3):219-242, 1980. Triangle's improvement of the */
-/* divide-and-conquer algorithm by alternating between vertical and */
-/* horizontal cuts was introduced by Rex A. Dwyer, "A Faster Divide-and- */
-/* Conquer Algorithm for Constructing Delaunay Triangulations," */
-/* Algorithmica 2(2):137-151, 1987. */
-/* */
-/* The incremental insertion algorithm was first proposed by C. L. Lawson, */
-/* "Software for C1 Surface Interpolation," in Mathematical Software III, */
-/* John R. Rice, editor, Academic Press, New York, pp. 161-194, 1977. */
-/* For point location, I use the algorithm of Ernst P. Mucke, Isaac */
-/* Saias, and Binhai Zhu, "Fast Randomized Point Location Without */
-/* Preprocessing in Two- and Three-Dimensional Delaunay Triangulations," */
-/* Proceedings of the Twelfth Annual Symposium on Computational Geometry, */
-/* ACM, May 1996. [*] If I were to randomize the order of vertex */
-/* insertion (I currently don't bother), their result combined with the */
-/* result of Kenneth L. Clarkson and Peter W. Shor, "Applications of */
-/* Random Sampling in Computational Geometry II," Discrete & */
-/* Computational Geometry 4(1):387-421, 1989, would yield an expected */
-/* O(n^{4/3}) bound on running time. */
-/* */
-/* The O(n log n) sweepline Delaunay triangulation algorithm is taken from */
-/* Steven Fortune, "A Sweepline Algorithm for Voronoi Diagrams", */
-/* Algorithmica 2(2):153-174, 1987. A random sample of edges on the */
-/* boundary of the triangulation are maintained in a splay tree for the */
-/* purpose of point location. Splay trees are described by Daniel */
-/* Dominic Sleator and Robert Endre Tarjan, "Self-Adjusting Binary Search */
-/* Trees," Journal of the ACM 32(3):652-686, July 1985, */
-/* http://portal.acm.org/citation.cfm?id=3835 . */
-/* */
-/* The algorithms for exact computation of the signs of determinants are */
-/* described in Jonathan Richard Shewchuk, "Adaptive Precision Floating- */
-/* Point Arithmetic and Fast Robust Geometric Predicates," Discrete & */
-/* Computational Geometry 18(3):305-363, October 1997. (Also available */
-/* as Technical Report CMU-CS-96-140, School of Computer Science, */
-/* Carnegie Mellon University, Pittsburgh, Pennsylvania, May 1996.) [*] */
-/* An abbreviated version appears as Jonathan Richard Shewchuk, "Robust */
-/* Adaptive Floating-Point Geometric Predicates," Proceedings of the */
-/* Twelfth Annual Symposium on Computational Geometry, ACM, May 1996. [*] */
-/* Many of the ideas for my exact arithmetic routines originate with */
-/* Douglas M. Priest, "Algorithms for Arbitrary Precision Floating Point */
-/* Arithmetic," Tenth Symposium on Computer Arithmetic, pp. 132-143, IEEE */
-/* Computer Society Press, 1991. [*] Many of the ideas for the correct */
-/* evaluation of the signs of determinants are taken from Steven Fortune */
-/* and Christopher J. Van Wyk, "Efficient Exact Arithmetic for Computa- */
-/* tional Geometry," Proceedings of the Ninth Annual Symposium on */
-/* Computational Geometry, ACM, pp. 163-172, May 1993, and from Steven */
-/* Fortune, "Numerical Stability of Algorithms for 2D Delaunay Triangu- */
-/* lations," International Journal of Computational Geometry & Applica- */
-/* tions 5(1-2):193-213, March-June 1995. */
-/* */
-/* The method of inserting new vertices off-center (not precisely at the */
-/* circumcenter of every poor-quality triangle) is from Alper Ungor, */
-/* "Off-centers: A New Type of Steiner Points for Computing Size-Optimal */
-/* Quality-Guaranteed Delaunay Triangulations," Proceedings of LATIN */
-/* 2004 (Buenos Aires, Argentina), April 2004. */
-/* */
-/* For definitions of and results involving Delaunay triangulations, */
-/* constrained and conforming versions thereof, and other aspects of */
-/* triangular mesh generation, see the excellent survey by Marshall Bern */
-/* and David Eppstein, "Mesh Generation and Optimal Triangulation," in */
-/* Computing and Euclidean Geometry, Ding-Zhu Du and Frank Hwang, */
-/* editors, World Scientific, Singapore, pp. 23-90, 1992. [*] */
-/* */
-/* The time for incrementally adding PSLG (planar straight line graph) */
-/* segments to create a constrained Delaunay triangulation is probably */
-/* O(t^2) per segment in the worst case and O(t) per segment in the */
-/* common case, where t is the number of triangles that intersect the */
-/* segment before it is inserted. This doesn't count point location, */
-/* which can be much more expensive. I could improve this to O(d log d) */
-/* time, but d is usually quite small, so it's not worth the bother. */
-/* (This note does not apply when the -s switch is used, invoking a */
-/* different method is used to insert segments.) */
-/* */
-/* The time for deleting a vertex from a Delaunay triangulation is O(d^2) */
-/* in the worst case and O(d) in the common case, where d is the degree */
-/* of the vertex being deleted. I could improve this to O(d log d) time, */
-/* but d is usually quite small, so it's not worth the bother. */
-/* */
-/* Ruppert's Delaunay refinement algorithm typically generates triangles */
-/* at a linear rate (constant time per triangle) after the initial */
-/* triangulation is formed. There may be pathological cases where */
-/* quadratic time is required, but these never arise in practice. */
-/* */
-/* The geometric predicates (circumcenter calculations, segment */
-/* intersection formulae, etc.) appear in my "Lecture Notes on Geometric */
-/* Robustness" at http://www.cs.berkeley.edu/~jrs/mesh . */
-/* */
-/* If you make any improvements to this code, please please please let me */
-/* know, so that I may obtain the improvements. Even if you don't change */
-/* the code, I'd still love to hear what it's being used for. */
-/* */
-/*****************************************************************************/
-
-/* Maximum number of characters in a file name (including the null). */
-
-#define FILENAMESIZE 2048
-
-/* Maximum number of characters in a line read from a file (including the */
-/* null). */
-
-#define INPUTLINESIZE 1024
-
-/* For efficiency, a variety of data structures are allocated in bulk. The */
-/* following constants determine how many of each structure is allocated */
-/* at once. */
-
-#define TRIPERBLOCK 4092 /* Number of triangles allocated at once. */
-#define SUBSEGPERBLOCK 508 /* Number of subsegments allocated at once. */
-#define VERTEXPERBLOCK 4092 /* Number of vertices allocated at once. */
-#define VIRUSPERBLOCK 1020 /* Number of virus triangles allocated at once. */
-#define BADSUBSEGPERBLOCK 252 /* Number of encroached subsegments allocated at once. */
-#define BADTRIPERBLOCK 4092 /* Number of skinny triangles allocated at once. */
-#define FLIPSTACKERPERBLOCK 252 /* Number of flipped triangles allocated at once. */
-#define SPLAYNODEPERBLOCK 508 /* Number of splay tree nodes allocated at once. */
-
-/* The vertex types. A DEADVERTEX has been deleted entirely. An */
-/* UNDEADVERTEX is not part of the mesh, but is written to the output */
-/* .node file and affects the node indexing in the other output files. */
-
-#define INPUTVERTEX 0
-#define SEGMENTVERTEX 1
-#define FREEVERTEX 2
-#define DEADVERTEX -32768
-#define UNDEADVERTEX -32767
-
-/* Two constants for algorithms based on random sampling. Both constants */
-/* have been chosen empirically to optimize their respective algorithms. */
-
-/* Used for the point location scheme of Mucke, Saias, and Zhu, to decide */
-/* how large a random sample of triangles to inspect. */
-
-#define SAMPLEFACTOR 11
-
-/* Used in Fortune's sweepline Delaunay algorithm to determine what fraction */
-/* of boundary edges should be maintained in the splay tree for point */
-/* location on the front. */
-
-#define SAMPLERATE 10
-
-/* A number that speaks for itself, every kissable digit. */
-
-#define PI 3.141592653589793238462643383279502884197169399375105820974944592308
-
-/* Another fave. */
-
-#define SQUAREROOTTWO 1.4142135623730950488016887242096980785696718753769480732
-
-/* And here's one for those of you who are intimidated by math. */
-
-#define ONETHIRD 0.333333333333333333333333333333333333333333333333333333333333
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include "triangle.h"
-
-/* Labels that signify the result of point location. The result of a */
-/* search indicates that the point falls in the interior of a triangle, on */
-/* an edge, on a vertex, or outside the mesh. */
-
-enum locateresult {INTRIANGLE, ONEDGE, ONVERTEX, OUTSIDE};
-
-/* Labels that signify the result of vertex insertion. The result indicates */
-/* that the vertex was inserted with complete success, was inserted but */
-/* encroaches upon a subsegment, was not inserted because it lies on a */
-/* segment, or was not inserted because another vertex occupies the same */
-/* location. */
-
-enum insertvertexresult {SUCCESSFULVERTEX, ENCROACHINGVERTEX, VIOLATINGVERTEX,
- DUPLICATEVERTEX};
-
-/* Labels that signify the result of direction finding. The result */
-/* indicates that a segment connecting the two query points falls within */
-/* the direction triangle, along the left edge of the direction triangle, */
-/* or along the right edge of the direction triangle. */
-
-enum finddirectionresult {WITHIN, LEFTCOLLINEAR, RIGHTCOLLINEAR};
-
-/*****************************************************************************/
-/* */
-/* The basic mesh data structures */
-/* */
-/* There are three: vertices, triangles, and subsegments (abbreviated */
-/* `subseg'). These three data structures, linked by pointers, comprise */
-/* the mesh. A vertex simply represents a mesh vertex and its properties. */
-/* A triangle is a triangle. A subsegment is a special data structure used */
-/* to represent an impenetrable edge of the mesh (perhaps on the outer */
-/* boundary, on the boundary of a hole, or part of an internal boundary */
-/* separating two triangulated regions). Subsegments represent boundaries, */
-/* defined by the user, that triangles may not lie across. */
-/* */
-/* A triangle consists of a list of three vertices, a list of three */
-/* adjoining triangles, a list of three adjoining subsegments (when */
-/* segments exist), an arbitrary number of optional user-defined */
-/* floating-point attributes, and an optional area constraint. The latter */
-/* is an upper bound on the permissible area of each triangle in a region, */
-/* used for mesh refinement. */
-/* */
-/* For a triangle on a boundary of the mesh, some or all of the neighboring */
-/* triangles may not be present. For a triangle in the interior of the */
-/* mesh, often no neighboring subsegments are present. Such absent */
-/* triangles and subsegments are never represented by NULL pointers; they */
-/* are represented by two special records: `dummytri', the triangle that */
-/* fills "outer space", and `dummysub', the omnipresent subsegment. */
-/* `dummytri' and `dummysub' are used for several reasons; for instance, */
-/* they can be dereferenced and their contents examined without violating */
-/* protected memory. */
-/* */
-/* However, it is important to understand that a triangle includes other */
-/* information as well. The pointers to adjoining vertices, triangles, and */
-/* subsegments are ordered in a way that indicates their geometric relation */
-/* to each other. Furthermore, each of these pointers contains orientation */
-/* information. Each pointer to an adjoining triangle indicates which face */
-/* of that triangle is contacted. Similarly, each pointer to an adjoining */
-/* subsegment indicates which side of that subsegment is contacted, and how */
-/* the subsegment is oriented relative to the triangle. */
-/* */
-/* The data structure representing a subsegment may be thought to be */
-/* abutting the edge of one or two triangle data structures: either */
-/* sandwiched between two triangles, or resting against one triangle on an */
-/* exterior boundary or hole boundary. */
-/* */
-/* A subsegment consists of a list of four vertices--the vertices of the */
-/* subsegment, and the vertices of the segment it is a part of--a list of */
-/* two adjoining subsegments, and a list of two adjoining triangles. One */
-/* of the two adjoining triangles may not be present (though there should */
-/* always be one), and neighboring subsegments might not be present. */
-/* Subsegments also store a user-defined integer "boundary marker". */
-/* Typically, this integer is used to indicate what boundary conditions are */
-/* to be applied at that location in a finite element simulation. */
-/* */
-/* Like triangles, subsegments maintain information about the relative */
-/* orientation of neighboring objects. */
-/* */
-/* Vertices are relatively simple. A vertex is a list of floating-point */
-/* numbers, starting with the x, and y coordinates, followed by an */
-/* arbitrary number of optional user-defined floating-point attributes, */
-/* followed by an integer boundary marker. During the segment insertion */
-/* phase, there is also a pointer from each vertex to a triangle that may */
-/* contain it. Each pointer is not always correct, but when one is, it */
-/* speeds up segment insertion. These pointers are assigned values once */
-/* at the beginning of the segment insertion phase, and are not used or */
-/* updated except during this phase. Edge flipping during segment */
-/* insertion will render some of them incorrect. Hence, don't rely upon */
-/* them for anything. */
-/* */
-/* Other than the exception mentioned above, vertices have no information */
-/* about what triangles, subfacets, or subsegments they are linked to. */
-/* */
-/*****************************************************************************/
-
-/*****************************************************************************/
-/* */
-/* Handles */
-/* */
-/* The oriented triangle (`otri') and oriented subsegment (`osub') data */
-/* structures defined below do not themselves store any part of the mesh. */
-/* The mesh itself is made of `triangle's, `subseg's, and `vertex's. */
-/* */
-/* Oriented triangles and oriented subsegments will usually be referred to */
-/* as "handles." A handle is essentially a pointer into the mesh; it */
-/* allows you to "hold" one particular part of the mesh. Handles are used */
-/* to specify the regions in which one is traversing and modifying the mesh.*/
-/* A single `triangle' may be held by many handles, or none at all. (The */
-/* latter case is not a memory leak, because the triangle is still */
-/* connected to other triangles in the mesh.) */
-/* */
-/* An `otri' is a handle that holds a triangle. It holds a specific edge */
-/* of the triangle. An `osub' is a handle that holds a subsegment. It */
-/* holds either the left or right side of the subsegment. */
-/* */
-/* Navigation about the mesh is accomplished through a set of mesh */
-/* manipulation primitives, further below. Many of these primitives take */
-/* a handle and produce a new handle that holds the mesh near the first */
-/* handle. Other primitives take two handles and glue the corresponding */
-/* parts of the mesh together. The orientation of the handles is */
-/* important. For instance, when two triangles are glued together by the */
-/* bond() primitive, they are glued at the edges on which the handles lie. */
-/* */
-/* Because vertices have no information about which triangles they are */
-/* attached to, I commonly represent a vertex by use of a handle whose */
-/* origin is the vertex. A single handle can simultaneously represent a */
-/* triangle, an edge, and a vertex. */
-/* */
-/*****************************************************************************/
-
-/* The triangle data structure. Each triangle contains three pointers to */
-/* adjoining triangles, plus three pointers to vertices, plus three */
-/* pointers to subsegments (declared below; these pointers are usually */
-/* `dummysub'). It may or may not also contain user-defined attributes */
-/* and/or a floating-point "area constraint." It may also contain extra */
-/* pointers for nodes, when the user asks for high-order elements. */
-/* Because the size and structure of a `triangle' is not decided until */
-/* runtime, I haven't simply declared the type `triangle' as a struct. */
-
-typedef float **triangle; /* Really: typedef triangle *triangle */
-
-/* An oriented triangle: includes a pointer to a triangle and orientation. */
-/* The orientation denotes an edge of the triangle. Hence, there are */
-/* three possible orientations. By convention, each edge always points */
-/* counterclockwise about the corresponding triangle. */
-
-struct otri {
- triangle *tri;
- int orient; /* Ranges from 0 to 2. */
-};
-
-/* The subsegment data structure. Each subsegment contains two pointers to */
-/* adjoining subsegments, plus four pointers to vertices, plus two */
-/* pointers to adjoining triangles, plus one boundary marker, plus one */
-/* segment number. */
-
-typedef float **subseg; /* Really: typedef subseg *subseg */
-
-/* An oriented subsegment: includes a pointer to a subsegment and an */
-/* orientation. The orientation denotes a side of the edge. Hence, there */
-/* are two possible orientations. By convention, the edge is always */
-/* directed so that the "side" denoted is the right side of the edge. */
-
-struct osub {
- subseg *ss;
- int ssorient; /* Ranges from 0 to 1. */
-};
-
-/* The vertex data structure. Each vertex is actually an array of floats. */
-/* The number of floats is unknown until runtime. An integer boundary */
-/* marker, and sometimes a pointer to a triangle, is appended after the */
-/* floats. */
-
-typedef float *vertex;
-
-/* A queue used to store encroached subsegments. Each subsegment's vertices */
-/* are stored so that we can check whether a subsegment is still the same. */
-
-struct badsubseg {
- subseg encsubseg; /* An encroached subsegment. */
- vertex subsegorg, subsegdest; /* Its two vertices. */
-};
-
-/* A queue used to store bad triangles. The key is the square of the cosine */
-/* of the smallest angle of the triangle. Each triangle's vertices are */
-/* stored so that one can check whether a triangle is still the same. */
-
-struct badtriang {
- triangle poortri; /* A skinny or too-large triangle. */
- float key; /* cos^2 of smallest (apical) angle. */
- vertex triangorg, triangdest, triangapex; /* Its three vertices. */
- struct badtriang *nexttriang; /* Pointer to next bad triangle. */
-};
-
-/* A stack of triangles flipped during the most recent vertex insertion. */
-/* The stack is used to undo the vertex insertion if the vertex encroaches */
-/* upon a subsegment. */
-
-struct flipstacker {
- triangle flippedtri; /* A recently flipped triangle. */
- struct flipstacker *prevflip; /* Previous flip in the stack. */
-};
-
-/* A node in a heap used to store events for the sweepline Delaunay */
-/* algorithm. Nodes do not point directly to their parents or children in */
-/* the heap. Instead, each node knows its position in the heap, and can */
-/* look up its parent and children in a separate array. The `eventptr' */
-/* points either to a `vertex' or to a triangle (in encoded format, so */
-/* that an orientation is included). In the latter case, the origin of */
-/* the oriented triangle is the apex of a "circle event" of the sweepline */
-/* algorithm. To distinguish site events from circle events, all circle */
-/* events are given an invalid (smaller than `xmin') x-coordinate `xkey'. */
-
-struct event {
- float xkey, ykey; /* Coordinates of the event. */
- int *eventptr; /* Can be a vertex or the location of a circle event. */
- int heapposition; /* Marks this event's position in the heap. */
-};
-
-/* A node in the splay tree. Each node holds an oriented ghost triangle */
-/* that represents a boundary edge of the growing triangulation. When a */
-/* circle event covers two boundary edges with a triangle, so that they */
-/* are no longer boundary edges, those edges are not immediately deleted */
-/* from the tree; rather, they are lazily deleted when they are next */
-/* encountered. (Since only a random sample of boundary edges are kept */
-/* in the tree, lazy deletion is faster.) `keydest' is used to verify */
-/* that a triangle is still the same as when it entered the splay tree; if */
-/* it has been rotated (due to a circle event), it no longer represents a */
-/* boundary edge and should be deleted. */
-
-struct splaynode {
- struct otri keyedge; /* Lprev of an edge on the front. */
- vertex keydest; /* Used to verify that splay node is still live. */
- struct splaynode *lchild, *rchild; /* Children in splay tree. */
-};
-
-/* A type used to allocate memory. firstblock is the first block of items. */
-/* nowblock is the block from which items are currently being allocated. */
-/* nextitem points to the next slab of free memory for an item. */
-/* deaditemstack is the head of a linked list (stack) of deallocated items */
-/* that can be recycled. unallocateditems is the number of items that */
-/* remain to be allocated from nowblock. */
-/* */
-/* Traversal is the process of walking through the entire list of items, and */
-/* is separate from allocation. Note that a traversal will visit items on */
-/* the "deaditemstack" stack as well as live items. pathblock points to */
-/* the block currently being traversed. pathitem points to the next item */
-/* to be traversed. pathitemsleft is the number of items that remain to */
-/* be traversed in pathblock. */
-/* */
-/* alignbytes determines how new records should be aligned in memory. */
-/* itembytes is the length of a record in bytes (after rounding up). */
-/* itemsperblock is the number of items allocated at once in a single */
-/* block. itemsfirstblock is the number of items in the first block, */
-/* which can vary from the others. items is the number of currently */
-/* allocated items. maxitems is the maximum number of items that have */
-/* been allocated at once; it is the current number of items plus the */
-/* number of records kept on deaditemstack. */
-
-struct memorypool {
- int **firstblock, **nowblock;
- int *nextitem;
- int *deaditemstack;
- int **pathblock;
- int *pathitem;
- int alignbytes;
- int itembytes;
- int itemsperblock;
- int itemsfirstblock;
- long items, maxitems;
- int unallocateditems;
- int pathitemsleft;
-};
-
-
-/* Global constants. */
-
-float splitter; /* Used to split float factors for exact multiplication. */
-float epsilon; /* Floating-point machine epsilon. */
-float resulterrbound;
-float ccwerrboundA, ccwerrboundB, ccwerrboundC;
-float iccerrboundA, iccerrboundB, iccerrboundC;
-float o3derrboundA, o3derrboundB, o3derrboundC;
-
-/* Random number seed is not constant, but I've made it global anyway. */
-
-unsigned long long randomseed; /* Current random number seed. */
-
-
-/* Mesh data structure. Triangle operates on only one mesh, but the mesh */
-/* structure is used (instead of global variables) to allow reentrancy. */
-
-struct mesh {
-
-/* Variables used to allocate memory for triangles, subsegments, vertices, */
-/* viri (triangles being eaten), encroached segments, bad (skinny or too */
-/* large) triangles, and splay tree nodes. */
-
- struct memorypool triangles;
- struct memorypool subsegs;
- struct memorypool vertices;
- struct memorypool viri;
- struct memorypool badsubsegs;
- struct memorypool badtriangles;
- struct memorypool flipstackers;
- struct memorypool splaynodes;
-
-/* Variables that maintain the bad triangle queues. The queues are */
-/* ordered from 4095 (highest priority) to 0 (lowest priority). */
-
- struct badtriang *queuefront[4096];
- struct badtriang *queuetail[4096];
- int nextnonemptyq[4096];
- int firstnonemptyq;
-
-/* Variable that maintains the stack of recently flipped triangles. */
-
- struct flipstacker *lastflip;
-
-/* Other variables. */
-
- float xmin, xmax, ymin, ymax; /* x and y bounds. */
- float xminextreme; /* Nonexistent x value used as a flag in sweepline. */
- int invertices; /* Number of input vertices. */
- int inelements; /* Number of input triangles. */
- int insegments; /* Number of input segments. */
- int holes; /* Number of input holes. */
- int regions; /* Number of input regions. */
- int undeads; /* Number of input vertices that don't appear in the mesh. */
- long edges; /* Number of output edges. */
- int mesh_dim; /* Dimension (ought to be 2). */
- int nextras; /* Number of attributes per vertex. */
- int eextras; /* Number of attributes per triangle. */
- long hullsize; /* Number of edges in convex hull. */
- int steinerleft; /* Number of Steiner points not yet used. */
- int vertexmarkindex; /* Index to find boundary marker of a vertex. */
- int vertex2triindex; /* Index to find a triangle adjacent to a vertex. */
- int highorderindex; /* Index to find extra nodes for high-order elements. */
- int elemattribindex; /* Index to find attributes of a triangle. */
- int areaboundindex; /* Index to find area bound of a triangle. */
- int checksegments; /* Are there segments in the triangulation yet? */
- int checkquality; /* Has quality triangulation begun yet? */
- int readnodefile; /* Has a .node file been read? */
- long samples; /* Number of random samples for point location. */
-
- long incirclecount; /* Number of incircle tests performed. */
- long counterclockcount; /* Number of counterclockwise tests performed. */
- long orient3dcount; /* Number of 3D orientation tests performed. */
- long hyperbolacount; /* Number of right-of-hyperbola tests performed. */
- long circumcentercount; /* Number of circumcenter calculations performed. */
- long circletopcount; /* Number of circle top calculations performed. */
-
-/* Triangular bounding box vertices. */
-
- vertex infvertex1, infvertex2, infvertex3;
-
-/* Pointer to the `triangle' that occupies all of "outer space." */
-
- triangle *dummytri;
- triangle *dummytribase; /* Keep base address so we can free() it later. */
-
-/* Pointer to the omnipresent subsegment. Referenced by any triangle or */
-/* subsegment that isn't really connected to a subsegment at that */
-/* location. */
-
- subseg *dummysub;
- subseg *dummysubbase; /* Keep base address so we can free() it later. */
-
-/* Pointer to a recently visited triangle. Improves point location if */
-/* proximate vertices are inserted sequentially. */
-
- struct otri recenttri;
-
-}; /* End of `struct mesh'. */
-
-
-/* Data structure for command line switches and file names. This structure */
-/* is used (instead of global variables) to allow reentrancy. */
-
-struct behavior {
-
-/* Switches for the triangulator. */
-/* poly: -p switch. refine: -r switch. */
-/* quality: -q switch. */
-/* minangle: minimum angle bound, specified after -q switch. */
-/* goodangle: cosine squared of minangle. */
-/* offconstant: constant used to place off-center Steiner points. */
-/* vararea: -a switch without number. */
-/* fixedarea: -a switch with number. */
-/* maxarea: maximum area bound, specified after -a switch. */
-/* usertest: -u switch. */
-/* regionattrib: -A switch. convex: -c switch. */
-/* weighted: 1 for -w switch, 2 for -W switch. jettison: -j switch */
-/* firstnumber: inverse of -z switch. All items are numbered starting */
-/* from `firstnumber'. */
-/* edgesout: -e switch. voronoi: -v switch. */
-/* neighbors: -n switch. geomview: -g switch. */
-/* nobound: -B switch. nopolywritten: -P switch. */
-/* nonodewritten: -N switch. noelewritten: -E switch. */
-/* noiterationnum: -I switch. noholes: -O switch. */
-/* noexact: -X switch. */
-/* order: element order, specified after -o switch. */
-/* nobisect: count of how often -Y switch is selected. */
-/* steiner: maximum number of Steiner points, specified after -S switch. */
-/* incremental: -i switch. sweepline: -F switch. */
-/* dwyer: inverse of -l switch. */
-/* splitseg: -s switch. */
-/* conformdel: -D switch. docheck: -C switch. */
-/* quiet: -Q switch. verbose: count of how often -V switch is selected. */
-/* usesegments: -p, -r, -q, or -c switch; determines whether segments are */
-/* used at all. */
-/* */
-/* Read the instructions to find out the meaning of these switches. */
-
- int poly, refine, quality, vararea, fixedarea, usertest;
- int regionattrib, convex, weighted, jettison;
- int firstnumber;
- int edgesout, voronoi, neighbors, geomview;
- int nobound, nopolywritten, nonodewritten, noelewritten, noiterationnum;
- int noholes, noexact, conformdel;
- int incremental, sweepline, dwyer;
- int splitseg;
- int docheck;
- int quiet, verbose;
- int usesegments;
- int order;
- int nobisect;
- int steiner;
- float minangle, goodangle, offconstant;
- float maxarea;
-
-/* Variables for file names. */
-
-}; /* End of `struct behavior'. */
-
-
-/*****************************************************************************/
-/* */
-/* Mesh manipulation primitives. Each triangle contains three pointers to */
-/* other triangles, with orientations. Each pointer points not to the */
-/* first byte of a triangle, but to one of the first three bytes of a */
-/* triangle. It is necessary to extract both the triangle itself and the */
-/* orientation. To save memory, I keep both pieces of information in one */
-/* pointer. To make this possible, I assume that all triangles are aligned */
-/* to four-byte boundaries. The decode() routine below decodes a pointer, */
-/* extracting an orientation (in the range 0 to 2) and a pointer to the */
-/* beginning of a triangle. The encode() routine compresses a pointer to a */
-/* triangle and an orientation into a single pointer. My assumptions that */
-/* triangles are four-byte-aligned and that the `unsigned long' type is */
-/* long enough to hold a pointer are two of the few kludges in this program.*/
-/* */
-/* Subsegments are manipulated similarly. A pointer to a subsegment */
-/* carries both an address and an orientation in the range 0 to 1. */
-/* */
-/* The other primitives take an oriented triangle or oriented subsegment, */
-/* and return an oriented triangle or oriented subsegment or vertex; or */
-/* they change the connections in the data structure. */
-/* */
-/* Below, triangles and subsegments are denoted by their vertices. The */
-/* triangle abc has origin (org) a, destination (dest) b, and apex (apex) */
-/* c. These vertices occur in counterclockwise order about the triangle. */
-/* The handle abc may simultaneously denote vertex a, edge ab, and triangle */
-/* abc. */
-/* */
-/* Similarly, the subsegment ab has origin (sorg) a and destination (sdest) */
-/* b. If ab is thought to be directed upward (with b directly above a), */
-/* then the handle ab is thought to grasp the right side of ab, and may */
-/* simultaneously denote vertex a and edge ab. */
-/* */
-/* An asterisk (*) denotes a vertex whose identity is unknown. */
-/* */
-/* Given this notation, a partial list of mesh manipulation primitives */
-/* follows. */
-/* */
-/* */
-/* For triangles: */
-/* */
-/* sym: Find the abutting triangle; same edge. */
-/* sym(abc) -> ba* */
-/* */
-/* lnext: Find the next edge (counterclockwise) of a triangle. */
-/* lnext(abc) -> bca */
-/* */
-/* lprev: Find the previous edge (clockwise) of a triangle. */
-/* lprev(abc) -> cab */
-/* */
-/* onext: Find the next edge counterclockwise with the same origin. */
-/* onext(abc) -> ac* */
-/* */
-/* oprev: Find the next edge clockwise with the same origin. */
-/* oprev(abc) -> a*b */
-/* */
-/* dnext: Find the next edge counterclockwise with the same destination. */
-/* dnext(abc) -> *ba */
-/* */
-/* dprev: Find the next edge clockwise with the same destination. */
-/* dprev(abc) -> cb* */
-/* */
-/* rnext: Find the next edge (counterclockwise) of the adjacent triangle. */
-/* rnext(abc) -> *a* */
-/* */
-/* rprev: Find the previous edge (clockwise) of the adjacent triangle. */
-/* rprev(abc) -> b** */
-/* */
-/* org: Origin dest: Destination apex: Apex */
-/* org(abc) -> a dest(abc) -> b apex(abc) -> c */
-/* */
-/* bond: Bond two triangles together at the resepective handles. */
-/* bond(abc, bad) */
-/* */
-/* */
-/* For subsegments: */
-/* */
-/* ssym: Reverse the orientation of a subsegment. */
-/* ssym(ab) -> ba */
-/* */
-/* spivot: Find adjoining subsegment with the same origin. */
-/* spivot(ab) -> a* */
-/* */
-/* snext: Find next subsegment in sequence. */
-/* snext(ab) -> b* */
-/* */
-/* sorg: Origin sdest: Destination */
-/* sorg(ab) -> a sdest(ab) -> b */
-/* */
-/* sbond: Bond two subsegments together at the respective origins. */
-/* sbond(ab, ac) */
-/* */
-/* */
-/* For interacting tetrahedra and subfacets: */
-/* */
-/* tspivot: Find a subsegment abutting a triangle. */
-/* tspivot(abc) -> ba */
-/* */
-/* stpivot: Find a triangle abutting a subsegment. */
-/* stpivot(ab) -> ba* */
-/* */
-/* tsbond: Bond a triangle to a subsegment. */
-/* tsbond(abc, ba) */
-/* */
-/*****************************************************************************/
-
-/********* Mesh manipulation primitives begin here *********/
-/** **/
-/** **/
-
-/* Fast lookup arrays to speed some of the mesh manipulation primitives. */
-
-int plus1mod3[3] = {1, 2, 0};
-int minus1mod3[3] = {2, 0, 1};
-
-/********* Primitives for triangles *********/
-/* */
-/* */
-
-/* decode() converts a pointer to an oriented triangle. The orientation is */
-/* extracted from the two least significant bits of the pointer. */
-
-#define decode(ptr, otri) \
- (otri).orient = (int) ((unsigned long long) (ptr) & (unsigned long long) 3l); \
- (otri).tri = (triangle *) \
- ((unsigned long long) (ptr) ^ (unsigned long long) (otri).orient)
-
-/* encode() compresses an oriented triangle into a single pointer. It */
-/* relies on the assumption that all triangles are aligned to four-byte */
-/* boundaries, so the two least significant bits of (otri).tri are zero. */
-
-#define encode(otri) \
- (triangle) ((unsigned long long) (otri).tri | (unsigned long long) (otri).orient)
-
-/* The following handle manipulation primitives are all described by Guibas */
-/* and Stolfi. However, Guibas and Stolfi use an edge-based data */
-/* structure, whereas I use a triangle-based data structure. */
-
-/* sym() finds the abutting triangle, on the same edge. Note that the edge */
-/* direction is necessarily reversed, because the handle specified by an */
-/* oriented triangle is directed counterclockwise around the triangle. */
-
-#define sym(otri1, otri2) \
- ptr = (otri1).tri[(otri1).orient]; \
- decode(ptr, otri2);
-
-#define symself(otri) \
- ptr = (otri).tri[(otri).orient]; \
- decode(ptr, otri);
-
-/* lnext() finds the next edge (counterclockwise) of a triangle. */
-
-#define lnext(otri1, otri2) \
- (otri2).tri = (otri1).tri; \
- (otri2).orient = plus1mod3[(otri1).orient]
-
-#define lnextself(otri) \
- (otri).orient = plus1mod3[(otri).orient]
-
-/* lprev() finds the previous edge (clockwise) of a triangle. */
-
-#define lprev(otri1, otri2) \
- (otri2).tri = (otri1).tri; \
- (otri2).orient = minus1mod3[(otri1).orient]
-
-#define lprevself(otri) \
- (otri).orient = minus1mod3[(otri).orient]
-
-/* onext() spins counterclockwise around a vertex; that is, it finds the */
-/* next edge with the same origin in the counterclockwise direction. This */
-/* edge is part of a different triangle. */
-
-#define onext(otri1, otri2) \
- lprev(otri1, otri2); \
- symself(otri2);
-
-#define onextself(otri) \
- lprevself(otri); \
- symself(otri);
-
-/* oprev() spins clockwise around a vertex; that is, it finds the next edge */
-/* with the same origin in the clockwise direction. This edge is part of */
-/* a different triangle. */
-
-#define oprev(otri1, otri2) \
- sym(otri1, otri2); \
- lnextself(otri2);
-
-#define oprevself(otri) \
- symself(otri); \
- lnextself(otri);
-
-/* dnext() spins counterclockwise around a vertex; that is, it finds the */
-/* next edge with the same destination in the counterclockwise direction. */
-/* This edge is part of a different triangle. */
-
-#define dnext(otri1, otri2) \
- sym(otri1, otri2); \
- lprevself(otri2);
-
-#define dnextself(otri) \
- symself(otri); \
- lprevself(otri);
-
-/* dprev() spins clockwise around a vertex; that is, it finds the next edge */
-/* with the same destination in the clockwise direction. This edge is */
-/* part of a different triangle. */
-
-#define dprev(otri1, otri2) \
- lnext(otri1, otri2); \
- symself(otri2);
-
-#define dprevself(otri) \
- lnextself(otri); \
- symself(otri);
-
-/* rnext() moves one edge counterclockwise about the adjacent triangle. */
-/* (It's best understood by reading Guibas and Stolfi. It involves */
-/* changing triangles twice.) */
-
-#define rnext(otri1, otri2) \
- sym(otri1, otri2); \
- lnextself(otri2); \
- symself(otri2);
-
-#define rnextself(otri) \
- symself(otri); \
- lnextself(otri); \
- symself(otri);
-
-/* rprev() moves one edge clockwise about the adjacent triangle. */
-/* (It's best understood by reading Guibas and Stolfi. It involves */
-/* changing triangles twice.) */
-
-#define rprev(otri1, otri2) \
- sym(otri1, otri2); \
- lprevself(otri2); \
- symself(otri2);
-
-#define rprevself(otri) \
- symself(otri); \
- lprevself(otri); \
- symself(otri);
-
-/* These primitives determine or set the origin, destination, or apex of a */
-/* triangle. */
-
-#define org(otri, vertexptr) \
- vertexptr = (vertex) (otri).tri[plus1mod3[(otri).orient] + 3]
-
-#define dest(otri, vertexptr) \
- vertexptr = (vertex) (otri).tri[minus1mod3[(otri).orient] + 3]
-
-#define apex(otri, vertexptr) \
- vertexptr = (vertex) (otri).tri[(otri).orient + 3]
-
-#define setorg(otri, vertexptr) \
- (otri).tri[plus1mod3[(otri).orient] + 3] = (triangle) vertexptr
-
-#define setdest(otri, vertexptr) \
- (otri).tri[minus1mod3[(otri).orient] + 3] = (triangle) vertexptr
-
-#define setapex(otri, vertexptr) \
- (otri).tri[(otri).orient + 3] = (triangle) vertexptr
-
-/* Bond two triangles together. */
-
-#define bond(otri1, otri2) \
- (otri1).tri[(otri1).orient] = encode(otri2); \
- (otri2).tri[(otri2).orient] = encode(otri1)
-
-/* Dissolve a bond (from one side). Note that the other triangle will still */
-/* think it's connected to this triangle. Usually, however, the other */
-/* triangle is being deleted entirely, or bonded to another triangle, so */
-/* it doesn't matter. */
-
-#define dissolve(otri) \
- (otri).tri[(otri).orient] = (triangle) m->dummytri
-
-/* Copy an oriented triangle. */
-
-#define otricopy(otri1, otri2) \
- (otri2).tri = (otri1).tri; \
- (otri2).orient = (otri1).orient
-
-/* Test for equality of oriented triangles. */
-
-#define otriequal(otri1, otri2) \
- (((otri1).tri == (otri2).tri) && \
- ((otri1).orient == (otri2).orient))
-
-/* Primitives to infect or cure a triangle with the virus. These rely on */
-/* the assumption that all subsegments are aligned to four-byte boundaries.*/
-
-#define infect(otri) \
- (otri).tri[6] = (triangle) \
- ((unsigned long long) (otri).tri[6] | (unsigned long long) 2l)
-
-#define uninfect(otri) \
- (otri).tri[6] = (triangle) \
- ((unsigned long long) (otri).tri[6] & ~ (unsigned long long) 2l)
-
-/* Test a triangle for viral infection. */
-
-#define infected(otri) \
- (((unsigned long long) (otri).tri[6] & (unsigned long long) 2l) != 0l)
-
-/* Check or set a triangle's attributes. */
-
-#define elemattribute(otri, attnum) \
- ((float *) (otri).tri)[m->elemattribindex + (attnum)]
-
-#define setelemattribute(otri, attnum, value) \
- ((float *) (otri).tri)[m->elemattribindex + (attnum)] = value
-
-/* Check or set a triangle's maximum area bound. */
-
-#define areabound(otri) ((float *) (otri).tri)[m->areaboundindex]
-
-#define setareabound(otri, value) \
- ((float *) (otri).tri)[m->areaboundindex] = value
-
-/* Check or set a triangle's deallocation. Its second pointer is set to */
-/* NULL to indicate that it is not allocated. (Its first pointer is used */
-/* for the stack of dead items.) Its fourth pointer (its first vertex) */
-/* is set to NULL in case a `badtriang' structure points to it. */
-
-#define deadtri(tria) ((tria)[1] == (triangle) NULL)
-
-#define killtri(tria) \
- (tria)[1] = (triangle) NULL; \
- (tria)[3] = (triangle) NULL
-
-/********* Primitives for subsegments *********/
-/* */
-/* */
-
-/* sdecode() converts a pointer to an oriented subsegment. The orientation */
-/* is extracted from the least significant bit of the pointer. The two */
-/* least significant bits (one for orientation, one for viral infection) */
-/* are masked out to produce the real pointer. */
-
-#define sdecode(sptr, osub) \
- (osub).ssorient = (int) ((unsigned long long) (sptr) & (unsigned long long) 1l); \
- (osub).ss = (subseg *) \
- ((unsigned long long) (sptr) & ~ (unsigned long long) 3l)
-
-/* sencode() compresses an oriented subsegment into a single pointer. It */
-/* relies on the assumption that all subsegments are aligned to two-byte */
-/* boundaries, so the least significant bit of (osub).ss is zero. */
-
-#define sencode(osub) \
- (subseg) ((unsigned long long) (osub).ss | (unsigned long long) (osub).ssorient)
-
-/* ssym() toggles the orientation of a subsegment. */
-
-#define ssym(osub1, osub2) \
- (osub2).ss = (osub1).ss; \
- (osub2).ssorient = 1 - (osub1).ssorient
-
-#define ssymself(osub) \
- (osub).ssorient = 1 - (osub).ssorient
-
-/* spivot() finds the other subsegment (from the same segment) that shares */
-/* the same origin. */
-
-#define spivot(osub1, osub2) \
- sptr = (osub1).ss[(osub1).ssorient]; \
- sdecode(sptr, osub2)
-
-#define spivotself(osub) \
- sptr = (osub).ss[(osub).ssorient]; \
- sdecode(sptr, osub)
-
-/* snext() finds the next subsegment (from the same segment) in sequence; */
-/* one whose origin is the input subsegment's destination. */
-
-#define snext(osub1, osub2) \
- sptr = (osub1).ss[1 - (osub1).ssorient]; \
- sdecode(sptr, osub2)
-
-#define snextself(osub) \
- sptr = (osub).ss[1 - (osub).ssorient]; \
- sdecode(sptr, osub)
-
-/* These primitives determine or set the origin or destination of a */
-/* subsegment or the segment that includes it. */
-
-#define sorg(osub, vertexptr) \
- vertexptr = (vertex) (osub).ss[2 + (osub).ssorient]
-
-#define sdest(osub, vertexptr) \
- vertexptr = (vertex) (osub).ss[3 - (osub).ssorient]
-
-#define setsorg(osub, vertexptr) \
- (osub).ss[2 + (osub).ssorient] = (subseg) vertexptr
-
-#define setsdest(osub, vertexptr) \
- (osub).ss[3 - (osub).ssorient] = (subseg) vertexptr
-
-#define segorg(osub, vertexptr) \
- vertexptr = (vertex) (osub).ss[4 + (osub).ssorient]
-
-#define segdest(osub, vertexptr) \
- vertexptr = (vertex) (osub).ss[5 - (osub).ssorient]
-
-#define setsegorg(osub, vertexptr) \
- (osub).ss[4 + (osub).ssorient] = (subseg) vertexptr
-
-#define setsegdest(osub, vertexptr) \
- (osub).ss[5 - (osub).ssorient] = (subseg) vertexptr
-
-/* These primitives read or set a boundary marker. Boundary markers are */
-/* used to hold user-defined tags for setting boundary conditions in */
-/* finite element solvers. */
-
-#define mark(osub) (* (int *) ((osub).ss + 8))
-
-#define setmark(osub, value) \
- * (int *) ((osub).ss + 8) = value
-
-/* Bond two subsegments together. */
-
-#define sbond(osub1, osub2) \
- (osub1).ss[(osub1).ssorient] = sencode(osub2); \
- (osub2).ss[(osub2).ssorient] = sencode(osub1)
-
-/* Dissolve a subsegment bond (from one side). Note that the other */
-/* subsegment will still think it's connected to this subsegment. */
-
-#define sdissolve(osub) \
- (osub).ss[(osub).ssorient] = (subseg) m->dummysub
-
-/* Copy a subsegment. */
-
-#define subsegcopy(osub1, osub2) \
- (osub2).ss = (osub1).ss; \
- (osub2).ssorient = (osub1).ssorient
-
-/* Test for equality of subsegments. */
-
-#define subsegequal(osub1, osub2) \
- (((osub1).ss == (osub2).ss) && \
- ((osub1).ssorient == (osub2).ssorient))
-
-/* Check or set a subsegment's deallocation. Its second pointer is set to */
-/* NULL to indicate that it is not allocated. (Its first pointer is used */
-/* for the stack of dead items.) Its third pointer (its first vertex) */
-/* is set to NULL in case a `badsubseg' structure points to it. */
-
-#define deadsubseg(sub) ((sub)[1] == (subseg) NULL)
-
-#define killsubseg(sub) \
- (sub)[1] = (subseg) NULL; \
- (sub)[2] = (subseg) NULL
-
-/********* Primitives for interacting triangles and subsegments *********/
-/* */
-/* */
-
-/* tspivot() finds a subsegment abutting a triangle. */
-
-#define tspivot(otri, osub) \
- sptr = (subseg) (otri).tri[6 + (otri).orient]; \
- sdecode(sptr, osub)
-
-/* stpivot() finds a triangle abutting a subsegment. It requires that the */
-/* variable `ptr' of type `triangle' be defined. */
-
-#define stpivot(osub, otri) \
- ptr = (triangle) (osub).ss[6 + (osub).ssorient]; \
- decode(ptr, otri)
-
-/* Bond a triangle to a subsegment. */
-
-#define tsbond(otri, osub) \
- (otri).tri[6 + (otri).orient] = (triangle) sencode(osub); \
- (osub).ss[6 + (osub).ssorient] = (subseg) encode(otri)
-
-/* Dissolve a bond (from the triangle side). */
-
-#define tsdissolve(otri) \
- (otri).tri[6 + (otri).orient] = (triangle) m->dummysub
-
-/* Dissolve a bond (from the subsegment side). */
-
-#define stdissolve(osub) \
- (osub).ss[6 + (osub).ssorient] = (subseg) m->dummytri
-
-/********* Primitives for vertices *********/
-/* */
-/* */
-
-#define vertexmark(vx) ((int *) (vx))[m->vertexmarkindex]
-
-#define setvertexmark(vx, value) \
- ((int *) (vx))[m->vertexmarkindex] = value
-
-#define vertextype(vx) ((int *) (vx))[m->vertexmarkindex + 1]
-
-#define setvertextype(vx, value) \
- ((int *) (vx))[m->vertexmarkindex + 1] = value
-
-#define vertex2tri(vx) ((triangle *) (vx))[m->vertex2triindex]
-
-#define setvertex2tri(vx, value) \
- ((triangle *) (vx))[m->vertex2triindex] = value
-
-/** **/
-/** **/
-/********* Mesh manipulation primitives end here *********/
-
-/********* Memory allocation and program exit wrappers begin here *********/
-/** **/
-/** **/
-
-void triexit(int status)
-{
- exit(status);
-}
-
-int *trimalloc(int size)
-{
- int *memptr;
-
- memptr = (int *) malloc((unsigned int) size);
- if (memptr == (int *) NULL) {
- printf("Error: Out of memory.\n");
- triexit(1);
- }
- return(memptr);
-}
-
-void trifree(int *memptr)
-{
- free(memptr);
-}
-
-/** **/
-/** **/
-/********* Memory allocation and program exit wrappers end here *********/
-
-/*****************************************************************************/
-/* */
-/* internalerror() Ask the user to send me the defective product. Exit. */
-/* */
-/*****************************************************************************/
-
-void internalerror()
-{
- printf(" Please report this bug to jrs@cs.berkeley.edu\n");
- printf(" Include the message above, your input data set, and the exact\n");
- printf(" command line you used to run Triangle.\n");
- triexit(1);
-}
-
-/*****************************************************************************/
-/* */
-/* parsecommandline() Read the command line, identify switches, and set */
-/* up options and file names. */
-/* */
-/*****************************************************************************/
-
-void parsecommandline(int argc, char **argv, struct behavior *b) {
- int i, j, k;
- char workstring[FILENAMESIZE];
-
- b->poly = b->refine = b->quality = 0;
- b->vararea = b->fixedarea = b->usertest = 0;
- b->regionattrib = b->convex = b->weighted = b->jettison = 0;
- b->firstnumber = 1;
- b->edgesout = b->voronoi = b->neighbors = b->geomview = 0;
- b->nobound = b->nopolywritten = b->nonodewritten = b->noelewritten = 0;
- b->noiterationnum = 0;
- b->noholes = b->noexact = 0;
- b->incremental = b->sweepline = 0;
- b->dwyer = 1;
- b->splitseg = 0;
- b->docheck = 0;
- b->nobisect = 0;
- b->conformdel = 0;
- b->steiner = -1;
- b->order = 1;
- b->minangle = 0.0;
- b->maxarea = -1.0;
- b->quiet = b->verbose = 0;
-
- for (i = 0; i < argc; i++) {
- for (j = 0; argv[i][j] != '\0'; j++) {
- if (argv[i][j] == 'p') {
- b->poly = 1;
- }
- if (argv[i][j] == 'A') {
- b->regionattrib = 1;
- }
- if (argv[i][j] == 'c') {
- b->convex = 1;
- }
- if (argv[i][j] == 'w') {
- b->weighted = 1;
- }
- if (argv[i][j] == 'W') {
- b->weighted = 2;
- }
- if (argv[i][j] == 'j') {
- b->jettison = 1;
- }
- if (argv[i][j] == 'z') {
- b->firstnumber = 0;
- }
- if (argv[i][j] == 'e') {
- b->edgesout = 1;
- }
- if (argv[i][j] == 'v') {
- b->voronoi = 1;
- }
- if (argv[i][j] == 'n') {
- b->neighbors = 1;
- }
- if (argv[i][j] == 'g') {
- b->geomview = 1;
- }
- if (argv[i][j] == 'B') {
- b->nobound = 1;
- }
- if (argv[i][j] == 'P') {
- b->nopolywritten = 1;
- }
- if (argv[i][j] == 'N') {
- b->nonodewritten = 1;
- }
- if (argv[i][j] == 'E') {
- b->noelewritten = 1;
- }
- if (argv[i][j] == 'O') {
- b->noholes = 1;
- }
- if (argv[i][j] == 'X') {
- b->noexact = 1;
- }
- if (argv[i][j] == 'o') {
- if (argv[i][j + 1] == '2') {
- j++;
- b->order = 2;
- }
- }
- if (argv[i][j] == 'l') {
- b->dwyer = 0;
- }
- if (argv[i][j] == 'Q') {
- b->quiet = 1;
- }
- if (argv[i][j] == 'V') {
- b->verbose++;
- }
- }
- }
- b->usesegments = b->poly || b->refine || b->quality || b->convex;
- b->goodangle = cos(b->minangle * PI / 180.0);
- if (b->goodangle == 1.0) {
- b->offconstant = 0.0;
- } else {
- b->offconstant = 0.475 * sqrt((1.0 + b->goodangle) / (1.0 - b->goodangle));
- }
- b->goodangle *= b->goodangle;
- if (b->refine && b->noiterationnum) {
- printf(
- "Error: You cannot use the -I switch when refining a triangulation.\n");
- triexit(1);
- }
- /* Be careful not to allocate space for element area constraints that */
- /* will never be assigned any value (other than the default -1.0). */
- if (!b->refine && !b->poly) {
- b->vararea = 0;
- }
- /* Be careful not to add an extra attribute to each element unless the */
- /* input supports it (PSLG in, but not refining a preexisting mesh). */
- if (b->refine || !b->poly) {
- b->regionattrib = 0;
- }
- /* Regular/weighted triangulations are incompatible with PSLGs */
- /* and meshing. */
- if (b->weighted && (b->poly || b->quality)) {
- b->weighted = 0;
- if (!b->quiet) {
- printf("Warning: weighted triangulations (-w, -W) are incompatible\n");
- printf(" with PSLGs (-p) and meshing (-q, -a, -u). Weights ignored.\n"
- );
- }
- }
- if (b->jettison && b->nonodewritten && !b->quiet) {
- printf("Warning: -j and -N switches are somewhat incompatible.\n");
- printf(" If any vertices are jettisoned, you will need the output\n");
- printf(" .node file to reconstruct the new node indices.");
- }
-}
-
-/** **/
-/** **/
-/********* User interaction routines begin here *********/
-
-/********* Debugging routines begin here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* printtriangle() Print out the details of an oriented triangle. */
-/* */
-/* I originally wrote this procedure to simplify debugging; it can be */
-/* called directly from the debugger, and presents information about an */
-/* oriented triangle in digestible form. It's also used when the */
-/* highest level of verbosity (`-VVV') is specified. */
-/* */
-/*****************************************************************************/
-
-void printtriangle(struct mesh *m, struct behavior *b, struct otri *t)
-{
- struct otri printtri;
- struct osub printsh;
- vertex printvertex;
-
- printf("triangle x%lx with orientation %d:\n", (unsigned long) t->tri,
- t->orient);
- decode(t->tri[0], printtri);
- if (printtri.tri == m->dummytri) {
- printf(" [0] = Outer space\n");
- } else {
- printf(" [0] = x%lx %d\n", (unsigned long) printtri.tri,
- printtri.orient);
- }
- decode(t->tri[1], printtri);
- if (printtri.tri == m->dummytri) {
- printf(" [1] = Outer space\n");
- } else {
- printf(" [1] = x%lx %d\n", (unsigned long) printtri.tri,
- printtri.orient);
- }
- decode(t->tri[2], printtri);
- if (printtri.tri == m->dummytri) {
- printf(" [2] = Outer space\n");
- } else {
- printf(" [2] = x%lx %d\n", (unsigned long) printtri.tri,
- printtri.orient);
- }
-
- org(*t, printvertex);
- if (printvertex == (vertex) NULL)
- printf(" Origin[%d] = NULL\n", (t->orient + 1) % 3 + 3);
- else
- printf(" Origin[%d] = x%lx (%.12g, %.12g)\n",
- (t->orient + 1) % 3 + 3, (unsigned long) printvertex,
- printvertex[0], printvertex[1]);
- dest(*t, printvertex);
- if (printvertex == (vertex) NULL)
- printf(" Dest [%d] = NULL\n", (t->orient + 2) % 3 + 3);
- else
- printf(" Dest [%d] = x%lx (%.12g, %.12g)\n",
- (t->orient + 2) % 3 + 3, (unsigned long) printvertex,
- printvertex[0], printvertex[1]);
- apex(*t, printvertex);
- if (printvertex == (vertex) NULL)
- printf(" Apex [%d] = NULL\n", t->orient + 3);
- else
- printf(" Apex [%d] = x%lx (%.12g, %.12g)\n",
- t->orient + 3, (unsigned long) printvertex,
- printvertex[0], printvertex[1]);
-
- if (b->usesegments) {
- sdecode(t->tri[6], printsh);
- if (printsh.ss != m->dummysub) {
- printf(" [6] = x%lx %d\n", (unsigned long) printsh.ss,
- printsh.ssorient);
- }
- sdecode(t->tri[7], printsh);
- if (printsh.ss != m->dummysub) {
- printf(" [7] = x%lx %d\n", (unsigned long) printsh.ss,
- printsh.ssorient);
- }
- sdecode(t->tri[8], printsh);
- if (printsh.ss != m->dummysub) {
- printf(" [8] = x%lx %d\n", (unsigned long) printsh.ss,
- printsh.ssorient);
- }
- }
-
- if (b->vararea) {
- printf(" Area constraint: %.4g\n", areabound(*t));
- }
-}
-
-/*****************************************************************************/
-/* */
-/* printsubseg() Print out the details of an oriented subsegment. */
-/* */
-/* I originally wrote this procedure to simplify debugging; it can be */
-/* called directly from the debugger, and presents information about an */
-/* oriented subsegment in digestible form. It's also used when the highest */
-/* level of verbosity (`-VVV') is specified. */
-/* */
-/*****************************************************************************/
-
-void printsubseg(struct mesh *m, struct behavior *b, struct osub *s)
-{
- struct osub printsh;
- struct otri printtri;
- vertex printvertex;
-
- printf("subsegment x%lx with orientation %d and mark %d:\n",
- (unsigned long) s->ss, s->ssorient, mark(*s));
- sdecode(s->ss[0], printsh);
- if (printsh.ss == m->dummysub) {
- printf(" [0] = No subsegment\n");
- } else {
- printf(" [0] = x%lx %d\n", (unsigned long) printsh.ss,
- printsh.ssorient);
- }
- sdecode(s->ss[1], printsh);
- if (printsh.ss == m->dummysub) {
- printf(" [1] = No subsegment\n");
- } else {
- printf(" [1] = x%lx %d\n", (unsigned long) printsh.ss,
- printsh.ssorient);
- }
-
- sorg(*s, printvertex);
- if (printvertex == (vertex) NULL)
- printf(" Origin[%d] = NULL\n", 2 + s->ssorient);
- else
- printf(" Origin[%d] = x%lx (%.12g, %.12g)\n",
- 2 + s->ssorient, (unsigned long) printvertex,
- printvertex[0], printvertex[1]);
- sdest(*s, printvertex);
- if (printvertex == (vertex) NULL)
- printf(" Dest [%d] = NULL\n", 3 - s->ssorient);
- else
- printf(" Dest [%d] = x%lx (%.12g, %.12g)\n",
- 3 - s->ssorient, (unsigned long) printvertex,
- printvertex[0], printvertex[1]);
-
- decode(s->ss[6], printtri);
- if (printtri.tri == m->dummytri) {
- printf(" [6] = Outer space\n");
- } else {
- printf(" [6] = x%lx %d\n", (unsigned long) printtri.tri,
- printtri.orient);
- }
- decode(s->ss[7], printtri);
- if (printtri.tri == m->dummytri) {
- printf(" [7] = Outer space\n");
- } else {
- printf(" [7] = x%lx %d\n", (unsigned long) printtri.tri,
- printtri.orient);
- }
-
- segorg(*s, printvertex);
- if (printvertex == (vertex) NULL)
- printf(" Segment origin[%d] = NULL\n", 4 + s->ssorient);
- else
- printf(" Segment origin[%d] = x%lx (%.12g, %.12g)\n",
- 4 + s->ssorient, (unsigned long) printvertex,
- printvertex[0], printvertex[1]);
- segdest(*s, printvertex);
- if (printvertex == (vertex) NULL)
- printf(" Segment dest [%d] = NULL\n", 5 - s->ssorient);
- else
- printf(" Segment dest [%d] = x%lx (%.12g, %.12g)\n",
- 5 - s->ssorient, (unsigned long) printvertex,
- printvertex[0], printvertex[1]);
-}
-
-/** **/
-/** **/
-/********* Debugging routines end here *********/
-
-/********* Memory management routines begin here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* poolzero() Set all of a pool's fields to zero. */
-/* */
-/* This procedure should never be called on a pool that has any memory */
-/* allocated to it, as that memory would leak. */
-/* */
-/*****************************************************************************/
-
-void poolzero(struct memorypool *pool)
-{
- pool->firstblock = (int **) NULL;
- pool->nowblock = (int **) NULL;
- pool->nextitem = (int *) NULL;
- pool->deaditemstack = (int *) NULL;
- pool->pathblock = (int **) NULL;
- pool->pathitem = (int *) NULL;
- pool->alignbytes = 0;
- pool->itembytes = 0;
- pool->itemsperblock = 0;
- pool->itemsfirstblock = 0;
- pool->items = 0;
- pool->maxitems = 0;
- pool->unallocateditems = 0;
- pool->pathitemsleft = 0;
-}
-
-/*****************************************************************************/
-/* */
-/* poolrestart() Deallocate all items in a pool. */
-/* */
-/* The pool is returned to its starting state, except that no memory is */
-/* freed to the operating system. Rather, the previously allocated blocks */
-/* are ready to be reused. */
-/* */
-/*****************************************************************************/
-
-void poolrestart(struct memorypool *pool)
-{
- unsigned long long alignptr;
-
- pool->items = 0;
- pool->maxitems = 0;
-
- /* Set the currently active block. */
- pool->nowblock = pool->firstblock;
- /* Find the first item in the pool. Increment by the size of (int *). */
- alignptr = (unsigned long long) (pool->nowblock + 1);
- /* Align the item on an `alignbytes'-byte boundary. */
- pool->nextitem = (int *)
- (alignptr + (unsigned long long) pool->alignbytes -
- (alignptr % (unsigned long long) pool->alignbytes));
- /* There are lots of unallocated items left in this block. */
- pool->unallocateditems = pool->itemsfirstblock;
- /* The stack of deallocated items is empty. */
- pool->deaditemstack = (int *) NULL;
-}
-
-/*****************************************************************************/
-/* */
-/* poolinit() Initialize a pool of memory for allocation of items. */
-/* */
-/* This routine initializes the machinery for allocating items. A `pool' */
-/* is created whose records have size at least `bytecount'. Items will be */
-/* allocated in `itemcount'-item blocks. Each item is assumed to be a */
-/* collection of words, and either pointers or floating-point values are */
-/* assumed to be the "primary" word type. (The "primary" word type is used */
-/* to determine alignment of items.) If `alignment' isn't zero, all items */
-/* will be `alignment'-byte aligned in memory. `alignment' must be either */
-/* a multiple or a factor of the primary word size; powers of two are safe. */
-/* `alignment' is normally used to create a few unused bits at the bottom */
-/* of each item's pointer, in which information may be stored. */
-/* */
-/* Don't change this routine unless you understand it. */
-/* */
-/*****************************************************************************/
-
-void poolinit(struct memorypool *pool, int bytecount, int itemcount,
- int firstitemcount, int alignment)
-{
- /* Find the proper alignment, which must be at least as large as: */
- /* - The parameter `alignment'. */
- /* - sizeof(int *), so the stack of dead items can be maintained */
- /* without unaligned accesses. */
- if (alignment > sizeof(int *)) {
- pool->alignbytes = alignment;
- } else {
- pool->alignbytes = sizeof(int *);
- }
- pool->itembytes = ((bytecount - 1) / pool->alignbytes + 1) *
- pool->alignbytes;
- pool->itemsperblock = itemcount;
- if (firstitemcount == 0) {
- pool->itemsfirstblock = itemcount;
- } else {
- pool->itemsfirstblock = firstitemcount;
- }
-
- /* Allocate a block of items. Space for `itemsfirstblock' items and one */
- /* pointer (to point to the next block) are allocated, as well as space */
- /* to ensure alignment of the items. */
- pool->firstblock = (int **)
- trimalloc(pool->itemsfirstblock * pool->itembytes + (int) sizeof(int *) +
- pool->alignbytes);
- /* Set the next block pointer to NULL. */
- *(pool->firstblock) = (int *) NULL;
- poolrestart(pool);
-}
-
-/*****************************************************************************/
-/* */
-/* pooldeinit() Free to the operating system all memory taken by a pool. */
-/* */
-/*****************************************************************************/
-
-void pooldeinit(struct memorypool *pool)
-{
- while (pool->firstblock != (int **) NULL) {
- pool->nowblock = (int **) *(pool->firstblock);
- trifree((int *) pool->firstblock);
- pool->firstblock = pool->nowblock;
- }
-}
-
-/*****************************************************************************/
-/* */
-/* poolalloc() Allocate space for an item. */
-/* */
-/*****************************************************************************/
-
-int *poolalloc(struct memorypool *pool)
-{
- int *newitem;
- int **newblock;
- unsigned long long alignptr;
-
- /* First check the linked list of dead items. If the list is not */
- /* empty, allocate an item from the list rather than a fresh one. */
- if (pool->deaditemstack != (int *) NULL) {
- newitem = pool->deaditemstack; /* Take first item in list. */
- pool->deaditemstack = * (int **) pool->deaditemstack;
- } else {
- /* Check if there are any free items left in the current block. */
- if (pool->unallocateditems == 0) {
- /* Check if another block must be allocated. */
- if (*(pool->nowblock) == (int *) NULL) {
- /* Allocate a new block of items, pointed to by the previous block. */
- newblock = (int **) trimalloc(pool->itemsperblock * pool->itembytes +
- (int) sizeof(int *) +
- pool->alignbytes);
- *(pool->nowblock) = (int *) newblock;
- /* The next block pointer is NULL. */
- *newblock = (int *) NULL;
- }
-
- /* Move to the new block. */
- pool->nowblock = (int **) *(pool->nowblock);
- /* Find the first item in the block. */
- /* Increment by the size of (int *). */
- alignptr = (unsigned long long) (pool->nowblock + 1);
- /* Align the item on an `alignbytes'-byte boundary. */
- pool->nextitem = (int *)
- (alignptr + (unsigned long long) pool->alignbytes -
- (alignptr % (unsigned long long) pool->alignbytes));
- /* There are lots of unallocated items left in this block. */
- pool->unallocateditems = pool->itemsperblock;
- }
-
- /* Allocate a new item. */
- newitem = pool->nextitem;
- /* Advance `nextitem' pointer to next free item in block. */
- pool->nextitem = (int *) ((char *) pool->nextitem + pool->itembytes);
- pool->unallocateditems--;
- pool->maxitems++;
- }
- pool->items++;
- return newitem;
-}
-
-/*****************************************************************************/
-/* */
-/* pooldealloc() Deallocate space for an item. */
-/* */
-/* The deallocated space is stored in a queue for later reuse. */
-/* */
-/*****************************************************************************/
-
-void pooldealloc(struct memorypool *pool, int *dyingitem)
-{
- /* Push freshly killed item onto stack. */
- *((int **) dyingitem) = pool->deaditemstack;
- pool->deaditemstack = dyingitem;
- pool->items--;
-}
-
-/*****************************************************************************/
-/* */
-/* traversalinit() Prepare to traverse the entire list of items. */
-/* */
-/* This routine is used in conjunction with traverse(). */
-/* */
-/*****************************************************************************/
-
-void traversalinit(struct memorypool *pool)
-{
- unsigned long long alignptr;
-
- /* Begin the traversal in the first block. */
- pool->pathblock = pool->firstblock;
- /* Find the first item in the block. Increment by the size of (int *). */
- alignptr = (unsigned long long) (pool->pathblock + 1);
- /* Align with item on an `alignbytes'-byte boundary. */
- pool->pathitem = (int *)
- (alignptr + (unsigned long long) pool->alignbytes -
- (alignptr % (unsigned long long) pool->alignbytes));
- /* Set the number of items left in the current block. */
- pool->pathitemsleft = pool->itemsfirstblock;
-}
-
-/*****************************************************************************/
-/* */
-/* traverse() Find the next item in the list. */
-/* */
-/* This routine is used in conjunction with traversalinit(). Be forewarned */
-/* that this routine successively returns all items in the list, including */
-/* deallocated ones on the deaditemqueue. It's up to you to figure out */
-/* which ones are actually dead. Why? I don't want to allocate extra */
-/* space just to demarcate dead items. It can usually be done more */
-/* space-efficiently by a routine that knows something about the structure */
-/* of the item. */
-/* */
-/*****************************************************************************/
-
-int *traverse(struct memorypool *pool)
-{
- int *newitem;
- unsigned long long alignptr;
-
- /* Stop upon exhausting the list of items. */
- if (pool->pathitem == pool->nextitem) {
- return (int *) NULL;
- }
-
- /* Check whether any untraversed items remain in the current block. */
- if (pool->pathitemsleft == 0) {
- /* Find the next block. */
- pool->pathblock = (int **) *(pool->pathblock);
- /* Find the first item in the block. Increment by the size of (int *). */
- alignptr = (unsigned long long) (pool->pathblock + 1);
- /* Align with item on an `alignbytes'-byte boundary. */
- pool->pathitem = (int *)
- (alignptr + (unsigned long long) pool->alignbytes -
- (alignptr % (unsigned long long) pool->alignbytes));
- /* Set the number of items left in the current block. */
- pool->pathitemsleft = pool->itemsperblock;
- }
-
- newitem = pool->pathitem;
- /* Find the next item in the block. */
- pool->pathitem = (int *) ((char *) pool->pathitem + pool->itembytes);
- pool->pathitemsleft--;
- return newitem;
-}
-
-/*****************************************************************************/
-/* */
-/* dummyinit() Initialize the triangle that fills "outer space" and the */
-/* omnipresent subsegment. */
-/* */
-/* The triangle that fills "outer space," called `dummytri', is pointed to */
-/* by every triangle and subsegment on a boundary (be it outer or inner) of */
-/* the triangulation. Also, `dummytri' points to one of the triangles on */
-/* the convex hull (until the holes and concavities are carved), making it */
-/* possible to find a starting triangle for point location. */
-/* */
-/* The omnipresent subsegment, `dummysub', is pointed to by every triangle */
-/* or subsegment that doesn't have a full complement of real subsegments */
-/* to point to. */
-/* */
-/* `dummytri' and `dummysub' are generally required to fulfill only a few */
-/* invariants: their vertices must remain NULL and `dummytri' must always */
-/* be bonded (at offset zero) to some triangle on the convex hull of the */
-/* mesh, via a boundary edge. Otherwise, the connections of `dummytri' and */
-/* `dummysub' may change willy-nilly. This makes it possible to avoid */
-/* writing a good deal of special-case code (in the edge flip, for example) */
-/* for dealing with the boundary of the mesh, places where no subsegment is */
-/* present, and so forth. Other entities are frequently bonded to */
-/* `dummytri' and `dummysub' as if they were real mesh entities, with no */
-/* harm done. */
-/* */
-/*****************************************************************************/
-
-void dummyinit(struct mesh *m, struct behavior *b, int trianglebytes,
- int subsegbytes)
-{
- unsigned long long alignptr;
-
- /* Set up `dummytri', the `triangle' that occupies "outer space." */
- m->dummytribase = (triangle *) trimalloc(trianglebytes +
- m->triangles.alignbytes);
- /* Align `dummytri' on a `triangles.alignbytes'-byte boundary. */
- alignptr = (unsigned long long) m->dummytribase;
- m->dummytri = (triangle *)
- (alignptr + (unsigned long long) m->triangles.alignbytes -
- (alignptr % (unsigned long long) m->triangles.alignbytes));
- /* Initialize the three adjoining triangles to be "outer space." These */
- /* will eventually be changed by various bonding operations, but their */
- /* values don't really matter, as long as they can legally be */
- /* dereferenced. */
- m->dummytri[0] = (triangle) m->dummytri;
- m->dummytri[1] = (triangle) m->dummytri;
- m->dummytri[2] = (triangle) m->dummytri;
- /* Three NULL vertices. */
- m->dummytri[3] = (triangle) NULL;
- m->dummytri[4] = (triangle) NULL;
- m->dummytri[5] = (triangle) NULL;
-
- if (b->usesegments) {
- /* Set up `dummysub', the omnipresent subsegment pointed to by any */
- /* triangle side or subsegment end that isn't attached to a real */
- /* subsegment. */
- m->dummysubbase = (subseg *) trimalloc(subsegbytes +
- m->subsegs.alignbytes);
- /* Align `dummysub' on a `subsegs.alignbytes'-byte boundary. */
- alignptr = (unsigned long long) m->dummysubbase;
- m->dummysub = (subseg *)
- (alignptr + (unsigned long long) m->subsegs.alignbytes -
- (alignptr % (unsigned long long) m->subsegs.alignbytes));
- /* Initialize the two adjoining subsegments to be the omnipresent */
- /* subsegment. These will eventually be changed by various bonding */
- /* operations, but their values don't really matter, as long as they */
- /* can legally be dereferenced. */
- m->dummysub[0] = (subseg) m->dummysub;
- m->dummysub[1] = (subseg) m->dummysub;
- /* Four NULL vertices. */
- m->dummysub[2] = (subseg) NULL;
- m->dummysub[3] = (subseg) NULL;
- m->dummysub[4] = (subseg) NULL;
- m->dummysub[5] = (subseg) NULL;
- /* Initialize the two adjoining triangles to be "outer space." */
- m->dummysub[6] = (subseg) m->dummytri;
- m->dummysub[7] = (subseg) m->dummytri;
- /* Set the boundary marker to zero. */
- * (int *) (m->dummysub + 8) = 0;
-
- /* Initialize the three adjoining subsegments of `dummytri' to be */
- /* the omnipresent subsegment. */
- m->dummytri[6] = (triangle) m->dummysub;
- m->dummytri[7] = (triangle) m->dummysub;
- m->dummytri[8] = (triangle) m->dummysub;
- }
-}
-
-/*****************************************************************************/
-/* */
-/* initializevertexpool() Calculate the size of the vertex data structure */
-/* and initialize its memory pool. */
-/* */
-/* This routine also computes the `vertexmarkindex' and `vertex2triindex' */
-/* indices used to find values within each vertex. */
-/* */
-/*****************************************************************************/
-
-void initializevertexpool(struct mesh *m, struct behavior *b)
-{
- int vertexsize;
-
- /* The index within each vertex at which the boundary marker is found, */
- /* followed by the vertex type. Ensure the vertex marker is aligned to */
- /* a sizeof(int)-byte address. */
- m->vertexmarkindex = ((m->mesh_dim + m->nextras) * sizeof(float) +
- sizeof(int) - 1) /
- sizeof(int);
- vertexsize = (m->vertexmarkindex + 2) * sizeof(int);
- if (b->poly) {
- /* The index within each vertex at which a triangle pointer is found. */
- /* Ensure the pointer is aligned to a sizeof(triangle)-byte address. */
- m->vertex2triindex = (vertexsize + sizeof(triangle) - 1) /
- sizeof(triangle);
- vertexsize = (m->vertex2triindex + 1) * sizeof(triangle);
- }
-
- /* Initialize the pool of vertices. */
- poolinit(&m->vertices, vertexsize, VERTEXPERBLOCK,
- m->invertices > VERTEXPERBLOCK ? m->invertices : VERTEXPERBLOCK,
- sizeof(float));
-}
-
-/*****************************************************************************/
-/* */
-/* initializetrisubpools() Calculate the sizes of the triangle and */
-/* subsegment data structures and initialize */
-/* their memory pools. */
-/* */
-/* This routine also computes the `highorderindex', `elemattribindex', and */
-/* `areaboundindex' indices used to find values within each triangle. */
-/* */
-/*****************************************************************************/
-
-void initializetrisubpools(struct mesh *m, struct behavior *b)
-{
- int trisize;
-
- /* The index within each triangle at which the extra nodes (above three) */
- /* associated with high order elements are found. There are three */
- /* pointers to other triangles, three pointers to corners, and possibly */
- /* three pointers to subsegments before the extra nodes. */
- m->highorderindex = 6 + (b->usesegments * 3);
- /* The number of bytes occupied by a triangle. */
- trisize = ((b->order + 1) * (b->order + 2) / 2 + (m->highorderindex - 3)) *
- sizeof(triangle);
- /* The index within each triangle at which its attributes are found, */
- /* where the index is measured in floats. */
- m->elemattribindex = (trisize + sizeof(float) - 1) / sizeof(float);
- /* The index within each triangle at which the maximum area constraint */
- /* is found, where the index is measured in floats. Note that if the */
- /* `regionattrib' flag is set, an additional attribute will be added. */
- m->areaboundindex = m->elemattribindex + m->eextras + b->regionattrib;
- /* If triangle attributes or an area bound are needed, increase the number */
- /* of bytes occupied by a triangle. */
- if (b->vararea) {
- trisize = (m->areaboundindex + 1) * sizeof(float);
- } else if (m->eextras + b->regionattrib > 0) {
- trisize = m->areaboundindex * sizeof(float);
- }
- /* If a Voronoi diagram or triangle neighbor graph is requested, make */
- /* sure there's room to store an integer index in each triangle. This */
- /* integer index can occupy the same space as the subsegment pointers */
- /* or attributes or area constraint or extra nodes. */
- if ((b->voronoi || b->neighbors) &&
- (trisize < 6 * sizeof(triangle) + sizeof(int))) {
- trisize = 6 * sizeof(triangle) + sizeof(int);
- }
-
- /* Having determined the memory size of a triangle, initialize the pool. */
- poolinit(&m->triangles, trisize, TRIPERBLOCK,
- (2 * m->invertices - 2) > TRIPERBLOCK ? (2 * m->invertices - 2) :
- TRIPERBLOCK, 4);
-
- if (b->usesegments) {
- /* Initialize the pool of subsegments. Take into account all eight */
- /* pointers and one boundary marker. */
- poolinit(&m->subsegs, 8 * sizeof(triangle) + sizeof(int),
- SUBSEGPERBLOCK, SUBSEGPERBLOCK, 4);
-
- /* Initialize the "outer space" triangle and omnipresent subsegment. */
- dummyinit(m, b, m->triangles.itembytes, m->subsegs.itembytes);
- } else {
- /* Initialize the "outer space" triangle. */
- dummyinit(m, b, m->triangles.itembytes, 0);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* triangledealloc() Deallocate space for a triangle, marking it dead. */
-/* */
-/*****************************************************************************/
-
-void triangledealloc(struct mesh *m, triangle *dyingtriangle)
-{
- /* Mark the triangle as dead. This makes it possible to detect dead */
- /* triangles when traversing the list of all triangles. */
- killtri(dyingtriangle);
- pooldealloc(&m->triangles, (int *) dyingtriangle);
-}
-
-/*****************************************************************************/
-/* */
-/* triangletraverse() Traverse the triangles, skipping dead ones. */
-/* */
-/*****************************************************************************/
-
-triangle *triangletraverse(struct mesh *m)
-{
- triangle *newtriangle;
-
- do {
- newtriangle = (triangle *) traverse(&m->triangles);
- if (newtriangle == (triangle *) NULL) {
- return (triangle *) NULL;
- }
- } while (deadtri(newtriangle)); /* Skip dead ones. */
- return newtriangle;
-}
-
-/*****************************************************************************/
-/* */
-/* subsegdealloc() Deallocate space for a subsegment, marking it dead. */
-/* */
-/*****************************************************************************/
-
-void subsegdealloc(struct mesh *m, subseg *dyingsubseg)
-{
- /* Mark the subsegment as dead. This makes it possible to detect dead */
- /* subsegments when traversing the list of all subsegments. */
- killsubseg(dyingsubseg);
- pooldealloc(&m->subsegs, (int *) dyingsubseg);
-}
-
-/*****************************************************************************/
-/* */
-/* subsegtraverse() Traverse the subsegments, skipping dead ones. */
-/* */
-/*****************************************************************************/
-
-subseg *subsegtraverse(struct mesh *m)
-{
- subseg *newsubseg;
-
- do {
- newsubseg = (subseg *) traverse(&m->subsegs);
- if (newsubseg == (subseg *) NULL) {
- return (subseg *) NULL;
- }
- } while (deadsubseg(newsubseg)); /* Skip dead ones. */
- return newsubseg;
-}
-
-/*****************************************************************************/
-/* */
-/* vertexdealloc() Deallocate space for a vertex, marking it dead. */
-/* */
-/*****************************************************************************/
-
-void vertexdealloc(struct mesh *m, vertex dyingvertex)
-{
- /* Mark the vertex as dead. This makes it possible to detect dead */
- /* vertices when traversing the list of all vertices. */
- setvertextype(dyingvertex, DEADVERTEX);
- pooldealloc(&m->vertices, (int *) dyingvertex);
-}
-
-/*****************************************************************************/
-/* */
-/* vertextraverse() Traverse the vertices, skipping dead ones. */
-/* */
-/*****************************************************************************/
-
-vertex vertextraverse(struct mesh *m)
-{
- vertex newvertex;
-
- do {
- newvertex = (vertex) traverse(&m->vertices);
- if (newvertex == (vertex) NULL) {
- return (vertex) NULL;
- }
- } while (vertextype(newvertex) == DEADVERTEX); /* Skip dead ones. */
- return newvertex;
-}
-
-/*****************************************************************************/
-/* */
-/* getvertex() Get a specific vertex, by number, from the list. */
-/* */
-/* The first vertex is number 'firstnumber'. */
-/* */
-/* Note that this takes O(n) time (with a small constant, if VERTEXPERBLOCK */
-/* is large). I don't care to take the trouble to make it work in constant */
-/* time. */
-/* */
-/*****************************************************************************/
-
-vertex getvertex(struct mesh *m, struct behavior *b, int number)
-{
- int **getblock;
- char *foundvertex;
- unsigned long long alignptr;
- int current;
-
- getblock = m->vertices.firstblock;
- current = b->firstnumber;
-
- /* Find the right block. */
- if (current + m->vertices.itemsfirstblock <= number) {
- getblock = (int **) *getblock;
- current += m->vertices.itemsfirstblock;
- while (current + m->vertices.itemsperblock <= number) {
- getblock = (int **) *getblock;
- current += m->vertices.itemsperblock;
- }
- }
-
- /* Now find the right vertex. */
- alignptr = (unsigned long long) (getblock + 1);
- foundvertex = (char *) (alignptr + (unsigned long long) m->vertices.alignbytes -
- (alignptr % (unsigned long long) m->vertices.alignbytes));
- return (vertex) (foundvertex + m->vertices.itembytes * (number - current));
-}
-
-/*****************************************************************************/
-/* */
-/* triangledeinit() Free all remaining allocated memory. */
-/* */
-/*****************************************************************************/
-
-void triangledeinit(struct mesh *m, struct behavior *b)
-{
- pooldeinit(&m->triangles);
- trifree((int *) m->dummytribase);
- if (b->usesegments) {
- pooldeinit(&m->subsegs);
- trifree((int *) m->dummysubbase);
- }
- pooldeinit(&m->vertices);
-}
-
-/** **/
-/** **/
-/********* Memory management routines end here *********/
-
-/********* Constructors begin here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* maketriangle() Create a new triangle with orientation zero. */
-/* */
-/*****************************************************************************/
-
-void maketriangle(struct mesh *m, struct behavior *b, struct otri *newotri)
-{
- int i;
-
- newotri->tri = (triangle *) poolalloc(&m->triangles);
- /* Initialize the three adjoining triangles to be "outer space". */
- newotri->tri[0] = (triangle) m->dummytri;
- newotri->tri[1] = (triangle) m->dummytri;
- newotri->tri[2] = (triangle) m->dummytri;
- /* Three NULL vertices. */
- newotri->tri[3] = (triangle) NULL;
- newotri->tri[4] = (triangle) NULL;
- newotri->tri[5] = (triangle) NULL;
- if (b->usesegments) {
- /* Initialize the three adjoining subsegments to be the omnipresent */
- /* subsegment. */
- newotri->tri[6] = (triangle) m->dummysub;
- newotri->tri[7] = (triangle) m->dummysub;
- newotri->tri[8] = (triangle) m->dummysub;
- }
- for (i = 0; i < m->eextras; i++) {
- setelemattribute(*newotri, i, 0.0);
- }
- if (b->vararea) {
- setareabound(*newotri, -1.0);
- }
-
- newotri->orient = 0;
-}
-
-/*****************************************************************************/
-/* */
-/* makesubseg() Create a new subsegment with orientation zero. */
-/* */
-/*****************************************************************************/
-
-void makesubseg(struct mesh *m, struct osub *newsubseg)
-{
- newsubseg->ss = (subseg *) poolalloc(&m->subsegs);
- /* Initialize the two adjoining subsegments to be the omnipresent */
- /* subsegment. */
- newsubseg->ss[0] = (subseg) m->dummysub;
- newsubseg->ss[1] = (subseg) m->dummysub;
- /* Four NULL vertices. */
- newsubseg->ss[2] = (subseg) NULL;
- newsubseg->ss[3] = (subseg) NULL;
- newsubseg->ss[4] = (subseg) NULL;
- newsubseg->ss[5] = (subseg) NULL;
- /* Initialize the two adjoining triangles to be "outer space." */
- newsubseg->ss[6] = (subseg) m->dummytri;
- newsubseg->ss[7] = (subseg) m->dummytri;
- /* Set the boundary marker to zero. */
- setmark(*newsubseg, 0);
-
- newsubseg->ssorient = 0;
-}
-
-/** **/
-/** **/
-/********* Constructors end here *********/
-
-/********* Geometric primitives begin here *********/
-/** **/
-/** **/
-
-/* The adaptive exact arithmetic geometric predicates implemented herein are */
-/* described in detail in my paper, "Adaptive Precision Floating-Point */
-/* Arithmetic and Fast Robust Geometric Predicates." See the header for a */
-/* full citation. */
-
-/* Which of the following two methods of finding the absolute values is */
-/* fastest is compiler-dependent. A few compilers can inline and optimize */
-/* the fabs() call; but most will incur the overhead of a function call, */
-/* which is disastrously slow. A faster way on IEEE machines might be to */
-/* mask the appropriate bit, but that's difficult to do in C without */
-/* forcing the value to be stored to memory (rather than be kept in the */
-/* register to which the optimizer assigned it). */
-
-#define Absolute(a) ((a) >= 0.0 ? (a) : -(a))
-/* #define Absolute(a) fabs(a) */
-
-/* Many of the operations are broken up into two pieces, a main part that */
-/* performs an approximate operation, and a "tail" that computes the */
-/* roundoff error of that operation. */
-/* */
-/* The operations Fast_Two_Sum(), Fast_Two_Diff(), Two_Sum(), Two_Diff(), */
-/* Split(), and Two_Product() are all implemented as described in the */
-/* reference. Each of these macros requires certain variables to be */
-/* defined in the calling routine. The variables `bvirt', `c', `abig', */
-/* `_i', `_j', `_k', `_l', `_m', and `_n' are declared `INEXACT' because */
-/* they store the result of an operation that may incur roundoff error. */
-/* The input parameter `x' (or the highest numbered `x_' parameter) must */
-/* also be declared `INEXACT'. */
-
-#define Fast_Two_Sum_Tail(a, b, x, y) \
- bvirt = x - a; \
- y = b - bvirt
-
-#define Fast_Two_Sum(a, b, x, y) \
- x = (float) (a + b); \
- Fast_Two_Sum_Tail(a, b, x, y)
-
-#define Two_Sum_Tail(a, b, x, y) \
- bvirt = (float) (x - a); \
- avirt = x - bvirt; \
- bround = b - bvirt; \
- around = a - avirt; \
- y = around + bround
-
-#define Two_Sum(a, b, x, y) \
- x = (float) (a + b); \
- Two_Sum_Tail(a, b, x, y)
-
-#define Two_Diff_Tail(a, b, x, y) \
- bvirt = (float) (a - x); \
- avirt = x + bvirt; \
- bround = bvirt - b; \
- around = a - avirt; \
- y = around + bround
-
-#define Two_Diff(a, b, x, y) \
- x = (float) (a - b); \
- Two_Diff_Tail(a, b, x, y)
-
-#define Split(a, ahi, alo) \
- c = (float) (splitter * a); \
- abig = (float) (c - a); \
- ahi = c - abig; \
- alo = a - ahi
-
-#define Two_Product_Tail(a, b, x, y) \
- Split(a, ahi, alo); \
- Split(b, bhi, blo); \
- err1 = x - (ahi * bhi); \
- err2 = err1 - (alo * bhi); \
- err3 = err2 - (ahi * blo); \
- y = (alo * blo) - err3
-
-#define Two_Product(a, b, x, y) \
- x = (float) (a * b); \
- Two_Product_Tail(a, b, x, y)
-
-/* Two_Product_Presplit() is Two_Product() where one of the inputs has */
-/* already been split. Avoids redundant splitting. */
-
-#define Two_Product_Presplit(a, b, bhi, blo, x, y) \
- x = (float) (a * b); \
- Split(a, ahi, alo); \
- err1 = x - (ahi * bhi); \
- err2 = err1 - (alo * bhi); \
- err3 = err2 - (ahi * blo); \
- y = (alo * blo) - err3
-
-/* Square() can be done more quickly than Two_Product(). */
-
-#define Square_Tail(a, x, y) \
- Split(a, ahi, alo); \
- err1 = x - (ahi * ahi); \
- err3 = err1 - ((ahi + ahi) * alo); \
- y = (alo * alo) - err3
-
-#define Square(a, x, y) \
- x = (float) (a * a); \
- Square_Tail(a, x, y)
-
-/* Macros for summing expansions of various fixed lengths. These are all */
-/* unrolled versions of Expansion_Sum(). */
-
-#define Two_One_Sum(a1, a0, b, x2, x1, x0) \
- Two_Sum(a0, b , _i, x0); \
- Two_Sum(a1, _i, x2, x1)
-
-#define Two_One_Diff(a1, a0, b, x2, x1, x0) \
- Two_Diff(a0, b , _i, x0); \
- Two_Sum( a1, _i, x2, x1)
-
-#define Two_Two_Sum(a1, a0, b1, b0, x3, x2, x1, x0) \
- Two_One_Sum(a1, a0, b0, _j, _0, x0); \
- Two_One_Sum(_j, _0, b1, x3, x2, x1)
-
-#define Two_Two_Diff(a1, a0, b1, b0, x3, x2, x1, x0) \
- Two_One_Diff(a1, a0, b0, _j, _0, x0); \
- Two_One_Diff(_j, _0, b1, x3, x2, x1)
-
-/* Macro for multiplying a two-component expansion by a single component. */
-
-#define Two_One_Product(a1, a0, b, x3, x2, x1, x0) \
- Split(b, bhi, blo); \
- Two_Product_Presplit(a0, b, bhi, blo, _i, x0); \
- Two_Product_Presplit(a1, b, bhi, blo, _j, _0); \
- Two_Sum(_i, _0, _k, x1); \
- Fast_Two_Sum(_j, _k, x3, x2)
-
-/*****************************************************************************/
-/* */
-/* exactinit() Initialize the variables used for exact arithmetic. */
-/* */
-/* `epsilon' is the largest power of two such that 1.0 + epsilon = 1.0 in */
-/* floating-point arithmetic. `epsilon' bounds the relative roundoff */
-/* error. It is used for floating-point error analysis. */
-/* */
-/* `splitter' is used to split floating-point numbers into two half- */
-/* length significands for exact multiplication. */
-/* */
-/* I imagine that a highly optimizing compiler might be too smart for its */
-/* own good, and somehow cause this routine to fail, if it pretends that */
-/* floating-point arithmetic is too much like real arithmetic. */
-/* */
-/* Don't change this routine unless you fully understand it. */
-/* */
-/*****************************************************************************/
-
-void exactinit()
-{
- float half;
- float check, lastcheck;
- int every_other;
- every_other = 1;
- half = 0.5;
- epsilon = 1.0;
- splitter = 1.0;
- check = 1.0;
- /* Repeatedly divide `epsilon' by two until it is too small to add to */
- /* one without causing roundoff. (Also check if the sum is equal to */
- /* the previous sum, for machines that round up instead of using exact */
- /* rounding. Not that these routines will work on such machines.) */
- do {
- lastcheck = check;
- epsilon *= half;
- if (every_other) {
- splitter *= 2.0;
- }
- every_other = !every_other;
- check = 1.0 + epsilon;
- } while ((check != 1.0) && (check != lastcheck));
- splitter += 1.0;
- /* Error bounds for orientation and incircle tests. */
- resulterrbound = (3.0 + 8.0 * epsilon) * epsilon;
- ccwerrboundA = (3.0 + 16.0 * epsilon) * epsilon;
- ccwerrboundB = (2.0 + 12.0 * epsilon) * epsilon;
- ccwerrboundC = (9.0 + 64.0 * epsilon) * epsilon * epsilon;
- iccerrboundA = (10.0 + 96.0 * epsilon) * epsilon;
- iccerrboundB = (4.0 + 48.0 * epsilon) * epsilon;
- iccerrboundC = (44.0 + 576.0 * epsilon) * epsilon * epsilon;
- o3derrboundA = (7.0 + 56.0 * epsilon) * epsilon;
- o3derrboundB = (3.0 + 28.0 * epsilon) * epsilon;
- o3derrboundC = (26.0 + 288.0 * epsilon) * epsilon * epsilon;
-}
-
-/*****************************************************************************/
-/* */
-/* fast_expansion_sum_zeroelim() Sum two expansions, eliminating zero */
-/* components from the output expansion. */
-/* */
-/* Sets h = e + f. See my Robust Predicates paper for details. */
-/* */
-/* If round-to-even is used (as with IEEE 754), maintains the strongly */
-/* nonoverlapping property. (That is, if e is strongly nonoverlapping, h */
-/* will be also.) Does NOT maintain the nonoverlapping or nonadjacent */
-/* properties. */
-/* */
-/*****************************************************************************/
-
-int fast_expansion_sum_zeroelim(int elen, float *e, int flen, float *f, float *h)
-{
- float Q;
- float Qnew;
- float hh;
- float bvirt;
- float avirt, bround, around;
- int eindex, findex, hindex;
- float enow, fnow;
-
- enow = e[0];
- fnow = f[0];
- eindex = findex = 0;
- if ((fnow > enow) == (fnow > -enow)) {
- Q = enow;
- enow = e[++eindex];
- } else {
- Q = fnow;
- fnow = f[++findex];
- }
- hindex = 0;
- if ((eindex < elen) && (findex < flen)) {
- if ((fnow > enow) == (fnow > -enow)) {
- Fast_Two_Sum(enow, Q, Qnew, hh);
- enow = e[++eindex];
- } else {
- Fast_Two_Sum(fnow, Q, Qnew, hh);
- fnow = f[++findex];
- }
- Q = Qnew;
- if (hh != 0.0) {
- h[hindex++] = hh;
- }
- while ((eindex < elen) && (findex < flen)) {
- if ((fnow > enow) == (fnow > -enow)) {
- Two_Sum(Q, enow, Qnew, hh);
- enow = e[++eindex];
- } else {
- Two_Sum(Q, fnow, Qnew, hh);
- fnow = f[++findex];
- }
- Q = Qnew;
- if (hh != 0.0) {
- h[hindex++] = hh;
- }
- }
- }
- while (eindex < elen) {
- Two_Sum(Q, enow, Qnew, hh);
- enow = e[++eindex];
- Q = Qnew;
- if (hh != 0.0) {
- h[hindex++] = hh;
- }
- }
- while (findex < flen) {
- Two_Sum(Q, fnow, Qnew, hh);
- fnow = f[++findex];
- Q = Qnew;
- if (hh != 0.0) {
- h[hindex++] = hh;
- }
- }
- if ((Q != 0.0) || (hindex == 0)) {
- h[hindex++] = Q;
- }
- return hindex;
-}
-
-/*****************************************************************************/
-/* */
-/* scale_expansion_zeroelim() Multiply an expansion by a scalar, */
-/* eliminating zero components from the */
-/* output expansion. */
-/* */
-/* Sets h = be. See my Robust Predicates paper for details. */
-/* */
-/* Maintains the nonoverlapping property. If round-to-even is used (as */
-/* with IEEE 754), maintains the strongly nonoverlapping and nonadjacent */
-/* properties as well. (That is, if e has one of these properties, so */
-/* will h.) */
-/* */
-/*****************************************************************************/
-
-int scale_expansion_zeroelim(int elen, float *e, float b, float *h)
-{
- float Q, sum;
- float hh;
- float product1;
- float product0;
- int eindex, hindex;
- float enow;
- float bvirt;
- float avirt, bround, around;
- float c;
- float abig;
- float ahi, alo, bhi, blo;
- float err1, err2, err3;
-
- Split(b, bhi, blo);
- Two_Product_Presplit(e[0], b, bhi, blo, Q, hh);
- hindex = 0;
- if (hh != 0) {
- h[hindex++] = hh;
- }
- for (eindex = 1; eindex < elen; eindex++) {
- enow = e[eindex];
- Two_Product_Presplit(enow, b, bhi, blo, product1, product0);
- Two_Sum(Q, product0, sum, hh);
- if (hh != 0) {
- h[hindex++] = hh;
- }
- Fast_Two_Sum(product1, sum, Q, hh);
- if (hh != 0) {
- h[hindex++] = hh;
- }
- }
- if ((Q != 0.0) || (hindex == 0)) {
- h[hindex++] = Q;
- }
- return hindex;
-}
-
-/*****************************************************************************/
-/* */
-/* estimate() Produce a one-word estimate of an expansion's value. */
-/* */
-/* See my Robust Predicates paper for details. */
-/* */
-/*****************************************************************************/
-
-float estimate(int elen, float *e)
-{
- float Q;
- int eindex;
- Q = e[0];
- for (eindex = 1; eindex < elen; eindex++) {
- Q += e[eindex];
- }
- return Q;
-}
-
-/*****************************************************************************/
-/* */
-/* counterclockwise() Return a positive value if the points pa, pb, and */
-/* pc occur in counterclockwise order; a negative */
-/* value if they occur in clockwise order; and zero */
-/* if they are collinear. The result is also a rough */
-/* approximation of twice the signed area of the */
-/* triangle defined by the three points. */
-/* */
-/* Uses exact arithmetic if necessary to ensure a correct answer. The */
-/* result returned is the determinant of a matrix. This determinant is */
-/* computed adaptively, in the sense that exact arithmetic is used only to */
-/* the degree it is needed to ensure that the returned value has the */
-/* correct sign. Hence, this function is usually quite fast, but will run */
-/* more slowly when the input points are collinear or nearly so. */
-/* */
-/* See my Robust Predicates paper for details. */
-/* */
-/*****************************************************************************/
-
-float counterclockwiseadapt(vertex pa, vertex pb, vertex pc, float detsum)
-{
- float acx, acy, bcx, bcy;
- float acxtail, acytail, bcxtail, bcytail;
- float detleft, detright;
- float detlefttail, detrighttail;
- float det, errbound;
- float B[4], C1[8], C2[12], D[16];
- float B3;
- int C1length, C2length, Dlength;
- float u[4];
- float u3;
- float s1, t1;
- float s0, t0;
-
- float bvirt;
- float avirt, bround, around;
- float c;
- float abig;
- float ahi, alo, bhi, blo;
- float err1, err2, err3;
- float _i, _j;
- float _0;
-
- acx = (float) (pa[0] - pc[0]);
- bcx = (float) (pb[0] - pc[0]);
- acy = (float) (pa[1] - pc[1]);
- bcy = (float) (pb[1] - pc[1]);
-
- Two_Product(acx, bcy, detleft, detlefttail);
- Two_Product(acy, bcx, detright, detrighttail);
-
- Two_Two_Diff(detleft, detlefttail, detright, detrighttail,
- B3, B[2], B[1], B[0]);
- B[3] = B3;
-
- det = estimate(4, B);
- errbound = ccwerrboundB * detsum;
- if ((det >= errbound) || (-det >= errbound)) {
- return det;
- }
-
- Two_Diff_Tail(pa[0], pc[0], acx, acxtail);
- Two_Diff_Tail(pb[0], pc[0], bcx, bcxtail);
- Two_Diff_Tail(pa[1], pc[1], acy, acytail);
- Two_Diff_Tail(pb[1], pc[1], bcy, bcytail);
-
- if ((acxtail == 0.0) && (acytail == 0.0)
- && (bcxtail == 0.0) && (bcytail == 0.0)) {
- return det;
- }
-
- errbound = ccwerrboundC * detsum + resulterrbound * Absolute(det);
- det += (acx * bcytail + bcy * acxtail)
- - (acy * bcxtail + bcx * acytail);
- if ((det >= errbound) || (-det >= errbound)) {
- return det;
- }
-
- Two_Product(acxtail, bcy, s1, s0);
- Two_Product(acytail, bcx, t1, t0);
- Two_Two_Diff(s1, s0, t1, t0, u3, u[2], u[1], u[0]);
- u[3] = u3;
- C1length = fast_expansion_sum_zeroelim(4, B, 4, u, C1);
-
- Two_Product(acx, bcytail, s1, s0);
- Two_Product(acy, bcxtail, t1, t0);
- Two_Two_Diff(s1, s0, t1, t0, u3, u[2], u[1], u[0]);
- u[3] = u3;
- C2length = fast_expansion_sum_zeroelim(C1length, C1, 4, u, C2);
-
- Two_Product(acxtail, bcytail, s1, s0);
- Two_Product(acytail, bcxtail, t1, t0);
- Two_Two_Diff(s1, s0, t1, t0, u3, u[2], u[1], u[0]);
- u[3] = u3;
- Dlength = fast_expansion_sum_zeroelim(C2length, C2, 4, u, D);
-
- return(D[Dlength - 1]);
-}
-
-float counterclockwise(struct mesh *m, struct behavior *b,
- vertex pa, vertex pb, vertex pc)
-{
- float detleft, detright, det;
- float detsum, errbound;
-
- m->counterclockcount++;
-
- detleft = (pa[0] - pc[0]) * (pb[1] - pc[1]);
- detright = (pa[1] - pc[1]) * (pb[0] - pc[0]);
- det = detleft - detright;
-
- if (b->noexact) {
- return det;
- }
-
- if (detleft > 0.0) {
- if (detright <= 0.0) {
- return det;
- } else {
- detsum = detleft + detright;
- }
- } else if (detleft < 0.0) {
- if (detright >= 0.0) {
- return det;
- } else {
- detsum = -detleft - detright;
- }
- } else {
- return det;
- }
-
- errbound = ccwerrboundA * detsum;
- if ((det >= errbound) || (-det >= errbound)) {
- return det;
- }
-
- return counterclockwiseadapt(pa, pb, pc, detsum);
-}
-
-/*****************************************************************************/
-/* */
-/* incircle() Return a positive value if the point pd lies inside the */
-/* circle passing through pa, pb, and pc; a negative value if */
-/* it lies outside; and zero if the four points are cocircular.*/
-/* The points pa, pb, and pc must be in counterclockwise */
-/* order, or the sign of the result will be reversed. */
-/* */
-/* Uses exact arithmetic if necessary to ensure a correct answer. The */
-/* result returned is the determinant of a matrix. This determinant is */
-/* computed adaptively, in the sense that exact arithmetic is used only to */
-/* the degree it is needed to ensure that the returned value has the */
-/* correct sign. Hence, this function is usually quite fast, but will run */
-/* more slowly when the input points are cocircular or nearly so. */
-/* */
-/* See my Robust Predicates paper for details. */
-/* */
-/*****************************************************************************/
-
-float incircleadapt(vertex pa, vertex pb, vertex pc, vertex pd, float permanent)
-{
- float adx, bdx, cdx, ady, bdy, cdy;
- float det, errbound;
-
- float bdxcdy1, cdxbdy1, cdxady1, adxcdy1, adxbdy1, bdxady1;
- float bdxcdy0, cdxbdy0, cdxady0, adxcdy0, adxbdy0, bdxady0;
- float bc[4], ca[4], ab[4];
- float bc3, ca3, ab3;
- float axbc[8], axxbc[16], aybc[8], ayybc[16], adet[32];
- int axbclen, axxbclen, aybclen, ayybclen, alen;
- float bxca[8], bxxca[16], byca[8], byyca[16], bdet[32];
- int bxcalen, bxxcalen, bycalen, byycalen, blen;
- float cxab[8], cxxab[16], cyab[8], cyyab[16], cdet[32];
- int cxablen, cxxablen, cyablen, cyyablen, clen;
- float abdet[64];
- int ablen;
- float fin1[1152], fin2[1152];
- float *finnow, *finother, *finswap;
- int finlength;
-
- float adxtail, bdxtail, cdxtail, adytail, bdytail, cdytail;
- float adxadx1, adyady1, bdxbdx1, bdybdy1, cdxcdx1, cdycdy1;
- float adxadx0, adyady0, bdxbdx0, bdybdy0, cdxcdx0, cdycdy0;
- float aa[4], bb[4], cc[4];
- float aa3, bb3, cc3;
- float ti1, tj1;
- float ti0, tj0;
- float u[4], v[4];
- float u3, v3;
- float temp8[8], temp16a[16], temp16b[16], temp16c[16];
- float temp32a[32], temp32b[32], temp48[48], temp64[64];
- int temp8len, temp16alen, temp16blen, temp16clen;
- int temp32alen, temp32blen, temp48len, temp64len;
- float axtbb[8], axtcc[8], aytbb[8], aytcc[8];
- int axtbblen, axtcclen, aytbblen, aytcclen;
- float bxtaa[8], bxtcc[8], bytaa[8], bytcc[8];
- int bxtaalen, bxtcclen, bytaalen, bytcclen;
- float cxtaa[8], cxtbb[8], cytaa[8], cytbb[8];
- int cxtaalen, cxtbblen, cytaalen, cytbblen;
- float axtbc[8], aytbc[8], bxtca[8], bytca[8], cxtab[8], cytab[8];
- int axtbclen, aytbclen, bxtcalen, bytcalen, cxtablen, cytablen;
- float axtbct[16], aytbct[16], bxtcat[16], bytcat[16], cxtabt[16], cytabt[16];
- int axtbctlen, aytbctlen, bxtcatlen, bytcatlen, cxtabtlen, cytabtlen;
- float axtbctt[8], aytbctt[8], bxtcatt[8];
- float bytcatt[8], cxtabtt[8], cytabtt[8];
- int axtbcttlen, aytbcttlen, bxtcattlen, bytcattlen, cxtabttlen, cytabttlen;
- float abt[8], bct[8], cat[8];
- int abtlen, bctlen, catlen;
- float abtt[4], bctt[4], catt[4];
- int abttlen, bcttlen, cattlen;
- float abtt3, bctt3, catt3;
- float negate;
-
- float bvirt;
- float avirt, bround, around;
- float c;
- float abig;
- float ahi, alo, bhi, blo;
- float err1, err2, err3;
- float _i, _j;
- float _0;
-
- adx = (float) (pa[0] - pd[0]);
- bdx = (float) (pb[0] - pd[0]);
- cdx = (float) (pc[0] - pd[0]);
- ady = (float) (pa[1] - pd[1]);
- bdy = (float) (pb[1] - pd[1]);
- cdy = (float) (pc[1] - pd[1]);
-
- Two_Product(bdx, cdy, bdxcdy1, bdxcdy0);
- Two_Product(cdx, bdy, cdxbdy1, cdxbdy0);
- Two_Two_Diff(bdxcdy1, bdxcdy0, cdxbdy1, cdxbdy0, bc3, bc[2], bc[1], bc[0]);
- bc[3] = bc3;
- axbclen = scale_expansion_zeroelim(4, bc, adx, axbc);
- axxbclen = scale_expansion_zeroelim(axbclen, axbc, adx, axxbc);
- aybclen = scale_expansion_zeroelim(4, bc, ady, aybc);
- ayybclen = scale_expansion_zeroelim(aybclen, aybc, ady, ayybc);
- alen = fast_expansion_sum_zeroelim(axxbclen, axxbc, ayybclen, ayybc, adet);
-
- Two_Product(cdx, ady, cdxady1, cdxady0);
- Two_Product(adx, cdy, adxcdy1, adxcdy0);
- Two_Two_Diff(cdxady1, cdxady0, adxcdy1, adxcdy0, ca3, ca[2], ca[1], ca[0]);
- ca[3] = ca3;
- bxcalen = scale_expansion_zeroelim(4, ca, bdx, bxca);
- bxxcalen = scale_expansion_zeroelim(bxcalen, bxca, bdx, bxxca);
- bycalen = scale_expansion_zeroelim(4, ca, bdy, byca);
- byycalen = scale_expansion_zeroelim(bycalen, byca, bdy, byyca);
- blen = fast_expansion_sum_zeroelim(bxxcalen, bxxca, byycalen, byyca, bdet);
-
- Two_Product(adx, bdy, adxbdy1, adxbdy0);
- Two_Product(bdx, ady, bdxady1, bdxady0);
- Two_Two_Diff(adxbdy1, adxbdy0, bdxady1, bdxady0, ab3, ab[2], ab[1], ab[0]);
- ab[3] = ab3;
- cxablen = scale_expansion_zeroelim(4, ab, cdx, cxab);
- cxxablen = scale_expansion_zeroelim(cxablen, cxab, cdx, cxxab);
- cyablen = scale_expansion_zeroelim(4, ab, cdy, cyab);
- cyyablen = scale_expansion_zeroelim(cyablen, cyab, cdy, cyyab);
- clen = fast_expansion_sum_zeroelim(cxxablen, cxxab, cyyablen, cyyab, cdet);
-
- ablen = fast_expansion_sum_zeroelim(alen, adet, blen, bdet, abdet);
- finlength = fast_expansion_sum_zeroelim(ablen, abdet, clen, cdet, fin1);
-
- det = estimate(finlength, fin1);
- errbound = iccerrboundB * permanent;
- if ((det >= errbound) || (-det >= errbound)) {
- return det;
- }
-
- Two_Diff_Tail(pa[0], pd[0], adx, adxtail);
- Two_Diff_Tail(pa[1], pd[1], ady, adytail);
- Two_Diff_Tail(pb[0], pd[0], bdx, bdxtail);
- Two_Diff_Tail(pb[1], pd[1], bdy, bdytail);
- Two_Diff_Tail(pc[0], pd[0], cdx, cdxtail);
- Two_Diff_Tail(pc[1], pd[1], cdy, cdytail);
- if ((adxtail == 0.0) && (bdxtail == 0.0) && (cdxtail == 0.0)
- && (adytail == 0.0) && (bdytail == 0.0) && (cdytail == 0.0)) {
- return det;
- }
-
- errbound = iccerrboundC * permanent + resulterrbound * Absolute(det);
- det += ((adx * adx + ady * ady) * ((bdx * cdytail + cdy * bdxtail)
- - (bdy * cdxtail + cdx * bdytail))
- + 2.0 * (adx * adxtail + ady * adytail) * (bdx * cdy - bdy * cdx))
- + ((bdx * bdx + bdy * bdy) * ((cdx * adytail + ady * cdxtail)
- - (cdy * adxtail + adx * cdytail))
- + 2.0 * (bdx * bdxtail + bdy * bdytail) * (cdx * ady - cdy * adx))
- + ((cdx * cdx + cdy * cdy) * ((adx * bdytail + bdy * adxtail)
- - (ady * bdxtail + bdx * adytail))
- + 2.0 * (cdx * cdxtail + cdy * cdytail) * (adx * bdy - ady * bdx));
- if ((det >= errbound) || (-det >= errbound)) {
- return det;
- }
-
- finnow = fin1;
- finother = fin2;
-
- if ((bdxtail != 0.0) || (bdytail != 0.0)
- || (cdxtail != 0.0) || (cdytail != 0.0)) {
- Square(adx, adxadx1, adxadx0);
- Square(ady, adyady1, adyady0);
- Two_Two_Sum(adxadx1, adxadx0, adyady1, adyady0, aa3, aa[2], aa[1], aa[0]);
- aa[3] = aa3;
- }
- if ((cdxtail != 0.0) || (cdytail != 0.0)
- || (adxtail != 0.0) || (adytail != 0.0)) {
- Square(bdx, bdxbdx1, bdxbdx0);
- Square(bdy, bdybdy1, bdybdy0);
- Two_Two_Sum(bdxbdx1, bdxbdx0, bdybdy1, bdybdy0, bb3, bb[2], bb[1], bb[0]);
- bb[3] = bb3;
- }
- if ((adxtail != 0.0) || (adytail != 0.0)
- || (bdxtail != 0.0) || (bdytail != 0.0)) {
- Square(cdx, cdxcdx1, cdxcdx0);
- Square(cdy, cdycdy1, cdycdy0);
- Two_Two_Sum(cdxcdx1, cdxcdx0, cdycdy1, cdycdy0, cc3, cc[2], cc[1], cc[0]);
- cc[3] = cc3;
- }
-
- if (adxtail != 0.0) {
- axtbclen = scale_expansion_zeroelim(4, bc, adxtail, axtbc);
- temp16alen = scale_expansion_zeroelim(axtbclen, axtbc, 2.0 * adx,
- temp16a);
-
- axtcclen = scale_expansion_zeroelim(4, cc, adxtail, axtcc);
- temp16blen = scale_expansion_zeroelim(axtcclen, axtcc, bdy, temp16b);
-
- axtbblen = scale_expansion_zeroelim(4, bb, adxtail, axtbb);
- temp16clen = scale_expansion_zeroelim(axtbblen, axtbb, -cdy, temp16c);
-
- temp32alen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16clen, temp16c,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (adytail != 0.0) {
- aytbclen = scale_expansion_zeroelim(4, bc, adytail, aytbc);
- temp16alen = scale_expansion_zeroelim(aytbclen, aytbc, 2.0 * ady,
- temp16a);
-
- aytbblen = scale_expansion_zeroelim(4, bb, adytail, aytbb);
- temp16blen = scale_expansion_zeroelim(aytbblen, aytbb, cdx, temp16b);
-
- aytcclen = scale_expansion_zeroelim(4, cc, adytail, aytcc);
- temp16clen = scale_expansion_zeroelim(aytcclen, aytcc, -bdx, temp16c);
-
- temp32alen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16clen, temp16c,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (bdxtail != 0.0) {
- bxtcalen = scale_expansion_zeroelim(4, ca, bdxtail, bxtca);
- temp16alen = scale_expansion_zeroelim(bxtcalen, bxtca, 2.0 * bdx,
- temp16a);
-
- bxtaalen = scale_expansion_zeroelim(4, aa, bdxtail, bxtaa);
- temp16blen = scale_expansion_zeroelim(bxtaalen, bxtaa, cdy, temp16b);
-
- bxtcclen = scale_expansion_zeroelim(4, cc, bdxtail, bxtcc);
- temp16clen = scale_expansion_zeroelim(bxtcclen, bxtcc, -ady, temp16c);
-
- temp32alen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16clen, temp16c,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (bdytail != 0.0) {
- bytcalen = scale_expansion_zeroelim(4, ca, bdytail, bytca);
- temp16alen = scale_expansion_zeroelim(bytcalen, bytca, 2.0 * bdy,
- temp16a);
-
- bytcclen = scale_expansion_zeroelim(4, cc, bdytail, bytcc);
- temp16blen = scale_expansion_zeroelim(bytcclen, bytcc, adx, temp16b);
-
- bytaalen = scale_expansion_zeroelim(4, aa, bdytail, bytaa);
- temp16clen = scale_expansion_zeroelim(bytaalen, bytaa, -cdx, temp16c);
-
- temp32alen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16clen, temp16c,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (cdxtail != 0.0) {
- cxtablen = scale_expansion_zeroelim(4, ab, cdxtail, cxtab);
- temp16alen = scale_expansion_zeroelim(cxtablen, cxtab, 2.0 * cdx,
- temp16a);
-
- cxtbblen = scale_expansion_zeroelim(4, bb, cdxtail, cxtbb);
- temp16blen = scale_expansion_zeroelim(cxtbblen, cxtbb, ady, temp16b);
-
- cxtaalen = scale_expansion_zeroelim(4, aa, cdxtail, cxtaa);
- temp16clen = scale_expansion_zeroelim(cxtaalen, cxtaa, -bdy, temp16c);
-
- temp32alen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16clen, temp16c,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (cdytail != 0.0) {
- cytablen = scale_expansion_zeroelim(4, ab, cdytail, cytab);
- temp16alen = scale_expansion_zeroelim(cytablen, cytab, 2.0 * cdy,
- temp16a);
-
- cytaalen = scale_expansion_zeroelim(4, aa, cdytail, cytaa);
- temp16blen = scale_expansion_zeroelim(cytaalen, cytaa, bdx, temp16b);
-
- cytbblen = scale_expansion_zeroelim(4, bb, cdytail, cytbb);
- temp16clen = scale_expansion_zeroelim(cytbblen, cytbb, -adx, temp16c);
-
- temp32alen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16clen, temp16c,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
-
- if ((adxtail != 0.0) || (adytail != 0.0)) {
- if ((bdxtail != 0.0) || (bdytail != 0.0)
- || (cdxtail != 0.0) || (cdytail != 0.0)) {
- Two_Product(bdxtail, cdy, ti1, ti0);
- Two_Product(bdx, cdytail, tj1, tj0);
- Two_Two_Sum(ti1, ti0, tj1, tj0, u3, u[2], u[1], u[0]);
- u[3] = u3;
- negate = -bdy;
- Two_Product(cdxtail, negate, ti1, ti0);
- negate = -bdytail;
- Two_Product(cdx, negate, tj1, tj0);
- Two_Two_Sum(ti1, ti0, tj1, tj0, v3, v[2], v[1], v[0]);
- v[3] = v3;
- bctlen = fast_expansion_sum_zeroelim(4, u, 4, v, bct);
-
- Two_Product(bdxtail, cdytail, ti1, ti0);
- Two_Product(cdxtail, bdytail, tj1, tj0);
- Two_Two_Diff(ti1, ti0, tj1, tj0, bctt3, bctt[2], bctt[1], bctt[0]);
- bctt[3] = bctt3;
- bcttlen = 4;
- } else {
- bct[0] = 0.0;
- bctlen = 1;
- bctt[0] = 0.0;
- bcttlen = 1;
- }
-
- if (adxtail != 0.0) {
- temp16alen = scale_expansion_zeroelim(axtbclen, axtbc, adxtail, temp16a);
- axtbctlen = scale_expansion_zeroelim(bctlen, bct, adxtail, axtbct);
- temp32alen = scale_expansion_zeroelim(axtbctlen, axtbct, 2.0 * adx,
- temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (bdytail != 0.0) {
- temp8len = scale_expansion_zeroelim(4, cc, adxtail, temp8);
- temp16alen = scale_expansion_zeroelim(temp8len, temp8, bdytail,
- temp16a);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp16alen,
- temp16a, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (cdytail != 0.0) {
- temp8len = scale_expansion_zeroelim(4, bb, -adxtail, temp8);
- temp16alen = scale_expansion_zeroelim(temp8len, temp8, cdytail,
- temp16a);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp16alen,
- temp16a, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
-
- temp32alen = scale_expansion_zeroelim(axtbctlen, axtbct, adxtail,
- temp32a);
- axtbcttlen = scale_expansion_zeroelim(bcttlen, bctt, adxtail, axtbctt);
- temp16alen = scale_expansion_zeroelim(axtbcttlen, axtbctt, 2.0 * adx,
- temp16a);
- temp16blen = scale_expansion_zeroelim(axtbcttlen, axtbctt, adxtail,
- temp16b);
- temp32blen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32b);
- temp64len = fast_expansion_sum_zeroelim(temp32alen, temp32a,
- temp32blen, temp32b, temp64);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp64len,
- temp64, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (adytail != 0.0) {
- temp16alen = scale_expansion_zeroelim(aytbclen, aytbc, adytail, temp16a);
- aytbctlen = scale_expansion_zeroelim(bctlen, bct, adytail, aytbct);
- temp32alen = scale_expansion_zeroelim(aytbctlen, aytbct, 2.0 * ady,
- temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
-
-
- temp32alen = scale_expansion_zeroelim(aytbctlen, aytbct, adytail,
- temp32a);
- aytbcttlen = scale_expansion_zeroelim(bcttlen, bctt, adytail, aytbctt);
- temp16alen = scale_expansion_zeroelim(aytbcttlen, aytbctt, 2.0 * ady,
- temp16a);
- temp16blen = scale_expansion_zeroelim(aytbcttlen, aytbctt, adytail,
- temp16b);
- temp32blen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32b);
- temp64len = fast_expansion_sum_zeroelim(temp32alen, temp32a,
- temp32blen, temp32b, temp64);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp64len,
- temp64, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- if ((bdxtail != 0.0) || (bdytail != 0.0)) {
- if ((cdxtail != 0.0) || (cdytail != 0.0)
- || (adxtail != 0.0) || (adytail != 0.0)) {
- Two_Product(cdxtail, ady, ti1, ti0);
- Two_Product(cdx, adytail, tj1, tj0);
- Two_Two_Sum(ti1, ti0, tj1, tj0, u3, u[2], u[1], u[0]);
- u[3] = u3;
- negate = -cdy;
- Two_Product(adxtail, negate, ti1, ti0);
- negate = -cdytail;
- Two_Product(adx, negate, tj1, tj0);
- Two_Two_Sum(ti1, ti0, tj1, tj0, v3, v[2], v[1], v[0]);
- v[3] = v3;
- catlen = fast_expansion_sum_zeroelim(4, u, 4, v, cat);
-
- Two_Product(cdxtail, adytail, ti1, ti0);
- Two_Product(adxtail, cdytail, tj1, tj0);
- Two_Two_Diff(ti1, ti0, tj1, tj0, catt3, catt[2], catt[1], catt[0]);
- catt[3] = catt3;
- cattlen = 4;
- } else {
- cat[0] = 0.0;
- catlen = 1;
- catt[0] = 0.0;
- cattlen = 1;
- }
-
- if (bdxtail != 0.0) {
- temp16alen = scale_expansion_zeroelim(bxtcalen, bxtca, bdxtail, temp16a);
- bxtcatlen = scale_expansion_zeroelim(catlen, cat, bdxtail, bxtcat);
- temp32alen = scale_expansion_zeroelim(bxtcatlen, bxtcat, 2.0 * bdx,
- temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (cdytail != 0.0) {
- temp8len = scale_expansion_zeroelim(4, aa, bdxtail, temp8);
- temp16alen = scale_expansion_zeroelim(temp8len, temp8, cdytail,
- temp16a);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp16alen,
- temp16a, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (adytail != 0.0) {
- temp8len = scale_expansion_zeroelim(4, cc, -bdxtail, temp8);
- temp16alen = scale_expansion_zeroelim(temp8len, temp8, adytail,
- temp16a);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp16alen,
- temp16a, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
-
- temp32alen = scale_expansion_zeroelim(bxtcatlen, bxtcat, bdxtail,
- temp32a);
- bxtcattlen = scale_expansion_zeroelim(cattlen, catt, bdxtail, bxtcatt);
- temp16alen = scale_expansion_zeroelim(bxtcattlen, bxtcatt, 2.0 * bdx,
- temp16a);
- temp16blen = scale_expansion_zeroelim(bxtcattlen, bxtcatt, bdxtail,
- temp16b);
- temp32blen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32b);
- temp64len = fast_expansion_sum_zeroelim(temp32alen, temp32a,
- temp32blen, temp32b, temp64);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp64len,
- temp64, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (bdytail != 0.0) {
- temp16alen = scale_expansion_zeroelim(bytcalen, bytca, bdytail, temp16a);
- bytcatlen = scale_expansion_zeroelim(catlen, cat, bdytail, bytcat);
- temp32alen = scale_expansion_zeroelim(bytcatlen, bytcat, 2.0 * bdy,
- temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
-
-
- temp32alen = scale_expansion_zeroelim(bytcatlen, bytcat, bdytail,
- temp32a);
- bytcattlen = scale_expansion_zeroelim(cattlen, catt, bdytail, bytcatt);
- temp16alen = scale_expansion_zeroelim(bytcattlen, bytcatt, 2.0 * bdy,
- temp16a);
- temp16blen = scale_expansion_zeroelim(bytcattlen, bytcatt, bdytail,
- temp16b);
- temp32blen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32b);
- temp64len = fast_expansion_sum_zeroelim(temp32alen, temp32a,
- temp32blen, temp32b, temp64);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp64len,
- temp64, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- if ((cdxtail != 0.0) || (cdytail != 0.0)) {
- if ((adxtail != 0.0) || (adytail != 0.0)
- || (bdxtail != 0.0) || (bdytail != 0.0)) {
- Two_Product(adxtail, bdy, ti1, ti0);
- Two_Product(adx, bdytail, tj1, tj0);
- Two_Two_Sum(ti1, ti0, tj1, tj0, u3, u[2], u[1], u[0]);
- u[3] = u3;
- negate = -ady;
- Two_Product(bdxtail, negate, ti1, ti0);
- negate = -adytail;
- Two_Product(bdx, negate, tj1, tj0);
- Two_Two_Sum(ti1, ti0, tj1, tj0, v3, v[2], v[1], v[0]);
- v[3] = v3;
- abtlen = fast_expansion_sum_zeroelim(4, u, 4, v, abt);
-
- Two_Product(adxtail, bdytail, ti1, ti0);
- Two_Product(bdxtail, adytail, tj1, tj0);
- Two_Two_Diff(ti1, ti0, tj1, tj0, abtt3, abtt[2], abtt[1], abtt[0]);
- abtt[3] = abtt3;
- abttlen = 4;
- } else {
- abt[0] = 0.0;
- abtlen = 1;
- abtt[0] = 0.0;
- abttlen = 1;
- }
-
- if (cdxtail != 0.0) {
- temp16alen = scale_expansion_zeroelim(cxtablen, cxtab, cdxtail, temp16a);
- cxtabtlen = scale_expansion_zeroelim(abtlen, abt, cdxtail, cxtabt);
- temp32alen = scale_expansion_zeroelim(cxtabtlen, cxtabt, 2.0 * cdx,
- temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (adytail != 0.0) {
- temp8len = scale_expansion_zeroelim(4, bb, cdxtail, temp8);
- temp16alen = scale_expansion_zeroelim(temp8len, temp8, adytail,
- temp16a);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp16alen,
- temp16a, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (bdytail != 0.0) {
- temp8len = scale_expansion_zeroelim(4, aa, -cdxtail, temp8);
- temp16alen = scale_expansion_zeroelim(temp8len, temp8, bdytail,
- temp16a);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp16alen,
- temp16a, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
-
- temp32alen = scale_expansion_zeroelim(cxtabtlen, cxtabt, cdxtail,
- temp32a);
- cxtabttlen = scale_expansion_zeroelim(abttlen, abtt, cdxtail, cxtabtt);
- temp16alen = scale_expansion_zeroelim(cxtabttlen, cxtabtt, 2.0 * cdx,
- temp16a);
- temp16blen = scale_expansion_zeroelim(cxtabttlen, cxtabtt, cdxtail,
- temp16b);
- temp32blen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32b);
- temp64len = fast_expansion_sum_zeroelim(temp32alen, temp32a,
- temp32blen, temp32b, temp64);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp64len,
- temp64, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (cdytail != 0.0) {
- temp16alen = scale_expansion_zeroelim(cytablen, cytab, cdytail, temp16a);
- cytabtlen = scale_expansion_zeroelim(abtlen, abt, cdytail, cytabt);
- temp32alen = scale_expansion_zeroelim(cytabtlen, cytabt, 2.0 * cdy,
- temp32a);
- temp48len = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp32alen, temp32a, temp48);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp48len,
- temp48, finother);
- finswap = finnow; finnow = finother; finother = finswap;
-
-
- temp32alen = scale_expansion_zeroelim(cytabtlen, cytabt, cdytail,
- temp32a);
- cytabttlen = scale_expansion_zeroelim(abttlen, abtt, cdytail, cytabtt);
- temp16alen = scale_expansion_zeroelim(cytabttlen, cytabtt, 2.0 * cdy,
- temp16a);
- temp16blen = scale_expansion_zeroelim(cytabttlen, cytabtt, cdytail,
- temp16b);
- temp32blen = fast_expansion_sum_zeroelim(temp16alen, temp16a,
- temp16blen, temp16b, temp32b);
- temp64len = fast_expansion_sum_zeroelim(temp32alen, temp32a,
- temp32blen, temp32b, temp64);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, temp64len,
- temp64, finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
-
- return finnow[finlength - 1];
-}
-
-float incircle(struct mesh *m, struct behavior *b,
- vertex pa, vertex pb, vertex pc, vertex pd)
-{
- float adx, bdx, cdx, ady, bdy, cdy;
- float bdxcdy, cdxbdy, cdxady, adxcdy, adxbdy, bdxady;
- float alift, blift, clift;
- float det;
- float permanent, errbound;
-
- m->incirclecount++;
-
- adx = pa[0] - pd[0];
- bdx = pb[0] - pd[0];
- cdx = pc[0] - pd[0];
- ady = pa[1] - pd[1];
- bdy = pb[1] - pd[1];
- cdy = pc[1] - pd[1];
-
- bdxcdy = bdx * cdy;
- cdxbdy = cdx * bdy;
- alift = adx * adx + ady * ady;
-
- cdxady = cdx * ady;
- adxcdy = adx * cdy;
- blift = bdx * bdx + bdy * bdy;
-
- adxbdy = adx * bdy;
- bdxady = bdx * ady;
- clift = cdx * cdx + cdy * cdy;
-
- det = alift * (bdxcdy - cdxbdy)
- + blift * (cdxady - adxcdy)
- + clift * (adxbdy - bdxady);
-
- if (b->noexact) {
- return det;
- }
-
- permanent = (Absolute(bdxcdy) + Absolute(cdxbdy)) * alift
- + (Absolute(cdxady) + Absolute(adxcdy)) * blift
- + (Absolute(adxbdy) + Absolute(bdxady)) * clift;
- errbound = iccerrboundA * permanent;
- if ((det > errbound) || (-det > errbound)) {
- return det;
- }
-
- return incircleadapt(pa, pb, pc, pd, permanent);
-}
-
-/*****************************************************************************/
-/* */
-/* orient3d() Return a positive value if the point pd lies below the */
-/* plane passing through pa, pb, and pc; "below" is defined so */
-/* that pa, pb, and pc appear in counterclockwise order when */
-/* viewed from above the plane. Returns a negative value if */
-/* pd lies above the plane. Returns zero if the points are */
-/* coplanar. The result is also a rough approximation of six */
-/* times the signed volume of the tetrahedron defined by the */
-/* four points. */
-/* */
-/* Uses exact arithmetic if necessary to ensure a correct answer. The */
-/* result returned is the determinant of a matrix. This determinant is */
-/* computed adaptively, in the sense that exact arithmetic is used only to */
-/* the degree it is needed to ensure that the returned value has the */
-/* correct sign. Hence, this function is usually quite fast, but will run */
-/* more slowly when the input points are coplanar or nearly so. */
-/* */
-/* See my Robust Predicates paper for details. */
-/* */
-/*****************************************************************************/
-
-float orient3dadapt(vertex pa, vertex pb, vertex pc, vertex pd,
- float aheight, float bheight, float cheight, float dheight,
- float permanent)
-{
- float adx, bdx, cdx, ady, bdy, cdy, adheight, bdheight, cdheight;
- float det, errbound;
-
- float bdxcdy1, cdxbdy1, cdxady1, adxcdy1, adxbdy1, bdxady1;
- float bdxcdy0, cdxbdy0, cdxady0, adxcdy0, adxbdy0, bdxady0;
- float bc[4], ca[4], ab[4];
- float bc3, ca3, ab3;
- float adet[8], bdet[8], cdet[8];
- int alen, blen, clen;
- float abdet[16];
- int ablen;
- float *finnow, *finother, *finswap;
- float fin1[192], fin2[192];
- int finlength;
-
- float adxtail, bdxtail, cdxtail;
- float adytail, bdytail, cdytail;
- float adheighttail, bdheighttail, cdheighttail;
- float at_blarge, at_clarge;
- float bt_clarge, bt_alarge;
- float ct_alarge, ct_blarge;
- float at_b[4], at_c[4], bt_c[4], bt_a[4], ct_a[4], ct_b[4];
- int at_blen, at_clen, bt_clen, bt_alen, ct_alen, ct_blen;
- float bdxt_cdy1, cdxt_bdy1, cdxt_ady1;
- float adxt_cdy1, adxt_bdy1, bdxt_ady1;
- float bdxt_cdy0, cdxt_bdy0, cdxt_ady0;
- float adxt_cdy0, adxt_bdy0, bdxt_ady0;
- float bdyt_cdx1, cdyt_bdx1, cdyt_adx1;
- float adyt_cdx1, adyt_bdx1, bdyt_adx1;
- float bdyt_cdx0, cdyt_bdx0, cdyt_adx0;
- float adyt_cdx0, adyt_bdx0, bdyt_adx0;
- float bct[8], cat[8], abt[8];
- int bctlen, catlen, abtlen;
- float bdxt_cdyt1, cdxt_bdyt1, cdxt_adyt1;
- float adxt_cdyt1, adxt_bdyt1, bdxt_adyt1;
- float bdxt_cdyt0, cdxt_bdyt0, cdxt_adyt0;
- float adxt_cdyt0, adxt_bdyt0, bdxt_adyt0;
- float u[4], v[12], w[16];
- float u3;
- int vlength, wlength;
- float negate;
-
- float bvirt;
- float avirt, bround, around;
- float c;
- float abig;
- float ahi, alo, bhi, blo;
- float err1, err2, err3;
- float _i, _j, _k;
- float _0;
-
- adx = (float) (pa[0] - pd[0]);
- bdx = (float) (pb[0] - pd[0]);
- cdx = (float) (pc[0] - pd[0]);
- ady = (float) (pa[1] - pd[1]);
- bdy = (float) (pb[1] - pd[1]);
- cdy = (float) (pc[1] - pd[1]);
- adheight = (float) (aheight - dheight);
- bdheight = (float) (bheight - dheight);
- cdheight = (float) (cheight - dheight);
-
- Two_Product(bdx, cdy, bdxcdy1, bdxcdy0);
- Two_Product(cdx, bdy, cdxbdy1, cdxbdy0);
- Two_Two_Diff(bdxcdy1, bdxcdy0, cdxbdy1, cdxbdy0, bc3, bc[2], bc[1], bc[0]);
- bc[3] = bc3;
- alen = scale_expansion_zeroelim(4, bc, adheight, adet);
-
- Two_Product(cdx, ady, cdxady1, cdxady0);
- Two_Product(adx, cdy, adxcdy1, adxcdy0);
- Two_Two_Diff(cdxady1, cdxady0, adxcdy1, adxcdy0, ca3, ca[2], ca[1], ca[0]);
- ca[3] = ca3;
- blen = scale_expansion_zeroelim(4, ca, bdheight, bdet);
-
- Two_Product(adx, bdy, adxbdy1, adxbdy0);
- Two_Product(bdx, ady, bdxady1, bdxady0);
- Two_Two_Diff(adxbdy1, adxbdy0, bdxady1, bdxady0, ab3, ab[2], ab[1], ab[0]);
- ab[3] = ab3;
- clen = scale_expansion_zeroelim(4, ab, cdheight, cdet);
-
- ablen = fast_expansion_sum_zeroelim(alen, adet, blen, bdet, abdet);
- finlength = fast_expansion_sum_zeroelim(ablen, abdet, clen, cdet, fin1);
-
- det = estimate(finlength, fin1);
- errbound = o3derrboundB * permanent;
- if ((det >= errbound) || (-det >= errbound)) {
- return det;
- }
-
- Two_Diff_Tail(pa[0], pd[0], adx, adxtail);
- Two_Diff_Tail(pb[0], pd[0], bdx, bdxtail);
- Two_Diff_Tail(pc[0], pd[0], cdx, cdxtail);
- Two_Diff_Tail(pa[1], pd[1], ady, adytail);
- Two_Diff_Tail(pb[1], pd[1], bdy, bdytail);
- Two_Diff_Tail(pc[1], pd[1], cdy, cdytail);
- Two_Diff_Tail(aheight, dheight, adheight, adheighttail);
- Two_Diff_Tail(bheight, dheight, bdheight, bdheighttail);
- Two_Diff_Tail(cheight, dheight, cdheight, cdheighttail);
-
- if ((adxtail == 0.0) && (bdxtail == 0.0) && (cdxtail == 0.0) &&
- (adytail == 0.0) && (bdytail == 0.0) && (cdytail == 0.0) &&
- (adheighttail == 0.0) &&
- (bdheighttail == 0.0) &&
- (cdheighttail == 0.0)) {
- return det;
- }
-
- errbound = o3derrboundC * permanent + resulterrbound * Absolute(det);
- det += (adheight * ((bdx * cdytail + cdy * bdxtail) -
- (bdy * cdxtail + cdx * bdytail)) +
- adheighttail * (bdx * cdy - bdy * cdx)) +
- (bdheight * ((cdx * adytail + ady * cdxtail) -
- (cdy * adxtail + adx * cdytail)) +
- bdheighttail * (cdx * ady - cdy * adx)) +
- (cdheight * ((adx * bdytail + bdy * adxtail) -
- (ady * bdxtail + bdx * adytail)) +
- cdheighttail * (adx * bdy - ady * bdx));
- if ((det >= errbound) || (-det >= errbound)) {
- return det;
- }
-
- finnow = fin1;
- finother = fin2;
-
- if (adxtail == 0.0) {
- if (adytail == 0.0) {
- at_b[0] = 0.0;
- at_blen = 1;
- at_c[0] = 0.0;
- at_clen = 1;
- } else {
- negate = -adytail;
- Two_Product(negate, bdx, at_blarge, at_b[0]);
- at_b[1] = at_blarge;
- at_blen = 2;
- Two_Product(adytail, cdx, at_clarge, at_c[0]);
- at_c[1] = at_clarge;
- at_clen = 2;
- }
- } else {
- if (adytail == 0.0) {
- Two_Product(adxtail, bdy, at_blarge, at_b[0]);
- at_b[1] = at_blarge;
- at_blen = 2;
- negate = -adxtail;
- Two_Product(negate, cdy, at_clarge, at_c[0]);
- at_c[1] = at_clarge;
- at_clen = 2;
- } else {
- Two_Product(adxtail, bdy, adxt_bdy1, adxt_bdy0);
- Two_Product(adytail, bdx, adyt_bdx1, adyt_bdx0);
- Two_Two_Diff(adxt_bdy1, adxt_bdy0, adyt_bdx1, adyt_bdx0,
- at_blarge, at_b[2], at_b[1], at_b[0]);
- at_b[3] = at_blarge;
- at_blen = 4;
- Two_Product(adytail, cdx, adyt_cdx1, adyt_cdx0);
- Two_Product(adxtail, cdy, adxt_cdy1, adxt_cdy0);
- Two_Two_Diff(adyt_cdx1, adyt_cdx0, adxt_cdy1, adxt_cdy0,
- at_clarge, at_c[2], at_c[1], at_c[0]);
- at_c[3] = at_clarge;
- at_clen = 4;
- }
- }
- if (bdxtail == 0.0) {
- if (bdytail == 0.0) {
- bt_c[0] = 0.0;
- bt_clen = 1;
- bt_a[0] = 0.0;
- bt_alen = 1;
- } else {
- negate = -bdytail;
- Two_Product(negate, cdx, bt_clarge, bt_c[0]);
- bt_c[1] = bt_clarge;
- bt_clen = 2;
- Two_Product(bdytail, adx, bt_alarge, bt_a[0]);
- bt_a[1] = bt_alarge;
- bt_alen = 2;
- }
- } else {
- if (bdytail == 0.0) {
- Two_Product(bdxtail, cdy, bt_clarge, bt_c[0]);
- bt_c[1] = bt_clarge;
- bt_clen = 2;
- negate = -bdxtail;
- Two_Product(negate, ady, bt_alarge, bt_a[0]);
- bt_a[1] = bt_alarge;
- bt_alen = 2;
- } else {
- Two_Product(bdxtail, cdy, bdxt_cdy1, bdxt_cdy0);
- Two_Product(bdytail, cdx, bdyt_cdx1, bdyt_cdx0);
- Two_Two_Diff(bdxt_cdy1, bdxt_cdy0, bdyt_cdx1, bdyt_cdx0,
- bt_clarge, bt_c[2], bt_c[1], bt_c[0]);
- bt_c[3] = bt_clarge;
- bt_clen = 4;
- Two_Product(bdytail, adx, bdyt_adx1, bdyt_adx0);
- Two_Product(bdxtail, ady, bdxt_ady1, bdxt_ady0);
- Two_Two_Diff(bdyt_adx1, bdyt_adx0, bdxt_ady1, bdxt_ady0,
- bt_alarge, bt_a[2], bt_a[1], bt_a[0]);
- bt_a[3] = bt_alarge;
- bt_alen = 4;
- }
- }
- if (cdxtail == 0.0) {
- if (cdytail == 0.0) {
- ct_a[0] = 0.0;
- ct_alen = 1;
- ct_b[0] = 0.0;
- ct_blen = 1;
- } else {
- negate = -cdytail;
- Two_Product(negate, adx, ct_alarge, ct_a[0]);
- ct_a[1] = ct_alarge;
- ct_alen = 2;
- Two_Product(cdytail, bdx, ct_blarge, ct_b[0]);
- ct_b[1] = ct_blarge;
- ct_blen = 2;
- }
- } else {
- if (cdytail == 0.0) {
- Two_Product(cdxtail, ady, ct_alarge, ct_a[0]);
- ct_a[1] = ct_alarge;
- ct_alen = 2;
- negate = -cdxtail;
- Two_Product(negate, bdy, ct_blarge, ct_b[0]);
- ct_b[1] = ct_blarge;
- ct_blen = 2;
- } else {
- Two_Product(cdxtail, ady, cdxt_ady1, cdxt_ady0);
- Two_Product(cdytail, adx, cdyt_adx1, cdyt_adx0);
- Two_Two_Diff(cdxt_ady1, cdxt_ady0, cdyt_adx1, cdyt_adx0,
- ct_alarge, ct_a[2], ct_a[1], ct_a[0]);
- ct_a[3] = ct_alarge;
- ct_alen = 4;
- Two_Product(cdytail, bdx, cdyt_bdx1, cdyt_bdx0);
- Two_Product(cdxtail, bdy, cdxt_bdy1, cdxt_bdy0);
- Two_Two_Diff(cdyt_bdx1, cdyt_bdx0, cdxt_bdy1, cdxt_bdy0,
- ct_blarge, ct_b[2], ct_b[1], ct_b[0]);
- ct_b[3] = ct_blarge;
- ct_blen = 4;
- }
- }
-
- bctlen = fast_expansion_sum_zeroelim(bt_clen, bt_c, ct_blen, ct_b, bct);
- wlength = scale_expansion_zeroelim(bctlen, bct, adheight, w);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, wlength, w,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
-
- catlen = fast_expansion_sum_zeroelim(ct_alen, ct_a, at_clen, at_c, cat);
- wlength = scale_expansion_zeroelim(catlen, cat, bdheight, w);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, wlength, w,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
-
- abtlen = fast_expansion_sum_zeroelim(at_blen, at_b, bt_alen, bt_a, abt);
- wlength = scale_expansion_zeroelim(abtlen, abt, cdheight, w);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, wlength, w,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
-
- if (adheighttail != 0.0) {
- vlength = scale_expansion_zeroelim(4, bc, adheighttail, v);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, vlength, v,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (bdheighttail != 0.0) {
- vlength = scale_expansion_zeroelim(4, ca, bdheighttail, v);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, vlength, v,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (cdheighttail != 0.0) {
- vlength = scale_expansion_zeroelim(4, ab, cdheighttail, v);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, vlength, v,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
-
- if (adxtail != 0.0) {
- if (bdytail != 0.0) {
- Two_Product(adxtail, bdytail, adxt_bdyt1, adxt_bdyt0);
- Two_One_Product(adxt_bdyt1, adxt_bdyt0, cdheight, u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (cdheighttail != 0.0) {
- Two_One_Product(adxt_bdyt1, adxt_bdyt0, cdheighttail,
- u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- if (cdytail != 0.0) {
- negate = -adxtail;
- Two_Product(negate, cdytail, adxt_cdyt1, adxt_cdyt0);
- Two_One_Product(adxt_cdyt1, adxt_cdyt0, bdheight, u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (bdheighttail != 0.0) {
- Two_One_Product(adxt_cdyt1, adxt_cdyt0, bdheighttail,
- u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- }
- if (bdxtail != 0.0) {
- if (cdytail != 0.0) {
- Two_Product(bdxtail, cdytail, bdxt_cdyt1, bdxt_cdyt0);
- Two_One_Product(bdxt_cdyt1, bdxt_cdyt0, adheight, u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (adheighttail != 0.0) {
- Two_One_Product(bdxt_cdyt1, bdxt_cdyt0, adheighttail,
- u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- if (adytail != 0.0) {
- negate = -bdxtail;
- Two_Product(negate, adytail, bdxt_adyt1, bdxt_adyt0);
- Two_One_Product(bdxt_adyt1, bdxt_adyt0, cdheight, u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (cdheighttail != 0.0) {
- Two_One_Product(bdxt_adyt1, bdxt_adyt0, cdheighttail,
- u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- }
- if (cdxtail != 0.0) {
- if (adytail != 0.0) {
- Two_Product(cdxtail, adytail, cdxt_adyt1, cdxt_adyt0);
- Two_One_Product(cdxt_adyt1, cdxt_adyt0, bdheight, u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (bdheighttail != 0.0) {
- Two_One_Product(cdxt_adyt1, cdxt_adyt0, bdheighttail,
- u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- if (bdytail != 0.0) {
- negate = -cdxtail;
- Two_Product(negate, bdytail, cdxt_bdyt1, cdxt_bdyt0);
- Two_One_Product(cdxt_bdyt1, cdxt_bdyt0, adheight, u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- if (adheighttail != 0.0) {
- Two_One_Product(cdxt_bdyt1, cdxt_bdyt0, adheighttail,
- u3, u[2], u[1], u[0]);
- u[3] = u3;
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, 4, u,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- }
- }
-
- if (adheighttail != 0.0) {
- wlength = scale_expansion_zeroelim(bctlen, bct, adheighttail, w);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, wlength, w,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (bdheighttail != 0.0) {
- wlength = scale_expansion_zeroelim(catlen, cat, bdheighttail, w);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, wlength, w,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
- if (cdheighttail != 0.0) {
- wlength = scale_expansion_zeroelim(abtlen, abt, cdheighttail, w);
- finlength = fast_expansion_sum_zeroelim(finlength, finnow, wlength, w,
- finother);
- finswap = finnow; finnow = finother; finother = finswap;
- }
-
- return finnow[finlength - 1];
-}
-
-float orient3d(struct mesh *m, struct behavior *b,
- vertex pa, vertex pb, vertex pc, vertex pd,
- float aheight, float bheight, float cheight, float dheight)
-{
- float adx, bdx, cdx, ady, bdy, cdy, adheight, bdheight, cdheight;
- float bdxcdy, cdxbdy, cdxady, adxcdy, adxbdy, bdxady;
- float det;
- float permanent, errbound;
-
- m->orient3dcount++;
-
- adx = pa[0] - pd[0];
- bdx = pb[0] - pd[0];
- cdx = pc[0] - pd[0];
- ady = pa[1] - pd[1];
- bdy = pb[1] - pd[1];
- cdy = pc[1] - pd[1];
- adheight = aheight - dheight;
- bdheight = bheight - dheight;
- cdheight = cheight - dheight;
-
- bdxcdy = bdx * cdy;
- cdxbdy = cdx * bdy;
-
- cdxady = cdx * ady;
- adxcdy = adx * cdy;
-
- adxbdy = adx * bdy;
- bdxady = bdx * ady;
-
- det = adheight * (bdxcdy - cdxbdy)
- + bdheight * (cdxady - adxcdy)
- + cdheight * (adxbdy - bdxady);
-
- if (b->noexact) {
- return det;
- }
-
- permanent = (Absolute(bdxcdy) + Absolute(cdxbdy)) * Absolute(adheight)
- + (Absolute(cdxady) + Absolute(adxcdy)) * Absolute(bdheight)
- + (Absolute(adxbdy) + Absolute(bdxady)) * Absolute(cdheight);
- errbound = o3derrboundA * permanent;
- if ((det > errbound) || (-det > errbound)) {
- return det;
- }
-
- return orient3dadapt(pa, pb, pc, pd, aheight, bheight, cheight, dheight,
- permanent);
-}
-
-/*****************************************************************************/
-/* */
-/* nonregular() Return a positive value if the point pd is incompatible */
-/* with the circle or plane passing through pa, pb, and pc */
-/* (meaning that pd is inside the circle or below the */
-/* plane); a negative value if it is compatible; and zero if */
-/* the four points are cocircular/coplanar. The points pa, */
-/* pb, and pc must be in counterclockwise order, or the sign */
-/* of the result will be reversed. */
-/* */
-/* If the -w switch is used, the points are lifted onto the parabolic */
-/* lifting map, then they are dropped according to their weights, then the */
-/* 3D orientation test is applied. If the -W switch is used, the points' */
-/* heights are already provided, so the 3D orientation test is applied */
-/* directly. If neither switch is used, the incircle test is applied. */
-/* */
-/*****************************************************************************/
-
-float nonregular(struct mesh *m, struct behavior *b,
- vertex pa, vertex pb, vertex pc, vertex pd)
-{
- if (b->weighted == 0) {
- return incircle(m, b, pa, pb, pc, pd);
- } else if (b->weighted == 1) {
- return orient3d(m, b, pa, pb, pc, pd,
- pa[0] * pa[0] + pa[1] * pa[1] - pa[2],
- pb[0] * pb[0] + pb[1] * pb[1] - pb[2],
- pc[0] * pc[0] + pc[1] * pc[1] - pc[2],
- pd[0] * pd[0] + pd[1] * pd[1] - pd[2]);
- } else {
- return orient3d(m, b, pa, pb, pc, pd, pa[2], pb[2], pc[2], pd[2]);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* findcircumcenter() Find the circumcenter of a triangle. */
-/* */
-/* The result is returned both in terms of x-y coordinates and xi-eta */
-/* (barycentric) coordinates. The xi-eta coordinate system is defined in */
-/* terms of the triangle: the origin of the triangle is the origin of the */
-/* coordinate system; the destination of the triangle is one unit along the */
-/* xi axis; and the apex of the triangle is one unit along the eta axis. */
-/* This procedure also returns the square of the length of the triangle's */
-/* shortest edge. */
-/* */
-/*****************************************************************************/
-
-void findcircumcenter(struct mesh *m, struct behavior *b,
- vertex torg, vertex tdest, vertex tapex,
- vertex circumcenter, float *xi, float *eta, int offcenter)
-{
- float xdo, ydo, xao, yao;
- float dodist, aodist, dadist;
- float denominator;
- float dx, dy, dxoff, dyoff;
-
- m->circumcentercount++;
-
- /* Compute the circumcenter of the triangle. */
- xdo = tdest[0] - torg[0];
- ydo = tdest[1] - torg[1];
- xao = tapex[0] - torg[0];
- yao = tapex[1] - torg[1];
- dodist = xdo * xdo + ydo * ydo;
- aodist = xao * xao + yao * yao;
- dadist = (tdest[0] - tapex[0]) * (tdest[0] - tapex[0]) +
- (tdest[1] - tapex[1]) * (tdest[1] - tapex[1]);
- if (b->noexact) {
- denominator = 0.5 / (xdo * yao - xao * ydo);
- } else {
- /* Use the counterclockwise() routine to ensure a positive (and */
- /* reasonably accurate) result, avoiding any possibility of */
- /* division by zero. */
- denominator = 0.5 / counterclockwise(m, b, tdest, tapex, torg);
- /* Don't count the above as an orientation test. */
- m->counterclockcount--;
- }
- dx = (yao * dodist - ydo * aodist) * denominator;
- dy = (xdo * aodist - xao * dodist) * denominator;
-
- /* Find the (squared) length of the triangle's shortest edge. This */
- /* serves as a conservative estimate of the insertion radius of the */
- /* circumcenter's parent. The estimate is used to ensure that */
- /* the algorithm terminates even if very small angles appear in */
- /* the input PSLG. */
- if ((dodist < aodist) && (dodist < dadist)) {
- if (offcenter && (b->offconstant > 0.0)) {
- /* Find the position of the off-center, as described by Alper Ungor. */
- dxoff = 0.5 * xdo - b->offconstant * ydo;
- dyoff = 0.5 * ydo + b->offconstant * xdo;
- /* If the off-center is closer to the origin than the */
- /* circumcenter, use the off-center instead. */
- if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
- dx = dxoff;
- dy = dyoff;
- }
- }
- } else if (aodist < dadist) {
- if (offcenter && (b->offconstant > 0.0)) {
- dxoff = 0.5 * xao + b->offconstant * yao;
- dyoff = 0.5 * yao - b->offconstant * xao;
- /* If the off-center is closer to the origin than the */
- /* circumcenter, use the off-center instead. */
- if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
- dx = dxoff;
- dy = dyoff;
- }
- }
- } else {
- if (offcenter && (b->offconstant > 0.0)) {
- dxoff = 0.5 * (tapex[0] - tdest[0]) -
- b->offconstant * (tapex[1] - tdest[1]);
- dyoff = 0.5 * (tapex[1] - tdest[1]) +
- b->offconstant * (tapex[0] - tdest[0]);
- /* If the off-center is closer to the destination than the */
- /* circumcenter, use the off-center instead. */
- if (dxoff * dxoff + dyoff * dyoff <
- (dx - xdo) * (dx - xdo) + (dy - ydo) * (dy - ydo)) {
- dx = xdo + dxoff;
- dy = ydo + dyoff;
- }
- }
- }
-
- circumcenter[0] = torg[0] + dx;
- circumcenter[1] = torg[1] + dy;
-
- /* To interpolate vertex attributes for the new vertex inserted at */
- /* the circumcenter, define a coordinate system with a xi-axis, */
- /* directed from the triangle's origin to its destination, and */
- /* an eta-axis, directed from its origin to its apex. */
- /* Calculate the xi and eta coordinates of the circumcenter. */
- *xi = (yao * dx - xao * dy) * (2.0 * denominator);
- *eta = (xdo * dy - ydo * dx) * (2.0 * denominator);
-}
-
-/** **/
-/** **/
-/********* Geometric primitives end here *********/
-
-/*****************************************************************************/
-/* */
-/* triangleinit() Initialize some variables. */
-/* */
-/*****************************************************************************/
-
-void triangleinit(struct mesh *m)
-{
- poolzero(&m->vertices);
- poolzero(&m->triangles);
- poolzero(&m->subsegs);
- poolzero(&m->viri);
- poolzero(&m->badsubsegs);
- poolzero(&m->badtriangles);
- poolzero(&m->flipstackers);
- poolzero(&m->splaynodes);
-
- m->recenttri.tri = (triangle *) NULL; /* No triangle has been visited yet. */
- m->undeads = 0; /* No eliminated input vertices yet. */
- m->samples = 1; /* Point location should take at least one sample. */
- m->checksegments = 0; /* There are no segments in the triangulation yet. */
- m->checkquality = 0; /* The quality triangulation stage has not begun. */
- m->incirclecount = m->counterclockcount = m->orient3dcount = 0;
- m->hyperbolacount = m->circletopcount = m->circumcentercount = 0;
- randomseed = 1;
-
- exactinit(); /* Initialize exact arithmetic constants. */
-}
-
-/*****************************************************************************/
-/* */
-/* randomnation() Generate a random number between 0 and `choices' - 1. */
-/* */
-/* This is a simple linear congruential random number generator. Hence, it */
-/* is a bad random number generator, but good enough for most randomized */
-/* geometric algorithms. */
-/* */
-/*****************************************************************************/
-
-unsigned long long randomnation(unsigned int choices)
-{
- randomseed = (randomseed * 1366l + 150889l) % 714025l;
- return randomseed / (714025l / choices + 1);
-}
-
-/********* Point location routines begin here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* makevertexmap() Construct a mapping from vertices to triangles to */
-/* improve the speed of point location for segment */
-/* insertion. */
-/* */
-/* Traverses all the triangles, and provides each corner of each triangle */
-/* with a pointer to that triangle. Of course, pointers will be */
-/* overwritten by other pointers because (almost) each vertex is a corner */
-/* of several triangles, but in the end every vertex will point to some */
-/* triangle that contains it. */
-/* */
-/*****************************************************************************/
-
-void makevertexmap(struct mesh *m, struct behavior *b)
-{
- struct otri triangleloop;
- vertex triorg;
-
- if (b->verbose) {
- printf(" Constructing mapping from vertices to triangles.\n");
- }
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- while (triangleloop.tri != (triangle *) NULL) {
- /* Check all three vertices of the triangle. */
- for (triangleloop.orient = 0; triangleloop.orient < 3;
- triangleloop.orient++) {
- org(triangleloop, triorg);
- setvertex2tri(triorg, encode(triangleloop));
- }
- triangleloop.tri = triangletraverse(m);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* preciselocate() Find a triangle or edge containing a given point. */
-/* */
-/* Begins its search from `searchtri'. It is important that `searchtri' */
-/* be a handle with the property that `searchpoint' is strictly to the left */
-/* of the edge denoted by `searchtri', or is collinear with that edge and */
-/* does not intersect that edge. (In particular, `searchpoint' should not */
-/* be the origin or destination of that edge.) */
-/* */
-/* These conditions are imposed because preciselocate() is normally used in */
-/* one of two situations: */
-/* */
-/* (1) To try to find the location to insert a new point. Normally, we */
-/* know an edge that the point is strictly to the left of. In the */
-/* incremental Delaunay algorithm, that edge is a bounding box edge. */
-/* In Ruppert's Delaunay refinement algorithm for quality meshing, */
-/* that edge is the shortest edge of the triangle whose circumcenter */
-/* is being inserted. */
-/* */
-/* (2) To try to find an existing point. In this case, any edge on the */
-/* convex hull is a good starting edge. You must screen out the */
-/* possibility that the vertex sought is an endpoint of the starting */
-/* edge before you call preciselocate(). */
-/* */
-/* On completion, `searchtri' is a triangle that contains `searchpoint'. */
-/* */
-/* This implementation differs from that given by Guibas and Stolfi. It */
-/* walks from triangle to triangle, crossing an edge only if `searchpoint' */
-/* is on the other side of the line containing that edge. After entering */
-/* a triangle, there are two edges by which one can leave that triangle. */
-/* If both edges are valid (`searchpoint' is on the other side of both */
-/* edges), one of the two is chosen by drawing a line perpendicular to */
-/* the entry edge (whose endpoints are `forg' and `fdest') passing through */
-/* `fapex'. Depending on which side of this perpendicular `searchpoint' */
-/* falls on, an exit edge is chosen. */
-/* */
-/* This implementation is empirically faster than the Guibas and Stolfi */
-/* point location routine (which I originally used), which tends to spiral */
-/* in toward its target. */
-/* */
-/* Returns ONVERTEX if the point lies on an existing vertex. `searchtri' */
-/* is a handle whose origin is the existing vertex. */
-/* */
-/* Returns ONEDGE if the point lies on a mesh edge. `searchtri' is a */
-/* handle whose primary edge is the edge on which the point lies. */
-/* */
-/* Returns INTRIANGLE if the point lies strictly within a triangle. */
-/* `searchtri' is a handle on the triangle that contains the point. */
-/* */
-/* Returns OUTSIDE if the point lies outside the mesh. `searchtri' is a */
-/* handle whose primary edge the point is to the right of. This might */
-/* occur when the circumcenter of a triangle falls just slightly outside */
-/* the mesh due to floating-point roundoff error. It also occurs when */
-/* seeking a hole or region point that a foolish user has placed outside */
-/* the mesh. */
-/* */
-/* If `stopatsubsegment' is nonzero, the search will stop if it tries to */
-/* walk through a subsegment, and will return OUTSIDE. */
-/* */
-/* WARNING: This routine is designed for convex triangulations, and will */
-/* not generally work after the holes and concavities have been carved. */
-/* However, it can still be used to find the circumcenter of a triangle, as */
-/* long as the search is begun from the triangle in question. */
-/* */
-/*****************************************************************************/
-
-enum locateresult preciselocate(struct mesh *m, struct behavior *b,
- vertex searchpoint, struct otri *searchtri,
- int stopatsubsegment)
-{
- struct otri backtracktri;
- struct osub checkedge;
- vertex forg, fdest, fapex;
- float orgorient, destorient;
- int moveleft;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- if (b->verbose > 2) {
- printf(" Searching for point (%.12g, %.12g).\n",
- searchpoint[0], searchpoint[1]);
- }
- /* Where are we? */
- org(*searchtri, forg);
- dest(*searchtri, fdest);
- apex(*searchtri, fapex);
- while (1) {
- if (b->verbose > 2) {
- printf(" At (%.12g, %.12g) (%.12g, %.12g) (%.12g, %.12g)\n",
- forg[0], forg[1], fdest[0], fdest[1], fapex[0], fapex[1]);
- }
- /* Check whether the apex is the point we seek. */
- if ((fapex[0] == searchpoint[0]) && (fapex[1] == searchpoint[1])) {
- lprevself(*searchtri);
- return ONVERTEX;
- }
- /* Does the point lie on the other side of the line defined by the */
- /* triangle edge opposite the triangle's destination? */
- destorient = counterclockwise(m, b, forg, fapex, searchpoint);
- /* Does the point lie on the other side of the line defined by the */
- /* triangle edge opposite the triangle's origin? */
- orgorient = counterclockwise(m, b, fapex, fdest, searchpoint);
- if (destorient > 0.0) {
- if (orgorient > 0.0) {
- /* Move left if the inner product of (fapex - searchpoint) and */
- /* (fdest - forg) is positive. This is equivalent to drawing */
- /* a line perpendicular to the line (forg, fdest) and passing */
- /* through `fapex', and determining which side of this line */
- /* `searchpoint' falls on. */
- moveleft = (fapex[0] - searchpoint[0]) * (fdest[0] - forg[0]) +
- (fapex[1] - searchpoint[1]) * (fdest[1] - forg[1]) > 0.0;
- } else {
- moveleft = 1;
- }
- } else {
- if (orgorient > 0.0) {
- moveleft = 0;
- } else {
- /* The point we seek must be on the boundary of or inside this */
- /* triangle. */
- if (destorient == 0.0) {
- lprevself(*searchtri);
- return ONEDGE;
- }
- if (orgorient == 0.0) {
- lnextself(*searchtri);
- return ONEDGE;
- }
- return INTRIANGLE;
- }
- }
-
- /* Move to another triangle. Leave a trace `backtracktri' in case */
- /* floating-point roundoff or some such bogey causes us to walk */
- /* off a boundary of the triangulation. */
- if (moveleft) {
- lprev(*searchtri, backtracktri);
- fdest = fapex;
- } else {
- lnext(*searchtri, backtracktri);
- forg = fapex;
- }
- sym(backtracktri, *searchtri);
-
- if (m->checksegments && stopatsubsegment) {
- /* Check for walking through a subsegment. */
- tspivot(backtracktri, checkedge);
- if (checkedge.ss != m->dummysub) {
- /* Go back to the last triangle. */
- otricopy(backtracktri, *searchtri);
- return OUTSIDE;
- }
- }
- /* Check for walking right out of the triangulation. */
- if (searchtri->tri == m->dummytri) {
- /* Go back to the last triangle. */
- otricopy(backtracktri, *searchtri);
- return OUTSIDE;
- }
-
- apex(*searchtri, fapex);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* locate() Find a triangle or edge containing a given point. */
-/* */
-/* Searching begins from one of: the input `searchtri', a recently */
-/* encountered triangle `recenttri', or from a triangle chosen from a */
-/* random sample. The choice is made by determining which triangle's */
-/* origin is closest to the point we are searching for. Normally, */
-/* `searchtri' should be a handle on the convex hull of the triangulation. */
-/* */
-/* Details on the random sampling method can be found in the Mucke, Saias, */
-/* and Zhu paper cited in the header of this code. */
-/* */
-/* On completion, `searchtri' is a triangle that contains `searchpoint'. */
-/* */
-/* Returns ONVERTEX if the point lies on an existing vertex. `searchtri' */
-/* is a handle whose origin is the existing vertex. */
-/* */
-/* Returns ONEDGE if the point lies on a mesh edge. `searchtri' is a */
-/* handle whose primary edge is the edge on which the point lies. */
-/* */
-/* Returns INTRIANGLE if the point lies strictly within a triangle. */
-/* `searchtri' is a handle on the triangle that contains the point. */
-/* */
-/* Returns OUTSIDE if the point lies outside the mesh. `searchtri' is a */
-/* handle whose primary edge the point is to the right of. This might */
-/* occur when the circumcenter of a triangle falls just slightly outside */
-/* the mesh due to floating-point roundoff error. It also occurs when */
-/* seeking a hole or region point that a foolish user has placed outside */
-/* the mesh. */
-/* */
-/* WARNING: This routine is designed for convex triangulations, and will */
-/* not generally work after the holes and concavities have been carved. */
-/* */
-/*****************************************************************************/
-
-enum locateresult locate(struct mesh *m, struct behavior *b,
- vertex searchpoint, struct otri *searchtri)
-{
- int **sampleblock;
- char *firsttri;
- struct otri sampletri;
- vertex torg, tdest;
- unsigned long long alignptr;
- float searchdist, dist;
- float ahead;
- long samplesperblock, totalsamplesleft, samplesleft;
- long population, totalpopulation;
- triangle ptr; /* Temporary variable used by sym(). */
-
- if (b->verbose > 2) {
- printf(" Randomly sampling for a triangle near point (%.12g, %.12g).\n",
- searchpoint[0], searchpoint[1]);
- }
- /* Record the distance from the suggested starting triangle to the */
- /* point we seek. */
- org(*searchtri, torg);
- searchdist = (searchpoint[0] - torg[0]) * (searchpoint[0] - torg[0]) +
- (searchpoint[1] - torg[1]) * (searchpoint[1] - torg[1]);
- if (b->verbose > 2) {
- printf(" Boundary triangle has origin (%.12g, %.12g).\n",
- torg[0], torg[1]);
- }
-
- /* If a recently encountered triangle has been recorded and has not been */
- /* deallocated, test it as a good starting point. */
- if (m->recenttri.tri != (triangle *) NULL) {
- if (!deadtri(m->recenttri.tri)) {
- org(m->recenttri, torg);
- if ((torg[0] == searchpoint[0]) && (torg[1] == searchpoint[1])) {
- otricopy(m->recenttri, *searchtri);
- return ONVERTEX;
- }
- dist = (searchpoint[0] - torg[0]) * (searchpoint[0] - torg[0]) +
- (searchpoint[1] - torg[1]) * (searchpoint[1] - torg[1]);
- if (dist < searchdist) {
- otricopy(m->recenttri, *searchtri);
- searchdist = dist;
- if (b->verbose > 2) {
- printf(" Choosing recent triangle with origin (%.12g, %.12g).\n",
- torg[0], torg[1]);
- }
- }
- }
- }
-
- /* The number of random samples taken is proportional to the cube root of */
- /* the number of triangles in the mesh. The next bit of code assumes */
- /* that the number of triangles increases monotonically (or at least */
- /* doesn't decrease enough to matter). */
- while (SAMPLEFACTOR * m->samples * m->samples * m->samples <
- m->triangles.items) {
- m->samples++;
- }
-
- /* We'll draw ceiling(samples * TRIPERBLOCK / maxitems) random samples */
- /* from each block of triangles (except the first)--until we meet the */
- /* sample quota. The ceiling means that blocks at the end might be */
- /* neglected, but I don't care. */
- samplesperblock = (m->samples * TRIPERBLOCK - 1) / m->triangles.maxitems + 1;
- /* We'll draw ceiling(samples * itemsfirstblock / maxitems) random samples */
- /* from the first block of triangles. */
- samplesleft = (m->samples * m->triangles.itemsfirstblock - 1) /
- m->triangles.maxitems + 1;
- totalsamplesleft = m->samples;
- population = m->triangles.itemsfirstblock;
- totalpopulation = m->triangles.maxitems;
- sampleblock = m->triangles.firstblock;
- sampletri.orient = 0;
- while (totalsamplesleft > 0) {
- /* If we're in the last block, `population' needs to be corrected. */
- if (population > totalpopulation) {
- population = totalpopulation;
- }
- /* Find a pointer to the first triangle in the block. */
- alignptr = (unsigned long long) (sampleblock + 1);
- firsttri = (char *) (alignptr +
- (unsigned long long) m->triangles.alignbytes -
- (alignptr %
- (unsigned long long) m->triangles.alignbytes));
-
- /* Choose `samplesleft' randomly sampled triangles in this block. */
- do {
- sampletri.tri = (triangle *) (firsttri +
- (randomnation((unsigned int) population) *
- m->triangles.itembytes));
- if (!deadtri(sampletri.tri)) {
- org(sampletri, torg);
- dist = (searchpoint[0] - torg[0]) * (searchpoint[0] - torg[0]) +
- (searchpoint[1] - torg[1]) * (searchpoint[1] - torg[1]);
- if (dist < searchdist) {
- otricopy(sampletri, *searchtri);
- searchdist = dist;
- if (b->verbose > 2) {
- printf(" Choosing triangle with origin (%.12g, %.12g).\n",
- torg[0], torg[1]);
- }
- }
- }
-
- samplesleft--;
- totalsamplesleft--;
- } while ((samplesleft > 0) && (totalsamplesleft > 0));
-
- if (totalsamplesleft > 0) {
- sampleblock = (int **) *sampleblock;
- samplesleft = samplesperblock;
- totalpopulation -= population;
- population = TRIPERBLOCK;
- }
- }
-
- /* Where are we? */
- org(*searchtri, torg);
- dest(*searchtri, tdest);
- /* Check the starting triangle's vertices. */
- if ((torg[0] == searchpoint[0]) && (torg[1] == searchpoint[1])) {
- return ONVERTEX;
- }
- if ((tdest[0] == searchpoint[0]) && (tdest[1] == searchpoint[1])) {
- lnextself(*searchtri);
- return ONVERTEX;
- }
- /* Orient `searchtri' to fit the preconditions of calling preciselocate(). */
- ahead = counterclockwise(m, b, torg, tdest, searchpoint);
- if (ahead < 0.0) {
- /* Turn around so that `searchpoint' is to the left of the */
- /* edge specified by `searchtri'. */
- symself(*searchtri);
- } else if (ahead == 0.0) {
- /* Check if `searchpoint' is between `torg' and `tdest'. */
- if (((torg[0] < searchpoint[0]) == (searchpoint[0] < tdest[0])) &&
- ((torg[1] < searchpoint[1]) == (searchpoint[1] < tdest[1]))) {
- return ONEDGE;
- }
- }
- return preciselocate(m, b, searchpoint, searchtri, 0);
-}
-
-/** **/
-/** **/
-/********* Point location routines end here *********/
-
-/********* Mesh transformation routines begin here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* insertsubseg() Create a new subsegment and insert it between two */
-/* triangles. */
-/* */
-/* The new subsegment is inserted at the edge described by the handle */
-/* `tri'. Its vertices are properly initialized. The marker `subsegmark' */
-/* is applied to the subsegment and, if appropriate, its vertices. */
-/* */
-/*****************************************************************************/
-
-void insertsubseg(struct mesh *m, struct behavior *b, struct otri *tri,
- int subsegmark)
-{
- struct otri oppotri;
- struct osub newsubseg;
- vertex triorg, tridest;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- org(*tri, triorg);
- dest(*tri, tridest);
- /* Mark vertices if possible. */
- if (vertexmark(triorg) == 0) {
- setvertexmark(triorg, subsegmark);
- }
- if (vertexmark(tridest) == 0) {
- setvertexmark(tridest, subsegmark);
- }
- /* Check if there's already a subsegment here. */
- tspivot(*tri, newsubseg);
- if (newsubseg.ss == m->dummysub) {
- /* Make new subsegment and initialize its vertices. */
- makesubseg(m, &newsubseg);
- setsorg(newsubseg, tridest);
- setsdest(newsubseg, triorg);
- setsegorg(newsubseg, tridest);
- setsegdest(newsubseg, triorg);
- /* Bond new subsegment to the two triangles it is sandwiched between. */
- /* Note that the facing triangle `oppotri' might be equal to */
- /* `dummytri' (outer space), but the new subsegment is bonded to it */
- /* all the same. */
- tsbond(*tri, newsubseg);
- sym(*tri, oppotri);
- ssymself(newsubseg);
- tsbond(oppotri, newsubseg);
- setmark(newsubseg, subsegmark);
- if (b->verbose > 2) {
- printf(" Inserting new ");
- printsubseg(m, b, &newsubseg);
- }
- } else {
- if (mark(newsubseg) == 0) {
- setmark(newsubseg, subsegmark);
- }
- }
-}
-
-/*****************************************************************************/
-/* */
-/* Terminology */
-/* */
-/* A "local transformation" replaces a small set of triangles with another */
-/* set of triangles. This may or may not involve inserting or deleting a */
-/* vertex. */
-/* */
-/* The term "casing" is used to describe the set of triangles that are */
-/* attached to the triangles being transformed, but are not transformed */
-/* themselves. Think of the casing as a fixed hollow structure inside */
-/* which all the action happens. A "casing" is only defined relative to */
-/* a single transformation; each occurrence of a transformation will */
-/* involve a different casing. */
-/* */
-/*****************************************************************************/
-
-/*****************************************************************************/
-/* */
-/* flip() Transform two triangles to two different triangles by flipping */
-/* an edge counterclockwise within a quadrilateral. */
-/* */
-/* Imagine the original triangles, abc and bad, oriented so that the */
-/* shared edge ab lies in a horizontal plane, with the vertex b on the left */
-/* and the vertex a on the right. The vertex c lies below the edge, and */
-/* the vertex d lies above the edge. The `flipedge' handle holds the edge */
-/* ab of triangle abc, and is directed left, from vertex a to vertex b. */
-/* */
-/* The triangles abc and bad are deleted and replaced by the triangles cdb */
-/* and dca. The triangles that represent abc and bad are NOT deallocated; */
-/* they are reused for dca and cdb, respectively. Hence, any handles that */
-/* may have held the original triangles are still valid, although not */
-/* directed as they were before. */
-/* */
-/* Upon completion of this routine, the `flipedge' handle holds the edge */
-/* dc of triangle dca, and is directed down, from vertex d to vertex c. */
-/* (Hence, the two triangles have rotated counterclockwise.) */
-/* */
-/* WARNING: This transformation is geometrically valid only if the */
-/* quadrilateral adbc is convex. Furthermore, this transformation is */
-/* valid only if there is not a subsegment between the triangles abc and */
-/* bad. This routine does not check either of these preconditions, and */
-/* it is the responsibility of the calling routine to ensure that they are */
-/* met. If they are not, the streets shall be filled with wailing and */
-/* gnashing of teeth. */
-/* */
-/*****************************************************************************/
-
-void flip(struct mesh *m, struct behavior *b, struct otri *flipedge)
-{
- struct otri botleft, botright;
- struct otri topleft, topright;
- struct otri top;
- struct otri botlcasing, botrcasing;
- struct otri toplcasing, toprcasing;
- struct osub botlsubseg, botrsubseg;
- struct osub toplsubseg, toprsubseg;
- vertex leftvertex, rightvertex, botvertex;
- vertex farvertex;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- /* Identify the vertices of the quadrilateral. */
- org(*flipedge, rightvertex);
- dest(*flipedge, leftvertex);
- apex(*flipedge, botvertex);
- sym(*flipedge, top);
- apex(top, farvertex);
-
- /* Identify the casing of the quadrilateral. */
- lprev(top, topleft);
- sym(topleft, toplcasing);
- lnext(top, topright);
- sym(topright, toprcasing);
- lnext(*flipedge, botleft);
- sym(botleft, botlcasing);
- lprev(*flipedge, botright);
- sym(botright, botrcasing);
- /* Rotate the quadrilateral one-quarter turn counterclockwise. */
- bond(topleft, botlcasing);
- bond(botleft, botrcasing);
- bond(botright, toprcasing);
- bond(topright, toplcasing);
-
- if (m->checksegments) {
- /* Check for subsegments and rebond them to the quadrilateral. */
- tspivot(topleft, toplsubseg);
- tspivot(botleft, botlsubseg);
- tspivot(botright, botrsubseg);
- tspivot(topright, toprsubseg);
- if (toplsubseg.ss == m->dummysub) {
- tsdissolve(topright);
- } else {
- tsbond(topright, toplsubseg);
- }
- if (botlsubseg.ss == m->dummysub) {
- tsdissolve(topleft);
- } else {
- tsbond(topleft, botlsubseg);
- }
- if (botrsubseg.ss == m->dummysub) {
- tsdissolve(botleft);
- } else {
- tsbond(botleft, botrsubseg);
- }
- if (toprsubseg.ss == m->dummysub) {
- tsdissolve(botright);
- } else {
- tsbond(botright, toprsubseg);
- }
- }
-
- /* New vertex assignments for the rotated quadrilateral. */
- setorg(*flipedge, farvertex);
- setdest(*flipedge, botvertex);
- setapex(*flipedge, rightvertex);
- setorg(top, botvertex);
- setdest(top, farvertex);
- setapex(top, leftvertex);
- if (b->verbose > 2) {
- printf(" Edge flip results in left ");
- printtriangle(m, b, &top);
- printf(" and right ");
- printtriangle(m, b, flipedge);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* unflip() Transform two triangles to two different triangles by */
-/* flipping an edge clockwise within a quadrilateral. Reverses */
-/* the flip() operation so that the data structures representing */
-/* the triangles are back where they were before the flip(). */
-/* */
-/* Imagine the original triangles, abc and bad, oriented so that the */
-/* shared edge ab lies in a horizontal plane, with the vertex b on the left */
-/* and the vertex a on the right. The vertex c lies below the edge, and */
-/* the vertex d lies above the edge. The `flipedge' handle holds the edge */
-/* ab of triangle abc, and is directed left, from vertex a to vertex b. */
-/* */
-/* The triangles abc and bad are deleted and replaced by the triangles cdb */
-/* and dca. The triangles that represent abc and bad are NOT deallocated; */
-/* they are reused for cdb and dca, respectively. Hence, any handles that */
-/* may have held the original triangles are still valid, although not */
-/* directed as they were before. */
-/* */
-/* Upon completion of this routine, the `flipedge' handle holds the edge */
-/* cd of triangle cdb, and is directed up, from vertex c to vertex d. */
-/* (Hence, the two triangles have rotated clockwise.) */
-/* */
-/* WARNING: This transformation is geometrically valid only if the */
-/* quadrilateral adbc is convex. Furthermore, this transformation is */
-/* valid only if there is not a subsegment between the triangles abc and */
-/* bad. This routine does not check either of these preconditions, and */
-/* it is the responsibility of the calling routine to ensure that they are */
-/* met. If they are not, the streets shall be filled with wailing and */
-/* gnashing of teeth. */
-/* */
-/*****************************************************************************/
-
-void unflip(struct mesh *m, struct behavior *b, struct otri *flipedge)
-{
- struct otri botleft, botright;
- struct otri topleft, topright;
- struct otri top;
- struct otri botlcasing, botrcasing;
- struct otri toplcasing, toprcasing;
- struct osub botlsubseg, botrsubseg;
- struct osub toplsubseg, toprsubseg;
- vertex leftvertex, rightvertex, botvertex;
- vertex farvertex;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- /* Identify the vertices of the quadrilateral. */
- org(*flipedge, rightvertex);
- dest(*flipedge, leftvertex);
- apex(*flipedge, botvertex);
- sym(*flipedge, top);
- apex(top, farvertex);
-
- /* Identify the casing of the quadrilateral. */
- lprev(top, topleft);
- sym(topleft, toplcasing);
- lnext(top, topright);
- sym(topright, toprcasing);
- lnext(*flipedge, botleft);
- sym(botleft, botlcasing);
- lprev(*flipedge, botright);
- sym(botright, botrcasing);
- /* Rotate the quadrilateral one-quarter turn clockwise. */
- bond(topleft, toprcasing);
- bond(botleft, toplcasing);
- bond(botright, botlcasing);
- bond(topright, botrcasing);
-
- if (m->checksegments) {
- /* Check for subsegments and rebond them to the quadrilateral. */
- tspivot(topleft, toplsubseg);
- tspivot(botleft, botlsubseg);
- tspivot(botright, botrsubseg);
- tspivot(topright, toprsubseg);
- if (toplsubseg.ss == m->dummysub) {
- tsdissolve(botleft);
- } else {
- tsbond(botleft, toplsubseg);
- }
- if (botlsubseg.ss == m->dummysub) {
- tsdissolve(botright);
- } else {
- tsbond(botright, botlsubseg);
- }
- if (botrsubseg.ss == m->dummysub) {
- tsdissolve(topright);
- } else {
- tsbond(topright, botrsubseg);
- }
- if (toprsubseg.ss == m->dummysub) {
- tsdissolve(topleft);
- } else {
- tsbond(topleft, toprsubseg);
- }
- }
-
- /* New vertex assignments for the rotated quadrilateral. */
- setorg(*flipedge, botvertex);
- setdest(*flipedge, farvertex);
- setapex(*flipedge, leftvertex);
- setorg(top, farvertex);
- setdest(top, botvertex);
- setapex(top, rightvertex);
- if (b->verbose > 2) {
- printf(" Edge unflip results in left ");
- printtriangle(m, b, flipedge);
- printf(" and right ");
- printtriangle(m, b, &top);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* insertvertex() Insert a vertex into a Delaunay triangulation, */
-/* performing flips as necessary to maintain the Delaunay */
-/* property. */
-/* */
-/* The point `insertvertex' is located. If `searchtri.tri' is not NULL, */
-/* the search for the containing triangle begins from `searchtri'. If */
-/* `searchtri.tri' is NULL, a full point location procedure is called. */
-/* If `insertvertex' is found inside a triangle, the triangle is split into */
-/* three; if `insertvertex' lies on an edge, the edge is split in two, */
-/* thereby splitting the two adjacent triangles into four. Edge flips are */
-/* used to restore the Delaunay property. If `insertvertex' lies on an */
-/* existing vertex, no action is taken, and the value DUPLICATEVERTEX is */
-/* returned. On return, `searchtri' is set to a handle whose origin is the */
-/* existing vertex. */
-/* */
-/* Normally, the parameter `splitseg' is set to NULL, implying that no */
-/* subsegment should be split. In this case, if `insertvertex' is found to */
-/* lie on a segment, no action is taken, and the value VIOLATINGVERTEX is */
-/* returned. On return, `searchtri' is set to a handle whose primary edge */
-/* is the violated subsegment. */
-/* */
-/* If the calling routine wishes to split a subsegment by inserting a */
-/* vertex in it, the parameter `splitseg' should be that subsegment. In */
-/* this case, `searchtri' MUST be the triangle handle reached by pivoting */
-/* from that subsegment; no point location is done. */
-/* */
-/* `segmentflaws' and `triflaws' are flags that indicate whether or not */
-/* there should be checks for the creation of encroached subsegments or bad */
-/* quality triangles. If a newly inserted vertex encroaches upon */
-/* subsegments, these subsegments are added to the list of subsegments to */
-/* be split if `segmentflaws' is set. If bad triangles are created, these */
-/* are added to the queue if `triflaws' is set. */
-/* */
-/* If a duplicate vertex or violated segment does not prevent the vertex */
-/* from being inserted, the return value will be ENCROACHINGVERTEX if the */
-/* vertex encroaches upon a subsegment (and checking is enabled), or */
-/* SUCCESSFULVERTEX otherwise. In either case, `searchtri' is set to a */
-/* handle whose origin is the newly inserted vertex. */
-/* */
-/* insertvertex() does not use flip() for reasons of speed; some */
-/* information can be reused from edge flip to edge flip, like the */
-/* locations of subsegments. */
-/* */
-/*****************************************************************************/
-
-enum insertvertexresult insertvertex(struct mesh *m, struct behavior *b,
- vertex newvertex, struct otri *searchtri,
- struct osub *splitseg,
- int segmentflaws, int triflaws)
-{
- struct otri horiz;
- struct otri top;
- struct otri botleft, botright;
- struct otri topleft, topright;
- struct otri newbotleft, newbotright;
- struct otri newtopright;
- struct otri botlcasing, botrcasing;
- struct otri toplcasing, toprcasing;
- struct otri testtri;
- struct osub botlsubseg, botrsubseg;
- struct osub toplsubseg, toprsubseg;
- struct osub brokensubseg;
- struct osub checksubseg;
- struct osub rightsubseg;
- struct osub newsubseg;
- struct badsubseg *encroached;
- struct flipstacker *newflip;
- vertex first;
- vertex leftvertex, rightvertex, botvertex, topvertex, farvertex;
- vertex segmentorg, segmentdest;
- float attrib;
- float area;
- enum insertvertexresult success;
- enum locateresult intersect;
- int doflip;
- int mirrorflag;
- int enq;
- int i;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by spivot() and tspivot(). */
-
- if (b->verbose > 1) {
- printf(" Inserting (%.12g, %.12g).\n", newvertex[0], newvertex[1]);
- }
-
- if (splitseg == (struct osub *) NULL) {
- /* Find the location of the vertex to be inserted. Check if a good */
- /* starting triangle has already been provided by the caller. */
- if (searchtri->tri == m->dummytri) {
- /* Find a boundary triangle. */
- horiz.tri = m->dummytri;
- horiz.orient = 0;
- symself(horiz);
- /* Search for a triangle containing `newvertex'. */
- intersect = locate(m, b, newvertex, &horiz);
- } else {
- /* Start searching from the triangle provided by the caller. */
- otricopy(*searchtri, horiz);
- intersect = preciselocate(m, b, newvertex, &horiz, 1);
- }
- } else {
- /* The calling routine provides the subsegment in which */
- /* the vertex is inserted. */
- otricopy(*searchtri, horiz);
- intersect = ONEDGE;
- }
-
- if (intersect == ONVERTEX) {
- /* There's already a vertex there. Return in `searchtri' a triangle */
- /* whose origin is the existing vertex. */
- otricopy(horiz, *searchtri);
- otricopy(horiz, m->recenttri);
- return DUPLICATEVERTEX;
- }
- if ((intersect == ONEDGE) || (intersect == OUTSIDE)) {
- /* The vertex falls on an edge or boundary. */
- if (m->checksegments && (splitseg == (struct osub *) NULL)) {
- /* Check whether the vertex falls on a subsegment. */
- tspivot(horiz, brokensubseg);
- if (brokensubseg.ss != m->dummysub) {
- /* The vertex falls on a subsegment, and hence will not be inserted. */
- if (segmentflaws) {
- enq = b->nobisect != 2;
- if (enq && (b->nobisect == 1)) {
- /* This subsegment may be split only if it is an */
- /* internal boundary. */
- sym(horiz, testtri);
- enq = testtri.tri != m->dummytri;
- }
- if (enq) {
- /* Add the subsegment to the list of encroached subsegments. */
- encroached = (struct badsubseg *) poolalloc(&m->badsubsegs);
- encroached->encsubseg = sencode(brokensubseg);
- sorg(brokensubseg, encroached->subsegorg);
- sdest(brokensubseg, encroached->subsegdest);
- if (b->verbose > 2) {
- printf(
- " Queueing encroached subsegment (%.12g, %.12g) (%.12g, %.12g).\n",
- encroached->subsegorg[0], encroached->subsegorg[1],
- encroached->subsegdest[0], encroached->subsegdest[1]);
- }
- }
- }
- /* Return a handle whose primary edge contains the vertex, */
- /* which has not been inserted. */
- otricopy(horiz, *searchtri);
- otricopy(horiz, m->recenttri);
- return VIOLATINGVERTEX;
- }
- }
-
- /* Insert the vertex on an edge, dividing one triangle into two (if */
- /* the edge lies on a boundary) or two triangles into four. */
- lprev(horiz, botright);
- sym(botright, botrcasing);
- sym(horiz, topright);
- /* Is there a second triangle? (Or does this edge lie on a boundary?) */
- mirrorflag = topright.tri != m->dummytri;
- if (mirrorflag) {
- lnextself(topright);
- sym(topright, toprcasing);
- maketriangle(m, b, &newtopright);
- } else {
- /* Splitting a boundary edge increases the number of boundary edges. */
- m->hullsize++;
- }
- maketriangle(m, b, &newbotright);
-
- /* Set the vertices of changed and new triangles. */
- org(horiz, rightvertex);
- dest(horiz, leftvertex);
- apex(horiz, botvertex);
- setorg(newbotright, botvertex);
- setdest(newbotright, rightvertex);
- setapex(newbotright, newvertex);
- setorg(horiz, newvertex);
- for (i = 0; i < m->eextras; i++) {
- /* Set the element attributes of a new triangle. */
- setelemattribute(newbotright, i, elemattribute(botright, i));
- }
- if (b->vararea) {
- /* Set the area constraint of a new triangle. */
- setareabound(newbotright, areabound(botright));
- }
- if (mirrorflag) {
- dest(topright, topvertex);
- setorg(newtopright, rightvertex);
- setdest(newtopright, topvertex);
- setapex(newtopright, newvertex);
- setorg(topright, newvertex);
- for (i = 0; i < m->eextras; i++) {
- /* Set the element attributes of another new triangle. */
- setelemattribute(newtopright, i, elemattribute(topright, i));
- }
- if (b->vararea) {
- /* Set the area constraint of another new triangle. */
- setareabound(newtopright, areabound(topright));
- }
- }
-
- /* There may be subsegments that need to be bonded */
- /* to the new triangle(s). */
- if (m->checksegments) {
- tspivot(botright, botrsubseg);
- if (botrsubseg.ss != m->dummysub) {
- tsdissolve(botright);
- tsbond(newbotright, botrsubseg);
- }
- if (mirrorflag) {
- tspivot(topright, toprsubseg);
- if (toprsubseg.ss != m->dummysub) {
- tsdissolve(topright);
- tsbond(newtopright, toprsubseg);
- }
- }
- }
-
- /* Bond the new triangle(s) to the surrounding triangles. */
- bond(newbotright, botrcasing);
- lprevself(newbotright);
- bond(newbotright, botright);
- lprevself(newbotright);
- if (mirrorflag) {
- bond(newtopright, toprcasing);
- lnextself(newtopright);
- bond(newtopright, topright);
- lnextself(newtopright);
- bond(newtopright, newbotright);
- }
-
- if (splitseg != (struct osub *) NULL) {
- /* Split the subsegment into two. */
- setsdest(*splitseg, newvertex);
- segorg(*splitseg, segmentorg);
- segdest(*splitseg, segmentdest);
- ssymself(*splitseg);
- spivot(*splitseg, rightsubseg);
- insertsubseg(m, b, &newbotright, mark(*splitseg));
- tspivot(newbotright, newsubseg);
- setsegorg(newsubseg, segmentorg);
- setsegdest(newsubseg, segmentdest);
- sbond(*splitseg, newsubseg);
- ssymself(newsubseg);
- sbond(newsubseg, rightsubseg);
- ssymself(*splitseg);
- /* Transfer the subsegment's boundary marker to the vertex */
- /* if required. */
- if (vertexmark(newvertex) == 0) {
- setvertexmark(newvertex, mark(*splitseg));
- }
- }
-
- if (m->checkquality) {
- poolrestart(&m->flipstackers);
- m->lastflip = (struct flipstacker *) poolalloc(&m->flipstackers);
- m->lastflip->flippedtri = encode(horiz);
- m->lastflip->prevflip = (struct flipstacker *) &insertvertex;
- }
- if (b->verbose > 2) {
- printf(" Updating bottom left ");
- printtriangle(m, b, &botright);
- if (mirrorflag) {
- printf(" Updating top left ");
- printtriangle(m, b, &topright);
- printf(" Creating top right ");
- printtriangle(m, b, &newtopright);
- }
- printf(" Creating bottom right ");
- printtriangle(m, b, &newbotright);
- }
-
- /* Position `horiz' on the first edge to check for */
- /* the Delaunay property. */
- lnextself(horiz);
- } else {
- /* Insert the vertex in a triangle, splitting it into three. */
- lnext(horiz, botleft);
- lprev(horiz, botright);
- sym(botleft, botlcasing);
- sym(botright, botrcasing);
- maketriangle(m, b, &newbotleft);
- maketriangle(m, b, &newbotright);
-
- /* Set the vertices of changed and new triangles. */
- org(horiz, rightvertex);
- dest(horiz, leftvertex);
- apex(horiz, botvertex);
- setorg(newbotleft, leftvertex);
- setdest(newbotleft, botvertex);
- setapex(newbotleft, newvertex);
- setorg(newbotright, botvertex);
- setdest(newbotright, rightvertex);
- setapex(newbotright, newvertex);
- setapex(horiz, newvertex);
- for (i = 0; i < m->eextras; i++) {
- /* Set the element attributes of the new triangles. */
- attrib = elemattribute(horiz, i);
- setelemattribute(newbotleft, i, attrib);
- setelemattribute(newbotright, i, attrib);
- }
- if (b->vararea) {
- /* Set the area constraint of the new triangles. */
- area = areabound(horiz);
- setareabound(newbotleft, area);
- setareabound(newbotright, area);
- }
-
- /* There may be subsegments that need to be bonded */
- /* to the new triangles. */
- if (m->checksegments) {
- tspivot(botleft, botlsubseg);
- if (botlsubseg.ss != m->dummysub) {
- tsdissolve(botleft);
- tsbond(newbotleft, botlsubseg);
- }
- tspivot(botright, botrsubseg);
- if (botrsubseg.ss != m->dummysub) {
- tsdissolve(botright);
- tsbond(newbotright, botrsubseg);
- }
- }
-
- /* Bond the new triangles to the surrounding triangles. */
- bond(newbotleft, botlcasing);
- bond(newbotright, botrcasing);
- lnextself(newbotleft);
- lprevself(newbotright);
- bond(newbotleft, newbotright);
- lnextself(newbotleft);
- bond(botleft, newbotleft);
- lprevself(newbotright);
- bond(botright, newbotright);
-
- if (m->checkquality) {
- poolrestart(&m->flipstackers);
- m->lastflip = (struct flipstacker *) poolalloc(&m->flipstackers);
- m->lastflip->flippedtri = encode(horiz);
- m->lastflip->prevflip = (struct flipstacker *) NULL;
- }
- if (b->verbose > 2) {
- printf(" Updating top ");
- printtriangle(m, b, &horiz);
- printf(" Creating left ");
- printtriangle(m, b, &newbotleft);
- printf(" Creating right ");
- printtriangle(m, b, &newbotright);
- }
- }
-
- /* The insertion is successful by default, unless an encroached */
- /* subsegment is found. */
- success = SUCCESSFULVERTEX;
- /* Circle around the newly inserted vertex, checking each edge opposite */
- /* it for the Delaunay property. Non-Delaunay edges are flipped. */
- /* `horiz' is always the edge being checked. `first' marks where to */
- /* stop circling. */
- org(horiz, first);
- rightvertex = first;
- dest(horiz, leftvertex);
- /* Circle until finished. */
- while (1) {
- /* By default, the edge will be flipped. */
- doflip = 1;
-
- if (m->checksegments) {
- /* Check for a subsegment, which cannot be flipped. */
- tspivot(horiz, checksubseg);
- if (checksubseg.ss != m->dummysub) {
- /* The edge is a subsegment and cannot be flipped. */
- doflip = 0;
- }
- }
-
- if (doflip) {
- /* Check if the edge is a boundary edge. */
- sym(horiz, top);
- if (top.tri == m->dummytri) {
- /* The edge is a boundary edge and cannot be flipped. */
- doflip = 0;
- } else {
- /* Find the vertex on the other side of the edge. */
- apex(top, farvertex);
- /* In the incremental Delaunay triangulation algorithm, any of */
- /* `leftvertex', `rightvertex', and `farvertex' could be vertices */
- /* of the triangular bounding box. These vertices must be */
- /* treated as if they are infinitely distant, even though their */
- /* "coordinates" are not. */
- if ((leftvertex == m->infvertex1) || (leftvertex == m->infvertex2) ||
- (leftvertex == m->infvertex3)) {
- /* `leftvertex' is infinitely distant. Check the convexity of */
- /* the boundary of the triangulation. 'farvertex' might be */
- /* infinite as well, but trust me, this same condition should */
- /* be applied. */
- doflip = counterclockwise(m, b, newvertex, rightvertex, farvertex)
- > 0.0;
- } else if ((rightvertex == m->infvertex1) ||
- (rightvertex == m->infvertex2) ||
- (rightvertex == m->infvertex3)) {
- /* `rightvertex' is infinitely distant. Check the convexity of */
- /* the boundary of the triangulation. 'farvertex' might be */
- /* infinite as well, but trust me, this same condition should */
- /* be applied. */
- doflip = counterclockwise(m, b, farvertex, leftvertex, newvertex)
- > 0.0;
- } else if ((farvertex == m->infvertex1) ||
- (farvertex == m->infvertex2) ||
- (farvertex == m->infvertex3)) {
- /* `farvertex' is infinitely distant and cannot be inside */
- /* the circumcircle of the triangle `horiz'. */
- doflip = 0;
- } else {
- /* Test whether the edge is locally Delaunay. */
- doflip = incircle(m, b, leftvertex, newvertex, rightvertex,
- farvertex) > 0.0;
- }
- if (doflip) {
- /* We made it! Flip the edge `horiz' by rotating its containing */
- /* quadrilateral (the two triangles adjacent to `horiz'). */
- /* Identify the casing of the quadrilateral. */
- lprev(top, topleft);
- sym(topleft, toplcasing);
- lnext(top, topright);
- sym(topright, toprcasing);
- lnext(horiz, botleft);
- sym(botleft, botlcasing);
- lprev(horiz, botright);
- sym(botright, botrcasing);
- /* Rotate the quadrilateral one-quarter turn counterclockwise. */
- bond(topleft, botlcasing);
- bond(botleft, botrcasing);
- bond(botright, toprcasing);
- bond(topright, toplcasing);
- if (m->checksegments) {
- /* Check for subsegments and rebond them to the quadrilateral. */
- tspivot(topleft, toplsubseg);
- tspivot(botleft, botlsubseg);
- tspivot(botright, botrsubseg);
- tspivot(topright, toprsubseg);
- if (toplsubseg.ss == m->dummysub) {
- tsdissolve(topright);
- } else {
- tsbond(topright, toplsubseg);
- }
- if (botlsubseg.ss == m->dummysub) {
- tsdissolve(topleft);
- } else {
- tsbond(topleft, botlsubseg);
- }
- if (botrsubseg.ss == m->dummysub) {
- tsdissolve(botleft);
- } else {
- tsbond(botleft, botrsubseg);
- }
- if (toprsubseg.ss == m->dummysub) {
- tsdissolve(botright);
- } else {
- tsbond(botright, toprsubseg);
- }
- }
- /* New vertex assignments for the rotated quadrilateral. */
- setorg(horiz, farvertex);
- setdest(horiz, newvertex);
- setapex(horiz, rightvertex);
- setorg(top, newvertex);
- setdest(top, farvertex);
- setapex(top, leftvertex);
- for (i = 0; i < m->eextras; i++) {
- /* Take the average of the two triangles' attributes. */
- attrib = 0.5 * (elemattribute(top, i) + elemattribute(horiz, i));
- setelemattribute(top, i, attrib);
- setelemattribute(horiz, i, attrib);
- }
- if (b->vararea) {
- if ((areabound(top) <= 0.0) || (areabound(horiz) <= 0.0)) {
- area = -1.0;
- } else {
- /* Take the average of the two triangles' area constraints. */
- /* This prevents small area constraints from migrating a */
- /* long, long way from their original location due to flips. */
- area = 0.5 * (areabound(top) + areabound(horiz));
- }
- setareabound(top, area);
- setareabound(horiz, area);
- }
-
- if (m->checkquality) {
- newflip = (struct flipstacker *) poolalloc(&m->flipstackers);
- newflip->flippedtri = encode(horiz);
- newflip->prevflip = m->lastflip;
- m->lastflip = newflip;
- }
- if (b->verbose > 2) {
- printf(" Edge flip results in left ");
- lnextself(topleft);
- printtriangle(m, b, &topleft);
- printf(" and right ");
- printtriangle(m, b, &horiz);
- }
- /* On the next iterations, consider the two edges that were */
- /* exposed (this is, are now visible to the newly inserted */
- /* vertex) by the edge flip. */
- lprevself(horiz);
- leftvertex = farvertex;
- }
- }
- }
- if (!doflip) {
- /* The handle `horiz' is accepted as locally Delaunay. */
- /* Look for the next edge around the newly inserted vertex. */
- lnextself(horiz);
- sym(horiz, testtri);
- /* Check for finishing a complete revolution about the new vertex, or */
- /* falling outside of the triangulation. The latter will happen */
- /* when a vertex is inserted at a boundary. */
- if ((leftvertex == first) || (testtri.tri == m->dummytri)) {
- /* We're done. Return a triangle whose origin is the new vertex. */
- lnext(horiz, *searchtri);
- lnext(horiz, m->recenttri);
- return success;
- }
- /* Finish finding the next edge around the newly inserted vertex. */
- lnext(testtri, horiz);
- rightvertex = leftvertex;
- dest(horiz, leftvertex);
- }
- }
-}
-
-/*****************************************************************************/
-/* */
-/* triangulatepolygon() Find the Delaunay triangulation of a polygon that */
-/* has a certain "nice" shape. This includes the */
-/* polygons that result from deletion of a vertex or */
-/* insertion of a segment. */
-/* */
-/* This is a conceptually difficult routine. The starting assumption is */
-/* that we have a polygon with n sides. n - 1 of these sides are currently */
-/* represented as edges in the mesh. One side, called the "base", need not */
-/* be. */
-/* */
-/* Inside the polygon is a structure I call a "fan", consisting of n - 1 */
-/* triangles that share a common origin. For each of these triangles, the */
-/* edge opposite the origin is one of the sides of the polygon. The */
-/* primary edge of each triangle is the edge directed from the origin to */
-/* the destination; note that this is not the same edge that is a side of */
-/* the polygon. `firstedge' is the primary edge of the first triangle. */
-/* From there, the triangles follow in counterclockwise order about the */
-/* polygon, until `lastedge', the primary edge of the last triangle. */
-/* `firstedge' and `lastedge' are probably connected to other triangles */
-/* beyond the extremes of the fan, but their identity is not important, as */
-/* long as the fan remains connected to them. */
-/* */
-/* Imagine the polygon oriented so that its base is at the bottom. This */
-/* puts `firstedge' on the far right, and `lastedge' on the far left. */
-/* The right vertex of the base is the destination of `firstedge', and the */
-/* left vertex of the base is the apex of `lastedge'. */
-/* */
-/* The challenge now is to find the right sequence of edge flips to */
-/* transform the fan into a Delaunay triangulation of the polygon. Each */
-/* edge flip effectively removes one triangle from the fan, committing it */
-/* to the polygon. The resulting polygon has one fewer edge. If `doflip' */
-/* is set, the final flip will be performed, resulting in a fan of one */
-/* (useless?) triangle. If `doflip' is not set, the final flip is not */
-/* performed, resulting in a fan of two triangles, and an unfinished */
-/* triangular polygon that is not yet filled out with a single triangle. */
-/* On completion of the routine, `lastedge' is the last remaining triangle, */
-/* or the leftmost of the last two. */
-/* */
-/* Although the flips are performed in the order described above, the */
-/* decisions about what flips to perform are made in precisely the reverse */
-/* order. The recursive triangulatepolygon() procedure makes a decision, */
-/* uses up to two recursive calls to triangulate the "subproblems" */
-/* (polygons with fewer edges), and then performs an edge flip. */
-/* */
-/* The "decision" it makes is which vertex of the polygon should be */
-/* connected to the base. This decision is made by testing every possible */
-/* vertex. Once the best vertex is found, the two edges that connect this */
-/* vertex to the base become the bases for two smaller polygons. These */
-/* are triangulated recursively. Unfortunately, this approach can take */
-/* O(n^2) time not only in the worst case, but in many common cases. It's */
-/* rarely a big deal for vertex deletion, where n is rarely larger than */
-/* ten, but it could be a big deal for segment insertion, especially if */
-/* there's a lot of long segments that each cut many triangles. I ought to */
-/* code a faster algorithm some day. */
-/* */
-/* The `edgecount' parameter is the number of sides of the polygon, */
-/* including its base. `triflaws' is a flag that determines whether the */
-/* new triangles should be tested for quality, and enqueued if they are */
-/* bad. */
-/* */
-/*****************************************************************************/
-
-void triangulatepolygon(struct mesh *m, struct behavior *b,
- struct otri *firstedge, struct otri *lastedge,
- int edgecount, int doflip, int triflaws)
-{
- struct otri testtri;
- struct otri besttri;
- struct otri tempedge;
- vertex leftbasevertex, rightbasevertex;
- vertex testvertex;
- vertex bestvertex;
- int bestnumber;
- int i;
- triangle ptr; /* Temporary variable used by sym(), onext(), and oprev(). */
-
- /* Identify the base vertices. */
- apex(*lastedge, leftbasevertex);
- dest(*firstedge, rightbasevertex);
- if (b->verbose > 2) {
- printf(" Triangulating interior polygon at edge\n");
- printf(" (%.12g, %.12g) (%.12g, %.12g)\n", leftbasevertex[0],
- leftbasevertex[1], rightbasevertex[0], rightbasevertex[1]);
- }
- /* Find the best vertex to connect the base to. */
- onext(*firstedge, besttri);
- dest(besttri, bestvertex);
- otricopy(besttri, testtri);
- bestnumber = 1;
- for (i = 2; i <= edgecount - 2; i++) {
- onextself(testtri);
- dest(testtri, testvertex);
- /* Is this a better vertex? */
- if (incircle(m, b, leftbasevertex, rightbasevertex, bestvertex,
- testvertex) > 0.0) {
- otricopy(testtri, besttri);
- bestvertex = testvertex;
- bestnumber = i;
- }
- }
- if (b->verbose > 2) {
- printf(" Connecting edge to (%.12g, %.12g)\n", bestvertex[0],
- bestvertex[1]);
- }
- if (bestnumber > 1) {
- /* Recursively triangulate the smaller polygon on the right. */
- oprev(besttri, tempedge);
- triangulatepolygon(m, b, firstedge, &tempedge, bestnumber + 1, 1,
- triflaws);
- }
- if (bestnumber < edgecount - 2) {
- /* Recursively triangulate the smaller polygon on the left. */
- sym(besttri, tempedge);
- triangulatepolygon(m, b, &besttri, lastedge, edgecount - bestnumber, 1,
- triflaws);
- /* Find `besttri' again; it may have been lost to edge flips. */
- sym(tempedge, besttri);
- }
- if (doflip) {
- /* Do one final edge flip. */
- flip(m, b, &besttri);
- }
- /* Return the base triangle. */
- otricopy(besttri, *lastedge);
-}
-
-/** **/
-/** **/
-/********* Mesh transformation routines end here *********/
-
-/********* Divide-and-conquer Delaunay triangulation begins here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* The divide-and-conquer bounding box */
-/* */
-/* I originally implemented the divide-and-conquer and incremental Delaunay */
-/* triangulations using the edge-based data structure presented by Guibas */
-/* and Stolfi. Switching to a triangle-based data structure doubled the */
-/* speed. However, I had to think of a few extra tricks to maintain the */
-/* elegance of the original algorithms. */
-/* */
-/* The "bounding box" used by my variant of the divide-and-conquer */
-/* algorithm uses one triangle for each edge of the convex hull of the */
-/* triangulation. These bounding triangles all share a common apical */
-/* vertex, which is represented by NULL and which represents nothing. */
-/* The bounding triangles are linked in a circular fan about this NULL */
-/* vertex, and the edges on the convex hull of the triangulation appear */
-/* opposite the NULL vertex. You might find it easiest to imagine that */
-/* the NULL vertex is a point in 3D space behind the center of the */
-/* triangulation, and that the bounding triangles form a sort of cone. */
-/* */
-/* This bounding box makes it easy to represent degenerate cases. For */
-/* instance, the triangulation of two vertices is a single edge. This edge */
-/* is represented by two bounding box triangles, one on each "side" of the */
-/* edge. These triangles are also linked together in a fan about the NULL */
-/* vertex. */
-/* */
-/* The bounding box also makes it easy to traverse the convex hull, as the */
-/* divide-and-conquer algorithm needs to do. */
-/* */
-/*****************************************************************************/
-
-/*****************************************************************************/
-/* */
-/* vertexsort() Sort an array of vertices by x-coordinate, using the */
-/* y-coordinate as a secondary key. */
-/* */
-/* Uses quicksort. Randomized O(n log n) time. No, I did not make any of */
-/* the usual quicksort mistakes. */
-/* */
-/*****************************************************************************/
-
-void vertexsort(vertex *sortarray, int arraysize)
-{
- int left, right;
- int pivot;
- float pivotx, pivoty;
- vertex temp;
-
- if (arraysize == 2) {
- /* Recursive base case. */
- if ((sortarray[0][0] > sortarray[1][0]) ||
- ((sortarray[0][0] == sortarray[1][0]) &&
- (sortarray[0][1] > sortarray[1][1]))) {
- temp = sortarray[1];
- sortarray[1] = sortarray[0];
- sortarray[0] = temp;
- }
- return;
- }
- /* Choose a random pivot to split the array. */
- pivot = (int) randomnation((unsigned int) arraysize);
- pivotx = sortarray[pivot][0];
- pivoty = sortarray[pivot][1];
- /* Split the array. */
- left = -1;
- right = arraysize;
- while (left < right) {
- /* Search for a vertex whose x-coordinate is too large for the left. */
- do {
- left++;
- } while ((left <= right) && ((sortarray[left][0] < pivotx) ||
- ((sortarray[left][0] == pivotx) &&
- (sortarray[left][1] < pivoty))));
- /* Search for a vertex whose x-coordinate is too small for the right. */
- do {
- right--;
- } while ((left <= right) && ((sortarray[right][0] > pivotx) ||
- ((sortarray[right][0] == pivotx) &&
- (sortarray[right][1] > pivoty))));
- if (left < right) {
- /* Swap the left and right vertices. */
- temp = sortarray[left];
- sortarray[left] = sortarray[right];
- sortarray[right] = temp;
- }
- }
- if (left > 1) {
- /* Recursively sort the left subset. */
- vertexsort(sortarray, left);
- }
- if (right < arraysize - 2) {
- /* Recursively sort the right subset. */
- vertexsort(&sortarray[right + 1], arraysize - right - 1);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* vertexmedian() An order statistic algorithm, almost. Shuffles an */
-/* array of vertices so that the first `median' vertices */
-/* occur lexicographically before the remaining vertices. */
-/* */
-/* Uses the x-coordinate as the primary key if axis == 0; the y-coordinate */
-/* if axis == 1. Very similar to the vertexsort() procedure, but runs in */
-/* randomized linear time. */
-/* */
-/*****************************************************************************/
-
-void vertexmedian(vertex *sortarray, int arraysize, int median, int axis)
-{
- int left, right;
- int pivot;
- float pivot1, pivot2;
- vertex temp;
-
- if (arraysize == 2) {
- /* Recursive base case. */
- if ((sortarray[0][axis] > sortarray[1][axis]) ||
- ((sortarray[0][axis] == sortarray[1][axis]) &&
- (sortarray[0][1 - axis] > sortarray[1][1 - axis]))) {
- temp = sortarray[1];
- sortarray[1] = sortarray[0];
- sortarray[0] = temp;
- }
- return;
- }
- /* Choose a random pivot to split the array. */
- pivot = (int) randomnation((unsigned int) arraysize);
- pivot1 = sortarray[pivot][axis];
- pivot2 = sortarray[pivot][1 - axis];
- /* Split the array. */
- left = -1;
- right = arraysize;
- while (left < right) {
- /* Search for a vertex whose x-coordinate is too large for the left. */
- do {
- left++;
- } while ((left <= right) && ((sortarray[left][axis] < pivot1) ||
- ((sortarray[left][axis] == pivot1) &&
- (sortarray[left][1 - axis] < pivot2))));
- /* Search for a vertex whose x-coordinate is too small for the right. */
- do {
- right--;
- } while ((left <= right) && ((sortarray[right][axis] > pivot1) ||
- ((sortarray[right][axis] == pivot1) &&
- (sortarray[right][1 - axis] > pivot2))));
- if (left < right) {
- /* Swap the left and right vertices. */
- temp = sortarray[left];
- sortarray[left] = sortarray[right];
- sortarray[right] = temp;
- }
- }
- /* Unlike in vertexsort(), at most one of the following */
- /* conditionals is true. */
- if (left > median) {
- /* Recursively shuffle the left subset. */
- vertexmedian(sortarray, left, median, axis);
- }
- if (right < median - 1) {
- /* Recursively shuffle the right subset. */
- vertexmedian(&sortarray[right + 1], arraysize - right - 1,
- median - right - 1, axis);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* alternateaxes() Sorts the vertices as appropriate for the divide-and- */
-/* conquer algorithm with alternating cuts. */
-/* */
-/* Partitions by x-coordinate if axis == 0; by y-coordinate if axis == 1. */
-/* For the base case, subsets containing only two or three vertices are */
-/* always sorted by x-coordinate. */
-/* */
-/*****************************************************************************/
-
-void alternateaxes(vertex *sortarray, int arraysize, int axis)
-{
- int divider;
-
- divider = arraysize >> 1;
- if (arraysize <= 3) {
- /* Recursive base case: subsets of two or three vertices will be */
- /* handled specially, and should always be sorted by x-coordinate. */
- axis = 0;
- }
- /* Partition with a horizontal or vertical cut. */
- vertexmedian(sortarray, arraysize, divider, axis);
- /* Recursively partition the subsets with a cross cut. */
- if (arraysize - divider >= 2) {
- if (divider >= 2) {
- alternateaxes(sortarray, divider, 1 - axis);
- }
- alternateaxes(&sortarray[divider], arraysize - divider, 1 - axis);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* mergehulls() Merge two adjacent Delaunay triangulations into a */
-/* single Delaunay triangulation. */
-/* */
-/* This is similar to the algorithm given by Guibas and Stolfi, but uses */
-/* a triangle-based, rather than edge-based, data structure. */
-/* */
-/* The algorithm walks up the gap between the two triangulations, knitting */
-/* them together. As they are merged, some of their bounding triangles */
-/* are converted into real triangles of the triangulation. The procedure */
-/* pulls each hull's bounding triangles apart, then knits them together */
-/* like the teeth of two gears. The Delaunay property determines, at each */
-/* step, whether the next "tooth" is a bounding triangle of the left hull */
-/* or the right. When a bounding triangle becomes real, its apex is */
-/* changed from NULL to a real vertex. */
-/* */
-/* Only two new triangles need to be allocated. These become new bounding */
-/* triangles at the top and bottom of the seam. They are used to connect */
-/* the remaining bounding triangles (those that have not been converted */
-/* into real triangles) into a single fan. */
-/* */
-/* On entry, `farleft' and `innerleft' are bounding triangles of the left */
-/* triangulation. The origin of `farleft' is the leftmost vertex, and */
-/* the destination of `innerleft' is the rightmost vertex of the */
-/* triangulation. Similarly, `innerright' and `farright' are bounding */
-/* triangles of the right triangulation. The origin of `innerright' and */
-/* destination of `farright' are the leftmost and rightmost vertices. */
-/* */
-/* On completion, the origin of `farleft' is the leftmost vertex of the */
-/* merged triangulation, and the destination of `farright' is the rightmost */
-/* vertex. */
-/* */
-/*****************************************************************************/
-
-void mergehulls(struct mesh *m, struct behavior *b, struct otri *farleft,
- struct otri *innerleft, struct otri *innerright,
- struct otri *farright, int axis)
-{
- struct otri leftcand, rightcand;
- struct otri baseedge;
- struct otri nextedge;
- struct otri sidecasing, topcasing, outercasing;
- struct otri checkedge;
- vertex innerleftdest;
- vertex innerrightorg;
- vertex innerleftapex, innerrightapex;
- vertex farleftpt, farrightpt;
- vertex farleftapex, farrightapex;
- vertex lowerleft, lowerright;
- vertex upperleft, upperright;
- vertex nextapex;
- vertex checkvertex;
- int changemade;
- int badedge;
- int leftfinished, rightfinished;
- triangle ptr; /* Temporary variable used by sym(). */
-
- dest(*innerleft, innerleftdest);
- apex(*innerleft, innerleftapex);
- org(*innerright, innerrightorg);
- apex(*innerright, innerrightapex);
- /* Special treatment for horizontal cuts. */
- if (b->dwyer && (axis == 1)) {
- org(*farleft, farleftpt);
- apex(*farleft, farleftapex);
- dest(*farright, farrightpt);
- apex(*farright, farrightapex);
- /* The pointers to the extremal vertices are shifted to point to the */
- /* topmost and bottommost vertex of each hull, rather than the */
- /* leftmost and rightmost vertices. */
- while (farleftapex[1] < farleftpt[1]) {
- lnextself(*farleft);
- symself(*farleft);
- farleftpt = farleftapex;
- apex(*farleft, farleftapex);
- }
- sym(*innerleft, checkedge);
- apex(checkedge, checkvertex);
- while (checkvertex[1] > innerleftdest[1]) {
- lnext(checkedge, *innerleft);
- innerleftapex = innerleftdest;
- innerleftdest = checkvertex;
- sym(*innerleft, checkedge);
- apex(checkedge, checkvertex);
- }
- while (innerrightapex[1] < innerrightorg[1]) {
- lnextself(*innerright);
- symself(*innerright);
- innerrightorg = innerrightapex;
- apex(*innerright, innerrightapex);
- }
- sym(*farright, checkedge);
- apex(checkedge, checkvertex);
- while (checkvertex[1] > farrightpt[1]) {
- lnext(checkedge, *farright);
- farrightapex = farrightpt;
- farrightpt = checkvertex;
- sym(*farright, checkedge);
- apex(checkedge, checkvertex);
- }
- }
- /* Find a line tangent to and below both hulls. */
- do {
- changemade = 0;
- /* Make innerleftdest the "bottommost" vertex of the left hull. */
- if (counterclockwise(m, b, innerleftdest, innerleftapex, innerrightorg) >
- 0.0) {
- lprevself(*innerleft);
- symself(*innerleft);
- innerleftdest = innerleftapex;
- apex(*innerleft, innerleftapex);
- changemade = 1;
- }
- /* Make innerrightorg the "bottommost" vertex of the right hull. */
- if (counterclockwise(m, b, innerrightapex, innerrightorg, innerleftdest) >
- 0.0) {
- lnextself(*innerright);
- symself(*innerright);
- innerrightorg = innerrightapex;
- apex(*innerright, innerrightapex);
- changemade = 1;
- }
- } while (changemade);
- /* Find the two candidates to be the next "gear tooth." */
- sym(*innerleft, leftcand);
- sym(*innerright, rightcand);
- /* Create the bottom new bounding triangle. */
- maketriangle(m, b, &baseedge);
- /* Connect it to the bounding boxes of the left and right triangulations. */
- bond(baseedge, *innerleft);
- lnextself(baseedge);
- bond(baseedge, *innerright);
- lnextself(baseedge);
- setorg(baseedge, innerrightorg);
- setdest(baseedge, innerleftdest);
- /* Apex is intentionally left NULL. */
- if (b->verbose > 2) {
- printf(" Creating base bounding ");
- printtriangle(m, b, &baseedge);
- }
- /* Fix the extreme triangles if necessary. */
- org(*farleft, farleftpt);
- if (innerleftdest == farleftpt) {
- lnext(baseedge, *farleft);
- }
- dest(*farright, farrightpt);
- if (innerrightorg == farrightpt) {
- lprev(baseedge, *farright);
- }
- /* The vertices of the current knitting edge. */
- lowerleft = innerleftdest;
- lowerright = innerrightorg;
- /* The candidate vertices for knitting. */
- apex(leftcand, upperleft);
- apex(rightcand, upperright);
- /* Walk up the gap between the two triangulations, knitting them together. */
- while (1) {
- /* Have we reached the top? (This isn't quite the right question, */
- /* because even though the left triangulation might seem finished now, */
- /* moving up on the right triangulation might reveal a new vertex of */
- /* the left triangulation. And vice-versa.) */
- leftfinished = counterclockwise(m, b, upperleft, lowerleft, lowerright) <=
- 0.0;
- rightfinished = counterclockwise(m, b, upperright, lowerleft, lowerright)
- <= 0.0;
- if (leftfinished && rightfinished) {
- /* Create the top new bounding triangle. */
- maketriangle(m, b, &nextedge);
- setorg(nextedge, lowerleft);
- setdest(nextedge, lowerright);
- /* Apex is intentionally left NULL. */
- /* Connect it to the bounding boxes of the two triangulations. */
- bond(nextedge, baseedge);
- lnextself(nextedge);
- bond(nextedge, rightcand);
- lnextself(nextedge);
- bond(nextedge, leftcand);
- if (b->verbose > 2) {
- printf(" Creating top bounding ");
- printtriangle(m, b, &nextedge);
- }
- /* Special treatment for horizontal cuts. */
- if (b->dwyer && (axis == 1)) {
- org(*farleft, farleftpt);
- apex(*farleft, farleftapex);
- dest(*farright, farrightpt);
- apex(*farright, farrightapex);
- sym(*farleft, checkedge);
- apex(checkedge, checkvertex);
- /* The pointers to the extremal vertices are restored to the */
- /* leftmost and rightmost vertices (rather than topmost and */
- /* bottommost). */
- while (checkvertex[0] < farleftpt[0]) {
- lprev(checkedge, *farleft);
- farleftapex = farleftpt;
- farleftpt = checkvertex;
- sym(*farleft, checkedge);
- apex(checkedge, checkvertex);
- }
- while (farrightapex[0] > farrightpt[0]) {
- lprevself(*farright);
- symself(*farright);
- farrightpt = farrightapex;
- apex(*farright, farrightapex);
- }
- }
- return;
- }
- /* Consider eliminating edges from the left triangulation. */
- if (!leftfinished) {
- /* What vertex would be exposed if an edge were deleted? */
- lprev(leftcand, nextedge);
- symself(nextedge);
- apex(nextedge, nextapex);
- /* If nextapex is NULL, then no vertex would be exposed; the */
- /* triangulation would have been eaten right through. */
- if (nextapex != (vertex) NULL) {
- /* Check whether the edge is Delaunay. */
- badedge = incircle(m, b, lowerleft, lowerright, upperleft, nextapex) >
- 0.0;
- while (badedge) {
- /* Eliminate the edge with an edge flip. As a result, the */
- /* left triangulation will have one more boundary triangle. */
- lnextself(nextedge);
- sym(nextedge, topcasing);
- lnextself(nextedge);
- sym(nextedge, sidecasing);
- bond(nextedge, topcasing);
- bond(leftcand, sidecasing);
- lnextself(leftcand);
- sym(leftcand, outercasing);
- lprevself(nextedge);
- bond(nextedge, outercasing);
- /* Correct the vertices to reflect the edge flip. */
- setorg(leftcand, lowerleft);
- setdest(leftcand, NULL);
- setapex(leftcand, nextapex);
- setorg(nextedge, NULL);
- setdest(nextedge, upperleft);
- setapex(nextedge, nextapex);
- /* Consider the newly exposed vertex. */
- upperleft = nextapex;
- /* What vertex would be exposed if another edge were deleted? */
- otricopy(sidecasing, nextedge);
- apex(nextedge, nextapex);
- if (nextapex != (vertex) NULL) {
- /* Check whether the edge is Delaunay. */
- badedge = incircle(m, b, lowerleft, lowerright, upperleft,
- nextapex) > 0.0;
- } else {
- /* Avoid eating right through the triangulation. */
- badedge = 0;
- }
- }
- }
- }
- /* Consider eliminating edges from the right triangulation. */
- if (!rightfinished) {
- /* What vertex would be exposed if an edge were deleted? */
- lnext(rightcand, nextedge);
- symself(nextedge);
- apex(nextedge, nextapex);
- /* If nextapex is NULL, then no vertex would be exposed; the */
- /* triangulation would have been eaten right through. */
- if (nextapex != (vertex) NULL) {
- /* Check whether the edge is Delaunay. */
- badedge = incircle(m, b, lowerleft, lowerright, upperright, nextapex) >
- 0.0;
- while (badedge) {
- /* Eliminate the edge with an edge flip. As a result, the */
- /* right triangulation will have one more boundary triangle. */
- lprevself(nextedge);
- sym(nextedge, topcasing);
- lprevself(nextedge);
- sym(nextedge, sidecasing);
- bond(nextedge, topcasing);
- bond(rightcand, sidecasing);
- lprevself(rightcand);
- sym(rightcand, outercasing);
- lnextself(nextedge);
- bond(nextedge, outercasing);
- /* Correct the vertices to reflect the edge flip. */
- setorg(rightcand, NULL);
- setdest(rightcand, lowerright);
- setapex(rightcand, nextapex);
- setorg(nextedge, upperright);
- setdest(nextedge, NULL);
- setapex(nextedge, nextapex);
- /* Consider the newly exposed vertex. */
- upperright = nextapex;
- /* What vertex would be exposed if another edge were deleted? */
- otricopy(sidecasing, nextedge);
- apex(nextedge, nextapex);
- if (nextapex != (vertex) NULL) {
- /* Check whether the edge is Delaunay. */
- badedge = incircle(m, b, lowerleft, lowerright, upperright,
- nextapex) > 0.0;
- } else {
- /* Avoid eating right through the triangulation. */
- badedge = 0;
- }
- }
- }
- }
- if (leftfinished || (!rightfinished &&
- (incircle(m, b, upperleft, lowerleft, lowerright, upperright) >
- 0.0))) {
- /* Knit the triangulations, adding an edge from `lowerleft' */
- /* to `upperright'. */
- bond(baseedge, rightcand);
- lprev(rightcand, baseedge);
- setdest(baseedge, lowerleft);
- lowerright = upperright;
- sym(baseedge, rightcand);
- apex(rightcand, upperright);
- } else {
- /* Knit the triangulations, adding an edge from `upperleft' */
- /* to `lowerright'. */
- bond(baseedge, leftcand);
- lnext(leftcand, baseedge);
- setorg(baseedge, lowerright);
- lowerleft = upperleft;
- sym(baseedge, leftcand);
- apex(leftcand, upperleft);
- }
- if (b->verbose > 2) {
- printf(" Connecting ");
- printtriangle(m, b, &baseedge);
- }
- }
-}
-
-/*****************************************************************************/
-/* */
-/* divconqrecurse() Recursively form a Delaunay triangulation by the */
-/* divide-and-conquer method. */
-/* */
-/* Recursively breaks down the problem into smaller pieces, which are */
-/* knitted together by mergehulls(). The base cases (problems of two or */
-/* three vertices) are handled specially here. */
-/* */
-/* On completion, `farleft' and `farright' are bounding triangles such that */
-/* the origin of `farleft' is the leftmost vertex (breaking ties by */
-/* choosing the highest leftmost vertex), and the destination of */
-/* `farright' is the rightmost vertex (breaking ties by choosing the */
-/* lowest rightmost vertex). */
-/* */
-/*****************************************************************************/
-
-void divconqrecurse(struct mesh *m, struct behavior *b, vertex *sortarray,
- int vertices, int axis,
- struct otri *farleft, struct otri *farright)
-{
- struct otri midtri, tri1, tri2, tri3;
- struct otri innerleft, innerright;
- float area;
- int divider;
-
- if (b->verbose > 2) {
- printf(" Triangulating %d vertices.\n", vertices);
- }
- if (vertices == 2) {
- /* The triangulation of two vertices is an edge. An edge is */
- /* represented by two bounding triangles. */
- maketriangle(m, b, farleft);
- setorg(*farleft, sortarray[0]);
- setdest(*farleft, sortarray[1]);
- /* The apex is intentionally left NULL. */
- maketriangle(m, b, farright);
- setorg(*farright, sortarray[1]);
- setdest(*farright, sortarray[0]);
- /* The apex is intentionally left NULL. */
- bond(*farleft, *farright);
- lprevself(*farleft);
- lnextself(*farright);
- bond(*farleft, *farright);
- lprevself(*farleft);
- lnextself(*farright);
- bond(*farleft, *farright);
- if (b->verbose > 2) {
- printf(" Creating ");
- printtriangle(m, b, farleft);
- printf(" Creating ");
- printtriangle(m, b, farright);
- }
- /* Ensure that the origin of `farleft' is sortarray[0]. */
- lprev(*farright, *farleft);
- return;
- } else if (vertices == 3) {
- /* The triangulation of three vertices is either a triangle (with */
- /* three bounding triangles) or two edges (with four bounding */
- /* triangles). In either case, four triangles are created. */
- maketriangle(m, b, &midtri);
- maketriangle(m, b, &tri1);
- maketriangle(m, b, &tri2);
- maketriangle(m, b, &tri3);
- area = counterclockwise(m, b, sortarray[0], sortarray[1], sortarray[2]);
- if (area == 0.0) {
- /* Three collinear vertices; the triangulation is two edges. */
- setorg(midtri, sortarray[0]);
- setdest(midtri, sortarray[1]);
- setorg(tri1, sortarray[1]);
- setdest(tri1, sortarray[0]);
- setorg(tri2, sortarray[2]);
- setdest(tri2, sortarray[1]);
- setorg(tri3, sortarray[1]);
- setdest(tri3, sortarray[2]);
- /* All apices are intentionally left NULL. */
- bond(midtri, tri1);
- bond(tri2, tri3);
- lnextself(midtri);
- lprevself(tri1);
- lnextself(tri2);
- lprevself(tri3);
- bond(midtri, tri3);
- bond(tri1, tri2);
- lnextself(midtri);
- lprevself(tri1);
- lnextself(tri2);
- lprevself(tri3);
- bond(midtri, tri1);
- bond(tri2, tri3);
- /* Ensure that the origin of `farleft' is sortarray[0]. */
- otricopy(tri1, *farleft);
- /* Ensure that the destination of `farright' is sortarray[2]. */
- otricopy(tri2, *farright);
- } else {
- /* The three vertices are not collinear; the triangulation is one */
- /* triangle, namely `midtri'. */
- setorg(midtri, sortarray[0]);
- setdest(tri1, sortarray[0]);
- setorg(tri3, sortarray[0]);
- /* Apices of tri1, tri2, and tri3 are left NULL. */
- if (area > 0.0) {
- /* The vertices are in counterclockwise order. */
- setdest(midtri, sortarray[1]);
- setorg(tri1, sortarray[1]);
- setdest(tri2, sortarray[1]);
- setapex(midtri, sortarray[2]);
- setorg(tri2, sortarray[2]);
- setdest(tri3, sortarray[2]);
- } else {
- /* The vertices are in clockwise order. */
- setdest(midtri, sortarray[2]);
- setorg(tri1, sortarray[2]);
- setdest(tri2, sortarray[2]);
- setapex(midtri, sortarray[1]);
- setorg(tri2, sortarray[1]);
- setdest(tri3, sortarray[1]);
- }
- /* The topology does not depend on how the vertices are ordered. */
- bond(midtri, tri1);
- lnextself(midtri);
- bond(midtri, tri2);
- lnextself(midtri);
- bond(midtri, tri3);
- lprevself(tri1);
- lnextself(tri2);
- bond(tri1, tri2);
- lprevself(tri1);
- lprevself(tri3);
- bond(tri1, tri3);
- lnextself(tri2);
- lprevself(tri3);
- bond(tri2, tri3);
- /* Ensure that the origin of `farleft' is sortarray[0]. */
- otricopy(tri1, *farleft);
- /* Ensure that the destination of `farright' is sortarray[2]. */
- if (area > 0.0) {
- otricopy(tri2, *farright);
- } else {
- lnext(*farleft, *farright);
- }
- }
- if (b->verbose > 2) {
- printf(" Creating ");
- printtriangle(m, b, &midtri);
- printf(" Creating ");
- printtriangle(m, b, &tri1);
- printf(" Creating ");
- printtriangle(m, b, &tri2);
- printf(" Creating ");
- printtriangle(m, b, &tri3);
- }
- return;
- } else {
- /* Split the vertices in half. */
- divider = vertices >> 1;
- /* Recursively triangulate each half. */
- divconqrecurse(m, b, sortarray, divider, 1 - axis, farleft, &innerleft);
- divconqrecurse(m, b, &sortarray[divider], vertices - divider, 1 - axis,
- &innerright, farright);
- if (b->verbose > 1) {
- printf(" Joining triangulations with %d and %d vertices.\n", divider,
- vertices - divider);
- }
- /* Merge the two triangulations into one. */
- mergehulls(m, b, farleft, &innerleft, &innerright, farright, axis);
- }
-}
-
-long removeghosts(struct mesh *m, struct behavior *b, struct otri *startghost)
-{
- struct otri searchedge;
- struct otri dissolveedge;
- struct otri deadtriangle;
- vertex markorg;
- long hullsize;
- triangle ptr; /* Temporary variable used by sym(). */
-
- if (b->verbose) {
- printf(" Removing ghost triangles.\n");
- }
- /* Find an edge on the convex hull to start point location from. */
- lprev(*startghost, searchedge);
- symself(searchedge);
- m->dummytri[0] = encode(searchedge);
- /* Remove the bounding box and count the convex hull edges. */
- otricopy(*startghost, dissolveedge);
- hullsize = 0;
- do {
- hullsize++;
- lnext(dissolveedge, deadtriangle);
- lprevself(dissolveedge);
- symself(dissolveedge);
- /* If no PSLG is involved, set the boundary markers of all the vertices */
- /* on the convex hull. If a PSLG is used, this step is done later. */
- if (!b->poly) {
- /* Watch out for the case where all the input vertices are collinear. */
- if (dissolveedge.tri != m->dummytri) {
- org(dissolveedge, markorg);
- if (vertexmark(markorg) == 0) {
- setvertexmark(markorg, 1);
- }
- }
- }
- /* Remove a bounding triangle from a convex hull triangle. */
- dissolve(dissolveedge);
- /* Find the next bounding triangle. */
- sym(deadtriangle, dissolveedge);
- /* Delete the bounding triangle. */
- triangledealloc(m, deadtriangle.tri);
- } while (!otriequal(dissolveedge, *startghost));
- return hullsize;
-}
-
-/*****************************************************************************/
-/* */
-/* divconqdelaunay() Form a Delaunay triangulation by the divide-and- */
-/* conquer method. */
-/* */
-/* Sorts the vertices, calls a recursive procedure to triangulate them, and */
-/* removes the bounding box, setting boundary markers as appropriate. */
-/* */
-/*****************************************************************************/
-
-long divconqdelaunay(struct mesh *m, struct behavior *b)
-{
- vertex *sortarray;
- struct otri hullleft, hullright;
- int divider;
- int i, j;
-
- if (b->verbose) {
- printf(" Sorting vertices.\n");
- }
-
- /* Allocate an array of pointers to vertices for sorting. */
- sortarray = (vertex *) trimalloc(m->invertices * (int) sizeof(vertex));
- traversalinit(&m->vertices);
- for (i = 0; i < m->invertices; i++) {
- sortarray[i] = vertextraverse(m);
- }
- /* Sort the vertices. */
- vertexsort(sortarray, m->invertices);
- /* Discard duplicate vertices, which can really mess up the algorithm. */
- i = 0;
- for (j = 1; j < m->invertices; j++) {
- if ((sortarray[i][0] == sortarray[j][0])
- && (sortarray[i][1] == sortarray[j][1])) {
- if (!b->quiet) {
- printf(
-"Warning: A duplicate vertex at (%.12g, %.12g) appeared and was ignored.\n",
- sortarray[j][0], sortarray[j][1]);
- }
- setvertextype(sortarray[j], UNDEADVERTEX);
- m->undeads++;
- } else {
- i++;
- sortarray[i] = sortarray[j];
- }
- }
- i++;
- if (b->dwyer) {
- /* Re-sort the array of vertices to accommodate alternating cuts. */
- divider = i >> 1;
- if (i - divider >= 2) {
- if (divider >= 2) {
- alternateaxes(sortarray, divider, 1);
- }
- alternateaxes(&sortarray[divider], i - divider, 1);
- }
- }
-
- if (b->verbose) {
- printf(" Forming triangulation.\n");
- }
-
- /* Form the Delaunay triangulation. */
- divconqrecurse(m, b, sortarray, i, 0, &hullleft, &hullright);
- trifree((int *) sortarray);
-
- return removeghosts(m, b, &hullleft);
-}
-
-/** **/
-/** **/
-/********* Divide-and-conquer Delaunay triangulation ends here *********/
-
-/********* General mesh construction routines begin here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* delaunay() Form a Delaunay triangulation. */
-/* */
-/*****************************************************************************/
-
-long delaunay(struct mesh *m, struct behavior *b)
-{
- long hulledges;
-
- m->eextras = 0;
- initializetrisubpools(m, b);
-
- if (!b->quiet) {
- printf(
- "Constructing Delaunay triangulation by divide-and-conquer method.\n");
- }
- hulledges = divconqdelaunay(m, b);
-
- if (m->triangles.items == 0) {
- /* The input vertices were all collinear, so there are no triangles. */
- return 0l;
- } else {
- return hulledges;
- }
-}
-
-/** **/
-/** **/
-/********* General mesh construction routines end here *********/
-
-/********* Segment insertion begins here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* finddirection() Find the first triangle on the path from one point */
-/* to another. */
-/* */
-/* Finds the triangle that intersects a line segment drawn from the */
-/* origin of `searchtri' to the point `searchpoint', and returns the result */
-/* in `searchtri'. The origin of `searchtri' does not change, even though */
-/* the triangle returned may differ from the one passed in. This routine */
-/* is used to find the direction to move in to get from one point to */
-/* another. */
-/* */
-/* The return value notes whether the destination or apex of the found */
-/* triangle is collinear with the two points in question. */
-/* */
-/*****************************************************************************/
-
-enum finddirectionresult finddirection(struct mesh *m, struct behavior *b,
- struct otri *searchtri,
- vertex searchpoint)
-{
- struct otri checktri;
- vertex startvertex;
- vertex leftvertex, rightvertex;
- float leftccw, rightccw;
- int leftflag, rightflag;
- triangle ptr; /* Temporary variable used by onext() and oprev(). */
-
- org(*searchtri, startvertex);
- dest(*searchtri, rightvertex);
- apex(*searchtri, leftvertex);
- /* Is `searchpoint' to the left? */
- leftccw = counterclockwise(m, b, searchpoint, startvertex, leftvertex);
- leftflag = leftccw > 0.0;
- /* Is `searchpoint' to the right? */
- rightccw = counterclockwise(m, b, startvertex, searchpoint, rightvertex);
- rightflag = rightccw > 0.0;
- if (leftflag && rightflag) {
- /* `searchtri' faces directly away from `searchpoint'. We could go left */
- /* or right. Ask whether it's a triangle or a boundary on the left. */
- onext(*searchtri, checktri);
- if (checktri.tri == m->dummytri) {
- leftflag = 0;
- } else {
- rightflag = 0;
- }
- }
- while (leftflag) {
- /* Turn left until satisfied. */
- onextself(*searchtri);
- if (searchtri->tri == m->dummytri) {
- printf("Internal error in finddirection(): Unable to find a\n");
- printf(" triangle leading from (%.12g, %.12g) to", startvertex[0],
- startvertex[1]);
- printf(" (%.12g, %.12g).\n", searchpoint[0], searchpoint[1]);
- internalerror();
- }
- apex(*searchtri, leftvertex);
- rightccw = leftccw;
- leftccw = counterclockwise(m, b, searchpoint, startvertex, leftvertex);
- leftflag = leftccw > 0.0;
- }
- while (rightflag) {
- /* Turn right until satisfied. */
- oprevself(*searchtri);
- if (searchtri->tri == m->dummytri) {
- printf("Internal error in finddirection(): Unable to find a\n");
- printf(" triangle leading from (%.12g, %.12g) to", startvertex[0],
- startvertex[1]);
- printf(" (%.12g, %.12g).\n", searchpoint[0], searchpoint[1]);
- internalerror();
- }
- dest(*searchtri, rightvertex);
- leftccw = rightccw;
- rightccw = counterclockwise(m, b, startvertex, searchpoint, rightvertex);
- rightflag = rightccw > 0.0;
- }
- if (leftccw == 0.0) {
- return LEFTCOLLINEAR;
- } else if (rightccw == 0.0) {
- return RIGHTCOLLINEAR;
- } else {
- return WITHIN;
- }
-}
-
-/*****************************************************************************/
-/* */
-/* segmentintersection() Find the intersection of an existing segment */
-/* and a segment that is being inserted. Insert */
-/* a vertex at the intersection, splitting an */
-/* existing subsegment. */
-/* */
-/* The segment being inserted connects the apex of splittri to endpoint2. */
-/* splitsubseg is the subsegment being split, and MUST adjoin splittri. */
-/* Hence, endpoints of the subsegment being split are the origin and */
-/* destination of splittri. */
-/* */
-/* On completion, splittri is a handle having the newly inserted */
-/* intersection point as its origin, and endpoint1 as its destination. */
-/* */
-/*****************************************************************************/
-
-void segmentintersection(struct mesh *m, struct behavior *b,
- struct otri *splittri, struct osub *splitsubseg,
- vertex endpoint2)
-{
- struct osub opposubseg;
- vertex endpoint1;
- vertex torg, tdest;
- vertex leftvertex, rightvertex;
- vertex newvertex;
- enum insertvertexresult success;
- enum finddirectionresult collinear;
- float ex, ey;
- float tx, ty;
- float etx, ety;
- float split, denom;
- int i;
- triangle ptr; /* Temporary variable used by onext(). */
- subseg sptr; /* Temporary variable used by snext(). */
-
- /* Find the other three segment endpoints. */
- apex(*splittri, endpoint1);
- org(*splittri, torg);
- dest(*splittri, tdest);
- /* Segment intersection formulae; see the Antonio reference. */
- tx = tdest[0] - torg[0];
- ty = tdest[1] - torg[1];
- ex = endpoint2[0] - endpoint1[0];
- ey = endpoint2[1] - endpoint1[1];
- etx = torg[0] - endpoint2[0];
- ety = torg[1] - endpoint2[1];
- denom = ty * ex - tx * ey;
- if (denom == 0.0) {
- printf("Internal error in segmentintersection():");
- printf(" Attempt to find intersection of parallel segments.\n");
- internalerror();
- }
- split = (ey * etx - ex * ety) / denom;
- /* Create the new vertex. */
- newvertex = (vertex) poolalloc(&m->vertices);
- /* Interpolate its coordinate and attributes. */
- for (i = 0; i < 2 + m->nextras; i++) {
- newvertex[i] = torg[i] + split * (tdest[i] - torg[i]);
- }
- setvertexmark(newvertex, mark(*splitsubseg));
- setvertextype(newvertex, INPUTVERTEX);
- if (b->verbose > 1) {
- printf(
- " Splitting subsegment (%.12g, %.12g) (%.12g, %.12g) at (%.12g, %.12g).\n",
- torg[0], torg[1], tdest[0], tdest[1], newvertex[0], newvertex[1]);
- }
- /* Insert the intersection vertex. This should always succeed. */
- success = insertvertex(m, b, newvertex, splittri, splitsubseg, 0, 0);
- if (success != SUCCESSFULVERTEX) {
- printf("Internal error in segmentintersection():\n");
- printf(" Failure to split a segment.\n");
- internalerror();
- }
- /* Record a triangle whose origin is the new vertex. */
- setvertex2tri(newvertex, encode(*splittri));
- if (m->steinerleft > 0) {
- m->steinerleft--;
- }
-
- /* Divide the segment into two, and correct the segment endpoints. */
- ssymself(*splitsubseg);
- spivot(*splitsubseg, opposubseg);
- sdissolve(*splitsubseg);
- sdissolve(opposubseg);
- do {
- setsegorg(*splitsubseg, newvertex);
- snextself(*splitsubseg);
- } while (splitsubseg->ss != m->dummysub);
- do {
- setsegorg(opposubseg, newvertex);
- snextself(opposubseg);
- } while (opposubseg.ss != m->dummysub);
-
- /* Inserting the vertex may have caused edge flips. We wish to rediscover */
- /* the edge connecting endpoint1 to the new intersection vertex. */
- collinear = finddirection(m, b, splittri, endpoint1);
- dest(*splittri, rightvertex);
- apex(*splittri, leftvertex);
- if ((leftvertex[0] == endpoint1[0]) && (leftvertex[1] == endpoint1[1])) {
- onextself(*splittri);
- } else if ((rightvertex[0] != endpoint1[0]) ||
- (rightvertex[1] != endpoint1[1])) {
- printf("Internal error in segmentintersection():\n");
- printf(" Topological inconsistency after splitting a segment.\n");
- internalerror();
- }
- /* `splittri' should have destination endpoint1. */
-}
-
-/*****************************************************************************/
-/* */
-/* scoutsegment() Scout the first triangle on the path from one endpoint */
-/* to another, and check for completion (reaching the */
-/* second endpoint), a collinear vertex, or the */
-/* intersection of two segments. */
-/* */
-/* Returns one if the entire segment is successfully inserted, and zero if */
-/* the job must be finished by conformingedge() or constrainededge(). */
-/* */
-/* If the first triangle on the path has the second endpoint as its */
-/* destination or apex, a subsegment is inserted and the job is done. */
-/* */
-/* If the first triangle on the path has a destination or apex that lies on */
-/* the segment, a subsegment is inserted connecting the first endpoint to */
-/* the collinear vertex, and the search is continued from the collinear */
-/* vertex. */
-/* */
-/* If the first triangle on the path has a subsegment opposite its origin, */
-/* then there is a segment that intersects the segment being inserted. */
-/* Their intersection vertex is inserted, splitting the subsegment. */
-/* */
-/*****************************************************************************/
-
-int scoutsegment(struct mesh *m, struct behavior *b, struct otri *searchtri,
- vertex endpoint2, int newmark)
-{
- struct otri crosstri;
- struct osub crosssubseg;
- vertex leftvertex, rightvertex;
- enum finddirectionresult collinear;
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- collinear = finddirection(m, b, searchtri, endpoint2);
- dest(*searchtri, rightvertex);
- apex(*searchtri, leftvertex);
- if (((leftvertex[0] == endpoint2[0]) && (leftvertex[1] == endpoint2[1])) ||
- ((rightvertex[0] == endpoint2[0]) && (rightvertex[1] == endpoint2[1]))) {
- /* The segment is already an edge in the mesh. */
- if ((leftvertex[0] == endpoint2[0]) && (leftvertex[1] == endpoint2[1])) {
- lprevself(*searchtri);
- }
- /* Insert a subsegment, if there isn't already one there. */
- insertsubseg(m, b, searchtri, newmark);
- return 1;
- } else if (collinear == LEFTCOLLINEAR) {
- /* We've collided with a vertex between the segment's endpoints. */
- /* Make the collinear vertex be the triangle's origin. */
- lprevself(*searchtri);
- insertsubseg(m, b, searchtri, newmark);
- /* Insert the remainder of the segment. */
- return scoutsegment(m, b, searchtri, endpoint2, newmark);
- } else if (collinear == RIGHTCOLLINEAR) {
- /* We've collided with a vertex between the segment's endpoints. */
- insertsubseg(m, b, searchtri, newmark);
- /* Make the collinear vertex be the triangle's origin. */
- lnextself(*searchtri);
- /* Insert the remainder of the segment. */
- return scoutsegment(m, b, searchtri, endpoint2, newmark);
- } else {
- lnext(*searchtri, crosstri);
- tspivot(crosstri, crosssubseg);
- /* Check for a crossing segment. */
- if (crosssubseg.ss == m->dummysub) {
- return 0;
- } else {
- /* Insert a vertex at the intersection. */
- segmentintersection(m, b, &crosstri, &crosssubseg, endpoint2);
- otricopy(crosstri, *searchtri);
- insertsubseg(m, b, searchtri, newmark);
- /* Insert the remainder of the segment. */
- return scoutsegment(m, b, searchtri, endpoint2, newmark);
- }
- }
-}
-
-/*****************************************************************************/
-/* */
-/* delaunayfixup() Enforce the Delaunay condition at an edge, fanning out */
-/* recursively from an existing vertex. Pay special */
-/* attention to stacking inverted triangles. */
-/* */
-/* This is a support routine for inserting segments into a constrained */
-/* Delaunay triangulation. */
-/* */
-/* The origin of fixuptri is treated as if it has just been inserted, and */
-/* the local Delaunay condition needs to be enforced. It is only enforced */
-/* in one sector, however, that being the angular range defined by */
-/* fixuptri. */
-/* */
-/* This routine also needs to make decisions regarding the "stacking" of */
-/* triangles. (Read the description of constrainededge() below before */
-/* reading on here, so you understand the algorithm.) If the position of */
-/* the new vertex (the origin of fixuptri) indicates that the vertex before */
-/* it on the polygon is a reflex vertex, then "stack" the triangle by */
-/* doing nothing. (fixuptri is an inverted triangle, which is how stacked */
-/* triangles are identified.) */
-/* */
-/* Otherwise, check whether the vertex before that was a reflex vertex. */
-/* If so, perform an edge flip, thereby eliminating an inverted triangle */
-/* (popping it off the stack). The edge flip may result in the creation */
-/* of a new inverted triangle, depending on whether or not the new vertex */
-/* is visible to the vertex three edges behind on the polygon. */
-/* */
-/* If neither of the two vertices behind the new vertex are reflex */
-/* vertices, fixuptri and fartri, the triangle opposite it, are not */
-/* inverted; hence, ensure that the edge between them is locally Delaunay. */
-/* */
-/* `leftside' indicates whether or not fixuptri is to the left of the */
-/* segment being inserted. (Imagine that the segment is pointing up from */
-/* endpoint1 to endpoint2.) */
-/* */
-/*****************************************************************************/
-
-void delaunayfixup(struct mesh *m, struct behavior *b,
- struct otri *fixuptri, int leftside)
-{
- struct otri neartri;
- struct otri fartri;
- struct osub faredge;
- vertex nearvertex, leftvertex, rightvertex, farvertex;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- lnext(*fixuptri, neartri);
- sym(neartri, fartri);
- /* Check if the edge opposite the origin of fixuptri can be flipped. */
- if (fartri.tri == m->dummytri) {
- return;
- }
- tspivot(neartri, faredge);
- if (faredge.ss != m->dummysub) {
- return;
- }
- /* Find all the relevant vertices. */
- apex(neartri, nearvertex);
- org(neartri, leftvertex);
- dest(neartri, rightvertex);
- apex(fartri, farvertex);
- /* Check whether the previous polygon vertex is a reflex vertex. */
- if (leftside) {
- if (counterclockwise(m, b, nearvertex, leftvertex, farvertex) <= 0.0) {
- /* leftvertex is a reflex vertex too. Nothing can */
- /* be done until a convex section is found. */
- return;
- }
- } else {
- if (counterclockwise(m, b, farvertex, rightvertex, nearvertex) <= 0.0) {
- /* rightvertex is a reflex vertex too. Nothing can */
- /* be done until a convex section is found. */
- return;
- }
- }
- if (counterclockwise(m, b, rightvertex, leftvertex, farvertex) > 0.0) {
- /* fartri is not an inverted triangle, and farvertex is not a reflex */
- /* vertex. As there are no reflex vertices, fixuptri isn't an */
- /* inverted triangle, either. Hence, test the edge between the */
- /* triangles to ensure it is locally Delaunay. */
- if (incircle(m, b, leftvertex, farvertex, rightvertex, nearvertex) <=
- 0.0) {
- return;
- }
- /* Not locally Delaunay; go on to an edge flip. */
- } /* else fartri is inverted; remove it from the stack by flipping. */
- flip(m, b, &neartri);
- lprevself(*fixuptri); /* Restore the origin of fixuptri after the flip. */
- /* Recursively process the two triangles that result from the flip. */
- delaunayfixup(m, b, fixuptri, leftside);
- delaunayfixup(m, b, &fartri, leftside);
-}
-
-/*****************************************************************************/
-/* */
-/* constrainededge() Force a segment into a constrained Delaunay */
-/* triangulation by deleting the triangles it */
-/* intersects, and triangulating the polygons that */
-/* form on each side of it. */
-/* */
-/* Generates a single subsegment connecting `endpoint1' to `endpoint2'. */
-/* The triangle `starttri' has `endpoint1' as its origin. `newmark' is the */
-/* boundary marker of the segment. */
-/* */
-/* To insert a segment, every triangle whose interior intersects the */
-/* segment is deleted. The union of these deleted triangles is a polygon */
-/* (which is not necessarily monotone, but is close enough), which is */
-/* divided into two polygons by the new segment. This routine's task is */
-/* to generate the Delaunay triangulation of these two polygons. */
-/* */
-/* You might think of this routine's behavior as a two-step process. The */
-/* first step is to walk from endpoint1 to endpoint2, flipping each edge */
-/* encountered. This step creates a fan of edges connected to endpoint1, */
-/* including the desired edge to endpoint2. The second step enforces the */
-/* Delaunay condition on each side of the segment in an incremental manner: */
-/* proceeding along the polygon from endpoint1 to endpoint2 (this is done */
-/* independently on each side of the segment), each vertex is "enforced" */
-/* as if it had just been inserted, but affecting only the previous */
-/* vertices. The result is the same as if the vertices had been inserted */
-/* in the order they appear on the polygon, so the result is Delaunay. */
-/* */
-/* In truth, constrainededge() interleaves these two steps. The procedure */
-/* walks from endpoint1 to endpoint2, and each time an edge is encountered */
-/* and flipped, the newly exposed vertex (at the far end of the flipped */
-/* edge) is "enforced" upon the previously flipped edges, usually affecting */
-/* only one side of the polygon (depending upon which side of the segment */
-/* the vertex falls on). */
-/* */
-/* The algorithm is complicated by the need to handle polygons that are not */
-/* convex. Although the polygon is not necessarily monotone, it can be */
-/* triangulated in a manner similar to the stack-based algorithms for */
-/* monotone polygons. For each reflex vertex (local concavity) of the */
-/* polygon, there will be an inverted triangle formed by one of the edge */
-/* flips. (An inverted triangle is one with negative area - that is, its */
-/* vertices are arranged in clockwise order - and is best thought of as a */
-/* wrinkle in the fabric of the mesh.) Each inverted triangle can be */
-/* thought of as a reflex vertex pushed on the stack, waiting to be fixed */
-/* later. */
-/* */
-/* A reflex vertex is popped from the stack when a vertex is inserted that */
-/* is visible to the reflex vertex. (However, if the vertex behind the */
-/* reflex vertex is not visible to the reflex vertex, a new inverted */
-/* triangle will take its place on the stack.) These details are handled */
-/* by the delaunayfixup() routine above. */
-/* */
-/*****************************************************************************/
-
-void constrainededge(struct mesh *m, struct behavior *b,
- struct otri *starttri, vertex endpoint2, int newmark)
-{
- struct otri fixuptri, fixuptri2;
- struct osub crosssubseg;
- vertex endpoint1;
- vertex farvertex;
- float area;
- int collision;
- int done;
- triangle ptr; /* Temporary variable used by sym() and oprev(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- org(*starttri, endpoint1);
- lnext(*starttri, fixuptri);
- flip(m, b, &fixuptri);
- /* `collision' indicates whether we have found a vertex directly */
- /* between endpoint1 and endpoint2. */
- collision = 0;
- done = 0;
- do {
- org(fixuptri, farvertex);
- /* `farvertex' is the extreme point of the polygon we are "digging" */
- /* to get from endpoint1 to endpoint2. */
- if ((farvertex[0] == endpoint2[0]) && (farvertex[1] == endpoint2[1])) {
- oprev(fixuptri, fixuptri2);
- /* Enforce the Delaunay condition around endpoint2. */
- delaunayfixup(m, b, &fixuptri, 0);
- delaunayfixup(m, b, &fixuptri2, 1);
- done = 1;
- } else {
- /* Check whether farvertex is to the left or right of the segment */
- /* being inserted, to decide which edge of fixuptri to dig */
- /* through next. */
- area = counterclockwise(m, b, endpoint1, endpoint2, farvertex);
- if (area == 0.0) {
- /* We've collided with a vertex between endpoint1 and endpoint2. */
- collision = 1;
- oprev(fixuptri, fixuptri2);
- /* Enforce the Delaunay condition around farvertex. */
- delaunayfixup(m, b, &fixuptri, 0);
- delaunayfixup(m, b, &fixuptri2, 1);
- done = 1;
- } else {
- if (area > 0.0) { /* farvertex is to the left of the segment. */
- oprev(fixuptri, fixuptri2);
- /* Enforce the Delaunay condition around farvertex, on the */
- /* left side of the segment only. */
- delaunayfixup(m, b, &fixuptri2, 1);
- /* Flip the edge that crosses the segment. After the edge is */
- /* flipped, one of its endpoints is the fan vertex, and the */
- /* destination of fixuptri is the fan vertex. */
- lprevself(fixuptri);
- } else { /* farvertex is to the right of the segment. */
- delaunayfixup(m, b, &fixuptri, 0);
- /* Flip the edge that crosses the segment. After the edge is */
- /* flipped, one of its endpoints is the fan vertex, and the */
- /* destination of fixuptri is the fan vertex. */
- oprevself(fixuptri);
- }
- /* Check for two intersecting segments. */
- tspivot(fixuptri, crosssubseg);
- if (crosssubseg.ss == m->dummysub) {
- flip(m, b, &fixuptri); /* May create inverted triangle at left. */
- } else {
- /* We've collided with a segment between endpoint1 and endpoint2. */
- collision = 1;
- /* Insert a vertex at the intersection. */
- segmentintersection(m, b, &fixuptri, &crosssubseg, endpoint2);
- done = 1;
- }
- }
- }
- } while (!done);
- /* Insert a subsegment to make the segment permanent. */
- insertsubseg(m, b, &fixuptri, newmark);
- /* If there was a collision with an interceding vertex, install another */
- /* segment connecting that vertex with endpoint2. */
- if (collision) {
- /* Insert the remainder of the segment. */
- if (!scoutsegment(m, b, &fixuptri, endpoint2, newmark)) {
- constrainededge(m, b, &fixuptri, endpoint2, newmark);
- }
- }
-}
-
-/*****************************************************************************/
-/* */
-/* insertsegment() Insert a PSLG segment into a triangulation. */
-/* */
-/*****************************************************************************/
-
-void insertsegment(struct mesh *m, struct behavior *b,
- vertex endpoint1, vertex endpoint2, int newmark)
-{
- struct otri searchtri1, searchtri2;
- triangle encodedtri;
- vertex checkvertex;
- triangle ptr; /* Temporary variable used by sym(). */
-
- if (b->verbose > 1) {
- printf(" Connecting (%.12g, %.12g) to (%.12g, %.12g).\n",
- endpoint1[0], endpoint1[1], endpoint2[0], endpoint2[1]);
- }
-
- /* Find a triangle whose origin is the segment's first endpoint. */
- checkvertex = (vertex) NULL;
- encodedtri = vertex2tri(endpoint1);
- if (encodedtri != (triangle) NULL) {
- decode(encodedtri, searchtri1);
- org(searchtri1, checkvertex);
- }
- if (checkvertex != endpoint1) {
- /* Find a boundary triangle to search from. */
- searchtri1.tri = m->dummytri;
- searchtri1.orient = 0;
- symself(searchtri1);
- /* Search for the segment's first endpoint by point location. */
- if (locate(m, b, endpoint1, &searchtri1) != ONVERTEX) {
- printf(
- "Internal error in insertsegment(): Unable to locate PSLG vertex\n");
- printf(" (%.12g, %.12g) in triangulation.\n",
- endpoint1[0], endpoint1[1]);
- internalerror();
- }
- }
- /* Remember this triangle to improve subsequent point location. */
- otricopy(searchtri1, m->recenttri);
- /* Scout the beginnings of a path from the first endpoint */
- /* toward the second. */
- if (scoutsegment(m, b, &searchtri1, endpoint2, newmark)) {
- /* The segment was easily inserted. */
- return;
- }
- /* The first endpoint may have changed if a collision with an intervening */
- /* vertex on the segment occurred. */
- org(searchtri1, endpoint1);
-
- /* Find a triangle whose origin is the segment's second endpoint. */
- checkvertex = (vertex) NULL;
- encodedtri = vertex2tri(endpoint2);
- if (encodedtri != (triangle) NULL) {
- decode(encodedtri, searchtri2);
- org(searchtri2, checkvertex);
- }
- if (checkvertex != endpoint2) {
- /* Find a boundary triangle to search from. */
- searchtri2.tri = m->dummytri;
- searchtri2.orient = 0;
- symself(searchtri2);
- /* Search for the segment's second endpoint by point location. */
- if (locate(m, b, endpoint2, &searchtri2) != ONVERTEX) {
- printf(
- "Internal error in insertsegment(): Unable to locate PSLG vertex\n");
- printf(" (%.12g, %.12g) in triangulation.\n",
- endpoint2[0], endpoint2[1]);
- internalerror();
- }
- }
- /* Remember this triangle to improve subsequent point location. */
- otricopy(searchtri2, m->recenttri);
- /* Scout the beginnings of a path from the second endpoint */
- /* toward the first. */
- if (scoutsegment(m, b, &searchtri2, endpoint1, newmark)) {
- /* The segment was easily inserted. */
- return;
- }
- /* The second endpoint may have changed if a collision with an intervening */
- /* vertex on the segment occurred. */
- org(searchtri2, endpoint2);
-
- /* Insert the segment directly into the triangulation. */
- constrainededge(m, b, &searchtri1, endpoint2, newmark);
-}
-
-/*****************************************************************************/
-/* */
-/* markhull() Cover the convex hull of a triangulation with subsegments. */
-/* */
-/*****************************************************************************/
-
-void markhull(struct mesh *m, struct behavior *b)
-{
- struct otri hulltri;
- struct otri nexttri;
- struct otri starttri;
- triangle ptr; /* Temporary variable used by sym() and oprev(). */
-
- /* Find a triangle handle on the hull. */
- hulltri.tri = m->dummytri;
- hulltri.orient = 0;
- symself(hulltri);
- /* Remember where we started so we know when to stop. */
- otricopy(hulltri, starttri);
- /* Go once counterclockwise around the convex hull. */
- do {
- /* Create a subsegment if there isn't already one here. */
- insertsubseg(m, b, &hulltri, 1);
- /* To find the next hull edge, go clockwise around the next vertex. */
- lnextself(hulltri);
- oprev(hulltri, nexttri);
- while (nexttri.tri != m->dummytri) {
- otricopy(nexttri, hulltri);
- oprev(hulltri, nexttri);
- }
- } while (!otriequal(hulltri, starttri));
-}
-
-/*****************************************************************************/
-/* */
-/* formskeleton() Create the segments of a triangulation, including PSLG */
-/* segments and edges on the convex hull. */
-/* */
-/* The PSLG segments are read from a .poly file. The return value is the */
-/* number of segments in the file. */
-/* */
-/*****************************************************************************/
-
-void formskeleton(struct mesh *m, struct behavior *b, int *segmentlist,
- int *segmentmarkerlist, int numberofsegments)
-{
- char polyfilename[6];
- int index;
- vertex endpoint1, endpoint2;
- int segmentmarkers;
- int end1, end2;
- int boundmarker;
- int i;
-
- if (b->poly) {
- if (!b->quiet) {
- printf("Recovering segments in Delaunay triangulation.\n");
- }
- strcpy(polyfilename, "input");
- m->insegments = numberofsegments;
- segmentmarkers = segmentmarkerlist != (int *) NULL;
- index = 0;
- /* If the input vertices are collinear, there is no triangulation, */
- /* so don't try to insert segments. */
- if (m->triangles.items == 0) {
- return;
- }
-
- /* If segments are to be inserted, compute a mapping */
- /* from vertices to triangles. */
- if (m->insegments > 0) {
- makevertexmap(m, b);
- if (b->verbose) {
- printf(" Recovering PSLG segments.\n");
- }
- }
-
- boundmarker = 0;
- /* Read and insert the segments. */
- for (i = 0; i < m->insegments; i++) {
- end1 = segmentlist[index++];
- end2 = segmentlist[index++];
- if (segmentmarkers) {
- boundmarker = segmentmarkerlist[i];
- }
- if ((end1 < b->firstnumber) ||
- (end1 >= b->firstnumber + m->invertices)) {
- if (!b->quiet) {
- printf("Warning: Invalid first endpoint of segment %d in %s.\n",
- b->firstnumber + i, polyfilename);
- }
- } else if ((end2 < b->firstnumber) ||
- (end2 >= b->firstnumber + m->invertices)) {
- if (!b->quiet) {
- printf("Warning: Invalid second endpoint of segment %d in %s.\n",
- b->firstnumber + i, polyfilename);
- }
- } else {
- /* Find the vertices numbered `end1' and `end2'. */
- endpoint1 = getvertex(m, b, end1);
- endpoint2 = getvertex(m, b, end2);
- if ((endpoint1[0] == endpoint2[0]) && (endpoint1[1] == endpoint2[1])) {
- if (!b->quiet) {
- printf("Warning: Endpoints of segment %d are coincident in %s.\n",
- b->firstnumber + i, polyfilename);
- }
- } else {
- insertsegment(m, b, endpoint1, endpoint2, boundmarker);
- }
- }
- }
- } else {
- m->insegments = 0;
- }
- if (b->convex || !b->poly) {
- /* Enclose the convex hull with subsegments. */
- if (b->verbose) {
- printf(" Enclosing convex hull with segments.\n");
- }
- markhull(m, b);
- }
-}
-
-/** **/
-/** **/
-/********* Segment insertion ends here *********/
-
-/********* Carving out holes and concavities begins here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* infecthull() Virally infect all of the triangles of the convex hull */
-/* that are not protected by subsegments. Where there are */
-/* subsegments, set boundary markers as appropriate. */
-/* */
-/*****************************************************************************/
-
-void infecthull(struct mesh *m, struct behavior *b)
-{
- struct otri hulltri;
- struct otri nexttri;
- struct otri starttri;
- struct osub hullsubseg;
- triangle **deadtriangle;
- vertex horg, hdest;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- if (b->verbose) {
- printf(" Marking concavities (external triangles) for elimination.\n");
- }
- /* Find a triangle handle on the hull. */
- hulltri.tri = m->dummytri;
- hulltri.orient = 0;
- symself(hulltri);
- /* Remember where we started so we know when to stop. */
- otricopy(hulltri, starttri);
- /* Go once counterclockwise around the convex hull. */
- do {
- /* Ignore triangles that are already infected. */
- if (!infected(hulltri)) {
- /* Is the triangle protected by a subsegment? */
- tspivot(hulltri, hullsubseg);
- if (hullsubseg.ss == m->dummysub) {
- /* The triangle is not protected; infect it. */
- if (!infected(hulltri)) {
- infect(hulltri);
- deadtriangle = (triangle **) poolalloc(&m->viri);
- *deadtriangle = hulltri.tri;
- }
- } else {
- /* The triangle is protected; set boundary markers if appropriate. */
- if (mark(hullsubseg) == 0) {
- setmark(hullsubseg, 1);
- org(hulltri, horg);
- dest(hulltri, hdest);
- if (vertexmark(horg) == 0) {
- setvertexmark(horg, 1);
- }
- if (vertexmark(hdest) == 0) {
- setvertexmark(hdest, 1);
- }
- }
- }
- }
- /* To find the next hull edge, go clockwise around the next vertex. */
- lnextself(hulltri);
- oprev(hulltri, nexttri);
- while (nexttri.tri != m->dummytri) {
- otricopy(nexttri, hulltri);
- oprev(hulltri, nexttri);
- }
- } while (!otriequal(hulltri, starttri));
-}
-
-/*****************************************************************************/
-/* */
-/* plague() Spread the virus from all infected triangles to any neighbors */
-/* not protected by subsegments. Delete all infected triangles. */
-/* */
-/* This is the procedure that actually creates holes and concavities. */
-/* */
-/* This procedure operates in two phases. The first phase identifies all */
-/* the triangles that will die, and marks them as infected. They are */
-/* marked to ensure that each triangle is added to the virus pool only */
-/* once, so the procedure will terminate. */
-/* */
-/* The second phase actually eliminates the infected triangles. It also */
-/* eliminates orphaned vertices. */
-/* */
-/*****************************************************************************/
-
-void plague(struct mesh *m, struct behavior *b)
-{
- struct otri testtri;
- struct otri neighbor;
- triangle **virusloop;
- triangle **deadtriangle;
- struct osub neighborsubseg;
- vertex testvertex;
- vertex norg, ndest;
- vertex deadorg, deaddest, deadapex;
- int killorg;
- triangle ptr; /* Temporary variable used by sym() and onext(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- if (b->verbose) {
- printf(" Marking neighbors of marked triangles.\n");
- }
- /* Loop through all the infected triangles, spreading the virus to */
- /* their neighbors, then to their neighbors' neighbors. */
- traversalinit(&m->viri);
- virusloop = (triangle **) traverse(&m->viri);
- while (virusloop != (triangle **) NULL) {
- testtri.tri = *virusloop;
- /* A triangle is marked as infected by messing with one of its pointers */
- /* to subsegments, setting it to an illegal value. Hence, we have to */
- /* temporarily uninfect this triangle so that we can examine its */
- /* adjacent subsegments. */
- uninfect(testtri);
- if (b->verbose > 2) {
- /* Assign the triangle an orientation for convenience in */
- /* checking its vertices. */
- testtri.orient = 0;
- org(testtri, deadorg);
- dest(testtri, deaddest);
- apex(testtri, deadapex);
- printf(" Checking (%.12g, %.12g) (%.12g, %.12g) (%.12g, %.12g)\n",
- deadorg[0], deadorg[1], deaddest[0], deaddest[1],
- deadapex[0], deadapex[1]);
- }
- /* Check each of the triangle's three neighbors. */
- for (testtri.orient = 0; testtri.orient < 3; testtri.orient++) {
- /* Find the neighbor. */
- sym(testtri, neighbor);
- /* Check for a subsegment between the triangle and its neighbor. */
- tspivot(testtri, neighborsubseg);
- /* Check if the neighbor is nonexistent or already infected. */
- if ((neighbor.tri == m->dummytri) || infected(neighbor)) {
- if (neighborsubseg.ss != m->dummysub) {
- /* There is a subsegment separating the triangle from its */
- /* neighbor, but both triangles are dying, so the subsegment */
- /* dies too. */
- subsegdealloc(m, neighborsubseg.ss);
- if (neighbor.tri != m->dummytri) {
- /* Make sure the subsegment doesn't get deallocated again */
- /* later when the infected neighbor is visited. */
- uninfect(neighbor);
- tsdissolve(neighbor);
- infect(neighbor);
- }
- }
- } else { /* The neighbor exists and is not infected. */
- if (neighborsubseg.ss == m->dummysub) {
- /* There is no subsegment protecting the neighbor, so */
- /* the neighbor becomes infected. */
- if (b->verbose > 2) {
- org(neighbor, deadorg);
- dest(neighbor, deaddest);
- apex(neighbor, deadapex);
- printf(
- " Marking (%.12g, %.12g) (%.12g, %.12g) (%.12g, %.12g)\n",
- deadorg[0], deadorg[1], deaddest[0], deaddest[1],
- deadapex[0], deadapex[1]);
- }
- infect(neighbor);
- /* Ensure that the neighbor's neighbors will be infected. */
- deadtriangle = (triangle **) poolalloc(&m->viri);
- *deadtriangle = neighbor.tri;
- } else { /* The neighbor is protected by a subsegment. */
- /* Remove this triangle from the subsegment. */
- stdissolve(neighborsubseg);
- /* The subsegment becomes a boundary. Set markers accordingly. */
- if (mark(neighborsubseg) == 0) {
- setmark(neighborsubseg, 1);
- }
- org(neighbor, norg);
- dest(neighbor, ndest);
- if (vertexmark(norg) == 0) {
- setvertexmark(norg, 1);
- }
- if (vertexmark(ndest) == 0) {
- setvertexmark(ndest, 1);
- }
- }
- }
- }
- /* Remark the triangle as infected, so it doesn't get added to the */
- /* virus pool again. */
- infect(testtri);
- virusloop = (triangle **) traverse(&m->viri);
- }
-
- if (b->verbose) {
- printf(" Deleting marked triangles.\n");
- }
-
- traversalinit(&m->viri);
- virusloop = (triangle **) traverse(&m->viri);
- while (virusloop != (triangle **) NULL) {
- testtri.tri = *virusloop;
-
- /* Check each of the three corners of the triangle for elimination. */
- /* This is done by walking around each vertex, checking if it is */
- /* still connected to at least one live triangle. */
- for (testtri.orient = 0; testtri.orient < 3; testtri.orient++) {
- org(testtri, testvertex);
- /* Check if the vertex has already been tested. */
- if (testvertex != (vertex) NULL) {
- killorg = 1;
- /* Mark the corner of the triangle as having been tested. */
- setorg(testtri, NULL);
- /* Walk counterclockwise about the vertex. */
- onext(testtri, neighbor);
- /* Stop upon reaching a boundary or the starting triangle. */
- while ((neighbor.tri != m->dummytri) &&
- (!otriequal(neighbor, testtri))) {
- if (infected(neighbor)) {
- /* Mark the corner of this triangle as having been tested. */
- setorg(neighbor, NULL);
- } else {
- /* A live triangle. The vertex survives. */
- killorg = 0;
- }
- /* Walk counterclockwise about the vertex. */
- onextself(neighbor);
- }
- /* If we reached a boundary, we must walk clockwise as well. */
- if (neighbor.tri == m->dummytri) {
- /* Walk clockwise about the vertex. */
- oprev(testtri, neighbor);
- /* Stop upon reaching a boundary. */
- while (neighbor.tri != m->dummytri) {
- if (infected(neighbor)) {
- /* Mark the corner of this triangle as having been tested. */
- setorg(neighbor, NULL);
- } else {
- /* A live triangle. The vertex survives. */
- killorg = 0;
- }
- /* Walk clockwise about the vertex. */
- oprevself(neighbor);
- }
- }
- if (killorg) {
- if (b->verbose > 1) {
- printf(" Deleting vertex (%.12g, %.12g)\n",
- testvertex[0], testvertex[1]);
- }
- setvertextype(testvertex, UNDEADVERTEX);
- m->undeads++;
- }
- }
- }
-
- /* Record changes in the number of boundary edges, and disconnect */
- /* dead triangles from their neighbors. */
- for (testtri.orient = 0; testtri.orient < 3; testtri.orient++) {
- sym(testtri, neighbor);
- if (neighbor.tri == m->dummytri) {
- /* There is no neighboring triangle on this edge, so this edge */
- /* is a boundary edge. This triangle is being deleted, so this */
- /* boundary edge is deleted. */
- m->hullsize--;
- } else {
- /* Disconnect the triangle from its neighbor. */
- dissolve(neighbor);
- /* There is a neighboring triangle on this edge, so this edge */
- /* becomes a boundary edge when this triangle is deleted. */
- m->hullsize++;
- }
- }
- /* Return the dead triangle to the pool of triangles. */
- triangledealloc(m, testtri.tri);
- virusloop = (triangle **) traverse(&m->viri);
- }
- /* Empty the virus pool. */
- poolrestart(&m->viri);
-}
-
-/*****************************************************************************/
-/* */
-/* regionplague() Spread regional attributes and/or area constraints */
-/* (from a .poly file) throughout the mesh. */
-/* */
-/* This procedure operates in two phases. The first phase spreads an */
-/* attribute and/or an area constraint through a (segment-bounded) region. */
-/* The triangles are marked to ensure that each triangle is added to the */
-/* virus pool only once, so the procedure will terminate. */
-/* */
-/* The second phase uninfects all infected triangles, returning them to */
-/* normal. */
-/* */
-/*****************************************************************************/
-
-void regionplague(struct mesh *m, struct behavior *b,
- float attribute, float area)
-{
- struct otri testtri;
- struct otri neighbor;
- triangle **virusloop;
- triangle **regiontri;
- struct osub neighborsubseg;
- vertex regionorg, regiondest, regionapex;
- triangle ptr; /* Temporary variable used by sym() and onext(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- if (b->verbose > 1) {
- printf(" Marking neighbors of marked triangles.\n");
- }
- /* Loop through all the infected triangles, spreading the attribute */
- /* and/or area constraint to their neighbors, then to their neighbors' */
- /* neighbors. */
- traversalinit(&m->viri);
- virusloop = (triangle **) traverse(&m->viri);
- while (virusloop != (triangle **) NULL) {
- testtri.tri = *virusloop;
- /* A triangle is marked as infected by messing with one of its pointers */
- /* to subsegments, setting it to an illegal value. Hence, we have to */
- /* temporarily uninfect this triangle so that we can examine its */
- /* adjacent subsegments. */
- uninfect(testtri);
- if (b->regionattrib) {
- /* Set an attribute. */
- setelemattribute(testtri, m->eextras, attribute);
- }
- if (b->vararea) {
- /* Set an area constraint. */
- setareabound(testtri, area);
- }
- if (b->verbose > 2) {
- /* Assign the triangle an orientation for convenience in */
- /* checking its vertices. */
- testtri.orient = 0;
- org(testtri, regionorg);
- dest(testtri, regiondest);
- apex(testtri, regionapex);
- printf(" Checking (%.12g, %.12g) (%.12g, %.12g) (%.12g, %.12g)\n",
- regionorg[0], regionorg[1], regiondest[0], regiondest[1],
- regionapex[0], regionapex[1]);
- }
- /* Check each of the triangle's three neighbors. */
- for (testtri.orient = 0; testtri.orient < 3; testtri.orient++) {
- /* Find the neighbor. */
- sym(testtri, neighbor);
- /* Check for a subsegment between the triangle and its neighbor. */
- tspivot(testtri, neighborsubseg);
- /* Make sure the neighbor exists, is not already infected, and */
- /* isn't protected by a subsegment. */
- if ((neighbor.tri != m->dummytri) && !infected(neighbor)
- && (neighborsubseg.ss == m->dummysub)) {
- if (b->verbose > 2) {
- org(neighbor, regionorg);
- dest(neighbor, regiondest);
- apex(neighbor, regionapex);
- printf(" Marking (%.12g, %.12g) (%.12g, %.12g) (%.12g, %.12g)\n",
- regionorg[0], regionorg[1], regiondest[0], regiondest[1],
- regionapex[0], regionapex[1]);
- }
- /* Infect the neighbor. */
- infect(neighbor);
- /* Ensure that the neighbor's neighbors will be infected. */
- regiontri = (triangle **) poolalloc(&m->viri);
- *regiontri = neighbor.tri;
- }
- }
- /* Remark the triangle as infected, so it doesn't get added to the */
- /* virus pool again. */
- infect(testtri);
- virusloop = (triangle **) traverse(&m->viri);
- }
-
- /* Uninfect all triangles. */
- if (b->verbose > 1) {
- printf(" Unmarking marked triangles.\n");
- }
- traversalinit(&m->viri);
- virusloop = (triangle **) traverse(&m->viri);
- while (virusloop != (triangle **) NULL) {
- testtri.tri = *virusloop;
- uninfect(testtri);
- virusloop = (triangle **) traverse(&m->viri);
- }
- /* Empty the virus pool. */
- poolrestart(&m->viri);
-}
-
-/*****************************************************************************/
-/* */
-/* carveholes() Find the holes and infect them. Find the area */
-/* constraints and infect them. Infect the convex hull. */
-/* Spread the infection and kill triangles. Spread the */
-/* area constraints. */
-/* */
-/* This routine mainly calls other routines to carry out all these */
-/* functions. */
-/* */
-/*****************************************************************************/
-
-void carveholes(struct mesh *m, struct behavior *b, float *holelist, int holes,
- float *regionlist, int regions)
-{
- struct otri searchtri;
- struct otri triangleloop;
- struct otri *regiontris;
- triangle **holetri;
- triangle **regiontri;
- vertex searchorg, searchdest;
- enum locateresult intersect;
- int i;
- triangle ptr; /* Temporary variable used by sym(). */
-
- if (!(b->quiet || (b->noholes && b->convex))) {
- printf("Removing unwanted triangles.\n");
- if (b->verbose && (holes > 0)) {
- printf(" Marking holes for elimination.\n");
- }
- }
-
- if (regions > 0) {
- /* Allocate storage for the triangles in which region points fall. */
- regiontris = (struct otri *) trimalloc(regions *
- (int) sizeof(struct otri));
- } else {
- regiontris = (struct otri *) NULL;
- }
-
- if (((holes > 0) && !b->noholes) || !b->convex || (regions > 0)) {
- /* Initialize a pool of viri to be used for holes, concavities, */
- /* regional attributes, and/or regional area constraints. */
- poolinit(&m->viri, sizeof(triangle *), VIRUSPERBLOCK, VIRUSPERBLOCK, 0);
- }
-
- if (!b->convex) {
- /* Mark as infected any unprotected triangles on the boundary. */
- /* This is one way by which concavities are created. */
- infecthull(m, b);
- }
-
- if ((holes > 0) && !b->noholes) {
- /* Infect each triangle in which a hole lies. */
- for (i = 0; i < 2 * holes; i += 2) {
- /* Ignore holes that aren't within the bounds of the mesh. */
- if ((holelist[i] >= m->xmin) && (holelist[i] <= m->xmax)
- && (holelist[i + 1] >= m->ymin) && (holelist[i + 1] <= m->ymax)) {
- /* Start searching from some triangle on the outer boundary. */
- searchtri.tri = m->dummytri;
- searchtri.orient = 0;
- symself(searchtri);
- /* Ensure that the hole is to the left of this boundary edge; */
- /* otherwise, locate() will falsely report that the hole */
- /* falls within the starting triangle. */
- org(searchtri, searchorg);
- dest(searchtri, searchdest);
- if (counterclockwise(m, b, searchorg, searchdest, &holelist[i]) >
- 0.0) {
- /* Find a triangle that contains the hole. */
- intersect = locate(m, b, &holelist[i], &searchtri);
- if ((intersect != OUTSIDE) && (!infected(searchtri))) {
- /* Infect the triangle. This is done by marking the triangle */
- /* as infected and including the triangle in the virus pool. */
- infect(searchtri);
- holetri = (triangle **) poolalloc(&m->viri);
- *holetri = searchtri.tri;
- }
- }
- }
- }
- }
-
- /* Now, we have to find all the regions BEFORE we carve the holes, because */
- /* locate() won't work when the triangulation is no longer convex. */
- /* (Incidentally, this is the reason why regional attributes and area */
- /* constraints can't be used when refining a preexisting mesh, which */
- /* might not be convex; they can only be used with a freshly */
- /* triangulated PSLG.) */
- if (regions > 0) {
- /* Find the starting triangle for each region. */
- for (i = 0; i < regions; i++) {
- regiontris[i].tri = m->dummytri;
- /* Ignore region points that aren't within the bounds of the mesh. */
- if ((regionlist[4 * i] >= m->xmin) && (regionlist[4 * i] <= m->xmax) &&
- (regionlist[4 * i + 1] >= m->ymin) &&
- (regionlist[4 * i + 1] <= m->ymax)) {
- /* Start searching from some triangle on the outer boundary. */
- searchtri.tri = m->dummytri;
- searchtri.orient = 0;
- symself(searchtri);
- /* Ensure that the region point is to the left of this boundary */
- /* edge; otherwise, locate() will falsely report that the */
- /* region point falls within the starting triangle. */
- org(searchtri, searchorg);
- dest(searchtri, searchdest);
- if (counterclockwise(m, b, searchorg, searchdest, ®ionlist[4 * i]) >
- 0.0) {
- /* Find a triangle that contains the region point. */
- intersect = locate(m, b, ®ionlist[4 * i], &searchtri);
- if ((intersect != OUTSIDE) && (!infected(searchtri))) {
- /* Record the triangle for processing after the */
- /* holes have been carved. */
- otricopy(searchtri, regiontris[i]);
- }
- }
- }
- }
- }
-
- if (m->viri.items > 0) {
- /* Carve the holes and concavities. */
- plague(m, b);
- }
- /* The virus pool should be empty now. */
-
- if (regions > 0) {
- if (!b->quiet) {
- if (b->regionattrib) {
- if (b->vararea) {
- printf("Spreading regional attributes and area constraints.\n");
- } else {
- printf("Spreading regional attributes.\n");
- }
- } else {
- printf("Spreading regional area constraints.\n");
- }
- }
- if (b->regionattrib && !b->refine) {
- /* Assign every triangle a regional attribute of zero. */
- traversalinit(&m->triangles);
- triangleloop.orient = 0;
- triangleloop.tri = triangletraverse(m);
- while (triangleloop.tri != (triangle *) NULL) {
- setelemattribute(triangleloop, m->eextras, 0.0);
- triangleloop.tri = triangletraverse(m);
- }
- }
- for (i = 0; i < regions; i++) {
- if (regiontris[i].tri != m->dummytri) {
- /* Make sure the triangle under consideration still exists. */
- /* It may have been eaten by the virus. */
- if (!deadtri(regiontris[i].tri)) {
- /* Put one triangle in the virus pool. */
- infect(regiontris[i]);
- regiontri = (triangle **) poolalloc(&m->viri);
- *regiontri = regiontris[i].tri;
- /* Apply one region's attribute and/or area constraint. */
- regionplague(m, b, regionlist[4 * i + 2], regionlist[4 * i + 3]);
- /* The virus pool should be empty now. */
- }
- }
- }
- if (b->regionattrib && !b->refine) {
- /* Note the fact that each triangle has an additional attribute. */
- m->eextras++;
- }
- }
-
- /* Free up memory. */
- if (((holes > 0) && !b->noholes) || !b->convex || (regions > 0)) {
- pooldeinit(&m->viri);
- }
- if (regions > 0) {
- trifree((int *) regiontris);
- }
-}
-
-/** **/
-/** **/
-/********* Carving out holes and concavities ends here *********/
-
-/*****************************************************************************/
-/* */
-/* highorder() Create extra nodes for quadratic subparametric elements. */
-/* */
-/*****************************************************************************/
-
-void highorder(struct mesh *m, struct behavior *b)
-{
- struct otri triangleloop, trisym;
- struct osub checkmark;
- vertex newvertex;
- vertex torg, tdest;
- int i;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- if (!b->quiet) {
- printf("Adding vertices for second-order triangles.\n");
- }
- /* The following line ensures that dead items in the pool of nodes */
- /* cannot be allocated for the extra nodes associated with high */
- /* order elements. This ensures that the primary nodes (at the */
- /* corners of elements) will occur earlier in the output files, and */
- /* have lower indices, than the extra nodes. */
- m->vertices.deaditemstack = (int *) NULL;
-
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- /* To loop over the set of edges, loop over all triangles, and look at */
- /* the three edges of each triangle. If there isn't another triangle */
- /* adjacent to the edge, operate on the edge. If there is another */
- /* adjacent triangle, operate on the edge only if the current triangle */
- /* has a smaller pointer than its neighbor. This way, each edge is */
- /* considered only once. */
- while (triangleloop.tri != (triangle *) NULL) {
- for (triangleloop.orient = 0; triangleloop.orient < 3;
- triangleloop.orient++) {
- sym(triangleloop, trisym);
- if ((triangleloop.tri < trisym.tri) || (trisym.tri == m->dummytri)) {
- org(triangleloop, torg);
- dest(triangleloop, tdest);
- /* Create a new node in the middle of the edge. Interpolate */
- /* its attributes. */
- newvertex = (vertex) poolalloc(&m->vertices);
- for (i = 0; i < 2 + m->nextras; i++) {
- newvertex[i] = 0.5 * (torg[i] + tdest[i]);
- }
- /* Set the new node's marker to zero or one, depending on */
- /* whether it lies on a boundary. */
- setvertexmark(newvertex, trisym.tri == m->dummytri);
- setvertextype(newvertex,
- trisym.tri == m->dummytri ? FREEVERTEX : SEGMENTVERTEX);
- if (b->usesegments) {
- tspivot(triangleloop, checkmark);
- /* If this edge is a segment, transfer the marker to the new node. */
- if (checkmark.ss != m->dummysub) {
- setvertexmark(newvertex, mark(checkmark));
- setvertextype(newvertex, SEGMENTVERTEX);
- }
- }
- if (b->verbose > 1) {
- printf(" Creating (%.12g, %.12g).\n", newvertex[0], newvertex[1]);
- }
- /* Record the new node in the (one or two) adjacent elements. */
- triangleloop.tri[m->highorderindex + triangleloop.orient] =
- (triangle) newvertex;
- if (trisym.tri != m->dummytri) {
- trisym.tri[m->highorderindex + trisym.orient] = (triangle) newvertex;
- }
- }
- }
- triangleloop.tri = triangletraverse(m);
- }
-}
-
-/********* File I/O routines begin here *********/
-/** **/
-/** **/
-
-/*****************************************************************************/
-/* */
-/* transfernodes() Read the vertices from memory. */
-/* */
-/*****************************************************************************/
-
-void transfernodes(struct mesh *m, struct behavior *b, float *pointlist,
- float *pointattriblist, int *pointmarkerlist,
- int numberofpoints, int numberofpointattribs)
-{
- vertex vertexloop;
- float x, y;
- int i, j;
- int coordindex;
- int attribindex;
-
- m->invertices = numberofpoints;
- m->mesh_dim = 2;
- m->nextras = numberofpointattribs;
- m->readnodefile = 0;
- if (m->invertices < 3) {
- printf("Error: Input must have at least three input vertices.\n");
- triexit(1);
- }
- if (m->nextras == 0) {
- b->weighted = 0;
- }
-
- initializevertexpool(m, b);
-
- /* Read the vertices. */
- coordindex = 0;
- attribindex = 0;
- for (i = 0; i < m->invertices; i++) {
- vertexloop = (vertex) poolalloc(&m->vertices);
- /* Read the vertex coordinates. */
- x = vertexloop[0] = pointlist[coordindex++];
- y = vertexloop[1] = pointlist[coordindex++];
- /* Read the vertex attributes. */
- for (j = 0; j < numberofpointattribs; j++) {
- vertexloop[2 + j] = pointattriblist[attribindex++];
- }
- if (pointmarkerlist != (int *) NULL) {
- /* Read a vertex marker. */
- setvertexmark(vertexloop, pointmarkerlist[i]);
- } else {
- /* If no markers are specified, they default to zero. */
- setvertexmark(vertexloop, 0);
- }
- setvertextype(vertexloop, INPUTVERTEX);
- /* Determine the smallest and largest x and y coordinates. */
- if (i == 0) {
- m->xmin = m->xmax = x;
- m->ymin = m->ymax = y;
- } else {
- m->xmin = (x < m->xmin) ? x : m->xmin;
- m->xmax = (x > m->xmax) ? x : m->xmax;
- m->ymin = (y < m->ymin) ? y : m->ymin;
- m->ymax = (y > m->ymax) ? y : m->ymax;
- }
- }
-
- /* Nonexistent x value used as a flag to mark circle events in sweepline */
- /* Delaunay algorithm. */
- m->xminextreme = 10 * m->xmin - 9 * m->xmax;
-}
-
-/*****************************************************************************/
-/* */
-/* writenodes() Number the vertices and write them to a .node file. */
-/* */
-/* To save memory, the vertex numbers are written over the boundary markers */
-/* after the vertices are written to a file. */
-/* */
-/*****************************************************************************/
-
-void writenodes(struct mesh *m, struct behavior *b, float **pointlist,
- float **pointattriblist, int **pointmarkerlist)
-{
- float *plist;
- float *palist;
- int *pmlist;
- int coordindex;
- int attribindex;
- vertex vertexloop;
- long outvertices;
- int vertexnumber;
- int i;
-
- if (b->jettison) {
- outvertices = m->vertices.items - m->undeads;
- } else {
- outvertices = m->vertices.items;
- }
-
- if (!b->quiet) {
- printf("Writing vertices.\n");
- }
- /* Allocate memory for output vertices if necessary. */
- if (*pointlist == (float *) NULL) {
- *pointlist = (float *) trimalloc((int) (outvertices * 2 * sizeof(float)));
- }
- /* Allocate memory for output vertex attributes if necessary. */
- if ((m->nextras > 0) && (*pointattriblist == (float *) NULL)) {
- *pointattriblist = (float *) trimalloc((int) (outvertices * m->nextras *
- sizeof(float)));
- }
- /* Allocate memory for output vertex markers if necessary. */
- if (!b->nobound && (*pointmarkerlist == (int *) NULL)) {
- *pointmarkerlist = (int *) trimalloc((int) (outvertices * sizeof(int)));
- }
- plist = *pointlist;
- palist = *pointattriblist;
- pmlist = *pointmarkerlist;
- coordindex = 0;
- attribindex = 0;
- traversalinit(&m->vertices);
- vertexnumber = b->firstnumber;
- vertexloop = vertextraverse(m);
- while (vertexloop != (vertex) NULL) {
- if (!b->jettison || (vertextype(vertexloop) != UNDEADVERTEX)) {
- /* X and y coordinates. */
- plist[coordindex++] = vertexloop[0];
- plist[coordindex++] = vertexloop[1];
- /* Vertex attributes. */
- for (i = 0; i < m->nextras; i++) {
- palist[attribindex++] = vertexloop[2 + i];
- }
- if (!b->nobound) {
- /* Copy the boundary marker. */
- pmlist[vertexnumber - b->firstnumber] = vertexmark(vertexloop);
- }
- setvertexmark(vertexloop, vertexnumber);
- vertexnumber++;
- }
- vertexloop = vertextraverse(m);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* numbernodes() Number the vertices. */
-/* */
-/* Each vertex is assigned a marker equal to its number. */
-/* */
-/* Used when writenodes() is not called because no .node file is written. */
-/* */
-/*****************************************************************************/
-
-void numbernodes(struct mesh *m, struct behavior *b)
-{
- vertex vertexloop;
- int vertexnumber;
-
- traversalinit(&m->vertices);
- vertexnumber = b->firstnumber;
- vertexloop = vertextraverse(m);
- while (vertexloop != (vertex) NULL) {
- setvertexmark(vertexloop, vertexnumber);
- if (!b->jettison || (vertextype(vertexloop) != UNDEADVERTEX)) {
- vertexnumber++;
- }
- vertexloop = vertextraverse(m);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* writeelements() Write the triangles to an .ele file. */
-/* */
-/*****************************************************************************/
-
-void writeelements(struct mesh *m, struct behavior *b,
- int **trianglelist, float **triangleattriblist)
-{
- int *tlist;
- float *talist;
- int vertexindex;
- int attribindex;
- struct otri triangleloop;
- vertex p1, p2, p3;
- vertex mid1, mid2, mid3;
- long elementnumber;
- int i;
-
- if (!b->quiet) {
- printf("Writing triangles.\n");
- }
- /* Allocate memory for output triangles if necessary. */
- if (*trianglelist == (int *) NULL) {
- *trianglelist = (int *) trimalloc((int) (m->triangles.items *
- ((b->order + 1) * (b->order + 2) /
- 2) * sizeof(int)));
- }
- /* Allocate memory for output triangle attributes if necessary. */
- if ((m->eextras > 0) && (*triangleattriblist == (float *) NULL)) {
- *triangleattriblist = (float *) trimalloc((int) (m->triangles.items *
- m->eextras *
- sizeof(float)));
- }
- tlist = *trianglelist;
- talist = *triangleattriblist;
- vertexindex = 0;
- attribindex = 0;
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- triangleloop.orient = 0;
- elementnumber = b->firstnumber;
- while (triangleloop.tri != (triangle *) NULL) {
- org(triangleloop, p1);
- dest(triangleloop, p2);
- apex(triangleloop, p3);
- if (b->order == 1) {
- tlist[vertexindex++] = vertexmark(p1);
- tlist[vertexindex++] = vertexmark(p2);
- tlist[vertexindex++] = vertexmark(p3);
- } else {
- mid1 = (vertex) triangleloop.tri[m->highorderindex + 1];
- mid2 = (vertex) triangleloop.tri[m->highorderindex + 2];
- mid3 = (vertex) triangleloop.tri[m->highorderindex];
- tlist[vertexindex++] = vertexmark(p1);
- tlist[vertexindex++] = vertexmark(p2);
- tlist[vertexindex++] = vertexmark(p3);
- tlist[vertexindex++] = vertexmark(mid1);
- tlist[vertexindex++] = vertexmark(mid2);
- tlist[vertexindex++] = vertexmark(mid3);
- }
-
- for (i = 0; i < m->eextras; i++) {
- talist[attribindex++] = elemattribute(triangleloop, i);
- }
- triangleloop.tri = triangletraverse(m);
- elementnumber++;
- }
-}
-
-/*****************************************************************************/
-/* */
-/* writepoly() Write the segments and holes to a .poly file. */
-/* */
-/*****************************************************************************/
-
-void writepoly(struct mesh *m, struct behavior *b,
- int **segmentlist, int **segmentmarkerlist)
-{
- int *slist;
- int *smlist;
- int index;
- struct osub subsegloop;
- vertex endpoint1, endpoint2;
- long subsegnumber;
-
- if (!b->quiet) {
- printf("Writing segments.\n");
- }
- /* Allocate memory for output segments if necessary. */
- if (*segmentlist == (int *) NULL) {
- *segmentlist = (int *) trimalloc((int) (m->subsegs.items * 2 *
- sizeof(int)));
- }
- /* Allocate memory for output segment markers if necessary. */
- if (!b->nobound && (*segmentmarkerlist == (int *) NULL)) {
- *segmentmarkerlist = (int *) trimalloc((int) (m->subsegs.items *
- sizeof(int)));
- }
- slist = *segmentlist;
- smlist = *segmentmarkerlist;
- index = 0;
-
- traversalinit(&m->subsegs);
- subsegloop.ss = subsegtraverse(m);
- subsegloop.ssorient = 0;
- subsegnumber = b->firstnumber;
- while (subsegloop.ss != (subseg *) NULL) {
- sorg(subsegloop, endpoint1);
- sdest(subsegloop, endpoint2);
- /* Copy indices of the segment's two endpoints. */
- slist[index++] = vertexmark(endpoint1);
- slist[index++] = vertexmark(endpoint2);
- if (!b->nobound) {
- /* Copy the boundary marker. */
- smlist[subsegnumber - b->firstnumber] = mark(subsegloop);
- }
- subsegloop.ss = subsegtraverse(m);
- subsegnumber++;
- }
-}
-
-/*****************************************************************************/
-/* */
-/* writeedges() Write the edges to an .edge file. */
-/* */
-/*****************************************************************************/
-
-void writeedges(struct mesh *m, struct behavior *b,
- int **edgelist, int **edgemarkerlist)
-{
- int *elist;
- int *emlist;
- int index;
- struct otri triangleloop, trisym;
- struct osub checkmark;
- vertex p1, p2;
- long edgenumber;
- triangle ptr; /* Temporary variable used by sym(). */
- subseg sptr; /* Temporary variable used by tspivot(). */
-
- if (!b->quiet) {
- printf("Writing edges.\n");
- }
- /* Allocate memory for edges if necessary. */
- if (*edgelist == (int *) NULL) {
- *edgelist = (int *) trimalloc((int) (m->edges * 2 * sizeof(int)));
- }
- /* Allocate memory for edge markers if necessary. */
- if (!b->nobound && (*edgemarkerlist == (int *) NULL)) {
- *edgemarkerlist = (int *) trimalloc((int) (m->edges * sizeof(int)));
- }
- elist = *edgelist;
- emlist = *edgemarkerlist;
- index = 0;
-
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- edgenumber = b->firstnumber;
- /* To loop over the set of edges, loop over all triangles, and look at */
- /* the three edges of each triangle. If there isn't another triangle */
- /* adjacent to the edge, operate on the edge. If there is another */
- /* adjacent triangle, operate on the edge only if the current triangle */
- /* has a smaller pointer than its neighbor. This way, each edge is */
- /* considered only once. */
- while (triangleloop.tri != (triangle *) NULL) {
- for (triangleloop.orient = 0; triangleloop.orient < 3;
- triangleloop.orient++) {
- sym(triangleloop, trisym);
- if ((triangleloop.tri < trisym.tri) || (trisym.tri == m->dummytri)) {
- org(triangleloop, p1);
- dest(triangleloop, p2);
- elist[index++] = vertexmark(p1);
- elist[index++] = vertexmark(p2);
- if (b->nobound) {
- } else {
- /* Edge number, indices of two endpoints, and a boundary marker. */
- /* If there's no subsegment, the boundary marker is zero. */
- if (b->usesegments) {
- tspivot(triangleloop, checkmark);
- if (checkmark.ss == m->dummysub) {
- emlist[edgenumber - b->firstnumber] = 0;
- } else {
- emlist[edgenumber - b->firstnumber] = mark(checkmark);
- }
- } else {
- emlist[edgenumber - b->firstnumber] = trisym.tri == m->dummytri;
- }
- }
- edgenumber++;
- }
- }
- triangleloop.tri = triangletraverse(m);
- }
-}
-
-/*****************************************************************************/
-/* */
-/* writevoronoi() Write the Voronoi diagram to a .v.node and .v.edge */
-/* file. */
-/* */
-/* The Voronoi diagram is the geometric dual of the Delaunay triangulation. */
-/* Hence, the Voronoi vertices are listed by traversing the Delaunay */
-/* triangles, and the Voronoi edges are listed by traversing the Delaunay */
-/* edges. */
-/* */
-/* WARNING: In order to assign numbers to the Voronoi vertices, this */
-/* procedure messes up the subsegments or the extra nodes of every */
-/* element. Hence, you should call this procedure last. */
-/* */
-/*****************************************************************************/
-
-void writevoronoi(struct mesh *m, struct behavior *b, float **vpointlist,
- float **vpointattriblist, int **vpointmarkerlist,
- int **vedgelist, int **vedgemarkerlist, float **vnormlist)
-{
- float *plist;
- float *palist;
- int *elist;
- float *normlist;
- int coordindex;
- int attribindex;
- struct otri triangleloop, trisym;
- vertex torg, tdest, tapex;
- float circumcenter[2];
- float xi, eta;
- long vnodenumber, vedgenumber;
- int p1, p2;
- int i;
- triangle ptr; /* Temporary variable used by sym(). */
-
- if (!b->quiet) {
- printf("Writing Voronoi vertices.\n");
- }
- /* Allocate memory for Voronoi vertices if necessary. */
- if (*vpointlist == (float *) NULL) {
- *vpointlist = (float *) trimalloc((int) (m->triangles.items * 2 *
- sizeof(float)));
- }
- /* Allocate memory for Voronoi vertex attributes if necessary. */
- if (*vpointattriblist == (float *) NULL) {
- *vpointattriblist = (float *) trimalloc((int) (m->triangles.items *
- m->nextras * sizeof(float)));
- }
- *vpointmarkerlist = (int *) NULL;
- plist = *vpointlist;
- palist = *vpointattriblist;
- coordindex = 0;
- attribindex = 0;
-
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- triangleloop.orient = 0;
- vnodenumber = b->firstnumber;
- while (triangleloop.tri != (triangle *) NULL) {
- org(triangleloop, torg);
- dest(triangleloop, tdest);
- apex(triangleloop, tapex);
- findcircumcenter(m, b, torg, tdest, tapex, circumcenter, &xi, &eta, 0);
-
- /* X and y coordinates. */
- plist[coordindex++] = circumcenter[0];
- plist[coordindex++] = circumcenter[1];
- for (i = 2; i < 2 + m->nextras; i++) {
- /* Interpolate the vertex attributes at the circumcenter. */
- palist[attribindex++] = torg[i] + xi * (tdest[i] - torg[i])
- + eta * (tapex[i] - torg[i]);
- }
-
- * (int *) (triangleloop.tri + 6) = (int) vnodenumber;
- triangleloop.tri = triangletraverse(m);
- vnodenumber++;
- }
-
- if (!b->quiet) {
- printf("Writing Voronoi edges.\n");
- }
- /* Allocate memory for output Voronoi edges if necessary. */
- if (*vedgelist == (int *) NULL) {
- *vedgelist = (int *) trimalloc((int) (m->edges * 2 * sizeof(int)));
- }
- *vedgemarkerlist = (int *) NULL;
- /* Allocate memory for output Voronoi norms if necessary. */
- if (*vnormlist == (float *) NULL) {
- *vnormlist = (float *) trimalloc((int) (m->edges * 2 * sizeof(float)));
- }
- elist = *vedgelist;
- normlist = *vnormlist;
- coordindex = 0;
-
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- vedgenumber = b->firstnumber;
- /* To loop over the set of edges, loop over all triangles, and look at */
- /* the three edges of each triangle. If there isn't another triangle */
- /* adjacent to the edge, operate on the edge. If there is another */
- /* adjacent triangle, operate on the edge only if the current triangle */
- /* has a smaller pointer than its neighbor. This way, each edge is */
- /* considered only once. */
- while (triangleloop.tri != (triangle *) NULL) {
- for (triangleloop.orient = 0; triangleloop.orient < 3;
- triangleloop.orient++) {
- sym(triangleloop, trisym);
- if ((triangleloop.tri < trisym.tri) || (trisym.tri == m->dummytri)) {
- /* Find the number of this triangle (and Voronoi vertex). */
- p1 = * (int *) (triangleloop.tri + 6);
- if (trisym.tri == m->dummytri) {
- org(triangleloop, torg);
- dest(triangleloop, tdest);
- /* Copy an infinite ray. Index of one endpoint, and -1. */
- elist[coordindex] = p1;
- normlist[coordindex++] = tdest[1] - torg[1];
- elist[coordindex] = -1;
- normlist[coordindex++] = torg[0] - tdest[0];
- } else {
- /* Find the number of the adjacent triangle (and Voronoi vertex). */
- p2 = * (int *) (trisym.tri + 6);
- /* Finite edge. Write indices of two endpoints. */
- elist[coordindex] = p1;
- normlist[coordindex++] = 0.0;
- elist[coordindex] = p2;
- normlist[coordindex++] = 0.0;
- }
- vedgenumber++;
- }
- }
- triangleloop.tri = triangletraverse(m);
- }
-}
-
-
-void writeneighbors(struct mesh *m, struct behavior *b, int **neighborlist)
-{
- int *nlist;
- int index;
- struct otri triangleloop, trisym;
- long elementnumber;
- int neighbor1, neighbor2, neighbor3;
- triangle ptr; /* Temporary variable used by sym(). */
-
- if (!b->quiet) {
- printf("Writing neighbors.\n");
- }
- /* Allocate memory for neighbors if necessary. */
- if (*neighborlist == (int *) NULL) {
- *neighborlist = (int *) trimalloc((int) (m->triangles.items * 3 *
- sizeof(int)));
- }
- nlist = *neighborlist;
- index = 0;
-
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- triangleloop.orient = 0;
- elementnumber = b->firstnumber;
- while (triangleloop.tri != (triangle *) NULL) {
- * (int *) (triangleloop.tri + 6) = (int) elementnumber;
- triangleloop.tri = triangletraverse(m);
- elementnumber++;
- }
- * (int *) (m->dummytri + 6) = -1;
-
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- elementnumber = b->firstnumber;
- while (triangleloop.tri != (triangle *) NULL) {
- triangleloop.orient = 1;
- sym(triangleloop, trisym);
- neighbor1 = * (int *) (trisym.tri + 6);
- triangleloop.orient = 2;
- sym(triangleloop, trisym);
- neighbor2 = * (int *) (trisym.tri + 6);
- triangleloop.orient = 0;
- sym(triangleloop, trisym);
- neighbor3 = * (int *) (trisym.tri + 6);
- nlist[index++] = neighbor1;
- nlist[index++] = neighbor2;
- nlist[index++] = neighbor3;
-
- triangleloop.tri = triangletraverse(m);
- elementnumber++;
- }
-}
-
-/** **/
-/** **/
-/********* File I/O routines end here *********/
-
-/*****************************************************************************/
-/* */
-/* quality_statistics() Print statistics about the quality of the mesh. */
-/* */
-/*****************************************************************************/
-
-void quality_statistics(struct mesh *m, struct behavior *b)
-{
- struct otri triangleloop;
- vertex p[3];
- float cossquaretable[8];
- float ratiotable[16];
- float dx[3], dy[3];
- float edgelength[3];
- float dotproduct;
- float cossquare;
- float triarea;
- float shortest, longest;
- float trilongest2;
- float smallestarea, biggestarea;
- float triminaltitude2;
- float minaltitude;
- float triaspect2;
- float worstaspect;
- float smallestangle, biggestangle;
- float radconst, degconst;
- int angletable[18];
- int aspecttable[16];
- int aspectindex;
- int tendegree;
- int acutebiggest;
- int i, ii, j, k;
-
- printf("Mesh quality statistics:\n\n");
- radconst = PI / 18.0;
- degconst = 180.0 / PI;
- for (i = 0; i < 8; i++) {
- cossquaretable[i] = cos(radconst * (float) (i + 1));
- cossquaretable[i] = cossquaretable[i] * cossquaretable[i];
- }
- for (i = 0; i < 18; i++) {
- angletable[i] = 0;
- }
-
- ratiotable[0] = 1.5; ratiotable[1] = 2.0;
- ratiotable[2] = 2.5; ratiotable[3] = 3.0;
- ratiotable[4] = 4.0; ratiotable[5] = 6.0;
- ratiotable[6] = 10.0; ratiotable[7] = 15.0;
- ratiotable[8] = 25.0; ratiotable[9] = 50.0;
- ratiotable[10] = 100.0; ratiotable[11] = 300.0;
- ratiotable[12] = 1000.0; ratiotable[13] = 10000.0;
- ratiotable[14] = 100000.0; ratiotable[15] = 0.0;
- for (i = 0; i < 16; i++) {
- aspecttable[i] = 0;
- }
-
- worstaspect = 0.0;
- minaltitude = m->xmax - m->xmin + m->ymax - m->ymin;
- minaltitude = minaltitude * minaltitude;
- shortest = minaltitude;
- longest = 0.0;
- smallestarea = minaltitude;
- biggestarea = 0.0;
- worstaspect = 0.0;
- smallestangle = 0.0;
- biggestangle = 2.0;
- acutebiggest = 1;
-
- traversalinit(&m->triangles);
- triangleloop.tri = triangletraverse(m);
- triangleloop.orient = 0;
- while (triangleloop.tri != (triangle *) NULL) {
- org(triangleloop, p[0]);
- dest(triangleloop, p[1]);
- apex(triangleloop, p[2]);
- trilongest2 = 0.0;
-
- for (i = 0; i < 3; i++) {
- j = plus1mod3[i];
- k = minus1mod3[i];
- dx[i] = p[j][0] - p[k][0];
- dy[i] = p[j][1] - p[k][1];
- edgelength[i] = dx[i] * dx[i] + dy[i] * dy[i];
- if (edgelength[i] > trilongest2) {
- trilongest2 = edgelength[i];
- }
- if (edgelength[i] > longest) {
- longest = edgelength[i];
- }
- if (edgelength[i] < shortest) {
- shortest = edgelength[i];
- }
- }
-
- triarea = counterclockwise(m, b, p[0], p[1], p[2]);
- if (triarea < smallestarea) {
- smallestarea = triarea;
- }
- if (triarea > biggestarea) {
- biggestarea = triarea;
- }
- triminaltitude2 = triarea * triarea / trilongest2;
- if (triminaltitude2 < minaltitude) {
- minaltitude = triminaltitude2;
- }
- triaspect2 = trilongest2 / triminaltitude2;
- if (triaspect2 > worstaspect) {
- worstaspect = triaspect2;
- }
- aspectindex = 0;
- while ((triaspect2 > ratiotable[aspectindex] * ratiotable[aspectindex])
- && (aspectindex < 15)) {
- aspectindex++;
- }
- aspecttable[aspectindex]++;
-
- for (i = 0; i < 3; i++) {
- j = plus1mod3[i];
- k = minus1mod3[i];
- dotproduct = dx[j] * dx[k] + dy[j] * dy[k];
- cossquare = dotproduct * dotproduct / (edgelength[j] * edgelength[k]);
- tendegree = 8;
- for (ii = 7; ii >= 0; ii--) {
- if (cossquare > cossquaretable[ii]) {
- tendegree = ii;
- }
- }
- if (dotproduct <= 0.0) {
- angletable[tendegree]++;
- if (cossquare > smallestangle) {
- smallestangle = cossquare;
- }
- if (acutebiggest && (cossquare < biggestangle)) {
- biggestangle = cossquare;
- }
- } else {
- angletable[17 - tendegree]++;
- if (acutebiggest || (cossquare > biggestangle)) {
- biggestangle = cossquare;
- acutebiggest = 0;
- }
- }
- }
- triangleloop.tri = triangletraverse(m);
- }
-
- shortest = sqrt(shortest);
- longest = sqrt(longest);
- minaltitude = sqrt(minaltitude);
- worstaspect = sqrt(worstaspect);
- smallestarea *= 0.5;
- biggestarea *= 0.5;
- if (smallestangle >= 1.0) {
- smallestangle = 0.0;
- } else {
- smallestangle = degconst * acos(sqrt(smallestangle));
- }
- if (biggestangle >= 1.0) {
- biggestangle = 180.0;
- } else {
- if (acutebiggest) {
- biggestangle = degconst * acos(sqrt(biggestangle));
- } else {
- biggestangle = 180.0 - degconst * acos(sqrt(biggestangle));
- }
- }
-
- printf(" Smallest area: %16.5g | Largest area: %16.5g\n",
- smallestarea, biggestarea);
- printf(" Shortest edge: %16.5g | Longest edge: %16.5g\n",
- shortest, longest);
- printf(" Shortest altitude: %12.5g | Largest aspect ratio: %8.5g\n\n",
- minaltitude, worstaspect);
-
- printf(" Triangle aspect ratio histogram:\n");
- printf(" 1.1547 - %-6.6g : %8d | %6.6g - %-6.6g : %8d\n",
- ratiotable[0], aspecttable[0], ratiotable[7], ratiotable[8],
- aspecttable[8]);
- for (i = 1; i < 7; i++) {
- printf(" %6.6g - %-6.6g : %8d | %6.6g - %-6.6g : %8d\n",
- ratiotable[i - 1], ratiotable[i], aspecttable[i],
- ratiotable[i + 7], ratiotable[i + 8], aspecttable[i + 8]);
- }
- printf(" %6.6g - %-6.6g : %8d | %6.6g - : %8d\n",
- ratiotable[6], ratiotable[7], aspecttable[7], ratiotable[14],
- aspecttable[15]);
- printf(" (Aspect ratio is longest edge divided by shortest altitude)\n\n");
-
- printf(" Smallest angle: %15.5g | Largest angle: %15.5g\n\n",
- smallestangle, biggestangle);
-
- printf(" Angle histogram:\n");
- for (i = 0; i < 9; i++) {
- printf(" %3d - %3d degrees: %8d | %3d - %3d degrees: %8d\n",
- i * 10, i * 10 + 10, angletable[i],
- i * 10 + 90, i * 10 + 100, angletable[i + 9]);
- }
- printf("\n");
-}
-
-/*****************************************************************************/
-/* */
-/* statistics() Print all sorts of cool facts. */
-/* */
-/*****************************************************************************/
-
-void statistics(struct mesh *m, struct behavior *b)
-{
- printf("\nStatistics:\n\n");
- printf(" Input vertices: %d\n", m->invertices);
- if (b->refine) {
- printf(" Input triangles: %d\n", m->inelements);
- }
- if (b->poly) {
- printf(" Input segments: %d\n", m->insegments);
- if (!b->refine) {
- printf(" Input holes: %d\n", m->holes);
- }
- }
-
- printf("\n Mesh vertices: %ld\n", m->vertices.items - m->undeads);
- printf(" Mesh triangles: %ld\n", m->triangles.items);
- printf(" Mesh edges: %ld\n", m->edges);
- printf(" Mesh exterior boundary edges: %ld\n", m->hullsize);
- if (b->poly || b->refine) {
- printf(" Mesh interior boundary edges: %ld\n",
- m->subsegs.items - m->hullsize);
- printf(" Mesh subsegments (constrained edges): %ld\n",
- m->subsegs.items);
- }
- printf("\n");
-
- if (b->verbose) {
- quality_statistics(m, b);
- printf("Memory allocation statistics:\n\n");
- printf(" Maximum number of vertices: %ld\n", m->vertices.maxitems);
- printf(" Maximum number of triangles: %ld\n", m->triangles.maxitems);
- if (m->subsegs.maxitems > 0) {
- printf(" Maximum number of subsegments: %ld\n", m->subsegs.maxitems);
- }
- if (m->viri.maxitems > 0) {
- printf(" Maximum number of viri: %ld\n", m->viri.maxitems);
- }
- if (m->badsubsegs.maxitems > 0) {
- printf(" Maximum number of encroached subsegments: %ld\n",
- m->badsubsegs.maxitems);
- }
- if (m->badtriangles.maxitems > 0) {
- printf(" Maximum number of bad triangles: %ld\n",
- m->badtriangles.maxitems);
- }
- if (m->flipstackers.maxitems > 0) {
- printf(" Maximum number of stacked triangle flips: %ld\n",
- m->flipstackers.maxitems);
- }
- if (m->splaynodes.maxitems > 0) {
- printf(" Maximum number of splay tree nodes: %ld\n",
- m->splaynodes.maxitems);
- }
- printf(" Approximate heap memory use (bytes): %ld\n\n",
- m->vertices.maxitems * m->vertices.itembytes +
- m->triangles.maxitems * m->triangles.itembytes +
- m->subsegs.maxitems * m->subsegs.itembytes +
- m->viri.maxitems * m->viri.itembytes +
- m->badsubsegs.maxitems * m->badsubsegs.itembytes +
- m->badtriangles.maxitems * m->badtriangles.itembytes +
- m->flipstackers.maxitems * m->flipstackers.itembytes +
- m->splaynodes.maxitems * m->splaynodes.itembytes);
-
- printf("Algorithmic statistics:\n\n");
- if (!b->weighted) {
- printf(" Number of incircle tests: %ld\n", m->incirclecount);
- } else {
- printf(" Number of 3D orientation tests: %ld\n", m->orient3dcount);
- }
- printf(" Number of 2D orientation tests: %ld\n", m->counterclockcount);
- if (m->hyperbolacount > 0) {
- printf(" Number of right-of-hyperbola tests: %ld\n",
- m->hyperbolacount);
- }
- if (m->circletopcount > 0) {
- printf(" Number of circle top computations: %ld\n",
- m->circletopcount);
- }
- if (m->circumcentercount > 0) {
- printf(" Number of triangle circumcenter computations: %ld\n",
- m->circumcentercount);
- }
- printf("\n");
- }
-}
-
-/*****************************************************************************/
-/* */
-/* main() or triangulate() Gosh, do everything. */
-/* */
-/* The sequence is roughly as follows. Many of these steps can be skipped, */
-/* depending on the command line switches. */
-/* */
-/* - Initialize constants and parse the command line. */
-/* - Read the vertices from a file and either */
-/* - triangulate them (no -r), or */
-/* - read an old mesh from files and reconstruct it (-r). */
-/* - Insert the PSLG segments (-p), and possibly segments on the convex */
-/* hull (-c). */
-/* - Read the holes (-p), regional attributes (-pA), and regional area */
-/* constraints (-pa). Carve the holes and concavities, and spread the */
-/* regional attributes and area constraints. */
-/* - Enforce the constraints on minimum angle (-q) and maximum area (-a). */
-/* Also enforce the conforming Delaunay property (-q and -a). */
-/* - Compute the number of edges in the resulting mesh. */
-/* - Promote the mesh's linear triangles to higher order elements (-o). */
-/* - Write the output files and print the statistics. */
-/* - Check the consistency and Delaunay property of the mesh (-C). */
-/* */
-/*****************************************************************************/
-
-void triangulate(char *triswitches, struct triangulateio *in,
- struct triangulateio *out, struct triangulateio *vorout)
-{
- struct mesh m;
- struct behavior b;
- float *holearray; /* Array of holes. */
- float *regionarray; /* Array of regional attributes and area constraints. */
-
- triangleinit(&m);
- parsecommandline(1, &triswitches, &b);
- m.steinerleft = b.steiner;
-
- transfernodes(&m, &b, in->pointlist, in->pointattributelist,
- in->pointmarkerlist, in->numberofpoints,
- in->numberofpointattributes);
-
- m.hullsize = delaunay(&m, &b); /* Triangulate the vertices. */
- /* Ensure that no vertex can be mistaken for a triangular bounding */
- /* box vertex in insertvertex(). */
- m.infvertex1 = (vertex) NULL;
- m.infvertex2 = (vertex) NULL;
- m.infvertex3 = (vertex) NULL;
-
- if (b.usesegments) {
- m.checksegments = 1; /* Segments will be introduced next. */
- if (!b.refine) {
- /* Insert PSLG segments and/or convex hull segments. */
- formskeleton(&m, &b, in->segmentlist,
- in->segmentmarkerlist, in->numberofsegments);
- }
- }
-
- if (b.poly && (m.triangles.items > 0)) {
- holearray = in->holelist;
- m.holes = in->numberofholes;
- regionarray = in->regionlist;
- m.regions = in->numberofregions;
- if (!b.refine) {
- /* Carve out holes and concavities. */
- carveholes(&m, &b, holearray, m.holes, regionarray, m.regions);
- }
- } else {
- /* Without a PSLG, there can be no holes or regional attributes */
- /* or area constraints. The following are set to zero to avoid */
- /* an accidental free() later. */
- m.holes = 0;
- m.regions = 0;
- }
-
- /* Calculate the number of edges. */
- m.edges = (3l * m.triangles.items + m.hullsize) / 2l;
-
- if (b.order > 1) {
- highorder(&m, &b); /* Promote elements to higher polynomial order. */
- }
- if (!b.quiet) {
- printf("\n");
- }
-
- if (b.jettison) {
- out->numberofpoints = m.vertices.items - m.undeads;
- } else {
- out->numberofpoints = m.vertices.items;
- }
- out->numberofpointattributes = m.nextras;
- out->numberoftriangles = m.triangles.items;
- out->numberofcorners = (b.order + 1) * (b.order + 2) / 2;
- out->numberoftriangleattributes = m.eextras;
- out->numberofedges = m.edges;
- if (b.usesegments) {
- out->numberofsegments = m.subsegs.items;
- } else {
- out->numberofsegments = m.hullsize;
- }
- if (vorout != (struct triangulateio *) NULL) {
- vorout->numberofpoints = m.triangles.items;
- vorout->numberofpointattributes = m.nextras;
- vorout->numberofedges = m.edges;
- }
- /* If not using iteration numbers, don't write a .node file if one was */
- /* read, because the original one would be overwritten! */
- if (b.nonodewritten || (b.noiterationnum && m.readnodefile)) {
- if (!b.quiet) {
- printf("NOT writing vertices.\n");
- }
- numbernodes(&m, &b); /* We must remember to number the vertices. */
- } else {
- /* writenodes() numbers the vertices too. */
- writenodes(&m, &b, &out->pointlist, &out->pointattributelist,
- &out->pointmarkerlist);
- }
- if (b.noelewritten) {
- if (!b.quiet) {
- printf("NOT writing triangles.\n");
- }
- } else {
- writeelements(&m, &b, &out->trianglelist, &out->triangleattributelist);
- }
- /* The -c switch (convex switch) causes a PSLG to be written */
- /* even if none was read. */
- if (b.poly || b.convex) {
- /* If not using iteration numbers, don't overwrite the .poly file. */
- if (b.nopolywritten || b.noiterationnum) {
- if (!b.quiet) {
- printf("NOT writing segments.\n");
- }
- } else {
- writepoly(&m, &b, &out->segmentlist, &out->segmentmarkerlist);
- out->numberofholes = m.holes;
- out->numberofregions = m.regions;
- if (b.poly) {
- out->holelist = in->holelist;
- out->regionlist = in->regionlist;
- } else {
- out->holelist = (float *) NULL;
- out->regionlist = (float *) NULL;
- }
- }
- }
- if (b.edgesout) {
- writeedges(&m, &b, &out->edgelist, &out->edgemarkerlist);
- }
- if (b.voronoi) {
- writevoronoi(&m, &b, &vorout->pointlist, &vorout->pointattributelist,
- &vorout->pointmarkerlist, &vorout->edgelist,
- &vorout->edgemarkerlist, &vorout->normlist);
- }
- if (b.neighbors) {
- writeneighbors(&m, &b, &out->neighborlist);
- }
-
- if (!b.quiet) {
- statistics(&m, &b);
- }
-
- triangledeinit(&m, &b);
-}
diff --git a/vision/lib/elas/triangle.h b/vision/lib/elas/triangle.h
deleted file mode 100644
index 654dd5c5ff289299b1cf644ea71689470b319449..0000000000000000000000000000000000000000
--- a/vision/lib/elas/triangle.h
+++ /dev/null
@@ -1,285 +0,0 @@
-/*****************************************************************************/
-/* */
-/* (triangle.h) */
-/* */
-/* Include file for programs that call Triangle. */
-/* */
-/* Accompanies Triangle Version 1.6 */
-/* July 28, 2005 */
-/* */
-/* Copyright 1996, 2005 */
-/* Jonathan Richard Shewchuk */
-/* 2360 Woolsey #H */
-/* Berkeley, California 94705-1927 */
-/* jrs@cs.berkeley.edu */
-/* */
-/* Modified by Andreas Geiger, 2011 */
-/*****************************************************************************/
-
-/*****************************************************************************/
-/* */
-/* How to call Triangle from another program */
-/* */
-/* */
-/* If you haven't read Triangle's instructions (run "triangle -h" to read */
-/* them), you won't understand what follows. */
-/* */
-/* Triangle must be compiled into an object file (triangle.o) with the */
-/* TRILIBRARY symbol defined (generally by using the -DTRILIBRARY compiler */
-/* switch). The makefile included with Triangle will do this for you if */
-/* you run "make trilibrary". The resulting object file can be called via */
-/* the procedure triangulate(). */
-/* */
-/* If the size of the object file is important to you, you may wish to */
-/* generate a reduced version of triangle.o. The REDUCED symbol gets rid */
-/* of all features that are primarily of research interest. Specifically, */
-/* the -DREDUCED switch eliminates Triangle's -i, -F, -s, and -C switches. */
-/* The CDT_ONLY symbol gets rid of all meshing algorithms above and beyond */
-/* constrained Delaunay triangulation. Specifically, the -DCDT_ONLY switch */
-/* eliminates Triangle's -r, -q, -a, -u, -D, -Y, -S, and -s switches. */
-/* */
-/* IMPORTANT: These definitions (TRILIBRARY, REDUCED, CDT_ONLY) must be */
-/* made in the makefile or in triangle.c itself. Putting these definitions */
-/* in this file (triangle.h) will not create the desired effect. */
-/* */
-/* */
-/* The calling convention for triangulate() follows. */
-/* */
-/* void triangulate(triswitches, in, out, vorout) */
-/* char *triswitches; */
-/* struct triangulateio *in; */
-/* struct triangulateio *out; */
-/* struct triangulateio *vorout; */
-/* */
-/* `triswitches' is a string containing the command line switches you wish */
-/* to invoke. No initial dash is required. Some suggestions: */
-/* */
-/* - You'll probably find it convenient to use the `z' switch so that */
-/* points (and other items) are numbered from zero. This simplifies */
-/* indexing, because the first item of any type always starts at index */
-/* [0] of the corresponding array, whether that item's number is zero or */
-/* one. */
-/* - You'll probably want to use the `Q' (quiet) switch in your final code, */
-/* but you can take advantage of Triangle's printed output (including the */
-/* `V' switch) while debugging. */
-/* - If you are not using the `q', `a', `u', `D', `j', or `s' switches, */
-/* then the output points will be identical to the input points, except */
-/* possibly for the boundary markers. If you don't need the boundary */
-/* markers, you should use the `N' (no nodes output) switch to save */
-/* memory. (If you do need boundary markers, but need to save memory, a */
-/* good nasty trick is to set out->pointlist equal to in->pointlist */
-/* before calling triangulate(), so that Triangle overwrites the input */
-/* points with identical copies.) */
-/* - The `I' (no iteration numbers) and `g' (.off file output) switches */
-/* have no effect when Triangle is compiled with TRILIBRARY defined. */
-/* */
-/* `in', `out', and `vorout' are descriptions of the input, the output, */
-/* and the Voronoi output. If the `v' (Voronoi output) switch is not used, */
-/* `vorout' may be NULL. `in' and `out' may never be NULL. */
-/* */
-/* Certain fields of the input and output structures must be initialized, */
-/* as described below. */
-/* */
-/*****************************************************************************/
-
-/*****************************************************************************/
-/* */
-/* The `triangulateio' structure. */
-/* */
-/* Used to pass data into and out of the triangulate() procedure. */
-/* */
-/* */
-/* Arrays are used to store points, triangles, markers, and so forth. In */
-/* all cases, the first item in any array is stored starting at index [0]. */
-/* However, that item is item number `1' unless the `z' switch is used, in */
-/* which case it is item number `0'. Hence, you may find it easier to */
-/* index points (and triangles in the neighbor list) if you use the `z' */
-/* switch. Unless, of course, you're calling Triangle from a Fortran */
-/* program. */
-/* */
-/* Description of fields (except the `numberof' fields, which are obvious): */
-/* */
-/* `pointlist': An array of point coordinates. The first point's x */
-/* coordinate is at index [0] and its y coordinate at index [1], followed */
-/* by the coordinates of the remaining points. Each point occupies two */
-/* REALs. */
-/* `pointattributelist': An array of point attributes. Each point's */
-/* attributes occupy `numberofpointattributes' REALs. */
-/* `pointmarkerlist': An array of point markers; one int per point. */
-/* */
-/* `trianglelist': An array of triangle corners. The first triangle's */
-/* first corner is at index [0], followed by its other two corners in */
-/* counterclockwise order, followed by any other nodes if the triangle */
-/* represents a nonlinear element. Each triangle occupies */
-/* `numberofcorners' ints. */
-/* `triangleattributelist': An array of triangle attributes. Each */
-/* triangle's attributes occupy `numberoftriangleattributes' REALs. */
-/* `trianglearealist': An array of triangle area constraints; one REAL per */
-/* triangle. Input only. */
-/* `neighborlist': An array of triangle neighbors; three ints per */
-/* triangle. Output only. */
-/* */
-/* `segmentlist': An array of segment endpoints. The first segment's */
-/* endpoints are at indices [0] and [1], followed by the remaining */
-/* segments. Two ints per segment. */
-/* `segmentmarkerlist': An array of segment markers; one int per segment. */
-/* */
-/* `holelist': An array of holes. The first hole's x and y coordinates */
-/* are at indices [0] and [1], followed by the remaining holes. Two */
-/* REALs per hole. Input only, although the pointer is copied to the */
-/* output structure for your convenience. */
-/* */
-/* `regionlist': An array of regional attributes and area constraints. */
-/* The first constraint's x and y coordinates are at indices [0] and [1], */
-/* followed by the regional attribute at index [2], followed by the */
-/* maximum area at index [3], followed by the remaining area constraints. */
-/* Four REALs per area constraint. Note that each regional attribute is */
-/* used only if you select the `A' switch, and each area constraint is */
-/* used only if you select the `a' switch (with no number following), but */
-/* omitting one of these switches does not change the memory layout. */
-/* Input only, although the pointer is copied to the output structure for */
-/* your convenience. */
-/* */
-/* `edgelist': An array of edge endpoints. The first edge's endpoints are */
-/* at indices [0] and [1], followed by the remaining edges. Two ints per */
-/* edge. Output only. */
-/* `edgemarkerlist': An array of edge markers; one int per edge. Output */
-/* only. */
-/* `normlist': An array of normal vectors, used for infinite rays in */
-/* Voronoi diagrams. The first normal vector's x and y magnitudes are */
-/* at indices [0] and [1], followed by the remaining vectors. For each */
-/* finite edge in a Voronoi diagram, the normal vector written is the */
-/* zero vector. Two REALs per edge. Output only. */
-/* */
-/* */
-/* Any input fields that Triangle will examine must be initialized. */
-/* Furthermore, for each output array that Triangle will write to, you */
-/* must either provide space by setting the appropriate pointer to point */
-/* to the space you want the data written to, or you must initialize the */
-/* pointer to NULL, which tells Triangle to allocate space for the results. */
-/* The latter option is preferable, because Triangle always knows exactly */
-/* how much space to allocate. The former option is provided mainly for */
-/* people who need to call Triangle from Fortran code, though it also makes */
-/* possible some nasty space-saving tricks, like writing the output to the */
-/* same arrays as the input. */
-/* */
-/* Triangle will not free() any input or output arrays, including those it */
-/* allocates itself; that's up to you. You should free arrays allocated by */
-/* Triangle by calling the trifree() procedure defined below. (By default, */
-/* trifree() just calls the standard free() library procedure, but */
-/* applications that call triangulate() may replace trimalloc() and */
-/* trifree() in triangle.c to use specialized memory allocators.) */
-/* */
-/* Here's a guide to help you decide which fields you must initialize */
-/* before you call triangulate(). */
-/* */
-/* `in': */
-/* */
-/* - `pointlist' must always point to a list of points; `numberofpoints' */
-/* and `numberofpointattributes' must be properly set. */
-/* `pointmarkerlist' must either be set to NULL (in which case all */
-/* markers default to zero), or must point to a list of markers. If */
-/* `numberofpointattributes' is not zero, `pointattributelist' must */
-/* point to a list of point attributes. */
-/* - If the `r' switch is used, `trianglelist' must point to a list of */
-/* triangles, and `numberoftriangles', `numberofcorners', and */
-/* `numberoftriangleattributes' must be properly set. If */
-/* `numberoftriangleattributes' is not zero, `triangleattributelist' */
-/* must point to a list of triangle attributes. If the `a' switch is */
-/* used (with no number following), `trianglearealist' must point to a */
-/* list of triangle area constraints. `neighborlist' may be ignored. */
-/* - If the `p' switch is used, `segmentlist' must point to a list of */
-/* segments, `numberofsegments' must be properly set, and */
-/* `segmentmarkerlist' must either be set to NULL (in which case all */
-/* markers default to zero), or must point to a list of markers. */
-/* - If the `p' switch is used without the `r' switch, then */
-/* `numberofholes' and `numberofregions' must be properly set. If */
-/* `numberofholes' is not zero, `holelist' must point to a list of */
-/* holes. If `numberofregions' is not zero, `regionlist' must point to */
-/* a list of region constraints. */
-/* - If the `p' switch is used, `holelist', `numberofholes', */
-/* `regionlist', and `numberofregions' is copied to `out'. (You can */
-/* nonetheless get away with not initializing them if the `r' switch is */
-/* used.) */
-/* - `edgelist', `edgemarkerlist', `normlist', and `numberofedges' may be */
-/* ignored. */
-/* */
-/* `out': */
-/* */
-/* - `pointlist' must be initialized (NULL or pointing to memory) unless */
-/* the `N' switch is used. `pointmarkerlist' must be initialized */
-/* unless the `N' or `B' switch is used. If `N' is not used and */
-/* `in->numberofpointattributes' is not zero, `pointattributelist' must */
-/* be initialized. */
-/* - `trianglelist' must be initialized unless the `E' switch is used. */
-/* `neighborlist' must be initialized if the `n' switch is used. If */
-/* the `E' switch is not used and (`in->numberofelementattributes' is */
-/* not zero or the `A' switch is used), `elementattributelist' must be */
-/* initialized. `trianglearealist' may be ignored. */
-/* - `segmentlist' must be initialized if the `p' or `c' switch is used, */
-/* and the `P' switch is not used. `segmentmarkerlist' must also be */
-/* initialized under these circumstances unless the `B' switch is used. */
-/* - `edgelist' must be initialized if the `e' switch is used. */
-/* `edgemarkerlist' must be initialized if the `e' switch is used and */
-/* the `B' switch is not. */
-/* - `holelist', `regionlist', `normlist', and all scalars may be ignored.*/
-/* */
-/* `vorout' (only needed if `v' switch is used): */
-/* */
-/* - `pointlist' must be initialized. If `in->numberofpointattributes' */
-/* is not zero, `pointattributelist' must be initialized. */
-/* `pointmarkerlist' may be ignored. */
-/* - `edgelist' and `normlist' must both be initialized. */
-/* `edgemarkerlist' may be ignored. */
-/* - Everything else may be ignored. */
-/* */
-/* After a call to triangulate(), the valid fields of `out' and `vorout' */
-/* will depend, in an obvious way, on the choice of switches used. Note */
-/* that when the `p' switch is used, the pointers `holelist' and */
-/* `regionlist' are copied from `in' to `out', but no new space is */
-/* allocated; be careful that you don't free() the same array twice. On */
-/* the other hand, Triangle will never copy the `pointlist' pointer (or any */
-/* others); new space is allocated for `out->pointlist', or if the `N' */
-/* switch is used, `out->pointlist' remains uninitialized. */
-/* */
-/* All of the meaningful `numberof' fields will be properly set; for */
-/* instance, `numberofedges' will represent the number of edges in the */
-/* triangulation whether or not the edges were written. If segments are */
-/* not used, `numberofsegments' will indicate the number of boundary edges. */
-/* */
-/*****************************************************************************/
-
-struct triangulateio {
- float *pointlist; /* In / out */
- float *pointattributelist; /* In / out */
- int *pointmarkerlist; /* In / out */
- int numberofpoints; /* In / out */
- int numberofpointattributes; /* In / out */
-
- int *trianglelist; /* In / out */
- float *triangleattributelist; /* In / out */
- float *trianglearealist; /* In only */
- int *neighborlist; /* Out only */
- int numberoftriangles; /* In / out */
- int numberofcorners; /* In / out */
- int numberoftriangleattributes; /* In / out */
-
- int *segmentlist; /* In / out */
- int *segmentmarkerlist; /* In / out */
- int numberofsegments; /* In / out */
-
- float *holelist; /* In / pointer to array copied out */
- int numberofholes; /* In / copied out */
-
- float *regionlist; /* In / pointer to array copied out */
- int numberofregions; /* In / copied out */
-
- int *edgelist; /* Out only */
- int *edgemarkerlist; /* Not used with Voronoi diagram; out only */
- float *normlist; /* Used only with Voronoi diagram; out only */
- int numberofedges; /* Out only */
-};
-
-void triangulate(char *,triangulateio *,triangulateio *,triangulateio *);
-void trifree(int *memptr);
-
diff --git a/vision/src/calibrate.cpp b/vision/src/calibrate.cpp
deleted file mode 100644
index e5025a9d85a0c2470e504e11e7a8ffaa146914e7..0000000000000000000000000000000000000000
--- a/vision/src/calibrate.cpp
+++ /dev/null
@@ -1,614 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#include <glog/logging.h>
-#include <ftl/config.h>
-#include <ftl/configuration.hpp>
-
-#include <iostream>
-#include <sstream>
-#include <string>
-#include <ctime>
-#include <cstdio>
-
-#include <ftl/calibrate.hpp>
-
-#include <opencv2/core.hpp>
-#include <opencv2/core/utility.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/imgcodecs.hpp>
-#include <opencv2/videoio.hpp>
-#include <opencv2/highgui.hpp>
-
-using ftl::Calibrate;
-using cv::FileStorage;
-using cv::CALIB_FIX_PRINCIPAL_POINT;
-using cv::CALIB_ZERO_TANGENT_DIST;
-using cv::CALIB_FIX_ASPECT_RATIO;
-using cv::CALIB_FIX_K1;
-using cv::CALIB_FIX_K2;
-using cv::CALIB_FIX_K3;
-using cv::CALIB_FIX_K4;
-using cv::CALIB_FIX_K5;
-using cv::CALIB_ZERO_DISPARITY;
-using cv::CALIB_USE_INTRINSIC_GUESS;
-using cv::CALIB_SAME_FOCAL_LENGTH;
-using cv::CALIB_RATIONAL_MODEL;
-using cv::CALIB_CB_ADAPTIVE_THRESH;
-using cv::CALIB_CB_NORMALIZE_IMAGE;
-using cv::CALIB_CB_FAST_CHECK;
-using cv::CALIB_USE_LU;
-using cv::NORM_L2;
-using cv::INTER_LINEAR;
-using cv::COLOR_BGR2GRAY;
-using cv::FileNode;
-using cv::FileNodeIterator;
-using cv::Mat;
-using cv::Size;
-using cv::Point2f;
-using cv::Point3f;
-using cv::Matx33d;
-using cv::Scalar;
-using cv::Rect;
-using cv::TermCriteria;
-using cv::waitKey;
-using std::string;
-using std::vector;
-
-void Calibrate::Settings::write(FileStorage& fs) const {
- fs << "{"
- << "BoardSize_Width" << boardSize.width
- << "BoardSize_Height" << boardSize.height
- << "Square_Size" << squareSize
- << "Calibrate_Pattern" << patternToUse
- << "Calibrate_NrOfFrameToUse" << nrFrames
- << "Calibrate_FixAspectRatio" << aspectRatio
- << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
- << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint
- << "Write_DetectedFeaturePoints" << writePoints
- << "Write_extrinsicParameters" << writeExtrinsics
- << "Write_gridPoints" << writeGrid
- << "Input_FlipAroundHorizontalAxis" << flipVertical
- << "Input_Delay" << delay
- << "}";
-}
-
-void Calibrate::Settings::read(const nlohmann::json& node) {
- boardSize.width = node["board_size"][0];
- boardSize.height = node["board_size"][1];
- squareSize = node["square_size"];
- nrFrames = node["num_frames"];
- aspectRatio = node["fix_aspect_ratio"];
- calibZeroTangentDist = node["assume_zero_tangential_distortion"];
- calibFixPrincipalPoint = node["fix_principal_point_at_center"];
- useFisheye = node["use_fisheye_model"];
- flipVertical = node["flip_vertical"];
- delay = node["frame_delay"];
- fixK1 = node["fix_k1"];
- fixK2 = node["fix_k2"];
- fixK3 = node["fix_k3"];
- fixK4 = node["fix_k4"];
- fixK5 = node["fix_k5"];
-
- validate();
-}
-
-void Calibrate::Settings::validate() {
- goodInput = true;
- if (boardSize.width <= 0 || boardSize.height <= 0) {
- LOG(ERROR) << "Invalid Board size: " << boardSize.width << " " <<
- boardSize.height;
- goodInput = false;
- }
-
- if (squareSize <= 10e-6) {
- LOG(ERROR) << "Invalid square size " << squareSize;
- goodInput = false;
- }
-
- if (nrFrames <= 0) {
- LOG(ERROR) << "Invalid number of frames " << nrFrames;
- goodInput = false;
- }
-
- flag = 0;
- if (calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
- if (calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST;
- if (aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO;
- if (fixK1) flag |= CALIB_FIX_K1;
- if (fixK2) flag |= CALIB_FIX_K2;
- if (fixK3) flag |= CALIB_FIX_K3;
- if (fixK4) flag |= CALIB_FIX_K4;
- if (fixK5) flag |= CALIB_FIX_K5;
-
- if (useFisheye) {
- // the fisheye model has its own enum, so overwrite the flags
- flag = cv::fisheye::CALIB_FIX_SKEW |
- cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
- if (fixK1) flag |= cv::fisheye::CALIB_FIX_K1;
- if (fixK2) flag |= cv::fisheye::CALIB_FIX_K2;
- if (fixK3) flag |= cv::fisheye::CALIB_FIX_K3;
- if (fixK4) flag |= cv::fisheye::CALIB_FIX_K4;
- if (calibFixPrincipalPoint)
- flag |= cv::fisheye::CALIB_FIX_PRINCIPAL_POINT;
- }
-
- atImageList = 0;
-}
-
-Mat Calibrate::_nextImage(size_t cam) {
- Mat result;
- if (cam == 0) {
- local_->left(result);
- } else if (cam == 1 && local_->isStereo()) {
- local_->right(result);
- }
- return result;
-}
-
-bool Calibrate::Settings::readStringList(const string& filename,
- vector<string>& l) {
- l.clear();
- FileStorage fs(filename, FileStorage::READ);
- if ( !fs.isOpened() )
- return false;
- FileNode n = fs.getFirstTopLevelNode();
- if ( n.type() != FileNode::SEQ )
- return false;
- FileNodeIterator it = n.begin(), it_end = n.end();
- for ( ; it != it_end; ++it )
- l.push_back((string)*it);
- return true;
-}
-
-bool Calibrate::Settings::isListOfImages(const string& filename) {
- string s(filename);
- // Look for file extension
- if ( s.find(".xml") == string::npos && s.find(".yaml") == string::npos &&
- s.find(".yml") == string::npos )
- return false;
- else
- return true;
-}
-
-enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
-
-bool runCalibration(const Calibrate::Settings& s, Size imageSize,
- Mat& cameraMatrix, Mat& distCoeffs,
- vector<vector<Point2f> > imagePoints, float grid_width,
- bool release_object);
-
-
-Calibrate::Calibrate(ftl::LocalSource *s, nlohmann::json &config) : local_(s) {
- /*FileStorage fs(cal, FileStorage::READ); // Read the settings
- if (!fs.isOpened())
- {
- LOG(ERROR) << "Could not open the configuration file: \"" << cal << "\"";
- return;
- }*/
- // fs["Settings"] >> settings_;
- settings_.read(config);
- // fs.release();
-
- if (!settings_.goodInput) {
- LOG(ERROR) << "Invalid input detected. Application stopping.";
- return;
- }
-
- map1_.resize(2);
- map2_.resize(2);
-
- calibrated_ = _loadCalibration();
-
- if (calibrated_) {
- LOG(INFO) << "Calibration loaded from file";
- }
-}
-
-bool Calibrate::_loadCalibration() {
- // Capture one frame to get size;
- Mat l, r;
- local_->get(l, r);
- Size img_size = l.size();
- float scale = 1.0f;
-
- Rect roi1, roi2;
- FileStorage fs;
-
- // reading intrinsic parameters
- auto ifile = ftl::locateFile("intrinsics.yml");
- if (ifile) {
- fs.open((*ifile).c_str(), FileStorage::READ);
- if (!fs.isOpened()) {
- LOG(WARNING) << "Could not open intrinsics file";
- return false;
- }
-
- LOG(INFO) << "Intrinsics from: " << *ifile;
- } else {
- LOG(WARNING) << "Calibration intrinsics file not found";
- return false;
- }
-
- Mat M1, D1, M2, D2;
- fs["M1"] >> M1;
- fs["D1"] >> D1;
- fs["M2"] >> M2;
- fs["D2"] >> D2;
-
- M1 *= scale;
- M2 *= scale;
-
- auto efile = ftl::locateFile("extrinsics.yml");
- if (efile) {
- fs.open((*efile).c_str(), FileStorage::READ);
- if (!fs.isOpened()) {
- LOG(WARNING) << "Could not open extrinsics file";
- return false;
- }
-
- LOG(INFO) << "Extrinsics from: " << *efile;
- } else {
- LOG(WARNING) << "Calibration extrinsics file not found";
- return false;
- }
-
- Mat R, T, R1, P1, R2, P2;
- fs["R"] >> R;
- fs["T"] >> T;
- fs["R1"] >> R1;
- fs["R2"] >> R2;
- fs["P1"] >> P1;
- fs["P2"] >> P2;
- fs["Q"] >> Q_;
-
- stereoRectify(M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q_,
- CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2);
-
- Mat map11, map12, map21, map22;
- initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2,
- map1_[0], map2_[0]);
- initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2,
- map1_[1], map2_[1]);
- return true;
-}
-
-bool Calibrate::recalibrate() {
- vector<vector<Point2f>> imagePoints[2];
- Mat cameraMatrix[2], distCoeffs[2];
- Size imageSize[2];
-
- bool r = _recalibrate(imagePoints, cameraMatrix, distCoeffs, imageSize);
-
- if (r) calibrated_ = true;
-
- if (r && local_->isStereo()) {
- int nimages = static_cast<int>(imagePoints[0].size());
- auto squareSize = settings_.squareSize;
- vector<vector<Point3f>> objectPoints;
- objectPoints.resize(nimages);
-
- for (auto i = 0; i < nimages; i++) {
- for (auto j = 0; j < settings_.boardSize.height; j++)
- for (auto k = 0; k < settings_.boardSize.width; k++)
- objectPoints[i].push_back(Point3f(k*squareSize,
- j*squareSize, 0));
- }
-
- Mat R, T, E, F;
-
- LOG(INFO) << "Running stereo calibration...";
-
- double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
- cameraMatrix[0], distCoeffs[0],
- cameraMatrix[1], distCoeffs[1],
- imageSize[0], R, T, E, F,
- CALIB_FIX_ASPECT_RATIO +
- CALIB_ZERO_TANGENT_DIST +
- CALIB_USE_INTRINSIC_GUESS +
- CALIB_SAME_FOCAL_LENGTH +
- CALIB_RATIONAL_MODEL +
- CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
- TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5));
- LOG(INFO) << "... done with RMS error=" << rms;
-
- // save intrinsic parameters
- FileStorage fs(FTL_LOCAL_CONFIG_ROOT "/intrinsics.yml", FileStorage::WRITE);
- if (fs.isOpened()) {
- fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
- "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
- fs.release();
- } else {
- LOG(ERROR) << "Error: can not save the intrinsic parameters";
- }
-
- Mat R1, R2, P1, P2;
- Rect validRoi[2];
-
- stereoRectify(cameraMatrix[0], distCoeffs[0],
- cameraMatrix[1], distCoeffs[1],
- imageSize[0], R, T, R1, R2, P1, P2, Q_,
- CALIB_ZERO_DISPARITY, 1, imageSize[0],
- &validRoi[0], &validRoi[1]);
-
- fs.open(FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml", FileStorage::WRITE);
- if (fs.isOpened()) {
- fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" <<
- P1 << "P2" << P2 << "Q" << Q_;
- fs.release();
- } else {
- LOG(ERROR) << "Error: can not save the extrinsic parameters";
- }
-
- // Precompute maps for cv::remap()
- initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1,
- imageSize[0], CV_16SC2, map1_[0], map2_[0]);
- initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2,
- imageSize[0], CV_16SC2, map1_[1], map2_[1]);
-
- } else {
- int cam = 0;
- if (settings_.useFisheye) {
- Mat newCamMat;
- cv::fisheye::estimateNewCameraMatrixForUndistortRectify(
- cameraMatrix[cam], distCoeffs[cam], imageSize[cam],
- Matx33d::eye(), newCamMat, 1);
- cv::fisheye::initUndistortRectifyMap(
- cameraMatrix[cam], distCoeffs[cam], Matx33d::eye(),
- newCamMat, imageSize[cam],
- CV_16SC2, map1_[cam], map2_[cam]);
- } else {
- initUndistortRectifyMap(
- cameraMatrix[cam], distCoeffs[cam], Mat(),
- getOptimalNewCameraMatrix(cameraMatrix[cam], distCoeffs[cam],
- imageSize[cam], 1, imageSize[cam], 0),
- imageSize[cam], CV_16SC2, map1_[cam], map2_[cam]);
- }
- }
-
- return r;
-}
-
-bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
- Mat *cameraMatrix, Mat *distCoeffs, Size *imageSize) {
- int winSize = 11; // parser.get<int>("winSize");
-
- float grid_width = settings_.squareSize * (settings_.boardSize.width - 1);
- bool release_object = false;
- double prevTimestamp = 0.0;
- const Scalar RED(0, 0, 255), GREEN(0, 255, 0);
-
- int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
-
- if (!settings_.useFisheye) {
- // fast check erroneously fails with high distortions like fisheye
- chessBoardFlags |= CALIB_CB_FAST_CHECK;
- }
-
- for (;;) {
- Mat view[2];
- local_->get(view[0], view[1]);
- LOG(INFO) << "Grabbing calibration image...";
-
- if (view[0].empty() || (local_->isStereo() && view[1].empty()) ||
- imagePoints[0].size() >= (size_t)settings_.nrFrames) {
- settings_.outputFileName = FTL_LOCAL_CONFIG_ROOT "/cam0.xml";
- bool r = runCalibration(settings_, imageSize[0],
- cameraMatrix[0], distCoeffs[0], imagePoints[0],
- grid_width, release_object);
-
- if (local_->isStereo()) {
- settings_.outputFileName = FTL_LOCAL_CONFIG_ROOT "/cam1.xml";
- r &= runCalibration(settings_, imageSize[1],
- cameraMatrix[1], distCoeffs[1], imagePoints[1],
- grid_width, release_object);
- }
-
- if (!r && view[0].empty()) {
- LOG(ERROR) << "Not enough frames to calibrate";
- return false;
- } else if (r) {
- LOG(INFO) << "Calibration successful";
- break;
- }
- }
-
- imageSize[0] = view[0].size(); // Format input image.
- imageSize[1] = view[1].size();
- // if( settings_.flipVertical ) flip( view[cam], view[cam], 0 );
-
- vector<Point2f> pointBuf[2];
- bool found1, found2;
- found1 = findChessboardCorners(view[0], settings_.boardSize,
- pointBuf[0], chessBoardFlags);
- found2 = !local_->isStereo() || findChessboardCorners(view[1],
- settings_.boardSize, pointBuf[1], chessBoardFlags);
-
- if (found1 && found2 &&
- local_->getTimestamp()-prevTimestamp > settings_.delay) {
- prevTimestamp = local_->getTimestamp();
- // improve the found corners' coordinate accuracy for chessboard
- Mat viewGray;
- cvtColor(view[0], viewGray, COLOR_BGR2GRAY);
- cornerSubPix(viewGray, pointBuf[0], Size(winSize, winSize),
- Size(-1, -1), TermCriteria(
- TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001));
- imagePoints[0].push_back(pointBuf[0]);
-
- if (local_->isStereo()) {
- cvtColor(view[1], viewGray, COLOR_BGR2GRAY);
- cornerSubPix(viewGray, pointBuf[1], Size(winSize, winSize),
- Size(-1, -1), TermCriteria(
- TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001));
- imagePoints[1].push_back(pointBuf[1]);
- }
-
- // Draw the corners.
- drawChessboardCorners(view[0], settings_.boardSize,
- Mat(pointBuf[0]), found1);
- if (local_->isStereo()) drawChessboardCorners(view[1],
- settings_.boardSize, Mat(pointBuf[1]), found2);
- } else {
- LOG(WARNING) << "No calibration pattern found";
- }
-
- imshow("Left", view[0]);
- if (local_->isStereo()) imshow("Right", view[1]);
- char key = static_cast<char>(waitKey(50));
-
- if (key == 27)
- break;
- }
-
- return true;
-}
-
-bool Calibrate::undistort(cv::Mat &l, cv::Mat &r) {
- local_->get(l, r);
- if (!calibrated_) return false;
- if (l.empty()) return false;
- remap(l, l, map1_[0], map2_[0], INTER_LINEAR);
- if (local_->isStereo()) remap(r, r, map1_[1], map2_[1], INTER_LINEAR);
- return true;
-}
-
-bool Calibrate::rectified(cv::Mat &l, cv::Mat &r) {
- return undistort(l, r);
-}
-
-bool Calibrate::isCalibrated() {
- return calibrated_;
-}
-
-//! [compute_errors]
-static double computeReprojectionErrors(
- const vector<vector<Point3f> >& objectPoints,
- const vector<vector<Point2f> >& imagePoints,
- const vector<Mat>& rvecs, const vector<Mat>& tvecs,
- const Mat& cameraMatrix , const Mat& distCoeffs,
- vector<float>& perViewErrors, bool fisheye) {
- vector<Point2f> imagePoints2;
- size_t totalPoints = 0;
- double totalErr = 0;
- perViewErrors.resize(objectPoints.size());
-
- for (size_t i = 0; i < objectPoints.size(); ++i) {
- if (fisheye) {
- cv::fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i],
- tvecs[i], cameraMatrix, distCoeffs);
- } else {
- projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix,
- distCoeffs, imagePoints2);
- }
- double err = norm(imagePoints[i], imagePoints2, NORM_L2);
-
- size_t n = objectPoints[i].size();
- perViewErrors[i] = static_cast<float>(std::sqrt(err*err/n));
- totalErr += err*err;
- totalPoints += n;
- }
-
- return std::sqrt(totalErr/totalPoints);
-}
-//! [compute_errors]
-//! [board_corners]
-static void calcBoardCornerPositions(Size boardSize, float squareSize,
- vector<Point3f>& corners, Calibrate::Settings::Pattern patternType) {
- corners.clear();
-
- switch (patternType) {
- case Calibrate::Settings::CHESSBOARD:
- case Calibrate::Settings::CIRCLES_GRID:
- for (int i = 0; i < boardSize.height; ++i)
- for (int j = 0; j < boardSize.width; ++j)
- corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
- break;
-
- case Calibrate::Settings::ASYMMETRIC_CIRCLES_GRID:
- for (int i = 0; i < boardSize.height; i++)
- for (int j = 0; j < boardSize.width; j++)
- corners.push_back(Point3f((2*j + i % 2)*squareSize,
- i*squareSize, 0));
- break;
- default:
- break;
- }
-}
-//! [board_corners]
-static bool _runCalibration(const Calibrate::Settings& s, Size& imageSize,
- Mat& cameraMatrix, Mat& distCoeffs,
- vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs,
- vector<Mat>& tvecs, vector<float>& reprojErrs, double& totalAvgErr,
- vector<Point3f>& newObjPoints, float grid_width, bool release_object) {
- cameraMatrix = Mat::eye(3, 3, CV_64F);
- if (s.flag & CALIB_FIX_ASPECT_RATIO)
- cameraMatrix.at<double>(0, 0) = s.aspectRatio;
-
- if (s.useFisheye) {
- distCoeffs = Mat::zeros(4, 1, CV_64F);
- } else {
- distCoeffs = Mat::zeros(8, 1, CV_64F);
- }
-
- vector<vector<Point3f> > objectPoints(1);
- calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0],
- Calibrate::Settings::CHESSBOARD);
- objectPoints[0][s.boardSize.width - 1].x = objectPoints[0][0].x +
- grid_width;
- newObjPoints = objectPoints[0];
-
- objectPoints.resize(imagePoints.size(), objectPoints[0]);
-
- // Find intrinsic and extrinsic camera parameters
- double rms;
-
- if (s.useFisheye) {
- Mat _rvecs, _tvecs;
- rms = cv::fisheye::calibrate(objectPoints, imagePoints, imageSize,
- cameraMatrix, distCoeffs, _rvecs, _tvecs, s.flag);
-
- rvecs.reserve(_rvecs.rows);
- tvecs.reserve(_tvecs.rows);
- for (int i = 0; i < static_cast<int>(objectPoints.size()); i++) {
- rvecs.push_back(_rvecs.row(i));
- tvecs.push_back(_tvecs.row(i));
- }
- } else {
- rms = calibrateCamera(objectPoints, imagePoints, imageSize,
- cameraMatrix, distCoeffs, rvecs, tvecs,
- s.flag | CALIB_USE_LU);
- }
-
- LOG(INFO) << "Re-projection error reported by calibrateCamera: "<< rms;
-
- bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
-
- objectPoints.clear();
- objectPoints.resize(imagePoints.size(), newObjPoints);
- totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs,
- tvecs, cameraMatrix, distCoeffs, reprojErrs, s.useFisheye);
-
- return ok;
-}
-
-//! [run_and_save]
-bool runCalibration(const Calibrate::Settings& s, Size imageSize,
- Mat& cameraMatrix, Mat& distCoeffs,
- vector<vector<Point2f> > imagePoints, float grid_width,
- bool release_object) {
- vector<Mat> rvecs, tvecs;
- vector<float> reprojErrs;
- double totalAvgErr = 0;
- vector<Point3f> newObjPoints;
-
- bool ok = _runCalibration(s, imageSize, cameraMatrix, distCoeffs,
- imagePoints, rvecs, tvecs, reprojErrs, totalAvgErr, newObjPoints,
- grid_width, release_object);
- LOG(INFO) << (ok ? "Calibration succeeded" : "Calibration failed")
- << ". avg re projection error = " << totalAvgErr;
-
- return ok;
-}
-//! [run_and_save]