Sculpt: Pose Brush Scale/Transform deform mode
[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 pose_solve_translate_chain(SculptPoseIKChain *ik_chain, const float delta[3])
135 {
136   SculptPoseIKChainSegment *segments = ik_chain->segments;
137   const int tot_segments = ik_chain->tot_segments;
138
139   for (int i = 0; i < tot_segments; i++) {
140     /* Move the origin and head of each segment by delta. */
141     add_v3_v3v3(segments[i].head, segments[i].initial_head, delta);
142     add_v3_v3v3(segments[i].orig, segments[i].initial_orig, delta);
143
144     /* Reset the segment rotation. */
145     unit_qt(segments[i].rot);
146   }
147 }
148
149 static void pose_solve_scale_chain(SculptPoseIKChain *ik_chain, const float scale)
150 {
151   SculptPoseIKChainSegment *segments = ik_chain->segments;
152   const int tot_segments = ik_chain->tot_segments;
153
154   for (int i = 0; i < tot_segments; i++) {
155     /* Assign the scale to each segment. */
156     segments[i].scale = scale;
157   }
158 }
159
160 static void do_pose_brush_task_cb_ex(void *__restrict userdata,
161                                      const int n,
162                                      const TaskParallelTLS *__restrict UNUSED(tls))
163 {
164   SculptThreadedTaskData *data = userdata;
165   SculptSession *ss = data->ob->sculpt;
166   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
167   SculptPoseIKChainSegment *segments = ik_chain->segments;
168
169   PBVHVertexIter vd;
170   float disp[3], new_co[3];
171   float final_pos[3];
172
173   SculptOrigVertData orig_data;
174   SCULPT_orig_vert_data_init(&orig_data, data->ob, data->nodes[n]);
175
176   BKE_pbvh_vertex_iter_begin(ss->pbvh, data->nodes[n], vd, PBVH_ITER_UNIQUE)
177   {
178     SCULPT_orig_vert_data_update(&orig_data, &vd);
179
180     float total_disp[3];
181     zero_v3(total_disp);
182
183     ePaintSymmetryAreas symm_area = SCULPT_get_vertex_symm_area(orig_data.co);
184
185     /* Calculate the displacement of each vertex for all the segments in the chain. */
186     for (int ik = 0; ik < ik_chain->tot_segments; ik++) {
187       copy_v3_v3(new_co, orig_data.co);
188
189       /* Get the transform matrix for the vertex symmetry area to calculate a displacement in the
190        * vertex. */
191       mul_m4_v3(segments[ik].pivot_mat_inv[(int)symm_area], new_co);
192       mul_m4_v3(segments[ik].trans_mat[(int)symm_area], new_co);
193       mul_m4_v3(segments[ik].pivot_mat[(int)symm_area], new_co);
194
195       /* Apply the segment weight of the vertex to the displacement. */
196       sub_v3_v3v3(disp, new_co, orig_data.co);
197       mul_v3_fl(disp, segments[ik].weights[vd.index]);
198
199       /* Apply the vertex mask to the displacement. */
200       float mask = vd.mask ? *vd.mask : 0.0f;
201       mul_v3_fl(disp, 1.0f - mask);
202
203       /* Accumulate the displacement. */
204       add_v3_v3(total_disp, disp);
205     }
206
207     /* Apply the accumulated displacement to the vertex. */
208     add_v3_v3v3(final_pos, orig_data.co, total_disp);
209     copy_v3_v3(vd.co, final_pos);
210
211     if (vd.mvert) {
212       vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
213     }
214   }
215   BKE_pbvh_vertex_iter_end;
216 }
217
218 typedef struct PoseGrowFactorTLSData {
219   float pos_avg[3];
220   int pos_count;
221 } PoseGrowFactorTLSData;
222
223 static void pose_brush_grow_factor_task_cb_ex(void *__restrict userdata,
224                                               const int n,
225                                               const TaskParallelTLS *__restrict tls)
226 {
227   SculptThreadedTaskData *data = userdata;
228   PoseGrowFactorTLSData *gftd = tls->userdata_chunk;
229   SculptSession *ss = data->ob->sculpt;
230   const char symm = data->sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL;
231   PBVHVertexIter vd;
232   BKE_pbvh_vertex_iter_begin(ss->pbvh, data->nodes[n], vd, PBVH_ITER_UNIQUE)
233   {
234     SculptVertexNeighborIter ni;
235     float max = 0.0f;
236
237     /* Grow the factor. */
238     SCULPT_VERTEX_NEIGHBORS_ITER_BEGIN (ss, vd.index, ni) {
239       float vmask_f = data->prev_mask[ni.index];
240       max = MAX2(vmask_f, max);
241     }
242     SCULPT_VERTEX_NEIGHBORS_ITER_END(ni);
243
244     /* Keep the count of the vertices that where added to the factors in this grow iteration. */
245     if (max > data->prev_mask[vd.index]) {
246       data->pose_factor[vd.index] = max;
247       if (SCULPT_check_vertex_pivot_symmetry(vd.co, data->pose_initial_co, symm)) {
248         add_v3_v3(gftd->pos_avg, vd.co);
249         gftd->pos_count++;
250       }
251     }
252   }
253
254   BKE_pbvh_vertex_iter_end;
255 }
256
257 static void pose_brush_grow_factor_reduce(const void *__restrict UNUSED(userdata),
258                                           void *__restrict chunk_join,
259                                           void *__restrict chunk)
260 {
261   PoseGrowFactorTLSData *join = chunk_join;
262   PoseGrowFactorTLSData *gftd = chunk;
263   add_v3_v3(join->pos_avg, gftd->pos_avg);
264   join->pos_count += gftd->pos_count;
265 }
266
267 /* Grow the factor until its boundary is near to the offset pose origin or outside the target
268  * distance. */
269 static void sculpt_pose_grow_pose_factor(Sculpt *sd,
270                                          Object *ob,
271                                          SculptSession *ss,
272                                          float pose_origin[3],
273                                          float pose_target[3],
274                                          float max_len,
275                                          float *r_pose_origin,
276                                          float *pose_factor)
277 {
278   PBVHNode **nodes;
279   PBVH *pbvh = ob->sculpt->pbvh;
280   int totnode;
281
282   BKE_pbvh_search_gather(pbvh, NULL, NULL, &nodes, &totnode);
283   SculptThreadedTaskData data = {
284       .sd = sd,
285       .ob = ob,
286       .nodes = nodes,
287       .totnode = totnode,
288       .pose_factor = pose_factor,
289   };
290
291   data.pose_initial_co = pose_target;
292   TaskParallelSettings settings;
293   PoseGrowFactorTLSData gftd;
294   gftd.pos_count = 0;
295   zero_v3(gftd.pos_avg);
296   BKE_pbvh_parallel_range_settings(&settings, (sd->flags & SCULPT_USE_OPENMP), totnode);
297   settings.func_reduce = pose_brush_grow_factor_reduce;
298   settings.userdata_chunk = &gftd;
299   settings.userdata_chunk_size = sizeof(PoseGrowFactorTLSData);
300
301   bool grow_next_iteration = true;
302   float prev_len = FLT_MAX;
303   data.prev_mask = MEM_mallocN(SCULPT_vertex_count_get(ss) * sizeof(float), "prev mask");
304   while (grow_next_iteration) {
305     zero_v3(gftd.pos_avg);
306     gftd.pos_count = 0;
307     memcpy(data.prev_mask, pose_factor, SCULPT_vertex_count_get(ss) * sizeof(float));
308     BLI_task_parallel_range(0, totnode, &data, pose_brush_grow_factor_task_cb_ex, &settings);
309
310     if (gftd.pos_count != 0) {
311       mul_v3_fl(gftd.pos_avg, 1.0f / (float)gftd.pos_count);
312       if (pose_origin) {
313         /* Test with pose origin. Used when growing the factors to compensate the Origin Offset. */
314         /* Stop when the factor's avg_pos starts moving away from the origin instead of getting
315          * closer to it. */
316         float len = len_v3v3(gftd.pos_avg, pose_origin);
317         if (len < prev_len) {
318           prev_len = len;
319           grow_next_iteration = true;
320         }
321         else {
322           grow_next_iteration = false;
323           memcpy(pose_factor, data.prev_mask, SCULPT_vertex_count_get(ss) * sizeof(float));
324         }
325       }
326       else {
327         /* Test with length. Used to calculate the origin positions of the IK chain. */
328         /* Stops when the factors have grown enough to generate a new segment origin. */
329         float len = len_v3v3(gftd.pos_avg, pose_target);
330         if (len < max_len) {
331           prev_len = len;
332           grow_next_iteration = true;
333         }
334         else {
335           grow_next_iteration = false;
336           if (r_pose_origin) {
337             copy_v3_v3(r_pose_origin, gftd.pos_avg);
338           }
339           memcpy(pose_factor, data.prev_mask, SCULPT_vertex_count_get(ss) * sizeof(float));
340         }
341       }
342     }
343     else {
344       if (r_pose_origin) {
345         copy_v3_v3(r_pose_origin, pose_target);
346       }
347       grow_next_iteration = false;
348     }
349   }
350   MEM_freeN(data.prev_mask);
351
352   MEM_SAFE_FREE(nodes);
353 }
354
355 static bool sculpt_pose_brush_is_vertex_inside_brush_radius(const float vertex[3],
356                                                             const float br_co[3],
357                                                             float radius,
358                                                             char symm)
359 {
360   for (char i = 0; i <= symm; ++i) {
361     if (SCULPT_is_symmetry_iteration_valid(i, symm)) {
362       float location[3];
363       flip_v3_v3(location, br_co, (char)i);
364       if (len_v3v3(location, vertex) < radius) {
365         return true;
366       }
367     }
368   }
369   return false;
370 }
371
372 typedef struct PoseFloodFillData {
373   float pose_initial_co[3];
374   float radius;
375   int symm;
376
377   float *pose_factor;
378   float pose_origin[3];
379   int tot_co;
380
381   int current_face_set;
382   int next_face_set;
383   int prev_face_set;
384   int next_vertex;
385
386   bool next_face_set_found;
387
388   /* Store the visited face sets to avoid going back when calculating the chain. */
389   GSet *visited_face_sets;
390
391   /* In face sets origin mode, each vertex can only be assigned to one face set. */
392   bool *is_weighted;
393
394   bool is_first_iteration;
395
396   /* Fallback origin. If we can't find any face set to continue, use the position of all vertices
397    * that have the current face set. */
398   float fallback_origin[3];
399   int fallback_count;
400 } PoseFloodFillData;
401
402 static bool pose_topology_floodfill_cb(
403     SculptSession *ss, int UNUSED(from_v), int to_v, bool is_duplicate, void *userdata)
404 {
405   PoseFloodFillData *data = userdata;
406
407   if (data->pose_factor) {
408     data->pose_factor[to_v] = 1.0f;
409   }
410
411   const float *co = SCULPT_vertex_co_get(ss, to_v);
412   if (sculpt_pose_brush_is_vertex_inside_brush_radius(
413           co, data->pose_initial_co, data->radius, data->symm)) {
414     return true;
415   }
416   else if (SCULPT_check_vertex_pivot_symmetry(co, data->pose_initial_co, data->symm)) {
417     if (!is_duplicate) {
418       add_v3_v3(data->pose_origin, co);
419       data->tot_co++;
420     }
421   }
422
423   return false;
424 }
425
426 static bool pose_face_sets_floodfill_cb(
427     SculptSession *ss, int UNUSED(from_v), int to_v, bool is_duplicate, void *userdata)
428 {
429   PoseFloodFillData *data = userdata;
430
431   const int index = to_v;
432   bool visit_next = false;
433
434   const float *co = SCULPT_vertex_co_get(ss, index);
435   const bool symmetry_check = SCULPT_check_vertex_pivot_symmetry(
436                                   co, data->pose_initial_co, data->symm) &&
437                               !is_duplicate;
438
439   /* First iteration. Continue expanding using topology until a vertex is outside the brush radius
440    * to determine the first face set. */
441   if (data->current_face_set == SCULPT_FACE_SET_NONE) {
442
443     data->pose_factor[index] = 1.0f;
444     data->is_weighted[index] = true;
445
446     if (sculpt_pose_brush_is_vertex_inside_brush_radius(
447             co, data->pose_initial_co, data->radius, data->symm)) {
448       const int visited_face_set = SCULPT_vertex_face_set_get(ss, index);
449       BLI_gset_add(data->visited_face_sets, POINTER_FROM_INT(visited_face_set));
450     }
451     else if (symmetry_check) {
452       data->current_face_set = SCULPT_vertex_face_set_get(ss, index);
453       BLI_gset_add(data->visited_face_sets, POINTER_FROM_INT(data->current_face_set));
454     }
455     return true;
456   }
457
458   /* We already have a current face set, so we can start checking the face sets of the vertices. */
459   /* In the first iteration we need to check all face sets we already visited as the flood fill may
460    * still not be finished in some of them. */
461   bool is_vertex_valid = false;
462   if (data->is_first_iteration) {
463     GSetIterator gs_iter;
464     GSET_ITER (gs_iter, data->visited_face_sets) {
465       const int visited_face_set = POINTER_AS_INT(BLI_gsetIterator_getKey(&gs_iter));
466       is_vertex_valid |= SCULPT_vertex_has_face_set(ss, index, visited_face_set);
467     }
468   }
469   else {
470     is_vertex_valid = SCULPT_vertex_has_face_set(ss, index, data->current_face_set);
471   }
472
473   if (is_vertex_valid) {
474
475     if (!data->is_weighted[index]) {
476       data->pose_factor[index] = 1.0f;
477       data->is_weighted[index] = true;
478       visit_next = true;
479     }
480
481     /* Fallback origin accumulation. */
482     if (symmetry_check) {
483       add_v3_v3(data->fallback_origin, SCULPT_vertex_co_get(ss, index));
484       data->fallback_count++;
485     }
486
487     if (symmetry_check && !SCULPT_vertex_has_unique_face_set(ss, index)) {
488
489       /* We only add coordinates for calculating the origin when it is possible to go from this
490        * vertex to another vertex in a valid face set for the next iteration. */
491       bool count_as_boundary = false;
492
493       SculptVertexNeighborIter ni;
494       SCULPT_VERTEX_NEIGHBORS_ITER_BEGIN (ss, index, ni) {
495         int next_face_set_candidate = SCULPT_vertex_face_set_get(ss, ni.index);
496
497         /* Check if we can get a valid face set for the next iteration from this neighbor. */
498         if (SCULPT_vertex_has_unique_face_set(ss, ni.index) &&
499             !BLI_gset_haskey(data->visited_face_sets, POINTER_FROM_INT(next_face_set_candidate))) {
500           if (!data->next_face_set_found) {
501             data->next_face_set = next_face_set_candidate;
502             data->next_vertex = ni.index;
503             data->next_face_set_found = true;
504           }
505           count_as_boundary = true;
506         }
507       }
508       SCULPT_VERTEX_NEIGHBORS_ITER_END(ni);
509
510       /* Origin accumulation. */
511       if (count_as_boundary) {
512         add_v3_v3(data->pose_origin, SCULPT_vertex_co_get(ss, index));
513         data->tot_co++;
514       }
515     }
516   }
517   return visit_next;
518 }
519
520 /* Public functions. */
521
522 /* Calculate the pose origin and (Optionaly the pose factor) that is used when using the pose brush
523  *
524  * r_pose_origin must be a valid pointer. the r_pose_factor is optional. When set to NULL it won't
525  * be calculated. */
526 void SCULPT_pose_calc_pose_data(Sculpt *sd,
527                                 Object *ob,
528                                 SculptSession *ss,
529                                 float initial_location[3],
530                                 float radius,
531                                 float pose_offset,
532                                 float *r_pose_origin,
533                                 float *r_pose_factor)
534 {
535   SCULPT_vertex_random_access_init(ss);
536
537   /* Calculate the pose rotation point based on the boundaries of the brush factor. */
538   SculptFloodFill flood;
539   SCULPT_floodfill_init(ss, &flood);
540   SCULPT_floodfill_add_active(sd, ob, ss, &flood, (r_pose_factor) ? radius : 0.0f);
541
542   PoseFloodFillData fdata = {
543       .radius = radius,
544       .symm = sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL,
545       .pose_factor = r_pose_factor,
546       .tot_co = 0,
547   };
548   zero_v3(fdata.pose_origin);
549   copy_v3_v3(fdata.pose_initial_co, initial_location);
550   SCULPT_floodfill_execute(ss, &flood, pose_topology_floodfill_cb, &fdata);
551   SCULPT_floodfill_free(&flood);
552
553   if (fdata.tot_co > 0) {
554     mul_v3_fl(fdata.pose_origin, 1.0f / (float)fdata.tot_co);
555   }
556
557   /* Offset the pose origin. */
558   float pose_d[3];
559   sub_v3_v3v3(pose_d, fdata.pose_origin, fdata.pose_initial_co);
560   normalize_v3(pose_d);
561   madd_v3_v3fl(fdata.pose_origin, pose_d, radius * pose_offset);
562   copy_v3_v3(r_pose_origin, fdata.pose_origin);
563
564   /* Do the initial grow of the factors to get the first segment of the chain with Origin Offset.
565    */
566   if (pose_offset != 0.0f && r_pose_factor) {
567     sculpt_pose_grow_pose_factor(
568         sd, ob, ss, fdata.pose_origin, fdata.pose_origin, 0, NULL, r_pose_factor);
569   }
570 }
571
572 static void pose_brush_init_task_cb_ex(void *__restrict userdata,
573                                        const int n,
574                                        const TaskParallelTLS *__restrict UNUSED(tls))
575 {
576   SculptThreadedTaskData *data = userdata;
577   SculptSession *ss = data->ob->sculpt;
578   PBVHVertexIter vd;
579   BKE_pbvh_vertex_iter_begin(ss->pbvh, data->nodes[n], vd, PBVH_ITER_UNIQUE)
580   {
581     SculptVertexNeighborIter ni;
582     float avg = 0.0f;
583     int total = 0;
584     SCULPT_VERTEX_NEIGHBORS_ITER_BEGIN (ss, vd.index, ni) {
585       avg += data->pose_factor[ni.index];
586       total++;
587     }
588     SCULPT_VERTEX_NEIGHBORS_ITER_END(ni);
589
590     if (total > 0) {
591       data->pose_factor[vd.index] = avg / total;
592     }
593   }
594   BKE_pbvh_vertex_iter_end;
595 }
596
597 /* Init the IK chain with empty weights. */
598 static SculptPoseIKChain *pose_ik_chain_new(const int totsegments, const int totverts)
599 {
600   SculptPoseIKChain *ik_chain = MEM_callocN(sizeof(SculptPoseIKChain), "Pose IK Chain");
601   ik_chain->tot_segments = totsegments;
602   ik_chain->segments = MEM_callocN(totsegments * sizeof(SculptPoseIKChainSegment),
603                                    "Pose IK Chain Segments");
604   for (int i = 0; i < totsegments; i++) {
605     ik_chain->segments[i].weights = MEM_callocN(totverts * sizeof(float), "Pose IK weights");
606   }
607   return ik_chain;
608 }
609
610 /* Init the origin/head pairs of all the segments from the calculated origins. */
611 static void pose_ik_chain_origin_heads_init(SculptPoseIKChain *ik_chain,
612                                             const float initial_location[3])
613 {
614   float origin[3];
615   float head[3];
616   for (int i = 0; i < ik_chain->tot_segments; i++) {
617     if (i == 0) {
618       copy_v3_v3(head, initial_location);
619       copy_v3_v3(origin, ik_chain->segments[i].orig);
620     }
621     else {
622       copy_v3_v3(head, ik_chain->segments[i - 1].orig);
623       copy_v3_v3(origin, ik_chain->segments[i].orig);
624     }
625     copy_v3_v3(ik_chain->segments[i].orig, origin);
626     copy_v3_v3(ik_chain->segments[i].initial_orig, origin);
627     copy_v3_v3(ik_chain->segments[i].initial_head, head);
628     ik_chain->segments[i].len = len_v3v3(head, origin);
629     ik_chain->segments[i].scale = 1.0f;
630   }
631 }
632
633 static int pose_brush_num_effective_segments(const Brush *brush)
634 {
635   /* Scaling multiple segments at the same time is not supported as the IK solver can't handle
636    * changes in the segment's length. It will also required a better weight distribution to avoid
637    * artifacts in the areas affected by multiple segments. */
638   if (brush->pose_deform_type == BRUSH_POSE_DEFORM_SCALE_TRASLATE) {
639     return 1;
640   }
641   return brush->pose_ik_segments;
642 }
643
644 static SculptPoseIKChain *pose_ik_chain_init_topology(Sculpt *sd,
645                                                       Object *ob,
646                                                       SculptSession *ss,
647                                                       Brush *br,
648                                                       const float initial_location[3],
649                                                       const float radius)
650 {
651
652   const float chain_segment_len = radius * (1.0f + br->pose_offset);
653   float next_chain_segment_target[3];
654
655   int totvert = SCULPT_vertex_count_get(ss);
656   int nearest_vertex_index = SCULPT_nearest_vertex_get(sd, ob, initial_location, FLT_MAX, true);
657
658   /* Init the buffers used to keep track of the changes in the pose factors as more segments are
659    * added to the IK chain. */
660
661   /* This stores the whole pose factors values as they grow through the mesh. */
662   float *pose_factor_grow = MEM_callocN(totvert * sizeof(float), "Pose Factor Grow");
663
664   /* This stores the previous status of the factors when growing a new iteration. */
665   float *pose_factor_grow_prev = MEM_callocN(totvert * sizeof(float),
666                                              "Pose Factor Grow Prev Iteration");
667
668   pose_factor_grow[nearest_vertex_index] = 1.0f;
669
670   const int tot_segments = pose_brush_num_effective_segments(br);
671   SculptPoseIKChain *ik_chain = pose_ik_chain_new(tot_segments, totvert);
672
673   /* Calculate the first segment in the chain using the brush radius and the pose origin offset. */
674   copy_v3_v3(next_chain_segment_target, initial_location);
675   SCULPT_pose_calc_pose_data(sd,
676                              ob,
677                              ss,
678                              next_chain_segment_target,
679                              radius,
680                              br->pose_offset,
681                              ik_chain->segments[0].orig,
682                              pose_factor_grow);
683
684   copy_v3_v3(next_chain_segment_target, ik_chain->segments[0].orig);
685
686   /* Init the weights of this segment and store the status of the pose factors to start calculating
687    * new segment origins. */
688   for (int j = 0; j < totvert; j++) {
689     ik_chain->segments[0].weights[j] = pose_factor_grow[j];
690     pose_factor_grow_prev[j] = pose_factor_grow[j];
691   }
692
693   /* Calculate the next segments in the chain growing the pose factors. */
694   for (int i = 1; i < ik_chain->tot_segments; i++) {
695
696     /* Grow the factors to get the new segment origin. */
697     sculpt_pose_grow_pose_factor(sd,
698                                  ob,
699                                  ss,
700                                  NULL,
701                                  next_chain_segment_target,
702                                  chain_segment_len,
703                                  ik_chain->segments[i].orig,
704                                  pose_factor_grow);
705     copy_v3_v3(next_chain_segment_target, ik_chain->segments[i].orig);
706
707     /* Create the weights for this segment from the difference between the previous grow factor
708      * iteration an the current iteration. */
709     for (int j = 0; j < totvert; j++) {
710       ik_chain->segments[i].weights[j] = pose_factor_grow[j] - pose_factor_grow_prev[j];
711       /* Store the current grow factor status for the next interation. */
712       pose_factor_grow_prev[j] = pose_factor_grow[j];
713     }
714   }
715
716   pose_ik_chain_origin_heads_init(ik_chain, initial_location);
717
718   MEM_freeN(pose_factor_grow);
719   MEM_freeN(pose_factor_grow_prev);
720
721   return ik_chain;
722 }
723
724 static SculptPoseIKChain *pose_ik_chain_init_face_sets(
725     Sculpt *sd, Object *ob, SculptSession *ss, Brush *br, const float radius)
726 {
727
728   int totvert = SCULPT_vertex_count_get(ss);
729
730   const int tot_segments = pose_brush_num_effective_segments(br);
731
732   SculptPoseIKChain *ik_chain = pose_ik_chain_new(tot_segments, totvert);
733
734   GSet *visited_face_sets = BLI_gset_int_new_ex("visited_face_sets", ik_chain->tot_segments);
735
736   bool *is_weighted = MEM_callocN(sizeof(bool) * totvert, "weighted");
737
738   int current_face_set = SCULPT_FACE_SET_NONE;
739   int prev_face_set = SCULPT_FACE_SET_NONE;
740
741   int current_vertex = SCULPT_active_vertex_get(ss);
742
743   for (int s = 0; s < ik_chain->tot_segments; s++) {
744
745     SculptFloodFill flood;
746     SCULPT_floodfill_init(ss, &flood);
747     SCULPT_floodfill_add_initial_with_symmetry(sd, ob, ss, &flood, current_vertex, FLT_MAX);
748
749     BLI_gset_add(visited_face_sets, POINTER_FROM_INT(current_face_set));
750
751     PoseFloodFillData fdata = {
752         .radius = radius,
753         .symm = sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL,
754         .pose_factor = ik_chain->segments[s].weights,
755         .tot_co = 0,
756         .fallback_count = 0,
757         .current_face_set = current_face_set,
758         .prev_face_set = prev_face_set,
759         .visited_face_sets = visited_face_sets,
760         .is_weighted = is_weighted,
761         .next_face_set_found = false,
762         .is_first_iteration = s == 0,
763     };
764     zero_v3(fdata.pose_origin);
765     zero_v3(fdata.fallback_origin);
766     copy_v3_v3(fdata.pose_initial_co, SCULPT_vertex_co_get(ss, current_vertex));
767     SCULPT_floodfill_execute(ss, &flood, pose_face_sets_floodfill_cb, &fdata);
768     SCULPT_floodfill_free(&flood);
769
770     if (fdata.tot_co > 0) {
771       mul_v3_fl(fdata.pose_origin, 1.0f / (float)fdata.tot_co);
772       copy_v3_v3(ik_chain->segments[s].orig, fdata.pose_origin);
773     }
774     else if (fdata.fallback_count > 0) {
775       mul_v3_fl(fdata.fallback_origin, 1.0f / (float)fdata.fallback_count);
776       copy_v3_v3(ik_chain->segments[s].orig, fdata.fallback_origin);
777     }
778     else {
779       zero_v3(ik_chain->segments[s].orig);
780     }
781
782     prev_face_set = fdata.current_face_set;
783     current_face_set = fdata.next_face_set;
784     current_vertex = fdata.next_vertex;
785   }
786
787   BLI_gset_free(visited_face_sets, NULL);
788
789   pose_ik_chain_origin_heads_init(ik_chain, SCULPT_active_vertex_co_get(ss));
790
791   MEM_SAFE_FREE(is_weighted);
792
793   return ik_chain;
794 }
795
796 SculptPoseIKChain *SCULPT_pose_ik_chain_init(Sculpt *sd,
797                                              Object *ob,
798                                              SculptSession *ss,
799                                              Brush *br,
800                                              const float initial_location[3],
801                                              const float radius)
802 {
803   switch (br->pose_origin_type) {
804     case BRUSH_POSE_ORIGIN_TOPOLOGY:
805       return pose_ik_chain_init_topology(sd, ob, ss, br, initial_location, radius);
806       break;
807     case BRUSH_POSE_ORIGIN_FACE_SETS:
808       return pose_ik_chain_init_face_sets(sd, ob, ss, br, radius);
809       break;
810   }
811   return NULL;
812 }
813
814 void SCULPT_pose_brush_init(Sculpt *sd, Object *ob, SculptSession *ss, Brush *br)
815 {
816   PBVHNode **nodes;
817   PBVH *pbvh = ob->sculpt->pbvh;
818   int totnode;
819
820   BKE_pbvh_search_gather(pbvh, NULL, NULL, &nodes, &totnode);
821
822   SculptThreadedTaskData data = {
823       .sd = sd,
824       .ob = ob,
825       .brush = br,
826       .nodes = nodes,
827   };
828
829   /* Init the IK chain that is going to be used to deform the vertices. */
830   ss->cache->pose_ik_chain = SCULPT_pose_ik_chain_init(
831       sd, ob, ss, br, ss->cache->true_location, ss->cache->radius);
832
833   /* Smooth the weights of each segment for cleaner deformation. */
834   for (int ik = 0; ik < ss->cache->pose_ik_chain->tot_segments; ik++) {
835     data.pose_factor = ss->cache->pose_ik_chain->segments[ik].weights;
836     for (int i = 0; i < br->pose_smooth_iterations; i++) {
837       TaskParallelSettings settings;
838       BKE_pbvh_parallel_range_settings(&settings, (sd->flags & SCULPT_USE_OPENMP), totnode);
839       BLI_task_parallel_range(0, totnode, &data, pose_brush_init_task_cb_ex, &settings);
840     }
841   }
842
843   MEM_SAFE_FREE(nodes);
844 }
845
846 static void sculpt_pose_do_translate_deform(SculptSession *ss, Brush *brush)
847 {
848   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
849   BKE_curvemapping_initialize(brush->curve);
850   pose_solve_translate_chain(ik_chain, ss->cache->grab_delta);
851 }
852
853 static void sculpt_pose_do_scale_deform(SculptSession *ss, Brush *brush)
854 {
855   float ik_target[3];
856   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
857
858   copy_v3_v3(ik_target, ss->cache->true_location);
859   add_v3_v3(ik_target, ss->cache->grab_delta);
860
861   /* Solve the IK for the first segment to include rotation as part of scale. */
862   pose_solve_ik_chain(ik_chain, ik_target, brush->flag2 & BRUSH_POSE_IK_ANCHORED);
863
864   /* Calculate a scale factor based on the grab delta. */
865   float plane[4];
866   float segment_dir[3];
867   sub_v3_v3v3(segment_dir, ik_chain->segments[0].initial_head, ik_chain->segments[0].initial_orig);
868   normalize_v3(segment_dir);
869   plane_from_point_normal_v3(plane, ik_chain->segments[0].initial_head, segment_dir);
870   const float segment_len = ik_chain->segments[0].len;
871   const float scale = segment_len / (segment_len - dist_signed_to_plane_v3(ik_target, plane));
872
873   /* Write the scale into the segments. */
874   pose_solve_scale_chain(ik_chain, scale);
875 }
876
877 static void sculpt_pose_do_twist_deform(SculptSession *ss, Brush *brush)
878 {
879   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
880
881   /* Calculate the maximum roll. 0.02 radians per pixel works fine. */
882   float roll = (ss->cache->initial_mouse[0] - ss->cache->mouse[0]) * ss->cache->bstrength * 0.02f;
883   BKE_curvemapping_initialize(brush->curve);
884   pose_solve_roll_chain(ik_chain, brush, roll);
885 }
886
887 static void sculpt_pose_do_rotate_deform(SculptSession *ss, Brush *brush)
888 {
889   float ik_target[3];
890   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
891
892   /* Calculate the IK target. */
893   copy_v3_v3(ik_target, ss->cache->true_location);
894   add_v3_v3(ik_target, ss->cache->grab_delta);
895
896   /* Solve the IK positions. */
897   pose_solve_ik_chain(ik_chain, ik_target, brush->flag2 & BRUSH_POSE_IK_ANCHORED);
898 }
899
900 static void sculpt_pose_do_rotate_twist_deform(SculptSession *ss, Brush *brush)
901 {
902   if (ss->cache->invert) {
903     sculpt_pose_do_twist_deform(ss, brush);
904   }
905   else {
906     sculpt_pose_do_rotate_deform(ss, brush);
907   }
908 }
909
910 static void sculpt_pose_do_scale_translate_deform(SculptSession *ss, Brush *brush)
911 {
912   if (ss->cache->invert) {
913     sculpt_pose_do_translate_deform(ss, brush);
914   }
915   else {
916     sculpt_pose_do_scale_deform(ss, brush);
917   }
918 }
919
920 /* Main Brush Function. */
921 void SCULPT_do_pose_brush(Sculpt *sd, Object *ob, PBVHNode **nodes, int totnode)
922 {
923   SculptSession *ss = ob->sculpt;
924   Brush *brush = BKE_paint_brush(&sd->paint);
925   float grab_delta[3];
926   const ePaintSymmetryFlags symm = sd->paint.symmetry_flags & PAINT_SYMM_AXIS_ALL;
927
928   /* The pose brush applies all enabled symmetry axis in a single iteration, so the rest can be
929    * ignored. */
930   if (ss->cache->mirror_symmetry_pass != 0) {
931     return;
932   }
933
934   SculptPoseIKChain *ik_chain = ss->cache->pose_ik_chain;
935   copy_v3_v3(grab_delta, ss->cache->grab_delta);
936
937   switch (brush->pose_deform_type) {
938     case BRUSH_POSE_DEFORM_ROTATE_TWIST:
939       sculpt_pose_do_rotate_twist_deform(ss, brush);
940       break;
941     case BRUSH_POSE_DEFORM_SCALE_TRASLATE:
942       sculpt_pose_do_scale_translate_deform(ss, brush);
943       break;
944   }
945
946   /* Flip the segment chain in all symmetry axis and calculate the transform matrices for each
947    * possible combination. */
948   /* This can be optimized by skipping the calculation of matrices where the symmetry is not
949    * enabled. */
950   for (int symm_it = 0; symm_it < PAINT_SYMM_AREAS; symm_it++) {
951     for (int i = 0; i < ik_chain->tot_segments; i++) {
952       float symm_rot[4];
953       float symm_orig[3];
954       float symm_initial_orig[3];
955
956       ePaintSymmetryAreas symm_area = symm_it;
957
958       copy_qt_qt(symm_rot, ik_chain->segments[i].rot);
959       copy_v3_v3(symm_orig, ik_chain->segments[i].orig);
960       copy_v3_v3(symm_initial_orig, ik_chain->segments[i].initial_orig);
961
962       /* Flip the origins and rotation quats of each segment. */
963       SCULPT_flip_quat_by_symm_area(symm_rot, symm, symm_area, ss->cache->orig_grab_location);
964       SCULPT_flip_v3_by_symm_area(symm_orig, symm, symm_area, ss->cache->orig_grab_location);
965       SCULPT_flip_v3_by_symm_area(
966           symm_initial_orig, symm, symm_area, ss->cache->orig_grab_location);
967
968       /* Create the transform matrix and store it in the segment. */
969       unit_m4(ik_chain->segments[i].pivot_mat[symm_it]);
970       quat_to_mat4(ik_chain->segments[i].trans_mat[symm_it], symm_rot);
971       mul_m4_fl(ik_chain->segments[i].trans_mat[symm_it], ik_chain->segments[i].scale);
972
973       translate_m4(ik_chain->segments[i].trans_mat[symm_it],
974                    symm_orig[0] - symm_initial_orig[0],
975                    symm_orig[1] - symm_initial_orig[1],
976                    symm_orig[2] - symm_initial_orig[2]);
977       translate_m4(
978           ik_chain->segments[i].pivot_mat[symm_it], symm_orig[0], symm_orig[1], symm_orig[2]);
979       invert_m4_m4(ik_chain->segments[i].pivot_mat_inv[symm_it],
980                    ik_chain->segments[i].pivot_mat[symm_it]);
981     }
982   }
983
984   SculptThreadedTaskData data = {
985       .sd = sd,
986       .ob = ob,
987       .brush = brush,
988       .nodes = nodes,
989       .grab_delta = grab_delta,
990   };
991
992   TaskParallelSettings settings;
993   BKE_pbvh_parallel_range_settings(&settings, (sd->flags & SCULPT_USE_OPENMP), totnode);
994   BLI_task_parallel_range(0, totnode, &data, do_pose_brush_task_cb_ex, &settings);
995 }
996
997 void SCULPT_pose_ik_chain_free(SculptPoseIKChain *ik_chain)
998 {
999   for (int i = 0; i < ik_chain->tot_segments; i++) {
1000     MEM_SAFE_FREE(ik_chain->segments[i].weights);
1001   }
1002   MEM_SAFE_FREE(ik_chain->segments);
1003   MEM_SAFE_FREE(ik_chain);
1004 }