From 5ba2e901f19812a7af6da9e9ffcddc73b7825dd8 Mon Sep 17 00:00:00 2001 From: Yueh-Cheng Liu Date: Thu, 10 May 2018 10:44:25 +0800 Subject: [PATCH] First commit, add all files --- Makefile | 10 + README.md | 35 +++ mesh2vox.cpp | 301 ++++++++++++++++++++++ polygon_3darea.cpp | 186 ++++++++++++++ read_buffer.py | 77 ++++++ tinyply.cpp | 621 +++++++++++++++++++++++++++++++++++++++++++++ tinyply.h | 119 +++++++++ triangleCube.cpp | 481 +++++++++++++++++++++++++++++++++++ triangleCube.h | 45 ++++ 9 files changed, 1875 insertions(+) create mode 100644 Makefile create mode 100644 README.md create mode 100644 mesh2vox.cpp create mode 100644 polygon_3darea.cpp create mode 100644 read_buffer.py create mode 100644 tinyply.cpp create mode 100644 tinyply.h create mode 100644 triangleCube.cpp create mode 100644 triangleCube.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..3fca7e5 --- /dev/null +++ b/Makefile @@ -0,0 +1,10 @@ +CPPFLAGS= -Wall -O2 -std=c++11 +TARGET=mesh2vox.cpp +UTILS=tinyply.cpp triangleCube.cpp polygon_3darea.cpp +#all: +# g++ $(CPPFLAGS) $(TARGET) $(UTILS) +MEAN: + g++ $(CPPFLAGS) $(TARGET) $(UTILS) -o mesh2vox_mean +MAX: + g++ $(CPPFLAGS) -D TAKE_MAX_AREA_COLOR $(TARGET) $(UTILS) -o mesh2vox_max + diff --git a/README.md b/README.md new file mode 100644 index 0000000..3c7e72a --- /dev/null +++ b/README.md @@ -0,0 +1,35 @@ +# Convert Mesh to Color Voxel + +Take `.ply` mesh format as input, convert it into voxel with color (rgba). + +## Requirement + +- c++11: for the package **tinyply**. +- numpy + +## Usage + +### Take the largest face inside the cube (voxel) to represent to color. +``` +make MAX +./mesh2vox_max [input ply file] // output file is .dat +``` +### Take weighted mean (by area size of faces) for the color of the voxel. +``` +make MEAN +./mesh2vox_mean [input ply file] // output file is .dat +``` +### Visualization + +``` +python3 read_buffer.py // generate .ply point cloud for visualization +``` +Use tools like meshlab to open the `.ply` file. + +## References + +- tinyply: https://github.com/ddiakopoulos/tinyply +- 3d polygon area: http://geomalgorithms.com/a01-_area.html +- triangle test: http://blackpawn.com/texts/pointinpoly/ +- triangle cube intersect test: http://www.realtimerendering.com/resources/GraphicsGems/gemsiii/triangleCube.c + diff --git a/mesh2vox.cpp b/mesh2vox.cpp new file mode 100644 index 0000000..d3e6c0a --- /dev/null +++ b/mesh2vox.cpp @@ -0,0 +1,301 @@ +#include "triangleCube.h" +#include "tinyply.h" +#include +#include +#include +#include +#include +#include +#include +#include +using namespace tinyply; +using namespace std; + +// #define TAKE_MAX_AREA_COLOR +#define AREA_THRESHOLD 1e-5 +#define MAX_VOX_X 200 +#define MAX_VOX_Y 200 +#define MAX_VOX_Z 150 +#define CUBESIZE 48 // 48 mm +typedef struct int3 { + int3() {} + int3(int a, int b, int c) { + v[0] = a, v[1] = b, v[2] = c; + } + int v[3]; +} int3; + +typedef struct int4 { + int4() {} + int4(int a, int b, int c) { + v[0] = a, v[1] = b, v[2] = c, v[3] = 255; + } + int4(int a, int b, int c, int d) { + v[0] = a, v[1] = b, v[2] = c, v[3] = d; + } + int v[4]; +} int4; + +typedef struct { + unsigned char v[4]; +} uchar4; + + +typedef enum { + B_MAX, B_MIN +} boundary_type; + +// Debug +void print_point3(Point3 p) { + cout << '(' << p.x << ", " << p.y << ", " << p.z << ')' << endl; +} + +void print_triangle3(Triangle3 &tri) { + cout << "v1: "; + print_point3(tri.v1); + cout << "v2: "; + print_point3(tri.v2); + cout << "v3: "; + print_point3(tri.v3); +} +// Set maximum size +int4 global_voxel[MAX_VOX_X][MAX_VOX_Y][MAX_VOX_Z]; +float voxel_buffer[MAX_VOX_X][MAX_VOX_Y][MAX_VOX_Z][5]; +Point3 triangle_boundary(Triangle3 &tri, boundary_type type) { + Point3 p; + + if (type == B_MAX) { + p.x = MAX3(tri.v1.x, tri.v2.x, tri.v3.x); + p.y = MAX3(tri.v1.y, tri.v2.y, tri.v3.y); + p.z = MAX3(tri.v1.z, tri.v2.z, tri.v3.z); + } + else if (type == B_MIN) { + p.x = MIN3(tri.v1.x, tri.v2.x, tri.v3.x); + p.y = MIN3(tri.v1.y, tri.v2.y, tri.v3.y); + p.z = MIN3(tri.v1.z, tri.v2.z, tri.v3.z); + } + return p; +} +int3 point3_to_voxel_cord(Point3 p) { + int3 voxel_cord; + voxel_cord.v[0] = (int) (p.x / CUBESIZE); + voxel_cord.v[1] = (int) (p.y / CUBESIZE); + voxel_cord.v[2] = (int) (p.z / CUBESIZE); + return voxel_cord; +} + +Point3 point3_scale(Point3 p, float ratio) { + Point3 new_p; + new_p.x = p.x * ratio; + new_p.y = p.y * ratio; + new_p.z = p.z * ratio; + return new_p; +} + +void triangle_cord_transform(Triangle3 &tri, float scale, Point3 trans) { + // Firest translate, then resize + ADD(tri.v1, trans, tri.v1); + ADD(tri.v2, trans, tri.v2); + ADD(tri.v3, trans, tri.v3); + + tri.v1 = point3_scale(tri.v1, scale); + tri.v2 = point3_scale(tri.v2, scale); + tri.v3 = point3_scale(tri.v3, scale); +} +Point3 point_float_mult(Point3 p, float a) { + Point3 t(p.x*a, p.y*a, p.z*a); + return t; +} + +void mark_occupy_voxel(Triangle3 &tri, int4 &color) { + Point3 tri_bound_max = triangle_boundary(tri, B_MAX); + Point3 tri_bound_min = triangle_boundary(tri, B_MIN); + int3 vox_bound_max = point3_to_voxel_cord(tri_bound_max); + int3 vox_bound_min = point3_to_voxel_cord(tri_bound_min); + + for (int i = vox_bound_min.v[0]; i <= vox_bound_max.v[0]; i++) { + for (int j = vox_bound_min.v[1]; j <= vox_bound_max.v[1]; j++) { + for (int k = vox_bound_min.v[2]; k <= vox_bound_max.v[2]; k++) { + // Calculate difference between voxel and unit cube + float scale = 1. / CUBESIZE; + Point3 unit_cube_centroid(0, 0, 0); + Point3 target_cube_centroid((i+.5)*CUBESIZE, (j+.5)*CUBESIZE, (k+.5)*CUBESIZE); + Point3 trans; + SUB(unit_cube_centroid, target_cube_centroid, trans); + Triangle3 tmp_tri = tri; + // Transform triangle into unit cube coordinate + triangle_cord_transform(tmp_tri, scale, trans); + + // DO occupy check + if (t_c_intersection(tmp_tri) == INSIDE) { + float area = triangle_area_inside_cube(tmp_tri); + // CHECK Threshold + if (area < AREA_THRESHOLD) continue; +#ifdef TAKE_MAX_AREA_COLOR + if (area > voxel_buffer[i][j][k][4]) { + voxel_buffer[i][j][k][0] = color.v[0]; + voxel_buffer[i][j][k][1] = color.v[1]; + voxel_buffer[i][j][k][2] = color.v[2]; + voxel_buffer[i][j][k][3] = color.v[3]; + + } +#else + // Save for weighted mean + voxel_buffer[i][j][k][0] += color.v[0] * area; + voxel_buffer[i][j][k][1] += color.v[1] * area; + voxel_buffer[i][j][k][2] += color.v[2] * area; + voxel_buffer[i][j][k][3] += color.v[3] * area; + voxel_buffer[i][j][k][4] += area; +#endif + } + } + } + } +} + +void dump_voxel(int4 voxel[MAX_VOX_X][MAX_VOX_Y][MAX_VOX_Z], int3 dim, const char fname[], const char scene[]){ + int hdim = dim.v[0]; + int wdim = dim.v[1]; + int ddim = dim.v[2]; + int cdim = 4; + ofstream fout(fname, ios::out | ios::binary); + fout << "CV\n"; + fout << "# "<< scene<< "color voxel. Height, width, depth, channel\n"; + fout << hdim <<" "<< wdim <<" "<< ddim <<" "<< cdim <<"\n"; + fout << hdim*wdim*ddim*cdim<<"\n"; + for(int h = 0; h < hdim; ++h){ + for(int w = 0; w < wdim; ++w){ + for(int d = 0; d < ddim; ++d){ + fout.write(reinterpret_cast(&voxel[h][w][d].v[0]), sizeof(int)); + fout.write(reinterpret_cast(&voxel[h][w][d].v[1]), sizeof(int)); + fout.write(reinterpret_cast(&voxel[h][w][d].v[2]), sizeof(int)); + fout.write(reinterpret_cast(&voxel[h][w][d].v[3]), sizeof(int)); + } + } + } + fout.close(); +} + +void ply_to_voxel(const string &filename) { + try + { + // Read the file and create a std::istringstream suitable + // for the lib -- tinyply does not perform any file i/o. + std::ifstream ss(filename, std::ios::binary); + + if (ss.fail()) + { + throw std::runtime_error("failed to open " + filename); + } + + // Read ply file + PlyFile file; + shared_ptr faces, vertices, colors; + file.parse_header(ss); + faces = file.request_properties_from_element("face", { "vertex_indices" }); + vertices = file.request_properties_from_element("vertex", {"x", "y", "z"}); + colors = file.request_properties_from_element("vertex", {"red", "green", "blue", "alpha"}); + + file.read(ss); + + // Copy data from plyfile reader to vectors + const size_t numVerticesBytes = vertices->buffer.size_bytes(); + const size_t numFacesBytes = faces->buffer.size_bytes(); + const size_t numColorBytes = colors->buffer.size_bytes(); + vector v(vertices->count); + vector f(faces->count); + vector v_color(vertices->count); + memcpy(v.data(), vertices->buffer.get(), numVerticesBytes); + memcpy(f.data(), faces->buffer.get(), numFacesBytes); + memcpy(v_color.data(), colors->buffer.get(), numColorBytes); + // Find the boundary of the mesh + Point3 max_boundary = v[0], min_boundary = v[0]; + for (int i = 0; i < v.size(); i++) { + v[i].x *= 1e3; + v[i].y *= 1e3; + v[i].z *= 1e3; + + max_boundary.x = max(max_boundary.x, v[i].x); + max_boundary.y = max(max_boundary.y, v[i].y); + max_boundary.z = max(max_boundary.z, v[i].z); + min_boundary.x = min(min_boundary.x, v[i].x); + min_boundary.y = min(min_boundary.y, v[i].y); + min_boundary.z = min(min_boundary.z, v[i].z); + } + // Only consider min_boundary is (0, 0, 0) + const int max_vox_x = (int)(max_boundary.x / CUBESIZE) + 1; + const int max_vox_y = (int)(max_boundary.y / CUBESIZE) + 1; + const int max_vox_z = (int)(max_boundary.z / CUBESIZE) + 1; + cout << max_vox_x << ' ' << max_vox_y << ' ' << max_vox_z << endl; + + // Reset globel voxel + memset(global_voxel, 0, sizeof(global_voxel)); + for (int i = 0; i < MAX_VOX_X; i++) + for (int j = 0; j < MAX_VOX_Y; j++) + for (int k = 0; k < MAX_VOX_Z; k++) + for (int l = 0; l < 5; l++) + voxel_buffer[i][j][k][l] = 0; + + // Check if it is larger than max size + assert(max_vox_x < MAX_VOX_X); + assert(max_vox_y < MAX_VOX_Y); + assert(max_vox_z < MAX_VOX_Z); + + // Setup all trangles from ids to real points + vector tri(faces->count); + vector tri_color(faces->count); + for (int i = 0; i < faces->count; i++) { + tri[i].v1 = v[f[i].v[0]]; + tri[i].v2 = v[f[i].v[1]]; + tri[i].v3 = v[f[i].v[2]]; + + // Calc color for the face + tri_color[i].v[0] = (v_color[f[i].v[0]].v[0] + v_color[f[i].v[1]].v[0] + v_color[f[i].v[2]].v[0]) / 3; + tri_color[i].v[1] = (v_color[f[i].v[0]].v[1] + v_color[f[i].v[1]].v[1] + v_color[f[i].v[2]].v[1]) / 3; + tri_color[i].v[2] = (v_color[f[i].v[0]].v[2] + v_color[f[i].v[1]].v[2] + v_color[f[i].v[2]].v[2]) / 3; + tri_color[i].v[3] = (v_color[f[i].v[0]].v[3] + v_color[f[i].v[1]].v[3] + v_color[f[i].v[2]].v[3]) / 3; + // Do occupying check + mark_occupy_voxel(tri[i], tri_color[i]); + } + + // Weighted mean + for (int i = 0; i < MAX_VOX_X; i++) + for (int j = 0; j < MAX_VOX_Y; j++) + for (int k = 0; k < MAX_VOX_Z; k++) { +#ifdef TAKE_MAX_AREA_COLOR + + global_voxel[i][j][k].v[0] = voxel_buffer[i][j][k][0]; + global_voxel[i][j][k].v[1] = voxel_buffer[i][j][k][1]; + global_voxel[i][j][k].v[2] = voxel_buffer[i][j][k][2]; + global_voxel[i][j][k].v[3] = voxel_buffer[i][j][k][3]; +#else + // Weighted mean + if (voxel_buffer[i][j][k][4] < AREA_THRESHOLD) continue; + global_voxel[i][j][k].v[0] = voxel_buffer[i][j][k][0] / voxel_buffer[i][j][k][4]; + global_voxel[i][j][k].v[1] = voxel_buffer[i][j][k][1] / voxel_buffer[i][j][k][4]; + global_voxel[i][j][k].v[2] = voxel_buffer[i][j][k][2] / voxel_buffer[i][j][k][4]; + global_voxel[i][j][k].v[3] = voxel_buffer[i][j][k][3] / voxel_buffer[i][j][k][4]; + // printf("area: %f\n", voxel_buffer[i][j][k][4]); +#endif + } + + // Write to file + int3 dim(max_vox_x, max_vox_y, max_vox_z); + dump_voxel(global_voxel, dim, "sceneXXXX_XX.dat", "sceneXXXX_XX"); + } + catch (const std::exception & e) + { + std::cerr << "Caught exception: " << e.what() << std::endl; + } +} + +int main(int argc, char *argv[]) { +#ifdef TAKE_MAX_AREA_COLOR + printf("Using max area for color\n"); +#else + printf("Using weighted mean for color\n"); +#endif + printf("Reading file %s\n", argv[1]); + ply_to_voxel(argv[1]); + return 0; +} diff --git a/polygon_3darea.cpp b/polygon_3darea.cpp new file mode 100644 index 0000000..a1e4529 --- /dev/null +++ b/polygon_3darea.cpp @@ -0,0 +1,186 @@ +#include "triangleCube.h" +#include +using namespace std; +// Copyright 2000 softSurfer, 2012 Dan Sunday +// This code may be freely used and modified for any purpose +// providing that this copyright notice is included with it. +// iSurfer.org makes no warranty for this code, and cannot be held +// liable for any real or imagined damage resulting from its use. +// Users of this code must verify correctness for their application. + + + +// a Triangle is given by three points: Point3 V0, V1, V2 + +// a Polygon is given by: +// int n = number of vertex points +// Point3* V[] = an array of n+1 vertex points with V[n]=V[0] + + +// Note: for efficiency low-level short functions are declared to be inline. + + +// isLeft(): test if a point is Left|On|Right of an infinite 2D line. +// Input: three points P0, P1, and P2 +// Return: >0 for P2 left of the line through P0 to P1 +// =0 for P2 on the line +// <0 for P2 right of the line +inline int +isLeft( Point3 P0, Point3 P1, Point3 P2 ) +{ + return ( (P1.x - P0.x) * (P2.y - P0.y) + - (P2.x - P0.x) * (P1.y - P0.y) ); +} +//=================================================================== + + +// orientation2D_Triangle(): test the orientation of a 2D triangle +// Input: three vertex points V0, V1, V2 +// Return: >0 for counterclockwise +// =0 for none (degenerate) +// <0 for clockwise +inline int +orientation2D_Triangle( Point3 V0, Point3 V1, Point3 V2 ) +{ + return isLeft(V0, V1, V2); +} +//=================================================================== + + +// area2D_Triangle(): compute the area of a 2D triangle +// Input: three vertex points V0, V1, V2 +// Return: the (float) area of triangle T +inline float +area2D_Triangle( Point3 V0, Point3 V1, Point3 V2 ) +{ + return (float)isLeft(V0, V1, V2) / 2.0; +} +//=================================================================== + + +// orientation2D_Polygon(): test the orientation of a simple 2D polygon +// Input: int n = the number of vertices in the polygon +// Point3* V = an array of n+1 vertex points with V[n]=V[0] +// Return: >0 for counterclockwise +// =0 for none (degenerate) +// <0 for clockwise +// Note: this algorithm is faster than computing the signed area. +int +orientation2D_Polygon( int n, vector& V ) +{ + // first find rightmost lowest vertex of the polygon + int rmin = 0; + int xmin = V[0].x; + int ymin = V[0].y; + + for (int i=1; i ymin) + continue; + if (V[i].y == ymin) { // just as low + if (V[i].x < xmin) // and to left + continue; + } + rmin = i; // a new rightmost lowest vertex + xmin = V[i].x; + ymin = V[i].y; + } + + // test orientation at the rmin vertex + // ccw <=> the edge leaving V[rmin] is left of the entering edge + if (rmin == 0) + return isLeft( V[n-1], V[0], V[1] ); + else + return isLeft( V[rmin-1], V[rmin], V[rmin+1] ); +} + //=================================================================== + + +// area2D_Polygon(): compute the area of a 2D polygon +// Input: int n = the number of vertices in the polygon +// Point3* V = an array of n+1 vertex points with V[n]=V[0] +// Return: the (float) area of the polygon +float +area2D_Polygon( int n, vector& V ) +{ + float area = 0; + int i, j, k; // indices + + if (n < 3) return 0; // a degenerate polygon + + for (i=1, j=2, k=0; i& V, Point3 N ) +{ + float area = 0; + float an, ax, ay, az; // abs value of normal and its coords + int coord; // coord to ignore: 1=x, 2=y, 3=z + int i, j, k; // loop indices + + if (n < 3) return 0; // a degenerate polygon + + // select largest abs coordinate to ignore for projection + ax = (N.x>0 ? N.x : -N.x); // abs x-coord + ay = (N.y>0 ? N.y : -N.y); // abs y-coord + az = (N.z>0 ? N.z : -N.z); // abs z-coord + + coord = 3; // ignore z-coord + if (ax > ay) { + if (ax > az) coord = 1; // ignore x-coord + } + else if (ay > az) coord = 2; // ignore y-coord + + // compute area of the 2D projection + switch (coord) { + case 1: + for (i=1, j=2, k=0; i +#include +#include +#include +#include + +using namespace tinyply; +using namespace std; + +////////////////// +// Endian Utils // +////////////////// + +template T endian_swap(const T & v) { return v; } +template<> inline uint16_t endian_swap(const uint16_t & v) { return (v << 8) | (v >> 8); } +template<> inline uint32_t endian_swap(const uint32_t & v) { return (v << 24) | ((v << 8) & 0x00ff0000) | ((v >> 8) & 0x0000ff00) | (v >> 24); } +template<> inline uint64_t endian_swap(const uint64_t & v) +{ + return (((v & 0x00000000000000ffLL) << 56) | + ((v & 0x000000000000ff00LL) << 40) | + ((v & 0x0000000000ff0000LL) << 24) | + ((v & 0x00000000ff000000LL) << 8) | + ((v & 0x000000ff00000000LL) >> 8) | + ((v & 0x0000ff0000000000LL) >> 24) | + ((v & 0x00ff000000000000LL) >> 40) | + ((v & 0xff00000000000000LL) >> 56)); +} +template<> inline int16_t endian_swap(const int16_t & v) { uint16_t r = endian_swap(*(uint16_t*)&v); return *(int16_t*)&r; } +template<> inline int32_t endian_swap(const int32_t & v) { uint32_t r = endian_swap(*(uint32_t*)&v); return *(int32_t*)&r; } +template<> inline int64_t endian_swap(const int64_t & v) { uint64_t r = endian_swap(*(uint64_t*)&v); return *(int64_t*)&r; } +inline float endian_swap_float(const uint32_t & v) { union { float f; uint32_t i; }; i = endian_swap(v); return f; } +inline double endian_swap_double(const uint64_t & v) { union { double d; uint64_t i; }; i = endian_swap(v); return d; } + +///////////////////////////// +// Internal Implementation // +///////////////////////////// + +inline Type property_type_from_string(const std::string & t) +{ + if (t == "int8" || t == "char") return Type::INT8; + else if (t == "uint8" || t == "uchar") return Type::UINT8; + else if (t == "int16" || t == "short") return Type::INT16; + else if (t == "uint16" || t == "ushort") return Type::UINT16; + else if (t == "int32" || t == "int") return Type::INT32; + else if (t == "uint32" || t == "uint") return Type::UINT32; + else if (t == "float32" || t == "float") return Type::FLOAT32; + else if (t == "float64" || t == "double") return Type::FLOAT64; + return Type::INVALID; +} + +struct PlyFile::PlyFileImpl +{ + struct PlyCursor + { + size_t byteOffset; + size_t totalSizeBytes; + }; + + struct ParsingHelper + { + std::shared_ptr data; + std::shared_ptr cursor; + }; + + std::map userData; + + bool isBinary = false; + bool isBigEndian = false; + std::vector elements; + std::vector comments; + std::vector objInfo; + + void read(std::istream & is); + void write(std::ostream & os, bool isBinary); + + std::shared_ptr request_properties_from_element(const std::string & elementKey, const std::initializer_list propertyKeys); + void add_properties_to_element(const std::string & elementKey, const std::initializer_list propertyKeys, const Type type, const size_t count, uint8_t * data, const Type listType, const size_t listCount); + + size_t read_property_binary(const Type t, void * dest, size_t & destOffset, std::istream & is); + size_t read_property_ascii(const Type t, void * dest, size_t & destOffset, std::istream & is); + size_t skip_property_binary(const PlyProperty & property, std::istream & is); + size_t skip_property_ascii(const PlyProperty & property, std::istream & is); + + bool parse_header(std::istream & is); + void parse_data(std::istream & is, bool firstPass); + void read_header_format(std::istream & is); + void read_header_element(std::istream & is); + void read_header_property(std::istream & is); + void read_header_text(std::string line, std::istream & is, std::vector & place, int erase = 0); + + void write_header(std::ostream & os); + void write_ascii_internal(std::ostream & os); + void write_binary_internal(std::ostream & os); + void write_property_ascii(Type t, std::ostream & os, uint8_t * src, size_t & srcOffset); + void write_property_binary(Type t, std::ostream & os, uint8_t * src, size_t & srcOffset); +}; + +////////////////// +// PLY Property // +////////////////// + +PlyProperty::PlyProperty(std::istream & is) : isList(false) +{ + std::string type; + is >> type; + if (type == "list") + { + std::string countType; + is >> countType >> type; + listType = property_type_from_string(countType); + isList = true; + } + propertyType = property_type_from_string(type); + is >> name; +} + +///////////////// +// PLY Element // +///////////////// + +PlyElement::PlyElement(std::istream & is) +{ + is >> name >> size; +} + +/////////// +// Utils // +/////////// + +std::string make_key(const std::string & a, const std::string & b) +{ + return (a + "-" + b); +} + +template void ply_cast(void * dest, const char * src, bool be) +{ + *(static_cast(dest)) = (be) ? endian_swap(*(reinterpret_cast(src))) : *(reinterpret_cast(src)); +} + +template void ply_cast_float(void * dest, const char * src, bool be) +{ + *(static_cast(dest)) = (be) ? endian_swap_float(*(reinterpret_cast(src))) : *(reinterpret_cast(src)); +} + +template void ply_cast_double(void * dest, const char * src, bool be) +{ + *(static_cast(dest)) = (be) ? endian_swap_double(*(reinterpret_cast(src))) : *(reinterpret_cast(src)); +} + +template T ply_read_ascii(std::istream & is) +{ + T data; + is >> data; + return data; +} + +template void ply_cast_ascii(void * dest, std::istream & is) +{ + *(static_cast(dest)) = ply_read_ascii(is); +} + +size_t find_element(const std::string & key, const std::vector & list) +{ + for (size_t i = 0; i < list.size(); i++) if (list[i].name == key) return i; + return -1; +} + +size_t find_property(const std::string & key, const std::vector & list) +{ + for (size_t i = 0; i < list.size(); ++i) if (list[i].name == key) return i; + return -1; +} + +////////////// +// PLY File // +////////////// + +bool PlyFile::PlyFileImpl::parse_header(std::istream & is) +{ + std::string line; + while (std::getline(is, line)) + { + std::istringstream ls(line); + std::string token; + ls >> token; + if (token == "ply" || token == "PLY" || token == "") continue; + else if (token == "comment") read_header_text(line, ls, comments, 8); + else if (token == "format") read_header_format(ls); + else if (token == "element") read_header_element(ls); + else if (token == "property") read_header_property(ls); + else if (token == "obj_info") read_header_text(line, ls, objInfo, 9); + else if (token == "end_header") break; + else return false; + } + return true; +} + +void PlyFile::PlyFileImpl::read_header_text(std::string line, std::istream & is, std::vector& place, int erase) +{ + place.push_back((erase > 0) ? line.erase(0, erase) : line); +} + +void PlyFile::PlyFileImpl::read_header_format(std::istream & is) +{ + std::string s; + (is >> s); + if (s == "binary_little_endian") isBinary = true; + else if (s == "binary_big_endian") isBinary = isBigEndian = true; +} + +void PlyFile::PlyFileImpl::read_header_element(std::istream & is) +{ + elements.emplace_back(is); +} + +void PlyFile::PlyFileImpl::read_header_property(std::istream & is) +{ + elements.back().properties.emplace_back(is); +} + +size_t PlyFile::PlyFileImpl::skip_property_binary(const PlyProperty & p, std::istream & is) +{ + static std::vector skip(PropertyTable[p.propertyType].stride); + if (p.isList) + { + size_t listSize = 0; + size_t dummyCount = 0; + read_property_binary(p.listType, &listSize, dummyCount, is); + for (size_t i = 0; i < listSize; ++i) is.read(skip.data(), PropertyTable[p.propertyType].stride); + return listSize * PropertyTable[p.propertyType].stride; // in bytes + } + else + { + is.read(skip.data(), PropertyTable[p.propertyType].stride); + return PropertyTable[p.propertyType].stride; + } +} + +size_t PlyFile::PlyFileImpl::skip_property_ascii(const PlyProperty & p, std::istream & is) +{ + std::string skip; + if (p.isList) + { + size_t listSize = 0; + size_t dummyCount = 0; + read_property_ascii(p.listType, &listSize, dummyCount, is); + for (size_t i = 0; i < listSize; ++i) is >> skip; + return listSize * PropertyTable[p.propertyType].stride; // in bytes + } + else + { + is >> skip; + return PropertyTable[p.propertyType].stride; + } +} + +size_t PlyFile::PlyFileImpl::read_property_binary(const Type t, void * dest, size_t & destOffset, std::istream & is) +{ + destOffset += PropertyTable[t].stride; + + std::vector src(PropertyTable[t].stride); + is.read(src.data(), PropertyTable[t].stride); + + switch (t) + { + case Type::INT8: ply_cast(dest, src.data(), isBigEndian); break; + case Type::UINT8: ply_cast(dest, src.data(), isBigEndian); break; + case Type::INT16: ply_cast(dest, src.data(), isBigEndian); break; + case Type::UINT16: ply_cast(dest, src.data(), isBigEndian); break; + case Type::INT32: ply_cast(dest, src.data(), isBigEndian); break; + case Type::UINT32: ply_cast(dest, src.data(), isBigEndian); break; + case Type::FLOAT32: ply_cast_float(dest, src.data(), isBigEndian); break; + case Type::FLOAT64: ply_cast_double(dest, src.data(), isBigEndian); break; + case Type::INVALID: throw std::invalid_argument("invalid ply property"); + } + + return PropertyTable[t].stride; +} + +size_t PlyFile::PlyFileImpl::read_property_ascii(const Type t, void * dest, size_t & destOffset, std::istream & is) +{ + destOffset += PropertyTable[t].stride; + + switch (t) + { + case Type::INT8: *((int8_t *)dest) = ply_read_ascii(is); break; + case Type::UINT8: *((uint8_t *)dest) = ply_read_ascii(is); break; + case Type::INT16: ply_cast_ascii(dest, is); break; + case Type::UINT16: ply_cast_ascii(dest, is); break; + case Type::INT32: ply_cast_ascii(dest, is); break; + case Type::UINT32: ply_cast_ascii(dest, is); break; + case Type::FLOAT32: ply_cast_ascii(dest, is); break; + case Type::FLOAT64: ply_cast_ascii(dest, is); break; + case Type::INVALID: throw std::invalid_argument("invalid ply property"); + } + return PropertyTable[t].stride; +} + +void PlyFile::PlyFileImpl::write_property_ascii(Type t, std::ostream & os, uint8_t * src, size_t & srcOffset) +{ + switch (t) + { + case Type::INT8: os << static_cast(*reinterpret_cast(src)); break; + case Type::UINT8: os << static_cast(*reinterpret_cast(src)); break; + case Type::INT16: os << *reinterpret_cast(src); break; + case Type::UINT16: os << *reinterpret_cast(src); break; + case Type::INT32: os << *reinterpret_cast(src); break; + case Type::UINT32: os << *reinterpret_cast(src); break; + case Type::FLOAT32: os << *reinterpret_cast(src); break; + case Type::FLOAT64: os << *reinterpret_cast(src); break; + case Type::INVALID: throw std::invalid_argument("invalid ply property"); + } + os << " "; + srcOffset += PropertyTable[t].stride; +} + +void PlyFile::PlyFileImpl::write_property_binary(Type t, std::ostream & os, uint8_t * src, size_t & srcOffset) +{ + os.write((char *)src, PropertyTable[t].stride); + srcOffset += PropertyTable[t].stride; +} + +void PlyFile::PlyFileImpl::read(std::istream & is) +{ + // Parse but only get the data size + parse_data(is, true); + + std::vector> buffers; + for (auto & entry : userData) buffers.push_back(entry.second.data); + + // Since group-requested properties share the same cursor, we need to find unique cursors so we only allocate once + std::sort(buffers.begin(), buffers.end()); + buffers.erase(std::unique(buffers.begin(), buffers.end()), buffers.end()); + + // Not great, but since we sorted by ptrs on PlyData, need to remap back onto its cursor in the userData table + for (auto & b : buffers) + { + for (auto & entry : userData) + { + if (entry.second.data == b && b->buffer.get() == nullptr) + { + b->buffer = Buffer(entry.second.cursor->totalSizeBytes); + } + } + } + + // Populate the data + parse_data(is, false); +} + +void PlyFile::PlyFileImpl::write(std::ostream & os, bool _isBinary) +{ + if (_isBinary) write_binary_internal(os); + else write_ascii_internal(os); +} + +void PlyFile::PlyFileImpl::write_binary_internal(std::ostream & os) +{ + isBinary = true; + write_header(os); + + for (auto & e : elements) + { + for (size_t i = 0; i < e.size; ++i) + { + for (auto & p : e.properties) + { + auto & helper = userData[make_key(e.name, p.name)]; + if (p.isList) + { + uint8_t listSize[4] = {0, 0, 0, 0}; + std::memcpy(listSize, &p.listCount, sizeof(uint32_t)); + size_t dummyCount = 0; + write_property_binary(p.listType, os, listSize, dummyCount); + for (int j = 0; j < p.listCount; ++j) + { + write_property_binary(p.propertyType, os, (helper.data->buffer.get() + helper.cursor->byteOffset), helper.cursor->byteOffset); + } + } + else + { + write_property_binary(p.propertyType, os, (helper.data->buffer.get() + helper.cursor->byteOffset), helper.cursor->byteOffset); + } + } + } + } +} + +void PlyFile::PlyFileImpl::write_ascii_internal(std::ostream & os) +{ + write_header(os); + + for (auto & e : elements) + { + for (size_t i = 0; i < e.size; ++i) + { + for (auto & p : e.properties) + { + auto & helper = userData[make_key(e.name, p.name)]; + if (p.isList) + { + os << p.listCount << " "; + for (int j = 0; j < p.listCount; ++j) + { + write_property_ascii(p.propertyType, os, (helper.data->buffer.get() + helper.cursor->byteOffset), helper.cursor->byteOffset); + } + } + else + { + write_property_ascii(p.propertyType, os, (helper.data->buffer.get() + helper.cursor->byteOffset), helper.cursor->byteOffset); + } + } + os << "\n"; + } + } +} + +void PlyFile::PlyFileImpl::write_header(std::ostream & os) +{ + const std::locale & fixLoc = std::locale("C"); + os.imbue(fixLoc); + + os << "ply\n"; + if (isBinary) os << ((isBigEndian) ? "format binary_big_endian 1.0" : "format binary_little_endian 1.0") << "\n"; + else os << "format ascii 1.0\n"; + + for (const auto & comment : comments) os << "comment " << comment << "\n"; + + for (auto & e : elements) + { + os << "element " << e.name << " " << e.size << "\n"; + for (const auto & p : e.properties) + { + if (p.isList) + { + os << "property list " << PropertyTable[p.listType].str << " " + << PropertyTable[p.propertyType].str << " " << p.name << "\n"; + } + else + { + os << "property " << PropertyTable[p.propertyType].str << " " << p.name << "\n"; + } + } + } + os << "end_header\n"; +} + +// Returns the size (in bytes) +std::shared_ptr PlyFile::PlyFileImpl::request_properties_from_element(const std::string & elementKey, const std::initializer_list propertyKeys) +{ + // All requested properties in the userDataTable share the same cursor (thrown into the same flat array) + ParsingHelper helper; + helper.data = std::make_shared(); + helper.data->count = 0; + helper.data->t = Type::INVALID; + helper.cursor = std::make_shared(); + helper.cursor->byteOffset = 0; + helper.cursor->totalSizeBytes = 0; + + if (elements.size() == 0) throw std::runtime_error("parsed header had no elements defined. malformed file?"); + if (!propertyKeys.size()) throw std::invalid_argument("`propertyKeys` argument is empty"); + if (elementKey.size() == 0) throw std::invalid_argument("`elementKey` argument is empty"); + + const int elementIndex = find_element(elementKey, elements); + + // Sanity check if the user requested element is in the pre-parsed header + if (elementIndex >= 0) + { + // We found the element + const PlyElement & element = elements[elementIndex]; + + helper.data->count = element.size; + + // Find each of the keys + for (auto key : propertyKeys) + { + const int propertyIndex = find_property(key, element.properties); + + if (propertyIndex >= 0) + { + // We found the property + const PlyProperty & property = element.properties[propertyIndex]; + + helper.data->t = property.propertyType; // hmm.... + + auto result = userData.insert(std::pair(make_key(element.name, property.name), helper)); + if (result.second == false) throw std::invalid_argument("element-property key has already been requested: " + make_key(element.name, property.name)); + } + else throw std::invalid_argument("one of the property keys was not found in the header: " + key); + } + } + else throw std::invalid_argument("the element key was not found in the header: " + elementKey); + + return helper.data; +} + +void PlyFile::PlyFileImpl::add_properties_to_element(const std::string & elementKey, const std::initializer_list propertyKeys, const Type type, const size_t count, uint8_t * data, const Type listType, const size_t listCount) +{ + ParsingHelper helper; + helper.data = std::make_shared(); + helper.data->count = count; + helper.data->t = type; + helper.data->buffer = Buffer(data); + helper.cursor = std::make_shared(); + helper.cursor->byteOffset = 0; + helper.cursor->totalSizeBytes = 0; + + auto create_property_on_element = [&](PlyElement & e) + { + for (auto key : propertyKeys) + { + PlyProperty newProp = (listType == Type::INVALID) ? PlyProperty(type, key) : PlyProperty(listType, type, key, listCount); + /* auto result = */userData.insert(std::pair(make_key(elementKey, key), helper)); + e.properties.push_back(newProp); + } + }; + + int idx = find_element(elementKey, elements); + if (idx >= 0) + { + PlyElement & e = elements[idx]; + create_property_on_element(e); + } + else + { + PlyElement newElement = (listType == Type::INVALID) ? PlyElement(elementKey, count / propertyKeys.size()) : PlyElement(elementKey, count / listCount); + create_property_on_element(newElement); + elements.push_back(newElement); + } +} + +void PlyFile::PlyFileImpl::parse_data(std::istream & is, bool firstPass) +{ + std::function read; + std::function skip; + + const auto start = is.tellg(); + + if (isBinary) + { + read = [&](const Type t, void * dest, size_t & destOffset, std::istream & _is) { return read_property_binary(t, dest, destOffset, _is); }; + skip = [&](const PlyProperty & p, std::istream & _is) { return skip_property_binary(p, _is); }; + } + else + { + read = [&](const Type t, void * dest, size_t & destOffset, std::istream & _is) { return read_property_ascii(t, dest, destOffset, _is); }; + skip = [&](const PlyProperty & p, std::istream & _is) { return skip_property_ascii(p, _is); }; + } + + for (auto & element : elements) + { + for (size_t count = 0; count < element.size; ++count) + { + for (auto & property : element.properties) + { + auto cursorIt = userData.find(make_key(element.name, property.name)); + if (cursorIt != userData.end()) + { + auto & helper = cursorIt->second; + if (!firstPass) + { + if (property.isList) + { + size_t listSize = 0; + size_t dummyCount = 0; + read(property.listType, &listSize, dummyCount, is); + for (size_t i = 0; i < listSize; ++i) + { + read(property.propertyType, (helper.data->buffer.get() + helper.cursor->byteOffset), helper.cursor->byteOffset, is); + } + } + else + { + read(property.propertyType, (helper.data->buffer.get() + helper.cursor->byteOffset), helper.cursor->byteOffset, is); + } + } + else + { + helper.cursor->totalSizeBytes += skip(property, is); + } + } + else + { + skip(property, is); + } + } + } + } + + // Reset istream reader to the beginning + if (firstPass) is.seekg(start, is.beg); +} + +/////////////////////////////////// +// Pass-Through Public Interface // +/////////////////////////////////// + +PlyFile::PlyFile() { impl.reset(new PlyFileImpl()); }; +PlyFile::~PlyFile() { }; +bool PlyFile::parse_header(std::istream & is) { return impl->parse_header(is); } +void PlyFile::read(std::istream & is) { return impl->read(is); } +void PlyFile::write(std::ostream & os, bool isBinary) { return impl->write(os, isBinary); } +std::vector PlyFile::get_elements() const { return impl->elements; } +std::vector & PlyFile::get_comments() { return impl->comments; } +std::vector PlyFile::get_info() const { return impl->objInfo; } +std::shared_ptr PlyFile::request_properties_from_element(const std::string & elementKey, const std::initializer_list propertyKeys) +{ + return impl->request_properties_from_element(elementKey, propertyKeys); +} +void PlyFile::add_properties_to_element(const std::string & elementKey, const std::initializer_list propertyKeys, const Type type, const size_t count, uint8_t * data, const Type listType, const size_t listCount) +{ + return impl->add_properties_to_element(elementKey, propertyKeys, type, count, data, listType, listCount); +} \ No newline at end of file diff --git a/tinyply.h b/tinyply.h new file mode 100644 index 0000000..08fedf6 --- /dev/null +++ b/tinyply.h @@ -0,0 +1,119 @@ +// This software is in the public domain. Where that dedication is not +// recognized, you are granted a perpetual, irrevocable license to copy, +// distribute, and modify this file as you see fit. +// Authored in 2015 by Dimitri Diakopoulos (http://www.dimitridiakopoulos.com) +// https://github.com/ddiakopoulos/tinyply +// Version 2.0 + +#ifndef tinyply_h +#define tinyply_h + +#include +#include +#include +#include +#include +#include + +namespace tinyply +{ + + enum class Type : uint8_t + { + INVALID, + INT8, + UINT8, + INT16, + UINT16, + INT32, + UINT32, + FLOAT32, + FLOAT64 + }; + + struct PropertyInfo + { + int stride; + std::string str; + }; + + static std::map PropertyTable + { + { Type::INT8,{ 1, "char" } }, + { Type::UINT8,{ 1, "uchar" } }, + { Type::INT16,{ 2, "short" } }, + { Type::UINT16,{ 2, "ushort" } }, + { Type::INT32,{ 4, "int" } }, + { Type::UINT32,{ 4, "uint" } }, + { Type::FLOAT32,{ 4, "float" } }, + { Type::FLOAT64,{ 8, "double" } }, + { Type::INVALID,{ 0, "INVALID" } } + }; + + class Buffer + { + uint8_t * alias{ nullptr }; + struct delete_array { void operator()(uint8_t * p) { delete[] p; } }; + std::unique_ptr data; + size_t size; + public: + Buffer() {}; + Buffer(const size_t size) : data(new uint8_t[size], delete_array()), size(size) { alias = data.get(); } // allocating + Buffer(uint8_t * ptr) { alias = ptr; } // non-allocating, fixme: set size? + uint8_t * get() { return alias; } + size_t size_bytes() const { return size; } + }; + + struct PlyData + { + Type t; + size_t count; + Buffer buffer; + }; + + struct PlyProperty + { + PlyProperty(std::istream & is); + PlyProperty(Type type, std::string & _name) : name(_name), propertyType(type) {} + PlyProperty(Type list_type, Type prop_type, std::string & _name, int list_count) : name(_name), propertyType(prop_type), isList(true), listType(list_type), listCount(list_count) {} + std::string name; + Type propertyType; + bool isList{ false }; + Type listType{ Type::INVALID }; + int listCount{ 0 }; + }; + + struct PlyElement + { + PlyElement(std::istream & istream); + PlyElement(const std::string & _name, size_t count) : name(_name), size(count) {} + std::string name; + size_t size; + std::vector properties; + }; + + struct PlyFile + { + struct PlyFileImpl; + std::unique_ptr impl; + + PlyFile(); + ~PlyFile(); + + bool parse_header(std::istream & is); + + void read(std::istream & is); + + void write(std::ostream & os, bool isBinary); + + std::vector get_elements() const; + std::vector & get_comments(); + std::vector get_info() const; + + std::shared_ptr request_properties_from_element(const std::string & elementKey, const std::initializer_list propertyKeys); + void add_properties_to_element(const std::string & elementKey, const std::initializer_list propertyKeys, const Type type, const size_t count, uint8_t * data, const Type listType, const size_t listCount); + }; + +} // namesapce tinyply + +#endif // tinyply_h diff --git a/triangleCube.cpp b/triangleCube.cpp new file mode 100644 index 0000000..093a07e --- /dev/null +++ b/triangleCube.cpp @@ -0,0 +1,481 @@ +#include +#include +#include +#include +#include "triangleCube.h" +/* this version of SIGN3 shows some numerical instability, and is improved + * by using the uncommented macro that follows, and a different test with it */ +#ifdef OLD_TEST + #define SIGN3( A ) (((A).x<0)?4:0 | ((A).y<0)?2:0 | ((A).z<0)?1:0) +#else + #define EPS 10e-5 + #define SIGN3( A ) \ + (((A).x < EPS) ? 4 : 0 | ((A).x > -EPS) ? 32 : 0 | \ + ((A).y < EPS) ? 2 : 0 | ((A).y > -EPS) ? 16 : 0 | \ + ((A).z < EPS) ? 1 : 0 | ((A).z > -EPS) ? 8 : 0) +#endif + +/*___________________________________________________________________________*/ + +/* Which of the six face-plane(s) is point P outside of? */ + +long face_plane(Point3 p) +{ +long outcode; + + outcode = 0; + if (p.x > .5) outcode |= 0x01; + if (p.x < -.5) outcode |= 0x02; + if (p.y > .5) outcode |= 0x04; + if (p.y < -.5) outcode |= 0x08; + if (p.z > .5) outcode |= 0x10; + if (p.z < -.5) outcode |= 0x20; + return(outcode); +} + +/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */ + +/* Which of the twelve edge plane(s) is point P outside of? */ + +long bevel_2d(Point3 p) +{ +long outcode; + + outcode = 0; + if ( p.x + p.y > 1.0) outcode |= 0x001; + if ( p.x - p.y > 1.0) outcode |= 0x002; + if (-p.x + p.y > 1.0) outcode |= 0x004; + if (-p.x - p.y > 1.0) outcode |= 0x008; + if ( p.x + p.z > 1.0) outcode |= 0x010; + if ( p.x - p.z > 1.0) outcode |= 0x020; + if (-p.x + p.z > 1.0) outcode |= 0x040; + if (-p.x - p.z > 1.0) outcode |= 0x080; + if ( p.y + p.z > 1.0) outcode |= 0x100; + if ( p.y - p.z > 1.0) outcode |= 0x200; + if (-p.y + p.z > 1.0) outcode |= 0x400; + if (-p.y - p.z > 1.0) outcode |= 0x800; + return(outcode); +} + +/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */ + +/* Which of the eight corner plane(s) is point P outside of? */ + +long bevel_3d(Point3 p) +{ +long outcode; + + outcode = 0; + if (( p.x + p.y + p.z) > 1.5) outcode |= 0x01; + if (( p.x + p.y - p.z) > 1.5) outcode |= 0x02; + if (( p.x - p.y + p.z) > 1.5) outcode |= 0x04; + if (( p.x - p.y - p.z) > 1.5) outcode |= 0x08; + if ((-p.x + p.y + p.z) > 1.5) outcode |= 0x10; + if ((-p.x + p.y - p.z) > 1.5) outcode |= 0x20; + if ((-p.x - p.y + p.z) > 1.5) outcode |= 0x40; + if ((-p.x - p.y - p.z) > 1.5) outcode |= 0x80; + return(outcode); +} + +/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */ + +/* Test the point "alpha" of the way from P1 to P2 */ +/* See if it is on a face of the cube */ +/* Consider only faces in "mask" */ + +long check_point(Point3 p1, Point3 p2, float alpha, long mask) +{ +Point3 plane_point; + + plane_point.x = LERP(alpha, p1.x, p2.x); + plane_point.y = LERP(alpha, p1.y, p2.y); + plane_point.z = LERP(alpha, p1.z, p2.z); + return(face_plane(plane_point) & mask); +} + +/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */ + +/* Compute intersection of P1 --> P2 line segment with face planes */ +/* Then test intersection point to see if it is on cube face */ +/* Consider only face planes in "outcode_diff" */ +/* Note: Zero bits in "outcode_diff" means face line is outside of */ + +long check_line(Point3 p1, Point3 p2, long outcode_diff) +{ + + if ((0x01 & outcode_diff) != 0) + if (check_point(p1,p2,( .5-p1.x)/(p2.x-p1.x),0x3e) == INSIDE) return(INSIDE); + if ((0x02 & outcode_diff) != 0) + if (check_point(p1,p2,(-.5-p1.x)/(p2.x-p1.x),0x3d) == INSIDE) return(INSIDE); + if ((0x04 & outcode_diff) != 0) + if (check_point(p1,p2,( .5-p1.y)/(p2.y-p1.y),0x3b) == INSIDE) return(INSIDE); + if ((0x08 & outcode_diff) != 0) + if (check_point(p1,p2,(-.5-p1.y)/(p2.y-p1.y),0x37) == INSIDE) return(INSIDE); + if ((0x10 & outcode_diff) != 0) + if (check_point(p1,p2,( .5-p1.z)/(p2.z-p1.z),0x2f) == INSIDE) return(INSIDE); + if ((0x20 & outcode_diff) != 0) + if (check_point(p1,p2,(-.5-p1.z)/(p2.z-p1.z),0x1f) == INSIDE) return(INSIDE); + return(OUTSIDE); +} + +/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */ + +/* Test if 3D point is inside 3D triangle */ + +long point_triangle_intersection(Point3 p, Triangle3 t) +{ +long sign12,sign23,sign31; +Point3 vect12,vect23,vect31,vect1h,vect2h,vect3h; +Point3 cross12_1p,cross23_2p,cross31_3p; + +/* First, a quick bounding-box test: */ +/* If P is outside triangle bbox, there cannot be an intersection. */ + + if (p.x > MAX3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE); + if (p.y > MAX3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE); + if (p.z > MAX3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE); + if (p.x < MIN3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE); + if (p.y < MIN3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE); + if (p.z < MIN3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE); + +/* For each triangle side, make a vector out of it by subtracting vertexes; */ +/* make another vector from one vertex to point P. */ +/* The crossproduct of these two vectors is orthogonal to both and the */ +/* signs of its X,Y,Z components indicate whether P was to the inside or */ +/* to the outside of this triangle side. */ + + SUB(t.v1, t.v2, vect12) + SUB(t.v1, p, vect1h); + CROSS(vect12, vect1h, cross12_1p) + sign12 = SIGN3(cross12_1p); /* Extract X,Y,Z signs as 0..7 or 0...63 integer */ + + SUB(t.v2, t.v3, vect23) + SUB(t.v2, p, vect2h); + CROSS(vect23, vect2h, cross23_2p) + sign23 = SIGN3(cross23_2p); + + SUB(t.v3, t.v1, vect31) + SUB(t.v3, p, vect3h); + CROSS(vect31, vect3h, cross31_3p) + sign31 = SIGN3(cross31_3p); + +/* If all three crossproduct vectors agree in their component signs, */ +/* then the point must be inside all three. */ +/* P cannot be OUTSIDE all three sides simultaneously. */ + + /* this is the old test; with the revised SIGN3() macro, the test + * needs to be revised. */ +#ifdef OLD_TEST + if ((sign12 == sign23) && (sign23 == sign31)) + return(INSIDE); + else + return(OUTSIDE); +#else + return ((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE; +#endif +} + +/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */ + +/**********************************************/ +/* This is the main algorithm procedure. */ +/* Triangle t is compared with a unit cube, */ +/* centered on the origin. */ +/* It returns INSIDE (0) or OUTSIDE(1) if t */ +/* intersects or does not intersect the cube. */ +/**********************************************/ + +long t_c_intersection(Triangle3 t) +{ +long v1_test,v2_test,v3_test; +float d; +Point3 vect12,vect13,norm; +Point3 hitpp,hitpn,hitnp,hitnn; + +/* First compare all three vertexes with all six face-planes */ +/* If any vertex is inside the cube, return immediately! */ + + if ((v1_test = face_plane(t.v1)) == INSIDE) return(INSIDE); + if ((v2_test = face_plane(t.v2)) == INSIDE) return(INSIDE); + if ((v3_test = face_plane(t.v3)) == INSIDE) return(INSIDE); + +/* If all three vertexes were outside of one or more face-planes, */ +/* return immediately with a trivial rejection! */ + + if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE); + +/* Now do the same trivial rejection test for the 12 edge planes */ + + v1_test |= bevel_2d(t.v1) << 8; + v2_test |= bevel_2d(t.v2) << 8; + v3_test |= bevel_2d(t.v3) << 8; + if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE); + +/* Now do the same trivial rejection test for the 8 corner planes */ + + v1_test |= bevel_3d(t.v1) << 24; + v2_test |= bevel_3d(t.v2) << 24; + v3_test |= bevel_3d(t.v3) << 24; + if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE); + +/* If vertex 1 and 2, as a pair, cannot be trivially rejected */ +/* by the above tests, then see if the v1-->v2 triangle edge */ +/* intersects the cube. Do the same for v1-->v3 and v2-->v3. */ +/* Pass to the intersection algorithm the "OR" of the outcode */ +/* bits, so that only those cube faces which are spanned by */ +/* each triangle edge need be tested. */ + + if ((v1_test & v2_test) == 0) + if (check_line(t.v1,t.v2,v1_test|v2_test) == INSIDE) return(INSIDE); + if ((v1_test & v3_test) == 0) + if (check_line(t.v1,t.v3,v1_test|v3_test) == INSIDE) return(INSIDE); + if ((v2_test & v3_test) == 0) + if (check_line(t.v2,t.v3,v2_test|v3_test) == INSIDE) return(INSIDE); + +/* By now, we know that the triangle is not off to any side, */ +/* and that its sides do not penetrate the cube. We must now */ +/* test for the cube intersecting the interior of the triangle. */ +/* We do this by looking for intersections between the cube */ +/* diagonals and the triangle...first finding the intersection */ +/* of the four diagonals with the plane of the triangle, and */ +/* then if that intersection is inside the cube, pursuing */ +/* whether the intersection point is inside the triangle itself. */ + +/* To find plane of the triangle, first perform crossproduct on */ +/* two triangle side vectors to compute the normal vector. */ + + SUB(t.v1,t.v2,vect12); + SUB(t.v1,t.v3,vect13); + CROSS(vect12,vect13,norm) + +/* The normal vector "norm" X,Y,Z components are the coefficients */ +/* of the triangles AX + BY + CZ + D = 0 plane equation. If we */ +/* solve the plane equation for X=Y=Z (a diagonal), we get */ +/* -D/(A+B+C) as a metric of the distance from cube center to the */ +/* diagonal/plane intersection. If this is between -0.5 and 0.5, */ +/* the intersection is inside the cube. If so, we continue by */ +/* doing a point/triangle intersection. */ +/* Do this for all four diagonals. */ + + d = norm.x * t.v1.x + norm.y * t.v1.y + norm.z * t.v1.z; + float denom; + + /* if one of the diagonals is parallel to the plane, the other will intersect the plane */ + if(fabs(denom=(norm.x + norm.y + norm.z))>EPS) + /* skip parallel diagonals to the plane; division by 0 can occur */ + { + hitpp.x = hitpp.y = hitpp.z = d / denom; + if (fabs(hitpp.x) <= 0.5) + if (point_triangle_intersection(hitpp,t) == INSIDE) return(INSIDE); + } + if(fabs(denom=(norm.x + norm.y - norm.z))>EPS) + { + hitpn.z = -(hitpn.x = hitpn.y = d / denom); + if (fabs(hitpn.x) <= 0.5) + if (point_triangle_intersection(hitpn,t) == INSIDE) return(INSIDE); + } + if(fabs(denom=(norm.x - norm.y + norm.z))>EPS) + { + hitnp.y = -(hitnp.x = hitnp.z = d / denom); + if (fabs(hitnp.x) <= 0.5) + if (point_triangle_intersection(hitnp,t) == INSIDE) return(INSIDE); + } + if(fabs(denom=(norm.x - norm.y - norm.z))>EPS) + { + hitnn.y = hitnn.z = -(hitnn.x = d / denom); + if (fabs(hitnn.x) <= 0.5) + if (point_triangle_intersection(hitnn,t) == INSIDE) return(INSIDE); + } + +/* No edge touched the cube; no cube diagonal touched the triangle. */ +/* We're done...there was no intersection. */ + + return(OUTSIDE); + +} + +bool fequal(float x, float y) { return fabs(x-y) < 1e-9; } +int line_cube_face_intersection(Point3 p1, Point3 p2, Point3 cube_face, Point3 *intersec) { + // Cube face == (0, 0, -0.5) which means the face with z=-0.5 + // intersec is the return intersection point + // return value == 0 for no intersection is found + + Point3 p1_to_p2; + SUB(p2, p1, p1_to_p2); + + Point3 dim_mask; + dim_mask.x = cube_face.x == 0 ? 0 : 1; + dim_mask.y = cube_face.y == 0 ? 0 : 1; + dim_mask.z = cube_face.z == 0 ? 0 : 1; + // dimension mask should be like (0, 0, 1) + + // x + v*t = y + float x = DOT(p1, dim_mask); + float v = DOT(p1_to_p2, dim_mask); + float y = DOT(cube_face, dim_mask); + + if (fequal(v, 0.0) && !fequal(x, y)) return 0; + + float t = (y - x) / v; + if (t > 1 || t < 0) return 0; + + intersec->x = p1.x + p1_to_p2.x * t; + intersec->y = p1.y + p1_to_p2.y * t; + intersec->z = p1.z + p1_to_p2.z * t; + + if (dim_mask.x == 0) { + if (fabs(intersec->x) > 0.5) return 0; + } + if (dim_mask.y == 0) { + if (fabs(intersec->y) > 0.5) return 0; + } + if (dim_mask.z == 0) { + if (fabs(intersec->z) > 0.5) return 0; + } + return 1; +} +int line_triangle_intersec(Triangle3 tri, Point3 p1, Point3 p2, Point3 *intersec) { + Point3 t1, t2, norm; + SUB(tri.v2, tri.v1, t1); + SUB(tri.v3, tri.v1, t2); + CROSS(t1, t2, norm); + + // norm (a, b, c): ax+by+cz=k + float k = DOT(norm, tri.v1); + + Point3 p1_to_p2; + SUB(p2, p1, p1_to_p2); + // a(p1+v*t)_x + b(p1+v*t)_y + c(p1+v*t)_z = k + // t = (k - a*p1_x - b*p1_y - c*p1_z) / (a*v_x + b*v_y + c*v_z) + float numerator = k - DOT(norm, p1); + float denominator = DOT(norm, p1_to_p2); + + if (fequal(denominator, 0) && !fequal(numerator, 0)) return 0; + float t = numerator / denominator; + if (t > 1 || t < 0) return 0; + + intersec->x = p1.x + p1_to_p2.x * t; + intersec->y = p1.y + p1_to_p2.y * t; + intersec->z = p1.z + p1_to_p2.z * t; + + // Determine if the interec point is in the triangle + Point3 p; + SUB(*intersec, tri.v1, p); + + // intersec = p1 + a*(p2-p1) + b*(p3-p1) + // p = intersec - p1 + // By http://blackpawn.com/texts/pointinpoly/ + // u = ((v1.v1)(v2.v0)-(v1.v0)(v2.v1)) / ((v0.v0)(v1.v1) - (v0.v1)(v1.v0)) + // v = ((v0.v0)(v2.v1)-(v0.v1)(v2.v0)) / ((v0.v0)(v1.v1) - (v0.v1)(v1.v0)) + float pp = DOT(p,p); + float pt1 = DOT(p,t1); + float pt2 = DOT(p,t2); + float t1t1 = DOT(t1,t1); + float t1t2 = DOT(t1,t2); + denominator = pp*t1t1 - pt1*pt1; + float a = (t1t1*pt2 - pt1*t1t2) / denominator; + float b = (pp*t1t2 - pt1*pt2) / denominator; + + if( (a >= 0) && (b >= 0) && (a+b <= 1) ) return 1; + return 0; +} +Point3 points_centroid(std::vector& pt, int n) { + Point3 total(0,0,0); + for (int i = 0; i < n; i++) { + total.x += pt[i].x; + total.y += pt[i].y; + total.z += pt[i].z; + } + total.x /= n; + total.y /= n; + total.z /= n; + return total; +} + +const Point3 CUBE_FACES[] = {Point3(0,0,0.5), Point3(0,0,-0.5), Point3(0,0.5,0), Point3(0,-0.5,0), Point3(0.5,0,0), Point3(-0.5,0,0)}; +// const Point3 CUBE_VERTICES[] = {Point3(0.5,0.5,0.5), Point3(0.5,0.5,-0.5), Point3(0.5,-0.5,0.5), Point3(0.5,-0.5,-0.5), Point3(-0.5,0.5,0.5), Point3(-0.5,0.5,-0.5), Point3(-0.5,-0.5,0.5), Point3(-0.5,-0.5,-0.5)}; +const Point3 CUBE_EDGES[12][2] = { + { Point3(.5, .5, .5), Point3(.5, -.5, .5) }, + { Point3(.5, .5, .5), Point3(-.5, .5, .5) }, + { Point3(-.5, .5, .5), Point3(-.5, -.5, .5) }, + { Point3(-.5, -.5, .5), Point3(.5, -.5, .5) }, + // + { Point3(-.5, -.5, -.5), Point3(.5, -.5, -.5) }, + { Point3(-.5, -.5, -.5), Point3(.5, -.5, -.5) }, + { Point3(-.5, -.5, -.5), Point3(.5, -.5, -.5) }, + { Point3(-.5, -.5, -.5), Point3(.5, -.5, -.5) }, + // + { Point3(.5, .5, .5), Point3(.5, .5, -.5) }, + { Point3(.5, -.5, .5), Point3(.5, -.5, -.5) }, + { Point3(-.5, .5, .5), Point3(-.5, .5, -.5) }, + { Point3(-.5, -.5, .5), Point3(-.5, -.5, -.5) } +}; + +bool point_equal(Point3 l, Point3 r) { return fabs(l.x-r.x)+fabs(l.y-r.y)+fabs(l.z-r.z) < 1e-7; } +float triangle_area_inside_cube(Triangle3 tri) { + // Compute each face and line interesection + // pt save all vertices of the inside area ploygonal + std::vector pt; + + // If the vertex of triangle lies inside the cube, it's the vertex of the ploygonal + if (face_plane(tri.v1) == 0) pt.push_back(tri.v1); + if (face_plane(tri.v2) == 0) pt.push_back(tri.v2); + if (face_plane(tri.v3) == 0) pt.push_back(tri.v3); + + Point3 intersec; + // Cube face & triangle line + for (int i = 0; i < 6; i++) { + if (line_cube_face_intersection(tri.v1, tri.v2, CUBE_FACES[i], &intersec) == 1) pt.push_back(intersec); + if (line_cube_face_intersection(tri.v2, tri.v3, CUBE_FACES[i], &intersec) == 1) pt.push_back(intersec); + if (line_cube_face_intersection(tri.v3, tri.v1, CUBE_FACES[i], &intersec) == 1) pt.push_back(intersec); + } + + for (int i = 0; i < 12; i++) { + if (line_triangle_intersec(tri, CUBE_EDGES[i][0], CUBE_EDGES[i][1], &intersec)) { + // printf("inter %f %f %f\n", CUBE_EDGES[i][0].x, CUBE_EDGES[i][0].y, CUBE_EDGES[i][0].z); + // printf("inter %f %f %f\n", CUBE_EDGES[i][1].x, CUBE_EDGES[i][1].y, CUBE_EDGES[i][1].z); + // printf("\n"); + pt.push_back(intersec); + } + } + + + // Remove duplicate points + // printf("Found %d points:\n", pt.size()); + if (pt.size() < 3) return 0; + // for (int i = 0; i < pt.size(); i++) {printf("%f %f %f\n", pt[i].x, pt[i].y, pt[i].z);} + std::vector::iterator it; + it = std::unique(pt.begin(), pt.end(), point_equal); + pt.resize( std::distance(pt.begin(), it-1) ); + // printf("%d\n", pt.size()); + + // it len(pt) < 3, no area inside cube + if (pt.size() < 3) return 0; + + // Sort each point with (any-)clockwise order + Point3 tmp1, tmp2, tmp3, tmp4; + Point3 normal; + SUB(tri.v2, tri.v1, tmp1); + SUB(tri.v3, tri.v1, tmp2); + CROSS(tmp1, tmp2, normal); + Point3 centroid = points_centroid(pt, pt.size()); + // printf("%f %f %f\n", centroid.x, centroid.y, centroid.z); + for (int i = 0; i < pt.size(); i++) { + for (int j = 0; j < i; j++) { + SUB(pt[i], centroid, tmp1); + SUB(pt[j], centroid, tmp2); + + CROSS(tmp1, tmp2, tmp3); + + if (DOT(tmp3, normal) >= 0) { + std::swap(pt[i], pt[j]); + } + } + } + // Let pt[n] = pt[0] to complete polygon + pt.push_back(pt[0]); + // printf("\nRemoved Duplicated and sorted, %d points: \n", pt.size()); + // for (int i = 0; i < pt.size(); i++) {printf("%f %f %f\n", pt[i].x, pt[i].y, pt[i].z);} + // Calculate size of area + return fabs(area3D_Polygon(pt.size()-1, pt, normal)); +} diff --git a/triangleCube.h b/triangleCube.h new file mode 100644 index 0000000..cc0515d --- /dev/null +++ b/triangleCube.h @@ -0,0 +1,45 @@ +#include +#include +#ifndef triangleCube_h +#define triangleCube_h +#define DOT(A, B) ((A).x * (B).x + (A).y * (B).y + (A).z * (B).z) +#define CROSS( A, B, C ) { \ + (C).x = (A).y * (B).z - (A).z * (B).y; \ + (C).y = -(A).x * (B).z + (A).z * (B).x; \ + (C).z = (A).x * (B).y - (A).y * (B).x; \ + } +#define SUB( A, B, C ) { \ + (C).x = (A).x - (B).x; \ + (C).y = (A).y - (B).y; \ + (C).z = (A).z - (B).z; \ + } + +#define ADD( A, B, C ) { \ + (C).x = (A).x + (B).x; \ + (C).y = (A).y + (B).y; \ + (C).z = (A).z + (B).z; \ + } +#define LERP( A, B, C) ((B)+(A)*((C)-(B))) +#define MIN3(a,b,c) ((((a)<(b))&&((a)<(c))) ? (a) : (((b)<(c)) ? (b) : (c))) +#define MAX3(a,b,c) ((((a)>(b))&&((a)>(c))) ? (a) : (((b)>(c)) ? (b) : (c))) +#define INSIDE 0 +#define OUTSIDE 1 + +typedef struct Point3 { + Point3 () {} + Point3 (float _x, float _y, float _z): x(_x), y(_y), z(_z) {} + float x; + float y; + float z; +} Point3; + +typedef struct{ + Point3 v1; /* Vertex1 */ + Point3 v2; /* Vertex2 */ + Point3 v3; /* Vertex3 */ + } Triangle3; + +long t_c_intersection(Triangle3); +float area3D_Polygon(int, std::vector&, Point3); +float triangle_area_inside_cube(Triangle3); +#endif