diff options
Diffstat (limited to 'core/math/triangle_mesh.cpp')
-rw-r--r-- | core/math/triangle_mesh.cpp | 416 |
1 files changed, 191 insertions, 225 deletions
diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 247cb90a48..93c6b2786e 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -29,81 +29,73 @@ #include "triangle_mesh.h" #include "sort.h" +int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc) { - -int TriangleMesh::_create_bvh(BVH*p_bvh,BVH** p_bb,int p_from,int p_size,int p_depth,int&max_depth,int&max_alloc) { - - - if (p_depth>max_depth) { - max_depth=p_depth; + if (p_depth > max_depth) { + max_depth = p_depth; } - if (p_size==1) { + if (p_size == 1) { - - return p_bb[p_from]-p_bvh; - } else if (p_size==0) { + return p_bb[p_from] - p_bvh; + } else if (p_size == 0) { return -1; } - Rect3 aabb; - aabb=p_bb[p_from]->aabb; - for(int i=1;i<p_size;i++) { + aabb = p_bb[p_from]->aabb; + for (int i = 1; i < p_size; i++) { - aabb.merge_with(p_bb[p_from+i]->aabb); + aabb.merge_with(p_bb[p_from + i]->aabb); } - int li=aabb.get_longest_axis_index(); + int li = aabb.get_longest_axis_index(); - switch(li) { + switch (li) { case Vector3::AXIS_X: { - SortArray<BVH*,BVHCmpX> sort_x; - sort_x.nth_element(0,p_size,p_size/2,&p_bb[p_from]); + SortArray<BVH *, BVHCmpX> sort_x; + sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]); //sort_x.sort(&p_bb[p_from],p_size); } break; case Vector3::AXIS_Y: { - SortArray<BVH*,BVHCmpY> sort_y; - sort_y.nth_element(0,p_size,p_size/2,&p_bb[p_from]); + SortArray<BVH *, BVHCmpY> sort_y; + sort_y.nth_element(0, p_size, p_size / 2, &p_bb[p_from]); //sort_y.sort(&p_bb[p_from],p_size); } break; case Vector3::AXIS_Z: { - SortArray<BVH*,BVHCmpZ> sort_z; - sort_z.nth_element(0,p_size,p_size/2,&p_bb[p_from]); + SortArray<BVH *, BVHCmpZ> sort_z; + sort_z.nth_element(0, p_size, p_size / 2, &p_bb[p_from]); //sort_z.sort(&p_bb[p_from],p_size); } break; } + int left = _create_bvh(p_bvh, p_bb, p_from, p_size / 2, p_depth + 1, max_depth, max_alloc); + int right = _create_bvh(p_bvh, p_bb, p_from + p_size / 2, p_size - p_size / 2, p_depth + 1, max_depth, max_alloc); - int left = _create_bvh(p_bvh,p_bb,p_from,p_size/2,p_depth+1,max_depth,max_alloc); - int right = _create_bvh(p_bvh,p_bb,p_from+p_size/2,p_size-p_size/2,p_depth+1,max_depth,max_alloc); - - int index=max_alloc++; + int index = max_alloc++; BVH *_new = &p_bvh[index]; - _new->aabb=aabb; - _new->center=aabb.pos+aabb.size*0.5; - _new->face_index=-1; - _new->left=left; - _new->right=right; + _new->aabb = aabb; + _new->center = aabb.pos + aabb.size * 0.5; + _new->face_index = -1; + _new->left = left; + _new->right = right; return index; - } +void TriangleMesh::create(const PoolVector<Vector3> &p_faces) { -void TriangleMesh::create(const PoolVector<Vector3>& p_faces) { + valid = false; - valid=false; - - int fc=p_faces.size(); - ERR_FAIL_COND(!fc || ((fc%3) != 0)); - fc/=3; + int fc = p_faces.size(); + ERR_FAIL_COND(!fc || ((fc % 3) != 0)); + fc /= 3; triangles.resize(fc); - bvh.resize(fc*3); //will never be larger than this (todo make better) + bvh.resize(fc * 3); //will never be larger than this (todo make better) PoolVector<BVH>::Write bw = bvh.write(); { @@ -114,148 +106,143 @@ void TriangleMesh::create(const PoolVector<Vector3>& p_faces) { PoolVector<Vector3>::Read r = p_faces.read(); PoolVector<Triangle>::Write w = triangles.write(); - Map<Vector3,int> db; + Map<Vector3, int> db; - for(int i=0;i<fc;i++) { + for (int i = 0; i < fc; i++) { - Triangle&f=w[i]; - const Vector3 *v=&r[i*3]; + Triangle &f = w[i]; + const Vector3 *v = &r[i * 3]; - for(int j=0;j<3;j++) { + for (int j = 0; j < 3; j++) { - int vidx=-1; - Vector3 vs=v[j].snapped(0.0001); - Map<Vector3,int>::Element *E=db.find(vs); + int vidx = -1; + Vector3 vs = v[j].snapped(0.0001); + Map<Vector3, int>::Element *E = db.find(vs); if (E) { - vidx=E->get(); + vidx = E->get(); } else { - vidx=db.size(); - db[vs]=vidx; + vidx = db.size(); + db[vs] = vidx; } - f.indices[j]=vidx; - if (j==0) - bw[i].aabb.pos=vs; + f.indices[j] = vidx; + if (j == 0) + bw[i].aabb.pos = vs; else bw[i].aabb.expand_to(vs); } - f.normal=Face3(r[i*3+0],r[i*3+1],r[i*3+2]).get_plane().get_normal(); + f.normal = Face3(r[i * 3 + 0], r[i * 3 + 1], r[i * 3 + 2]).get_plane().get_normal(); - bw[i].left=-1; - bw[i].right=-1; - bw[i].face_index=i; - bw[i].center=bw[i].aabb.pos+bw[i].aabb.size*0.5; + bw[i].left = -1; + bw[i].right = -1; + bw[i].face_index = i; + bw[i].center = bw[i].aabb.pos + bw[i].aabb.size * 0.5; } vertices.resize(db.size()); PoolVector<Vector3>::Write vw = vertices.write(); - for (Map<Vector3,int>::Element *E=db.front();E;E=E->next()) { - vw[E->get()]=E->key(); + for (Map<Vector3, int>::Element *E = db.front(); E; E = E->next()) { + vw[E->get()] = E->key(); } - } - PoolVector<BVH*> bwptrs; + PoolVector<BVH *> bwptrs; bwptrs.resize(fc); - PoolVector<BVH*>::Write bwp = bwptrs.write(); - for(int i=0;i<fc;i++) { + PoolVector<BVH *>::Write bwp = bwptrs.write(); + for (int i = 0; i < fc; i++) { - bwp[i]=&bw[i]; + bwp[i] = &bw[i]; } - max_depth=0; - int max_alloc=fc; - int max=_create_bvh(bw.ptr(),bwp.ptr(),0,fc,1,max_depth,max_alloc); + max_depth = 0; + int max_alloc = fc; + int max = _create_bvh(bw.ptr(), bwp.ptr(), 0, fc, 1, max_depth, max_alloc); - bw=PoolVector<BVH>::Write(); //clearup + bw = PoolVector<BVH>::Write(); //clearup bvh.resize(max_alloc); //resize back - valid=true; - + valid = true; } +Vector3 TriangleMesh::get_area_normal(const Rect3 &p_aabb) const { -Vector3 TriangleMesh::get_area_normal(const Rect3& p_aabb) const { - - uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth); + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { - TEST_AABB_BIT=0, - VISIT_LEFT_BIT=1, - VISIT_RIGHT_BIT=2, - VISIT_DONE_BIT=3, - VISITED_BIT_SHIFT=29, - NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1, - VISITED_BIT_MASK=~NODE_IDX_MASK, - + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, }; - int n_count=0; + int n_count = 0; Vector3 n; - int level=0; + int level = 0; PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr=vertices.read(); - PoolVector<BVH>::Read bvhr=bvh.read(); + PoolVector<Vector3>::Read verticesr = vertices.read(); + PoolVector<BVH>::Read bvhr = bvh.read(); - const Triangle *triangleptr=trianglesr.ptr(); - int pos=bvh.size()-1; + const Triangle *triangleptr = trianglesr.ptr(); + int pos = bvh.size() - 1; const BVH *bvhptr = bvhr.ptr(); - stack[0]=pos; - while(true) { + stack[0] = pos; + while (true) { - uint32_t node = stack[level]&NODE_IDX_MASK; - const BVH &b = bvhptr[ node ]; - bool done=false; + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &b = bvhptr[node]; + bool done = false; - switch(stack[level]>>VISITED_BIT_SHIFT) { + switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = b.aabb.intersects(p_aabb); if (!valid) { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index>=0) { + if (b.face_index >= 0) { - const Triangle &s=triangleptr[ b.face_index ]; - n+=s.normal; + const Triangle &s = triangleptr[b.face_index]; + n += s.normal; n_count++; - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.left|TEST_AABB_BIT; + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.right|TEST_AABB_BIT; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level==0) { - done=true; + if (level == 0) { + done = true; break; } else level--; @@ -263,121 +250,111 @@ Vector3 TriangleMesh::get_area_normal(const Rect3& p_aabb) const { } } - if (done) break; } - - if (n_count>0) - n/=n_count; + if (n_count > 0) + n /= n_count; return n; - } +bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { -bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const { - - - uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth); + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { - TEST_AABB_BIT=0, - VISIT_LEFT_BIT=1, - VISIT_RIGHT_BIT=2, - VISIT_DONE_BIT=3, - VISITED_BIT_SHIFT=29, - NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1, - VISITED_BIT_MASK=~NODE_IDX_MASK, - + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, }; - Vector3 n = (p_end-p_begin).normalized(); - real_t d=1e10; - bool inters=false; + Vector3 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; - int level=0; + int level = 0; PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr=vertices.read(); - PoolVector<BVH>::Read bvhr=bvh.read(); + PoolVector<Vector3>::Read verticesr = vertices.read(); + PoolVector<BVH>::Read bvhr = bvh.read(); - const Triangle *triangleptr=trianglesr.ptr(); - const Vector3 *vertexptr=verticesr.ptr(); - int pos=bvh.size()-1; + const Triangle *triangleptr = trianglesr.ptr(); + const Vector3 *vertexptr = verticesr.ptr(); + int pos = bvh.size() - 1; const BVH *bvhptr = bvhr.ptr(); - stack[0]=pos; - while(true) { + stack[0] = pos; + while (true) { - uint32_t node = stack[level]&NODE_IDX_MASK; - const BVH &b = bvhptr[ node ]; - bool done=false; + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &b = bvhptr[node]; + bool done = false; - switch(stack[level]>>VISITED_BIT_SHIFT) { + switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - - bool valid = b.aabb.intersects_segment(p_begin,p_end); + bool valid = b.aabb.intersects_segment(p_begin, p_end); //bool valid = b.aabb.intersects(ray_aabb); if (!valid) { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index>=0) { - - const Triangle &s=triangleptr[ b.face_index ]; - Face3 f3(vertexptr[ s.indices[0] ],vertexptr[ s.indices[1] ],vertexptr[ s.indices[2] ]); + if (b.face_index >= 0) { + const Triangle &s = triangleptr[b.face_index]; + Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]); Vector3 res; - if (f3.intersects_segment(p_begin,p_end,&res)) { - + if (f3.intersects_segment(p_begin, p_end, &res)) { real_t nd = n.dot(res); - if (nd<d) { + if (nd < d) { - d=nd; - r_point=res; - r_normal=f3.get_plane().get_normal(); - inters=true; + d = nd; + r_point = res; + r_normal = f3.get_plane().get_normal(); + inters = true; } - } - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.left|TEST_AABB_BIT; + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.right|TEST_AABB_BIT; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level==0) { - done=true; + if (level == 0) { + done = true; break; } else level--; @@ -385,121 +362,112 @@ bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end } } - if (done) break; } - if (inters) { - if (n.dot(r_normal)>0) - r_normal=-r_normal; + if (n.dot(r_normal) > 0) + r_normal = -r_normal; } return inters; } +bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal) const { -bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vector3 &r_point, Vector3 &r_normal) const { - - - uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth); + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); enum { - TEST_AABB_BIT=0, - VISIT_LEFT_BIT=1, - VISIT_RIGHT_BIT=2, - VISIT_DONE_BIT=3, - VISITED_BIT_SHIFT=29, - NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1, - VISITED_BIT_MASK=~NODE_IDX_MASK, - + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, }; Vector3 n = p_dir; - real_t d=1e20; - bool inters=false; + real_t d = 1e20; + bool inters = false; - int level=0; + int level = 0; PoolVector<Triangle>::Read trianglesr = triangles.read(); - PoolVector<Vector3>::Read verticesr=vertices.read(); - PoolVector<BVH>::Read bvhr=bvh.read(); + PoolVector<Vector3>::Read verticesr = vertices.read(); + PoolVector<BVH>::Read bvhr = bvh.read(); - const Triangle *triangleptr=trianglesr.ptr(); - const Vector3 *vertexptr=verticesr.ptr(); - int pos=bvh.size()-1; + const Triangle *triangleptr = trianglesr.ptr(); + const Vector3 *vertexptr = verticesr.ptr(); + int pos = bvh.size() - 1; const BVH *bvhptr = bvhr.ptr(); - stack[0]=pos; - while(true) { + stack[0] = pos; + while (true) { - uint32_t node = stack[level]&NODE_IDX_MASK; - const BVH &b = bvhptr[ node ]; - bool done=false; + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &b = bvhptr[node]; + bool done = false; - switch(stack[level]>>VISITED_BIT_SHIFT) { + switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - - bool valid = b.aabb.intersects_ray(p_begin,p_dir); + bool valid = b.aabb.intersects_ray(p_begin, p_dir); if (!valid) { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (b.face_index>=0) { - - const Triangle &s=triangleptr[ b.face_index ]; - Face3 f3(vertexptr[ s.indices[0] ],vertexptr[ s.indices[1] ],vertexptr[ s.indices[2] ]); + if (b.face_index >= 0) { + const Triangle &s = triangleptr[b.face_index]; + Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]); Vector3 res; - if (f3.intersects_ray(p_begin,p_dir,&res)) { - + if (f3.intersects_ray(p_begin, p_dir, &res)) { real_t nd = n.dot(res); - if (nd<d) { + if (nd < d) { - d=nd; - r_point=res; - r_normal=f3.get_plane().get_normal(); - inters=true; + d = nd; + r_point = res; + r_normal = f3.get_plane().get_normal(); + inters = true; } - } - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } continue; } case VISIT_LEFT_BIT: { - stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.left|TEST_AABB_BIT; + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.left | TEST_AABB_BIT; level++; continue; } case VISIT_RIGHT_BIT: { - stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; - stack[level+1]=b.right|TEST_AABB_BIT; + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.right | TEST_AABB_BIT; level++; continue; } case VISIT_DONE_BIT: { - if (level==0) { - done=true; + if (level == 0) { + done = true; break; } else level--; @@ -507,16 +475,14 @@ bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vec } } - if (done) break; } - if (inters) { - if (n.dot(r_normal)>0) - r_normal=-r_normal; + if (n.dot(r_normal) > 0) + r_normal = -r_normal; } return inters; @@ -536,13 +502,13 @@ PoolVector<Face3> TriangleMesh::get_faces() const { int ts = triangles.size(); faces.resize(triangles.size()); - PoolVector<Face3>::Write w=faces.write(); + PoolVector<Face3>::Write w = faces.write(); PoolVector<Triangle>::Read r = triangles.read(); PoolVector<Vector3>::Read rv = vertices.read(); - for(int i=0;i<ts;i++) { - for(int j=0;j<3;j++) { - w[i].vertex[j]=rv[r[i].indices[j]]; + for (int i = 0; i < ts; i++) { + for (int j = 0; j < 3; j++) { + w[i].vertex[j] = rv[r[i].indices[j]]; } } @@ -552,6 +518,6 @@ PoolVector<Face3> TriangleMesh::get_faces() const { TriangleMesh::TriangleMesh() { - valid=false; - max_depth=0; + valid = false; + max_depth = 0; } |