Sculpt:
[blender.git] / source / blender / blenlib / intern / pbvh.c
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  */
22
23 #include <float.h>
24 #include <stdlib.h>
25 #include <string.h>
26
27 #include "MEM_guardedalloc.h"
28
29 #include "DNA_meshdata_types.h"
30
31 #include "BLI_math.h"
32 #include "BLI_ghash.h"
33 #include "BLI_pbvh.h"
34
35 #include "BKE_DerivedMesh.h"
36 #include "BKE_mesh.h"
37 #include "BKE_utildefines.h"
38
39 #include "gpu_buffers.h"
40
41 #define LEAF_LIMIT 10000
42
43 //#define PERFCNTRS
44
45 /* Bitmap */
46 typedef char* BLI_bitmap;
47
48 BLI_bitmap BLI_bitmap_new(int tot)
49 {
50         return MEM_callocN((tot >> 3) + 1, "BLI bitmap");
51 }
52
53 int BLI_bitmap_get(BLI_bitmap b, int index)
54 {
55         return b[index >> 3] & (1 << (index & 7));
56 }
57
58 void BLI_bitmap_set(BLI_bitmap b, int index)
59 {
60         b[index >> 3] |= (1 << (index & 7));
61 }
62
63 void BLI_bitmap_clear(BLI_bitmap b, int index)
64 {
65         b[index >> 3] &= ~(1 << (index & 7));
66 }
67
68 /* Axis-aligned bounding box */
69 typedef struct {
70         float bmin[3], bmax[3];
71 } BB;
72
73 /* Axis-aligned bounding box with centroid */
74 typedef struct {
75         float bmin[3], bmax[3], bcentroid[3];
76 } BBC;
77
78 struct PBVHNode {
79         /* Opaque handle for drawing code */
80         void *draw_buffers;
81
82         int *vert_indices;
83
84         /* Voxel bounds */
85         BB vb;
86         BB orig_vb;
87
88         /* For internal nodes */
89         int children_offset;
90
91         /* Pointer into bvh prim_indices */
92         int *prim_indices;
93         int *face_vert_indices;
94
95         unsigned int totprim;
96         unsigned int uniq_verts, face_verts;
97
98         char flag;
99 };
100
101 struct PBVH {
102         PBVHNode *nodes;
103         int node_mem_count, totnode;
104
105         int *prim_indices;
106         int totprim;
107         int totvert;
108
109         int leaf_limit;
110
111         /* Mesh data */
112         MVert *verts;
113         MFace *faces;
114
115         /* Grid Data */
116         DMGridData **grids;
117         DMGridAdjacency *gridadj;
118         void **gridfaces;
119         int totgrid;
120         int gridsize;
121
122         /* Only used during BVH build and update,
123            don't need to remain valid after */
124         BLI_bitmap vert_bitmap;
125
126 #ifdef PERFCNTRS
127         int perf_modified;
128 #endif
129 };
130
131 #define STACK_FIXED_DEPTH       100
132
133 typedef struct PBVHStack {
134         PBVHNode *node;
135         int revisiting;
136 } PBVHStack;
137
138 typedef struct PBVHIter {
139         PBVH *bvh;
140         BLI_pbvh_SearchCallback scb;
141         void *search_data;
142
143         PBVHStack *stack;
144         int stacksize;
145
146         PBVHStack stackfixed[STACK_FIXED_DEPTH];
147         int stackspace;
148 } PBVHIter;
149
150 static void BB_reset(BB *bb)
151 {
152         bb->bmin[0] = bb->bmin[1] = bb->bmin[2] = FLT_MAX;
153         bb->bmax[0] = bb->bmax[1] = bb->bmax[2] = -FLT_MAX;
154 }
155
156 /* Expand the bounding box to include a new coordinate */
157 static void BB_expand(BB *bb, float co[3])
158 {
159         int i;
160         for(i = 0; i < 3; ++i) {
161                 bb->bmin[i] = MIN2(bb->bmin[i], co[i]);
162                 bb->bmax[i] = MAX2(bb->bmax[i], co[i]);
163         }
164 }
165
166 /* Expand the bounding box to include another bounding box */
167 static void BB_expand_with_bb(BB *bb, BB *bb2)
168 {
169         int i;
170         for(i = 0; i < 3; ++i) {
171                 bb->bmin[i] = MIN2(bb->bmin[i], bb2->bmin[i]);
172                 bb->bmax[i] = MAX2(bb->bmax[i], bb2->bmax[i]);
173         }
174 }
175
176 /* Return 0, 1, or 2 to indicate the widest axis of the bounding box */
177 static int BB_widest_axis(BB *bb)
178 {
179         float dim[3];
180         int i;
181
182         for(i = 0; i < 3; ++i)
183                 dim[i] = bb->bmax[i] - bb->bmin[i];
184
185         if(dim[0] > dim[1]) {
186                 if(dim[0] > dim[2])
187                         return 0;
188                 else
189                         return 2;
190         }
191         else {
192                 if(dim[1] > dim[2])
193                         return 1;
194                 else
195                         return 2;
196         }
197 }
198
199 static void BBC_update_centroid(BBC *bbc)
200 {
201         int i;
202         for(i = 0; i < 3; ++i)
203                 bbc->bcentroid[i] = (bbc->bmin[i] + bbc->bmax[i]) * 0.5f;
204 }
205
206 /* Not recursive */
207 static void update_node_vb(PBVH *bvh, PBVHNode *node)
208 {
209         BB vb;
210
211         BB_reset(&vb);
212         
213         if(node->flag & PBVH_Leaf) {
214                 PBVHVertexIter vd;
215
216                 BLI_pbvh_vertex_iter_begin(bvh, node, vd, PBVH_ITER_ALL) {
217                         BB_expand(&vb, vd.co);
218                 }
219                 BLI_pbvh_vertex_iter_end;
220         }
221         else {
222                 BB_expand_with_bb(&vb,
223                                   &bvh->nodes[node->children_offset].vb);
224                 BB_expand_with_bb(&vb,
225                                   &bvh->nodes[node->children_offset + 1].vb);
226         }
227
228         node->vb= vb;
229 }
230
231 /* Adapted from BLI_kdopbvh.c */
232 /* Returns the index of the first element on the right of the partition */
233 static int partition_indices(int *prim_indices, int lo, int hi, int axis,
234                              float mid, BBC *prim_bbc)
235 {
236         int i=lo, j=hi;
237         for(;;) {
238                 for(; prim_bbc[prim_indices[i]].bcentroid[axis] < mid; i++);
239                 for(; mid < prim_bbc[prim_indices[j]].bcentroid[axis]; j--);
240                 
241                 if(!(i < j))
242                         return i;
243                 
244                 SWAP(int, prim_indices[i], prim_indices[j]);
245                 i++;
246         }
247 }
248
249 void check_partitioning(int *prim_indices, int lo, int hi, int axis,
250                                float mid, BBC *prim_bbc, int index_of_2nd_partition)
251 {
252         int i;
253         for(i = lo; i <= hi; ++i) {
254                 const float c = prim_bbc[prim_indices[i]].bcentroid[axis];
255
256                 if((i < index_of_2nd_partition && c > mid) ||
257                    (i > index_of_2nd_partition && c < mid)) {
258                         printf("fail\n");
259                 }
260         }
261 }
262
263 static void grow_nodes(PBVH *bvh, int totnode)
264 {
265         if(totnode > bvh->node_mem_count) {
266                 PBVHNode *prev = bvh->nodes;
267                 bvh->node_mem_count *= 1.33;
268                 if(bvh->node_mem_count < totnode)
269                         bvh->node_mem_count = totnode;
270                 bvh->nodes = MEM_callocN(sizeof(PBVHNode) * bvh->node_mem_count,
271                                          "bvh nodes");
272                 memcpy(bvh->nodes, prev, bvh->totnode * sizeof(PBVHNode));
273                 MEM_freeN(prev);
274         }
275
276         bvh->totnode = totnode;
277 }
278
279 /* Add a vertex to the map, with a positive value for unique vertices and
280    a negative value for additional vertices */
281 static int map_insert_vert(PBVH *bvh, GHash *map,
282                             unsigned int *face_verts,
283                             unsigned int *uniq_verts, int vertex)
284 {
285         void *value, *key = SET_INT_IN_POINTER(vertex);
286
287         if(!BLI_ghash_haskey(map, key)) {
288                 if(BLI_bitmap_get(bvh->vert_bitmap, vertex)) {
289                         value = SET_INT_IN_POINTER(-(*face_verts) - 1);
290                         ++(*face_verts);
291                 }
292                 else {
293                         BLI_bitmap_set(bvh->vert_bitmap, vertex);
294                         value = SET_INT_IN_POINTER(*uniq_verts);
295                         ++(*uniq_verts);
296                 }
297                 
298                 BLI_ghash_insert(map, key, value);
299                 return GET_INT_FROM_POINTER(value);
300         }
301         else
302                 return GET_INT_FROM_POINTER(BLI_ghash_lookup(map, key));
303 }
304
305 /* Find vertices used by the faces in this node and update the draw buffers */
306 static void build_mesh_leaf_node(PBVH *bvh, PBVHNode *node)
307 {
308         GHashIterator *iter;
309         GHash *map;
310         int i, j, totface;
311
312         map = BLI_ghash_new(BLI_ghashutil_inthash, BLI_ghashutil_intcmp);
313         
314         node->uniq_verts = node->face_verts = 0;
315         totface= node->totprim;
316
317         node->face_vert_indices = MEM_callocN(sizeof(int) *
318                                          4*totface, "bvh node face vert indices");
319
320         for(i = 0; i < totface; ++i) {
321                 MFace *f = bvh->faces + node->prim_indices[i];
322                 int sides = f->v4 ? 4 : 3;
323
324                 for(j = 0; j < sides; ++j) {
325                         node->face_vert_indices[i*4 + j]= 
326                                 map_insert_vert(bvh, map, &node->face_verts,
327                                                 &node->uniq_verts, (&f->v1)[j]);
328                 }
329         }
330
331         node->vert_indices = MEM_callocN(sizeof(int) *
332                                          (node->uniq_verts + node->face_verts),
333                                          "bvh node vert indices");
334
335         /* Build the vertex list, unique verts first */
336         for(iter = BLI_ghashIterator_new(map), i = 0;
337             !BLI_ghashIterator_isDone(iter);
338             BLI_ghashIterator_step(iter), ++i) {
339                 void *value = BLI_ghashIterator_getValue(iter);
340                 int ndx = GET_INT_FROM_POINTER(value);
341
342                 if(ndx < 0)
343                         ndx = -ndx + node->uniq_verts - 1;
344
345                 node->vert_indices[ndx] =
346                         GET_INT_FROM_POINTER(BLI_ghashIterator_getKey(iter));
347         }
348
349         for(i = 0; i < totface*4; ++i)
350                 if(node->face_vert_indices[i] < 0)
351                         node->face_vert_indices[i]= -node->face_vert_indices[i] + node->uniq_verts - 1;
352
353         node->draw_buffers =
354                 GPU_build_mesh_buffers(map, bvh->verts, bvh->faces,
355                                   node->prim_indices,
356                                   node->totprim, node->vert_indices,
357                                   node->uniq_verts,
358                                   node->uniq_verts + node->face_verts);
359
360         BLI_ghash_free(map, NULL, NULL);
361 }
362
363 static void build_grids_leaf_node(PBVH *bvh, PBVHNode *node)
364 {
365         node->draw_buffers =
366                 GPU_build_grid_buffers(bvh->grids, node->prim_indices,
367                                 node->totprim, bvh->gridsize);
368 }
369
370 /* Recursively build a node in the tree
371
372    vb is the voxel box around all of the primitives contained in
373    this node.
374
375    cb is the bounding box around all the centroids of the primitives
376    contained in this node
377
378    offset and start indicate a range in the array of primitive indices
379 */
380
381 void build_sub(PBVH *bvh, int node_index, BB *cb, BBC *prim_bbc,
382                int offset, int count)
383 {
384         int i, axis, end;
385         BB cb_backing;
386
387         /* Decide whether this is a leaf or not */
388         // XXX adapt leaf limit for grids
389         if(count <= bvh->leaf_limit) {
390                 bvh->nodes[node_index].flag |= PBVH_Leaf;
391
392                 bvh->nodes[node_index].prim_indices = bvh->prim_indices + offset;
393                 bvh->nodes[node_index].totprim = count;
394
395                 /* Still need vb for searches */
396                 BB_reset(&bvh->nodes[node_index].vb);
397                 for(i = offset + count - 1; i >= offset; --i) {
398                         BB_expand_with_bb(&bvh->nodes[node_index].vb,
399                                           (BB*)(prim_bbc +
400                                                 bvh->prim_indices[i]));
401                 }
402                 
403                 if(bvh->faces)
404                         build_mesh_leaf_node(bvh, bvh->nodes + node_index);
405                 else
406                         build_grids_leaf_node(bvh, bvh->nodes + node_index);
407                 bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
408
409                 /* Done with this subtree */
410                 return;
411         }
412         else {
413                 BB_reset(&bvh->nodes[node_index].vb);
414                 bvh->nodes[node_index].children_offset = bvh->totnode;
415                 grow_nodes(bvh, bvh->totnode + 2);
416
417                 if(!cb) {
418                         cb = &cb_backing;
419                         BB_reset(cb);
420                         for(i = offset + count - 1; i >= offset; --i)
421                                 BB_expand(cb, prim_bbc[bvh->prim_indices[i]].bcentroid);
422                 }
423         }
424
425         axis = BB_widest_axis(cb);
426
427         for(i = offset + count - 1; i >= offset; --i) {
428                 BB_expand_with_bb(&bvh->nodes[node_index].vb,
429                                   (BB*)(prim_bbc + bvh->prim_indices[i]));
430         }
431
432         bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
433
434         end = partition_indices(bvh->prim_indices, offset, offset + count - 1,
435                                 axis,
436                                 (cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
437                                 prim_bbc);
438         check_partitioning(bvh->prim_indices, offset, offset + count - 1,
439                            axis,
440                            (cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
441                            prim_bbc, end);
442
443         build_sub(bvh, bvh->nodes[node_index].children_offset, NULL,
444                   prim_bbc, offset, end - offset);
445         build_sub(bvh, bvh->nodes[node_index].children_offset + 1, NULL,
446                   prim_bbc, end, offset + count - end);
447 }
448
449 static void pbvh_build(PBVH *bvh, BB *cb, BBC *prim_bbc, int totprim)
450 {
451         int i;
452
453         if(totprim != bvh->totprim) {
454                 bvh->totprim = totprim;
455                 if(bvh->nodes) MEM_freeN(bvh->nodes);
456                 if(bvh->prim_indices) MEM_freeN(bvh->prim_indices);
457                 bvh->prim_indices = MEM_callocN(sizeof(int) * totprim,
458                                                 "bvh prim indices");
459                 for(i = 0; i < totprim; ++i)
460                         bvh->prim_indices[i] = i;
461                 bvh->totnode = 0;
462                 if(bvh->node_mem_count < 100) {
463                         bvh->node_mem_count = 100;
464                         bvh->nodes = MEM_callocN(sizeof(PBVHNode) *
465                                                  bvh->node_mem_count,
466                                                  "bvh initial nodes");
467                 }
468         }
469
470         bvh->totnode = 1;
471         build_sub(bvh, 0, cb, prim_bbc, 0, totprim);
472 }
473
474 /* Do a full rebuild with on Mesh data structure */
475 void BLI_pbvh_build_mesh(PBVH *bvh, MFace *faces, MVert *verts, int totface, int totvert)
476 {
477         BBC *prim_bbc = NULL;
478         BB cb;
479         int i, j;
480
481         bvh->faces = faces;
482         bvh->verts = verts;
483         bvh->vert_bitmap = BLI_bitmap_new(totvert);
484         bvh->totvert = totvert;
485         bvh->leaf_limit = LEAF_LIMIT;
486
487         BB_reset(&cb);
488
489         /* For each face, store the AABB and the AABB centroid */
490         prim_bbc = MEM_mallocN(sizeof(BBC) * totface, "prim_bbc");
491
492         for(i = 0; i < totface; ++i) {
493                 MFace *f = faces + i;
494                 const int sides = f->v4 ? 4 : 3;
495                 BBC *bbc = prim_bbc + i;
496
497                 BB_reset((BB*)bbc);
498
499                 for(j = 0; j < sides; ++j)
500                         BB_expand((BB*)bbc, verts[(&f->v1)[j]].co);
501
502                 BBC_update_centroid(bbc);
503
504                 BB_expand(&cb, bbc->bcentroid);
505         }
506
507         pbvh_build(bvh, &cb, prim_bbc, totface);
508
509         MEM_freeN(prim_bbc);
510         MEM_freeN(bvh->vert_bitmap);
511 }
512
513 /* Do a full rebuild with on Grids data structure */
514 void BLI_pbvh_build_grids(PBVH *bvh, DMGridData **grids, DMGridAdjacency *gridadj,
515         int totgrid, int gridsize, void **gridfaces)
516 {
517         BBC *prim_bbc = NULL;
518         BB cb;
519         int i, j;
520
521         bvh->grids= grids;
522         bvh->gridadj= gridadj;
523         bvh->gridfaces= gridfaces;
524         bvh->totgrid= totgrid;
525         bvh->gridsize= gridsize;
526         bvh->leaf_limit = MAX2(LEAF_LIMIT/((gridsize-1)*(gridsize-1)), 1);
527
528         BB_reset(&cb);
529
530         /* For each grid, store the AABB and the AABB centroid */
531         prim_bbc = MEM_mallocN(sizeof(BBC) * totgrid, "prim_bbc");
532
533         for(i = 0; i < totgrid; ++i) {
534                 DMGridData *grid= grids[i];
535                 BBC *bbc = prim_bbc + i;
536
537                 BB_reset((BB*)bbc);
538
539                 for(j = 0; j < gridsize*gridsize; ++j)
540                         BB_expand((BB*)bbc, grid[j].co);
541
542                 BBC_update_centroid(bbc);
543
544                 BB_expand(&cb, bbc->bcentroid);
545         }
546
547         pbvh_build(bvh, &cb, prim_bbc, totgrid);
548
549         MEM_freeN(prim_bbc);
550 }
551
552 PBVH *BLI_pbvh_new(void)
553 {
554         PBVH *bvh = MEM_callocN(sizeof(PBVH), "pbvh");
555
556         return bvh;
557 }
558
559 void BLI_pbvh_free(PBVH *bvh)
560 {
561         PBVHNode *node;
562         int i;
563
564         for(i = 0; i < bvh->totnode; ++i) {
565                 node= &bvh->nodes[i];
566
567                 if(node->flag & PBVH_Leaf) {
568                         if(node->draw_buffers)
569                                 GPU_free_buffers(node->draw_buffers);
570                         if(node->vert_indices)
571                                 MEM_freeN(node->vert_indices);
572                         if(node->face_vert_indices)
573                                 MEM_freeN(node->face_vert_indices);
574                 }
575         }
576
577         MEM_freeN(bvh->nodes);
578         MEM_freeN(bvh->prim_indices);
579         MEM_freeN(bvh);
580 }
581
582 static void pbvh_iter_begin(PBVHIter *iter, PBVH *bvh, BLI_pbvh_SearchCallback scb, void *search_data)
583 {
584         iter->bvh= bvh;
585         iter->scb= scb;
586         iter->search_data= search_data;
587
588         iter->stack= iter->stackfixed;
589         iter->stackspace= STACK_FIXED_DEPTH;
590
591         iter->stack[0].node= bvh->nodes;
592         iter->stack[0].revisiting= 0;
593         iter->stacksize= 1;
594 }
595
596 static void pbvh_iter_end(PBVHIter *iter)
597 {
598         if(iter->stackspace > STACK_FIXED_DEPTH)
599                 MEM_freeN(iter->stack);
600 }
601
602 static void pbvh_stack_push(PBVHIter *iter, PBVHNode *node, int revisiting)
603 {
604         if(iter->stacksize == iter->stackspace) {
605                 PBVHStack *newstack;
606
607                 iter->stackspace *= 2;
608                 newstack= MEM_callocN(sizeof(PBVHStack)*iter->stackspace, "PBVHStack");
609                 memcpy(newstack, iter->stack, sizeof(PBVHStack)*iter->stacksize);
610
611                 if(iter->stackspace > STACK_FIXED_DEPTH)
612                         MEM_freeN(iter->stack);
613                 iter->stack= newstack;
614         }
615
616         iter->stack[iter->stacksize].node= node;
617         iter->stack[iter->stacksize].revisiting= revisiting;
618         iter->stacksize++;
619 }
620
621 static PBVHNode *pbvh_iter_next(PBVHIter *iter)
622 {
623         PBVHNode *node;
624         int revisiting;
625         void *search_data;
626
627         /* purpose here is to traverse tree, visiting child nodes before their
628            parents, this order is necessary for e.g. computing bounding boxes */
629
630         while(iter->stacksize) {
631                 /* pop node */
632                 iter->stacksize--;
633                 node= iter->stack[iter->stacksize].node;
634                 revisiting= iter->stack[iter->stacksize].revisiting;
635
636                 /* revisiting node already checked */
637                 if(revisiting)
638                         return node;
639
640                 /* check search callback */
641                 search_data= iter->search_data;
642
643                 if(iter->scb && !iter->scb(node, search_data))
644                         continue; /* don't traverse, outside of search zone */
645
646                 if(node->flag & PBVH_Leaf) {
647                         /* immediately hit leaf node */
648                         return node;
649                 }
650                 else {
651                         /* come back later when children are done */
652                         pbvh_stack_push(iter, node, 1);
653
654                         /* push two child nodes on the stack */
655                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset+1, 0);
656                         pbvh_stack_push(iter, iter->bvh->nodes+node->children_offset, 0);
657                 }
658         }
659
660         return NULL;
661 }
662
663 void BLI_pbvh_search_gather(PBVH *bvh,
664         BLI_pbvh_SearchCallback scb, void *search_data,
665         PBVHNode ***r_array, int *r_tot)
666 {
667         PBVHIter iter;
668         PBVHNode **array= NULL, **newarray, *node;
669         int tot= 0, space= 0;
670
671         pbvh_iter_begin(&iter, bvh, scb, search_data);
672
673         while((node=pbvh_iter_next(&iter))) {
674                 if(node->flag & PBVH_Leaf) {
675                         if(tot == space) {
676                                 /* resize array if needed */
677                                 space= (tot == 0)? 32: space*2;
678                                 newarray= MEM_callocN(sizeof(PBVHNode)*space, "PBVHNodeSearch");
679
680                                 if(array) {
681                                         memcpy(newarray, array, sizeof(PBVHNode)*tot);
682                                         MEM_freeN(array);
683                                 }
684
685                                 array= newarray;
686                         }
687
688                         array[tot]= node;
689                         tot++;
690                 }
691         }
692
693         pbvh_iter_end(&iter);
694
695         *r_array= array;
696         *r_tot= tot;
697 }
698
699 void BLI_pbvh_search_callback(PBVH *bvh,
700         BLI_pbvh_SearchCallback scb, void *search_data,
701         BLI_pbvh_HitCallback hcb, void *hit_data)
702 {
703         PBVHIter iter;
704         PBVHNode *node;
705
706         pbvh_iter_begin(&iter, bvh, scb, search_data);
707
708         while((node=pbvh_iter_next(&iter)))
709                 if(node->flag & PBVH_Leaf)
710                         hcb(node, hit_data);
711
712         pbvh_iter_end(&iter);
713 }
714
715 static int update_search_cb(PBVHNode *node, void *data_v)
716 {
717         int flag= GET_INT_FROM_POINTER(data_v);
718
719         if(node->flag & PBVH_Leaf)
720                 return (node->flag & flag);
721         
722         return 1;
723 }
724
725 static void pbvh_update_normals(PBVH *bvh, PBVHNode **nodes,
726         int totnode, float (*face_nors)[3])
727 {
728         float (*vnor)[3];
729         int n;
730
731         if(bvh->grids)
732                 return;
733
734         /* could be per node to save some memory, but also means
735            we have to store for each vertex which node it is in */
736         vnor= MEM_callocN(sizeof(float)*3*bvh->totvert, "bvh temp vnors");
737
738         /* subtle assumptions:
739            - We know that for all edited vertices, the nodes with faces
740              adjacent to these vertices have been marked with PBVH_UpdateNormals.
741                  This is true because if the vertex is inside the brush radius, the
742                  bounding box of it's adjacent faces will be as well.
743            - However this is only true for the vertices that have actually been
744              edited, not for all vertices in the nodes marked for update, so we
745                  can only update vertices marked with ME_VERT_PBVH_UPDATE.
746         */
747
748         #pragma omp parallel for private(n) schedule(static)
749         for(n = 0; n < totnode; n++) {
750                 PBVHNode *node= nodes[n];
751
752                 if((node->flag & PBVH_UpdateNormals)) {
753                         int i, j, totface, *faces;
754
755                         faces= node->prim_indices;
756                         totface= node->totprim;
757
758                         for(i = 0; i < totface; ++i) {
759                                 MFace *f= bvh->faces + faces[i];
760                                 float fn[3];
761                                 unsigned int *fv = &f->v1;
762                                 int sides= (f->v4)? 4: 3;
763
764                                 if(f->v4)
765                                         normal_quad_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
766                                                                    bvh->verts[f->v3].co, bvh->verts[f->v4].co);
767                                 else
768                                         normal_tri_v3(fn, bvh->verts[f->v1].co, bvh->verts[f->v2].co,
769                                                                   bvh->verts[f->v3].co);
770
771                                 for(j = 0; j < sides; ++j) {
772                                         int v= fv[j];
773
774                                         if(bvh->verts[v].flag & ME_VERT_PBVH_UPDATE) {
775                                                 /* this seems like it could be very slow but profile
776                                                    does not show this, so just leave it for now? */
777                                                 #pragma omp atomic
778                                                 vnor[v][0] += fn[0];
779                                                 #pragma omp atomic
780                                                 vnor[v][1] += fn[1];
781                                                 #pragma omp atomic
782                                                 vnor[v][2] += fn[2];
783                                         }
784                                 }
785
786                                 if(face_nors)
787                                         copy_v3_v3(face_nors[faces[i]], fn);
788                         }
789                 }
790         }
791
792         #pragma omp parallel for private(n) schedule(static)
793         for(n = 0; n < totnode; n++) {
794                 PBVHNode *node= nodes[n];
795
796                 if(node->flag & PBVH_UpdateNormals) {
797                         int i, *verts, totvert;
798
799                         verts= node->vert_indices;
800                         totvert= node->uniq_verts;
801
802                         for(i = 0; i < totvert; ++i) {
803                                 const int v = verts[i];
804                                 MVert *mvert= &bvh->verts[v];
805
806                                 if(mvert->flag & ME_VERT_PBVH_UPDATE) {
807                                         float no[3];
808
809                                         copy_v3_v3(no, vnor[v]);
810                                         normalize_v3(no);
811                                         
812                                         mvert->no[0] = (short)(no[0]*32767.0f);
813                                         mvert->no[1] = (short)(no[1]*32767.0f);
814                                         mvert->no[2] = (short)(no[2]*32767.0f);
815                                         
816                                         mvert->flag &= ~ME_VERT_PBVH_UPDATE;
817                                 }
818                         }
819
820                         node->flag &= ~PBVH_UpdateNormals;
821                 }
822         }
823
824         MEM_freeN(vnor);
825 }
826
827 static void pbvh_update_BB_redraw(PBVH *bvh, PBVHNode **nodes,
828         int totnode, int flag)
829 {
830         int n;
831
832         /* update BB, redraw flag */
833         #pragma omp parallel for private(n) schedule(static)
834         for(n = 0; n < totnode; n++) {
835                 PBVHNode *node= nodes[n];
836
837                 if((flag & PBVH_UpdateBB) && (node->flag & PBVH_UpdateBB))
838                         /* don't clear flag yet, leave it for flushing later */
839                         update_node_vb(bvh, node);
840
841                 if((flag & PBVH_UpdateOriginalBB) && (node->flag & PBVH_UpdateOriginalBB))
842                         node->orig_vb= node->vb;
843
844                 if((flag & PBVH_UpdateRedraw) && (node->flag & PBVH_UpdateRedraw))
845                         node->flag &= ~PBVH_UpdateRedraw;
846         }
847 }
848
849 static void pbvh_update_draw_buffers(PBVH *bvh, PBVHNode **nodes, int totnode)
850 {
851         PBVHNode *node;
852         int n;
853
854         /* can't be done in parallel with OpenGL */
855         for(n = 0; n < totnode; n++) {
856                 node= nodes[n];
857
858                 if(node->flag & PBVH_UpdateDrawBuffers) {
859                         if(bvh->grids) {
860                                 GPU_update_grid_buffers(node->draw_buffers,
861                                                    bvh->grids,
862                                                    node->prim_indices,
863                                                    node->totprim,
864                                                    bvh->gridsize);
865                         }
866                         else {
867                                 GPU_update_mesh_buffers(node->draw_buffers,
868                                                    bvh->verts,
869                                                    node->vert_indices,
870                                                    node->uniq_verts +
871                                                    node->face_verts);
872                         }
873
874                         node->flag &= ~PBVH_UpdateDrawBuffers;
875                 }
876         }
877 }
878
879 static int pbvh_flush_bb(PBVH *bvh, PBVHNode *node, int flag)
880 {
881         int update= 0;
882
883         /* difficult to multithread well, we just do single threaded recursive */
884         if(node->flag & PBVH_Leaf) {
885                 if(flag & PBVH_UpdateBB) {
886                         update |= (node->flag & PBVH_UpdateBB);
887                         node->flag &= ~PBVH_UpdateBB;
888                 }
889
890                 if(flag & PBVH_UpdateOriginalBB) {
891                         update |= (node->flag & PBVH_UpdateOriginalBB);
892                         node->flag &= ~PBVH_UpdateOriginalBB;
893                 }
894
895                 return update;
896         }
897         else {
898                 update |= pbvh_flush_bb(bvh, bvh->nodes + node->children_offset, flag);
899                 update |= pbvh_flush_bb(bvh, bvh->nodes + node->children_offset + 1, flag);
900
901                 if(update & PBVH_UpdateBB)
902                         update_node_vb(bvh, node);
903                 if(update & PBVH_UpdateOriginalBB)
904                         node->orig_vb= node->vb;
905         }
906
907         return update;
908 }
909
910 void BLI_pbvh_update(PBVH *bvh, int flag, float (*face_nors)[3])
911 {
912         PBVHNode **nodes;
913         int totnode;
914
915         BLI_pbvh_search_gather(bvh, update_search_cb, SET_INT_IN_POINTER(flag),
916                 &nodes, &totnode);
917
918         if(flag & PBVH_UpdateNormals)
919                 pbvh_update_normals(bvh, nodes, totnode, face_nors);
920
921         if(flag & (PBVH_UpdateBB|PBVH_UpdateOriginalBB|PBVH_UpdateRedraw))
922                 pbvh_update_BB_redraw(bvh, nodes, totnode, flag);
923
924         if(flag & PBVH_UpdateDrawBuffers)
925                 pbvh_update_draw_buffers(bvh, nodes, totnode);
926
927         if(flag & (PBVH_UpdateBB|PBVH_UpdateOriginalBB))
928                 pbvh_flush_bb(bvh, bvh->nodes, flag);
929
930         if(nodes) MEM_freeN(nodes);
931 }
932
933 void BLI_pbvh_redraw_BB(PBVH *bvh, float bb_min[3], float bb_max[3])
934 {
935         PBVHIter iter;
936         PBVHNode *node;
937         BB bb;
938
939         BB_reset(&bb);
940
941         pbvh_iter_begin(&iter, bvh, NULL, NULL);
942
943         while((node=pbvh_iter_next(&iter)))
944                 if(node->flag & PBVH_UpdateRedraw)
945                         BB_expand_with_bb(&bb, &node->vb);
946
947         pbvh_iter_end(&iter);
948
949         copy_v3_v3(bb_min, bb.bmin);
950         copy_v3_v3(bb_max, bb.bmax);
951 }
952
953 void BLI_pbvh_get_grid_updates(PBVH *bvh, int clear, void ***gridfaces, int *totface)
954 {
955         PBVHIter iter;
956         PBVHNode *node;
957         GHashIterator *hiter;
958         GHash *map;
959         void *face, **faces;
960         int i, tot;
961
962         map = BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp);
963
964         pbvh_iter_begin(&iter, bvh, NULL, NULL);
965
966         while((node=pbvh_iter_next(&iter))) {
967                 if(node->flag & PBVH_UpdateNormals) {
968                         for(i = 0; i < node->totprim; ++i) {
969                                 face= bvh->gridfaces[node->prim_indices[i]];
970                                 if(!BLI_ghash_lookup(map, face))
971                                         BLI_ghash_insert(map, face, face);
972                         }
973
974                         if(clear)
975                                 node->flag &= ~PBVH_UpdateNormals;
976                 }
977         }
978
979         pbvh_iter_end(&iter);
980         
981         tot= BLI_ghash_size(map);
982         if(tot == 0) {
983                 *totface= 0;
984                 *gridfaces= NULL;
985                 BLI_ghash_free(map, NULL, NULL);
986                 return;
987         }
988
989         faces= MEM_callocN(sizeof(void*)*tot, "PBVH Grid Faces");
990
991         for(hiter = BLI_ghashIterator_new(map), i = 0;
992             !BLI_ghashIterator_isDone(hiter);
993             BLI_ghashIterator_step(hiter), ++i)
994                 faces[i]= BLI_ghashIterator_getKey(hiter);
995
996         BLI_ghash_free(map, NULL, NULL);
997
998         *totface= tot;
999         *gridfaces= faces;
1000 }
1001
1002 /***************************** Node Access ***********************************/
1003
1004 void BLI_pbvh_node_mark_update(PBVHNode *node)
1005 {
1006         node->flag |= PBVH_UpdateNormals|PBVH_UpdateBB|PBVH_UpdateOriginalBB|PBVH_UpdateDrawBuffers|PBVH_UpdateRedraw;
1007 }
1008
1009 void BLI_pbvh_node_get_verts(PBVH *bvh, PBVHNode *node, int **vert_indices, MVert **verts)
1010 {
1011         if(vert_indices) *vert_indices= node->vert_indices;
1012         if(verts) *verts= bvh->verts;
1013 }
1014
1015 void BLI_pbvh_node_num_verts(PBVH *bvh, PBVHNode *node, int *uniquevert, int *totvert)
1016 {
1017         if(bvh->grids) {
1018                 if(totvert) *totvert= node->totprim*bvh->gridsize*bvh->gridsize;
1019                 if(uniquevert) *uniquevert= *totvert;
1020         }
1021         else {
1022                 if(totvert) *totvert= node->uniq_verts + node->face_verts;
1023                 if(uniquevert) *uniquevert= node->uniq_verts;
1024         }
1025 }
1026
1027 void BLI_pbvh_node_get_grids(PBVH *bvh, PBVHNode *node, int **grid_indices, int *totgrid, int *maxgrid, int *gridsize, DMGridData ***griddata, DMGridAdjacency **gridadj)
1028 {
1029         if(bvh->grids) {
1030                 if(grid_indices) *grid_indices= node->prim_indices;
1031                 if(totgrid) *totgrid= node->totprim;
1032                 if(maxgrid) *maxgrid= bvh->totgrid;
1033                 if(gridsize) *gridsize= bvh->gridsize;
1034                 if(griddata) *griddata= bvh->grids;
1035                 if(gridadj) *gridadj= bvh->gridadj;
1036         }
1037         else {
1038                 if(grid_indices) *grid_indices= NULL;
1039                 if(totgrid) *totgrid= 0;
1040                 if(maxgrid) *maxgrid= 0;
1041                 if(gridsize) *gridsize= 0;
1042                 if(griddata) *griddata= NULL;
1043                 if(gridadj) *gridadj= NULL;
1044         }
1045 }
1046
1047 void BLI_pbvh_node_get_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1048 {
1049         copy_v3_v3(bb_min, node->vb.bmin);
1050         copy_v3_v3(bb_max, node->vb.bmax);
1051 }
1052
1053 void BLI_pbvh_node_get_original_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
1054 {
1055         copy_v3_v3(bb_min, node->orig_vb.bmin);
1056         copy_v3_v3(bb_max, node->orig_vb.bmax);
1057 }
1058
1059 /********************************* Raycast ***********************************/
1060
1061 typedef struct {
1062         /* Ray */
1063         float start[3];
1064         int sign[3];
1065         float inv_dir[3];
1066         int original;
1067 } RaycastData;
1068
1069 /* Adapted from here: http://www.gamedev.net/community/forums/topic.asp?topic_id=459973 */
1070 static int ray_aabb_intersect(PBVHNode *node, void *data_v)
1071 {
1072         RaycastData *ray = data_v;
1073         float bb_min[3], bb_max[3], bbox[2][3];
1074         float tmin, tmax, tymin, tymax, tzmin, tzmax;
1075
1076         if(ray->original)
1077                 BLI_pbvh_node_get_original_BB(node, bb_min, bb_max);
1078         else
1079                 BLI_pbvh_node_get_BB(node, bb_min, bb_max);
1080         
1081         copy_v3_v3(bbox[0], bb_min);
1082         copy_v3_v3(bbox[1], bb_max);
1083
1084         tmin = (bbox[ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1085         tmax = (bbox[1-ray->sign[0]][0] - ray->start[0]) * ray->inv_dir[0];
1086
1087         tymin = (bbox[ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1088         tymax = (bbox[1-ray->sign[1]][1] - ray->start[1]) * ray->inv_dir[1];
1089
1090         if((tmin > tymax) || (tymin > tmax))
1091                 return 0;
1092         if(tymin > tmin)
1093                 tmin = tymin;
1094         if(tymax < tmax)
1095                 tmax = tymax;
1096
1097         tzmin = (bbox[ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1098         tzmax = (bbox[1-ray->sign[2]][2] - ray->start[2]) * ray->inv_dir[2];
1099
1100         if((tmin > tzmax) || (tzmin > tmax))
1101                 return 0;
1102         
1103         return 1;
1104
1105         /* XXX: Not sure about this? 
1106            if(tzmin > tmin)
1107            tmin = tzmin;
1108            if(tzmax < tmax)
1109            tmax = tzmax;
1110            return ((tmin < t1) && (tmax > t0));
1111         */
1112
1113 }
1114
1115 void BLI_pbvh_raycast(PBVH *bvh, BLI_pbvh_HitCallback cb, void *data,
1116                       float ray_start[3], float ray_normal[3], int original)
1117 {
1118         RaycastData rcd;
1119
1120         copy_v3_v3(rcd.start, ray_start);
1121         rcd.inv_dir[0] = 1.0f / ray_normal[0];
1122         rcd.inv_dir[1] = 1.0f / ray_normal[1];
1123         rcd.inv_dir[2] = 1.0f / ray_normal[2];
1124         rcd.sign[0] = rcd.inv_dir[0] < 0;
1125         rcd.sign[1] = rcd.inv_dir[1] < 0;
1126         rcd.sign[2] = rcd.inv_dir[2] < 0;
1127         rcd.original = original;
1128
1129         BLI_pbvh_search_callback(bvh, ray_aabb_intersect, &rcd, cb, data);
1130 }
1131
1132 /* XXX: Code largely copied from bvhutils.c, could be unified */
1133 /* Returns 1 if a better intersection has been found */
1134 static int ray_face_intersection(float ray_start[3], float ray_normal[3],
1135                                  float *t0, float *t1, float *t2, float *t3,
1136                                  float *fdist)
1137 {
1138         int hit = 0;
1139
1140         do
1141         {       
1142                 float dist = FLT_MAX;
1143                         
1144                 if(!isect_ray_tri_epsilon_v3(ray_start, ray_normal, t0, t1, t2,
1145                                          &dist, NULL, 0.001f))
1146                         dist = FLT_MAX;
1147
1148                 if(dist >= 0 && dist < *fdist) {
1149                         hit = 1;
1150                         *fdist = dist;
1151                 }
1152
1153                 t1 = t2;
1154                 t2 = t3;
1155                 t3 = NULL;
1156
1157         } while(t2);
1158
1159         return hit;
1160 }
1161
1162 int BLI_pbvh_node_raycast(PBVH *bvh, PBVHNode *node, float (*origco)[3],
1163         float ray_start[3], float ray_normal[3], float *dist)
1164 {
1165         int hit= 0;
1166
1167         if(bvh->faces) {
1168                 MVert *vert = bvh->verts;
1169                 int *faces= node->prim_indices;
1170                 int *face_verts= node->face_vert_indices;
1171                 int totface= node->totprim;
1172                 int i;
1173
1174                 for(i = 0; i < totface; ++i) {
1175                         MFace *f = bvh->faces + faces[i];
1176
1177                         if(origco) {
1178                                 /* intersect with backuped original coordinates */
1179                                 hit |= ray_face_intersection(ray_start, ray_normal,
1180                                                          origco[face_verts[i*4+0]],
1181                                                          origco[face_verts[i*4+1]],
1182                                                          origco[face_verts[i*4+2]],
1183                                                          f->v4? origco[face_verts[i*4+3]]: NULL,
1184                                                          dist);
1185                         }
1186                         else {
1187                                 /* intersect with current coordinates */
1188                                 hit |= ray_face_intersection(ray_start, ray_normal,
1189                                                          vert[f->v1].co,
1190                                                          vert[f->v2].co,
1191                                                          vert[f->v3].co,
1192                                                          f->v4 ? vert[f->v4].co : NULL,
1193                                                          dist);
1194                         }
1195                 }
1196         }
1197         else {
1198                 int totgrid= node->totprim;
1199                 int gridsize= bvh->gridsize;
1200                 int i, x, y;
1201
1202                 for(i = 0; i < totgrid; ++i) {
1203                         DMGridData *grid= bvh->grids[node->prim_indices[i]];
1204
1205                         for(y = 0; y < gridsize-1; ++y) {
1206                                 for(x = 0; x < gridsize-1; ++x) {
1207                                         if(origco) {
1208                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1209                                                                          origco[y*gridsize + x],
1210                                                                          origco[y*gridsize + x+1],
1211                                                                          origco[(y+1)*gridsize + x+1],
1212                                                                          origco[(y+1)*gridsize + x],
1213                                                                          dist);
1214                                         }
1215                                         else {
1216                                                 hit |= ray_face_intersection(ray_start, ray_normal,
1217                                                                          grid[y*gridsize + x].co,
1218                                                                          grid[y*gridsize + x+1].co,
1219                                                                          grid[(y+1)*gridsize + x+1].co,
1220                                                                          grid[(y+1)*gridsize + x].co,
1221                                                                          dist);
1222                                         }
1223                                 }
1224                         }
1225
1226                         if(origco)
1227                                 origco += gridsize*gridsize;
1228                 }
1229         }
1230
1231         return hit;
1232 }
1233
1234 //#include <GL/glew.h>
1235
1236 void BLI_pbvh_node_draw(PBVHNode *node, void *data)
1237 {
1238 #if 0
1239         /* XXX: Just some quick code to show leaf nodes in different colors */
1240         float col[3]; int i;
1241
1242         if(0) { //is_partial) {
1243                 col[0] = (rand() / (float)RAND_MAX); col[1] = col[2] = 0.6;
1244         }
1245         else {
1246                 srand((long long)node);
1247                 for(i = 0; i < 3; ++i)
1248                         col[i] = (rand() / (float)RAND_MAX) * 0.3 + 0.7;
1249         }
1250         glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, col);
1251
1252         glColor3f(1, 0, 0);
1253 #endif
1254         GPU_draw_buffers(node->draw_buffers);
1255 }
1256
1257 /* Adapted from:
1258    http://www.gamedev.net/community/forums/topic.asp?topic_id=512123
1259    Returns true if the AABB is at least partially within the frustum
1260    (ok, not a real frustum), false otherwise.
1261 */
1262 int BLI_pbvh_node_planes_contain_AABB(PBVHNode *node, void *data)
1263 {
1264         float (*planes)[4] = data;
1265         int i, axis;
1266         float vmin[3], vmax[3], bb_min[3], bb_max[3];
1267
1268         BLI_pbvh_node_get_BB(node, bb_min, bb_max);
1269
1270         for(i = 0; i < 4; ++i) { 
1271                 for(axis = 0; axis < 3; ++axis) {
1272                         if(planes[i][axis] > 0) { 
1273                                 vmin[axis] = bb_min[axis];
1274                                 vmax[axis] = bb_max[axis];
1275                         }
1276                         else {
1277                                 vmin[axis] = bb_max[axis];
1278                                 vmax[axis] = bb_min[axis];
1279                         }
1280                 }
1281                 
1282                 if(dot_v3v3(planes[i], vmin) + planes[i][3] > 0)
1283                         return 0;
1284         } 
1285
1286         return 1;
1287 }
1288
1289 void BLI_pbvh_draw(PBVH *bvh, float (*planes)[4], float (*face_nors)[3])
1290 {
1291         BLI_pbvh_update(bvh, PBVH_UpdateNormals|PBVH_UpdateDrawBuffers, face_nors);
1292
1293         if(planes) {
1294                 BLI_pbvh_search_callback(bvh, BLI_pbvh_node_planes_contain_AABB,
1295                                 planes, BLI_pbvh_node_draw, NULL);
1296         }
1297         else {
1298                 BLI_pbvh_search_callback(bvh, NULL, NULL, BLI_pbvh_node_draw, NULL);
1299         }
1300 }
1301