Add mathutils.bvhtree API
[blender.git] / source / blender / python / mathutils / mathutils_bvhtree.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  * Contributor(s): Lukas Toenne, Campbell Barton
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  */
22
23 /** \file blender/python/mathutils/mathutils_bvhtree.c
24  *  \ingroup mathutils
25  *
26  * This file defines the 'mathutils.bvhtree' module, a general purpose module to access
27  * blenders bvhtree for mesh surface nearest-element search and ray casting.
28  */
29
30 #include <Python.h>
31
32 #include "MEM_guardedalloc.h"
33
34 #include "BLI_utildefines.h"
35 #include "BLI_kdopbvh.h"
36 #include "BLI_polyfill2d.h"
37 #include "BLI_math.h"
38 #include "BLI_ghash.h"
39 #include "BLI_memarena.h"
40
41 #include "BKE_bvhutils.h"
42
43 #include "../generic/py_capi_utils.h"
44 #include "../generic/python_utildefines.h"
45
46 #include "mathutils.h"
47 #include "mathutils_bvhtree.h"  /* own include */
48
49 #ifndef MATH_STANDALONE
50 #include "DNA_object_types.h"
51
52 #include "BKE_customdata.h"
53 #include "BKE_DerivedMesh.h"
54 #include "BKE_editmesh_bvh.h"
55
56 #include "bmesh.h"
57
58 #include "../bmesh/bmesh_py_types.h"
59 #endif  /* MATH_STANDALONE */
60
61
62 #include "BLI_strict_flags.h"
63
64
65 /* -------------------------------------------------------------------- */
66
67 /** \name Docstring (snippets)
68  * \{ */
69
70 #define PYBVH_FIND_GENERIC_DISTANCE_DOC \
71 "   :arg distance: Maximum distance threshold.\n" \
72 "   :type distance: float\n"
73
74 #define PYBVH_FIND_GENERIC_RETURN_DOC \
75 "   :return: Returns a tuple (:class:`Vector` location, :class:`Vector` normal, int index, float distance),\n" \
76 "      Values will all be None if no hit is found.\n" \
77 "   :rtype: :class:`tuple`\n"
78
79 #define PYBVH_FROM_GENERIC_EPSILON_DOC \
80 "   :arg epsilon: Increase the threshold for detecting overlap and raycast hits.\n" \
81 "   :type epsilon: float\n"
82
83 /** \} */
84
85 /* sqrt(FLT_MAX) */
86 #define PYBVH_MAX_DIST_STR "1.84467e+19"
87 static const float max_dist_default = 1.844674352395373e+19f;
88
89 static const char PY_BVH_TREE_TYPE_DEFAULT = 4;
90 static const char PY_BVH_AXIS_DEFAULT = 6;
91
92 typedef struct {
93         PyObject_HEAD
94         BVHTree *tree;
95         float epsilon;
96
97         float (*coords)[3];
98         unsigned int (*tris)[3];
99         unsigned int coords_len, tris_len;
100
101         /* Optional members */
102         /* aligned with 'tris' */
103         int *orig_index;
104         /* aligned with array that 'orig_index' points to */
105         float (*orig_normal)[3];
106 } PyBVHTree;
107
108
109 /* -------------------------------------------------------------------- */
110
111 /** \name Utility helper functions
112  * \{ */
113
114 static PyObject *bvhtree_CreatePyObject(
115         BVHTree *tree, float epsilon,
116
117         float (*coords)[3], unsigned int coords_len,
118         unsigned int (*tris)[3], unsigned int tris_len,
119
120         /* optional arrays */
121         int *orig_index, float (*orig_normal)[3])
122 {
123         PyBVHTree *result = PyObject_New(PyBVHTree, &PyBVHTree_Type);
124
125         result->tree = tree;
126         result->epsilon = epsilon;
127
128         result->coords = coords;
129         result->tris = tris;
130         result->coords_len = coords_len;
131         result->tris_len = tris_len;
132
133         result->orig_index = orig_index;
134         result->orig_normal = orig_normal;
135
136         return (PyObject *)result;
137 }
138
139 /** \} */
140
141
142 /* -------------------------------------------------------------------- */
143
144 /** \name BVHTreeRayHit to Python utilities
145  * \{ */
146
147 static void py_bvhtree_raycast_to_py_tuple(const BVHTreeRayHit *hit, PyObject *py_retval)
148 {
149         BLI_assert(hit->index >= 0);
150         BLI_assert(PyTuple_GET_SIZE(py_retval) == 4);
151
152         PyTuple_SET_ITEMS(py_retval,
153                 Vector_CreatePyObject(hit->co, 3, NULL),
154                 Vector_CreatePyObject(hit->no, 3, NULL),
155                 PyLong_FromLong(hit->index),
156                 PyFloat_FromDouble(hit->dist));
157
158 }
159
160 static PyObject *py_bvhtree_raycast_to_py(const BVHTreeRayHit *hit)
161 {
162         PyObject *py_retval = PyTuple_New(4);
163
164         py_bvhtree_raycast_to_py_tuple(hit, py_retval);
165
166         return py_retval;
167 }
168
169 static PyObject *py_bvhtree_raycast_to_py_none(void)
170 {
171         PyObject *py_retval = PyTuple_New(4);
172
173         PyC_Tuple_Fill(py_retval, Py_None);
174
175         return py_retval;
176 }
177
178 #if 0
179 static PyObject *py_bvhtree_raycast_to_py_and_check(const BVHTreeRayHit *hit)
180 {
181         PyObject *py_retval;
182
183         py_retval = PyTuple_New(4);
184
185         if (hit->index != -1) {
186                 py_bvhtree_raycast_to_py_tuple(hit, py_retval);
187         }
188         else {
189                 PyC_Tuple_Fill(py_retval, Py_None);
190         }
191
192         return py_retval;
193 }
194 #endif
195
196 /** \} */
197
198
199 /* -------------------------------------------------------------------- */
200
201 /** \name BVHTreeNearest to Python utilities
202  * \{ */
203
204 static void py_bvhtree_nearest_to_py_tuple(const BVHTreeNearest *nearest, PyObject *py_retval)
205 {
206         BLI_assert(nearest->index >= 0);
207         BLI_assert(PyTuple_GET_SIZE(py_retval) == 4);
208
209         PyTuple_SET_ITEMS(py_retval,
210                 Vector_CreatePyObject(nearest->co, 3, NULL),
211                 Vector_CreatePyObject(nearest->no, 3, NULL),
212                 PyLong_FromLong(nearest->index),
213                 PyFloat_FromDouble(sqrtf(nearest->dist_sq)));
214
215 }
216
217 static PyObject *py_bvhtree_nearest_to_py(const BVHTreeNearest *nearest)
218 {
219         PyObject *py_retval = PyTuple_New(4);
220
221         py_bvhtree_nearest_to_py_tuple(nearest, py_retval);
222
223         return py_retval;
224 }
225
226 static PyObject *py_bvhtree_nearest_to_py_none(void)
227 {
228         PyObject *py_retval = PyTuple_New(4);
229
230         PyC_Tuple_Fill(py_retval, Py_None);
231
232         return py_retval;
233 }
234
235 #if 0
236 static PyObject *py_bvhtree_nearest_to_py_and_check(const BVHTreeNearest *nearest)
237 {
238         PyObject *py_retval;
239
240         py_retval = PyTuple_New(4);
241
242         if (nearest->index != -1) {
243                 py_bvhtree_nearest_to_py_tuple(nearest, py_retval);
244         }
245         else {
246                 PyC_Tuple_Fill(py_retval, Py_None);
247         }
248
249         return py_retval;
250 }
251 #endif
252
253 /** \} */
254
255 static void py_bvhtree__tp_dealloc(PyBVHTree *self)
256 {
257         if (self->tree) {
258                 BLI_bvhtree_free(self->tree);
259         }
260
261         MEM_SAFE_FREE(self->coords);
262         MEM_SAFE_FREE(self->tris);
263
264         MEM_SAFE_FREE(self->orig_index);
265         MEM_SAFE_FREE(self->orig_normal);
266
267         Py_TYPE(self)->tp_free((PyObject *)self);
268 }
269
270
271 /* -------------------------------------------------------------------- */
272
273 /** \name Methods
274  * \{ */
275
276 static void py_bvhtree_raycast_cb(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
277 {
278         const PyBVHTree *self = userdata;
279
280         const float (*coords)[3] = self->coords;
281         const unsigned int *tri = self->tris[index];
282         const float *tri_co[3] = {coords[tri[0]], coords[tri[1]], coords[tri[2]]};
283         float dist;
284
285         if (self->epsilon == 0.0f) {
286                 dist = bvhtree_ray_tri_intersection(ray, hit->dist, UNPACK3(tri_co));
287         }
288         else {
289                 dist = bvhtree_sphereray_tri_intersection(ray, self->epsilon, hit->dist, UNPACK3(tri_co));
290         }
291
292         if (dist >= 0 && dist < hit->dist) {
293                 hit->index = self->orig_index ? self->orig_index[index] : index;
294                 hit->dist = dist;
295                 madd_v3_v3v3fl(hit->co, ray->origin, ray->direction, dist);
296                 if (self->orig_normal) {
297                         copy_v3_v3(hit->no, self->orig_normal[hit->index]);
298                 }
299                 else {
300                         normal_tri_v3(hit->no, UNPACK3(tri_co));
301                 }
302         }
303 }
304
305 static void py_bvhtree_nearest_point_cb(void *userdata, int index, const float co[3], BVHTreeNearest *nearest)
306 {
307         PyBVHTree *self = userdata;
308
309         const float (*coords)[3] = self->coords;
310         const unsigned int *tri = self->tris[index];
311         const float *tri_co[3] = {coords[tri[0]], coords[tri[1]], coords[tri[2]]};
312         float nearest_tmp[3], dist_sq;
313
314         closest_on_tri_to_point_v3(nearest_tmp, co, UNPACK3(tri_co));
315         dist_sq = len_squared_v3v3(co, nearest_tmp);
316
317         if (dist_sq < nearest->dist_sq) {
318                 nearest->index = self->orig_index ? self->orig_index[index] : index;
319                 nearest->dist_sq = dist_sq;
320                 copy_v3_v3(nearest->co, nearest_tmp);
321                 if (self->orig_normal) {
322                         copy_v3_v3(nearest->no, self->orig_normal[nearest->index]);
323                 }
324                 else {
325                         normal_tri_v3(nearest->no, UNPACK3(tri_co));
326                 }
327         }
328 }
329
330 PyDoc_STRVAR(py_bvhtree_ray_cast_doc,
331 ".. method:: ray_cast(co, direction, distance=sys.float_info.max)\n"
332 "\n"
333 "   Cast a ray onto the mesh.\n"
334 "\n"
335 "   :arg co: Start location of the ray in object space.\n"
336 "   :type co: :class:`Vector`\n"
337 "   :arg direction: Direction of the ray in object space.\n"
338 "   :type direction: :class:`Vector`\n"
339 PYBVH_FIND_GENERIC_DISTANCE_DOC
340 PYBVH_FIND_GENERIC_RETURN_DOC
341 );
342 static PyObject *py_bvhtree_ray_cast(PyBVHTree *self, PyObject *args)
343 {
344         const char *error_prefix = "ray_cast";
345         float co[3], direction[3];
346         float max_dist = FLT_MAX;
347         BVHTreeRayHit hit;
348
349         /* parse args */
350         {
351                 PyObject *py_co, *py_direction;
352
353                 if (!PyArg_ParseTuple(
354                         args, (char *)"OO|f:ray_cast",
355                         &py_co, &py_direction, max_dist))
356                 {
357                         return NULL;
358                 }
359
360                 if ((mathutils_array_parse(co, 2, 3 | MU_ARRAY_ZERO, py_co, error_prefix) == -1) ||
361                     (mathutils_array_parse(direction, 2, 3 | MU_ARRAY_ZERO, py_direction, error_prefix) == -1))
362                 {
363                         return NULL;
364                 }
365
366                 normalize_v3(direction);
367         }
368
369         hit.dist = max_dist;
370         hit.index = -1;
371
372         /* may fail if the mesh has no faces, in that case the ray-cast misses */
373         if (self->tree) {
374                 if (BLI_bvhtree_ray_cast(
375                         self->tree, co, direction, 0.0f, &hit,
376                         py_bvhtree_raycast_cb, self) != -1)
377                 {
378                         return py_bvhtree_raycast_to_py(&hit);
379                 }
380         }
381
382         return py_bvhtree_raycast_to_py_none();
383 }
384
385 PyDoc_STRVAR(py_bvhtree_find_nearest_doc,
386 ".. method:: find_nearest(co, distance=" PYBVH_MAX_DIST_STR ")\n"
387 "\n"
388 "   Find the nearest element to a point.\n"
389 "\n"
390 "   :arg co: Find nearest element to this point.\n"
391 "   :type co: :class:`Vector`\n"
392 PYBVH_FIND_GENERIC_DISTANCE_DOC
393 PYBVH_FIND_GENERIC_RETURN_DOC
394 );
395 static PyObject *py_bvhtree_find_nearest(PyBVHTree *self, PyObject *args)
396 {
397         const char *error_prefix = "find_nearest";
398         float co[3];
399         float max_dist = max_dist_default;
400
401         BVHTreeNearest nearest;
402
403         /* parse args */
404         {
405                 PyObject *py_co;
406
407                 if (!PyArg_ParseTuple(
408                         args, (char *)"O|f:find_nearest",
409                         &py_co, &max_dist))
410                 {
411                         return NULL;
412                 }
413
414                 if (mathutils_array_parse(co, 2, 3 | MU_ARRAY_ZERO, py_co, error_prefix) == -1) {
415                         return NULL;
416                 }
417         }
418
419         nearest.index = -1;
420         nearest.dist_sq = max_dist * max_dist;
421
422         /* may fail if the mesh has no faces, in that case the ray-cast misses */
423         if (self->tree) {
424                 if (BLI_bvhtree_find_nearest(
425                         self->tree, co, &nearest,
426                         py_bvhtree_nearest_point_cb, self) != -1)
427                 {
428                         return py_bvhtree_nearest_to_py(&nearest);
429                 }
430         }
431
432         return py_bvhtree_nearest_to_py_none();
433 }
434
435 BLI_INLINE unsigned int overlap_hash(const void *overlap_v)
436 {
437         const BVHTreeOverlap *overlap = overlap_v;
438         /* same constants as edge-hash */
439         return (((unsigned int)overlap->indexA * 65) ^ ((unsigned int)overlap->indexA * 31));
440 }
441
442 BLI_INLINE bool overlap_cmp(const void *a_v, const void *b_v)
443 {
444         const BVHTreeOverlap *a = a_v;
445         const BVHTreeOverlap *b = b_v;
446         return (memcmp(a, b, sizeof(*a)) != 0);
447 }
448
449 PyDoc_STRVAR(py_bvhtree_overlap_doc,
450 ".. method:: overlap(other_tree)\n"
451 "\n"
452 "   Find overlapping indices between 2 trees.\n"
453 "\n"
454 "   :arg other_tree: Other tree to preform overlap test on.\n"
455 "   :type other_tree: :class:`BVHTree`\n"
456 "   :return: Returns a list of unique index pairs,"
457 "      the first index referencing this tree, the second referencing the **other_tree**.\n"
458 "   :rtype: :class:`list`\n"
459 );
460 static PyObject *py_bvhtree_overlap(PyBVHTree *self, PyBVHTree *other)
461 {
462         BVHTreeOverlap *overlap;
463         unsigned int overlap_len = 0;
464         PyObject *ret;
465
466         if (!PyBVHTree_CheckExact(other)) {
467                 PyErr_SetString(PyExc_ValueError, "Expected a BVHTree argument");
468                 return NULL;
469         }
470
471         overlap = BLI_bvhtree_overlap(self->tree, other->tree, &overlap_len);
472
473         ret = PyList_New(0);
474
475         if (overlap == NULL) {
476                 /* pass */
477         }
478         else {
479                 const float epsilon = max_ff(self->epsilon, other->epsilon);
480                 bool use_unique = (self->orig_index || other->orig_index);
481                 GSet *pair_test = use_unique ? BLI_gset_new_ex(overlap_hash, overlap_cmp, __func__, overlap_len) : NULL;
482                 /* simple case, no index remapping */
483                 unsigned int i;
484
485                 for (i = 0; i < overlap_len; i++) {
486                         const unsigned int *tri_a = self->tris[overlap[i].indexA];
487                         const unsigned int *tri_b = other->tris[overlap[i].indexB];
488                         const float *tri_a_co[3] = {self->coords[tri_a[0]], self->coords[tri_a[1]], self->coords[tri_a[2]]};
489                         const float *tri_b_co[3] = {other->coords[tri_b[0]], other->coords[tri_b[1]], other->coords[tri_b[2]]};
490
491                         if (isect_tri_tri_epsilon_v3(UNPACK3(tri_a_co), UNPACK3(tri_b_co), NULL, NULL, epsilon)) {
492                                 PyObject *item;
493
494                                 if (use_unique) {
495                                         if (self->orig_index) {
496                                                 overlap[i].indexA = self->orig_index[overlap[i].indexA];
497                                         }
498                                         if (other->orig_index) {
499                                                 overlap[i].indexB = other->orig_index[overlap[i].indexB];
500                                         }
501
502                                         /* skip if its already added */
503                                         if (!BLI_gset_add(pair_test, &overlap[i])) {
504                                                 continue;
505                                         }
506                                 }
507
508                                 item = PyTuple_New(2);
509                                 PyTuple_SET_ITEMS(item,
510                                         PyLong_FromLong(overlap[i].indexA),
511                                         PyLong_FromLong(overlap[i].indexB));
512
513                                 PyList_Append(ret, item);
514                                 Py_DECREF(item);
515                         }
516                 }
517
518                 if (pair_test) {
519                         BLI_gset_free(pair_test, NULL);
520                 }
521         }
522
523         if (overlap) {
524                 MEM_freeN(overlap);
525         }
526
527         return ret;
528 }
529
530 /** \} */
531
532
533 /* -------------------------------------------------------------------- */
534
535 /** \name Class Methods
536  * \{ */
537
538 PyDoc_STRVAR(C_BVHTree_FromPolygons_doc,
539 ".. classmethod:: FromPolygons(vertices, polygons, all_triangles=False, epsilon=0.0)\n"
540 "\n"
541 "   BVH tree constructed geometry passed in as arguments.\n"
542 "\n"
543 "   :arg vertices: float triplets each representing ``(x, y, z)``\n"
544 "   :type vertices: float triplet sequence\n"
545 "   :arg polygons: Sequence of polyugons, each containing indices to the vertices argument.\n"
546 "   :type polygons: Sequence of sequences containing ints\n"
547 "   :arg all_triangles: Use when all **polygons** are triangles for more efficient conversion.\n"
548 "   :type all_triangles: bool\n"
549 PYBVH_FROM_GENERIC_EPSILON_DOC
550 );
551 static PyObject *C_BVHTree_FromPolygons(PyObject *UNUSED(cls), PyObject *args, PyObject *kwargs)
552 {
553         const char *error_prefix = "BVHTree.FromPolygons";
554         const char *keywords[] = {"vertices", "polygons", "all_triangles", "epsilon", NULL};
555
556         PyObject *py_coords, *py_tris;
557         PyObject *py_coords_fast = NULL, *py_tris_fast = NULL;
558
559         MemArena *poly_arena = NULL;
560         MemArena *pf_arena = NULL;
561
562         float (*coords)[3] = NULL;
563         unsigned int (*tris)[3] = NULL;
564         unsigned int coords_len, tris_len;
565         float epsilon = 0.0f;
566         int all_triangles = 0;
567
568         /* when all_triangles is False */
569         int *orig_index = NULL;
570         float (*orig_normal)[3] = NULL;
571
572         unsigned int i;
573         bool valid = true;
574
575
576         if (!PyArg_ParseTupleAndKeywords(
577                 args, kwargs, (char *)"OO|$if:BVHTree.FromPolygons", (char **)keywords,
578                 &py_coords, &py_tris, &all_triangles, &epsilon))
579         {
580                 return NULL;
581         }
582
583         if (!(py_coords_fast = PySequence_Fast(py_coords, error_prefix)) ||
584             !(py_tris_fast  = PySequence_Fast(py_tris, error_prefix)))
585         {
586                 Py_XDECREF(py_coords_fast);
587                 return NULL;
588         }
589
590         if (valid) {
591                 PyObject **py_coords_fast_items = PySequence_Fast_ITEMS(py_coords_fast);
592                 coords_len = (unsigned int)PySequence_Fast_GET_SIZE(py_coords_fast);
593                 coords = MEM_mallocN((size_t)coords_len * sizeof(*coords), __func__);
594
595                 for (i = 0; i < coords_len; i++) {
596                         PyObject *py_vert = py_coords_fast_items[i];
597
598                         if (mathutils_array_parse(coords[i], 3, 3, py_vert, "BVHTree vertex: ") == -1) {
599                                 valid = false;
600                                 break;
601                         }
602                 }
603         }
604
605         if (valid == false) {
606                 /* pass */
607         }
608         else if (all_triangles) {
609                 /* all triangles, simple case */
610                 PyObject **py_tris_fast_items = PySequence_Fast_ITEMS(py_tris_fast);
611                 tris_len = (unsigned int)PySequence_Fast_GET_SIZE(py_tris_fast);
612                 tris = MEM_mallocN((size_t)tris_len * sizeof(*tris), __func__);
613
614                 for (i = 0; i < tris_len; i++) {
615                         PyObject *py_tricoords = py_tris_fast_items[i];
616                         PyObject *py_tricoords_fast;
617                         PyObject **py_tricoords_fast_items;
618                         unsigned int *tri = tris[i];
619                         int j;
620
621                         if (!(py_tricoords_fast = PySequence_Fast(py_tricoords, error_prefix))) {
622                                 valid = false;
623                                 break;
624                         }
625
626                         if (PySequence_Fast_GET_SIZE(py_tricoords_fast) != 3) {
627                                 Py_DECREF(py_tricoords_fast);
628                                 PyErr_Format(PyExc_ValueError,
629                                              "%s: non triangle found at index %d with length of %d",
630                                              error_prefix, i, PySequence_Fast_GET_SIZE(py_tricoords_fast));
631                                 valid = false;
632                                 break;
633                         }
634
635                         py_tricoords_fast_items = PySequence_Fast_ITEMS(py_tricoords_fast);
636
637                         for (j = 0; j < 3; j++) {
638                                 tri[j] = (unsigned int)_PyLong_AsInt(py_tricoords_fast_items[j]);
639                                 if (UNLIKELY(tri[j] >= (unsigned int)coords_len)) {
640                                         PyErr_Format(PyExc_ValueError,
641                                                      "%s: index %d must be less than %d",
642                                                      error_prefix, tri[j], coords_len);
643
644                                         /* decref below */
645                                         valid = false;
646                                         break;
647                                 }
648                         }
649
650                         Py_DECREF(py_tricoords_fast);
651                 }
652         }
653         else {
654                 /* ngon support (much more involved) */
655                 const unsigned int polys_len = (unsigned int)PySequence_Fast_GET_SIZE(py_tris_fast);
656                 struct PolyLink {
657                         struct PolyLink *next;
658                         unsigned int len;
659                         unsigned int poly[0];
660                 } *plink_first = NULL, **p_plink_prev = &plink_first, *plink = NULL;
661                 int poly_index;
662
663                 tris_len = 0;
664
665                 poly_arena = BLI_memarena_new(BLI_MEMARENA_STD_BUFSIZE, __func__);
666
667                 for (i = 0; i < polys_len; i++) {
668                         PyObject *py_tricoords = PySequence_Fast_GET_ITEM(py_tris_fast, i);
669                         PyObject *py_tricoords_fast;
670                         PyObject **py_tricoords_fast_items;
671                         unsigned int py_tricoords_len;
672                         unsigned int j;
673
674                         if (!(py_tricoords_fast = PySequence_Fast(py_tricoords, error_prefix))) {
675                                 valid = false;
676                                 break;
677                         }
678
679                         py_tricoords_len = (unsigned int)PySequence_Fast_GET_SIZE(py_tricoords_fast);
680                         py_tricoords_fast_items = PySequence_Fast_ITEMS(py_tricoords_fast);
681
682                         plink = BLI_memarena_alloc(poly_arena, sizeof(*plink) + (sizeof(int) * (size_t)py_tricoords_len));
683
684                         plink->len = (unsigned int)py_tricoords_len;
685                         *p_plink_prev = plink;
686                         p_plink_prev = &plink->next;
687
688                         for (j = 0; j < py_tricoords_len; j++) {
689                                 plink->poly[j] = (unsigned int)_PyLong_AsInt(py_tricoords_fast_items[j]);
690                                 if (UNLIKELY(plink->poly[j] >= (unsigned int)coords_len)) {
691                                         PyErr_Format(PyExc_ValueError,
692                                                      "%s: index %d must be less than %d",
693                                                      error_prefix, plink->poly[j], coords_len);
694
695                                         Py_DECREF(py_tricoords_fast);
696                                         valid = false;
697                                         break;
698                                 }
699                         }
700
701                         if (py_tricoords_len >= 3) {
702                                 tris_len += (py_tricoords_len - 2);
703                         }
704                 }
705                 *p_plink_prev = NULL;
706
707                 /* all ngon's are parsed, now tessellate */
708
709                 pf_arena = BLI_memarena_new(BLI_POLYFILL_ARENA_SIZE, __func__);
710                 tris = MEM_mallocN(sizeof(*tris) * (size_t)tris_len, __func__);
711
712                 orig_index = MEM_mallocN(sizeof(*orig_index) * (size_t)tris_len, __func__);
713                 orig_normal = MEM_mallocN(sizeof(*orig_normal) * (size_t)polys_len, __func__);
714
715                 for (plink = plink_first, poly_index = 0, i = 0; plink; plink = plink->next, poly_index++) {
716                         if (plink->len == 3) {
717                                 unsigned int *tri = tris[i];
718                                 memcpy(tri, plink->poly, sizeof(unsigned int[3]));
719                                 orig_index[i] = poly_index;
720                                 normal_tri_v3(orig_normal[poly_index], coords[tri[0]], coords[tri[1]], coords[tri[2]]);
721                                 i++;
722                         }
723                         else if (plink->len > 3) {
724                                 float (*proj_coords)[2] = BLI_memarena_alloc(pf_arena, sizeof(*proj_coords) * plink->len);
725                                 float *normal = orig_normal[poly_index];
726                                 const float *co_prev;
727                                 const float *co_curr;
728                                 float axis_mat[3][3];
729                                 unsigned int (*tris_offset)[3] = &tris[i];
730                                 unsigned int j;
731
732                                 /* calc normal and setup 'proj_coords' */
733                                 zero_v3(normal);
734                                 co_prev = coords[plink->poly[plink->len - 1]];
735                                 for (j = 0; j < plink->len; j++) {
736                                         co_curr = coords[plink->poly[j]];
737                                         add_newell_cross_v3_v3v3(normal, co_prev, co_curr);
738                                         co_prev = co_curr;
739                                 }
740                                 normalize_v3(normal);
741
742                                 axis_dominant_v3_to_m3_negate(axis_mat, normal);
743
744                                 for (j = 0; j < plink->len; j++) {
745                                         mul_v2_m3v3(proj_coords[i], axis_mat, coords[plink->poly[j]]);
746                                 }
747
748                                 BLI_polyfill_calc_arena((const float (*)[2])proj_coords, plink->len, 1, tris_offset, pf_arena);
749
750                                 j = plink->len - 2;
751                                 while (j--) {
752                                         unsigned int *tri = tris_offset[j];
753                                         /* remap to global indices */
754                                         tri[0] = plink->poly[tri[0]];
755                                         tri[1] = plink->poly[tri[1]];
756                                         tri[2] = plink->poly[tri[2]];
757
758                                         orig_index[i] = poly_index;
759                                         i++;
760                                 }
761
762                                 BLI_memarena_clear(pf_arena);
763                         }
764                         else {
765                                 zero_v3(orig_normal[poly_index]);
766                         }
767                 }
768         }
769
770         Py_DECREF(py_coords_fast);
771         Py_DECREF(py_tris_fast);
772
773         if (pf_arena) {
774                 BLI_memarena_free(pf_arena);
775         }
776
777         if (poly_arena) {
778                 BLI_memarena_free(poly_arena);
779         }
780
781         if (valid) {
782                 BVHTree *tree;
783
784                 tree = BLI_bvhtree_new((int)tris_len, epsilon, PY_BVH_TREE_TYPE_DEFAULT, PY_BVH_AXIS_DEFAULT);
785                 if (tree) {
786                         for (i = 0; i < tris_len; i++) {
787                                 float co[3][3];
788
789                                 copy_v3_v3(co[0], coords[tris[i][0]]);
790                                 copy_v3_v3(co[1], coords[tris[i][1]]);
791                                 copy_v3_v3(co[2], coords[tris[i][2]]);
792
793                                 BLI_bvhtree_insert(tree, (int)i, co[0], 3);
794                         }
795
796                         BLI_bvhtree_balance(tree);
797                 }
798
799                 return bvhtree_CreatePyObject(
800                         tree, epsilon,
801                         coords, coords_len,
802                         tris, tris_len,
803                         orig_index, orig_normal);
804         }
805         else {
806                 if (coords)
807                         MEM_freeN(coords);
808                 if (tris)
809                         MEM_freeN(tris);
810
811                 return NULL;
812         }
813 }
814
815
816 #ifndef MATH_STANDALONE
817
818 PyDoc_STRVAR(C_BVHTree_FromBMesh_doc,
819 ".. classmethod:: FromBMesh(bmesh)\n"
820 "\n"
821 "   BVH tree based on :class:`BMesh` data.\n"
822 "\n"
823 "   :arg bmesh: BMesh data.\n"
824 "   :type bmesh: :class:`BMesh`\n"
825 );
826 static PyObject *C_BVHTree_FromBMesh(PyObject *UNUSED(cls), PyObject *args, PyObject *kwargs)
827 {
828         const char *keywords[] = {"bmesh", NULL};
829
830         BPy_BMesh *py_bm;
831
832         float (*coords)[3] = NULL;
833         unsigned int (*tris)[3] = NULL;
834         unsigned int coords_len, tris_len;
835         float epsilon = 0.0f;
836
837         BMesh *bm;
838         BMLoop *(*looptris)[3];
839
840         if (!PyArg_ParseTupleAndKeywords(
841                 args, kwargs, (char *)"O!|$f:BVHTree.FromBMesh", (char **)keywords,
842                 &BPy_BMesh_Type, &py_bm, &epsilon))
843         {
844                 return NULL;
845         }
846
847         bm = py_bm->bm;
848
849         /* Get data for tessellation */
850         {
851                 int tris_len_dummy;
852
853                 coords_len = (unsigned int)bm->totvert;
854                 tris_len = (unsigned int)poly_to_tri_count(bm->totface, bm->totloop);
855
856                 coords = MEM_mallocN(sizeof(*coords) * (size_t)coords_len, __func__);
857                 tris = MEM_mallocN(sizeof(*tris) * (size_t)tris_len, __func__);
858
859                 looptris = MEM_mallocN(sizeof(*looptris) * (size_t)tris_len, __func__);
860
861                 BM_bmesh_calc_tessellation(bm, looptris, &tris_len_dummy);
862                 BLI_assert(tris_len_dummy == (int)tris_len);
863         }
864
865         {
866                 BMIter iter;
867                 BVHTree *tree;
868                 unsigned int i;
869
870                 int *orig_index = NULL;
871                 float (*orig_normal)[3] = NULL;
872
873                 tree = BLI_bvhtree_new((int)tris_len, epsilon, PY_BVH_TREE_TYPE_DEFAULT, PY_BVH_AXIS_DEFAULT);
874                 if (tree) {
875                         BMFace *f;
876                         BMVert *v;
877
878                         orig_index = MEM_mallocN(sizeof(*orig_index) * (size_t)tris_len, __func__);
879                         orig_normal = MEM_mallocN(sizeof(*orig_normal) * (size_t)bm->totface, __func__);
880
881                         BM_ITER_MESH_INDEX (v, &iter, bm, BM_VERTS_OF_MESH, i) {
882                                 copy_v3_v3(coords[i], v->co);
883                                 BM_elem_index_set(v, (int)i);  /* set_inline */
884                         }
885                         BM_ITER_MESH_INDEX (f, &iter, bm, BM_FACES_OF_MESH, i) {
886                                 copy_v3_v3(orig_normal[i], f->no);
887                                 BM_elem_index_set(f, (int)i);  /* set_inline */
888                         }
889                         bm->elem_index_dirty &= (char)~(BM_VERT | BM_FACE);
890
891                         for (i = 0; i < tris_len; i++) {
892                                 float co[3][3];
893
894                                 tris[i][0] = (unsigned int)BM_elem_index_get(looptris[i][0]->v);
895                                 tris[i][1] = (unsigned int)BM_elem_index_get(looptris[i][1]->v);
896                                 tris[i][2] = (unsigned int)BM_elem_index_get(looptris[i][2]->v);
897
898                                 copy_v3_v3(co[0], coords[tris[i][0]]);
899                                 copy_v3_v3(co[1], coords[tris[i][1]]);
900                                 copy_v3_v3(co[2], coords[tris[i][2]]);
901
902                                 BLI_bvhtree_insert(tree, (int)i, co[0], 3);
903                                 orig_index[i] = BM_elem_index_get(looptris[i][0]->f);
904                         }
905
906                         BLI_bvhtree_balance(tree);
907                 }
908
909                 MEM_freeN(looptris);
910
911                 return bvhtree_CreatePyObject(
912                         tree, epsilon,
913                         coords, coords_len,
914                         tris, tris_len,
915                         orig_index, orig_normal);
916         }
917 }
918
919 /* return various derived meshes based on requested settings */
920 static DerivedMesh *bvh_get_derived_mesh(
921         const char *funcname, struct Scene *scene, Object *ob,
922         bool use_deform, bool use_render, bool use_cage)
923 {
924         /* we only need minimum mesh data for topology and vertex locations */
925         CustomDataMask mask = CD_MASK_BAREMESH;
926
927         /* Write the display mesh into the dummy mesh */
928         if (use_deform) {
929                 if (use_render) {
930                         if (use_cage) {
931                                 PyErr_Format(PyExc_ValueError,
932                                              "%s(...): cage arg is unsupported when (render=True)", funcname);
933                                 return NULL;
934                         }
935                         else {
936                                 return mesh_create_derived_render(scene, ob, mask);
937                         }
938                 }
939                 else {
940                         if (use_cage) {
941                                 return mesh_get_derived_deform(scene, ob, mask);  /* ob->derivedDeform */
942                         }
943                         else {
944                                 return mesh_get_derived_final(scene, ob, mask);  /* ob->derivedFinal */
945                         }
946                 }
947         }
948         else {
949                 /* !use_deform */
950                 if (use_render) {
951                         if (use_cage) {
952                                 PyErr_Format(PyExc_ValueError,
953                                              "%s(...): cage arg is unsupported when (render=True)", funcname);
954                                 return NULL;
955                         }
956                         else {
957                                 return mesh_create_derived_no_deform_render(scene, ob, NULL, mask);
958                         }
959                 }
960                 else {
961                         if (use_cage) {
962                                 PyErr_Format(PyExc_ValueError,
963                                              "%s(...): cage arg is unsupported when (deform=False, render=False)", funcname);
964                                 return NULL;
965                         }
966                         else {
967                                 return mesh_create_derived_no_deform(scene, ob, NULL, mask);
968                         }
969                 }
970         }
971 }
972
973 PyDoc_STRVAR(C_BVHTree_FromObject_doc,
974 ".. classmethod:: FromObject(object, scene, deform=True, render=False, cage=False, epsilon=0.0)\n"
975 "\n"
976 "   BVH tree based on :class:`Object` data.\n"
977 "\n"
978 "   :arg object: Object data.\n"
979 "   :type object: :class:`Object`\n"
980 "   :arg scene: Scene data to use for evaluating the mesh.\n"
981 "   :type scene: :class:`Scene`\n"
982 "   :arg deform: Use mesh with deformations.\n"
983 "   :type deform: bool\n"
984 "   :arg render: Use render settings.\n"
985 "   :type render: bool\n"
986 "   :arg cage: Use render settings.\n"
987 "   :type cage: bool\n"
988 PYBVH_FROM_GENERIC_EPSILON_DOC
989 );
990 static PyObject *C_BVHTree_FromObject(PyObject *UNUSED(cls), PyObject *args, PyObject *kwargs)
991 {
992         /* note, options here match 'bpy_bmesh_from_object' */
993         const char *keywords[] = {"object", "scene",  "deform", "render", "cage", "epsilon", NULL};
994
995         PyObject *py_ob, *py_scene;
996         Object *ob;
997         struct Scene *scene;
998         DerivedMesh *dm;
999         int use_deform = true;
1000         int use_render = false;
1001         int use_cage = false;
1002
1003         const MLoopTri *lt;
1004         const MLoop *mloop;
1005
1006         float (*coords)[3] = NULL;
1007         unsigned int (*tris)[3] = NULL;
1008         unsigned int coords_len, tris_len;
1009         float epsilon = 0.0f;
1010
1011         if (!PyArg_ParseTupleAndKeywords(
1012                 args, kwargs, (char *)"OO|$iiif:BVHTree.FromObject", (char **)keywords,
1013                 &py_ob, &py_scene, &use_deform, &use_render, &use_cage, &epsilon) ||
1014             ((ob = PyC_RNA_AsPointer(py_ob, "Object")) == NULL) ||
1015             ((scene = PyC_RNA_AsPointer(py_scene, "Scene")) == NULL))
1016         {
1017                 return NULL;
1018         }
1019
1020         dm = bvh_get_derived_mesh("BVHTree", scene, ob, use_deform, use_render, use_cage);
1021         if (dm == NULL) {
1022                 return NULL;
1023         }
1024
1025         /* Get data for tessellation */
1026         {
1027                 DM_ensure_looptri(dm);
1028                 lt = dm->getLoopTriArray(dm);
1029
1030                 tris_len = (unsigned int)dm->getNumLoopTri(dm);
1031                 coords_len = (unsigned int)dm->getNumVerts(dm);
1032
1033                 coords = MEM_mallocN(sizeof(*coords) * (size_t)coords_len, __func__);
1034                 tris = MEM_mallocN(sizeof(*tris) * (size_t)tris_len, __func__);
1035
1036                 dm->getVertCos(dm, coords);
1037
1038                 mloop = dm->getLoopArray(dm);
1039         }
1040
1041         {
1042                 BVHTree *tree;
1043                 unsigned int i;
1044
1045                 int *orig_index = NULL;
1046                 float (*orig_normal)[3] = NULL;
1047
1048                 tree = BLI_bvhtree_new((int)tris_len, epsilon, PY_BVH_TREE_TYPE_DEFAULT, PY_BVH_AXIS_DEFAULT);
1049                 if (tree) {
1050                         orig_index = MEM_mallocN(sizeof(*orig_index) * (size_t)tris_len, __func__);
1051                         orig_normal = dm->getPolyDataArray(dm, CD_NORMAL);  /* can be NULL */
1052                         if (orig_normal) {
1053                                 orig_normal = MEM_dupallocN(orig_normal);
1054                         }
1055
1056                         for (i = 0; i < tris_len; i++, lt++) {
1057                                 float co[3][3];
1058
1059                                 tris[i][0] = mloop[lt->tri[0]].v;
1060                                 tris[i][1] = mloop[lt->tri[1]].v;
1061                                 tris[i][2] = mloop[lt->tri[2]].v;
1062
1063                                 copy_v3_v3(co[0], coords[tris[i][0]]);
1064                                 copy_v3_v3(co[1], coords[tris[i][1]]);
1065                                 copy_v3_v3(co[2], coords[tris[i][2]]);
1066
1067                                 BLI_bvhtree_insert(tree, (int)i, co[0], 3);
1068                                 orig_index[i] = (int)lt->poly;
1069                         }
1070
1071                         BLI_bvhtree_balance(tree);
1072                 }
1073
1074                 dm->release(dm);
1075
1076                 return bvhtree_CreatePyObject(
1077                         tree, epsilon,
1078                         coords, coords_len,
1079                         tris, tris_len,
1080                         orig_index, orig_normal);
1081         }
1082 }
1083 #endif  /* MATH_STANDALONE */
1084
1085 /** \} */
1086
1087
1088 /* -------------------------------------------------------------------- */
1089
1090 /** \name Module & Type definition
1091  * \{ */
1092
1093 static PyMethodDef py_bvhtree_methods[] = {
1094         {"ray_cast", (PyCFunction)py_bvhtree_ray_cast, METH_VARARGS, py_bvhtree_ray_cast_doc},
1095         {"find", (PyCFunction)py_bvhtree_find_nearest, METH_VARARGS, py_bvhtree_find_nearest_doc},
1096         {"overlap", (PyCFunction)py_bvhtree_overlap, METH_O, py_bvhtree_overlap_doc},
1097
1098         /* class methods */
1099         {"FromPolygons", (PyCFunction) C_BVHTree_FromPolygons, METH_VARARGS | METH_KEYWORDS | METH_CLASS, C_BVHTree_FromPolygons_doc},
1100 #ifndef MATH_STANDALONE
1101         {"FromBMesh", (PyCFunction) C_BVHTree_FromBMesh, METH_VARARGS | METH_KEYWORDS | METH_CLASS, C_BVHTree_FromBMesh_doc},
1102         {"FromObject", (PyCFunction) C_BVHTree_FromObject, METH_VARARGS | METH_KEYWORDS | METH_CLASS, C_BVHTree_FromObject_doc},
1103 #endif
1104         {NULL, NULL, 0, NULL}
1105 };
1106
1107 PyTypeObject PyBVHTree_Type = {
1108         PyVarObject_HEAD_INIT(NULL, 0)
1109         "BVHTree",                                   /* tp_name */
1110         sizeof(PyBVHTree),                           /* tp_basicsize */
1111         0,                                           /* tp_itemsize */
1112         /* methods */
1113         (destructor)py_bvhtree__tp_dealloc,          /* tp_dealloc */
1114         NULL,                                        /* tp_print */
1115         NULL,                                        /* tp_getattr */
1116         NULL,                                        /* tp_setattr */
1117         NULL,                                        /* tp_compare */
1118         NULL,                                        /* tp_repr */
1119         NULL,                                        /* tp_as_number */
1120         NULL,                                        /* tp_as_sequence */
1121         NULL,                                        /* tp_as_mapping */
1122         NULL,                                        /* tp_hash */
1123         NULL,                                        /* tp_call */
1124         NULL,                                        /* tp_str */
1125         NULL,                                        /* tp_getattro */
1126         NULL,                                        /* tp_setattro */
1127         NULL,                                        /* tp_as_buffer */
1128         Py_TPFLAGS_DEFAULT,                          /* tp_flags */
1129         NULL,                                        /* Documentation string */
1130         NULL,                                        /* tp_traverse */
1131         NULL,                                        /* tp_clear */
1132         NULL,                                        /* tp_richcompare */
1133         0,                                           /* tp_weaklistoffset */
1134         NULL,                                        /* tp_iter */
1135         NULL,                                        /* tp_iternext */
1136         py_bvhtree_methods,                          /* tp_methods */
1137         NULL,                                        /* tp_members */
1138         NULL,                                        /* tp_getset */
1139         NULL,                                        /* tp_base */
1140         NULL,                                        /* tp_dict */
1141         NULL,                                        /* tp_descr_get */
1142         NULL,                                        /* tp_descr_set */
1143         0,                                           /* tp_dictoffset */
1144         NULL,                                        /* tp_init */
1145         (allocfunc)PyType_GenericAlloc,              /* tp_alloc */
1146         (newfunc)PyType_GenericNew,                  /* tp_new */
1147         (freefunc)0,                                 /* tp_free */
1148         NULL,                                        /* tp_is_gc */
1149         NULL,                                        /* tp_bases */
1150         NULL,                                        /* tp_mro */
1151         NULL,                                        /* tp_cache */
1152         NULL,                                        /* tp_subclasses */
1153         NULL,                                        /* tp_weaklist */
1154         (destructor) NULL                            /* tp_del */
1155 };
1156
1157 /* -------------------------------------------------------------------- */
1158 /* Module definition */
1159
1160 PyDoc_STRVAR(py_bvhtree_doc,
1161 "BVH tree structures for proximity searches and ray casts on geometry."
1162 );
1163 static struct PyModuleDef bvhtree_moduledef = {
1164         PyModuleDef_HEAD_INIT,
1165         "mathutils.bvhtree",                         /* m_name */
1166         py_bvhtree_doc,                              /* m_doc */
1167         0,                                           /* m_size */
1168         NULL,                                        /* m_methods */
1169         NULL,                                        /* m_reload */
1170         NULL,                                        /* m_traverse */
1171         NULL,                                        /* m_clear */
1172         NULL                                         /* m_free */
1173 };
1174
1175 PyMODINIT_FUNC PyInit_mathutils_bvhtree(void)
1176 {
1177         PyObject *m = PyModule_Create(&bvhtree_moduledef);
1178
1179         if (m == NULL) {
1180                 return NULL;
1181         }
1182
1183         /* Register classes */
1184         if (PyType_Ready(&PyBVHTree_Type) < 0) {
1185                 return NULL;
1186         }
1187
1188         PyModule_AddObject(m, "BVHTree", (PyObject *)&PyBVHTree_Type);
1189
1190         return m;
1191 }
1192
1193 /** \} */