00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "fcl/BVH/BVH_model.h"
00038 #include "fcl/BV/BV.h"
00039 #include <iostream>
00040 #include <string.h>
00041
00042 namespace fcl
00043 {
00044
00045 template<typename BV>
00046 BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other),
00047 num_tris(other.num_tris),
00048 num_vertices(other.num_vertices),
00049 build_state(other.build_state),
00050 bv_splitter(other.bv_splitter),
00051 bv_fitter(other.bv_fitter),
00052 num_tris_allocated(other.num_tris),
00053 num_vertices_allocated(other.num_vertices)
00054 {
00055 if(other.vertices)
00056 {
00057 vertices = new Vec3f[num_vertices];
00058 memcpy(vertices, other.vertices, sizeof(Vec3f) * num_vertices);
00059 }
00060 else
00061 vertices = NULL;
00062
00063 if(other.tri_indices)
00064 {
00065 tri_indices = new Triangle[num_tris];
00066 memcpy(tri_indices, other.tri_indices, sizeof(Triangle) * num_tris);
00067 }
00068 else
00069 tri_indices = NULL;
00070
00071 if(other.prev_vertices)
00072 {
00073 prev_vertices = new Vec3f[num_vertices];
00074 memcpy(prev_vertices, other.prev_vertices, sizeof(Vec3f) * num_vertices);
00075 }
00076 else
00077 prev_vertices = NULL;
00078
00079 if(other.primitive_indices)
00080 {
00081 int num_primitives = 0;
00082 switch(other.getModelType())
00083 {
00084 case BVH_MODEL_TRIANGLES:
00085 num_primitives = num_tris;
00086 break;
00087 case BVH_MODEL_POINTCLOUD:
00088 num_primitives = num_vertices;
00089 break;
00090 default:
00091 ;
00092 }
00093
00094 primitive_indices = new unsigned int[num_primitives];
00095 memcpy(primitive_indices, other.primitive_indices, sizeof(unsigned int) * num_primitives);
00096 }
00097 else
00098 primitive_indices = NULL;
00099
00100 num_bvs = num_bvs_allocated = other.num_bvs;
00101 if(other.bvs)
00102 {
00103 bvs = new BVNode<BV>[num_bvs];
00104 memcpy(bvs, other.bvs, sizeof(BVNode<BV>) * num_bvs);
00105 }
00106 else
00107 bvs = NULL;
00108 }
00109
00110
00111 template<typename BV>
00112 int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
00113 {
00114 if(build_state != BVH_BUILD_STATE_EMPTY)
00115 {
00116 delete [] vertices; vertices = NULL;
00117 delete [] tri_indices; tri_indices = NULL;
00118 delete [] bvs; bvs = NULL;
00119 delete [] prev_vertices; prev_vertices = NULL;
00120 delete [] primitive_indices; primitive_indices = NULL;
00121
00122 num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0;
00123 }
00124
00125 if(num_tris_ <= 0) num_tris_ = 8;
00126 if(num_vertices_ <= 0) num_vertices_ = 8;
00127
00128 num_vertices_allocated = num_vertices_;
00129 num_tris_allocated = num_tris_;
00130
00131 tri_indices = new Triangle[num_tris_allocated];
00132 vertices = new Vec3f[num_vertices_allocated];
00133
00134 if(!tri_indices)
00135 {
00136 std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl;
00137 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00138 }
00139 if(!vertices)
00140 {
00141 std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl;
00142 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00143 }
00144
00145 if(build_state != BVH_BUILD_STATE_EMPTY)
00146 {
00147 std::cerr << "BVH Warning! Call beginModel() on a BVHModel that is not empty. This model was cleared and previous triangles/vertices were lost." << std::endl;
00148 build_state = BVH_BUILD_STATE_EMPTY;
00149 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00150 }
00151
00152 build_state = BVH_BUILD_STATE_BEGUN;
00153
00154 return BVH_OK;
00155 }
00156
00157
00158 template<typename BV>
00159 int BVHModel<BV>::addVertex(const Vec3f& p)
00160 {
00161 if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
00162 {
00163 std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
00164 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00165 }
00166
00167 if(num_vertices >= num_vertices_allocated)
00168 {
00169 Vec3f* temp = new Vec3f[num_vertices_allocated * 2];
00170 if(!temp)
00171 {
00172 std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl;
00173 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00174 }
00175
00176 memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
00177 delete [] vertices;
00178 vertices = temp;
00179 num_vertices_allocated *= 2;
00180 }
00181
00182 vertices[num_vertices] = p;
00183 num_vertices += 1;
00184
00185 return BVH_OK;
00186 }
00187
00188 template<typename BV>
00189 int BVHModel<BV>::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
00190 {
00191 if(build_state == BVH_BUILD_STATE_PROCESSED)
00192 {
00193 std::cerr << "BVH Warning! Call addTriangle() in a wrong order. addTriangle() was ignored. Must do a beginModel() to clear the model for addition of new triangles." << std::endl;
00194 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00195 }
00196
00197 if(num_vertices + 2 >= num_vertices_allocated)
00198 {
00199 Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2];
00200 if(!temp)
00201 {
00202 std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl;
00203 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00204 }
00205
00206 memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
00207 delete [] vertices;
00208 vertices = temp;
00209 num_vertices_allocated = num_vertices_allocated * 2 + 2;
00210 }
00211
00212 int offset = num_vertices;
00213
00214 vertices[num_vertices] = p1;
00215 num_vertices++;
00216 vertices[num_vertices] = p2;
00217 num_vertices++;
00218 vertices[num_vertices] = p3;
00219 num_vertices++;
00220
00221 if(num_tris >= num_tris_allocated)
00222 {
00223 Triangle* temp = new Triangle[num_tris_allocated * 2];
00224 if(!temp)
00225 {
00226 std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl;
00227 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00228 }
00229
00230 memcpy(temp, tri_indices, sizeof(Triangle) * num_tris);
00231 delete [] tri_indices;
00232 tri_indices = temp;
00233 num_tris_allocated *= 2;
00234 }
00235
00236 tri_indices[num_tris].set(offset, offset + 1, offset + 2);
00237
00238 return BVH_OK;
00239 }
00240
00241 template<typename BV>
00242 int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps)
00243 {
00244 if(build_state == BVH_BUILD_STATE_PROCESSED)
00245 {
00246 std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
00247 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00248 }
00249
00250 int num_vertices_to_add = ps.size();
00251
00252 if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
00253 {
00254 Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
00255 if(!temp)
00256 {
00257 std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
00258 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00259 }
00260
00261 memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
00262 delete [] vertices;
00263 vertices = temp;
00264 num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
00265 }
00266
00267 for(int i = 0; i < num_vertices_to_add; ++i)
00268 {
00269 vertices[num_vertices] = ps[i];
00270 num_vertices++;
00271 }
00272
00273 return BVH_OK;
00274 }
00275
00276 template<typename BV>
00277 int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts)
00278 {
00279 if(build_state == BVH_BUILD_STATE_PROCESSED)
00280 {
00281 std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
00282 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00283 }
00284
00285 int num_vertices_to_add = ps.size();
00286
00287 if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
00288 {
00289 Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
00290 if(!temp)
00291 {
00292 std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
00293 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00294 }
00295
00296 memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
00297 delete [] vertices;
00298 vertices = temp;
00299 num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
00300 }
00301
00302 int offset = num_vertices;
00303
00304 for(int i = 0; i < num_vertices_to_add; ++i)
00305 {
00306 vertices[num_vertices] = ps[i];
00307 num_vertices++;
00308 }
00309
00310
00311 int num_tris_to_add = ts.size();
00312
00313 if(num_tris + num_tris_to_add - 1 >= num_tris_allocated)
00314 {
00315 Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
00316 if(!temp)
00317 {
00318 std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl;
00319 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00320 }
00321
00322 memcpy(temp, tri_indices, sizeof(Triangle) * num_tris);
00323 delete [] tri_indices;
00324 tri_indices = temp;
00325 num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1;
00326 }
00327
00328 for(int i = 0; i < num_tris_to_add; ++i)
00329 {
00330 const Triangle& t = ts[i];
00331 tri_indices[num_tris].set(t[0] + offset, t[1] + offset, t[2] + offset);
00332 num_tris++;
00333 }
00334
00335 return BVH_OK;
00336 }
00337
00338 template<typename BV>
00339 int BVHModel<BV>::endModel()
00340 {
00341 if(build_state != BVH_BUILD_STATE_BEGUN)
00342 {
00343 std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was ignored." << std::endl;
00344 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00345 }
00346
00347 if(num_tris == 0 && num_vertices == 0)
00348 {
00349 std::cerr << "BVH Error! endModel() called on model with no triangles and vertices." << std::endl;
00350 return BVH_ERR_BUILD_EMPTY_MODEL;
00351 }
00352
00353 if(num_tris_allocated > num_tris)
00354 {
00355 Triangle* new_tris = new Triangle[num_tris];
00356 if(!new_tris)
00357 {
00358 std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl;
00359 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00360 }
00361 memcpy(new_tris, tri_indices, sizeof(Triangle) * num_tris);
00362 delete [] tri_indices;
00363 tri_indices = new_tris;
00364 num_tris_allocated = num_tris;
00365 }
00366
00367 if(num_vertices_allocated > num_vertices)
00368 {
00369 Vec3f* new_vertices = new Vec3f[num_vertices];
00370 if(!new_vertices)
00371 {
00372 std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl;
00373 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00374 }
00375 memcpy(new_vertices, vertices, sizeof(Vec3f) * num_vertices);
00376 delete [] vertices;
00377 vertices = new_vertices;
00378 num_vertices_allocated = num_vertices;
00379 }
00380
00381
00382
00383 int num_bvs_to_be_allocated = 0;
00384 if(num_tris == 0)
00385 num_bvs_to_be_allocated = 2 * num_vertices - 1;
00386 else
00387 num_bvs_to_be_allocated = 2 * num_tris - 1;
00388
00389
00390 bvs = new BVNode<BV> [num_bvs_to_be_allocated];
00391 primitive_indices = new unsigned int [num_bvs_to_be_allocated];
00392 if(!bvs || !primitive_indices)
00393 {
00394 std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl;
00395 return BVH_ERR_MODEL_OUT_OF_MEMORY;
00396 }
00397 num_bvs_allocated = num_bvs_to_be_allocated;
00398 num_bvs = 0;
00399
00400 buildTree();
00401
00402
00403 build_state = BVH_BUILD_STATE_PROCESSED;
00404
00405 return BVH_OK;
00406 }
00407
00408
00409
00410 template<typename BV>
00411 int BVHModel<BV>::beginReplaceModel()
00412 {
00413 if(build_state != BVH_BUILD_STATE_PROCESSED)
00414 {
00415 std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl;
00416 return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
00417 }
00418
00419 if(prev_vertices) delete [] prev_vertices; prev_vertices = NULL;
00420
00421 num_vertex_updated = 0;
00422
00423 build_state = BVH_BUILD_STATE_REPLACE_BEGUN;
00424
00425 return BVH_OK;
00426 }
00427
00428 template<typename BV>
00429 int BVHModel<BV>::replaceVertex(const Vec3f& p)
00430 {
00431 if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
00432 {
00433 std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
00434 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00435 }
00436
00437 vertices[num_vertex_updated] = p;
00438 num_vertex_updated++;
00439
00440 return BVH_OK;
00441 }
00442
00443 template<typename BV>
00444 int BVHModel<BV>::replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
00445 {
00446 if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
00447 {
00448 std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
00449 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00450 }
00451
00452 vertices[num_vertex_updated] = p1; num_vertex_updated++;
00453 vertices[num_vertex_updated] = p2; num_vertex_updated++;
00454 vertices[num_vertex_updated] = p3; num_vertex_updated++;
00455 return BVH_OK;
00456 }
00457
00458 template<typename BV>
00459 int BVHModel<BV>::replaceSubModel(const std::vector<Vec3f>& ps)
00460 {
00461 if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
00462 {
00463 std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
00464 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00465 }
00466
00467 for(unsigned int i = 0; i < ps.size(); ++i)
00468 {
00469 vertices[num_vertex_updated] = ps[i];
00470 num_vertex_updated++;
00471 }
00472 return BVH_OK;
00473 }
00474
00475 template<typename BV>
00476 int BVHModel<BV>::endReplaceModel(bool refit, bool bottomup)
00477 {
00478 if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
00479 {
00480 std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl;
00481 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00482 }
00483
00484 if(num_vertex_updated != num_vertices)
00485 {
00486 std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl;
00487 return BVH_ERR_INCORRECT_DATA;
00488 }
00489
00490 if(refit)
00491 {
00492 refitTree(bottomup);
00493 }
00494 else
00495 {
00496 buildTree();
00497 }
00498
00499 build_state = BVH_BUILD_STATE_PROCESSED;
00500
00501 return BVH_OK;
00502 }
00503
00504
00505
00506
00507
00508 template<typename BV>
00509 int BVHModel<BV>::beginUpdateModel()
00510 {
00511 if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED)
00512 {
00513 std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl;
00514 return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
00515 }
00516
00517 if(prev_vertices)
00518 {
00519 Vec3f* temp = prev_vertices;
00520 prev_vertices = vertices;
00521 vertices = temp;
00522 }
00523 else
00524 {
00525 prev_vertices = vertices;
00526 vertices = new Vec3f[num_vertices];
00527 }
00528
00529 num_vertex_updated = 0;
00530
00531 build_state = BVH_BUILD_STATE_UPDATE_BEGUN;
00532
00533 return BVH_OK;
00534 }
00535
00536 template<typename BV>
00537 int BVHModel<BV>::updateVertex(const Vec3f& p)
00538 {
00539 if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
00540 {
00541 std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
00542 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00543 }
00544
00545 vertices[num_vertex_updated] = p;
00546 num_vertex_updated++;
00547
00548 return BVH_OK;
00549 }
00550
00551 template<typename BV>
00552 int BVHModel<BV>::updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
00553 {
00554 if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
00555 {
00556 std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
00557 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00558 }
00559
00560 vertices[num_vertex_updated] = p1; num_vertex_updated++;
00561 vertices[num_vertex_updated] = p2; num_vertex_updated++;
00562 vertices[num_vertex_updated] = p3; num_vertex_updated++;
00563 return BVH_OK;
00564 }
00565
00566 template<typename BV>
00567 int BVHModel<BV>::updateSubModel(const std::vector<Vec3f>& ps)
00568 {
00569 if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
00570 {
00571 std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
00572 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00573 }
00574
00575 for(unsigned int i = 0; i < ps.size(); ++i)
00576 {
00577 vertices[num_vertex_updated] = ps[i];
00578 num_vertex_updated++;
00579 }
00580 return BVH_OK;
00581 }
00582
00583 template<typename BV>
00584 int BVHModel<BV>::endUpdateModel(bool refit, bool bottomup)
00585 {
00586 if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
00587 {
00588 std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl;
00589 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
00590 }
00591
00592 if(num_vertex_updated != num_vertices)
00593 {
00594 std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl;
00595 return BVH_ERR_INCORRECT_DATA;
00596 }
00597
00598 if(refit)
00599 {
00600 refitTree(bottomup);
00601 }
00602 else
00603 {
00604 buildTree();
00605
00606
00607
00608 refitTree(bottomup);
00609 }
00610
00611
00612 build_state = BVH_BUILD_STATE_UPDATED;
00613
00614 return BVH_OK;
00615 }
00616
00617
00618
00619
00620 template<typename BV>
00621 int BVHModel<BV>::memUsage(int msg) const
00622 {
00623 int mem_bv_list = sizeof(BV) * num_bvs;
00624 int mem_tri_list = sizeof(Triangle) * num_tris;
00625 int mem_vertex_list = sizeof(Vec3f) * num_vertices;
00626
00627 int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + sizeof(BVHModel<BV>);
00628 if(msg)
00629 {
00630 std::cerr << "Total for model " << total_mem << " bytes." << std::endl;
00631 std::cerr << "BVs: " << num_bvs << " allocated." << std::endl;
00632 std::cerr << "Tris: " << num_tris << " allocated." << std::endl;
00633 std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl;
00634 }
00635
00636 return BVH_OK;
00637 }
00638
00639
00640 template<typename BV>
00641 int BVHModel<BV>::buildTree()
00642 {
00643
00644 bv_fitter->set(vertices, tri_indices, getModelType());
00645
00646 bv_splitter->set(vertices, tri_indices, getModelType());
00647
00648 num_bvs = 1;
00649
00650 int num_primitives = 0;
00651 switch(getModelType())
00652 {
00653 case BVH_MODEL_TRIANGLES:
00654 num_primitives = num_tris;
00655 break;
00656 case BVH_MODEL_POINTCLOUD:
00657 num_primitives = num_vertices;
00658 break;
00659 default:
00660 std::cerr << "BVH Error: Model type not supported!" << std::endl;
00661 return BVH_ERR_UNSUPPORTED_FUNCTION;
00662 }
00663
00664 for(int i = 0; i < num_primitives; ++i)
00665 primitive_indices[i] = i;
00666 recursiveBuildTree(0, 0, num_primitives);
00667
00668 bv_fitter->clear();
00669 bv_splitter->clear();
00670
00671 return BVH_OK;
00672 }
00673
00674 template<typename BV>
00675 int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_primitives)
00676 {
00677 BVHModelType type = getModelType();
00678 BVNode<BV>* bvnode = bvs + bv_id;
00679 unsigned int* cur_primitive_indices = primitive_indices + first_primitive;
00680
00681
00682 BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives);
00683 bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
00684
00685 bvnode->bv = bv;
00686 bvnode->first_primitive = first_primitive;
00687 bvnode->num_primitives = num_primitives;
00688
00689 if(num_primitives == 1)
00690 {
00691 bvnode->first_child = -((*cur_primitive_indices) + 1);
00692 }
00693 else
00694 {
00695 bvnode->first_child = num_bvs;
00696 num_bvs += 2;
00697
00698 int c1 = 0;
00699 for(int i = 0; i < num_primitives; ++i)
00700 {
00701 Vec3f p;
00702 if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]];
00703 else if(type == BVH_MODEL_TRIANGLES)
00704 {
00705 const Triangle& t = tri_indices[cur_primitive_indices[i]];
00706 const Vec3f& p1 = vertices[t[0]];
00707 const Vec3f& p2 = vertices[t[1]];
00708 const Vec3f& p3 = vertices[t[2]];
00709 FCL_REAL x = (p1[0] + p2[0] + p3[0]) / 3.0;
00710 FCL_REAL y = (p1[1] + p2[1] + p3[1]) / 3.0;
00711 FCL_REAL z = (p1[2] + p2[2] + p3[2]) / 3.0;
00712 p.setValue(x, y, z);
00713 }
00714 else
00715 {
00716 std::cerr << "BVH Error: Model type not supported!" << std::endl;
00717 return BVH_ERR_UNSUPPORTED_FUNCTION;
00718 }
00719
00720
00721
00722
00723
00724
00725
00726
00727 if(bv_splitter->apply(p))
00728 {
00729
00730 }
00731 else
00732 {
00733 unsigned int temp = cur_primitive_indices[i];
00734 cur_primitive_indices[i] = cur_primitive_indices[c1];
00735 cur_primitive_indices[c1] = temp;
00736 c1++;
00737 }
00738 }
00739
00740
00741 if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2;
00742
00743 int num_first_half = c1;
00744
00745 recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half);
00746 recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half);
00747 }
00748
00749 return BVH_OK;
00750 }
00751
00752 template<typename BV>
00753 int BVHModel<BV>::refitTree(bool bottomup)
00754 {
00755 if(bottomup)
00756 return refitTree_bottomup();
00757 else
00758 return refitTree_topdown();
00759 }
00760
00761 template<typename BV>
00762 int BVHModel<BV>::refitTree_bottomup()
00763 {
00764 int res = recursiveRefitTree_bottomup(0);
00765
00766 return res;
00767 }
00768
00769
00770 template<typename BV>
00771 int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id)
00772 {
00773 BVNode<BV>* bvnode = bvs + bv_id;
00774 if(bvnode->isLeaf())
00775 {
00776 BVHModelType type = getModelType();
00777 int primitive_id = -(bvnode->first_child + 1);
00778 if(type == BVH_MODEL_POINTCLOUD)
00779 {
00780 BV bv;
00781
00782 if(prev_vertices)
00783 {
00784 Vec3f v[2];
00785 v[0] = prev_vertices[primitive_id];
00786 v[1] = vertices[primitive_id];
00787 fit(v, 2, bv);
00788 }
00789 else
00790 fit(vertices + primitive_id, 1, bv);
00791
00792 bvnode->bv = bv;
00793 }
00794 else if(type == BVH_MODEL_TRIANGLES)
00795 {
00796 BV bv;
00797 const Triangle& triangle = tri_indices[primitive_id];
00798
00799 if(prev_vertices)
00800 {
00801 Vec3f v[6];
00802 for(int i = 0; i < 3; ++i)
00803 {
00804 v[i] = prev_vertices[triangle[i]];
00805 v[i + 3] = vertices[triangle[i]];
00806 }
00807
00808 fit(v, 6, bv);
00809 }
00810 else
00811 {
00812 Vec3f v[3];
00813 for(int i = 0; i < 3; ++i)
00814 {
00815 v[i] = vertices[triangle[i]];
00816 }
00817
00818 fit(v, 3, bv);
00819 }
00820
00821 bvnode->bv = bv;
00822 }
00823 else
00824 {
00825 std::cerr << "BVH Error: Model type not supported!" << std::endl;
00826 return BVH_ERR_UNSUPPORTED_FUNCTION;
00827 }
00828 }
00829 else
00830 {
00831 recursiveRefitTree_bottomup(bvnode->leftChild());
00832 recursiveRefitTree_bottomup(bvnode->rightChild());
00833 bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv;
00834 }
00835
00836 return BVH_OK;
00837 }
00838
00839 template<typename BV>
00840 int BVHModel<BV>::refitTree_topdown()
00841 {
00842 bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType());
00843 for(int i = 0; i < num_bvs; ++i)
00844 {
00845 BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives);
00846 bvs[i].bv = bv;
00847 }
00848
00849 bv_fitter->clear();
00850
00851 return BVH_OK;
00852 }
00853
00854 template<typename BV>
00855 void BVHModel<BV>::computeLocalAABB()
00856 {
00857 AABB aabb_;
00858 for(int i = 0; i < num_vertices; ++i)
00859 {
00860 aabb_ += vertices[i];
00861 }
00862
00863 aabb_center = aabb_.center();
00864
00865 aabb_radius = 0;
00866 for(int i = 0; i < num_vertices; ++i)
00867 {
00868 FCL_REAL r = (aabb_center - vertices[i]).sqrLength();
00869 if(r > aabb_radius) aabb_radius = r;
00870 }
00871
00872 aabb_radius = sqrt(aabb_radius);
00873
00874 aabb_local = aabb_;
00875 }
00876
00877
00878 template<>
00879 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
00880 {
00881 OBB& obb = bvs[bv_id].bv;
00882 if(!bvs[bv_id].isLeaf())
00883 {
00884 makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To);
00885
00886 makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To);
00887 }
00888
00889
00890 obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0]));
00891 obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1]));
00892 obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2]));
00893
00894 Vec3f t = obb.To - parent_c;
00895 obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
00896 }
00897
00898 template<>
00899 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
00900 {
00901 RSS& rss = bvs[bv_id].bv;
00902 if(!bvs[bv_id].isLeaf())
00903 {
00904 makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axis, rss.Tr);
00905
00906 makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axis, rss.Tr);
00907 }
00908
00909
00910 rss.axis[0] = Vec3f(parent_axis[0].dot(rss.axis[0]), parent_axis[1].dot(rss.axis[0]), parent_axis[2].dot(rss.axis[0]));
00911 rss.axis[1] = Vec3f(parent_axis[0].dot(rss.axis[1]), parent_axis[1].dot(rss.axis[1]), parent_axis[2].dot(rss.axis[1]));
00912 rss.axis[2] = Vec3f(parent_axis[0].dot(rss.axis[2]), parent_axis[1].dot(rss.axis[2]), parent_axis[2].dot(rss.axis[2]));
00913
00914 Vec3f t = rss.Tr - parent_c;
00915 rss.Tr = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
00916 }
00917
00918 template<>
00919 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
00920 {
00921 OBB& obb = bvs[bv_id].bv.obb;
00922 RSS& rss = bvs[bv_id].bv.rss;
00923 if(!bvs[bv_id].isLeaf())
00924 {
00925 makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To);
00926
00927 makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To);
00928 }
00929
00930
00931 obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0]));
00932 obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1]));
00933 obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2]));
00934
00935 rss.axis[0] = obb.axis[0];
00936 rss.axis[1] = obb.axis[1];
00937 rss.axis[2] = obb.axis[2];
00938
00939 Vec3f t = obb.To - parent_c;
00940 obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
00941 rss.Tr = obb.To;
00942 }
00943
00944
00945
00946 template<>
00947 NODE_TYPE BVHModel<AABB>::getNodeType() const
00948 {
00949 return BV_AABB;
00950 }
00951
00952 template<>
00953 NODE_TYPE BVHModel<OBB>::getNodeType() const
00954 {
00955 return BV_OBB;
00956 }
00957
00958 template<>
00959 NODE_TYPE BVHModel<RSS>::getNodeType() const
00960 {
00961 return BV_RSS;
00962 }
00963
00964 template<>
00965 NODE_TYPE BVHModel<kIOS>::getNodeType() const
00966 {
00967 return BV_kIOS;
00968 }
00969
00970 template<>
00971 NODE_TYPE BVHModel<OBBRSS>::getNodeType() const
00972 {
00973 return BV_OBBRSS;
00974 }
00975
00976 template<>
00977 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const
00978 {
00979 return BV_KDOP16;
00980 }
00981
00982 template<>
00983 NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const
00984 {
00985 return BV_KDOP18;
00986 }
00987
00988 template<>
00989 NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const
00990 {
00991 return BV_KDOP24;
00992 }
00993
00994
00995
00996
00997
00998
00999 template class BVHModel<KDOP<16> >;
01000 template class BVHModel<KDOP<18> >;
01001 template class BVHModel<KDOP<24> >;
01002 template class BVHModel<OBB>;
01003 template class BVHModel<AABB>;
01004 template class BVHModel<RSS>;
01005 template class BVHModel<kIOS>;
01006 template class BVHModel<OBBRSS>;
01007 }