--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// Intel License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of Intel Corporation may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "_cvaux.h"
+
+#define PATH_TO_E 1
+#define PATH_TO_SE 2
+#define PATH_TO_S 3
+
+#define K_S 2
+#define E_S 2
+#define C_S .01
+#define K_Z 5000
+#define K_NM 50000
+#define K_B 40
+#define NULL_EDGE 0.001f
+#define inf DBL_MAX
+
+typedef struct __CvWork
+{
+ double w_east;
+ double w_southeast;
+ double w_south;
+ char path_e;
+ char path_se;
+ char path_s;
+}_CvWork;
+
+
+double _cvBendingWork( CvPoint2D32f* B0,
+ CvPoint2D32f* F0,
+ CvPoint2D32f* B1,
+ CvPoint2D32f* F1/*,
+ CvPoint* K */);
+
+double _cvStretchingWork(CvPoint2D32f* P1,
+ CvPoint2D32f* P2);
+
+void _cvWorkEast (int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2);
+void _cvWorkSouthEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2);
+void _cvWorkSouth (int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2);
+
+static CvPoint2D32f null_edge = {0,0};
+
+double _cvStretchingWork(CvPoint2D32f* P1,
+ CvPoint2D32f* P2)
+{
+ double L1,L2, L_min, dL;
+
+ L1 = sqrt( (double)P1->x*P1->x + P1->y*P1->y);
+ L2 = sqrt( (double)P2->x*P2->x + P2->y*P2->y);
+
+ L_min = MIN(L1, L2);
+ dL = fabs( L1 - L2 );
+
+ return K_S * pow( dL, E_S ) / ( L_min + C_S*dL );
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////
+double _cvBendingWork( CvPoint2D32f* B0,
+ CvPoint2D32f* F0,
+ CvPoint2D32f* B1,
+ CvPoint2D32f* F1/*,
+ CvPoint* K*/)
+{
+ CvPoint2D32f Q( CvPoint2D32f q0, CvPoint2D32f q1, CvPoint2D32f q2, double t );
+ double angle( CvPoint2D32f A, CvPoint2D32f B );
+
+ CvPoint2D32f Q0, Q1, Q2;
+ CvPoint2D32f Q1_nm = { 0, 0 }, Q2_nm = { 0, 0 };
+ double d0, d1, d2, des, t_zero;
+ double k_zero, k_nonmon;
+ CvPoint2D32f center;
+ double check01, check02;
+ char check_origin;
+ double d_angle, d_nm_angle;
+/*
+ if( (B0->x==0) && (B0->y==0) )
+ {
+ if( (F0->x==0) && (F0->y==0) )
+ {
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+
+ d_angle = acos( (B1->x*F1->x + B1->y*F1->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(F1->x*F1->x + F1->y*F1->y) ) );
+ d_angle = CV_PI - d_angle;
+
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+
+ //return d_angle*K_B;
+ return 100;
+ }
+ K->x = -K->x;
+ K->y = -K->y;
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+
+ d_angle = acos( (B1->x*F1->x + B1->y*F1->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(F1->x*F1->x + F1->y*F1->y) ) );
+ d_angle = d_angle - acos( (F0->x*K->x + F0->y*K->y)/sqrt( (F0->x*F0->x + F0->y*F0->y)*(K->x*K->x + K->y*K->y) ) );
+ d_angle = d_angle - CV_PI*0.5;
+ d_angle = fabs(d_angle);
+
+
+ K->x = -K->x;
+ K->y = -K->y;
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+
+ //return d_angle*K_B;
+ return 100;
+ }
+
+
+ if( (F0->x==0) && (F0->y==0) )
+ {
+ K->x = -K->x;
+ K->y = -K->y;
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+
+ d_angle = acos( (B1->x*F1->x + B1->y*F1->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(F1->x*F1->x + F1->y*F1->y) ) );
+ d_angle = d_angle - acos( (B0->x*K->x + B0->y*K->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(K->x*K->x + K->y*K->y) ) );
+ d_angle = d_angle - CV_PI*0.5;
+ d_angle = fabs(d_angle);
+
+ K->x = -K->x;
+ K->y = -K->y;
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+
+ //return d_angle*K_B;
+ return 100;
+ }
+///////////////
+
+ if( (B1->x==0) && (B1->y==0) )
+ {
+ if( (F1->x==0) && (F1->y==0) )
+ {
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+
+ d_angle = acos( (B0->x*F0->x + B0->y*F0->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(F0->x*F0->x + F0->y*F0->y) ) );
+ d_angle = CV_PI - d_angle;
+
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+
+ //return d_angle*K_B;
+ return 100;
+ }
+ K->x = -K->x;
+ K->y = -K->y;
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+
+ d_angle = acos( (B0->x*F0->x + B0->y*F0->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(F0->x*F0->x + F0->y*F0->y) ) );
+ d_angle = d_angle - acos( (F1->x*K->x + F1->y*K->y)/sqrt( (F1->x*F1->x + F1->y*F1->y)*(K->x*K->x + K->y*K->y) ) );
+ d_angle = d_angle - CV_PI*0.5;
+ d_angle = fabs(d_angle);
+
+ K->x = -K->x;
+ K->y = -K->y;
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+
+ //return d_angle*K_B;
+ return 100;
+ }
+
+
+ if( (F1->x==0) && (F1->y==0) )
+ {
+ K->x = -K->x;
+ K->y = -K->y;
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+
+ d_angle = acos( (B0->x*F0->x + B0->y*F0->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(F0->x*F0->x + F0->y*F0->y) ) );
+ d_angle = d_angle - acos( (B1->x*K->x + B1->y*K->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(K->x*K->x + K->y*K->y) ) );
+ d_angle = d_angle - CV_PI*0.5;
+ d_angle = fabs(d_angle);
+
+ K->x = -K->x;
+ K->y = -K->y;
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+
+ //return d_angle*K_B;
+ return 100;
+ }
+
+*/
+
+/*
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+*/
+ Q0.x = F0->x * (-B0->x) + F0->y * (-B0->y);
+ Q0.y = F0->x * (-B0->y) - F0->y * (-B0->x);
+
+ Q1.x = 0.5f*( (F1->x * (-B0->x) + F1->y * (-B0->y)) + (F0->x * (-B1->x) + F0->y * (-B1->y)) );
+ Q1.y = 0.5f*( (F1->x * (-B0->y) - F1->y * (-B0->x)) + (F0->x * (-B1->y) - F0->y * (-B1->x)) );
+
+ Q2.x = F1->x * (-B1->x) + F1->y * (-B1->y);
+ Q2.y = F1->x * (-B1->y) - F1->y * (-B1->x);
+
+ d0 = Q0.x * Q1.y - Q0.y * Q1.x;
+ d1 = 0.5f*(Q0.x * Q2.y - Q0.y * Q2.x);
+ d2 = Q1.x * Q2.y - Q1.y * Q2.x;
+
+ // Check angles goes to zero
+ des = Q1.y*Q1.y - Q0.y*Q2.y;
+
+ k_zero = 0;
+
+ if( des >= 0 )
+ {
+ t_zero = ( Q0.y - Q1.y + sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y );
+
+ if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) )
+ {
+ k_zero = inf;
+ }
+
+ t_zero = ( Q0.y - Q1.y - sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y );
+
+ if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) )
+ {
+ k_zero = inf;
+ }
+ }
+
+ // Check nonmonotonic
+ des = d1*d1 - d0*d2;
+
+ k_nonmon = 0;
+
+ if( des >= 0 )
+ {
+ t_zero = ( d0 - d1 - sqrt(des) )/( d0 - 2*d1 + d2 );
+
+ if( (0 < t_zero) && (t_zero < 1) )
+ {
+ k_nonmon = 1;
+ Q1_nm = Q(Q0, Q1, Q2, t_zero);
+ }
+
+ t_zero = ( d0 - d1 + sqrt(des) )/( d0 - 2*d1 + d2 );
+
+ if( (0 < t_zero) && (t_zero < 1) )
+ {
+ k_nonmon += 2;
+ Q2_nm = Q(Q0, Q1, Q2, t_zero);
+ }
+ }
+
+ // Finde origin lie in Q0Q1Q2
+ check_origin = 1;
+
+ center.x = (Q0.x + Q1.x + Q2.x)/3;
+ center.y = (Q0.y + Q1.y + Q2.y)/3;
+
+ check01 = (center.x - Q0.x)*(Q1.y - Q0.y) + (center.y - Q0.y)*(Q1.x - Q0.x);
+ check02 = (-Q0.x)*(Q1.y - Q0.y) + (-Q0.y)*(Q1.x - Q0.x);
+ if( check01*check02 > 0 )
+ {
+ check01 = (center.x - Q1.x)*(Q2.y - Q1.y) + (center.y - Q1.y)*(Q2.x - Q1.x);
+ check02 = (-Q1.x)*(Q2.y - Q1.y) + (-Q1.y)*(Q2.x - Q1.x);
+ if( check01*check02 > 0 )
+ {
+ check01 = (center.x - Q2.x)*(Q0.y - Q2.y) + (center.y - Q2.y)*(Q0.x - Q2.x);
+ check02 = (-Q2.x)*(Q0.y - Q2.y) + (-Q2.y)*(Q0.x - Q2.x);
+ if( check01*check02 > 0 )
+ {
+ check_origin = 0;
+ }
+ }
+ }
+
+ // Calculate angle
+ d_nm_angle = 0;
+ d_angle = angle(Q0,Q2);
+ if( k_nonmon == 0 )
+ {
+ if( check_origin == 0 )
+ {
+ }
+ else
+ {
+ d_angle = 2*CV_PI - d_angle;
+ }
+ }
+ else
+ {
+ if( k_nonmon == 1 )
+ {
+ d_nm_angle = angle(Q0,Q1_nm);
+ if(d_nm_angle > d_angle)
+ {
+ d_nm_angle = d_nm_angle - d_angle;
+ }
+ }
+
+ if( k_nonmon == 2 )
+ {
+ d_nm_angle = angle(Q0,Q2_nm);
+ if(d_nm_angle > d_angle)
+ {
+ d_nm_angle = d_nm_angle - d_angle;
+ }
+ }
+
+ if( k_nonmon == 3 )
+ {
+ d_nm_angle = angle(Q0,Q1_nm);
+ if(d_nm_angle > d_angle)
+ {
+ d_nm_angle = d_nm_angle - d_angle;
+ d_nm_angle = d_nm_angle + angle(Q0, Q2_nm);
+ }
+ else
+ {
+ d_nm_angle = d_nm_angle + angle(Q2,Q2_nm);
+ }
+ }
+ }
+/*
+ B0->x = -B0->x;
+ B0->y = -B0->y;
+ B1->x = -B1->x;
+ B1->y = -B1->y;
+*/
+ return d_angle*K_B + d_nm_angle*K_NM + k_zero*K_Z;
+ //return 0;
+}
+
+
+/////////////////////////////////////////////////////////////////////////////////
+void _cvWorkEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
+{
+ double w1,w2;
+ CvPoint2D32f small_edge;
+
+ //W[i,j].w_east
+ w1 = W[i-1][j].w_east /*+ _cvBendingWork( &edges1[i-2],
+ &edges1[i-1],
+ &null_edge ,
+ &null_edge,
+ NULL)*/;
+
+ small_edge.x = NULL_EDGE*edges1[i-1].x;
+ small_edge.y = NULL_EDGE*edges1[i-1].y;
+
+ w2 = W[i-1][j].w_southeast + _cvBendingWork(&edges1[i-2],
+ &edges1[i-1],
+ &edges2[j-1],
+ /*&null_edge*/&small_edge/*,
+ &edges2[j]*/);
+
+ if(w1<w2)
+ {
+ W[i][j].w_east = w1 + _cvStretchingWork( &edges1[i-1], &null_edge );
+ W[i][j].path_e = PATH_TO_E;
+ }
+ else
+ {
+ W[i][j].w_east = w2 + _cvStretchingWork( &edges1[i-1], &null_edge );
+ W[i][j].path_e = PATH_TO_SE;
+ }
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////
+void _cvWorkSouthEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
+{
+ double w1,w2,w3;
+ CvPoint2D32f small_edge;
+
+ //W[i,j].w_southeast
+ small_edge.x = NULL_EDGE*edges1[i-2].x;
+ small_edge.y = NULL_EDGE*edges1[i-2].y;
+
+ w1 = W[i-1][j-1].w_east + _cvBendingWork(&edges1[i-2],
+ &edges1[i-1],
+ /*&null_edge*/&small_edge,
+ &edges2[j-1]/*,
+ &edges2[j-2]*/);
+
+ w2 = W[i-1][j-1].w_southeast + _cvBendingWork( &edges1[i-2],
+ &edges1[i-1],
+ &edges2[j-2],
+ &edges2[j-1]/*,
+ NULL*/);
+
+ small_edge.x = NULL_EDGE*edges2[j-2].x;
+ small_edge.y = NULL_EDGE*edges2[j-2].y;
+
+ w3 = W[i-1][j-1].w_south + _cvBendingWork( /*&null_edge*/&small_edge,
+ &edges1[i-1],
+ &edges2[j-2],
+ &edges2[j-1]/*,
+ &edges1[i-2]*/);
+
+ if( w1<w2 )
+ {
+ if(w1<w3)
+ {
+ W[i][j].w_southeast = w1 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
+ W[i][j].path_se = PATH_TO_E;
+ }
+ else
+ {
+ W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
+ W[i][j].path_se = 3;
+ }
+ }
+ else
+ {
+ if( w2<w3)
+ {
+ W[i][j].w_southeast = w2 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
+ W[i][j].path_se = PATH_TO_SE;
+ }
+ else
+ {
+ W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
+ W[i][j].path_se = 3;
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////
+void _cvWorkSouth(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
+{
+ double w1,w2;
+ CvPoint2D32f small_edge;
+
+ //W[i,j].w_south
+
+ small_edge.x = NULL_EDGE*edges2[j-1].x;
+ small_edge.y = NULL_EDGE*edges2[j-1].y;
+
+ w1 = W[i][j-1].w_southeast + _cvBendingWork(&edges1[i-1],
+ /*&null_edge*/&small_edge,
+ &edges2[j-2],
+ &edges2[j-1]/*,
+ &edges1[i]*/);
+
+ w2 = W[i][j-1].w_south /*+ _cvBendingWork( &null_edge ,
+ &null_edge,
+ &edges2[j-2],
+ &edges2[j-1],
+ NULL)*/;
+
+ if( w1<w2 )
+ {
+ W[i][j].w_south = w1 + _cvStretchingWork( &null_edge, &edges2[j-1] );
+ W[i][j].path_s = PATH_TO_SE;
+ }
+ else
+ {
+ W[i][j].w_south = w2 + _cvStretchingWork( &null_edge, &edges2[j-1] );
+ W[i][j].path_s = 3;
+ }
+}
+
+//===================================================
+CvPoint2D32f Q(CvPoint2D32f q0,CvPoint2D32f q1,CvPoint2D32f q2,double t)
+{
+ CvPoint2D32f q;
+
+ q.x = (float)(q0.x*(1-t)*(1-t) + 2*q1.x*t*(1-t) + q2.x*t*t);
+ q.y = (float)(q0.y*(1-t)*(1-t) + 2*q1.y*t*(1-t) + q2.y*t*t);
+
+ return q;
+}
+
+double angle(CvPoint2D32f A, CvPoint2D32f B)
+{
+ return acos( (A.x*B.x + A.y*B.y)/sqrt( (double)(A.x*A.x + A.y*A.y)*(B.x*B.x + B.y*B.y) ) );
+}
+
+/***************************************************************************************\
+*
+* This function compute intermediate polygon between contour1 and contour2
+*
+* Correspondence between points of contours specify by corr
+*
+* param = [0,1]; 0 correspondence to contour1, 1 - contour2
+*
+\***************************************************************************************/
+CvSeq* icvBlendContours(CvSeq* contour1,
+ CvSeq* contour2,
+ CvSeq* corr,
+ double param,
+ CvMemStorage* storage)
+{
+ int j;
+
+ CvSeqWriter writer01;
+ CvSeqReader reader01;
+
+ int Ni,Nj; // size of contours
+ int i; // counter
+
+ CvPoint* point1; // array of first contour point
+ CvPoint* point2; // array of second contour point
+
+ CvPoint point_output; // intermediate storage of ouput point
+
+ int corr_point;
+
+ // Create output sequence.
+ CvSeq* output = cvCreateSeq(0,
+ sizeof(CvSeq),
+ sizeof(CvPoint),
+ storage );
+
+ // Find size of contours.
+ Ni = contour1->total + 1;
+ Nj = contour2->total + 1;
+
+ point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) );
+ point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) );
+
+ // Initialize arrays of point
+ cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ );
+ cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ );
+
+ // First and last point mast be equal.
+ point1[Ni-1] = point1[0];
+ point2[Nj-1] = point2[0];
+
+ // Initializes process of writing to sequence.
+ cvStartAppendToSeq( output, &writer01);
+
+ i = Ni-1; //correspondence to points of contour1
+ for( ; corr; corr = corr->h_next )
+ {
+ //Initializes process of sequential reading from sequence
+ cvStartReadSeq( corr, &reader01, 0 );
+
+ for(j=0; j < corr->total; j++)
+ {
+ // Read element from sequence.
+ CV_READ_SEQ_ELEM( corr_point, reader01 );
+
+ // Compute point of intermediate polygon.
+ point_output.x = cvRound(point1[i].x + param*( point2[corr_point].x - point1[i].x ));
+ point_output.y = cvRound(point1[i].y + param*( point2[corr_point].y - point1[i].y ));
+
+ // Write element to sequence.
+ CV_WRITE_SEQ_ELEM( point_output, writer01 );
+ }
+ i--;
+ }
+ // Updates sequence header.
+ cvFlushSeqWriter( &writer01 );
+
+ return output;
+}
+
+/**************************************************************************************************
+*
+*
+*
+*
+*
+*
+*
+*
+*
+*
+**************************************************************************************************/
+
+
+void icvCalcContoursCorrespondence(CvSeq* contour1,
+ CvSeq* contour2,
+ CvSeq** corr,
+ CvMemStorage* storage)
+{
+ int i,j; // counter of cycles
+ int Ni,Nj; // size of contours
+ _CvWork** W; // graph for search minimum of work
+
+ CvPoint* point1; // array of first contour point
+ CvPoint* point2; // array of second contour point
+ CvPoint2D32f* edges1; // array of first contour edge
+ CvPoint2D32f* edges2; // array of second contour edge
+
+ //CvPoint null_edge = {0,0}; //
+ CvPoint2D32f small_edge;
+ //double inf; // infinity
+
+ CvSeq* corr01;
+ CvSeqWriter writer;
+
+ char path; //
+
+ // Find size of contours
+ Ni = contour1->total + 1;
+ Nj = contour2->total + 1;
+
+ // Create arrays
+ W = (_CvWork**)malloc(sizeof(_CvWork*)*Ni);
+ for(i=0; i<Ni; i++)
+ {
+ W[i] = (_CvWork*)malloc(sizeof(_CvWork)*Nj);
+ }
+
+ point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) );
+ point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) );
+ edges1 = (CvPoint2D32f* )malloc( (Ni-1)*sizeof(CvPoint2D32f) );
+ edges2 = (CvPoint2D32f* )malloc( (Nj-1)*sizeof(CvPoint2D32f) );
+
+ // Initialize arrays of point
+ cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ );
+ cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ );
+
+ point1[Ni-1] = point1[0];
+ point2[Nj-1] = point2[0];
+
+ for(i=0;i<Ni-1;i++)
+ {
+ edges1[i].x = (float)( point1[i+1].x - point1[i].x );
+ edges1[i].y = (float)( point1[i+1].y - point1[i].y );
+ };
+
+ for(i=0;i<Nj-1;i++)
+ {
+ edges2[i].x = (float)( point2[i+1].x - point2[i].x );
+ edges2[i].y = (float)( point2[i+1].y - point2[i].y );
+ };
+
+ // Find infinity constant
+ //inf=1;
+/////////////
+
+//Find min path in graph
+
+/////////////
+ W[0][0].w_east = 0;
+ W[0][0].w_south = 0;
+ W[0][0].w_southeast = 0;
+
+ W[1][1].w_southeast = _cvStretchingWork( &edges1[0], &edges2[0] );
+ W[1][1].w_east = inf;
+ W[1][1].w_south = inf;
+ W[1][1].path_se = PATH_TO_SE;
+
+ W[0][1].w_south = _cvStretchingWork( &null_edge, &edges2[0] );
+ W[0][1].path_s = 3;
+ W[1][0].w_east = _cvStretchingWork( &edges2[0], &null_edge );
+ W[1][0].path_e = PATH_TO_E;
+
+ for( i=1; i<Ni; i++ )
+ {
+ W[i][0].w_south = inf;
+ W[i][0].w_southeast = inf;
+ }
+
+ for(j=1; j<Nj; j++)
+ {
+ W[0][j].w_east = inf;
+ W[0][j].w_southeast = inf;
+ }
+
+ for(i=2; i<Ni; i++)
+ {
+ j=0;/////////
+ W[i][j].w_east = W[i-1][j].w_east;
+ W[i][j].w_east = W[i][j].w_east /*+
+ _cvBendingWork( &edges1[i-2], &edges1[i-1], &null_edge, &null_edge, NULL )*/;
+ W[i][j].w_east = W[i][j].w_east + _cvStretchingWork( &edges2[i-1], &null_edge );
+ W[i][j].path_e = PATH_TO_E;
+
+ j=1;//////////
+ W[i][j].w_south = inf;
+
+ _cvWorkEast (i, j, W, edges1, edges2);
+
+ W[i][j].w_southeast = W[i-1][j-1].w_east;
+ W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
+
+ small_edge.x = NULL_EDGE*edges1[i-2].x;
+ small_edge.y = NULL_EDGE*edges1[i-2].y;
+
+ W[i][j].w_southeast = W[i][j].w_southeast +
+ _cvBendingWork( &edges1[i-2], &edges1[i-1], /*&null_edge*/&small_edge, &edges2[j-1]/*, &edges2[Nj-2]*/);
+
+ W[i][j].path_se = PATH_TO_E;
+ }
+
+ for(j=2; j<Nj; j++)
+ {
+ i=0;//////////
+ W[i][j].w_south = W[i][j-1].w_south;
+ W[i][j].w_south = W[i][j].w_south + _cvStretchingWork( &null_edge, &edges2[j-1] );
+ W[i][j].w_south = W[i][j].w_south /*+
+ _cvBendingWork( &null_edge, &null_edge, &edges2[j-2], &edges2[j-1], NULL )*/;
+ W[i][j].path_s = 3;
+
+ i=1;///////////
+ W[i][j].w_east= inf;
+
+ _cvWorkSouth(i, j, W, edges1, edges2);
+
+ W[i][j].w_southeast = W[i-1][j-1].w_south;
+ W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
+
+ small_edge.x = NULL_EDGE*edges2[j-2].x;
+ small_edge.y = NULL_EDGE*edges2[j-2].y;
+
+ W[i][j].w_southeast = W[i][j].w_southeast +
+ _cvBendingWork( /*&null_edge*/&small_edge, &edges1[i-1], &edges2[j-2], &edges2[j-1]/*, &edges1[Ni-2]*/);
+ W[i][j].path_se = 3;
+ }
+
+ for(i=2; i<Ni; i++)
+ for(j=2; j<Nj; j++)
+ {
+ _cvWorkEast (i, j, W, edges1, edges2);
+ _cvWorkSouthEast(i, j, W, edges1, edges2);
+ _cvWorkSouth (i, j, W, edges1, edges2);
+ }
+
+ i=Ni-1;j=Nj-1;
+
+ *corr = cvCreateSeq(0,
+ sizeof(CvSeq),
+ sizeof(int),
+ storage );
+
+ corr01 = *corr;
+ cvStartAppendToSeq( corr01, &writer );
+ if( W[i][j].w_east > W[i][j].w_southeast )
+ {
+ if( W[i][j].w_southeast > W[i][j].w_south )
+ {
+ path = 3;
+ }
+ else
+ {
+ path = PATH_TO_SE;
+ }
+ }
+ else
+ {
+ if( W[i][j].w_east < W[i][j].w_south )
+ {
+ path = PATH_TO_E;
+ }
+ else
+ {
+ path = 3;
+ }
+ }
+ do
+ {
+ CV_WRITE_SEQ_ELEM( j, writer );
+
+ switch( path )
+ {
+ case PATH_TO_E:
+ path = W[i][j].path_e;
+ i--;
+ cvFlushSeqWriter( &writer );
+ corr01->h_next = cvCreateSeq( 0,
+ sizeof(CvSeq),
+ sizeof(int),
+ storage );
+ corr01 = corr01->h_next;
+ cvStartAppendToSeq( corr01, &writer );
+ break;
+
+ case PATH_TO_SE:
+ path = W[i][j].path_se;
+ j--; i--;
+ cvFlushSeqWriter( &writer );
+ corr01->h_next = cvCreateSeq( 0,
+ sizeof(CvSeq),
+ sizeof(int),
+ storage );
+ corr01 = corr01->h_next;
+ cvStartAppendToSeq( corr01, &writer );
+ break;
+
+ case 3:
+ path = W[i][j].path_s;
+ j--;
+ break;
+ }
+
+ } while( (i>=0) && (j>=0) );
+ cvFlushSeqWriter( &writer );
+
+ // Free memory
+ for(i=1;i<Ni;i++)
+ {
+ free(W[i]);
+ }
+ free(W);
+ free(point1);
+ free(point2);
+ free(edges1);
+ free(edges2);
+}
+
projects / opencv / blobdiff