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