From 8861128b5c8854ff4a288d50e563d354fab216d3 Mon Sep 17 00:00:00 2001 From: kevin Date: Thu, 16 Feb 2012 09:22:04 -0500 Subject: [PATCH] initial first commit of library --- .gitignore | 3 + CMakeLists.txt | 64 + Makefile | 1 + include/libviso2/filter.h | 99 + include/libviso2/matcher.h | 251 + include/libviso2/matrix.h | 133 + include/libviso2/reconstruction.h | 112 + include/libviso2/timer.h | 93 + include/libviso2/triangle.h | 285 + include/libviso2/viso.h | 135 + include/libviso2/viso_mono.h | 87 + include/libviso2/viso_stereo.h | 82 + mainpage.dox | 26 + manifest.xml | 22 + src/lib/filter.cpp | 468 ++ src/lib/matcher.cpp | 1481 +++++ src/lib/matrix.cpp | 850 +++ src/lib/reconstruction.cpp | 389 ++ src/lib/triangle.cpp | 8634 +++++++++++++++++++++++++++++ src/lib/viso.cpp | 114 + src/lib/viso_mono.cpp | 399 ++ src/lib/viso_stereo.cpp | 313 ++ src/mono_demo.cpp | 127 + src/mono_node.cpp | 205 + src/stereo_demo.cpp | 130 + 25 files changed, 14503 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 Makefile create mode 100644 include/libviso2/filter.h create mode 100644 include/libviso2/matcher.h create mode 100644 include/libviso2/matrix.h create mode 100644 include/libviso2/reconstruction.h create mode 100644 include/libviso2/timer.h create mode 100644 include/libviso2/triangle.h create mode 100644 include/libviso2/viso.h create mode 100644 include/libviso2/viso_mono.h create mode 100644 include/libviso2/viso_stereo.h create mode 100644 mainpage.dox create mode 100644 manifest.xml create mode 100644 src/lib/filter.cpp create mode 100644 src/lib/matcher.cpp create mode 100644 src/lib/matrix.cpp create mode 100644 src/lib/reconstruction.cpp create mode 100644 src/lib/triangle.cpp create mode 100644 src/lib/viso.cpp create mode 100644 src/lib/viso_mono.cpp create mode 100644 src/lib/viso_stereo.cpp create mode 100644 src/mono_demo.cpp create mode 100644 src/mono_node.cpp create mode 100644 src/stereo_demo.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..32cd097 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.DS_Store +build +bin diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..7602137 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 2.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +set(ROS_BUILD_TYPE Release) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) + +project (libviso2) + +# include directory +include_directories(${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}) + +# use sse3 instruction set +SET(CMAKE_CXX_FLAGS "-msse3") + +# OpenCV +find_package(OpenCV 2.3.1 REQUIRED) +if(OpenCV_FOUND) + message("======== ${OpenCV_VERSION} Found ========= ") +endif(OpenCV_FOUND) + + +# sources +FILE(GLOB LIBVISO2_SRC_FILES "src/lib/*.cpp") + +# build library +rosbuild_add_library(viso2 ${LIBVISO2_SRC_FILES}) + +# stereo demo program +rosbuild_add_executable(stereo_demo "src/stereo_demo.cpp") +target_link_libraries(stereo_demo ${OpenCV_LIBS} viso2) + +# mono demo program +rosbuild_add_executable(mono_demo "src/mono_demo.cpp") +target_link_libraries(mono_demo ${OpenCV_LIBS} viso2) + +# mono node +rosbuild_add_executable(mono_node "src/mono_node.cpp") +target_link_libraries(mono_node ${OpenCV_LIBS} viso2) + diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/include/libviso2/filter.h b/include/libviso2/filter.h new file mode 100644 index 0000000..6e2e37b --- /dev/null +++ b/include/libviso2/filter.h @@ -0,0 +1,99 @@ +/* +Copyright 2012. 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 +#include + +// define fixed-width datatypes for Visual Studio projects +#ifndef _MSC_VER + #include +#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/include/libviso2/matcher.h b/include/libviso2/matcher.h new file mode 100644 index 0000000..e90fcb3 --- /dev/null +++ b/include/libviso2/matcher.h @@ -0,0 +1,251 @@ +/* +Copyright 2012. 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 __MATCHER_H__ +#define __MATCHER_H__ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "matrix.h" + +// Define fixed-width datatypes for Visual Studio projects +#ifndef _MSC_VER + #include +#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 Matcher { + +public: + + // parameter settings + struct parameters { + + int32_t nms_n; // non-max-suppression: min. distance between maxima (in pixels) + int32_t nms_tau; // non-max-suppression: interest point peakiness threshold + int32_t match_binsize; // matching bin width/height (affects efficiency only) + int32_t match_radius; // matching radius (du/dv in pixels) + int32_t match_disp_tolerance; // dv tolerance for stereo matches (in pixels) + int32_t outlier_disp_tolerance; // outlier removal: disparity tolerance (in pixels) + int32_t outlier_flow_tolerance; // outlier removal: flow tolerance (in pixels) + int32_t multi_stage; // 0=disabled,1=multistage matching (denser and faster) + int32_t half_resolution; // 0=disabled,1=match at half resolution, refine at full resolution + int32_t refinement; // refinement (0=none,1=pixel,2=subpixel) + + // default settings + parameters () { + nms_n = 3; + nms_tau = 50; + match_binsize = 50; + match_radius = 200; + match_disp_tolerance = 2; + outlier_disp_tolerance = 5; + outlier_flow_tolerance = 5; + multi_stage = 1; + half_resolution = 1; + refinement = 1; + } + }; + + // constructor (with default parameters) + Matcher(parameters param); + + // deconstructor + ~Matcher(); + + // structure for storing matches + struct p_match { + float u1p,v1p; // u,v-coordinates in previous left image + int32_t i1p; // feature index (for tracking) + float u2p,v2p; // u,v-coordinates in previous right image + int32_t i2p; // feature index (for tracking) + float u1c,v1c; // u,v-coordinates in current left image + int32_t i1c; // feature index (for tracking) + float u2c,v2c; // u,v-coordinates in current right image + int32_t i2c; // feature index (for tracking) + p_match(){} + p_match(float u1p,float v1p,int32_t i1p,float u2p,float v2p,int32_t i2p, + float u1c,float v1c,int32_t i1c,float u2c,float v2c,int32_t i2c): + u1p(u1p),v1p(v1p),i1p(i1p),u2p(u2p),v2p(v2p),i2p(i2p), + u1c(u1c),v1c(v1c),i1c(i1c),u2c(u2c),v2c(v2c),i2c(i2c) {} + }; + + // computes features from left/right images and pushes them back to a ringbuffer, + // which interally stores the features of the current and previous image pair + // use this function for stereo or quad matching + // input: I1,I2 .......... pointers to left and right image (row-aligned), range [0..255] + // dims[0,1] ...... image width and height (both images must be rectified and of same size) + // dims[2] ........ bytes per line (often equals width) + // replace ........ if this flag is set, the current image is overwritten with + // the input images, otherwise the current image is first copied + // to the previous image (ring buffer functionality, descriptors need + // to be computed only once) + void pushBack (uint8_t *I1,uint8_t* I2,int32_t* dims,const bool replace); + + // computes features from a single image and pushes it back to a ringbuffer, + // which interally stores the features of the current and previous image pair + // use this function for flow computation + // parameter description see above + void pushBack (uint8_t *I1,int32_t* dims,const bool replace) { pushBack(I1,0,dims,replace); } + + // match features currently stored in ring buffer (current and previous frame) + // input: method ... 0 = flow, 1 = stereo, 2 = quad matching + void matchFeatures(int32_t method); + + // feature bucketing: keeps only max_features per bucket, where the domain + // is split into buckets of size (bucket_width,bucket_height) + void bucketFeatures(int32_t max_features,float bucket_width,float bucket_height); + + // return vector with matched feature points and indices + std::vector getMatches() { return p_matched_2; } + + // given a vector of inliers computes gain factor between the current and + // the previous frame. this function is useful if you want to reconstruct 3d + // and you want to cancel the change of (unknown) camera gain. + float getGain (std::vector inliers); + +private: + + // structure for storing interest points + struct maximum { + int32_t u; // u-coordinate + int32_t v; // v-coordinate + int32_t val; // value + int32_t c; // class + int32_t d1,d2,d3,d4,d5,d6,d7,d8; // descriptor + maximum() {} + maximum(int32_t u,int32_t v,int32_t val,int32_t c):u(u),v(v),val(val),c(c) {} + }; + + // u/v ranges for matching stage 0-3 + struct range { + float u_min[4]; + float u_max[4]; + float v_min[4]; + float v_max[4]; + }; + + struct delta { + float val[8]; + delta () {} + delta (float v) { + for (int32_t i=0; i<8; i++) + val[i] = v; + } + }; + + // computes the address offset for coordinates u,v of an image of given width + inline int32_t getAddressOffsetImage (const int32_t& u,const int32_t& v,const int32_t& width) { + return v*width+u; + } + + // Alexander Neubeck and Luc Van Gool: Efficient Non-Maximum Suppression, ICPR'06, algorithm 4 + void nonMaximumSuppression (int16_t* I_f1,int16_t* I_f2,const int32_t* dims,std::vector &maxima,int32_t nms_n); + + // descriptor functions + inline uint8_t saturate(int16_t in); + void filterImageAll (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,int16_t* I_f1,int16_t* I_f2,const int* dims); + void filterImageSobel (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,const int* dims); + inline void computeDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr); + inline void computeSmallDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr); + void computeDescriptors (uint8_t* I_du,uint8_t* I_dv,const int32_t bpl,std::vector &maxima); + + void getHalfResolutionDimensions(const int32_t *dims,int32_t *dims_half); + uint8_t* createHalfResolutionImage(uint8_t *I,const int32_t* dims); + + // compute sparse set of features from image + // inputs: I ........ image + // dims ..... image dimensions [width,height] + // n ........ non-max neighborhood + // tau ...... non-max threshold + // outputs: max ...... vector with maxima [u,v,value,class,descriptor (128 bits)] + // I_du ..... gradient in horizontal direction + // I_dv ..... gradient in vertical direction + // WARNING: max,I_du,I_dv has to be freed by yourself! + void computeFeatures (uint8_t *I,const int32_t* dims,int32_t* &max1,int32_t &num1,int32_t* &max2,int32_t &num2,uint8_t* &I_du,uint8_t* &I_dv,uint8_t* &I_du_full,uint8_t* &I_dv_full); + + // matching functions + void computePriorStatistics (std::vector &p_matched,int32_t method); + void createIndexVector (int32_t* m,int32_t n,std::vector *k,const int32_t &u_bin_num,const int32_t &v_bin_num); + inline void findMatch (int32_t* m1,const int32_t &i1,int32_t* m2,const int32_t &step_size, + std::vector *k2,const int32_t &u_bin_num,const int32_t &v_bin_num,const int32_t &stat_bin, + int32_t& min_ind,int32_t stage,bool flow,bool use_prior); + void matching (int32_t *m1p,int32_t *m2p,int32_t *m1c,int32_t *m2c, + int32_t n1p,int32_t n2p,int32_t n1c,int32_t n2c, + std::vector &p_matched,int32_t method,bool use_prior); + + // outlier removal + void removeOutliers (std::vector &p_matched,int32_t method); + + // parabolic fitting + bool parabolicFitting(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1, + const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2, + const float &u1,const float &v1, + float &u2,float &v2, + Matrix At,Matrix AtA, + uint8_t* desc_buffer); + void relocateMinimum(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1, + const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2, + const float &u1,const float &v1, + float &u2,float &v2, + uint8_t* desc_buffer); + void refinement (std::vector &p_matched,int32_t method); + + // mean for gain computation + inline float mean(const uint8_t* I,const int32_t &bpl,const int32_t &u_min,const int32_t &u_max,const int32_t &v_min,const int32_t &v_max); + + // parameters + parameters param; + int32_t margin; + + int32_t *m1p1,*m2p1,*m1c1,*m2c1; + int32_t *m1p2,*m2p2,*m1c2,*m2c2; + int32_t n1p1,n2p1,n1c1,n2c1; + int32_t n1p2,n2p2,n1c2,n2c2; + uint8_t *I1p,*I2p,*I1c,*I2c; + uint8_t *I1p_du,*I2p_du,*I1c_du,*I2c_du; + uint8_t *I1p_dv,*I2p_dv,*I1c_dv,*I2c_dv; + uint8_t *I1p_du_full,*I2p_du_full,*I1c_du_full,*I2c_du_full; // only needed for + uint8_t *I1p_dv_full,*I2p_dv_full,*I1c_dv_full,*I2c_dv_full; // half-res matching + int32_t dims_p[3],dims_c[3]; + + std::vector p_matched_1; + std::vector p_matched_2; + std::vector ranges; +}; + +#endif + diff --git a/include/libviso2/matrix.h b/include/libviso2/matrix.h new file mode 100644 index 0000000..5ec306c --- /dev/null +++ b/include/libviso2/matrix.h @@ -0,0 +1,133 @@ +/* +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 +#include +#include +#include +#include + +#ifndef _MSC_VER + #include +#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 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/include/libviso2/reconstruction.h b/include/libviso2/reconstruction.h new file mode 100644 index 0000000..6d53fe8 --- /dev/null +++ b/include/libviso2/reconstruction.h @@ -0,0 +1,112 @@ +/* +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 RECONSTRUCTION_H +#define RECONSTRUCTION_H + +#include +#include +#include +#include +#define _USE_MATH_DEFINES +#include + +#include "matcher.h" +#include "matrix.h" + +class Reconstruction { + +public: + + // constructor + Reconstruction (); + + // deconstructor + ~Reconstruction (); + + // a generic 3d point + struct point3d { + float x,y,z; + point3d () {} + point3d (float x,float y,float z) : x(x),y(y),z(z) {} + }; + + // set calibration parameters (intrinsics), must be called at least once + // input: f ....... focal length (assumes fu=fv) + // cu,cv ... principal point + void setCalibration (FLOAT f,FLOAT cu,FLOAT cv); + + // takes a set of monocular feature matches (flow method) and the egomotion + // estimate between the 2 frames Tr, tries to associate the features with previous + // frames (tracking) and computes 3d points once tracks gets lost. + // point types: 0 ..... everything + // 1 ..... road and above + // 2 ..... only above road + // min_track_length ... min number of frames a point needs to be tracked for reconstruction + // max_dist ........... maximum point distance from camera + // min_angle .......... avoid angles smaller than this for reconstruction (in degrees) + void update (std::vector p_matched,Matrix Tr,int32_t point_type=1,int32_t min_track_length=2,double max_dist=30,double min_angle=2); + + // return currently computed 3d points (finished tracks) + std::vector getPoints() { return points; } + +private: + + struct point2d { + float u,v; + point2d () {} + point2d (float u,float v) : u(u),v(v) {} + }; + + struct track { + std::vector pixels; + int32_t first_frame; + int32_t last_frame; + int32_t last_idx; + }; + + enum result { UPDATED, FAILED, CONVERGED }; + + bool initPoint(const track &t,point3d &p); + bool refinePoint(const track &t,point3d &p); + double pointDistance(const track &t,point3d &p); + double rayAngle(const track &t,point3d &p); + int32_t pointType(const track &t,point3d &p); + result updatePoint(const track &t,point3d &p,const FLOAT &step_size,const FLOAT &eps); + void computeObservations(const std::vector &p); + bool computePredictionsAndJacobian(const std::vector::iterator &P_begin,const std::vector::iterator &P_end,point3d &p); + void testJacobian(); + + // calibration matrices + Matrix K,Tr_cam_road; + + std::vector tracks; + std::vector Tr_total; + std::vector Tr_inv_total; + std::vector P_total; + std::vector points; + + FLOAT *J; // jacobian + FLOAT *p_observe,*p_predict; // observed and predicted 2d points + +}; + +#endif // RECONSTRUCTION_H diff --git a/include/libviso2/timer.h b/include/libviso2/timer.h new file mode 100644 index 0000000..6c70903 --- /dev/null +++ b/include/libviso2/timer.h @@ -0,0 +1,93 @@ +/* +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 __TIMER_H__ +#define __TIMER_H__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Timer { + +public: + + Timer() {} + + ~Timer() {} + + void start (std::string title) { + desc.push_back(title); + push_back_time(); + } + + void stop () { + if (time.size()<=desc.size()) + push_back_time(); + } + + void plot () { + stop(); + float total_time = 0; + for (int32_t i=0; i<(int32_t)desc.size(); i++) { + float curr_time = getTimeDifferenceMilliseconds(time[i],time[i+1]); + total_time += curr_time; + std::cout.width(30); + std::cout << desc[i] << " "; + std::cout << std::fixed << std::setprecision(1) << std::setw(6); + std::cout << curr_time; + std::cout << " ms" << std::endl; + } + std::cout << "========================================" << std::endl; + std::cout << " Total time "; + std::cout << std::fixed << std::setprecision(1) << std::setw(6); + std::cout << total_time; + std::cout << " ms" << std::endl << std::endl; + } + + void reset () { + desc.clear(); + time.clear(); + } + +private: + std::vector desc; + std::vector time; + + void push_back_time () { + timeval curr_time; + gettimeofday(&curr_time,0); + time.push_back(curr_time); + } + + float getTimeDifferenceMilliseconds(timeval a,timeval b) { + return ((float)(b.tv_sec -a.tv_sec ))*1e+3 + + ((float)(b.tv_usec-a.tv_usec))*1e-3; + } +}; + +#endif diff --git a/include/libviso2/triangle.h b/include/libviso2/triangle.h new file mode 100644 index 0000000..43dc8d5 --- /dev/null +++ b/include/libviso2/triangle.h @@ -0,0 +1,285 @@ +/*****************************************************************************/ +/* */ +/* (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, 2010 */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* */ +/* 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/include/libviso2/viso.h b/include/libviso2/viso.h new file mode 100644 index 0000000..ec4c099 --- /dev/null +++ b/include/libviso2/viso.h @@ -0,0 +1,135 @@ +/* +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 VISO_H +#define VISO_H + +#include "matrix.h" +#include "matcher.h" + +class VisualOdometry { + +public: + + // camera parameters (all are mandatory / need to be supplied) + struct calibration { + double f; // focal length (in pixels) + double cu; // principal point (u-coordinate) + double cv; // principal point (v-coordinate) + calibration () { + f = 1; + cu = 0; + cv = 0; + } + }; + + // bucketing parameters + struct bucketing { + int32_t max_features; // maximal number of features per bucket + double bucket_width; // width of bucket + double bucket_height; // height of bucket + bucketing () { + max_features = 2; + bucket_width = 50; + bucket_height = 50; + } + }; + + // general parameters + struct parameters { + Matcher::parameters match; // matching parameters + VisualOdometry::bucketing bucket; // bucketing parameters + VisualOdometry::calibration calib; // camera calibration parameters + }; + + // constructor, takes as input a parameter structure: + // do not instanciate this class directly, instanciate VisualOdometryMono + // or VisualOdometryStereo instead! + VisualOdometry (parameters param); + + // deconstructor + ~VisualOdometry (); + + // call this function instead of the specialized ones, if you already have + // feature matches, and simply want to compute visual odometry from them, without + // using the internal matching functions. + bool process (std::vector p_matched_) { + p_matched = p_matched_; + return updateMotion(); + } + + // returns transformation from previous to current coordinates as a 4x4 + // homogeneous transformation matrix Tr_delta, with the following semantics: + // p_t = Tr_delta * p_ {t-1} takes a point in the camera coordinate system + // at time t_1 and maps it to the camera coordinate system at time t. + // note: getMotion() returns the last transformation even when process() + // has failed. this is useful if you wish to linearly extrapolate occasional + // frames for which no correspondences have been found + Matrix getMotion () { return Tr_delta; } + + // returns previous to current feature matches from internal matcher + std::vector getMatches () { return matcher->getMatches(); } + + // returns the number of successfully matched points, after bucketing + int32_t getNumberOfMatches () { return p_matched.size(); } + + // returns the number of inliers: num_inliers <= num_matched + int32_t getNumberOfInliers () { return inliers.size(); } + + // streams out the current transformation matrix Tr_delta + friend std::ostream& operator<< (std::ostream &os,VisualOdometry &viso) { + Matrix p = viso.getMotion(); + os << p.val[0][0] << " " << p.val[0][1] << " " << p.val[0][2] << " " << p.val[0][3] << " "; + os << p.val[1][0] << " " << p.val[1][1] << " " << p.val[1][2] << " " << p.val[1][3] << " "; + os << p.val[2][0] << " " << p.val[2][1] << " " << p.val[2][2] << " " << p.val[2][3]; + return os; + } + +protected: + + // calls bucketing and motion estimation + bool updateMotion (); + + // compute transformation matrix from transformation vector + Matrix transformationVectorToMatrix (std::vector tr); + + // compute motion from previous to current coordinate system + // if motion could not be computed, resulting vector will be of size 0 + virtual std::vector estimateMotion (std::vector p_matched) = 0; + + // get random and unique sample of num numbers from 1:N + std::vector getRandomSample (int32_t N,int32_t num); + + Matrix Tr_delta; // transformation (previous -> current frame) + Matcher *matcher; // feature matcher + std::vector inliers; // inlier set + double *J; // jacobian + double *p_observe; // observed 2d points + double *p_predict; // predicted 2d points + +private: + + std::vector p_matched; // feature point matches + parameters param; // common parameters +}; + +#endif // VISO_H + diff --git a/include/libviso2/viso_mono.h b/include/libviso2/viso_mono.h new file mode 100644 index 0000000..f6884a7 --- /dev/null +++ b/include/libviso2/viso_mono.h @@ -0,0 +1,87 @@ +/* +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 VISO_MONO_H +#define VISO_MONO_H + +#include "viso.h" + +class VisualOdometryMono : public VisualOdometry { + +public: + + // monocular-specific parameters (mandatory: height,pitch) + struct parameters : public VisualOdometry::parameters { + double height; // camera height above ground (meters) + double pitch; // camera pitch (rad, neg=pointing down) + int32_t ransac_iters; // number of RANSAC iterations + double inlier_threshold; // fundamental matrix inlier threshold + double motion_threshold; // directly return false on small motions + parameters () { + height = 1.0; + pitch = 0.0; + ransac_iters = 2000; + inlier_threshold = 0.00001; + motion_threshold = 100.0; + } + }; + + // constructor, takes as inpute a parameter structure + VisualOdometryMono (parameters param); + + // deconstructor + ~VisualOdometryMono (); + + // process a new image, pushs the image back to an internal ring buffer. + // valid motion estimates are available after calling process for two times. + // inputs: I ......... pointer to rectified image (uint8, row-aligned) + // dims[0] ... width of I + // dims[1] ... height of I + // dims[2] ... bytes per line (often equal to width) + // replace ... replace current image with I, without copying last current + // image to previous image internally. this option can be used + // when small/no motions are observed to obtain Tr_delta wrt + // an older coordinate system / time step than the previous one. + // output: returns false if motion too small or an error occured + bool process (uint8_t *I,int32_t* dims,bool replace=false); + +private: + + template struct idx_cmp { + idx_cmp(const T arr) : arr(arr) {} + bool operator()(const size_t a, const size_t b) const { return arr[a] < arr[b]; } + const T arr; + }; + + std::vector estimateMotion (std::vector p_matched); + Matrix smallerThanMedian (Matrix &X,double &median); + bool normalizeFeaturePoints (std::vector &p_matched,Matrix &Tp,Matrix &Tc); + void fundamentalMatrix (const std::vector &p_matched,const std::vector &active,Matrix &F); + void EtoRt(Matrix &E,Matrix &K,std::vector &p_matched,std::vector &inliers,Matrix &X,Matrix &R,Matrix &t); + int32_t triangulateChieral (std::vector &p_matched,Matrix &K,Matrix &R,Matrix &t,Matrix &X); + std::vector getInlier (std::vector &p_matched,Matrix &F); + + // parameters + parameters param; +}; + +#endif // VISO_MONO_H + diff --git a/include/libviso2/viso_stereo.h b/include/libviso2/viso_stereo.h new file mode 100644 index 0000000..2f8433e --- /dev/null +++ b/include/libviso2/viso_stereo.h @@ -0,0 +1,82 @@ +/* +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 VISO_STEREO_H +#define VISO_STEREO_H + +#include "viso.h" + +class VisualOdometryStereo : public VisualOdometry { + +public: + + // stereo-specific parameters (mandatory: base) + struct parameters : public VisualOdometry::parameters { + double base; // baseline (meters) + int32_t ransac_iters; // number of RANSAC iterations + double inlier_threshold; // fundamental matrix inlier threshold + bool reweighting; // lower border weights (more robust to calibration errors) + parameters () { + base = 1.0; + ransac_iters = 200; + inlier_threshold = 1.5; + reweighting = true; + } + }; + + // constructor, takes as inpute a parameter structure + VisualOdometryStereo (parameters param); + + // deconstructor + ~VisualOdometryStereo (); + + // process a new images, push the images back to an internal ring buffer. + // valid motion estimates are available after calling process for two times. + // inputs: I1 ........ pointer to rectified left image (uint8, row-aligned) + // I2 ........ pointer to rectified right image (uint8, row-aligned) + // dims[0] ... width of I1 and I2 (both must be of same size) + // dims[1] ... height of I1 and I2 (both must be of same size) + // dims[2] ... bytes per line (often equal to width) + // replace ... replace current images with I1 and I2, without copying last current + // images to previous images internally. this option can be used + // when small/no motions are observed to obtain Tr_delta wrt + // an older coordinate system / time step than the previous one. + // output: returns false if an error occured + bool process (uint8_t *I1,uint8_t *I2,int32_t* dims,bool replace=false); + +private: + + std::vector estimateMotion (std::vector p_matched); + enum result { UPDATED, FAILED, CONVERGED }; + result updateParameters(std::vector &p_matched,std::vector &active,std::vector &tr,double step_size,double eps); + void computeObservations(std::vector &p_matched,std::vector &active); + void computeResidualsAndJacobian(std::vector &tr,std::vector &active); + std::vector getInlier(std::vector &p_matched,std::vector &tr); + + double *X,*Y,*Z; // 3d points + double *p_residual; // residuals (p_residual=p_observe-p_predict) + + // parameters + parameters param; +}; + +#endif // VISO_STEREO_H + diff --git a/mainpage.dox b/mainpage.dox new file mode 100644 index 0000000..955ca0c --- /dev/null +++ b/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b libviso2 is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/manifest.xml b/manifest.xml new file mode 100644 index 0000000..9f07bc1 --- /dev/null +++ b/manifest.xml @@ -0,0 +1,22 @@ + + + + libviso2 + + + Kevin Walchko + GPL Ver 2 + + http://ros.org/wiki/libviso2 + + + + + + + + + + + + diff --git a/src/lib/filter.cpp b/src/lib/filter.cpp new file mode 100644 index 0000000..e178e78 --- /dev/null +++ b/src/lib/filter.cpp @@ -0,0 +1,468 @@ +/* +Copyright 2012. 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 +#include +#include + +#include "filter.h" + +// define fixed-width datatypes for Visual Studio projects +#ifndef _MSC_VER + #include +#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 < 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/src/lib/matcher.cpp b/src/lib/matcher.cpp new file mode 100644 index 0000000..a40fc98 --- /dev/null +++ b/src/lib/matcher.cpp @@ -0,0 +1,1481 @@ +/* +Copyright 2012. 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 "matcher.h" +#include "triangle.h" +#include "filter.h" + +using namespace std; + +////////////////////// +// PUBLIC FUNCTIONS // +////////////////////// + +// constructor (with default parameters) +Matcher::Matcher(parameters param) : param(param) { + + // init match ring buffer to zero + m1p1 = 0; n1p1 = 0; + m1p2 = 0; n1p2 = 0; + m2p1 = 0; n2p1 = 0; + m2p2 = 0; n2p2 = 0; + m1c1 = 0; n1c1 = 0; + m1c2 = 0; n1c2 = 0; + m2c1 = 0; n2c1 = 0; + m2c2 = 0; n2c2 = 0; + I1p = 0; I2p = 0; + I1c = 0; I2c = 0; + I1p_du = 0; I1p_dv = 0; + I2p_du = 0; I2p_dv = 0; + I1c_du = 0; I1c_dv = 0; + I2c_du = 0; I2c_dv = 0; + I1p_du_full = 0; I1p_dv_full = 0; + I2p_du_full = 0; I2p_dv_full = 0; + I1c_du_full = 0; I1c_dv_full = 0; + I2c_du_full = 0; I2c_dv_full = 0; + + // margin needed to compute descriptor + sobel responses + margin = 5+1; + + // adjust match radius on half resolution + if (param.half_resolution) + this->param.match_radius /= 2; +} + +// deconstructor +Matcher::~Matcher() { + if (I1p) _mm_free(I1p); + if (I2p) _mm_free(I2p); + if (I1c) _mm_free(I1c); + if (I2c) _mm_free(I2c); + if (m1p1) _mm_free(m1p1); + if (m1p2) _mm_free(m1p2); + if (I1p_du) _mm_free(I1p_du); + if (I1p_dv) _mm_free(I1p_dv); + if (I1p_du_full) _mm_free(I1p_du_full); + if (I1p_dv_full) _mm_free(I1p_dv_full); + if (m2p1) _mm_free(m2p1); + if (m2p2) _mm_free(m2p2); + if (I2p_du) _mm_free(I2p_du); + if (I2p_dv) _mm_free(I2p_dv); + if (I2p_du_full) _mm_free(I2p_du_full); + if (I2p_dv_full) _mm_free(I2p_dv_full); + if (m1c1) _mm_free(m1c1); + if (m1c2) _mm_free(m1c2); + if (I1c_du) _mm_free(I1c_du); + if (I1c_dv) _mm_free(I1c_dv); + if (I1c_du_full) _mm_free(I1c_du_full); + if (I1c_dv_full) _mm_free(I1c_dv_full); + if (m2c1) _mm_free(m2c1); + if (m2c2) _mm_free(m2c2); + if (I2c_du) _mm_free(I2c_du); + if (I2c_dv) _mm_free(I2c_dv); + if (I2c_du_full) _mm_free(I2c_du_full); + if (I2c_dv_full) _mm_free(I2c_dv_full); +} + +void Matcher::pushBack (uint8_t *I1,uint8_t* I2,int32_t* dims,const bool replace) { + + // image dimensions + int32_t width = dims[0]; + int32_t height = dims[1]; + int32_t bpl = dims[2]; + + // sanity check + if (width<=0 || height<=0 || bpl0) + refinement(p_matched_2,method); + removeOutliers(p_matched_2,method); + + // single pass matching + } else { + matching(m1p2,m2p2,m1c2,m2c2,n1p2,n2p2,n1c2,n2c2,p_matched_2,method,false); + if (param.refinement>0) + refinement(p_matched_2,method); + removeOutliers(p_matched_2,method); + } +} + +void Matcher::bucketFeatures(int32_t max_features,float bucket_width,float bucket_height) { + + // find max values + float u_max = 0; + float v_max = 0; + for (vector::iterator it = p_matched_2.begin(); it!=p_matched_2.end(); it++) { + if (it->u1c>u_max) u_max=it->u1c; + if (it->v1c>v_max) v_max=it->v1c; + } + + // allocate number of buckets needed + int32_t bucket_cols = (int32_t)floor(u_max/bucket_width)+1; + int32_t bucket_rows = (int32_t)floor(v_max/bucket_height)+1; + vector *buckets = new vector[bucket_cols*bucket_rows]; + + // assign matches to their buckets + for (vector::iterator it=p_matched_2.begin(); it!=p_matched_2.end(); it++) { + int32_t u = (int32_t)floor(it->u1c/bucket_width); + int32_t v = (int32_t)floor(it->v1c/bucket_height); + buckets[v*bucket_cols+u].push_back(*it); + } + + // refill p_matched from buckets + p_matched_2.clear(); + for (int32_t i=0; i::iterator it=buckets[i].begin(); it!=buckets[i].end(); it++) { + p_matched_2.push_back(*it); + k++; + if (k>=max_features) + break; + } + } + + // free buckets + delete []buckets; +} + +float Matcher::getGain (vector inliers) { + + // check if two images are provided and matched + if (I1p==0 || I1c==0 || p_matched_2.size()==0 || inliers.size()==0) + return 1; + + int32_t window_size = 3; + float gain = 0; + int32_t num = 0; + int32_t u_min,u_max,v_min,v_max; + float mean_prev,mean_curr; + + for (vector::iterator it=inliers.begin(); it!=inliers.end(); it++) { + if (*it<(int32_t)p_matched_2.size()) { + + // mean in previous image + u_min = min(max((int32_t)p_matched_2[*it].u1p-window_size,0),dims_p[0]); + u_max = min(max((int32_t)p_matched_2[*it].u1p+window_size,0),dims_p[0]); + v_min = min(max((int32_t)p_matched_2[*it].v1p-window_size,0),dims_p[1]); + v_max = min(max((int32_t)p_matched_2[*it].v1p+window_size,0),dims_p[1]); + mean_prev = mean(I1p,dims_p[2],u_min,u_max,v_min,v_max); + + // mean in current image + u_min = min(max((int32_t)p_matched_2[*it].u1c-window_size,0),dims_p[0]); + u_max = min(max((int32_t)p_matched_2[*it].u1c+window_size,0),dims_p[0]); + v_min = min(max((int32_t)p_matched_2[*it].v1c-window_size,0),dims_p[1]); + v_max = min(max((int32_t)p_matched_2[*it].v1c+window_size,0),dims_p[1]); + mean_curr = mean(I1c,dims_c[2],u_min,u_max,v_min,v_max); + + if (mean_prev>10) { + gain += mean_curr/mean_prev; + num++; + } + } + } + + if (num>0) return gain/=(float)num; + else return 1; +} + +/////////////////////// +// PRIVATE FUNCTIONS // +/////////////////////// + +void Matcher::nonMaximumSuppression (int16_t* I_f1,int16_t* I_f2,const int32_t* dims,vector &maxima,int32_t nms_n) { + + // extract parameters + int32_t width = dims[0]; + int32_t height = dims[1]; + int32_t bpl = dims[2]; + int32_t n = nms_n; + int32_t tau = param.nms_tau; + + // loop variables + int32_t f1mini,f1minj,f1maxi,f1maxj,f2mini,f2minj,f2maxi,f2maxj; + int32_t f1minval,f1maxval,f2minval,f2maxval,currval; + int32_t addr; + + for (int32_t i=n+margin; if1maxval) { + f1maxi = i2; + f1maxj = j2; + f1maxval = currval; + } + currval = *(I_f2+addr); + if (currvalf2maxval) { + f2maxi = i2; + f2maxj = j2; + f2maxval = currval; + } + } + } + + // f1 minimum + for (int32_t i2=f1mini-n; i2<=min(f1mini+n,width-1-margin); i2++) { + for (int32_t j2=f1minj-n; j2<=min(f1minj+n,height-1-margin); j2++) { + currval = *(I_f1+getAddressOffsetImage(i2,j2,bpl)); + if (currvali+n || j2j+n)) + goto failed_f1min; + } + } + if (f1minval<=-tau) + maxima.push_back(Matcher::maximum(f1mini,f1minj,f1minval,0)); + failed_f1min:; + + // f1 maximum + for (int32_t i2=f1maxi-n; i2<=min(f1maxi+n,width-1-margin); i2++) { + for (int32_t j2=f1maxj-n; j2<=min(f1maxj+n,height-1-margin); j2++) { + currval = *(I_f1+getAddressOffsetImage(i2,j2,bpl)); + if (currval>f1maxval && (i2i+n || j2j+n)) + goto failed_f1max; + } + } + if (f1maxval>=tau) + maxima.push_back(Matcher::maximum(f1maxi,f1maxj,f1maxval,1)); + failed_f1max:; + + // f2 minimum + for (int32_t i2=f2mini-n; i2<=min(f2mini+n,width-1-margin); i2++) { + for (int32_t j2=f2minj-n; j2<=min(f2minj+n,height-1-margin); j2++) { + currval = *(I_f2+getAddressOffsetImage(i2,j2,bpl)); + if (currvali+n || j2j+n)) + goto failed_f2min; + } + } + if (f2minval<=-tau) + maxima.push_back(Matcher::maximum(f2mini,f2minj,f2minval,2)); + failed_f2min:; + + // f2 maximum + for (int32_t i2=f2maxi-n; i2<=min(f2maxi+n,width-1-margin); i2++) { + for (int32_t j2=f2maxj-n; j2<=min(f2maxj+n,height-1-margin); j2++) { + currval = *(I_f2+getAddressOffsetImage(i2,j2,bpl)); + if (currval>f2maxval && (i2i+n || j2j+n)) + goto failed_f2max; + } + } + if (f2maxval>=tau) + maxima.push_back(Matcher::maximum(f2maxi,f2maxj,f2maxval,3)); + failed_f2max:; + } + } +} + +inline void Matcher::computeDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr) { + + // get address indices + int32_t addr_m1 = getAddressOffsetImage(u,v-1,bpl); + int32_t addr_m3 = addr_m1-2*bpl; + int32_t addr_m5 = addr_m3-2*bpl; + int32_t addr_p1 = addr_m1+2*bpl; + int32_t addr_p3 = addr_p1+2*bpl; + int32_t addr_p5 = addr_p3+2*bpl; + + // compute descriptor + uint32_t k = 0; + desc_addr[k++] = I_du[addr_m1-3]; + desc_addr[k++] = I_dv[addr_m1-3]; + desc_addr[k++] = I_du[addr_p1-3]; + desc_addr[k++] = I_dv[addr_p1-3]; + desc_addr[k++] = I_du[addr_m1-1]; + desc_addr[k++] = I_dv[addr_m1-1]; + desc_addr[k++] = I_du[addr_p1-1]; + desc_addr[k++] = I_dv[addr_p1-1]; + desc_addr[k++] = I_du[addr_m1+3]; + desc_addr[k++] = I_dv[addr_m1+3]; + desc_addr[k++] = I_du[addr_p1+3]; + desc_addr[k++] = I_dv[addr_p1+3]; + desc_addr[k++] = I_du[addr_m1+1]; + desc_addr[k++] = I_dv[addr_m1+1]; + desc_addr[k++] = I_du[addr_p1+1]; + desc_addr[k++] = I_dv[addr_p1+1]; + desc_addr[k++] = I_du[addr_m5-1]; + desc_addr[k++] = I_dv[addr_m5-1]; + desc_addr[k++] = I_du[addr_p5-1]; + desc_addr[k++] = I_dv[addr_p5-1]; + desc_addr[k++] = I_du[addr_m5+1]; + desc_addr[k++] = I_dv[addr_m5+1]; + desc_addr[k++] = I_du[addr_p5+1]; + desc_addr[k++] = I_dv[addr_p5+1]; + desc_addr[k++] = I_du[addr_m3-5]; + desc_addr[k++] = I_dv[addr_m3-5]; + desc_addr[k++] = I_du[addr_p3-5]; + desc_addr[k++] = I_dv[addr_p3-5]; + desc_addr[k++] = I_du[addr_m3+5]; + desc_addr[k++] = I_dv[addr_m3+5]; + desc_addr[k++] = I_du[addr_p3+5]; + desc_addr[k++] = I_dv[addr_p3+5]; +} + +inline void Matcher::computeSmallDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr) { + + // get address indices + int32_t addr2 = getAddressOffsetImage(u,v,bpl); + int32_t addr1 = addr2-bpl; + int32_t addr0 = addr1-bpl; + int32_t addr3 = addr2+bpl; + int32_t addr4 = addr3+bpl; + + // compute ELAS-descriptor + uint32_t k = 0; + desc_addr[k++] = I_du[addr0]; + desc_addr[k++] = I_du[addr1-2]; + desc_addr[k++] = I_du[addr1]; + desc_addr[k++] = I_du[addr1+2]; + desc_addr[k++] = I_du[addr2-1]; + desc_addr[k++] = I_du[addr2]; + desc_addr[k++] = I_du[addr2]; + desc_addr[k++] = I_du[addr2+1]; + desc_addr[k++] = I_du[addr3-2]; + desc_addr[k++] = I_du[addr3]; + desc_addr[k++] = I_du[addr3+2]; + desc_addr[k++] = I_du[addr4]; + desc_addr[k++] = I_dv[addr1]; + desc_addr[k++] = I_dv[addr2-1]; + desc_addr[k++] = I_dv[addr2+1]; + desc_addr[k++] = I_dv[addr3]; +} + +void Matcher::computeDescriptors (uint8_t* I_du,uint8_t* I_dv,const int32_t bpl,std::vector &maxima) { + + // loop variables + int32_t u,v; + uint8_t *desc_addr; + + // for all maxima do + for (vector::iterator it=maxima.begin(); it!=maxima.end(); it++) { + u = (*it).u; + v = (*it).v; + desc_addr = (uint8_t*)(&((*it).d1)); + computeDescriptor(I_du,I_dv,bpl,u,v,desc_addr); + } +} + +inline uint8_t Matcher::saturate (int16_t in) { + if (in<0) return 0; + if (in>255) return 255; + return in; +} + +void Matcher::filterImageAll (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,int16_t* I_f1,int16_t* I_f2,const int* dims) { + + // get bpl and height + const int32_t height = dims[1]; + const int32_t bpl = dims[2]; + + // init sobel pointers + const uint8_t* p00 = I + 0; + const uint8_t* p01 = I + 1; + const uint8_t* p02 = I + 2; + const uint8_t* p03 = I + 3; + const uint8_t* p04 = I + 4; + const uint8_t* p10 = I + 0 + bpl; + const uint8_t* p11 = I + 1 + bpl; + const uint8_t* p12 = I + 2 + bpl; + const uint8_t* p13 = I + 3 + bpl; + const uint8_t* p14 = I + 4 + bpl; + const uint8_t* p20 = I + 0 + 2*bpl; + const uint8_t* p21 = I + 1 + 2*bpl; + const uint8_t* p22 = I + 2 + 2*bpl; + const uint8_t* p23 = I + 3 + 2*bpl; + const uint8_t* p24 = I + 4 + 2*bpl; + const uint8_t* p30 = I + 0 + 3*bpl; + const uint8_t* p31 = I + 1 + 3*bpl; + const uint8_t* p32 = I + 2 + 3*bpl; + const uint8_t* p33 = I + 3 + 3*bpl; + const uint8_t* p34 = I + 4 + 3*bpl; + const uint8_t* p40 = I + 0 + 4*bpl; + const uint8_t* p41 = I + 1 + 4*bpl; + const uint8_t* p42 = I + 2 + 4*bpl; + const uint8_t* p43 = I + 3 + 4*bpl; + const uint8_t* p44 = I + 4 + 4*bpl; + + // init output pointers + uint8_t* result_du = I_du + bpl + 1; + uint8_t* result_dv = I_dv + bpl + 1; + int16_t* result_f1 = I_f1 + 2*bpl + 2; + int16_t* result_f2 = I_f2 + 2*bpl + 2; + + // stop here + const uint8_t* end_input = I + bpl*height; + + // compute filter responses (border pixels are invalid) + for( ; p44 != end_input; p00++, p01++, p02++, p03++, p04++, + p10++, p11++, p12++, p13++, p14++, + p20++, p21++, p22++, p23++, p24++, + p30++, p31++, p32++, p33++, p34++, + p40++, p41++, p42++, p43++, p44++, + result_du++, result_dv++, result_f1++, result_f2++ ) { + int16_t temp_du = - *p00 - 2 * *p10 - *p20 + *p02 + 2 * *p12 + *p22; + int16_t temp_dv = - *p00 - 2 * *p01 - *p02 + *p20 + 2 * *p21 + *p22; + *result_du = saturate( temp_du/4 + 128 ); + *result_dv = saturate( temp_dv/4 + 128 ); + *result_f1 = - *p00 - *p01 - *p02 - *p03 - *p04 + - *p10 + *p11 + *p12 + *p13 - *p14 + - *p20 + *p21 + 8 * *p22 + *p23 - *p24 + - *p30 + *p31 + *p32 + *p33 - *p34 + - *p40 - *p41 - *p42 - *p43 - *p44; + *result_f2 = - *p00 - *p01 + *p03 + *p04 + - *p10 - *p11 + *p13 + *p14 + + *p30 + *p31 - *p33 - *p34 + + *p40 + *p41 - *p43 - *p44; + } +} + +void Matcher::filterImageSobel (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,const int* dims) { + + // get image width and height + const int32_t height = dims[1]; + const int32_t bpl = dims[2]; + + // init sobel pointers + const uint8_t* p00 = I + 0; + const uint8_t* p01 = I + 1; + const uint8_t* p02 = I + 2; + const uint8_t* p10 = I + 0 + bpl; + const uint8_t* p11 = I + 1 + bpl; + const uint8_t* p12 = I + 2 + bpl; + const uint8_t* p20 = I + 0 + 2*bpl; + const uint8_t* p21 = I + 1 + 2*bpl; + const uint8_t* p22 = I + 2 + 2*bpl; + + // init output pointers + uint8_t* result_du = I_du + bpl + 1; + uint8_t* result_dv = I_dv + bpl + 1; + + // stop here + const uint8_t* end_input = I + bpl*height; + + // compute filter responses (border pixels are invalid) + for( ; p22 != end_input; p00++, p01++, p02++, + p10++, p11++, p12++, + p20++, p21++, p22++, + result_du++, result_dv++) { + int16_t temp_du = - *p00 - 2 * *p10 - *p20 + *p02 + 2 * *p12 + *p22; + int16_t temp_dv = - *p00 - 2 * *p01 - *p02 + *p20 + 2 * *p21 + *p22; + *result_du = saturate( temp_du/4 + 128 ); + *result_dv = saturate( temp_dv/4 + 128 ); + } +} + +void Matcher::getHalfResolutionDimensions(const int32_t *dims,int32_t *dims_half) { + dims_half[0] = dims[0]/2; + dims_half[1] = dims[1]/2; + dims_half[2] = dims_half[0]+15-(dims_half[0]-1)%16; +} + +uint8_t* Matcher::createHalfResolutionImage(uint8_t *I,const int32_t* dims) { + int32_t dims_half[3]; + getHalfResolutionDimensions(dims,dims_half); + uint8_t* I_half = (uint8_t*)_mm_malloc(dims_half[2]*dims_half[1]*sizeof(uint8_t),16); + for (int32_t v=0; v maxima1; + if (param.multi_stage) { + int32_t nms_n_sparse = param.nms_n*3; + if (nms_n_sparse>10) + nms_n_sparse = max(param.nms_n,10); + nonMaximumSuppression(I_f1,I_f2,dims_matching,maxima1,nms_n_sparse); + computeDescriptors(I_du,I_dv,dims_matching[2],maxima1); + } + + // extract dense maxima (2nd pass) via non-maximum suppression + vector maxima2; + nonMaximumSuppression(I_f1,I_f2,dims_matching,maxima2,param.nms_n); + computeDescriptors(I_du,I_dv,dims_matching[2],maxima2); + + // release filter images + _mm_free(I_f1); + _mm_free(I_f2); + + // get number of interest points and init maxima pointer to NULL + num1 = maxima1.size(); + num2 = maxima2.size(); + max1 = 0; + max2 = 0; + + int32_t s = 1; + if (param.half_resolution) + s = 2; + + // return sparse maxima as 16-bytes aligned memory + if (num1!=0) { + max1 = (int32_t*)_mm_malloc(sizeof(Matcher::maximum)*num1,16); + int32_t k=0; + for (vector::iterator it=maxima1.begin(); it!=maxima1.end(); it++) { + *(max1+k++) = it->u*s; *(max1+k++) = it->v*s; *(max1+k++) = 0; *(max1+k++) = it->c; + *(max1+k++) = it->d1; *(max1+k++) = it->d2; *(max1+k++) = it->d3; *(max1+k++) = it->d4; + *(max1+k++) = it->d5; *(max1+k++) = it->d6; *(max1+k++) = it->d7; *(max1+k++) = it->d8; + } + } + + // return dense maxima as 16-bytes aligned memory + if (num2!=0) { + max2 = (int32_t*)_mm_malloc(sizeof(Matcher::maximum)*num2,16); + int32_t k=0; + for (vector::iterator it=maxima2.begin(); it!=maxima2.end(); it++) { + *(max2+k++) = it->u*s; *(max2+k++) = it->v*s; *(max2+k++) = 0; *(max2+k++) = it->c; + *(max2+k++) = it->d1; *(max2+k++) = it->d2; *(max2+k++) = it->d3; *(max2+k++) = it->d4; + *(max2+k++) = it->d5; *(max2+k++) = it->d6; *(max2+k++) = it->d7; *(max2+k++) = it->d8; + } + } +} + +void Matcher::computePriorStatistics (vector &p_matched,int32_t method) { + + // compute number of bins + int32_t u_bin_num = (int32_t)ceil((float)dims_c[0]/(float)param.match_binsize); + int32_t v_bin_num = (int32_t)ceil((float)dims_c[1]/(float)param.match_binsize); + int32_t bin_num = v_bin_num*u_bin_num; + + // number of matching stages + int32_t num_stages = 2; + if (method==2) + num_stages = 4; + + // allocate bin accumulator memory + vector *delta_accu = new vector[bin_num]; + + // fill bin accumulator + Matcher::delta delta_curr; + for (vector::iterator it=p_matched.begin(); it!=p_matched.end(); it++) { + + // method flow: compute position delta + if (method==0) { + delta_curr.val[0] = it->u1p - it->u1c; + delta_curr.val[1] = it->v1p - it->v1c; + delta_curr.val[2] = it->u1c - it->u1p; + delta_curr.val[3] = it->v1c - it->v1p; + + // method stereo: compute position delta + } else if (method==1) { + delta_curr.val[0] = it->u2c - it->u1c; + delta_curr.val[1] = 0; + delta_curr.val[2] = it->u1c - it->u2c; + delta_curr.val[3] = 0; + + // method quad matching: compute position delta + } else { + delta_curr.val[0] = it->u1p - it->u1c; + delta_curr.val[1] = it->v1p - it->v1c; + delta_curr.val[2] = it->u2p - it->u1p; + delta_curr.val[3] = 0; + delta_curr.val[4] = it->u2c - it->u2p; + delta_curr.val[5] = it->v2c - it->v2p; + delta_curr.val[6] = it->u1c - it->u2c; + delta_curr.val[7] = 0; + } + + // compute row and column of bin to which this observation belongs + int32_t u_bin_min = min(max((int32_t)floor(it->u1c/(float)param.match_binsize)-1,0),u_bin_num-1); + int32_t u_bin_max = min(max((int32_t)floor(it->u1c/(float)param.match_binsize)+1,0),u_bin_num-1); + int32_t v_bin_min = min(max((int32_t)floor(it->v1c/(float)param.match_binsize)-1,0),v_bin_num-1); + int32_t v_bin_max = min(max((int32_t)floor(it->v1c/(float)param.match_binsize)+1,0),v_bin_num-1); + + // add to accumulator + for (int32_t v_bin=v_bin_min; v_bin<=v_bin_max; v_bin++) + for (int32_t u_bin=u_bin_min; u_bin<=u_bin_max; u_bin++) + delta_accu[v_bin*u_bin_num+u_bin].push_back(delta_curr); + } + + // clear ranges + ranges.clear(); + + // for all bins compute statistics + for (int32_t v_bin=0; v_bin0) { + + // init displacements 'delta' to 'infinite' + delta_min = delta(+1000000); + delta_max = delta(-1000000); + + // find minimum and maximum displacements + for (vector::iterator it=delta_accu[v_bin*u_bin_num+u_bin].begin(); + it!=delta_accu[v_bin*u_bin_num+u_bin].end(); it++) { + for (int32_t i=0; ival[i]val[i]; + if (it->val[i]>delta_max.val[i]) delta_max.val[i] = it->val[i]; + } + } + } + + // set search range for this bin + range r; + for (int32_t i=0; i *k,const int32_t &u_bin_num,const int32_t &v_bin_num) { + + // descriptor step size + int32_t step_size = sizeof(Matcher::maximum)/sizeof(int32_t); + + // for all points do + for (int32_t i=0; i *k2, + const int32_t &u_bin_num,const int32_t &v_bin_num,const int32_t &stat_bin, + int32_t& min_ind,int32_t stage,bool flow,bool use_prior) { + + // init and load image coordinates + feature + min_ind = 0; + int32_t min_cost = 10000000; + int32_t u1 = *(m1+step_size*i1+0); + int32_t v1 = *(m1+step_size*i1+1); + int32_t c = *(m1+step_size*i1+3); + __m128i xmm1 = _mm_load_si128((__m128i*)(m1+step_size*i1+4)); + __m128i xmm2 = _mm_load_si128((__m128i*)(m1+step_size*i1+8)); + + float u_min,u_max,v_min,v_max; + + // restrict search range with prior + if (use_prior) { + u_min = u1+ranges[stat_bin].u_min[stage]; + u_max = u1+ranges[stat_bin].u_max[stage]; + v_min = v1+ranges[stat_bin].v_min[stage]; + v_max = v1+ranges[stat_bin].v_max[stage]; + + // otherwise: use full search space + } else { + u_min = u1-param.match_radius; + u_max = u1+param.match_radius; + v_min = v1-param.match_radius; + v_max = v1+param.match_radius; + } + + // if stereo search => constrain to 1d + if (!flow) { + v_min = v1-param.match_disp_tolerance; + v_max = v1+param.match_disp_tolerance; + } + + // bins of interest + int32_t u_bin_min = min(max((int32_t)floor(u_min/(float)param.match_binsize),0),u_bin_num-1); + int32_t u_bin_max = min(max((int32_t)floor(u_max/(float)param.match_binsize),0),u_bin_num-1); + int32_t v_bin_min = min(max((int32_t)floor(v_min/(float)param.match_binsize),0),v_bin_num-1); + int32_t v_bin_max = min(max((int32_t)floor(v_max/(float)param.match_binsize),0),v_bin_num-1); + + // for all bins of interest do + for (int32_t u_bin=u_bin_min; u_bin<=u_bin_max; u_bin++) { + for (int32_t v_bin=v_bin_min; v_bin<=v_bin_max; v_bin++) { + int32_t k2_ind = (c*v_bin_num+v_bin)*u_bin_num+u_bin; + for (vector::const_iterator i2_it=k2[k2_ind].begin(); i2_it!=k2[k2_ind].end(); i2_it++) { + int32_t u2 = *(m2+step_size*(*i2_it)+0); + int32_t v2 = *(m2+step_size*(*i2_it)+1); + if (u2>=u_min && u2<=u_max && v2>=v_min && v2<=v_max) { + __m128i xmm3 = _mm_load_si128((__m128i*)(m2+step_size*(*i2_it)+4)); + __m128i xmm4 = _mm_load_si128((__m128i*)(m2+step_size*(*i2_it)+8)); + xmm3 = _mm_sad_epu8 (xmm1,xmm3); + xmm4 = _mm_sad_epu8 (xmm2,xmm4); + xmm4 = _mm_add_epi16(xmm3,xmm4); + int32_t cost = _mm_extract_epi16(xmm4,0)+_mm_extract_epi16(xmm4,4); + if (cost &p_matched,int32_t method,bool use_prior) { + + // descriptor step size (number of int32_t elements in struct) + int32_t step_size = sizeof(Matcher::maximum)/sizeof(int32_t); + + // compute number of bins + int32_t u_bin_num = (int32_t)ceil((float)dims_c[0]/(float)param.match_binsize); + int32_t v_bin_num = (int32_t)ceil((float)dims_c[1]/(float)param.match_binsize); + int32_t bin_num = 4*v_bin_num*u_bin_num; // 4 classes + + // allocate memory for index vectors (needed for efficient search) + vector *k1p = new vector[bin_num]; + vector *k2p = new vector[bin_num]; + vector *k1c = new vector[bin_num]; + vector *k2c = new vector[bin_num]; + + // loop variables + int32_t* M = (int32_t*)calloc(dims_c[0]*dims_c[1],sizeof(int32_t)); + int32_t i1p,i2p,i1c,i2c,i1c2; + int32_t u1p,v1p,u2p,v2p,u1c,v1c,u2c,v2c; + + ///////////////////////////////////////////////////// + // method: flow + if (method==0) { + + // create position/class bin index vectors + createIndexVector(m1p,n1p,k1p,u_bin_num,v_bin_num); + createIndexVector(m1c,n1c,k1c,u_bin_num,v_bin_num); + + // for all points do + for (i1c=0; i1c=u2c) { + + // add match if this pixel isn't matched yet + if (*(M+getAddressOffsetImage(u1c,v1c,dims_c[0]))==0) { + p_matched.push_back(Matcher::p_match(-1,-1,-1,-1,-1,-1,u1c,v1c,i1c,u2c,v2c,i2c)); + *(M+getAddressOffsetImage(u1c,v1c,dims_c[0])) = 1; + } + } + } + } + + ///////////////////////////////////////////////////// + // method: quad matching + } else { + + // create position/class bin index vectors + createIndexVector(m1p,n1p,k1p,u_bin_num,v_bin_num); + createIndexVector(m2p,n2p,k2p,u_bin_num,v_bin_num); + createIndexVector(m1c,n1c,k1c,u_bin_num,v_bin_num); + createIndexVector(m2c,n2c,k2c,u_bin_num,v_bin_num); + + // for all points do + for (i1c=0; i1c=u2p && u1c>=u2c) { + + // add match if this pixel isn't matched yet + if (*(M+getAddressOffsetImage(u1c,v1c,dims_c[0]))==0) { + p_matched.push_back(Matcher::p_match(u1p,v1p,i1p,u2p,v2p,i2p, + u1c,v1c,i1c,u2c,v2c,i2c)); + *(M+getAddressOffsetImage(u1c,v1c,dims_c[0])) = 1; + } + + } + } + } + } + + // free memory + free(M); + delete []k1p; + delete []k2p; + delete []k1c; + delete []k2c; +} + +void Matcher::removeOutliers (vector &p_matched,int32_t method) { + + // do we have enough points for outlier removal? + if (p_matched.size()<=3) + return; + + // input/output structure for triangulation + struct triangulateio in, out; + + // inputs + in.numberofpoints = p_matched.size(); + in.pointlist = (float*)malloc(in.numberofpoints*2*sizeof(float)); + int32_t k=0; + + // create copy of p_matched, init vector with number of support points + // and fill triangle point vector for delaunay triangulation + vector p_matched_copy; + vector num_support; + for (vector::iterator it=p_matched.begin(); it!=p_matched.end(); it++) { + p_matched_copy.push_back(*it); + num_support.push_back(0); + in.pointlist[k++] = it->u1c; + in.pointlist[k++] = it->v1c; + } + + // input parameters + 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) + // attention: not using the B switch or using the n switch creates a memory leak (=> use valgrind!) + char parameters[] = "zQB"; + triangulate(parameters, &in, &out, NULL); + + // for all triangles do + for (int32_t i=0; i=4) + p_matched.push_back(p_matched_copy[i]); + + // free memory used for triangulation + free(in.pointlist); + free(out.pointlist); + free(out.trianglelist); +} + +bool Matcher::parabolicFitting(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1, + const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2, + const float &u1,const float &v1, + float &u2,float &v2, + Matrix At,Matrix AtA, + uint8_t* desc_buffer) { + + // check if parabolic fitting is feasible (descriptors are within margin) + if (u2-3dims2[0]-1-margin || v2-3dims2[1]-1-margin) + return false; + + // compute reference descriptor + __m128i xmm1,xmm2; + computeSmallDescriptor(I1_du,I1_dv,dims1[2],(int32_t)u1,(int32_t)v1,desc_buffer); + xmm1 = _mm_load_si128((__m128i*)(desc_buffer)); + + // compute cost matrix + int32_t cost[49]; + for (int32_t dv=0; dv<7; dv++) { + for (int32_t du=0; du<7; du++) { + computeSmallDescriptor(I2_du,I2_dv,dims2[2],(int32_t)u2+du-3,(int32_t)v2+dv-3,desc_buffer); + xmm2 = _mm_load_si128((__m128i*)(desc_buffer)); + xmm2 = _mm_sad_epu8(xmm1,xmm2); + cost[dv*7+du] = _mm_extract_epi16(xmm2,0)+_mm_extract_epi16(xmm2,4); + } + } + + // compute minimum + int32_t min_ind = 0; + int32_t min_cost = cost[0]; + for (int32_t i=1; i<49; i++) { + if (cost[i] remove this match + if (du==0 || du==6 || dv==0 || dv==6) + return false; + + // solve least squares system + Matrix c(9,1); + for (int32_t i=-1; i<=+1; i++) + for (int32_t j=-1; j<=+1; j++) + c.val[(i+1)*3+(j+1)][0] = cost[(dv+i)*7+(du+j)]; + Matrix b = At*c; + if (!b.solve(AtA)) + return false; + + // extract relative coordinates + float divisor = (b.val[2][0]*b.val[2][0]-4.0*b.val[0][0]*b.val[1][0]); + if (fabs(divisor)<1e-8 || fabs(b.val[2][0])<1e-8) + return false; + float ddv = (2.0*b.val[0][0]*b.val[4][0]-b.val[2][0]*b.val[3][0])/divisor; + float ddu = -(b.val[4][0]+2.0*b.val[1][0]*ddv)/b.val[2][0]; + if (fabs(ddu)>=1.0 || fabs(ddv)>=1.0) + return false; + + // update target + u2 += (float)du-3.0+ddu; + v2 += (float)dv-3.0+ddv; + + // return true on success + return true; +} + +void Matcher::relocateMinimum(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1, + const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2, + const float &u1,const float &v1, + float &u2,float &v2, + uint8_t* desc_buffer) { + + // check if parabolic fitting is feasible (descriptors are within margin) + if (u2-2dims2[0]-1-margin || v2-2dims2[1]-1-margin) + return; + + // compute reference descriptor + __m128i xmm1,xmm2; + computeSmallDescriptor(I1_du,I1_dv,dims1[2],(int32_t)u1,(int32_t)v1,desc_buffer); + xmm1 = _mm_load_si128((__m128i*)(desc_buffer)); + + // compute cost matrix + int32_t cost[25]; + for (int32_t dv=0; dv<5; dv++) { + for (int32_t du=0; du<5; du++) { + computeSmallDescriptor(I2_du,I2_dv,dims2[2],(int32_t)u2+du-2,(int32_t)v2+dv-2,desc_buffer); + xmm2 = _mm_load_si128((__m128i*)(desc_buffer)); + xmm2 = _mm_sad_epu8(xmm1,xmm2); + cost[dv*5+du] = _mm_extract_epi16(xmm2,0)+_mm_extract_epi16(xmm2,4); + } + } + + // compute minimum + int32_t min_ind = 0; + int32_t min_cost = cost[0]; + for (int32_t i=1; i<25; i++) { + if (cost[i] &p_matched,int32_t method) { + + // allocate aligned memory (32 bytes for 1 descriptors) + uint8_t* desc_buffer = (uint8_t*)_mm_malloc(32*sizeof(uint8_t),16); + + // copy vector (for refill) + vector p_matched_copy = p_matched; + p_matched.clear(); + + // create matrices for least square fitting + FLOAT A_data[9*6] = { 1, 1, 1,-1,-1, 1, + 0, 1, 0, 0,-1, 1, + 1, 1,-1, 1,-1, 1, + 1, 0, 0,-1, 0, 1, + 0, 0, 0, 0, 0, 1, + 1, 0, 0, 1, 0, 1, + 1, 1,-1,-1, 1, 1, + 0, 1, 0, 0, 1, 1, + 1, 1, 1, 1, 1, 1}; + Matrix A(9,6,A_data); + Matrix At = ~A; + Matrix AtA = At*A; + + uint8_t* I1p_du_fit = I1p_du; + uint8_t* I1p_dv_fit = I1p_dv; + uint8_t* I2p_du_fit = I2p_du; + uint8_t* I2p_dv_fit = I2p_dv; + uint8_t* I1c_du_fit = I1c_du; + uint8_t* I1c_dv_fit = I1c_dv; + uint8_t* I2c_du_fit = I2c_du; + uint8_t* I2c_dv_fit = I2c_dv; + if (param.half_resolution) { + I1p_du_fit = I1p_du_full; + I1p_dv_fit = I1p_dv_full; + I2p_du_fit = I2p_du_full; + I2p_dv_fit = I2p_dv_full; + I1c_du_fit = I1c_du_full; + I1c_dv_fit = I1c_dv_full; + I2c_du_fit = I2c_du_full; + I2c_dv_fit = I2c_dv_full; + } + + // for all matches do + for (vector::iterator it=p_matched_copy.begin(); it!=p_matched_copy.end(); it++) { + + // method: flow or quad matching + if (method==0 || method==2) { + if (param.refinement==2) { + if (!parabolicFitting(I1c_du_fit,I1c_dv_fit,dims_c,I1p_du_fit,I1p_dv_fit,dims_p, + it->u1c,it->v1c,it->u1p,it->v1p,At,AtA,desc_buffer)) + continue; + } else { + relocateMinimum(I1c_du_fit,I1c_dv_fit,dims_c,I1p_du_fit,I1p_dv_fit,dims_p, + it->u1c,it->v1c,it->u1p,it->v1p,desc_buffer); + } + } + + // method: stereo or quad matching + if (method==1 || method==2) { + if (param.refinement==2) { + if (!parabolicFitting(I1c_du_fit,I1c_dv_fit,dims_c,I2c_du_fit,I2c_dv_fit,dims_c, + it->u1c,it->v1c,it->u2c,it->v2c,At,AtA,desc_buffer)) + continue; + } else { + relocateMinimum(I1c_du_fit,I1c_dv_fit,dims_c,I2c_du_fit,I2c_dv_fit,dims_c, + it->u1c,it->v1c,it->u2c,it->v2c,desc_buffer); + } + } + + // method: quad matching + if (method==2) { + if (param.refinement==2) { + if (!parabolicFitting(I1c_du_fit,I1c_dv_fit,dims_c,I2p_du_fit,I2p_dv_fit,dims_p, + it->u1c,it->v1c,it->u2p,it->v2p,At,AtA,desc_buffer)) + continue; + } else { + relocateMinimum(I1c_du_fit,I1c_dv_fit,dims_c,I2p_du_fit,I2p_dv_fit,dims_p, + it->u1c,it->v1c,it->u2p,it->v2p,desc_buffer); + } + } + + // add this match + p_matched.push_back(*it); + } + + // free memory + _mm_free(desc_buffer); +} + +float Matcher::mean(const uint8_t* I,const int32_t &bpl,const int32_t &u_min,const int32_t &u_max,const int32_t &v_min,const int32_t &v_max) { + float mean; + for (int32_t v=v_min; v<=v_max; v++) + for (int32_t u=u_min; u<=u_max; u++) + mean += (float)*(I+getAddressOffsetImage(u,v,bpl)); + return + mean /= (float)((u_max-u_min+1)*(v_max-v_min+1)); +} diff --git a/src/lib/matrix.cpp b/src/lib/matrix.cpp new file mode 100644 index 0000000..1c888e3 --- /dev/null +++ b/src/lib/matrix.cpp @@ -0,0 +1,850 @@ +/* +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 + +#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; i0) + for (int32_t i=0; i=m || j1<0 || j2>=n || i2m || 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 idx) { + Matrix M(m,idx.size()); + for (int32_t j=0; j1 && M.n==1) { + Matrix D(M.m,M.m); + for (int32_t i=0; i1) { + Matrix D(M.n,M.n); + for (int32_t i=0; i=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=0;l--) { + if (indxr[l]!=indxc[l]) + for (k=0;kbig) + 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=big) { + big = dum; + imax = i; + } + } + if (j!=imax) { // Do we need to interchange rows? + for (k=0; k=0;i--) { // Accumulation of right-hand transformations. + if (i=0;i--) { // Accumulation of left-hand transformations. + l = i+1; + g = w[i]; + for (j=l;j=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 1); + for (k=0;k (m+n)/2) { + for (i=0;i 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/src/lib/reconstruction.cpp b/src/lib/reconstruction.cpp new file mode 100644 index 0000000..18823e5 --- /dev/null +++ b/src/lib/reconstruction.cpp @@ -0,0 +1,389 @@ +/* +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 "reconstruction.h" +#include + +using namespace std; + +Reconstruction::Reconstruction () { + K = Matrix::eye(3); +} + +Reconstruction::~Reconstruction () { +} + +void Reconstruction::setCalibration (FLOAT f,FLOAT cu,FLOAT cv) { + FLOAT K_data[9] = {f,0,cu,0,f,cv,0,0,1}; + K = Matrix(3,3,K_data); + FLOAT cam_pitch = -0.08; + FLOAT cam_height = 1.6; + Tr_cam_road = Matrix(4,4); + Tr_cam_road.val[0][0] = 1; + Tr_cam_road.val[1][1] = +cos(cam_pitch); + Tr_cam_road.val[1][2] = -sin(cam_pitch); + Tr_cam_road.val[2][1] = +sin(cam_pitch); + Tr_cam_road.val[2][2] = +cos(cam_pitch); + Tr_cam_road.val[0][3] = 0; + Tr_cam_road.val[1][3] = -cam_height; + Tr_cam_road.val[2][3] = 0; +} + +void Reconstruction::update (vector p_matched,Matrix Tr,int32_t point_type,int32_t min_track_length,double max_dist,double min_angle) { + + // update transformation vector + Matrix Tr_total_curr; + if (Tr_total.size()==0) Tr_total_curr = Matrix::inv(Tr); + else Tr_total_curr = Tr_total.back()*Matrix::inv(Tr); + Tr_total.push_back(Tr_total_curr); + Tr_inv_total.push_back(Matrix::inv(Tr_total_curr)); + + // update projection vector + Matrix P_total_curr = K*Matrix::inv(Tr_total_curr).getMat(0,0,2,3); + P_total.push_back(P_total_curr); + + // current frame + int32_t current_frame = Tr_total.size(); + + // create index vector + int32_t track_idx_max = 0; + for (vector::iterator m=p_matched.begin(); m!=p_matched.end(); m++) + if (m->i1p > track_idx_max) + track_idx_max = m->i1p; + for (vector::iterator t=tracks.begin(); t!=tracks.end(); t++) + if (t->last_idx > track_idx_max) + track_idx_max = t->last_idx; + int32_t *track_idx = new int32_t[track_idx_max+1]; + for (int32_t i=0; i<=track_idx_max; i++) + track_idx[i] = -1; + for (int32_t i=0; i::iterator m=p_matched.begin(); m!=p_matched.end(); m++) { + + // track index (-1 = no existing track) + int32_t idx = track_idx[m->i1p]; + + // add to existing track + if (idx>=0 && tracks[idx].last_frame==current_frame-1) { + + tracks[idx].pixels.push_back(point2d(m->u1c,m->v1c)); + tracks[idx].last_frame = current_frame; + tracks[idx].last_idx = m->i1c; + + // create new track + } else { + track t; + t.pixels.push_back(point2d(m->u1p,m->v1p)); + t.pixels.push_back(point2d(m->u1c,m->v1c)); + t.first_frame = current_frame-1; + t.last_frame = current_frame; + t.last_idx = m->i1c; + tracks.push_back(t); + } + } + + // copy tracks + vector tracks_copy = tracks; + tracks.clear(); + + // devise tracks into active or reconstruct 3d points + for (vector::iterator t=tracks_copy.begin(); t!=tracks_copy.end(); t++) { + + // track has been extended + if (t->last_frame==current_frame) { + + // push back to active tracks + tracks.push_back(*t); + + // track lost + } else { + + // add to 3d reconstruction + if (t->pixels.size()>=min_track_length) { + + // 3d point + point3d p; + + // try to init point from first and last track frame + if (initPoint(*t,p)) { + if (pointType(*t,p)>=point_type) + if (refinePoint(*t,p)) + if(pointDistance(*t,p)min_angle) + points.push_back(p); + } + } + } + } + + //cout << "P: " << points.size() << endl; + //testJacobian(); + + delete track_idx; +} + +bool Reconstruction::initPoint(const track &t,point3d &p) { + + // projection matrices + Matrix P1 = P_total[t.first_frame]; + Matrix P2 = P_total[t.last_frame]; + + // observations + point2d p1 = t.pixels.front(); + point2d p2 = t.pixels.back(); + + // triangulation via orthogonal regression + Matrix J(4,4); + Matrix U,S,V; + for (int32_t j=0; j<4; j++) { + J.val[0][j] = P1.val[2][j]*p1.u - P1.val[0][j]; + J.val[1][j] = P1.val[2][j]*p1.v - P1.val[1][j]; + J.val[2][j] = P2.val[2][j]*p2.u - P2.val[0][j]; + J.val[3][j] = P2.val[2][j]*p2.v - P2.val[1][j]; + } + J.svd(U,S,V); + + // return false if this point is at infinity + float w = V.val[3][3]; + if (fabs(w)<1e-10) + return false; + + // return 3d point + p = point3d(V.val[0][3]/w,V.val[1][3]/w,V.val[2][3]/w); + return true; +} + +bool Reconstruction::refinePoint(const track &t,point3d &p) { + + int32_t num_frames = t.pixels.size(); + J = new FLOAT[6*num_frames]; + p_observe = new FLOAT[2*num_frames]; + p_predict = new FLOAT[2*num_frames]; + + int32_t iter=0; + Reconstruction::result result = UPDATED; + while (result==UPDATED) { + result = updatePoint(t,p,1,1e-5); + if (iter++ > 20 || result==CONVERGED) + break; + } + + delete J; + delete p_observe; + delete p_predict; + + if (result==CONVERGED) + return true; + else + return false; +} + +double Reconstruction::pointDistance(const track &t,point3d &p) { + int32_t mid_frame = (t.first_frame+t.last_frame)/2; + double dx = Tr_total[mid_frame].val[0][3]-p.x; + double dy = Tr_total[mid_frame].val[1][3]-p.y; + double dz = Tr_total[mid_frame].val[2][3]-p.z; + return sqrt(dx*dx+dy*dy+dz*dz); +} + +double Reconstruction::rayAngle(const track &t,point3d &p) { + Matrix c1 = Tr_total[t.first_frame].getMat(0,3,2,3); + Matrix c2 = Tr_total[t.last_frame].getMat(0,3,2,3); + Matrix pt(3,1); + pt.val[0][0] = p.x; + pt.val[1][0] = p.y; + pt.val[2][0] = p.z; + Matrix v1 = c1-pt; + Matrix v2 = c2-pt; + FLOAT n1 = v1.l2norm(); + FLOAT n2 = v2.l2norm(); + if (n1<1e-10 || n2<1e-10) + return 1000; + v1 = v1/n1; + v2 = v2/n2; + return acos(fabs((~v1*v2).val[0][0]))*180.0/M_PI; +} + +int32_t Reconstruction::pointType(const track &t,point3d &p) { + + // project point to first and last camera coordinates + Matrix x(4,1); + x.val[0][0] = p.x; + x.val[1][0] = p.y; + x.val[2][0] = p.z; + x.val[3][0] = 1; + Matrix x1c = Tr_inv_total[t.first_frame]*x; + Matrix x2c = Tr_inv_total[t.last_frame]*x; + Matrix x2r = Tr_cam_road*x2c; + + // point not visible + if (x1c.val[2][0]<=1 || x2c.val[2][0]<=1) + return -1; + + // below road + if (x2r.val[1][0]>0.5) + return 0; + + // road + if (x2r.val[1][0]>-1) + return 1; + + // obstacle + return 2; +} + +Reconstruction::result Reconstruction::updatePoint(const track &t,point3d &p,const FLOAT &step_size,const FLOAT &eps) { + + // number of frames + int32_t num_frames = t.pixels.size(); + + // extract observations + computeObservations(t.pixels); + + // compute predictions + vector::iterator P_begin = P_total.begin()+t.first_frame; + vector::iterator P_end = P_begin+num_frames-1; + + if (!computePredictionsAndJacobian(P_begin,P_end,p)) + return FAILED; + + // init + Matrix A(3,3); + Matrix B(3,1); + + // fill matrices A and B + for (int32_t m=0; m<3; m++) { + for (int32_t n=0; n<3; n++) { + FLOAT a = 0; + for (int32_t i=0; i<2*num_frames; i++) + a += J[i*3+m]*J[i*3+n]; + A.val[m][n] = a; + } + FLOAT b = 0; + for (int32_t i=0; i<2*num_frames; i++) + b += J[i*3+m]*(p_observe[i]-p_predict[i]); + B.val[m][0] = b; + } + + // perform elimination + if (B.solve(A)) { + p.x += step_size*B.val[0][0]; + p.y += step_size*B.val[1][0]; + p.z += step_size*B.val[2][0]; + if (fabs(B.val[0][0]) &pixels) { + for (int32_t i=0; i::iterator &P_begin,const vector::iterator &P_end,point3d &p) { + + // for all frames do + int32_t k=0; + for (vector::iterator P=P_begin; P<=P_end; P++) { + + // precompute coefficients + FLOAT a = P->val[0][0]*p.x+P->val[0][1]*p.y+P->val[0][2]*p.z+P->val[0][3]; + FLOAT b = P->val[1][0]*p.x+P->val[1][1]*p.y+P->val[1][2]*p.z+P->val[1][3]; + FLOAT c = P->val[2][0]*p.x+P->val[2][1]*p.y+P->val[2][2]*p.z+P->val[2][3]; + FLOAT cc = c*c; + + // check singularities + if (cc<1e-10) + return false; + + // set jacobian entries + J[k*6+0] = (P->val[0][0]*c-P->val[2][0]*a)/cc; + J[k*6+1] = (P->val[0][1]*c-P->val[2][1]*a)/cc; + J[k*6+2] = (P->val[0][2]*c-P->val[2][2]*a)/cc; + J[k*6+3] = (P->val[1][0]*c-P->val[2][0]*b)/cc; + J[k*6+4] = (P->val[1][1]*c-P->val[2][1]*b)/cc; + J[k*6+5] = (P->val[1][2]*c-P->val[2][2]*b)/cc; + + // set prediction + p_predict[k*2+0] = a/c; // u + p_predict[k*2+1] = b/c; // v + + k++; + } + + // success + return true; +} + +void Reconstruction::testJacobian() { + cout << "=================================" << endl; + cout << "TESTING JACOBIAN" << endl; + FLOAT delta = 1e-5; + vector P; + Matrix A(3,4); + A.setMat(K,0,0); + P.push_back(A); + A.setMat(Matrix::rotMatX(0.1)*Matrix::rotMatY(0.1)*Matrix::rotMatZ(0.1),0,0); + A.val[1][3] = 1; + A.val[1][3] = 0.1; + A.val[1][3] = -1.5; + P.push_back(K*A); + cout << P[0] << endll; + cout << P[1] << endll; + J = new FLOAT[6*2]; + p_observe = new FLOAT[2*2]; + p_predict = new FLOAT[2*2]; + + point3d p_ref(0.1,0.2,0.3); + + FLOAT p_predict1[4]; + FLOAT p_predict2[4]; + point3d p1 = p_ref; + + for (int32_t i=0; i<3; i++) { + point3d p2 = p_ref; + if (i==0) p2.x += delta; + else if (i==1) p2.y += delta; + else p2.z += delta; + cout << endl << "Checking parameter " << i << ":" << endl; + cout << "param1: "; cout << p1.x << " " << p1.y << " " << p1.z << endl; + cout << "param2: "; cout << p2.x << " " << p2.y << " " << p2.z << endl; + computePredictionsAndJacobian(P.begin(),P.end(),p1); + memcpy(p_predict1,p_predict,4*sizeof(FLOAT)); + computePredictionsAndJacobian(P.begin(),P.end(),p2); + memcpy(p_predict2,p_predict,4*sizeof(FLOAT)); + for (int32_t j=0; j<4; j++) { + cout << "num: " << (p_predict2[j]-p_predict1[j])/delta; + cout << ", ana: " << J[j*3+i] << endl; + } + } + + delete J; + delete p_observe; + delete p_predict; + cout << "=================================" << endl; +} diff --git a/src/lib/triangle.cpp b/src/lib/triangle.cpp new file mode 100644 index 0000000..c2ba424 --- /dev/null +++ b/src/lib/triangle.cpp @@ -0,0 +1,8634 @@ +/*****************************************************************************/ +/* */ +/* 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, 2010 */ +/* */ +/* 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 +#include +#include +#include + +#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 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) (ptr) & (unsigned long) 3l); \ + (otri).tri = (triangle *) \ + ((unsigned long) (ptr) ^ (unsigned 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) (otri).tri | (unsigned 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) (otri).tri[6] | (unsigned long) 2l) + +#define uninfect(otri) \ + (otri).tri[6] = (triangle) \ + ((unsigned long) (otri).tri[6] & ~ (unsigned long) 2l) + +/* Test a triangle for viral infection. */ + +#define infected(otri) \ + (((unsigned long) (otri).tri[6] & (unsigned 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) (sptr) & (unsigned long) 1l); \ + (osub).ss = (subseg *) \ + ((unsigned long) (sptr) & ~ (unsigned 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) (osub).ss | (unsigned 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; + + 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 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) (pool->nowblock + 1); + /* Align the item on an `alignbytes'-byte boundary. */ + pool->nextitem = (int *) + (alignptr + (unsigned long) pool->alignbytes - + (alignptr % (unsigned 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 > (int)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 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) (pool->nowblock + 1); + /* Align the item on an `alignbytes'-byte boundary. */ + pool->nextitem = (int *) + (alignptr + (unsigned long) pool->alignbytes - + (alignptr % (unsigned 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 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) (pool->pathblock + 1); + /* Align with item on an `alignbytes'-byte boundary. */ + pool->pathitem = (int *) + (alignptr + (unsigned long) pool->alignbytes - + (alignptr % (unsigned 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 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) (pool->pathblock + 1); + /* Align with item on an `alignbytes'-byte boundary. */ + pool->pathitem = (int *) + (alignptr + (unsigned long) pool->alignbytes - + (alignptr % (unsigned 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 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) m->dummytribase; + m->dummytri = (triangle *) + (alignptr + (unsigned long) m->triangles.alignbytes - + (alignptr % (unsigned 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) m->dummysubbase; + m->dummysub = (subseg *) + (alignptr + (unsigned long) m->subsegs.alignbytes - + (alignptr % (unsigned 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 * (int)sizeof(triangle) + (int)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 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) (getblock + 1); + foundvertex = (char *) (alignptr + (unsigned long) m->vertices.alignbytes - + (alignptr % (unsigned 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 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 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) (sampleblock + 1); + firsttri = (char *) (alignptr + + (unsigned long) m->triangles.alignbytes - + (alignptr % + (unsigned 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/src/lib/viso.cpp b/src/lib/viso.cpp new file mode 100644 index 0000000..8a7b109 --- /dev/null +++ b/src/lib/viso.cpp @@ -0,0 +1,114 @@ +/* +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 "viso.h" + +#include + +using namespace std; + +VisualOdometry::VisualOdometry (parameters param) : param(param) { + J = 0; + p_observe = 0; + p_predict = 0; + matcher = new Matcher(param.match); + Tr_delta = Matrix::eye(4); + srand(0); +} + +VisualOdometry::~VisualOdometry () { + delete matcher; +} + +bool VisualOdometry::updateMotion () { + + // bucketing + matcher->bucketFeatures(param.bucket.max_features, + param.bucket.bucket_width, + param.bucket.bucket_height); + + // get feature matches + p_matched = matcher->getMatches(); + + // estimate motion + vector tr_delta = estimateMotion(p_matched); + + // on failure + if (tr_delta.size()!=6) + return false; + + // set transformation matrix (previous to current frame) + Tr_delta = transformationVectorToMatrix(tr_delta); + + // success + return true; +} + +Matrix VisualOdometry::transformationVectorToMatrix (vector tr) { + + // extract parameters + double rx = tr[0]; + double ry = tr[1]; + double rz = tr[2]; + double tx = tr[3]; + double ty = tr[4]; + double tz = tr[5]; + + // precompute sine/cosine + double sx = sin(rx); + double cx = cos(rx); + double sy = sin(ry); + double cy = cos(ry); + double sz = sin(rz); + double cz = cos(rz); + + // compute transformation + Matrix Tr(4,4); + Tr.val[0][0] = +cy*cz; Tr.val[0][1] = -cy*sz; Tr.val[0][2] = +sy; Tr.val[0][3] = tx; + Tr.val[1][0] = +sx*sy*cz+cx*sz; Tr.val[1][1] = -sx*sy*sz+cx*cz; Tr.val[1][2] = -sx*cy; Tr.val[1][3] = ty; + Tr.val[2][0] = -cx*sy*cz+sx*sz; Tr.val[2][1] = +cx*sy*sz+sx*cz; Tr.val[2][2] = +cx*cy; Tr.val[2][3] = tz; + Tr.val[3][0] = 0; Tr.val[3][1] = 0; Tr.val[3][2] = 0; Tr.val[3][3] = 1; + return Tr; +} + +vector VisualOdometry::getRandomSample(int32_t N,int32_t num) { + + // init sample and totalset + vector sample; + vector totalset; + + // create vector containing all indices + for (int32_t i=0; ipushBack(I,dims,replace); + matcher->matchFeatures(0); + return updateMotion(); +} + +vector VisualOdometryMono::estimateMotion (vector p_matched) { + + // get number of matches + int32_t N = p_matched.size(); + if (N<10) + return vector(); + + // create calibration matrix + double K_data[9] = {param.calib.f,0,param.calib.cu,0,param.calib.f,param.calib.cv,0,0,1}; + Matrix K(3,3,K_data); + + // normalize feature points and return on errors + Matrix Tp,Tc; + vector p_matched_normalized = p_matched; + if (!normalizeFeaturePoints(p_matched_normalized,Tp,Tc)) + return vector(); + + // initial RANSAC estimate of F + Matrix E,F; + inliers.clear(); + for (int32_t k=0;k active = getRandomSample(N,8); + + // estimate fundamental matrix and get inliers + fundamentalMatrix(p_matched_normalized,active,F); + vector inliers_curr = getInlier(p_matched_normalized,F); + + // update model if we are better + if (inliers_curr.size()>inliers.size()) + inliers = inliers_curr; + } + + // are there enough inliers? + if (inliers.size()<10) + return vector(); + + // refine F using all inliers + fundamentalMatrix(p_matched_normalized,inliers,F); + + // denormalise and extract essential matrix + F = ~Tc*F*Tp; + E = ~K*F*K; + + // re-enforce rank 2 constraint on essential matrix + Matrix U,W,V; + E.svd(U,W,V); + W.val[2][0] = 0; + E = U*Matrix::diag(W)*~V; + + // compute 3d points X and R|t up to scale + Matrix X,R,t; + EtoRt(E,K,p_matched,inliers,X,R,t); + + // normalize 3d points and remove points behind image plane + X = X/X.getMat(3,0,3,-1); + vector pos_idx; + for (int32_t i=0; i0) + pos_idx.push_back(i); + Matrix X_plane = X.extractCols(pos_idx); + + // we need at least 10 points to proceed + if (X_plane.n<10) + return vector(); + + // get elements closer than median + double median; + smallerThanMedian(X_plane,median); + + // return error on large median (litte motion) + if (median>param.motion_threshold) + return vector(); + + // project features to 2d + Matrix x_plane(2,X_plane.n); + x_plane.setMat(X_plane.getMat(1,0,2,-1),0,0); + + Matrix n(2,1); + n.val[0][0] = cos(-param.pitch); + n.val[1][0] = sin(-param.pitch); + Matrix d = ~n*x_plane; + double sigma = median/50.0; + double weight = 1.0/(2.0*sigma*sigma); + double best_sum = 0; + int32_t best_idx = 0; + + // find best plane + for (int32_t i=0; imedian/param.motion_threshold) { + double sum = 0; + for (int32_t j=0; jbest_sum) { + best_sum = sum; + best_idx = i; + } + } + } + t = t*param.height/d.val[0][best_idx]; + + // compute rotation angles + double ry = asin(R.val[0][2]); + double rx = asin(-R.val[1][2]/cos(ry)); + double rz = asin(-R.val[0][1]/cos(ry)); + + // return parameter vector + vector tr_delta; + tr_delta.resize(6); + tr_delta[0] = rx; + tr_delta[1] = ry; + tr_delta[2] = rz; + tr_delta[3] = t.val[0][0]; + tr_delta[4] = t.val[1][0]; + tr_delta[5] = t.val[2][0]; + return tr_delta; +} + +Matrix VisualOdometryMono::smallerThanMedian (Matrix &X,double &median) { + + // set distance and index vector + vector dist; + vector idx; + for (int32_t i=0; i&>(dist)); + + // get median + int32_t num_elem_half = idx.size()/2; + median = dist[idx[num_elem_half]]; + + // create matrix containing elements closer than median + Matrix X_small(4,num_elem_half+1); + for (int32_t j=0; j<=num_elem_half; j++) + for (int32_t i=0; i<4; i++) + X_small.val[i][j] = X.val[i][idx[j]]; + return X_small; +} + +bool VisualOdometryMono::normalizeFeaturePoints(vector &p_matched,Matrix &Tp,Matrix &Tc) { + + // shift origins to centroids + double cpu=0,cpv=0,ccu=0,ccv=0; + for (vector::iterator it = p_matched.begin(); it!=p_matched.end(); it++) { + cpu += it->u1p; + cpv += it->v1p; + ccu += it->u1c; + ccv += it->v1c; + } + cpu /= (double)p_matched.size(); + cpv /= (double)p_matched.size(); + ccu /= (double)p_matched.size(); + ccv /= (double)p_matched.size(); + for (vector::iterator it = p_matched.begin(); it!=p_matched.end(); it++) { + it->u1p -= cpu; + it->v1p -= cpv; + it->u1c -= ccu; + it->v1c -= ccv; + } + + // scale features such that mean distance from origin is sqrt(2) + double sp=0,sc=0; + for (vector::iterator it = p_matched.begin(); it!=p_matched.end(); it++) { + sp += sqrt(it->u1p*it->u1p+it->v1p*it->v1p); + sc += sqrt(it->u1c*it->u1c+it->v1c*it->v1c); + } + if (fabs(sp)<1e-10 || fabs(sc)<1e-10) + return false; + sp = sqrt(2.0)*(double)p_matched.size()/sp; + sc = sqrt(2.0)*(double)p_matched.size()/sc; + for (vector::iterator it = p_matched.begin(); it!=p_matched.end(); it++) { + it->u1p *= sp; + it->v1p *= sp; + it->u1c *= sc; + it->v1c *= sc; + } + + // compute corresponding transformation matrices + double Tp_data[9] = {sp,0,-sp*cpu,0,sp,-sp*cpv,0,0,1}; + double Tc_data[9] = {sc,0,-sc*ccu,0,sc,-sc*ccv,0,0,1}; + Tp = Matrix(3,3,Tp_data); + Tc = Matrix(3,3,Tc_data); + + // return true on success + return true; +} + +void VisualOdometryMono::fundamentalMatrix (const vector &p_matched,const vector &active,Matrix &F) { + + // number of active p_matched + int32_t N = active.size(); + + // create constraint matrix A + Matrix A(N,9); + for (int32_t i=0; i VisualOdometryMono::getInlier (vector &p_matched,Matrix &F) { + + // extract fundamental matrix + double f00 = F.val[0][0]; double f01 = F.val[0][1]; double f02 = F.val[0][2]; + double f10 = F.val[1][0]; double f11 = F.val[1][1]; double f12 = F.val[1][2]; + double f20 = F.val[2][0]; double f21 = F.val[2][1]; double f22 = F.val[2][2]; + + // loop variables + double u1,v1,u2,v2; + double x2tFx1; + double Fx1u,Fx1v,Fx1w; + double Ftx2u,Ftx2v; + + // vector with inliers + vector inliers; + + // for all matches do + for (int32_t i=0; i<(int32_t)p_matched.size(); i++) { + + // extract matches + u1 = p_matched[i].u1p; + v1 = p_matched[i].v1p; + u2 = p_matched[i].u1c; + v2 = p_matched[i].v1c; + + // F*x1 + Fx1u = f00*u1+f01*v1+f02; + Fx1v = f10*u1+f11*v1+f12; + Fx1w = f20*u1+f21*v1+f22; + + // F'*x2 + Ftx2u = f00*u2+f10*v2+f20; + Ftx2v = f01*u2+f11*v2+f21; + + // x2'*F*x1 + x2tFx1 = u2*Fx1u+v2*Fx1v+Fx1w; + + // sampson distance + double d = x2tFx1*x2tFx1 / (Fx1u*Fx1u+Fx1v*Fx1v+Ftx2u*Ftx2u+Ftx2v*Ftx2v); + + // check threshold + if (fabs(d) &p_matched,vector &inliers,Matrix &X,Matrix &R,Matrix &t) { + + // hartley matrices + double W_data[9] = {0,-1,0,+1,0,0,0,0,1}; + double Z_data[9] = {0,+1,0,-1,0,0,0,0,0}; + Matrix W(3,3,W_data); + Matrix Z(3,3,Z_data); + + // extract T,R1,R2 (8 solutions) + Matrix U,S,V; + E.svd(U,S,V); + Matrix T = U*Z*~U; + Matrix Ra = U*W*(~V); + Matrix Rb = U*(~W)*(~V); + + // convert T to t + t = Matrix(3,1); + t.val[0][0] = T.val[2][1]; + t.val[1][0] = T.val[0][2]; + t.val[2][0] = T.val[1][0]; + + // assure determinant to be positive + if (Ra.det()<0) Ra = -Ra; + if (Rb.det()<0) Rb = -Rb; + + // create vector containing all 4 solutions + vector R_vec; + vector t_vec; + R_vec.push_back(Ra); t_vec.push_back( t); + R_vec.push_back(Ra); t_vec.push_back(-t); + R_vec.push_back(Rb); t_vec.push_back( t); + R_vec.push_back(Rb); t_vec.push_back(-t); + + // try all 4 solutions + Matrix X_curr; + int32_t max_inliers = 0; + for (int32_t i=0; i<4; i++) { + int32_t num_inliers = triangulateChieral(p_matched,K,R_vec[i],t_vec[i],X_curr); + if (num_inliers>max_inliers) { + max_inliers = num_inliers; + X = X_curr; + R = R_vec[i]; + t = t_vec[i]; + } + } +} + +int32_t VisualOdometryMono::triangulateChieral (vector &p_matched,Matrix &K,Matrix &R,Matrix &t,Matrix &X) { + + // init 3d point matrix + X = Matrix(4,p_matched.size()); + + // projection matrices + Matrix P1(3,4); + Matrix P2(3,4); + P1.setMat(K,0,0); + P2.setMat(R,0,0); + P2.setMat(t,0,3); + P2 = K*P2; + + // triangulation via orthogonal regression + Matrix J(4,4); + Matrix U,S,V; + for (int32_t i=0; i<(int)p_matched.size(); i++) { + for (int32_t j=0; j<4; j++) { + J.val[0][j] = P1.val[2][j]*p_matched[i].u1p - P1.val[0][j]; + J.val[1][j] = P1.val[2][j]*p_matched[i].v1p - P1.val[1][j]; + J.val[2][j] = P2.val[2][j]*p_matched[i].u1c - P2.val[0][j]; + J.val[3][j] = P2.val[2][j]*p_matched[i].v1c - P2.val[1][j]; + } + J.svd(U,S,V); + X.setMat(V.getMat(0,3,3,3),0,i); + } + + // compute inliers + Matrix AX1 = P1*X; + Matrix BX1 = P2*X; + int32_t num = 0; + for (int32_t i=0; i0 && BX1.val[2][i]*X.val[3][i]>0) + num++; + + // return number of inliers + return num; +} + diff --git a/src/lib/viso_stereo.cpp b/src/lib/viso_stereo.cpp new file mode 100644 index 0000000..f286728 --- /dev/null +++ b/src/lib/viso_stereo.cpp @@ -0,0 +1,313 @@ +/* +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 "viso_stereo.h" +#include "timer.h" + +using namespace std; + +VisualOdometryStereo::VisualOdometryStereo (parameters param) : param(param), VisualOdometry(param) { +} + +VisualOdometryStereo::~VisualOdometryStereo() { +} + +bool VisualOdometryStereo::process (uint8_t *I1,uint8_t *I2,int32_t* dims,bool replace) { + matcher->pushBack(I1,I2,dims,replace); + matcher->matchFeatures(2); + return updateMotion(); +} + +vector VisualOdometryStereo::estimateMotion (vector p_matched) { + + // return value + bool success = true; + + // compute minimum distance for RANSAC samples + double width=0,height=0; + for (vector::iterator it=p_matched.begin(); it!=p_matched.end(); it++) { + if (it->u1c>width) width = it->u1c; + if (it->v1c>height) height = it->v1c; + } + double min_dist = min(width,height)/3.0; + + // get number of matches + int32_t N = p_matched.size(); + if (N<6) + return vector(); + + // allocate dynamic memory + X = new double[N]; + Y = new double[N]; + Z = new double[N]; + J = new double[4*N*6]; + p_predict = new double[4*N]; + p_observe = new double[4*N]; + p_residual = new double[4*N]; + + // project matches of previous image into 3d + for (int32_t i=0; i tr_delta; + vector tr_delta_curr; + tr_delta_curr.resize(6); + + // clear parameter vector + inliers.clear(); + + // initial RANSAC estimate + for (int32_t k=0;k active = getRandomSample(N,3); + + // clear parameter vector + for (int32_t i=0; i<6; i++) + tr_delta_curr[i] = 0; + + // minimize reprojection errors + VisualOdometryStereo::result result = UPDATED; + int32_t iter=0; + while (result==UPDATED) { + result = updateParameters(p_matched,active,tr_delta_curr,1,1e-6); + if (iter++ > 20 || result==CONVERGED) + break; + } + + // overwrite best parameters if we have more inliers + if (result!=FAILED) { + vector inliers_curr = getInlier(p_matched,tr_delta_curr); + if (inliers_curr.size()>inliers.size()) { + inliers = inliers_curr; + tr_delta = tr_delta_curr; + } + } + } + + // final optimization (refinement) + if (inliers.size()>=6) { + int32_t iter=0; + VisualOdometryStereo::result result = UPDATED; + while (result==UPDATED) { + result = updateParameters(p_matched,inliers,tr_delta,1,1e-8); + if (iter++ > 100 || result==CONVERGED) + break; + } + + // not converged + if (result!=CONVERGED) + success = false; + + // not enough inliers + } else { + success = false; + } + + // release dynamic memory + delete X; + delete Y; + delete Z; + delete J; + delete p_predict; + delete p_observe; + delete p_residual; + + // parameter estimate succeeded? + if (success) return tr_delta; + else return vector(); +} + +vector VisualOdometryStereo::getInlier(vector &p_matched,vector &tr) { + + // mark all observations active + vector active; + for (int32_t i=0; i<(int32_t)p_matched.size(); i++) + active.push_back(i); + + // extract observations and compute predictions + computeObservations(p_matched,active); + computeResidualsAndJacobian(tr,active); + + // compute inliers + vector inliers; + for (int32_t i=0; i<(int32_t)p_matched.size(); i++) + if (pow(p_observe[4*i+0]-p_predict[4*i+0],2)+pow(p_observe[4*i+1]-p_predict[4*i+1],2) + + pow(p_observe[4*i+2]-p_predict[4*i+2],2)+pow(p_observe[4*i+3]-p_predict[4*i+3],2) < param.inlier_threshold*param.inlier_threshold) + inliers.push_back(i); + return inliers; +} + +VisualOdometryStereo::result VisualOdometryStereo::updateParameters(vector &p_matched,vector &active,vector &tr,double step_size,double eps) { + + // we need at least 3 observations + if (active.size()<3) + return FAILED; + + // extract observations and compute predictions + computeObservations(p_matched,active); + computeResidualsAndJacobian(tr,active); + + // init + Matrix A(6,6); + Matrix B(6,1); + + // fill matrices A and B + for (int32_t m=0; m<6; m++) { + for (int32_t n=0; n<6; n++) { + double a = 0; + for (int32_t i=0; i<4*(int32_t)active.size(); i++) { + a += J[i*6+m]*J[i*6+n]; + } + A.val[m][n] = a; + } + double b = 0; + for (int32_t i=0; i<4*(int32_t)active.size(); i++) { + b += J[i*6+m]*(p_residual[i]); + } + B.val[m][0] = b; + } + + // perform elimination + if (B.solve(A)) { + bool converged = true; + for (int32_t m=0; m<6; m++) { + tr[m] += step_size*B.val[m][0]; + if (fabs(B.val[m][0])>eps) + converged = false; + } + if (converged) + return CONVERGED; + else + return UPDATED; + } else { + return FAILED; + } +} + +void VisualOdometryStereo::computeObservations(vector &p_matched,vector &active) { + + // set all observations + for (int32_t i=0; i<(int32_t)active.size(); i++) { + p_observe[4*i+0] = p_matched[active[i]].u1c; // u1 + p_observe[4*i+1] = p_matched[active[i]].v1c; // v1 + p_observe[4*i+2] = p_matched[active[i]].u2c; // u2 + p_observe[4*i+3] = p_matched[active[i]].v2c; // v2 + } +} + +void VisualOdometryStereo::computeResidualsAndJacobian(vector &tr,vector &active) { + + // extract motion parameters + double rx = tr[0]; double ry = tr[1]; double rz = tr[2]; + double tx = tr[3]; double ty = tr[4]; double tz = tr[5]; + + // precompute sine/cosine + double sx = sin(rx); double cx = cos(rx); double sy = sin(ry); + double cy = cos(ry); double sz = sin(rz); double cz = cos(rz); + + // compute rotation matrix and derivatives + double r00 = +cy*cz; double r01 = -cy*sz; double r02 = +sy; + double r10 = +sx*sy*cz+cx*sz; double r11 = -sx*sy*sz+cx*cz; double r12 = -sx*cy; + double r20 = -cx*sy*cz+sx*sz; double r21 = +cx*sy*sz+sx*cz; double r22 = +cx*cy; + double rdrx10 = +cx*sy*cz-sx*sz; double rdrx11 = -cx*sy*sz-sx*sz; double rdrx12 = -cx*cy; + double rdrx20 = +sx*sy*cz+cx*sz; double rdrx21 = -sx*sy*sz+cx*cz; double rdrx22 = -sx*cy; + double rdry00 = -sy*cz; double rdry01 = +sy*sz; double rdry02 = +cy; + double rdry10 = +sx*cy*cz; double rdry11 = -sx*cy*sz; double rdry12 = +sx*sy; + double rdry20 = -cx*cy*cz; double rdry21 = +cx*cy*sz; double rdry22 = -cx*sy; + double rdrz00 = -cy*sz; double rdrz01 = -cy*cz; + double rdrz10 = -sx*sy*sz+cx*cz; double rdrz11 = -sx*sy*cz-cx*sz; + double rdrz20 = +cx*sy*sz+sx*cz; double rdrz21 = +cx*sy*cz-sx*sz; + + // loop variables + double X1p,Y1p,Z1p; + double X1c,Y1c,Z1c,X2c; + double X1cd,Y1cd,Z1cd; + + // for all observations do + for (int32_t i=0; i<(int32_t)active.size(); i++) { + + // get 3d point in previous coordinate system + X1p = X[active[i]]; + Y1p = Y[active[i]]; + Z1p = Z[active[i]]; + + // compute 3d point in current left coordinate system + X1c = r00*X1p+r01*Y1p+r02*Z1p+tx; + Y1c = r10*X1p+r11*Y1p+r12*Z1p+ty; + Z1c = r20*X1p+r21*Y1p+r22*Z1p+tz; + + // weighting + double weight = 1.0; + if (param.reweighting) + weight = 1.0/(fabs(p_observe[4*i+0]-param.calib.cu)/fabs(param.calib.cu) + 0.05); + + // compute 3d point in current right coordinate system + X2c = X1c-param.base; + + // for all paramters do + for (int32_t j=0; j<6; j++) { + + // derivatives of 3d pt. in curr. left coordinates wrt. param j + switch (j) { + case 0: X1cd = 0; + Y1cd = rdrx10*X1p+rdrx11*Y1p+rdrx12*Z1p; + Z1cd = rdrx20*X1p+rdrx21*Y1p+rdrx22*Z1p; + break; + case 1: X1cd = rdry00*X1p+rdry01*Y1p+rdry02*Z1p; + Y1cd = rdry10*X1p+rdry11*Y1p+rdry12*Z1p; + Z1cd = rdry20*X1p+rdry21*Y1p+rdry22*Z1p; + break; + case 2: X1cd = rdrz00*X1p+rdrz01*Y1p; + Y1cd = rdrz10*X1p+rdrz11*Y1p; + Z1cd = rdrz20*X1p+rdrz21*Y1p; + break; + case 3: X1cd = 1; Y1cd = 0; Z1cd = 0; break; + case 4: X1cd = 0; Y1cd = 1; Z1cd = 0; break; + case 5: X1cd = 0; Y1cd = 0; Z1cd = 1; break; + } + + // set jacobian entries (project via K) + J[(4*i+0)*6+j] = weight*param.calib.f*(X1cd*Z1c-X1c*Z1cd)/(Z1c*Z1c); // left u' + J[(4*i+1)*6+j] = weight*param.calib.f*(Y1cd*Z1c-Y1c*Z1cd)/(Z1c*Z1c); // left v' + J[(4*i+2)*6+j] = weight*param.calib.f*(X1cd*Z1c-X2c*Z1cd)/(Z1c*Z1c); // right u' + J[(4*i+3)*6+j] = weight*param.calib.f*(Y1cd*Z1c-Y1c*Z1cd)/(Z1c*Z1c); // right v' + } + + // set prediction (project via K) + p_predict[4*i+0] = param.calib.f*X1c/Z1c+param.calib.cu; // left u + p_predict[4*i+1] = param.calib.f*Y1c/Z1c+param.calib.cv; // left v + p_predict[4*i+2] = param.calib.f*X2c/Z1c+param.calib.cu; // right u + p_predict[4*i+3] = param.calib.f*Y1c/Z1c+param.calib.cv; // right v + + // set residuals + p_residual[4*i+0] = weight*(p_observe[4*i+0]-p_predict[4*i+0]); + p_residual[4*i+1] = weight*(p_observe[4*i+1]-p_predict[4*i+1]); + p_residual[4*i+2] = weight*(p_observe[4*i+2]-p_predict[4*i+2]); + p_residual[4*i+3] = weight*(p_observe[4*i+3]-p_predict[4*i+3]); + } +} + diff --git a/src/mono_demo.cpp b/src/mono_demo.cpp new file mode 100644 index 0000000..406e062 --- /dev/null +++ b/src/mono_demo.cpp @@ -0,0 +1,127 @@ +/* +Copyright 2012. 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 +*/ + +/* + Documented C++ sample code of stereo visual odometry (modify to your needs) + To run this demonstration, download the Karlsruhe dataset sequence + '2010_03_09_drive_0019' from: www.cvlibs.net! + Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019 +*/ + +#include +#include +#include +#include + +#include +#include + +using namespace std; + +int main (int argc, char** argv) { + + // we need the path name to 2010_03_09_drive_0019 as input argument + if (argc<2) { + cerr << "Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019" << endl; + return 1; + } + + // sequence directory + string dir = argv[1]; + + // set most important visual odometry parameters + // for a full parameter list, look at: viso_stereo.h + VisualOdometryMono::parameters param; + + // calibration parameters for sequence 2010_03_09_drive_0019 + param.calib.f = 645.24; // focal length in pixels + param.calib.cu = 661.96; // principal point (u-coordinate) in pixels + param.calib.cv = 194.13; // principal point (v-coordinate) in pixels + param.height = 1.0; // m + + // init visual odometry + VisualOdometryMono viso(param); + + // current pose (this matrix transforms a point from the current + // frame's camera coordinates to the first frame's camera coordinates) + Matrix pose = Matrix::eye(4); + + cv::namedWindow("image",0); + + // loop through all frames i=0:372 + for (int32_t i=0; i<373; i++) { + + // input file names + char base_name[256]; sprintf(base_name,"%06d.png",i); + string left_img_file_name = dir + "/I1_" + base_name; + + // catch image read/write errors here + try { + + // load left and right input image + cv::Mat left_img = cv::imread(left_img_file_name.c_str(),0); // load as black and white image + + + cv::imshow("image",left_img); + + // image dimensions + int32_t dims[3]; + dims[0] = left_img.cols; // width + dims[1] = left_img.rows; // height + dims[2] = dims[0]; // image width, BW so it equals to width + + // get pointers to the image data + uint8_t* left_img_data = left_img.data; + + // status + cout << "Processing: Frame: " << i; + + // compute visual odometry + if (viso.process(left_img_data,dims)) { + + // on success, update current pose + pose = pose * Matrix::inv(viso.getMotion()); + + // output some statistics + double num_matches = viso.getNumberOfMatches(); + double num_inliers = viso.getNumberOfInliers(); + cout << ", Matches: " << num_matches; + cout << ", Inliers: " << 100.0*num_inliers/num_matches << " %" << ", Current pose: " << endl; + cout << pose << endl << endl; + + } else { + cout << " ... failed!" << endl; + } + + // catch image read errors here + } catch (...) { + cerr << "ERROR: Couldn't read input files!" << endl; + return 1; + } + } + + // output + cout << "Demo complete! Exiting ..." << endl; + + // exit + return 0; +} + diff --git a/src/mono_node.cpp b/src/mono_node.cpp new file mode 100644 index 0000000..8868e94 --- /dev/null +++ b/src/mono_node.cpp @@ -0,0 +1,205 @@ +/* +Copyright 2012. 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 +*/ + +/* + The demo program was modified to run in ROS + + 14 Feb 2012 Created Kevin J. Walchko +*/ + +// C++ +#include +#include +#include +#include + +// ROS +#include +#include +#include // handles raw or compressed images +#include +#include +#include + +// Lib Viso2 +#include + +// OpenCV +#include + +/** + * This node uses the libviso2 to track points through successive images and + * determine the pose (position and orientation) of the camera. This demo node + * shows simple ROS integration. + */ +class VisoOdometryMonoNode { +public: + + /** + * Constructor + */ + VisoOdometryMonoNode(ros::NodeHandle& node, std::string& s) : transport(node) { + pose = Matrix::eye(4); + viso = NULL; + debug = false; + + // setup subscribers and publishers + image_sub = transport.subscribe(s.c_str(), 1, &VisoOdometryMonoNode::imageCallback, this); + pose_pub = node.advertise("pose",10); + } + + /** + * Allow user to set default parameters + */ + inline void set(const VisualOdometryMono::parameters& p){ + param = p; + } + + inline void setDebug(const bool b){ + debug = b; + } + + void print(){ + // output some statistics + double num_matches = viso->getNumberOfMatches(); + double num_inliers = viso->getNumberOfInliers(); + std::cout << ", Matches: " << num_matches; + std::cout << ", Inliers: " << 100.0*num_inliers/num_matches << " %" << ", Current pose: " << std::endl; + std::cout << pose << std::endl << std::endl; + } + +private: + /** + * Create mono camera visual odometry using either a default param + * or one passed to us by the set() function + */ + void init(){ + viso = new VisualOdometryMono(param); + } + + /** + * Grabs image, processes it, and publishes results + */ + void imageCallback(const sensor_msgs::ImageConstPtr& msg){ + + cv_bridge::CvImagePtr cv_msg; + + try{ + cv_msg = cv_bridge::toCvCopy(msg, "bgr8"); + } + catch (sensor_msgs::CvBridgeException& error){ + ROS_ERROR("error"); + return; + } + + // if viso has not been created in init() yet, then + // do so now + if( viso == NULL){ + init(); + dims[0] = cv_msg->image.cols; + dims[1] = cv_msg->image.rows; + dims[2] = cv_msg->image.cols; + } + + // convert BGR image into Gray image + cv::Mat image; + cv::cvtColor(cv_msg->image,image,CV_BGR2GRAY); + + if(debug){ + cv::imshow("debug",image); + cv::waitKey(1); + } + + // find motion + bool ok = viso->process(image.data,dims); + + // update pose and publish only if process() was successful + if(ok){ + pose = pose * Matrix::inv(viso->getMotion()); + publish(); + } + } + + /** + * Breaks up the pose matrix into a position vector and an + * rotation quaternion and then publishes a Pose message + */ + bool publish(){ + // break pose down into position and quaternion + bool ok = false; + + //print(); + + geometry_msgs::Pose pose_msg; + pose_msg.position.x = pose.val[0][3]; + pose_msg.position.y = pose.val[1][3]; + pose_msg.position.z = pose.val[2][3]; + + pose_pub.publish(pose_msg); + + ok = true; + + return ok; + } + + VisualOdometryMono::parameters param; + VisualOdometryMono *viso; + Matrix pose; + int32_t dims[3]; + + image_transport::ImageTransport transport; + image_transport::Subscriber image_sub; + ros::Publisher pose_pub; + + bool debug; +}; + +int main (int argc, char** argv) { + + ros::init(argc, argv, "viso_node"); + ros::NodeHandle n("~"); + + + // need the topic + if (argc<2) { + std::cerr << "Usage: ./mono_node topic_name " << std::endl; + return 1; + } + + // set most important visual odometry parameters + // for a full parameter list, look at: viso_mono.h + VisualOdometryMono::parameters param; + param.calib.f = 645.24; // focal length in pixels + param.height = 1.0; // m + + std::string topic = argv[1]; + + // create node + VisoOdometryMonoNode viso(n,topic); + viso.set(param); + viso.setDebug(true); + + ros::spin(); + + // exit + return 0; +} + diff --git a/src/stereo_demo.cpp b/src/stereo_demo.cpp new file mode 100644 index 0000000..d256166 --- /dev/null +++ b/src/stereo_demo.cpp @@ -0,0 +1,130 @@ +/* +Copyright 2012. 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 +*/ + +/* + Documented C++ sample code of stereo visual odometry (modify to your needs) + To run this demonstration, download the Karlsruhe dataset sequence + '2010_03_09_drive_0019' from: www.cvlibs.net! + Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019 +*/ + +#include +#include +#include +#include + +#include +#include + +using namespace std; + +int main (int argc, char** argv) { + + // we need the path name to 2010_03_09_drive_0019 as input argument + if (argc<2) { + cerr << "Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019" << endl; + return 1; + } + + // sequence directory + string dir = argv[1]; + + // set most important visual odometry parameters + // for a full parameter list, look at: viso_stereo.h + VisualOdometryStereo::parameters param; + + // calibration parameters for sequence 2010_03_09_drive_0019 + param.calib.f = 645.24; // focal length in pixels + param.calib.cu = 661.96; // principal point (u-coordinate) in pixels + param.calib.cv = 194.13; // principal point (v-coordinate) in pixels + param.base = 0.5707; // baseline in meters + + // init visual odometry + VisualOdometryStereo viso(param); + + // current pose (this matrix transforms a point from the current + // frame's camera coordinates to the first frame's camera coordinates) + Matrix pose = Matrix::eye(4); + + cv::namedWindow("left",0); + cv::namedWindow("right",0); + + // loop through all frames i=0:372 + for (int32_t i=0; i<373; i++) { + + // input file names + char base_name[256]; sprintf(base_name,"%06d.png",i); + string left_img_file_name = dir + "/I1_" + base_name; + string right_img_file_name = dir + "/I2_" + base_name; + + // catch image read/write errors here + try { + + // load left and right input image + cv::Mat left_img = cv::imread(left_img_file_name.c_str(),0); // load as black and white image + cv::Mat right_img = cv::imread(right_img_file_name.c_str(),0); + + cv::imshow("left",left_img); + cv::imshow("right",right_img); + + // image dimensions + int32_t width = left_img.cols; + int32_t height = left_img.rows; + + // get pointers to the image data + uint8_t* left_img_data = left_img.data; + uint8_t* right_img_data = right_img.data; + + // status + cout << "Processing: Frame: " << i; + + // compute visual odometry + int32_t dims[] = {width,height,width}; + if (viso.process(left_img_data,right_img_data,dims)) { + + // on success, update current pose + pose = pose * Matrix::inv(viso.getMotion()); + + // output some statistics + double num_matches = viso.getNumberOfMatches(); + double num_inliers = viso.getNumberOfInliers(); + cout << ", Matches: " << num_matches; + cout << ", Inliers: " << 100.0*num_inliers/num_matches << " %" << ", Current pose: " << endl; + cout << pose << endl << endl; + + } else { + cout << " ... failed!" << endl; + } + + // catch image read errors here + } catch (...) { + cerr << "ERROR: Couldn't read input files!" << endl; + return 1; + } + } + + // output + cout << "Demo complete! Exiting ..." << endl; + + // exit + return 0; +} +