1 /*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /*
18 #sourcefile vpmotion/vp_motionmodel.c
19 #category motion-model
20 *
21 * Copyright 1998 Sarnoff Corporation
22 * All Rights Reserved
23 *
24 * Modification History
25 * Date: 02/14/98
26 * Author: supuns
27 * Shop Order: 17xxx
28 * @(#) $Id: vp_motionmodel.c,v 1.4 2011/06/17 14:04:33 mbansal Exp $
29 */
30
31 /*
32 * ===================================================================
33 * Include Files
34 */
35
36 #include <string.h> /* memmove */
37 #include <math.h>
38 #include "vp_motionmodel.h"
39
40 /* Static Functions */
41 static
Det3(double m[3][3])42 double Det3(double m[3][3])
43 {
44 double result;
45
46 result =
47 m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] +
48 m[0][2]*m[1][0]*m[2][1] - m[0][2]*m[1][1]*m[2][0] -
49 m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2];
50
51 return(result);
52 }
53
54 typedef double MATRIX[4][4];
55
56 static
Det4(MATRIX m)57 double Det4(MATRIX m)
58 {
59 /* ==> This is a poor implementation of determinant.
60 Writing the formula out in closed form is unnecessarily complicated
61 and mistakes are easy to make. */
62 double result;
63
64 result=
65 m[0][3] *m[1][2] *m[2][1] *m[3][0] - m[0][2] *m[1][3] *m[2][1] *m[3][0] - m[0][3] *m[1][1] *m[2][2] *m[3][0] +
66 m[0][1] *m[1][3] *m[2][2] *m[3][0] + m[0][2] *m[1][1] *m[2][3] *m[3][0] - m[0][1] *m[1][2] *m[2][3] *m[3][0] - m[0][3] *m[1][2] *m[2][0] *m[3][1] +
67 m[0][2] *m[1][3] *m[2][0] *m[3][1] + m[0][3] *m[1][0] *m[2][2] *m[3][1] - m[0][0] *m[1][3] *m[2][2] *m[3][1] - m[0][2] *m[1][0] *m[2][3] *m[3][1] +
68 m[0][0] *m[1][2] *m[2][3] *m[3][1] + m[0][3] *m[1][1] *m[2][0] *m[3][2] - m[0][1] *m[1][3] *m[2][0] *m[3][2] - m[0][3] *m[1][0] *m[2][1] *m[3][2] +
69 m[0][0] *m[1][3] *m[2][1] *m[3][2] + m[0][1] *m[1][0] *m[2][3] *m[3][2] - m[0][0] *m[1][1] *m[2][3] *m[3][2] - m[0][2] *m[1][1] *m[2][0] *m[3][3] +
70 m[0][1] *m[1][2] *m[2][0] *m[3][3] + m[0][2] *m[1][0] *m[2][1] *m[3][3] - m[0][0] *m[1][2] *m[2][1] *m[3][3] - m[0][1] *m[1][0] *m[2][2] *m[3][3] +
71 m[0][0] *m[1][1] *m[2][2] *m[3][3];
72 /*
73 m[0][0]*m[1][1]*m[2][2]*m[3][3]-m[0][1]*m[1][0]*m[2][2]*m[3][3]+
74 m[0][1]*m[1][2]*m[2][0]*m[3][3]-m[0][2]*m[1][1]*m[2][0]*m[3][3]+
75 m[0][2]*m[1][0]*m[2][1]*m[3][3]-m[0][0]*m[1][2]*m[2][1]*m[3][3]+
76 m[0][0]*m[1][2]*m[2][3]*m[3][1]-m[0][2]*m[1][0]*m[2][3]*m[3][1]+
77 m[0][2]*m[1][3]*m[2][0]*m[3][1]-m[0][3]*m[1][2]*m[2][0]*m[3][1]+
78 m[0][3]*m[1][0]*m[2][2]*m[3][1]-m[0][0]*m[1][3]*m[2][2]*m[3][1]+
79 m[0][0]*m[1][3]*m[2][1]*m[3][2]-m[0][3]*m[1][0]*m[2][3]*m[3][2]+
80 m[0][1]*m[1][0]*m[2][3]*m[3][2]-m[0][0]*m[1][1]*m[2][0]*m[3][2]+
81 m[0][3]*m[1][1]*m[2][0]*m[3][2]-m[0][1]*m[1][3]*m[2][1]*m[3][2]+
82 m[0][1]*m[1][3]*m[2][2]*m[3][0]-m[0][3]*m[1][1]*m[2][2]*m[3][0]+
83 m[0][2]*m[1][1]*m[2][3]*m[3][0]-m[0][1]*m[1][2]*m[2][3]*m[3][0]+
84 m[0][3]*m[1][2]*m[2][1]*m[3][0]-m[0][2]*m[1][3]*m[2][1]*m[3][0];
85 */
86 return(result);
87 }
88
89 static
inv4Mat(const VP_MOTION * in,VP_MOTION * out)90 int inv4Mat(const VP_MOTION* in, VP_MOTION* out)
91 {
92 /* ==> This is a poor implementation of inversion. The determinant
93 method is O(N^4), i.e. unnecessarily slow, and not numerically accurate.
94 The real complexity of inversion is O(N^3), and is best done using
95 LU decomposition. */
96
97 MATRIX inmat,outmat;
98 int i, j, k, l, m, n,ntemp;
99 double mat[3][3], indet, temp;
100
101 /* check for non-empty structures structure */
102 if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) {
103 return 1;
104 }
105
106 for(k=0,i=0;i<4;i++)
107 for(j=0;j<4;j++,k++)
108 inmat[i][j]=(double)in->par[k];
109
110 indet = Det4(inmat);
111 if (indet==0) return(-1);
112
113 for (i=0;i<4;i++) {
114 for (j=0;j<4;j++) {
115 m = 0;
116 for (k=0;k<4;k++) {
117 if (i != k) {
118 n = 0;
119 for (l=0;l<4;l++)
120 if (j != l) {
121 mat[m][n] = inmat[k][l];
122 n++;
123 }
124 m++;
125 }
126 }
127
128 temp = -1.;
129 ntemp = (i +j ) %2;
130 if( ntemp == 0) temp = 1.;
131
132 outmat[j][i] = temp * Det3(mat)/indet;
133 }
134 }
135
136 for(k=0,i=0;i<4;i++)
137 for(j=0;j<4;j++,k++)
138 out->par[k]=(VP_PAR)outmat[i][j]; /*lint !e771*/
139
140 return(0);
141 }
142
143 /*
144 * ===================================================================
145 * Public Functions
146 #htmlstart
147 */
148
149 /*
150 * ===================================================================
151 #fn vp_invert_motion
152 #ft invert a motion
153 #fd DEFINITION
154 Bool
155 vp_invert_motion(const VP_MOTION* in,VP_MOTION* out)
156 #fd PURPOSE
157 This inverts the motion given in 'in'.
158 All motion models upto VP_MOTION_SEMI_PROJ_3D are supported.
159 It is assumed that the all 16 parameters are properly
160 initialized although you may not be using them. You could
161 use the VP_KEEP_ macro's defined in vp_motionmodel.h to set
162 the un-initialized parameters. This uses a 4x4 matrix invertion
163 function internally.
164 It is SAFE to pass the same pointer as both the 'in' and 'out'
165 parameters.
166 #fd INPUTS
167 in - input motion
168 #fd OUTPUTS
169 out - output inverted motion. If singular matrix uninitialized.
170 if MWW(in) is non-zero it is also normalized.
171 #fd RETURNS
172 FALSE - matrix is singular or motion model not supported
173 TRUE - otherwise
174 #fd SIDE EFFECTS
175 None
176 #endfn
177 */
178
vp_invert_motion(const VP_MOTION * in,VP_MOTION * out)179 int vp_invert_motion(const VP_MOTION* in,VP_MOTION* out)
180 {
181 int refid;
182
183 /* check for non-empty structures structure */
184 if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) {
185 return FALSE;
186 }
187
188 if (in->type>VP_MOTION_SEMI_PROJ_3D) {
189 return FALSE;
190 }
191
192 if (inv4Mat(in,out)<0)
193 return FALSE;
194
195 /*VP_NORMALIZE(*out);*/
196 out->type = in->type;
197 refid=in->refid;
198 out->refid=in->insid;
199 out->insid=refid;
200 return TRUE;
201 }
202
203 /*
204 * ===================================================================
205 #fn vp_cascade_motion
206 #ft Cascade two motion transforms
207 #fd DEFINITION
208 Bool
209 vp_cascade_motion(const VP_MOTION* InAB,const VP_MOTION* InBC,VP_MOTION* OutAC)
210 #fd PURPOSE
211 Given Motion Transforms A->B and B->C, this function will
212 generate a New Motion that describes the transformation
213 from A->C.
214 More specifically, OutAC = InBC * InAC.
215 This function works ok if InAB,InBC and OutAC are the same pointer.
216 #fd INPUTS
217 InAB - First Motion Transform
218 InBC - Second Motion Tranform
219 #fd OUTPUTS
220 OutAC - Cascaded Motion
221 #fd RETURNS
222 FALSE - motion model not supported
223 TRUE - otherwise
224 #fd SIDE EFFECTS
225 None
226 #endfn
227 */
228
vp_cascade_motion(const VP_MOTION * InA,const VP_MOTION * InB,VP_MOTION * Out)229 int vp_cascade_motion(const VP_MOTION* InA, const VP_MOTION* InB,VP_MOTION* Out)
230 {
231 /* ==> This is a poor implementation of matrix multiplication.
232 Writing the formula out in closed form is unnecessarily complicated
233 and mistakes are easy to make. */
234 VP_PAR mxx,mxy,mxz,mxw;
235 VP_PAR myx,myy,myz,myw;
236 VP_PAR mzx,mzy,mzz,mzw;
237 VP_PAR mwx,mwy,mwz,mww;
238
239 /* check for non-empty structures structure */
240 if (((VP_MOTION *) NULL == InA) || ((VP_MOTION *) NULL == InB) ||
241 ((VP_MOTION *) NULL == Out)) {
242 return FALSE;
243 }
244
245 if (InA->type>VP_MOTION_PROJ_3D) {
246 return FALSE;
247 }
248
249 if (InB->type>VP_MOTION_PROJ_3D) {
250 return FALSE;
251 }
252
253 mxx = MXX(*InB)*MXX(*InA)+MXY(*InB)*MYX(*InA)+MXZ(*InB)*MZX(*InA)+MXW(*InB)*MWX(*InA);
254 mxy = MXX(*InB)*MXY(*InA)+MXY(*InB)*MYY(*InA)+MXZ(*InB)*MZY(*InA)+MXW(*InB)*MWY(*InA);
255 mxz = MXX(*InB)*MXZ(*InA)+MXY(*InB)*MYZ(*InA)+MXZ(*InB)*MZZ(*InA)+MXW(*InB)*MWZ(*InA);
256 mxw = MXX(*InB)*MXW(*InA)+MXY(*InB)*MYW(*InA)+MXZ(*InB)*MZW(*InA)+MXW(*InB)*MWW(*InA);
257 myx = MYX(*InB)*MXX(*InA)+MYY(*InB)*MYX(*InA)+MYZ(*InB)*MZX(*InA)+MYW(*InB)*MWX(*InA);
258 myy = MYX(*InB)*MXY(*InA)+MYY(*InB)*MYY(*InA)+MYZ(*InB)*MZY(*InA)+MYW(*InB)*MWY(*InA);
259 myz = MYX(*InB)*MXZ(*InA)+MYY(*InB)*MYZ(*InA)+MYZ(*InB)*MZZ(*InA)+MYW(*InB)*MWZ(*InA);
260 myw = MYX(*InB)*MXW(*InA)+MYY(*InB)*MYW(*InA)+MYZ(*InB)*MZW(*InA)+MYW(*InB)*MWW(*InA);
261 mzx = MZX(*InB)*MXX(*InA)+MZY(*InB)*MYX(*InA)+MZZ(*InB)*MZX(*InA)+MZW(*InB)*MWX(*InA);
262 mzy = MZX(*InB)*MXY(*InA)+MZY(*InB)*MYY(*InA)+MZZ(*InB)*MZY(*InA)+MZW(*InB)*MWY(*InA);
263 mzz = MZX(*InB)*MXZ(*InA)+MZY(*InB)*MYZ(*InA)+MZZ(*InB)*MZZ(*InA)+MZW(*InB)*MWZ(*InA);
264 mzw = MZX(*InB)*MXW(*InA)+MZY(*InB)*MYW(*InA)+MZZ(*InB)*MZW(*InA)+MZW(*InB)*MWW(*InA);
265 mwx = MWX(*InB)*MXX(*InA)+MWY(*InB)*MYX(*InA)+MWZ(*InB)*MZX(*InA)+MWW(*InB)*MWX(*InA);
266 mwy = MWX(*InB)*MXY(*InA)+MWY(*InB)*MYY(*InA)+MWZ(*InB)*MZY(*InA)+MWW(*InB)*MWY(*InA);
267 mwz = MWX(*InB)*MXZ(*InA)+MWY(*InB)*MYZ(*InA)+MWZ(*InB)*MZZ(*InA)+MWW(*InB)*MWZ(*InA);
268 mww = MWX(*InB)*MXW(*InA)+MWY(*InB)*MYW(*InA)+MWZ(*InB)*MZW(*InA)+MWW(*InB)*MWW(*InA);
269
270 MXX(*Out)=mxx; MXY(*Out)=mxy; MXZ(*Out)=mxz; MXW(*Out)=mxw;
271 MYX(*Out)=myx; MYY(*Out)=myy; MYZ(*Out)=myz; MYW(*Out)=myw;
272 MZX(*Out)=mzx; MZY(*Out)=mzy; MZZ(*Out)=mzz; MZW(*Out)=mzw;
273 MWX(*Out)=mwx; MWY(*Out)=mwy; MWZ(*Out)=mwz; MWW(*Out)=mww;
274 /* VP_NORMALIZE(*Out); */
275 Out->type= (InA->type > InB->type) ? InA->type : InB->type;
276 Out->refid=InA->refid;
277 Out->insid=InB->insid;
278
279 return TRUE;
280 }
281
282 /*
283 * ===================================================================
284 #fn vp_copy_motion
285 #ft Copies the source motion to the destination motion.
286 #fd DEFINITION
287 void
288 vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst)
289 #fd PURPOSE
290 Copies the source motion to the destination motion.
291 It is OK if src == dst.
292 NOTE THAT THE SOURCE IS THE FIRST ARGUMENT.
293 This is different from some of the other VP
294 copy functions.
295 #fd INPUTS
296 src is the source motion
297 dst is the destination motion
298 #fd RETURNS
299 void
300 #endfn
301 */
vp_copy_motion(const VP_MOTION * src,VP_MOTION * dst)302 void vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst)
303 {
304 /* Use memmove rather than memcpy because it handles overlapping memory
305 OK. */
306 memmove(dst, src, sizeof(VP_MOTION));
307 return;
308 } /* vp_copy_motion() */
309
310 #define VP_SQR(x) ( (x)*(x) )
vp_motion_cornerdiff(const VP_MOTION * mot_a,const VP_MOTION * mot_b,int xo,int yo,int w,int h)311 double vp_motion_cornerdiff(const VP_MOTION *mot_a, const VP_MOTION *mot_b,
312 int xo, int yo, int w, int h)
313 {
314 double ax1, ay1, ax2, ay2, ax3, ay3, ax4, ay4;
315 double bx1, by1, bx2, by2, bx3, by3, bx4, by4;
316 double err;
317
318 /*lint -e639 -e632 -e633 */
319 VP_WARP_POINT_2D(xo, yo, *mot_a, ax1, ay1);
320 VP_WARP_POINT_2D(xo+w-1, yo, *mot_a, ax2, ay2);
321 VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_a, ax3, ay3);
322 VP_WARP_POINT_2D(xo, yo+h-1, *mot_a, ax4, ay4);
323 VP_WARP_POINT_2D(xo, yo, *mot_b, bx1, by1);
324 VP_WARP_POINT_2D(xo+w-1, yo, *mot_b, bx2, by2);
325 VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_b, bx3, by3);
326 VP_WARP_POINT_2D(xo, yo+h-1, *mot_b, bx4, by4);
327 /*lint +e639 +e632 +e633 */
328
329 err = 0;
330 err += (VP_SQR(ax1 - bx1) + VP_SQR(ay1 - by1));
331 err += (VP_SQR(ax2 - bx2) + VP_SQR(ay2 - by2));
332 err += (VP_SQR(ax3 - bx3) + VP_SQR(ay3 - by3));
333 err += (VP_SQR(ax4 - bx4) + VP_SQR(ay4 - by4));
334
335 return(sqrt(err));
336 }
337
vp_zoom_motion2d(VP_MOTION * in,VP_MOTION * out,int n,int w,int h,double zoom)338 int vp_zoom_motion2d(VP_MOTION* in, VP_MOTION* out,
339 int n, int w, int h, double zoom)
340 {
341 int ii;
342 VP_PAR inv_zoom;
343 VP_PAR cx, cy;
344 VP_MOTION R2r,R2f;
345 VP_MOTION *res;
346
347 /* check for non-empty structures structure */
348 if (((VP_MOTION *) NULL == in)||(zoom <= 0.0)||(w <= 0)||(h <= 0)) {
349 return FALSE;
350 }
351
352 /* ==> Not sure why the special case of out=NULL is necessary. Why couldn't
353 the caller just pass the same pointer for both in and out? */
354 res = ((VP_MOTION *) NULL == out)?in:out;
355
356 cx = (VP_PAR) (w/2.0);
357 cy = (VP_PAR) (h/2.0);
358
359 VP_MOTION_ID(R2r);
360 inv_zoom = (VP_PAR)(1.0/zoom);
361 MXX(R2r) = inv_zoom;
362 MYY(R2r) = inv_zoom;
363 MXW(R2r)=cx*(((VP_PAR)1.0) - inv_zoom);
364 MYW(R2r)=cy*(((VP_PAR)1.0) - inv_zoom);
365
366 VP_KEEP_AFFINE_2D(R2r);
367
368 for(ii=0;ii<n;ii++) {
369 (void) vp_cascade_motion(&R2r,in+ii,&R2f);
370 res[ii]=R2f;
371 }
372
373 return TRUE;
374 } /* vp_zoom_motion2d() */
375
376 /* =================================================================== */
377 /* end vp_motionmodel.c */
378