Math Lib: rename mul_serie_m3 to mul_m3_series & reorder args
[blender-staging.git] / source / blender / blenkernel / intern / tracking_stabilize.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2011 Blender Foundation.
19  * All rights reserved.
20  *
21  * Contributor(s): Blender Foundation,
22  *                 Sergey Sharybin
23  *                 Keir Mierle
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/blenkernel/intern/tracking_stabilize.c
29  *  \ingroup bke
30  *
31  * This file contains implementation of 2D frame stabilization.
32  */
33
34 #include <limits.h>
35
36 #include "DNA_movieclip_types.h"
37
38 #include "BLI_utildefines.h"
39 #include "BLI_math.h"
40
41 #include "BKE_tracking.h"
42
43 #include "IMB_imbuf_types.h"
44 #include "IMB_imbuf.h"
45
46 /* Calculate median point of markers of tracks marked as used for
47  * 2D stabilization.
48  *
49  * NOTE: frame number should be in clip space, not scene space
50  */
51 static bool stabilization_median_point_get(MovieTracking *tracking, int framenr, float median[2])
52 {
53         bool ok = false;
54         float min[2], max[2];
55         MovieTrackingTrack *track;
56
57         INIT_MINMAX2(min, max);
58
59         track = tracking->tracks.first;
60         while (track) {
61                 if (track->flag & TRACK_USE_2D_STAB) {
62                         MovieTrackingMarker *marker = BKE_tracking_marker_get(track, framenr);
63
64                         minmax_v2v2_v2(min, max, marker->pos);
65
66                         ok = true;
67                 }
68
69                 track = track->next;
70         }
71
72         median[0] = (max[0] + min[0]) / 2.0f;
73         median[1] = (max[1] + min[1]) / 2.0f;
74
75         return ok;
76 }
77
78 /* Calculate stabilization data (translation, scale and rotation) from
79  * given median of first and current frame medians, tracking data and
80  * frame number.
81  *
82  * NOTE: frame number should be in clip space, not scene space
83  */
84 static void stabilization_calculate_data(MovieTracking *tracking, int framenr, int width, int height,
85                                          const float firstmedian[2], const float median[2],
86                                          float translation[2], float *scale, float *angle)
87 {
88         MovieTrackingStabilization *stab = &tracking->stabilization;
89
90         *scale = (stab->scale - 1.0f) * stab->scaleinf + 1.0f;
91         *angle = 0.0f;
92
93         translation[0] = (firstmedian[0] - median[0]) * width * (*scale);
94         translation[1] = (firstmedian[1] - median[1]) * height * (*scale);
95
96         mul_v2_fl(translation, stab->locinf);
97
98         if ((stab->flag & TRACKING_STABILIZE_ROTATION) && stab->rot_track && stab->rotinf) {
99                 MovieTrackingMarker *marker;
100                 float a[2], b[2];
101                 float x0 = (float)width / 2.0f, y0 = (float)height / 2.0f;
102                 float x = median[0] * width, y = median[1] * height;
103
104                 marker = BKE_tracking_marker_get(stab->rot_track, 1);
105                 sub_v2_v2v2(a, marker->pos, firstmedian);
106                 a[0] *= width;
107                 a[1] *= height;
108
109                 marker = BKE_tracking_marker_get(stab->rot_track, framenr);
110                 sub_v2_v2v2(b, marker->pos, median);
111                 b[0] *= width;
112                 b[1] *= height;
113
114                 *angle = -atan2f(a[0] * b[1] - a[1] * b[0], a[0] * b[0] + a[1] * b[1]);
115                 *angle *= stab->rotinf;
116
117                 /* convert to rotation around image center */
118                 translation[0] -= (x0 + (x - x0) * cosf(*angle) - (y - y0) * sinf(*angle) - x) * (*scale);
119                 translation[1] -= (y0 + (x - x0) * sinf(*angle) + (y - y0) * cosf(*angle) - y) * (*scale);
120         }
121 }
122
123 /* Calculate factor of a scale, which will eliminate black areas
124  * appearing on the frame caused by frame translation.
125  */
126 static float stabilization_calculate_autoscale_factor(MovieTracking *tracking, int width, int height)
127 {
128         float firstmedian[2];
129         MovieTrackingStabilization *stab = &tracking->stabilization;
130         float aspect = tracking->camera.pixel_aspect;
131
132         /* Early output if stabilization data is already up-to-date. */
133         if (stab->ok)
134                 return stab->scale;
135
136         /* See comment in BKE_tracking_stabilization_data_get about first frame. */
137         if (stabilization_median_point_get(tracking, 1, firstmedian)) {
138                 int sfra = INT_MAX, efra = INT_MIN, cfra;
139                 float scale = 1.0f;
140                 MovieTrackingTrack *track;
141
142                 stab->scale = 1.0f;
143
144                 /* Calculate frame range of tracks used for stabilization. */
145                 track = tracking->tracks.first;
146                 while (track) {
147                         if (track->flag & TRACK_USE_2D_STAB ||
148                             ((stab->flag & TRACKING_STABILIZE_ROTATION) && track == stab->rot_track))
149                         {
150                                 sfra = min_ii(sfra, track->markers[0].framenr);
151                                 efra = max_ii(efra, track->markers[track->markersnr - 1].framenr);
152                         }
153
154                         track = track->next;
155                 }
156
157                 /* For every frame we calculate scale factor needed to eliminate black
158                  * area and choose largest scale factor as final one.
159                  */
160                 for (cfra = sfra; cfra <= efra; cfra++) {
161                         float median[2];
162                         float translation[2], angle, tmp_scale;
163                         int i;
164                         float mat[4][4];
165                         float points[4][2] = {{0.0f, 0.0f}, {0.0f, height}, {width, height}, {width, 0.0f}};
166                         float si, co;
167
168                         stabilization_median_point_get(tracking, cfra, median);
169
170                         stabilization_calculate_data(tracking, cfra, width, height, firstmedian, median, translation,
171                                                      &tmp_scale, &angle);
172
173                         BKE_tracking_stabilization_data_to_mat4(width, height, aspect, translation, 1.0f, angle, mat);
174
175                         si = sinf(angle);
176                         co = cosf(angle);
177
178                         for (i = 0; i < 4; i++) {
179                                 int j;
180                                 float a[3] = {0.0f, 0.0f, 0.0f}, b[3] = {0.0f, 0.0f, 0.0f};
181
182                                 copy_v3_v3(a, points[i]);
183                                 copy_v3_v3(b, points[(i + 1) % 4]);
184
185                                 mul_m4_v3(mat, a);
186                                 mul_m4_v3(mat, b);
187
188                                 for (j = 0; j < 4; j++) {
189                                         float point[3] = {points[j][0], points[j][1], 0.0f};
190                                         float v1[3], v2[3];
191
192                                         sub_v3_v3v3(v1, b, a);
193                                         sub_v3_v3v3(v2, point, a);
194
195                                         if (cross_v2v2(v1, v2) >= 0.0f) {
196                                                 const float rotDx[4][2] = {{1.0f, 0.0f}, {0.0f, -1.0f}, {-1.0f, 0.0f}, {0.0f, 1.0f}};
197                                                 const float rotDy[4][2] = {{0.0f, 1.0f}, {1.0f, 0.0f}, {0.0f, -1.0f}, {-1.0f, 0.0f}};
198
199                                                 float dx = translation[0] * rotDx[j][0] + translation[1] * rotDx[j][1],
200                                                       dy = translation[0] * rotDy[j][0] + translation[1] * rotDy[j][1];
201
202                                                 float w, h, E, F, G, H, I, J, K, S;
203
204                                                 if (j % 2) {
205                                                         w = (float)height / 2.0f;
206                                                         h = (float)width / 2.0f;
207                                                 }
208                                                 else {
209                                                         w = (float)width / 2.0f;
210                                                         h = (float)height / 2.0f;
211                                                 }
212
213                                                 E = -w * co + h * si;
214                                                 F = -h * co - w * si;
215
216                                                 if ((i % 2) == (j % 2)) {
217                                                         G = -w * co - h * si;
218                                                         H = h * co - w * si;
219                                                 }
220                                                 else {
221                                                         G = w * co + h * si;
222                                                         H = -h * co + w * si;
223                                                 }
224
225                                                 I = F - H;
226                                                 J = G - E;
227                                                 K = G * F - E * H;
228
229                                                 S = (-w * I - h * J) / (dx * I + dy * J + K);
230
231                                                 scale = max_ff(scale, S);
232                                         }
233                                 }
234                         }
235                 }
236
237                 stab->scale = scale;
238
239                 if (stab->maxscale > 0.0f)
240                         stab->scale = min_ff(stab->scale, stab->maxscale);
241         }
242         else {
243                 stab->scale = 1.0f;
244         }
245
246         stab->ok = true;
247
248         return stab->scale;
249 }
250
251 /* Get stabilization data (translation, scaling and angle) for a given frame.
252  *
253  * NOTE: frame number should be in clip space, not scene space
254  */
255 void BKE_tracking_stabilization_data_get(MovieTracking *tracking, int framenr, int width, int height,
256                                          float translation[2], float *scale, float *angle)
257 {
258         float firstmedian[2], median[2];
259         MovieTrackingStabilization *stab = &tracking->stabilization;
260
261         /* Early output if stabilization is disabled. */
262         if ((stab->flag & TRACKING_2D_STABILIZATION) == 0) {
263                 zero_v2(translation);
264                 *scale = 1.0f;
265                 *angle = 0.0f;
266
267                 return;
268         }
269
270         /* Even if tracks does not start at frame 1, their position will
271          * be estimated at this frame, which will give reasonable result
272          * in most of cases.
273          *
274          * However, it's still better to replace this with real first
275          * frame number at which tracks are appearing.
276          */
277         if (stabilization_median_point_get(tracking, 1, firstmedian)) {
278                 stabilization_median_point_get(tracking, framenr, median);
279
280                 if ((stab->flag & TRACKING_AUTOSCALE) == 0)
281                         stab->scale = 1.0f;
282
283                 if (!stab->ok) {
284                         if (stab->flag & TRACKING_AUTOSCALE)
285                                 stabilization_calculate_autoscale_factor(tracking, width, height);
286
287                         stabilization_calculate_data(tracking, framenr, width, height, firstmedian, median,
288                                                      translation, scale, angle);
289
290                         stab->ok = true;
291                 }
292                 else {
293                         stabilization_calculate_data(tracking, framenr, width, height, firstmedian, median,
294                                                      translation, scale, angle);
295                 }
296         }
297         else {
298                 zero_v2(translation);
299                 *scale = 1.0f;
300                 *angle = 0.0f;
301         }
302 }
303
304 /* Stabilize given image buffer using stabilization data for
305  * a specified frame number.
306  *
307  * NOTE: frame number should be in clip space, not scene space
308  */
309 ImBuf *BKE_tracking_stabilize_frame(MovieTracking *tracking, int framenr, ImBuf *ibuf,
310                                     float translation[2], float *scale, float *angle)
311 {
312         float tloc[2], tscale, tangle;
313         MovieTrackingStabilization *stab = &tracking->stabilization;
314         ImBuf *tmpibuf;
315         int width = ibuf->x, height = ibuf->y;
316         float aspect = tracking->camera.pixel_aspect;
317         float mat[4][4];
318         int j, filter = tracking->stabilization.filter;
319         void (*interpolation)(struct ImBuf *, struct ImBuf *, float, float, int, int) = NULL;
320         int ibuf_flags;
321
322         if (translation)
323                 copy_v2_v2(tloc, translation);
324
325         if (scale)
326                 tscale = *scale;
327
328         /* Perform early output if no stabilization is used. */
329         if ((stab->flag & TRACKING_2D_STABILIZATION) == 0) {
330                 if (translation)
331                         zero_v2(translation);
332
333                 if (scale)
334                         *scale = 1.0f;
335
336                 if (angle)
337                         *angle = 0.0f;
338
339                 return ibuf;
340         }
341
342         /* Allocate frame for stabilization result. */
343         ibuf_flags = 0;
344         if (ibuf->rect)
345                 ibuf_flags |= IB_rect;
346         if (ibuf->rect_float)
347                 ibuf_flags |= IB_rectfloat;
348
349         tmpibuf = IMB_allocImBuf(ibuf->x, ibuf->y, ibuf->planes, ibuf_flags);
350
351         /* Calculate stabilization matrix. */
352         BKE_tracking_stabilization_data_get(tracking, framenr, width, height, tloc, &tscale, &tangle);
353         BKE_tracking_stabilization_data_to_mat4(ibuf->x, ibuf->y, aspect, tloc, tscale, tangle, mat);
354         invert_m4(mat);
355
356         if (filter == TRACKING_FILTER_NEAREST)
357                 interpolation = nearest_interpolation;
358         else if (filter == TRACKING_FILTER_BILINEAR)
359                 interpolation = bilinear_interpolation;
360         else if (filter == TRACKING_FILTER_BICUBIC)
361                 interpolation = bicubic_interpolation;
362         else
363                 /* fallback to default interpolation method */
364                 interpolation = nearest_interpolation;
365
366         /* This function is only used for display in clip editor and
367          * sequencer only, which would only benefit of using threads
368          * here.
369          *
370          * But need to keep an eye on this if the function will be
371          * used in other cases.
372          */
373 #pragma omp parallel for if (tmpibuf->y > 128)
374         for (j = 0; j < tmpibuf->y; j++) {
375                 int i;
376                 for (i = 0; i < tmpibuf->x; i++) {
377                         float vec[3] = {i, j, 0.0f};
378
379                         mul_v3_m4v3(vec, mat, vec);
380
381                         interpolation(ibuf, tmpibuf, vec[0], vec[1], i, j);
382                 }
383         }
384
385         if (tmpibuf->rect_float)
386                 tmpibuf->userflags |= IB_RECT_INVALID;
387
388         if (translation)
389                 copy_v2_v2(translation, tloc);
390
391         if (scale)
392                 *scale = tscale;
393
394         if (angle)
395                 *angle = tangle;
396
397         return tmpibuf;
398 }
399
400 /* Get 4x4 transformation matrix which corresponds to
401  * stabilization data and used for easy coordinate
402  * transformation.
403  *
404  * NOTE: The reason it is 4x4 matrix is because it's
405  *       used for OpenGL drawing directly.
406  */
407 void BKE_tracking_stabilization_data_to_mat4(int width, int height, float aspect,
408                                              float translation[2], float scale, float angle,
409                                              float mat[4][4])
410 {
411         float translation_mat[4][4], rotation_mat[4][4], scale_mat[4][4],
412               center_mat[4][4], inv_center_mat[4][4],
413               aspect_mat[4][4], inv_aspect_mat[4][4];
414         float scale_vector[3] = {scale, scale, scale};
415
416         unit_m4(translation_mat);
417         unit_m4(rotation_mat);
418         unit_m4(scale_mat);
419         unit_m4(center_mat);
420         unit_m4(aspect_mat);
421
422         /* aspect ratio correction matrix */
423         aspect_mat[0][0] = 1.0f / aspect;
424         invert_m4_m4(inv_aspect_mat, aspect_mat);
425
426         /* image center as rotation center
427          *
428          * Rotation matrix is constructing in a way rotation happens around image center,
429          * and it's matter of calculating translation in a way, that applying translation
430          * after rotation would make it so rotation happens around median point of tracks
431          * used for translation stabilization.
432          */
433         center_mat[3][0] = (float)width / 2.0f;
434         center_mat[3][1] = (float)height / 2.0f;
435         invert_m4_m4(inv_center_mat, center_mat);
436
437         size_to_mat4(scale_mat, scale_vector);       /* scale matrix */
438         add_v2_v2(translation_mat[3], translation);  /* translation matrix */
439         rotate_m4(rotation_mat, 'Z', angle);         /* rotation matrix */
440
441         /* compose transformation matrix */
442         mul_m4_series(mat, translation_mat, center_mat, aspect_mat, rotation_mat, inv_aspect_mat,
443                      scale_mat, inv_center_mat);
444 }