add mesh distort display mode (highlights distorted faces)
[blender-staging.git] / source / blender / blenkernel / intern / editmesh_bvh.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2010 by Blender Foundation.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): Joseph Eagar
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/blenkernel/intern/editmesh_bvh.c
29  *  \ingroup bke
30  */
31
32 #include "MEM_guardedalloc.h"
33
34 #include "DNA_object_types.h"
35
36 #include "BLI_math.h"
37 #include "BLI_kdopbvh.h"
38
39 #include "BKE_editmesh.h"
40
41 #include "BKE_editmesh_bvh.h"  /* own include */
42
43
44 struct BMBVHTree {
45         BVHTree *tree;
46
47         BMEditMesh *em;
48         BMesh *bm;
49
50         const float (*cos_cage)[3];
51         bool cos_cage_free;
52
53         int flag;
54 };
55
56 BMBVHTree *BKE_bmbvh_new(BMEditMesh *em, int flag, const float (*cos_cage)[3], const bool cos_cage_free)
57 {
58         /* could become argument */
59         const float epsilon = FLT_EPSILON * 2.0f;
60
61         struct BMLoop *(*looptris)[3] = em->looptris;
62         BMBVHTree *bmtree = MEM_callocN(sizeof(*bmtree), "BMBVHTree");
63         float cos[3][3];
64         int i;
65         int tottri;
66
67         /* BKE_editmesh_tessface_calc() must be called already */
68         BLI_assert(em->tottri != 0 || em->bm->totface == 0);
69
70         if (cos_cage) {
71                 BM_mesh_elem_index_ensure(em->bm, BM_VERT);
72         }
73
74         bmtree->em = em;
75         bmtree->bm = em->bm;
76         bmtree->cos_cage = cos_cage;
77         bmtree->cos_cage_free = cos_cage_free;
78         bmtree->flag = flag;
79
80         if (flag & (BMBVH_RESPECT_SELECT)) {
81                 tottri = 0;
82                 for (i = 0; i < em->tottri; i++) {
83                         if (BM_elem_flag_test(looptris[i][0]->f, BM_ELEM_SELECT)) {
84                                 tottri++;
85                         }
86                 }
87         }
88         else if (flag & (BMBVH_RESPECT_HIDDEN)) {
89                 tottri = 0;
90                 for (i = 0; i < em->tottri; i++) {
91                         if (!BM_elem_flag_test(looptris[i][0]->f, BM_ELEM_HIDDEN)) {
92                                 tottri++;
93                         }
94                 }
95         }
96         else {
97                 tottri = em->tottri;
98         }
99
100         bmtree->tree = BLI_bvhtree_new(tottri, epsilon, 8, 8);
101
102         for (i = 0; i < em->tottri; i++) {
103
104                 if (flag & BMBVH_RESPECT_SELECT) {
105                         /* note, the arrays wont align now! take care */
106                         if (!BM_elem_flag_test(em->looptris[i][0]->f, BM_ELEM_SELECT)) {
107                                 continue;
108                         }
109                 }
110                 else if (flag & BMBVH_RESPECT_HIDDEN) {
111                         /* note, the arrays wont align now! take care */
112                         if (BM_elem_flag_test(looptris[i][0]->f, BM_ELEM_HIDDEN)) {
113                                 continue;
114                         }
115                 }
116
117                 if (cos_cage) {
118                         copy_v3_v3(cos[0], cos_cage[BM_elem_index_get(looptris[i][0]->v)]);
119                         copy_v3_v3(cos[1], cos_cage[BM_elem_index_get(looptris[i][1]->v)]);
120                         copy_v3_v3(cos[2], cos_cage[BM_elem_index_get(looptris[i][2]->v)]);
121                 }
122                 else {
123                         copy_v3_v3(cos[0], looptris[i][0]->v->co);
124                         copy_v3_v3(cos[1], looptris[i][1]->v->co);
125                         copy_v3_v3(cos[2], looptris[i][2]->v->co);
126                 }
127
128                 BLI_bvhtree_insert(bmtree->tree, i, (float *)cos, 3);
129         }
130         
131         BLI_bvhtree_balance(bmtree->tree);
132         
133         return bmtree;
134 }
135
136 void BKE_bmbvh_free(BMBVHTree *bmtree)
137 {
138         BLI_bvhtree_free(bmtree->tree);
139         
140         if (bmtree->cos_cage && bmtree->cos_cage_free) {
141                 MEM_freeN(bmtree->cos_cage);
142         }
143         
144         MEM_freeN(bmtree);
145 }
146
147 BVHTree *BKE_bmbvh_tree_get(BMBVHTree *bmtree)
148 {
149         return bmtree->tree;
150 }
151
152
153
154 /* -------------------------------------------------------------------- */
155 /* Utility BMesh cast/intersect functions */
156
157 /**
158  * Return the coords from a triangle.
159  */
160 static void bmbvh_tri_from_face(const float *cos[3],
161                                 const BMLoop **ltri, const float (*cos_cage)[3])
162 {
163         if (cos_cage == NULL) {
164                 cos[0] = ltri[0]->v->co;
165                 cos[1] = ltri[1]->v->co;
166                 cos[2] = ltri[2]->v->co;
167         }
168         else {
169                 cos[0] = cos_cage[BM_elem_index_get(ltri[0]->v)];
170                 cos[1] = cos_cage[BM_elem_index_get(ltri[1]->v)];
171                 cos[2] = cos_cage[BM_elem_index_get(ltri[2]->v)];
172         }
173 }
174
175
176 /* taken from bvhutils.c */
177
178 /* -------------------------------------------------------------------- */
179 /* BKE_bmbvh_ray_cast */
180
181 struct RayCastUserData {
182         /* from the bmtree */
183         const BMLoop *(*looptris)[3];
184         const float (*cos_cage)[3];
185
186         /* from the hit */
187         float uv[2];
188 };
189
190 static void bmbvh_ray_cast_cb(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
191 {
192         struct RayCastUserData *bmcb_data = userdata;
193         const BMLoop **ltri = bmcb_data->looptris[index];
194         float dist, uv[2];
195         const float *tri_cos[3];
196
197         bmbvh_tri_from_face(tri_cos, ltri, bmcb_data->cos_cage);
198
199         if (isect_ray_tri_v3(ray->origin, ray->direction, tri_cos[0], tri_cos[1], tri_cos[2], &dist, uv) &&
200             (dist < hit->dist))
201         {
202                 hit->dist = dist;
203                 hit->index = index;
204
205                 copy_v3_v3(hit->no, ltri[0]->f->no);
206
207                 copy_v3_v3(hit->co, ray->direction);
208                 normalize_v3(hit->co);
209                 mul_v3_fl(hit->co, dist);
210                 add_v3_v3(hit->co, ray->origin);
211                 
212                 copy_v2_v2(bmcb_data->uv, uv);
213         }
214 }
215
216 BMFace *BKE_bmbvh_ray_cast(BMBVHTree *bmtree, const float co[3], const float dir[3],
217                            float *r_dist, float r_hitout[3], float r_cagehit[3])
218 {
219         BVHTreeRayHit hit;
220         struct RayCastUserData bmcb_data;
221         const float dist = r_dist ? *r_dist : FLT_MAX;
222
223         if (bmtree->cos_cage) BLI_assert(!(bmtree->em->bm->elem_index_dirty & BM_VERT));
224
225         hit.dist = dist;
226         hit.index = -1;
227
228         /* ok to leave 'uv' uninitialized */
229         bmcb_data.looptris = (const BMLoop *(*)[3])bmtree->em->looptris;
230         bmcb_data.cos_cage = (const float (*)[3])bmtree->cos_cage;
231         
232         BLI_bvhtree_ray_cast(bmtree->tree, co, dir, 0.0f, &hit, bmbvh_ray_cast_cb, &bmcb_data);
233         if (hit.index != -1 && hit.dist != dist) {
234                 if (r_hitout) {
235                         if (bmtree->flag & BMBVH_RETURN_ORIG) {
236                                 BMLoop **ltri = bmtree->em->looptris[hit.index];
237                                 interp_v3_v3v3v3_uv(r_hitout, ltri[0]->v->co, ltri[1]->v->co, ltri[2]->v->co, bmcb_data.uv);
238                         }
239                         else {
240                                 copy_v3_v3(r_hitout, hit.co);
241                         }
242
243                         if (r_cagehit) {
244                                 copy_v3_v3(r_cagehit, hit.co);
245                         }
246                 }
247
248                 if (r_dist) {
249                         *r_dist = hit.dist;
250                 }
251
252                 return bmtree->em->looptris[hit.index][0]->f;
253         }
254
255         return NULL;
256 }
257
258
259 /* -------------------------------------------------------------------- */
260 /* BKE_bmbvh_find_face_segment */
261
262 struct SegmentUserData {
263         /* from the bmtree */
264         const BMLoop *(*looptris)[3];
265         const float (*cos_cage)[3];
266
267         /* from the hit */
268         float uv[2];
269         const float *co_a, *co_b;
270 };
271
272 static void bmbvh_find_face_segment_cb(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
273 {
274         struct SegmentUserData *bmcb_data = userdata;
275         const BMLoop **ltri = bmcb_data->looptris[index];
276         float dist, uv[2];
277         const float *tri_cos[3];
278
279         bmbvh_tri_from_face(tri_cos, ltri, bmcb_data->cos_cage);
280
281         if (equals_v3v3(bmcb_data->co_a, tri_cos[0]) ||
282             equals_v3v3(bmcb_data->co_a, tri_cos[1]) ||
283             equals_v3v3(bmcb_data->co_a, tri_cos[2]) ||
284
285             equals_v3v3(bmcb_data->co_b, tri_cos[0]) ||
286             equals_v3v3(bmcb_data->co_b, tri_cos[1]) ||
287             equals_v3v3(bmcb_data->co_b, tri_cos[2]))
288         {
289                 return;
290         }
291
292         if (isect_ray_tri_v3(ray->origin, ray->direction, tri_cos[0], tri_cos[1], tri_cos[2], &dist, uv) &&
293             (dist < hit->dist))
294         {
295                 hit->dist = dist;
296                 hit->index = index;
297
298                 copy_v3_v3(hit->no, ltri[0]->f->no);
299
300                 copy_v3_v3(hit->co, ray->direction);
301                 normalize_v3(hit->co);
302                 mul_v3_fl(hit->co, dist);
303                 add_v3_v3(hit->co, ray->origin);
304
305                 copy_v2_v2(bmcb_data->uv, uv);
306         }
307 }
308
309 BMFace *BKE_bmbvh_find_face_segment(BMBVHTree *bmtree, const float co_a[3], const float co_b[3],
310                                     float *r_fac, float r_hitout[3], float r_cagehit[3])
311 {
312         BVHTreeRayHit hit;
313         struct SegmentUserData bmcb_data;
314         const float dist = len_v3v3(co_a, co_b);
315         float dir[3];
316
317         if (bmtree->cos_cage) BLI_assert(!(bmtree->em->bm->elem_index_dirty & BM_VERT));
318
319         sub_v3_v3v3(dir, co_b, co_a);
320
321         hit.dist = dist;
322         hit.index = -1;
323
324         /* ok to leave 'uv' uninitialized */
325         bmcb_data.looptris = (const BMLoop *(*)[3])bmtree->em->looptris;
326         bmcb_data.cos_cage = (const float (*)[3])bmtree->cos_cage;
327         bmcb_data.co_a = co_a;
328         bmcb_data.co_b = co_b;
329
330         BLI_bvhtree_ray_cast(bmtree->tree, co_a, dir, 0.0f, &hit, bmbvh_find_face_segment_cb, &bmcb_data);
331         if (hit.index != -1 && hit.dist != dist) {
332                 /* duplicate of BKE_bmbvh_ray_cast() */
333                 if (r_hitout) {
334                         if (bmtree->flag & BMBVH_RETURN_ORIG) {
335                                 BMLoop **ltri = bmtree->em->looptris[hit.index];
336                                 interp_v3_v3v3v3_uv(r_hitout, ltri[0]->v->co, ltri[1]->v->co, ltri[2]->v->co, bmcb_data.uv);
337                         }
338                         else {
339                                 copy_v3_v3(r_hitout, hit.co);
340                         }
341
342                         if (r_cagehit) {
343                                 copy_v3_v3(r_cagehit, hit.co);
344                         }
345                 }
346                 /* end duplicate */
347
348                 if (r_fac) {
349                         *r_fac = hit.dist / dist;
350                 }
351
352                 return bmtree->em->looptris[hit.index][0]->f;
353         }
354
355         return NULL;
356 }
357
358
359 /* -------------------------------------------------------------------- */
360 /* BKE_bmbvh_find_vert_closest */
361
362 struct VertSearchUserData {
363         /* from the bmtree */
364         const BMLoop *(*looptris)[3];
365         const float (*cos_cage)[3];
366
367         /* from the hit */
368         float maxdist;
369         int   index_tri;
370 };
371
372 static void bmbvh_find_vert_closest_cb(void *userdata, int index, const float *UNUSED(co), BVHTreeNearest *hit)
373 {
374         struct VertSearchUserData *bmcb_data = userdata;
375         const BMLoop **ltri = bmcb_data->looptris[index];
376         const float maxdist = bmcb_data->maxdist;
377         float dist;
378         int i;
379
380         const float *tri_cos[3];
381
382         bmbvh_tri_from_face(tri_cos, ltri, bmcb_data->cos_cage);
383
384         for (i = 0; i < 3; i++) {
385                 dist = len_v3v3(hit->co, tri_cos[i]);
386                 if (dist < hit->dist && dist < maxdist) {
387                         copy_v3_v3(hit->co, tri_cos[i]);
388                         /* XXX, normal ignores cage */
389                         copy_v3_v3(hit->no, ltri[i]->v->no);
390                         hit->dist = dist;
391                         hit->index = index;
392                         bmcb_data->index_tri = i;
393                 }
394         }
395 }
396
397 BMVert *BKE_bmbvh_find_vert_closest(BMBVHTree *bmtree, const float co[3], const float maxdist)
398 {
399         BVHTreeNearest hit;
400         struct VertSearchUserData bmcb_data;
401
402         if (bmtree->cos_cage) BLI_assert(!(bmtree->em->bm->elem_index_dirty & BM_VERT));
403
404         copy_v3_v3(hit.co, co);
405         /* XXX, why x5, scampbell */
406         hit.dist = maxdist * 5;
407         hit.index = -1;
408
409         bmcb_data.looptris = (const BMLoop *(*)[3])bmtree->em->looptris;
410         bmcb_data.cos_cage = (const float (*)[3])bmtree->cos_cage;
411         bmcb_data.maxdist = maxdist;
412
413         BLI_bvhtree_find_nearest(bmtree->tree, co, &hit, bmbvh_find_vert_closest_cb, &bmcb_data);
414         if (hit.dist != FLT_MAX && hit.index != -1) {
415                 BMLoop **ltri = bmtree->em->looptris[hit.index];
416                 return ltri[bmcb_data.index_tri]->v;
417         }
418
419         return NULL;
420 }