| /* |
| * Copyright (C) 2011 The Android Open Source Project |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| /* |
| #sourcefile vpmotion/vp_motionmodel.c |
| #category motion-model |
| * |
| * Copyright 1998 Sarnoff Corporation |
| * All Rights Reserved |
| * |
| * Modification History |
| * Date: 02/14/98 |
| * Author: supuns |
| * Shop Order: 17xxx |
| * @(#) $Id: vp_motionmodel.c,v 1.4 2011/06/17 14:04:33 mbansal Exp $ |
| */ |
| |
| /* |
| * =================================================================== |
| * Include Files |
| */ |
| |
| #include <string.h> /* memmove */ |
| #include <math.h> |
| #include "vp_motionmodel.h" |
| |
| /* Static Functions */ |
| static |
| double Det3(double m[3][3]) |
| { |
| double result; |
| |
| result = |
| m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + |
| m[0][2]*m[1][0]*m[2][1] - m[0][2]*m[1][1]*m[2][0] - |
| m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2]; |
| |
| return(result); |
| } |
| |
| typedef double MATRIX[4][4]; |
| |
| static |
| double Det4(MATRIX m) |
| { |
| /* ==> This is a poor implementation of determinant. |
| Writing the formula out in closed form is unnecessarily complicated |
| and mistakes are easy to make. */ |
| double result; |
| |
| result= |
| 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] + |
| 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] + |
| 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] + |
| 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] + |
| 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] + |
| 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] + |
| m[0][0] *m[1][1] *m[2][2] *m[3][3]; |
| /* |
| m[0][0]*m[1][1]*m[2][2]*m[3][3]-m[0][1]*m[1][0]*m[2][2]*m[3][3]+ |
| m[0][1]*m[1][2]*m[2][0]*m[3][3]-m[0][2]*m[1][1]*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][0]*m[1][2]*m[2][3]*m[3][1]-m[0][2]*m[1][0]*m[2][3]*m[3][1]+ |
| m[0][2]*m[1][3]*m[2][0]*m[3][1]-m[0][3]*m[1][2]*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][0]*m[1][3]*m[2][1]*m[3][2]-m[0][3]*m[1][0]*m[2][3]*m[3][2]+ |
| m[0][1]*m[1][0]*m[2][3]*m[3][2]-m[0][0]*m[1][1]*m[2][0]*m[3][2]+ |
| m[0][3]*m[1][1]*m[2][0]*m[3][2]-m[0][1]*m[1][3]*m[2][1]*m[3][2]+ |
| m[0][1]*m[1][3]*m[2][2]*m[3][0]-m[0][3]*m[1][1]*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][1]*m[3][0]-m[0][2]*m[1][3]*m[2][1]*m[3][0]; |
| */ |
| return(result); |
| } |
| |
| static |
| int inv4Mat(const VP_MOTION* in, VP_MOTION* out) |
| { |
| /* ==> This is a poor implementation of inversion. The determinant |
| method is O(N^4), i.e. unnecessarily slow, and not numerically accurate. |
| The real complexity of inversion is O(N^3), and is best done using |
| LU decomposition. */ |
| |
| MATRIX inmat,outmat; |
| int i, j, k, l, m, n,ntemp; |
| double mat[3][3], indet, temp; |
| |
| /* check for non-empty structures structure */ |
| if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) { |
| return 1; |
| } |
| |
| for(k=0,i=0;i<4;i++) |
| for(j=0;j<4;j++,k++) |
| inmat[i][j]=(double)in->par[k]; |
| |
| indet = Det4(inmat); |
| if (indet==0) return(-1); |
| |
| for (i=0;i<4;i++) { |
| for (j=0;j<4;j++) { |
| m = 0; |
| for (k=0;k<4;k++) { |
| if (i != k) { |
| n = 0; |
| for (l=0;l<4;l++) |
| if (j != l) { |
| mat[m][n] = inmat[k][l]; |
| n++; |
| } |
| m++; |
| } |
| } |
| |
| temp = -1.; |
| ntemp = (i +j ) %2; |
| if( ntemp == 0) temp = 1.; |
| |
| outmat[j][i] = temp * Det3(mat)/indet; |
| } |
| } |
| |
| for(k=0,i=0;i<4;i++) |
| for(j=0;j<4;j++,k++) |
| out->par[k]=(VP_PAR)outmat[i][j]; /*lint !e771*/ |
| |
| return(0); |
| } |
| |
| /* |
| * =================================================================== |
| * Public Functions |
| #htmlstart |
| */ |
| |
| /* |
| * =================================================================== |
| #fn vp_invert_motion |
| #ft invert a motion |
| #fd DEFINITION |
| Bool |
| vp_invert_motion(const VP_MOTION* in,VP_MOTION* out) |
| #fd PURPOSE |
| This inverts the motion given in 'in'. |
| All motion models upto VP_MOTION_SEMI_PROJ_3D are supported. |
| It is assumed that the all 16 parameters are properly |
| initialized although you may not be using them. You could |
| use the VP_KEEP_ macro's defined in vp_motionmodel.h to set |
| the un-initialized parameters. This uses a 4x4 matrix invertion |
| function internally. |
| It is SAFE to pass the same pointer as both the 'in' and 'out' |
| parameters. |
| #fd INPUTS |
| in - input motion |
| #fd OUTPUTS |
| out - output inverted motion. If singular matrix uninitialized. |
| if MWW(in) is non-zero it is also normalized. |
| #fd RETURNS |
| FALSE - matrix is singular or motion model not supported |
| TRUE - otherwise |
| #fd SIDE EFFECTS |
| None |
| #endfn |
| */ |
| |
| int vp_invert_motion(const VP_MOTION* in,VP_MOTION* out) |
| { |
| int refid; |
| |
| /* check for non-empty structures structure */ |
| if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) { |
| return FALSE; |
| } |
| |
| if (in->type>VP_MOTION_SEMI_PROJ_3D) { |
| return FALSE; |
| } |
| |
| if (inv4Mat(in,out)<0) |
| return FALSE; |
| |
| /*VP_NORMALIZE(*out);*/ |
| out->type = in->type; |
| refid=in->refid; |
| out->refid=in->insid; |
| out->insid=refid; |
| return TRUE; |
| } |
| |
| /* |
| * =================================================================== |
| #fn vp_cascade_motion |
| #ft Cascade two motion transforms |
| #fd DEFINITION |
| Bool |
| vp_cascade_motion(const VP_MOTION* InAB,const VP_MOTION* InBC,VP_MOTION* OutAC) |
| #fd PURPOSE |
| Given Motion Transforms A->B and B->C, this function will |
| generate a New Motion that describes the transformation |
| from A->C. |
| More specifically, OutAC = InBC * InAC. |
| This function works ok if InAB,InBC and OutAC are the same pointer. |
| #fd INPUTS |
| InAB - First Motion Transform |
| InBC - Second Motion Tranform |
| #fd OUTPUTS |
| OutAC - Cascaded Motion |
| #fd RETURNS |
| FALSE - motion model not supported |
| TRUE - otherwise |
| #fd SIDE EFFECTS |
| None |
| #endfn |
| */ |
| |
| int vp_cascade_motion(const VP_MOTION* InA, const VP_MOTION* InB,VP_MOTION* Out) |
| { |
| /* ==> This is a poor implementation of matrix multiplication. |
| Writing the formula out in closed form is unnecessarily complicated |
| and mistakes are easy to make. */ |
| VP_PAR mxx,mxy,mxz,mxw; |
| VP_PAR myx,myy,myz,myw; |
| VP_PAR mzx,mzy,mzz,mzw; |
| VP_PAR mwx,mwy,mwz,mww; |
| |
| /* check for non-empty structures structure */ |
| if (((VP_MOTION *) NULL == InA) || ((VP_MOTION *) NULL == InB) || |
| ((VP_MOTION *) NULL == Out)) { |
| return FALSE; |
| } |
| |
| if (InA->type>VP_MOTION_PROJ_3D) { |
| return FALSE; |
| } |
| |
| if (InB->type>VP_MOTION_PROJ_3D) { |
| return FALSE; |
| } |
| |
| mxx = MXX(*InB)*MXX(*InA)+MXY(*InB)*MYX(*InA)+MXZ(*InB)*MZX(*InA)+MXW(*InB)*MWX(*InA); |
| mxy = MXX(*InB)*MXY(*InA)+MXY(*InB)*MYY(*InA)+MXZ(*InB)*MZY(*InA)+MXW(*InB)*MWY(*InA); |
| mxz = MXX(*InB)*MXZ(*InA)+MXY(*InB)*MYZ(*InA)+MXZ(*InB)*MZZ(*InA)+MXW(*InB)*MWZ(*InA); |
| mxw = MXX(*InB)*MXW(*InA)+MXY(*InB)*MYW(*InA)+MXZ(*InB)*MZW(*InA)+MXW(*InB)*MWW(*InA); |
| myx = MYX(*InB)*MXX(*InA)+MYY(*InB)*MYX(*InA)+MYZ(*InB)*MZX(*InA)+MYW(*InB)*MWX(*InA); |
| myy = MYX(*InB)*MXY(*InA)+MYY(*InB)*MYY(*InA)+MYZ(*InB)*MZY(*InA)+MYW(*InB)*MWY(*InA); |
| myz = MYX(*InB)*MXZ(*InA)+MYY(*InB)*MYZ(*InA)+MYZ(*InB)*MZZ(*InA)+MYW(*InB)*MWZ(*InA); |
| myw = MYX(*InB)*MXW(*InA)+MYY(*InB)*MYW(*InA)+MYZ(*InB)*MZW(*InA)+MYW(*InB)*MWW(*InA); |
| mzx = MZX(*InB)*MXX(*InA)+MZY(*InB)*MYX(*InA)+MZZ(*InB)*MZX(*InA)+MZW(*InB)*MWX(*InA); |
| mzy = MZX(*InB)*MXY(*InA)+MZY(*InB)*MYY(*InA)+MZZ(*InB)*MZY(*InA)+MZW(*InB)*MWY(*InA); |
| mzz = MZX(*InB)*MXZ(*InA)+MZY(*InB)*MYZ(*InA)+MZZ(*InB)*MZZ(*InA)+MZW(*InB)*MWZ(*InA); |
| mzw = MZX(*InB)*MXW(*InA)+MZY(*InB)*MYW(*InA)+MZZ(*InB)*MZW(*InA)+MZW(*InB)*MWW(*InA); |
| mwx = MWX(*InB)*MXX(*InA)+MWY(*InB)*MYX(*InA)+MWZ(*InB)*MZX(*InA)+MWW(*InB)*MWX(*InA); |
| mwy = MWX(*InB)*MXY(*InA)+MWY(*InB)*MYY(*InA)+MWZ(*InB)*MZY(*InA)+MWW(*InB)*MWY(*InA); |
| mwz = MWX(*InB)*MXZ(*InA)+MWY(*InB)*MYZ(*InA)+MWZ(*InB)*MZZ(*InA)+MWW(*InB)*MWZ(*InA); |
| mww = MWX(*InB)*MXW(*InA)+MWY(*InB)*MYW(*InA)+MWZ(*InB)*MZW(*InA)+MWW(*InB)*MWW(*InA); |
| |
| MXX(*Out)=mxx; MXY(*Out)=mxy; MXZ(*Out)=mxz; MXW(*Out)=mxw; |
| MYX(*Out)=myx; MYY(*Out)=myy; MYZ(*Out)=myz; MYW(*Out)=myw; |
| MZX(*Out)=mzx; MZY(*Out)=mzy; MZZ(*Out)=mzz; MZW(*Out)=mzw; |
| MWX(*Out)=mwx; MWY(*Out)=mwy; MWZ(*Out)=mwz; MWW(*Out)=mww; |
| /* VP_NORMALIZE(*Out); */ |
| Out->type= (InA->type > InB->type) ? InA->type : InB->type; |
| Out->refid=InA->refid; |
| Out->insid=InB->insid; |
| |
| return TRUE; |
| } |
| |
| /* |
| * =================================================================== |
| #fn vp_copy_motion |
| #ft Copies the source motion to the destination motion. |
| #fd DEFINITION |
| void |
| vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst) |
| #fd PURPOSE |
| Copies the source motion to the destination motion. |
| It is OK if src == dst. |
| NOTE THAT THE SOURCE IS THE FIRST ARGUMENT. |
| This is different from some of the other VP |
| copy functions. |
| #fd INPUTS |
| src is the source motion |
| dst is the destination motion |
| #fd RETURNS |
| void |
| #endfn |
| */ |
| void vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst) |
| { |
| /* Use memmove rather than memcpy because it handles overlapping memory |
| OK. */ |
| memmove(dst, src, sizeof(VP_MOTION)); |
| return; |
| } /* vp_copy_motion() */ |
| |
| #define VP_SQR(x) ( (x)*(x) ) |
| double vp_motion_cornerdiff(const VP_MOTION *mot_a, const VP_MOTION *mot_b, |
| int xo, int yo, int w, int h) |
| { |
| double ax1, ay1, ax2, ay2, ax3, ay3, ax4, ay4; |
| double bx1, by1, bx2, by2, bx3, by3, bx4, by4; |
| double err; |
| |
| /*lint -e639 -e632 -e633 */ |
| VP_WARP_POINT_2D(xo, yo, *mot_a, ax1, ay1); |
| VP_WARP_POINT_2D(xo+w-1, yo, *mot_a, ax2, ay2); |
| VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_a, ax3, ay3); |
| VP_WARP_POINT_2D(xo, yo+h-1, *mot_a, ax4, ay4); |
| VP_WARP_POINT_2D(xo, yo, *mot_b, bx1, by1); |
| VP_WARP_POINT_2D(xo+w-1, yo, *mot_b, bx2, by2); |
| VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_b, bx3, by3); |
| VP_WARP_POINT_2D(xo, yo+h-1, *mot_b, bx4, by4); |
| /*lint +e639 +e632 +e633 */ |
| |
| err = 0; |
| err += (VP_SQR(ax1 - bx1) + VP_SQR(ay1 - by1)); |
| err += (VP_SQR(ax2 - bx2) + VP_SQR(ay2 - by2)); |
| err += (VP_SQR(ax3 - bx3) + VP_SQR(ay3 - by3)); |
| err += (VP_SQR(ax4 - bx4) + VP_SQR(ay4 - by4)); |
| |
| return(sqrt(err)); |
| } |
| |
| int vp_zoom_motion2d(VP_MOTION* in, VP_MOTION* out, |
| int n, int w, int h, double zoom) |
| { |
| int ii; |
| VP_PAR inv_zoom; |
| VP_PAR cx, cy; |
| VP_MOTION R2r,R2f; |
| VP_MOTION *res; |
| |
| /* check for non-empty structures structure */ |
| if (((VP_MOTION *) NULL == in)||(zoom <= 0.0)||(w <= 0)||(h <= 0)) { |
| return FALSE; |
| } |
| |
| /* ==> Not sure why the special case of out=NULL is necessary. Why couldn't |
| the caller just pass the same pointer for both in and out? */ |
| res = ((VP_MOTION *) NULL == out)?in:out; |
| |
| cx = (VP_PAR) (w/2.0); |
| cy = (VP_PAR) (h/2.0); |
| |
| VP_MOTION_ID(R2r); |
| inv_zoom = (VP_PAR)(1.0/zoom); |
| MXX(R2r) = inv_zoom; |
| MYY(R2r) = inv_zoom; |
| MXW(R2r)=cx*(((VP_PAR)1.0) - inv_zoom); |
| MYW(R2r)=cy*(((VP_PAR)1.0) - inv_zoom); |
| |
| VP_KEEP_AFFINE_2D(R2r); |
| |
| for(ii=0;ii<n;ii++) { |
| (void) vp_cascade_motion(&R2r,in+ii,&R2f); |
| res[ii]=R2f; |
| } |
| |
| return TRUE; |
| } /* vp_zoom_motion2d() */ |
| |
| /* =================================================================== */ |
| /* end vp_motionmodel.c */ |