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 */ 41static 42double 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 54typedef double MATRIX[4][4]; 55 56static 57double 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 89static 90int 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 179int 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 229int 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*/ 302void 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) ) 311double 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 338int 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