c7511dfc80f48fdd361eb6a98c2bbad5eded081a
[blender.git] / source / blender / editors / sculpt_paint / sculpt_pose.c
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  *
16  * The Original Code is Copyright (C) 2020 Blender Foundation.
17  * All rights reserved.
18  */
19
20 /** \file
21  * \ingroup edsculpt
22  */
23
24 #include "MEM_guardedalloc.h"
25
26 #include "BLI_blenlib.h"
27 #include "BLI_math.h"
28 #include "BLI_task.h"
29
30 #include "DNA_brush_types.h"
31 #include "DNA_mesh_types.h"
32 #include "DNA_meshdata_types.h"
33 #include "DNA_object_types.h"
34
35 #include "BKE_brush.h"
36 #include "BKE_ccg.h"
37 #include "BKE_colortools.h"
38 #include "BKE_context.h"
39 #include "BKE_mesh.h"
40 #include "BKE_multires.h"
41 #include "BKE_node.h"
42 #include "BKE_object.h"
43 #include "BKE_paint.h"
44 #include "BKE_pbvh.h"
45 #include "BKE_scene.h"
46
47 #include "paint_intern.h"
48 #include "sculpt_intern.h"
49
50 #include "bmesh.h"
51
52 #include <math.h>
53 #include <stdlib.h>
54
55 static void pose_solve_ik_chain(SculptPoseIKChain *ik_chain,
56                                 const float initial_target[3],
57                                 const bool use_anchor)
58 {
59   SculptPoseIKChainSegment *segments = ik_chain->segments;
60   int tot_segments = ik_chain->tot_segments;
61
62   float target[3];
63
64   /* Set the initial target. */
65   copy_v3_v3(target, initial_target);
66
67   /* Solve the positions and rotations of all segments in the chain. */
68   for (int i = 0; i < tot_segments; i++) {
69     float initial_orientation[3];
70     float current_orientation[3];
71     float current_head_position[3];
72     float current_origin_position[3];
73
74     /* Calculate the rotation to orientate the segment to the target from its initial state. */
75     sub_v3_v3v3(current_orientation, target, segments[i].orig);
76     normalize_v3(current_orientation);
77     sub_v3_v3v3(initial_orientation, segments[i].initial_head, segments[i].initial_orig);
78     normalize_v3(initial_orientation);
79     rotation_between_vecs_to_quat(segments[i].rot, initial_orientation, current_orientation);
80
81     /* Rotate the segment by calculating a new head position. */
82     madd_v3_v3v3fl(current_head_position, segments[i].orig, current_orientation, segments[i].len);
83
84     /* Move the origin of the segment towards the target. */
85     sub_v3_v3v3(current_origin_position, target, current_head_position);
86
87     /* Store the new head and origin positions to the segment. */
88     copy_v3_v3(segments[i].head, current_head_position);
89     add_v3_v3(segments[i].orig, current_origin_position);
90
91     /* Use the origin of this segment as target for the next segment in the chain. */
92     copy_v3_v3(target, segments[i].orig);
93   }
94
95   /* Move back the whole chain to preserve the anchor point. */
96   if (use_anchor) {
97     float anchor_diff[3];
98     sub_v3_v3v3(
99         anchor_diff, segments[tot_segments - 1].initial_orig, segments[tot_segments - 1].orig);
100
101     for (int i = 0; i < tot_segments; i++) {
102       add_v3_v3(segments[i].orig, anchor_diff);
103       add_v3_v3(segments[i].head, anchor_diff);
104     }
105   }
106 }
107
108 static void pose_solve_roll_chain(SculptPoseIKChain *ik_chain,
109                                   const Brush *brush,
110                                   const float roll)
111 {
112   SculptPoseIKChainSegment *segments = ik_chain->segments;
113   int tot_segments = ik_chain->tot_segments;
114
115   for (int i = 0; i < tot_segments; i++) {
116     float initial_orientation[3];
117     float initial_rotation[4];
118     float current_rotation[4];
119
120     sub_v3_v3v3(initial_orientation, segments[i].initial_head, segments[i].initial_orig);
121     normalize_v3(initial_orientation);
122
123     /* Calculate the current roll angle using the brush curve. */
124     float current_roll = roll * BKE_brush_curve_strength(brush, i, tot_segments);
125
126     axis_angle_normalized_to_quat(initial_rotation, initial_orientation, 0.0f);
127     axis_angle_normalized_to_quat(current_rotation, initial_orientation, current_roll);
128
129     /* Store the difference of the rotations in the segment rotation. */
130     rotation_between_quats_to_quat(segments[i].rot, current_rotation, initial_rotation);
131   }
132 }
133
134 static void do_pose_brush_task_cb_ex(void *__restrict userdata,
135                                      const int n,
136                                      const TaskParallelTLS *__restrict UNUSED(tls))
137 {
138   SculptThreadedTaskData *data = userdata;
139   SculptSession *ss = data->ob->sculpt;
140   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
141   SculptPoseIKChainSegment *segments = ik_chain->segments;
142
143   PBVHVertexIter vd;
144   float disp[3], new_co[3];
145   float final_pos[3];
146
147   SculptOrigVertData orig_data;
148   SCULPT_orig_vert_data_init(&orig_data, data->ob, data->nodes[n]);
149
150   BKE_pbvh_vertex_iter_begin(ss->pbvh, data->nodes[n], vd, PBVH_ITER_UNIQUE)
151   {
152     SCULPT_orig_vert_data_update(&orig_data, &vd);
153
154     float total_disp[3];
155     zero_v3(total_disp);
156
157     ePaintSymmetryAreas symm_area = SCULPT_get_vertex_symm_area(orig_data.co);
158
159     /* Calculate the displacement of each vertex for all the segments in the chain. */
160     for (int ik = 0; ik < ik_chain->tot_segments; ik++) {
161       copy_v3_v3(new_co, orig_data.co);
162
163       /* Get the transform matrix for the vertex symmetry area to calculate a displacement in the
164        * vertex. */
165       mul_m4_v3(segments[ik].pivot_mat_inv[(int)symm_area], new_co);
166       mul_m4_v3(segments[ik].trans_mat[(int)symm_area], new_co);
167       mul_m4_v3(segments[ik].pivot_mat[(int)symm_area], new_co);
168
169       /* Apply the segment weight of the vertex to the displacement. */
170       sub_v3_v3v3(disp, new_co, orig_data.co);
171       mul_v3_fl(disp, segments[ik].weights[vd.index]);
172
173       /* Apply the vertex mask to the displacement. */
174       float mask = vd.mask ? *vd.mask : 0.0f;
175       mul_v3_fl(disp, 1.0f - mask);
176
177       /* Accumulate the displacement. */
178       add_v3_v3(total_disp, disp);
179     }
180
181     /* Apply the accumulated displacement to the vertex. */
182     add_v3_v3v3(final_pos, orig_data.co, total_disp);
183     copy_v3_v3(vd.co, final_pos);
184
185     if (vd.mvert) {
186       vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
187     }
188   }
189   BKE_pbvh_vertex_iter_end;
190 }
191
192 typedef struct PoseGrowFactorTLSData {
193   float pos_avg[3];
194   int pos_count;
195 } PoseGrowFactorTLSData;
196
197 static void pose_brush_grow_factor_task_cb_ex(void *__restrict userdata,
198                                               const int n,
199                                               const TaskParallelTLS *__restrict tls)
200 {
201   SculptThreadedTaskData *data = userdata;
202   PoseGrowFactorTLSData *gftd = tls->userdata_chunk;
203   SculptSession *ss = data->ob->sculpt;
204   const char symm = data->sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL;
205   PBVHVertexIter vd;
206   BKE_pbvh_vertex_iter_begin(ss->pbvh, data->nodes[n], vd, PBVH_ITER_UNIQUE)
207   {
208     SculptVertexNeighborIter ni;
209     float max = 0.0f;
210
211     /* Grow the factor. */
212     SCULPT_VERTEX_NEIGHBORS_ITER_BEGIN (ss, vd.index, ni) {
213       float vmask_f = data->prev_mask[ni.index];
214       max = MAX2(vmask_f, max);
215     }
216     SCULPT_VERTEX_NEIGHBORS_ITER_END(ni);
217
218     /* Keep the count of the vertices that where added to the factors in this grow iteration. */
219     if (max > data->prev_mask[vd.index]) {
220       data->pose_factor[vd.index] = max;
221       if (SCULPT_check_vertex_pivot_symmetry(vd.co, data->pose_initial_co, symm)) {
222         add_v3_v3(gftd->pos_avg, vd.co);
223         gftd->pos_count++;
224       }
225     }
226   }
227
228   BKE_pbvh_vertex_iter_end;
229 }
230
231 static void pose_brush_grow_factor_reduce(const void *__restrict UNUSED(userdata),
232                                           void *__restrict chunk_join,
233                                           void *__restrict chunk)
234 {
235   PoseGrowFactorTLSData *join = chunk_join;
236   PoseGrowFactorTLSData *gftd = chunk;
237   add_v3_v3(join->pos_avg, gftd->pos_avg);
238   join->pos_count += gftd->pos_count;
239 }
240
241 /* Grow the factor until its boundary is near to the offset pose origin or outside the target
242  * distance. */
243 static void sculpt_pose_grow_pose_factor(Sculpt *sd,
244                                          Object *ob,
245                                          SculptSession *ss,
246                                          float pose_origin[3],
247                                          float pose_target[3],
248                                          float max_len,
249                                          float *r_pose_origin,
250                                          float *pose_factor)
251 {
252   PBVHNode **nodes;
253   PBVH *pbvh = ob->sculpt->pbvh;
254   int totnode;
255
256   BKE_pbvh_search_gather(pbvh, NULL, NULL, &nodes, &totnode);
257   SculptThreadedTaskData data = {
258       .sd = sd,
259       .ob = ob,
260       .nodes = nodes,
261       .totnode = totnode,
262       .pose_factor = pose_factor,
263   };
264
265   data.pose_initial_co = pose_target;
266   TaskParallelSettings settings;
267   PoseGrowFactorTLSData gftd;
268   gftd.pos_count = 0;
269   zero_v3(gftd.pos_avg);
270   BKE_pbvh_parallel_range_settings(&settings, (sd->flags & SCULPT_USE_OPENMP), totnode);
271   settings.func_reduce = pose_brush_grow_factor_reduce;
272   settings.userdata_chunk = &gftd;
273   settings.userdata_chunk_size = sizeof(PoseGrowFactorTLSData);
274
275   bool grow_next_iteration = true;
276   float prev_len = FLT_MAX;
277   data.prev_mask = MEM_mallocN(SCULPT_vertex_count_get(ss) * sizeof(float), "prev mask");
278   while (grow_next_iteration) {
279     zero_v3(gftd.pos_avg);
280     gftd.pos_count = 0;
281     memcpy(data.prev_mask, pose_factor, SCULPT_vertex_count_get(ss) * sizeof(float));
282     BLI_task_parallel_range(0, totnode, &data, pose_brush_grow_factor_task_cb_ex, &settings);
283
284     if (gftd.pos_count != 0) {
285       mul_v3_fl(gftd.pos_avg, 1.0f / (float)gftd.pos_count);
286       if (pose_origin) {
287         /* Test with pose origin. Used when growing the factors to compensate the Origin Offset. */
288         /* Stop when the factor's avg_pos starts moving away from the origin instead of getting
289          * closer to it. */
290         float len = len_v3v3(gftd.pos_avg, pose_origin);
291         if (len < prev_len) {
292           prev_len = len;
293           grow_next_iteration = true;
294         }
295         else {
296           grow_next_iteration = false;
297           memcpy(pose_factor, data.prev_mask, SCULPT_vertex_count_get(ss) * sizeof(float));
298         }
299       }
300       else {
301         /* Test with length. Used to calculate the origin positions of the IK chain. */
302         /* Stops when the factors have grown enough to generate a new segment origin. */
303         float len = len_v3v3(gftd.pos_avg, pose_target);
304         if (len < max_len) {
305           prev_len = len;
306           grow_next_iteration = true;
307         }
308         else {
309           grow_next_iteration = false;
310           if (r_pose_origin) {
311             copy_v3_v3(r_pose_origin, gftd.pos_avg);
312           }
313           memcpy(pose_factor, data.prev_mask, SCULPT_vertex_count_get(ss) * sizeof(float));
314         }
315       }
316     }
317     else {
318       if (r_pose_origin) {
319         copy_v3_v3(r_pose_origin, pose_target);
320       }
321       grow_next_iteration = false;
322     }
323   }
324   MEM_freeN(data.prev_mask);
325
326   MEM_SAFE_FREE(nodes);
327 }
328
329 static bool sculpt_pose_brush_is_vertex_inside_brush_radius(const float vertex[3],
330                                                             const float br_co[3],
331                                                             float radius,
332                                                             char symm)
333 {
334   for (char i = 0; i <= symm; ++i) {
335     if (SCULPT_is_symmetry_iteration_valid(i, symm)) {
336       float location[3];
337       flip_v3_v3(location, br_co, (char)i);
338       if (len_v3v3(location, vertex) < radius) {
339         return true;
340       }
341     }
342   }
343   return false;
344 }
345
346 typedef struct PoseFloodFillData {
347   float pose_initial_co[3];
348   float radius;
349   int symm;
350
351   float *pose_factor;
352   float pose_origin[3];
353   int tot_co;
354
355   int current_face_set;
356   int next_face_set;
357   int prev_face_set;
358   int next_vertex;
359
360   bool next_face_set_found;
361
362   /* Store the visited face sets to avoid going back when calculating the chain. */
363   GSet *visited_face_sets;
364
365   /* In face sets origin mode, each vertex can only be assigned to one face set. */
366   bool *is_weighted;
367
368   bool is_first_iteration;
369
370   /* Fallback origin. If we can't find any face set to continue, use the position of all vertices
371    * that have the current face set. */
372   float fallback_origin[3];
373   int fallback_count;
374 } PoseFloodFillData;
375
376 static bool pose_topology_floodfill_cb(
377     SculptSession *ss, int UNUSED(from_v), int to_v, bool is_duplicate, void *userdata)
378 {
379   PoseFloodFillData *data = userdata;
380
381   if (data->pose_factor) {
382     data->pose_factor[to_v] = 1.0f;
383   }
384
385   const float *co = SCULPT_vertex_co_get(ss, to_v);
386   if (sculpt_pose_brush_is_vertex_inside_brush_radius(
387           co, data->pose_initial_co, data->radius, data->symm)) {
388     return true;
389   }
390   else if (SCULPT_check_vertex_pivot_symmetry(co, data->pose_initial_co, data->symm)) {
391     if (!is_duplicate) {
392       add_v3_v3(data->pose_origin, co);
393       data->tot_co++;
394     }
395   }
396
397   return false;
398 }
399
400 static bool pose_face_sets_floodfill_cb(
401     SculptSession *ss, int UNUSED(from_v), int to_v, bool is_duplicate, void *userdata)
402 {
403   PoseFloodFillData *data = userdata;
404
405   const int index = to_v;
406   bool visit_next = false;
407
408   const float *co = SCULPT_vertex_co_get(ss, index);
409   const bool symmetry_check = SCULPT_check_vertex_pivot_symmetry(
410                                   co, data->pose_initial_co, data->symm) &&
411                               !is_duplicate;
412
413   /* First iteration. Continue expanding using topology until a vertex is outside the brush radius
414    * to determine the first face set. */
415   if (data->current_face_set == SCULPT_FACE_SET_NONE) {
416
417     data->pose_factor[index] = 1.0f;
418     data->is_weighted[index] = true;
419
420     if (sculpt_pose_brush_is_vertex_inside_brush_radius(
421             co, data->pose_initial_co, data->radius, data->symm)) {
422       const int visited_face_set = SCULPT_vertex_face_set_get(ss, index);
423       BLI_gset_add(data->visited_face_sets, POINTER_FROM_INT(visited_face_set));
424     }
425     else if (symmetry_check) {
426       data->current_face_set = SCULPT_vertex_face_set_get(ss, index);
427       BLI_gset_add(data->visited_face_sets, POINTER_FROM_INT(data->current_face_set));
428     }
429     return true;
430   }
431
432   /* We already have a current face set, so we can start checking the face sets of the vertices. */
433   /* In the first iteration we need to check all face sets we already visited as the flood fill may
434    * still not be finished in some of them. */
435   bool is_vertex_valid = false;
436   if (data->is_first_iteration) {
437     GSetIterator gs_iter;
438     GSET_ITER (gs_iter, data->visited_face_sets) {
439       const int visited_face_set = POINTER_AS_INT(BLI_gsetIterator_getKey(&gs_iter));
440       is_vertex_valid |= SCULPT_vertex_has_face_set(ss, index, visited_face_set);
441     }
442   }
443   else {
444     is_vertex_valid = SCULPT_vertex_has_face_set(ss, index, data->current_face_set);
445   }
446
447   if (is_vertex_valid) {
448
449     if (!data->is_weighted[index]) {
450       data->pose_factor[index] = 1.0f;
451       data->is_weighted[index] = true;
452       visit_next = true;
453     }
454
455     /* Fallback origin accumulation. */
456     if (symmetry_check) {
457       add_v3_v3(data->fallback_origin, SCULPT_vertex_co_get(ss, index));
458       data->fallback_count++;
459     }
460
461     if (symmetry_check && !SCULPT_vertex_has_unique_face_set(ss, index)) {
462
463       /* We only add coordinates for calculating the origin when it is possible to go from this
464        * vertex to another vertex in a valid face set for the next iteration. */
465       bool count_as_boundary = false;
466
467       SculptVertexNeighborIter ni;
468       SCULPT_VERTEX_NEIGHBORS_ITER_BEGIN (ss, index, ni) {
469         int next_face_set_candidate = SCULPT_vertex_face_set_get(ss, ni.index);
470
471         /* Check if we can get a valid face set for the next iteration from this neighbor. */
472         if (SCULPT_vertex_has_unique_face_set(ss, ni.index) &&
473             !BLI_gset_haskey(data->visited_face_sets, POINTER_FROM_INT(next_face_set_candidate))) {
474           if (!data->next_face_set_found) {
475             data->next_face_set = next_face_set_candidate;
476             data->next_vertex = ni.index;
477             data->next_face_set_found = true;
478           }
479           count_as_boundary = true;
480         }
481       }
482       SCULPT_VERTEX_NEIGHBORS_ITER_END(ni);
483
484       /* Origin accumulation. */
485       if (count_as_boundary) {
486         add_v3_v3(data->pose_origin, SCULPT_vertex_co_get(ss, index));
487         data->tot_co++;
488       }
489     }
490   }
491   return visit_next;
492 }
493
494 /* Public functions. */
495
496 /* Calculate the pose origin and (Optionaly the pose factor) that is used when using the pose brush
497  *
498  * r_pose_origin must be a valid pointer. the r_pose_factor is optional. When set to NULL it won't
499  * be calculated. */
500 void SCULPT_pose_calc_pose_data(Sculpt *sd,
501                                 Object *ob,
502                                 SculptSession *ss,
503                                 float initial_location[3],
504                                 float radius,
505                                 float pose_offset,
506                                 float *r_pose_origin,
507                                 float *r_pose_factor)
508 {
509   SCULPT_vertex_random_access_init(ss);
510
511   /* Calculate the pose rotation point based on the boundaries of the brush factor. */
512   SculptFloodFill flood;
513   SCULPT_floodfill_init(ss, &flood);
514   SCULPT_floodfill_add_active(sd, ob, ss, &flood, (r_pose_factor) ? radius : 0.0f);
515
516   PoseFloodFillData fdata = {
517       .radius = radius,
518       .symm = sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL,
519       .pose_factor = r_pose_factor,
520       .tot_co = 0,
521   };
522   zero_v3(fdata.pose_origin);
523   copy_v3_v3(fdata.pose_initial_co, initial_location);
524   SCULPT_floodfill_execute(ss, &flood, pose_topology_floodfill_cb, &fdata);
525   SCULPT_floodfill_free(&flood);
526
527   if (fdata.tot_co > 0) {
528     mul_v3_fl(fdata.pose_origin, 1.0f / (float)fdata.tot_co);
529   }
530
531   /* Offset the pose origin. */
532   float pose_d[3];
533   sub_v3_v3v3(pose_d, fdata.pose_origin, fdata.pose_initial_co);
534   normalize_v3(pose_d);
535   madd_v3_v3fl(fdata.pose_origin, pose_d, radius * pose_offset);
536   copy_v3_v3(r_pose_origin, fdata.pose_origin);
537
538   /* Do the initial grow of the factors to get the first segment of the chain with Origin Offset.
539    */
540   if (pose_offset != 0.0f && r_pose_factor) {
541     sculpt_pose_grow_pose_factor(
542         sd, ob, ss, fdata.pose_origin, fdata.pose_origin, 0, NULL, r_pose_factor);
543   }
544 }
545
546 static void pose_brush_init_task_cb_ex(void *__restrict userdata,
547                                        const int n,
548                                        const TaskParallelTLS *__restrict UNUSED(tls))
549 {
550   SculptThreadedTaskData *data = userdata;
551   SculptSession *ss = data->ob->sculpt;
552   PBVHVertexIter vd;
553   BKE_pbvh_vertex_iter_begin(ss->pbvh, data->nodes[n], vd, PBVH_ITER_UNIQUE)
554   {
555     SculptVertexNeighborIter ni;
556     float avg = 0.0f;
557     int total = 0;
558     SCULPT_VERTEX_NEIGHBORS_ITER_BEGIN (ss, vd.index, ni) {
559       avg += data->pose_factor[ni.index];
560       total++;
561     }
562     SCULPT_VERTEX_NEIGHBORS_ITER_END(ni);
563
564     if (total > 0) {
565       data->pose_factor[vd.index] = avg / total;
566     }
567   }
568   BKE_pbvh_vertex_iter_end;
569 }
570
571 /* Init the IK chain with empty weights. */
572 static SculptPoseIKChain *pose_ik_chain_new(const int totsegments, const int totverts)
573 {
574   SculptPoseIKChain *ik_chain = MEM_callocN(sizeof(SculptPoseIKChain), "Pose IK Chain");
575   ik_chain->tot_segments = totsegments;
576   ik_chain->segments = MEM_callocN(totsegments * sizeof(SculptPoseIKChainSegment),
577                                    "Pose IK Chain Segments");
578   for (int i = 0; i < totsegments; i++) {
579     ik_chain->segments[i].weights = MEM_callocN(totverts * sizeof(float), "Pose IK weights");
580   }
581   return ik_chain;
582 }
583
584 /* Init the origin/head pairs of all the segments from the calculated origins. */
585 static void pose_ik_chain_origin_heads_init(SculptPoseIKChain *ik_chain,
586                                             const float initial_location[3])
587 {
588   float origin[3];
589   float head[3];
590   for (int i = 0; i < ik_chain->tot_segments; i++) {
591     if (i == 0) {
592       copy_v3_v3(head, initial_location);
593       copy_v3_v3(origin, ik_chain->segments[i].orig);
594     }
595     else {
596       copy_v3_v3(head, ik_chain->segments[i - 1].orig);
597       copy_v3_v3(origin, ik_chain->segments[i].orig);
598     }
599     copy_v3_v3(ik_chain->segments[i].orig, origin);
600     copy_v3_v3(ik_chain->segments[i].initial_orig, origin);
601     copy_v3_v3(ik_chain->segments[i].initial_head, head);
602     ik_chain->segments[i].len = len_v3v3(head, origin);
603   }
604 }
605
606 static SculptPoseIKChain *pose_ik_chain_init_topology(Sculpt *sd,
607                                                       Object *ob,
608                                                       SculptSession *ss,
609                                                       Brush *br,
610                                                       const float initial_location[3],
611                                                       const float radius)
612 {
613
614   const float chain_segment_len = radius * (1.0f + br->pose_offset);
615   float next_chain_segment_target[3];
616
617   int totvert = SCULPT_vertex_count_get(ss);
618   int nearest_vertex_index = SCULPT_nearest_vertex_get(sd, ob, initial_location, FLT_MAX, true);
619
620   /* Init the buffers used to keep track of the changes in the pose factors as more segments are
621    * added to the IK chain. */
622
623   /* This stores the whole pose factors values as they grow through the mesh. */
624   float *pose_factor_grow = MEM_callocN(totvert * sizeof(float), "Pose Factor Grow");
625
626   /* This stores the previous status of the factors when growing a new iteration. */
627   float *pose_factor_grow_prev = MEM_callocN(totvert * sizeof(float),
628                                              "Pose Factor Grow Prev Iteration");
629
630   pose_factor_grow[nearest_vertex_index] = 1.0f;
631
632   SculptPoseIKChain *ik_chain = pose_ik_chain_new(br->pose_ik_segments, totvert);
633
634   /* Calculate the first segment in the chain using the brush radius and the pose origin offset. */
635   copy_v3_v3(next_chain_segment_target, initial_location);
636   SCULPT_pose_calc_pose_data(sd,
637                              ob,
638                              ss,
639                              next_chain_segment_target,
640                              radius,
641                              br->pose_offset,
642                              ik_chain->segments[0].orig,
643                              pose_factor_grow);
644
645   copy_v3_v3(next_chain_segment_target, ik_chain->segments[0].orig);
646
647   /* Init the weights of this segment and store the status of the pose factors to start calculating
648    * new segment origins. */
649   for (int j = 0; j < totvert; j++) {
650     ik_chain->segments[0].weights[j] = pose_factor_grow[j];
651     pose_factor_grow_prev[j] = pose_factor_grow[j];
652   }
653
654   /* Calculate the next segments in the chain growing the pose factors. */
655   for (int i = 1; i < ik_chain->tot_segments; i++) {
656
657     /* Grow the factors to get the new segment origin. */
658     sculpt_pose_grow_pose_factor(sd,
659                                  ob,
660                                  ss,
661                                  NULL,
662                                  next_chain_segment_target,
663                                  chain_segment_len,
664                                  ik_chain->segments[i].orig,
665                                  pose_factor_grow);
666     copy_v3_v3(next_chain_segment_target, ik_chain->segments[i].orig);
667
668     /* Create the weights for this segment from the difference between the previous grow factor
669      * iteration an the current iteration. */
670     for (int j = 0; j < totvert; j++) {
671       ik_chain->segments[i].weights[j] = pose_factor_grow[j] - pose_factor_grow_prev[j];
672       /* Store the current grow factor status for the next interation. */
673       pose_factor_grow_prev[j] = pose_factor_grow[j];
674     }
675   }
676
677   pose_ik_chain_origin_heads_init(ik_chain, initial_location);
678
679   MEM_freeN(pose_factor_grow);
680   MEM_freeN(pose_factor_grow_prev);
681
682   return ik_chain;
683 }
684
685 static SculptPoseIKChain *pose_ik_chain_init_face_sets(
686     Sculpt *sd, Object *ob, SculptSession *ss, Brush *br, const float radius)
687 {
688
689   int totvert = SCULPT_vertex_count_get(ss);
690
691   SculptPoseIKChain *ik_chain = pose_ik_chain_new(br->pose_ik_segments, totvert);
692
693   GSet *visited_face_sets = BLI_gset_int_new_ex("visited_face_sets", ik_chain->tot_segments);
694
695   bool *is_weighted = MEM_callocN(sizeof(bool) * totvert, "weighted");
696
697   int current_face_set = SCULPT_FACE_SET_NONE;
698   int prev_face_set = SCULPT_FACE_SET_NONE;
699
700   int current_vertex = SCULPT_active_vertex_get(ss);
701
702   for (int s = 0; s < ik_chain->tot_segments; s++) {
703
704     SculptFloodFill flood;
705     SCULPT_floodfill_init(ss, &flood);
706     SCULPT_floodfill_add_initial_with_symmetry(sd, ob, ss, &flood, current_vertex, FLT_MAX);
707
708     BLI_gset_add(visited_face_sets, POINTER_FROM_INT(current_face_set));
709
710     PoseFloodFillData fdata = {
711         .radius = radius,
712         .symm = sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL,
713         .pose_factor = ik_chain->segments[s].weights,
714         .tot_co = 0,
715         .fallback_count = 0,
716         .current_face_set = current_face_set,
717         .prev_face_set = prev_face_set,
718         .visited_face_sets = visited_face_sets,
719         .is_weighted = is_weighted,
720         .next_face_set_found = false,
721         .is_first_iteration = s == 0,
722     };
723     zero_v3(fdata.pose_origin);
724     zero_v3(fdata.fallback_origin);
725     copy_v3_v3(fdata.pose_initial_co, SCULPT_vertex_co_get(ss, current_vertex));
726     SCULPT_floodfill_execute(ss, &flood, pose_face_sets_floodfill_cb, &fdata);
727     SCULPT_floodfill_free(&flood);
728
729     if (fdata.tot_co > 0) {
730       mul_v3_fl(fdata.pose_origin, 1.0f / (float)fdata.tot_co);
731       copy_v3_v3(ik_chain->segments[s].orig, fdata.pose_origin);
732     }
733     else if (fdata.fallback_count > 0) {
734       mul_v3_fl(fdata.fallback_origin, 1.0f / (float)fdata.fallback_count);
735       copy_v3_v3(ik_chain->segments[s].orig, fdata.fallback_origin);
736     }
737     else {
738       zero_v3(ik_chain->segments[s].orig);
739     }
740
741     prev_face_set = fdata.current_face_set;
742     current_face_set = fdata.next_face_set;
743     current_vertex = fdata.next_vertex;
744   }
745
746   BLI_gset_free(visited_face_sets, NULL);
747
748   pose_ik_chain_origin_heads_init(ik_chain, SCULPT_active_vertex_co_get(ss));
749
750   MEM_SAFE_FREE(is_weighted);
751
752   return ik_chain;
753 }
754
755 SculptPoseIKChain *SCULPT_pose_ik_chain_init(Sculpt *sd,
756                                              Object *ob,
757                                              SculptSession *ss,
758                                              Brush *br,
759                                              const float initial_location[3],
760                                              const float radius)
761 {
762   switch (br->pose_origin_type) {
763     case BRUSH_POSE_ORIGIN_TOPOLOGY:
764       return pose_ik_chain_init_topology(sd, ob, ss, br, initial_location, radius);
765       break;
766     case BRUSH_POSE_ORIGIN_FACE_SETS:
767       return pose_ik_chain_init_face_sets(sd, ob, ss, br, radius);
768       break;
769   }
770   return NULL;
771 }
772
773 void SCULPT_pose_brush_init(Sculpt *sd, Object *ob, SculptSession *ss, Brush *br)
774 {
775   PBVHNode **nodes;
776   PBVH *pbvh = ob->sculpt->pbvh;
777   int totnode;
778
779   BKE_pbvh_search_gather(pbvh, NULL, NULL, &nodes, &totnode);
780
781   SculptThreadedTaskData data = {
782       .sd = sd,
783       .ob = ob,
784       .brush = br,
785       .nodes = nodes,
786   };
787
788   /* Init the IK chain that is going to be used to deform the vertices. */
789   ss->cache->pose_ik_chain = SCULPT_pose_ik_chain_init(
790       sd, ob, ss, br, ss->cache->true_location, ss->cache->radius);
791
792   /* Smooth the weights of each segment for cleaner deformation. */
793   for (int ik = 0; ik < ss->cache->pose_ik_chain->tot_segments; ik++) {
794     data.pose_factor = ss->cache->pose_ik_chain->segments[ik].weights;
795     for (int i = 0; i < br->pose_smooth_iterations; i++) {
796       TaskParallelSettings settings;
797       BKE_pbvh_parallel_range_settings(&settings, (sd->flags & SCULPT_USE_OPENMP), totnode);
798       BLI_task_parallel_range(0, totnode, &data, pose_brush_init_task_cb_ex, &settings);
799     }
800   }
801
802   MEM_SAFE_FREE(nodes);
803 }
804
805 /* Main Brush Function. */
806 void SCULPT_do_pose_brush(Sculpt *sd, Object *ob, PBVHNode **nodes, int totnode)
807 {
808   SculptSession *ss = ob->sculpt;
809   Brush *brush = BKE_paint_brush(&sd->paint);
810   float grab_delta[3];
811   float ik_target[3];
812   const ePaintSymmetryFlags symm = sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL;
813
814   /* The pose brush applies all enabled symmetry axis in a single iteration, so the rest can be
815    * ignored. */
816   if (ss->cache->mirror_symmetry_pass != 0) {
817     return;
818   }
819
820   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
821
822   /* Solve the positions and rotations of the IK chain. */
823   if (ss->cache->invert) {
824     /* Roll Mode. */
825     /* Calculate the maximum roll. 0.02 radians per pixel works fine. */
826     float roll = (ss->cache->initial_mouse[0] - ss->cache->mouse[0]) * ss->cache->bstrength *
827                  0.02f;
828     BKE_curvemapping_initialize(brush->curve);
829     pose_solve_roll_chain(ik_chain, brush, roll);
830   }
831   else {
832     /* IK follow target mode. */
833     /* Calculate the IK target. */
834
835     copy_v3_v3(grab_delta, ss->cache->grab_delta);
836     copy_v3_v3(ik_target, ss->cache->true_location);
837     add_v3_v3(ik_target, ss->cache->grab_delta);
838
839     /* Solve the IK positions. */
840     pose_solve_ik_chain(ik_chain, ik_target, brush->flag2 & BRUSH_POSE_IK_ANCHORED);
841   }
842
843   /* Flip the segment chain in all symmetry axis and calculate the transform matrices for each
844    * possible combination. */
845   /* This can be optimized by skipping the calculation of matrices where the symmetry is not
846    * enabled. */
847   for (int symm_it = 0; symm_it < PAINT_SYMM_AREAS; symm_it++) {
848     for (int i = 0; i < brush->pose_ik_segments; i++) {
849       float symm_rot[4];
850       float symm_orig[3];
851       float symm_initial_orig[3];
852
853       ePaintSymmetryAreas symm_area = symm_it;
854
855       copy_qt_qt(symm_rot, ik_chain->segments[i].rot);
856       copy_v3_v3(symm_orig, ik_chain->segments[i].orig);
857       copy_v3_v3(symm_initial_orig, ik_chain->segments[i].initial_orig);
858
859       /* Flip the origins and rotation quats of each segment. */
860       SCULPT_flip_quat_by_symm_area(symm_rot, symm, symm_area, ss->cache->orig_grab_location);
861       SCULPT_flip_v3_by_symm_area(symm_orig, symm, symm_area, ss->cache->orig_grab_location);
862       SCULPT_flip_v3_by_symm_area(
863           symm_initial_orig, symm, symm_area, ss->cache->orig_grab_location);
864
865       /* Create the transform matrix and store it in the segment. */
866       unit_m4(ik_chain->segments[i].pivot_mat[symm_it]);
867       quat_to_mat4(ik_chain->segments[i].trans_mat[symm_it], symm_rot);
868
869       translate_m4(ik_chain->segments[i].trans_mat[symm_it],
870                    symm_orig[0] - symm_initial_orig[0],
871                    symm_orig[1] - symm_initial_orig[1],
872                    symm_orig[2] - symm_initial_orig[2]);
873       translate_m4(
874           ik_chain->segments[i].pivot_mat[symm_it], symm_orig[0], symm_orig[1], symm_orig[2]);
875       invert_m4_m4(ik_chain->segments[i].pivot_mat_inv[symm_it],
876                    ik_chain->segments[i].pivot_mat[symm_it]);
877     }
878   }
879
880   SculptThreadedTaskData data = {
881       .sd = sd,
882       .ob = ob,
883       .brush = brush,
884       .nodes = nodes,
885       .grab_delta = grab_delta,
886   };
887
888   TaskParallelSettings settings;
889   BKE_pbvh_parallel_range_settings(&settings, (sd->flags & SCULPT_USE_OPENMP), totnode);
890   BLI_task_parallel_range(0, totnode, &data, do_pose_brush_task_cb_ex, &settings);
891 }
892
893 void SCULPT_pose_ik_chain_free(SculptPoseIKChain *ik_chain)
894 {
895   for (int i = 0; i < ik_chain->tot_segments; i++) {
896     MEM_SAFE_FREE(ik_chain->segments[i].weights);
897   }
898   MEM_SAFE_FREE(ik_chain->segments);
899   MEM_SAFE_FREE(ik_chain);
900 }