16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cxcore.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvKMeans2( const CvArr* samples_arr, int cluster_count, 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvArr* labels_arr, CvTermCriteria termcrit ) 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* centers = 0; 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* old_centers = 0; 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* counters = 0; 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvKMeans2" ); 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat samples_stub, labels_stub; 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* samples = (CvMat*)samples_arr; 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* labels = (CvMat*)labels_arr; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* temp = 0; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRNG rng = CvRNG(-1); 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, sample_count, dims; 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ids_delta, iter; 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double max_dist; 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( samples )) 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( samples = cvGetMat( samples, &samples_stub )); 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( labels )) 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( labels = cvGetMat( labels, &labels_stub )); 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cluster_count < 1 ) 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of clusters should be positive" ); 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(samples->type) != CV_32F || CV_MAT_TYPE(labels->type) != CV_32SC1 ) 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "samples should be floating-point matrix, cluster_idx - integer vector" ); 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (labels->rows != 1 && (labels->cols != 1 || !CV_IS_MAT_CONT(labels->type))) || 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn labels->rows + labels->cols - 1 != samples->rows ) 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "cluster_idx should be 1D vector of the same number of elements as samples' number of rows" ); 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( termcrit = cvCheckTermCriteria( termcrit, 1e-6, 100 )); 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn termcrit.epsilon *= termcrit.epsilon; 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sample_count = samples->rows; 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cluster_count > sample_count ) 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cluster_count = sample_count; 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dims = samples->cols*CV_MAT_CN(samples->type); 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ids_delta = labels->step ? labels->step/(int)sizeof(int) : 1; 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( centers = cvCreateMat( cluster_count, dims, CV_64FC1 )); 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( old_centers = cvCreateMat( cluster_count, dims, CV_64FC1 )); 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( counters = cvCreateMat( 1, cluster_count, CV_32SC1 )); 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // init centers 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < sample_count; i++ ) 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn labels->data.i[i] = cvRandInt(&rng) % cluster_count; 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counters->cols = cluster_count; // cut down counters 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_dist = termcrit.epsilon*2; 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( iter = 0; iter < termcrit.max_iter; iter++ ) 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // computer centers 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( centers ); 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( counters ); 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < sample_count; i++ ) 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* s = (float*)(samples->data.ptr + i*samples->step); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k = labels->data.i[i*ids_delta]; 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* c = (double*)(centers->data.ptr + k*centers->step); 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j <= dims - 4; j += 4 ) 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0 = c[j] + s[j]; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t1 = c[j+1] + s[j+1]; 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[j] = t0; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[j+1] = t1; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = c[j+2] + s[j+2]; 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = c[j+3] + s[j+3]; 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[j+2] = t0; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[j+3] = t1; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; j < dims; j++ ) 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[j] += s[j]; 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counters->data.i[k]++; 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( iter > 0 ) 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_dist = 0; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < cluster_count; k++ ) 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* c = (double*)(centers->data.ptr + k*centers->step); 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( counters->data.i[k] != 0 ) 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double scale = 1./counters->data.i[k]; 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < dims; j++ ) 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[j] *= scale; 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = cvRandInt( &rng ) % sample_count; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* s = (float*)(samples->data.ptr + i*samples->step); 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < dims; j++ ) 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[j] = s[j]; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( iter > 0 ) 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist = 0; 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* c_o = (double*)(old_centers->data.ptr + k*old_centers->step); 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < dims; j++ ) 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = c[j] - c_o[j]; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist += t*t; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( max_dist < dist ) 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_dist = dist; 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // assign labels 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < sample_count; i++ ) 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* s = (float*)(samples->data.ptr + i*samples->step); 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int k_best = 0; 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double min_dist = DBL_MAX; 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < cluster_count; k++ ) 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* c = (double*)(centers->data.ptr + k*centers->step); 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist = 0; 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j = 0; 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; j <= dims - 4; j += 4 ) 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0 = c[j] - s[j]; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t1 = c[j+1] - s[j+1]; 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist += t0*t0 + t1*t1; 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = c[j+2] - s[j+2]; 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = c[j+3] - s[j+3]; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist += t0*t0 + t1*t1; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; j < dims; j++ ) 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = c[j] - s[j]; 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist += t*t; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( min_dist > dist ) 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_dist = dist; 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k_best = k; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn labels->data.i[i*ids_delta] = k_best; 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( max_dist < termcrit.epsilon ) 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( centers, old_centers, temp ); 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( counters ); 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < sample_count; i++ ) 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counters->data.i[labels->data.i[i]]++; 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // ensure that we do not have empty clusters 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < cluster_count; k++ ) 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( counters->data.i[k] == 0 ) 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(;;) 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = cvRandInt(&rng) % sample_count; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j = labels->data.i[i]; 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( counters->data.i[j] > 1 ) 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn labels->data.i[i] = k; 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counters->data.i[j]--; 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counters->data.i[k]++; 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( ¢ers ); 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &old_centers ); 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &counters ); 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Finds real roots of cubic, quadratic or linear equation. 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn The original code has been taken from Ken Turkowski web page 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (http://www.worldserver.com/turk/opensource/) and adopted for OpenCV. 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Here is the copyright notice. 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ----------------------------------------------------------------------- 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Copyright (C) 1978-1999 Ken Turkowski. <turk@computer.org> 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn All rights reserved. 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Warranty Information 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Even though I have reviewed this software, I make no warranty 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn or representation, either express or implied, with respect to this 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn software, its quality, accuracy, merchantability, or fitness for a 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn particular purpose. As a result, this software is provided "as is," 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn and you, its user, are assuming the entire risk as to its quality 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn and accuracy. 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn This code may be used and freely distributed as long as it includes 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn this copyright notice and the above warranty information. 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ----------------------------------------------------------------------- 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvSolveCubic( const CvMat* coeffs, CvMat* roots ) 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n = 0; 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvSolveCubic" ); 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a0 = 1., a1, a2, a3; 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x0 = 0., x1 = 0., x2 = 0.; 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = 1, coeff_count; 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(coeffs) ) 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !coeffs ? CV_StsNullPtr : CV_StsBadArg, "Input parameter is not a valid matrix" ); 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(roots) ) 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !roots ? CV_StsNullPtr : CV_StsBadArg, "Output parameter is not a valid matrix" ); 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_TYPE(coeffs->type) != CV_32FC1 && CV_MAT_TYPE(coeffs->type) != CV_64FC1) || 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(roots->type) != CV_32FC1 && CV_MAT_TYPE(roots->type) != CV_64FC1) ) 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Both matrices should be floating-point (single or double precision)" ); 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff_count = coeffs->rows + coeffs->cols - 1; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (coeffs->rows != 1 && coeffs->cols != 1) || (coeff_count != 3 && coeff_count != 4) ) 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The matrix of coefficients must be 1-dimensional vector of 3 or 4 elements" ); 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (roots->rows != 1 && roots->cols != 1) || 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn roots->rows + roots->cols - 1 != 3 ) 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The matrix of roots must be 1-dimensional vector of 3 elements" ); 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(coeffs->type) == CV_32FC1 ) 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* c = coeffs->data.fl; 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coeffs->rows > 1 ) 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = coeffs->step/sizeof(c[0]); 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coeff_count == 4 ) 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a0 = c[0], c += step; 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a1 = c[0]; 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a2 = c[step]; 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a3 = c[step*2]; 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* c = coeffs->data.db; 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coeffs->rows > 1 ) 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = coeffs->step/sizeof(c[0]); 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coeff_count == 4 ) 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a0 = c[0], c += step; 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a1 = c[0]; 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a2 = c[step]; 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a3 = c[step*2]; 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( a0 == 0 ) 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( a1 == 0 ) 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( a2 == 0 ) 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = a3 == 0 ? -1 : 0; 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // linear equation 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 = a3/a2; 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = 1; 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // quadratic equation 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = a2*a2 - 4*a1*a3; 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d >= 0 ) 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = sqrt(d); 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double q = (-a2 + (a2 < 0 ? -d : d)) * 0.5; 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 = q / a1; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = a3 / q; 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = d > 0 ? 2 : 1; 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a0 = 1./a0; 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a1 *= a0; 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a2 *= a0; 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a3 *= a0; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Q = (a1 * a1 - 3 * a2) * (1./9); 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double R = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) * (1./54); 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Qcubed = Q * Q * Q; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = Qcubed - R * R; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d >= 0 ) 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double theta = acos(R / sqrt(Qcubed)); 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sqrtQ = sqrt(Q); 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0 = -2 * sqrtQ; 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t1 = theta * (1./3); 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t2 = a1 * (1./3); 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 = t0 * cos(t1) - t2; 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = t0 * cos(t1 + (2.*CV_PI/3)) - t2; 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 = t0 * cos(t1 + (4.*CV_PI/3)) - t2; 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = 3; 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double e; 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = sqrt(-d); 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e = pow(d + fabs(R), 0.333333333333); 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( R > 0 ) 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e = -e; 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 = (e + Q / e) - a1 * (1./3); 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = 1; 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = 1; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(roots->type) == CV_32FC1 ) 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* r = roots->data.fl; 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( roots->rows > 1 ) 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = roots->step/sizeof(r[0]); 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r[0] = (float)x0; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r[step] = (float)x1; 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r[step*2] = (float)x2; 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* r = roots->data.db; 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( roots->rows > 1 ) 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = roots->step/sizeof(r[0]); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r[0] = x0; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r[step] = x1; 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r[step*2] = x2; 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return n; 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Finds real and complex roots of polynomials of any degree with real 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coefficients. The original code has been taken from Ken Turkowski's web 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn page (http://www.worldserver.com/turk/opensource/) and adopted for OpenCV. 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Here is the copyright notice. 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ----------------------------------------------------------------------- 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Copyright (C) 1981-1999 Ken Turkowski. <turk@computer.org> 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn All rights reserved. 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Warranty Information 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Even though I have reviewed this software, I make no warranty 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn or representation, either express or implied, with respect to this 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn software, its quality, accuracy, merchantability, or fitness for a 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn particular purpose. As a result, this software is provided "as is," 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn and you, its user, are assuming the entire risk as to its quality 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn and accuracy. 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn This code may be used and freely distributed as long as it includes 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn this copyright notice and the above warranty information. 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/******************************************************************************* 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * FindPolynomialRoots 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * The Bairstow and Newton correction formulae are used for a simultaneous 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * linear and quadratic iterated synthetic division. The coefficients of 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * a polynomial of degree n are given as a[i] (i=0,i,..., n) where a[0] is 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * the constant term. The coefficients are scaled by dividing them by 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * their geometric mean. The Bairstow or Newton iteration method will 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * nearly always converge to the number of figures carried, fig, either to 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * root values or to their reciprocals. If the simultaneous Newton and 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * Bairstow iteration fails to converge on root values or their 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * reciprocals in maxiter iterations, the convergence requirement will be 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * successively reduced by one decimal figure. This program anticipates 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * and protects against loss of significance in the quadratic synthetic 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * division. (Refer to "On Programming the Numerical Solution of 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * Polynomial Equations," by K. W. Ellenberger, Commun. ACM 3 (Dec. 1960), 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * 644-647.) The real and imaginary part of each root is stated as u[i] 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * and v[i], respectively. 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * ACM algorithm #30 - Numerical Solution of the Polynomial Equation 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * K. W. Ellenberger 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * Missle Division, North American Aviation, Downey, California 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * Converted to C, modified, optimized, and structured by 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * Ken Turkowski 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * CADLINC, Inc., Palo Alto, California 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *******************************************************************************/ 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define MAXN 16 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void icvFindPolynomialRoots(const double *a, double *u, int n, int maxiter, int fig) 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j; 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double h[MAXN + 3], b[MAXN + 3], c[MAXN + 3], d[MAXN + 3], e[MAXN + 3]; 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [-2 : n] 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double K, ps, qs, pt, qt, s, rev, r = 0; 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t; 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p = 0, q = 0, qq; 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Zero elements with negative indices 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[2 + -1] = b[2 + -2] = 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[2 + -1] = c[2 + -2] = 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d[2 + -1] = d[2 + -2] = 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e[2 + -1] = e[2 + -2] = 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + -1] = h[2 + -2] = 0.0; 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Copy polynomial coefficients to working storage 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = n; j >= 0; j--) 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + j] = *a++; // Note reversal of coefficients 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = 1; 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn K = pow(10.0, (double)(fig)); // Relative accuracy 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (; h[2 + n] == 0.0; n--) { // Look for zero high-order coeff. 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = 0.0; 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = 0.0; 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn INIT: 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n == 0) 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ps = qs = pt = qt = s = 0.0; 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rev = 1.0; 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn K = pow(10.0, (double)(fig)); 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n == 1) { 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = -h[2 + 1] / h[2 + 0]; 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto LINEAR; 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = n; j >= 0; j--) // Find geometric mean of coeff's 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (h[2 + j] != 0.0) 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s += log(fabs(h[2 + j])); 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = exp(s / (n + 1)); 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = n; j >= 0; j--) // Normalize coeff's by mean 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + j] /= s; 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (fabs(h[2 + 1] / h[2 + 0]) < fabs(h[2 + n - 1] / h[2 + n])) { 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn REVERSE: 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = -t; 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = (n - 1) / 2; j >= 0; j--) { 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = h[2 + j]; 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + j] = h[2 + n - j]; 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + n - j] = s; 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (qs != 0.0) { 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = ps; 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q = qs; 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (h[2 + n - 2] == 0.0) { 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q = 1.0; 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = -2.0; 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q = h[2 + n] / h[2 + n - 2]; 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = (h[2 + n - 1] - q * h[2 + n - 3]) / h[2 + n - 2]; 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n == 2) 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto QADRTIC; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = 0.0; 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ITERATE: 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (i = maxiter; i > 0; i--) { 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = 0; j <= n; j++) { // BAIRSTOW 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[2 + j] = h[2 + j] - p * b[2 + j - 1] - q * b[2 + j - 2]; 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[2 + j] = b[2 + j] - p * c[2 + j - 1] - q * c[2 + j - 2]; 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if ((h[2 + n - 1] != 0.0) && (b[2 + n - 1] != 0.0)) { 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (fabs(h[2 + n - 1] / b[2 + n - 1]) >= K) { 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[2 + n] = h[2 + n] - q * b[2 + n - 2]; 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (b[2 + n] == 0.0) 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto QADRTIC; 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (K < fabs(h[2 + n] / b[2 + n])) 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto QADRTIC; 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = 0; j <= n; j++) { // NEWTON 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d[2 + j] = h[2 + j] + r * d[2 + j - 1]; // Calculate polynomial at r 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e[2 + j] = d[2 + j] + r * e[2 + j - 1]; // Calculate derivative at r 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (d[2 + n] == 0.0) 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto LINEAR; 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (K < fabs(h[2 + n] / d[2 + n])) 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto LINEAR; 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c[2 + n - 1] = -p * c[2 + n - 2] - q * c[2 + n - 3]; 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = c[2 + n - 2] * c[2 + n - 2] - c[2 + n - 1] * c[2 + n - 3]; 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (s == 0.0) { 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p -= 2.0; 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q *= (q + 1.0); 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p += (b[2 + n - 1] * c[2 + n - 2] - b[2 + n] * c[2 + n - 3]) / s; 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q += (-b[2 + n - 1] * c[2 + n - 1] + b[2 + n] * c[2 + n - 2]) / s; 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (e[2 + n - 1] == 0.0) 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r -= 1.0; // Minimum step 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r -= d[2 + n] / e[2 + n - 1]; // Newton's iteration 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ps = pt; 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn qs = qt; 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt = p; 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn qt = q; 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (rev < 0.0) 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn K /= 10.0; 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rev = -rev; 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto REVERSE; 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn LINEAR: 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (t < 0) 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = 1.0 / r; 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n--; 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = r; 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = 0.0; 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = n; j >= 0; j--) { // Polynomial deflation by lin-nomial 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if ((d[2 + j] != 0.0) && (fabs(h[2 + j] / d[2 + j]) < K)) 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + j] = d[2 + j]; 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + j] = 0.0; 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n == 0) 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto ITERATE; 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn QADRTIC: 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (t < 0) { 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p /= q; 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q = 1.0 / q; 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n -= 2; 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (0.0 < (q - (p * p / 4.0))) { // Two complex roots 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = sqrt(q - (p * p / 4.0)); 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = -p / 2.0; 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = s; 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = -p / 2.0; 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = -s; 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { // Two real roots 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = sqrt(((p * p / 4.0)) - q); 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (p < 0.0) 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = qq = -p / 2.0 + s; 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = qq = -p / 2.0 - s; 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = 0.0; 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = q / qq; 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *u++ = 0.0; 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = n; j >= 0; j--) { // Polynomial deflation by quadratic 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if ((b[2 + j] != 0.0) && (fabs(h[2 + j] / b[2 + j]) < K)) 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + j] = b[2 + j]; 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[2 + j] = 0.0; 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goto INIT; 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#undef MAXN 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvSolvePoly(const CvMat* a, CvMat *r, int maxiter, int fig) 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int m, n; 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *ad = 0, *rd = 0; 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME("cvSolvePoly"); 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (CV_MAT_TYPE(a->type) != CV_32FC1 && 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(a->type) != CV_64FC1) 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnsupportedFormat, "coeffs must be either CV_32FC1 or CV_64FC1"); 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (CV_MAT_TYPE(r->type) != CV_32FC2 && 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(r->type) != CV_64FC2) 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnsupportedFormat, "roots must be either CV_32FC2 or CV_64FC2"); 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m = a->rows * a->cols; 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = r->rows * r->cols; 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (m - 1 != n) 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnmatchedFormats, "must have n + 1 coefficients"); 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(a->type) == CV_32F || !CV_IS_MAT_CONT(a->type)) 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ad = (double*)cvStackAlloc(m*sizeof(ad[0])); 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _a = cvMat( a->rows, a->cols, CV_64F, ad ); 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( a, &_a ); 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ad = a->data.db; 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(r->type) == CV_32F || !CV_IS_MAT_CONT(r->type)) 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rd = (double*)cvStackAlloc(n*sizeof(ad[0])); 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rd = r->data.db; 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFindPolynomialRoots( ad, rd, n, maxiter, fig); 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( rd != r->data.db ) 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _r = cvMat( r->rows, r->cols, CV_64F, rd ); 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_r, r ); 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void cvNormalize( const CvArr* src, CvArr* dst, 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a, double b, int norm_type, const CvArr* mask ) 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp = 0; 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvNormalize" ); 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double scale, shift; 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( norm_type == CV_MINMAX ) 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double smin = 0, smax = 0; 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dmin = MIN( a, b ), dmax = MAX( a, b ); 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMinMaxLoc( src, &smin, &smax, 0, 0, mask ); 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0); 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn shift = dmin - smin*scale; 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C ) 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *s = (CvMat*)src, *d = (CvMat*)dst; 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT(s) && CV_IS_MAT(d) && CV_IS_MAT_CONT(s->type & d->type) && 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ARE_TYPES_EQ(s,d) && CV_ARE_SIZES_EQ(s,d) && !mask && 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s->cols*s->rows <= CV_MAX_INLINE_MAT_OP_SIZE*CV_MAX_INLINE_MAT_OP_SIZE ) 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, len = s->cols*s->rows; 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm = 0, v; 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(s->type) == CV_32FC1 ) 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* sptr = s->data.fl; 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* dptr = d->data.fl; 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( norm_type == CV_L2 ) 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = sptr[i]; 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm += v*v; 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = sqrt(norm); 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( norm_type == CV_L1 ) 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = fabs((double)sptr[i]); 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm += v; 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = fabs((double)sptr[i]); 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = MAX(norm,v); 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = norm > DBL_EPSILON ? 1./norm : 0.; 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[i] = (float)(sptr[i]*norm); 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(s->type) == CV_64FC1 ) 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* sptr = s->data.db; 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* dptr = d->data.db; 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( norm_type == CV_L2 ) 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = sptr[i]; 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm += v*v; 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = sqrt(norm); 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( norm_type == CV_L1 ) 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = fabs(sptr[i]); 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm += v; 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = fabs(sptr[i]); 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = MAX(norm,v); 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = norm > DBL_EPSILON ? 1./norm : 0.; 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[i] = sptr[i]*norm; 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = cvNorm( src, 0, norm_type, mask ); 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = scale > DBL_EPSILON ? 1./scale : 0.; 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn shift = 0; 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Unknown/unsupported norm type" ); 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !mask ) 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertScale( src, dst, scale, shift ); 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *dmat; 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( dmat = cvGetMat(dst, &stub)); 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp = cvCreateMat(dmat->rows, dmat->cols, dmat->type) ); 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertScale( src, tmp, scale, shift ); 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( tmp, dst, mask ); 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tmp ) 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp ); 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* rng, double iter_factor ) 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvRandShuffle" ); 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int sizeof_int = (int)sizeof(int); 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *mat = (CvMat*)arr; 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, iters, delta = 0; 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cont_flag, arr_size, elem_size, cols, step; 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int pair_buf_sz = 100; 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* pair_buf = (int*)cvStackAlloc( pair_buf_sz*sizeof(pair_buf[0])*2 ); 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _pair_buf = cvMat( 1, pair_buf_sz*2, CV_32S, pair_buf ); 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRNG _rng = cvRNG(-1); 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* data = 0; 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* idata = 0; 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(mat) ) 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( mat = cvGetMat( mat, &stub )); 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !rng ) 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rng = &_rng; 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cols = mat->cols; 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = mat->step; 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr_size = cols*mat->rows; 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iters = cvRound(iter_factor*arr_size)*2; 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cont_flag = CV_IS_MAT_CONT(mat->type); 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size = CV_ELEM_SIZE(mat->type); 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( elem_size % sizeof_int == 0 && (cont_flag || step % sizeof_int == 0) ) 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn idata = mat->data.i; 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step /= sizeof_int; 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size /= sizeof_int; 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn data = mat->data.ptr; 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < iters; i += delta ) 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta = MIN( iters - i, pair_buf_sz*2 ); 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pair_buf.cols = delta; 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRandArr( rng, &_pair_buf, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(arr_size) ); 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cont_flag ) 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( idata ) 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < delta; j += 2 ) 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* p = idata + pair_buf[j]*elem_size, *q = idata + pair_buf[j+1]*elem_size, t; 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < elem_size; k++ ) 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( p[k], q[k], t ); 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < delta; j += 2 ) 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* p = data + pair_buf[j]*elem_size, *q = data + pair_buf[j+1]*elem_size, t; 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < elem_size; k++ ) 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( p[k], q[k], t ); 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( idata ) 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < delta; j += 2 ) 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2; 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* p, *q, t; 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn row1 = idx1/step; row2 = idx2/step; 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = idata + row1*step + (idx1 - row1*cols)*elem_size; 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q = idata + row2*step + (idx2 - row2*cols)*elem_size; 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < elem_size; k++ ) 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( p[k], q[k], t ); 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < delta; j += 2 ) 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2; 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* p, *q, t; 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn row1 = idx1/step; row2 = idx2/step; 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = data + row1*step + (idx1 - row1*cols)*elem_size; 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q = data + row2*step + (idx2 - row2*cols)*elem_size; 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < elem_size; k++ ) 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( p[k], q[k], t ); 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL CvArr* 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvRange( CvArr* arr, double start, double end ) 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ok = 0; 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvRange" ); 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *mat = (CvMat*)arr; 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double delta; 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type, step; 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = start; 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int rows, cols; 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(mat) ) 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( mat = cvGetMat( mat, &stub) ); 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rows = mat->rows; 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cols = mat->cols; 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE(mat->type); 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta = (end-start)/(rows*cols); 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT_CONT(mat->type) ) 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cols *= rows; 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rows = 1; 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = 1; 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = mat->step / CV_ELEM_SIZE(type); 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32SC1 ) 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* idata = mat->data.i; 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ival = cvRound(val), idelta = cvRound(delta); 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(val - ival) < DBL_EPSILON && 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fabs(delta - idelta) < DBL_EPSILON ) 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < rows; i++, idata += step ) 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < cols; j++, ival += idelta ) 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn idata[j] = ival; 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < rows; i++, idata += step ) 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < cols; j++, val += delta ) 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn idata[j] = cvRound(val); 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_32FC1 ) 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* fdata = mat->data.fl; 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < rows; i++, fdata += step ) 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < cols; j++, val += delta ) 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fdata[j] = (float)val; 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" ); 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ok = 1; 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return ok ? arr : 0; 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_LT_BY_IDX(a, b) (aux[a] < aux[b]) 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSortIdx64f, int, ICV_LT_BY_IDX, const double* ) 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSortIdx32f, int, ICV_LT_BY_IDX, const float* ) 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSortIdx32s, int, ICV_LT_BY_IDX, const int* ) 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSortIdx16u, int, ICV_LT_BY_IDX, const ushort* ) 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSortIdx16s, int, ICV_LT_BY_IDX, const short* ) 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSortIdx8u, int, ICV_LT_BY_IDX, const uchar* ) 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSortIdx8s, int, ICV_LT_BY_IDX, const schar* ) 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSort64f, double, CV_LT, int ) 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSort32f, float, CV_LT, int ) 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSort32s, int, CV_LT, int ) 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSort16u, ushort, CV_LT, int ) 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSort16s, short, CV_LT, int ) 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSort8u, uchar, CV_LT, int ) 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CV_IMPLEMENT_QSORT_EX( icvSort8s, schar, CV_LT, int ) 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef void (*CvSortFunc)( void* arr, size_t n, int ); 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef void (*CvSortIdxFunc)( int* arr, size_t n, const void* ); 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic inline void 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCopy1D( const void* src, int s, void* dst, int d, int n, int elemSize ) 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn switch( elemSize ) 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 1: 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((uchar*)dst)[i*d] = ((uchar*)src)[i*s]; 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 2: 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((ushort*)dst)[i*d] = ((ushort*)src)[i*s]; 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 4: 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((int*)dst)[i*d] = ((int*)src)[i*s]; 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 8: 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((int64*)dst)[i*d] = ((int64*)src)[i*s]; 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn default: 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(0); 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvShuffle1D( const uchar* src, const int* idx, uchar* dst, int d, int n, int elemSize ) 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn switch( elemSize ) 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 1: 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst[i*d] = src[idx[i]]; 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 2: 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((ushort*)dst)[i*d] = ((ushort*)src)[idx[i]]; 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 4: 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((int*)dst)[i*d] = ((int*)src)[idx[i]]; 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case 8: 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((int64*)dst)[i*d] = ((int64*)src)[idx[i]]; 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn default: 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(0); 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvSort( const CvArr* _src, CvArr* _dst, CvArr* _idx, int flags ) 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *tsrc = 0; 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* tidx = 0; 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME("cvSort"); 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat sstub, *src = cvGetMat(_src, &sstub); 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dstub, *dst = _dst ? cvGetMat(_dst, &dstub) : 0; 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat istub, *idx = _idx ? cvGetMat(_idx, &istub) : 0; 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type = CV_MAT_TYPE(src->type), elemSize = CV_ELEM_SIZE(type); 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sstep = src->step, dstep = dst ? dst->step : 0; 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int istep = idx ? idx->step/sizeof(int) : 0; 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, len = src->cols; 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSortFunc sortFunc = 0; 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSortIdxFunc sortIdxFunc = 0; 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_CN( src->type ) != 1 ) 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "The input matrix should be a one-channel matrix." ); 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( idx ) 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( idx->type ) != CV_32SC1) 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "The index matrix must be CV_32SC1." ); 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( idx, src )) 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "The input matrix and index matrix must be of the same size" ); 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dst ) 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ(src, dst) ) 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "The input and output matrix must be of the same type" ); 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ(src, dst) ) 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "The input and output matrix must be of the same size" ); 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !idx && !dst ) 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "At least one of index array or destination array must be non-NULL" ); 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_8U ) 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc = (CvSortIdxFunc)icvSortIdx8u, sortFunc = (CvSortFunc)icvSort8u; 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_8S ) 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc = (CvSortIdxFunc)icvSortIdx8s, sortFunc = (CvSortFunc)icvSort8s; 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_16U ) 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc = (CvSortIdxFunc)icvSortIdx16u, sortFunc = (CvSortFunc)icvSort16u; 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_16S ) 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc = (CvSortIdxFunc)icvSortIdx16s, sortFunc = (CvSortFunc)icvSort16s; 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_32S ) 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc = (CvSortIdxFunc)icvSortIdx32s, sortFunc = (CvSortFunc)icvSort32s; 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_32F ) 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc = (CvSortIdxFunc)icvSortIdx32f, sortFunc = (CvSortFunc)icvSort32f; 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_64F ) 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc = (CvSortIdxFunc)icvSortIdx64f, sortFunc = (CvSortFunc)icvSort64f; 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Unsupported format of the input array" ); 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // single-column case, where all of src, idx & dst arrays are continuous, is 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // equivalent to "sort every row" mode. 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (flags & CV_SORT_EVERY_COLUMN) && 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (src->cols > 1 || !CV_IS_MAT_CONT(src->type & 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (dst ? dst->type : -1) & (idx ? idx->type : -1))) ) 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dptr = dst ? dst->data.ptr : 0; 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* idxptr = idx ? idx->data.i : 0; 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = src->rows; 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tsrc = (uchar*)cvAlloc(len*elemSize); 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( idx ) 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tidx = (int*)cvAlloc(len*sizeof(tidx[0])); 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i++ ) 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tidx[i] = i; 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_SORT_DESCENDING ) 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr += dstep*(src->rows - 1); 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstep = -dstep; 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn idxptr += istep*(src->rows - 1); 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn istep = -istep; 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sstep /= elemSize; 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstep /= elemSize; 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < src->cols; i++ ) 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopy1D( src->data.ptr + i*elemSize, sstep, tsrc, 1, len, elemSize ); 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tidx ) 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc( tidx, len, tsrc ); 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dst ) 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvShuffle1D( tsrc, tidx, dptr + i*elemSize, dstep, len, elemSize ); 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopy1D( tidx, 1, idxptr + i, istep, len, sizeof(int) ); 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortFunc( tsrc, len, 0 ); 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopy1D( tsrc, 1, dptr + i*elemSize, dstep, len, elemSize ); 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j, t, count = src->rows; 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_SORT_EVERY_COLUMN ) 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( len, count, t ); 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (flags & CV_SORT_DESCENDING) || (idx && dst && dst->data.ptr == src->data.ptr) ) 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tsrc = (uchar*)cvAlloc(len*elemSize); 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !idx ) 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* sptr = src->data.ptr + i*sstep; 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dptr = dst->data.ptr + i*dstep; 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* ptr = flags & CV_SORT_DESCENDING ? tsrc : dptr; 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ptr != sptr ) 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopy1D( sptr, 1, ptr, 1, len, elemSize ); 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortFunc( ptr, len, 0 ); 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_SORT_DESCENDING ) 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopy1D( ptr + (len-1)*elemSize, -1, dptr, 1, len, elemSize ); 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* idx_ = idx->data.i + istep*i; 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* sptr = src->data.ptr + sstep*i; 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dptr = dst ? dst->data.ptr + dstep*i : 0; 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < len; j++ ) 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn idx_[j] = j; 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dptr && dptr == sptr ) 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopy1D( sptr, 1, tsrc, 1, len, elemSize ); 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sptr = tsrc; 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sortIdxFunc( idx_, len, sptr ); 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_SORT_DESCENDING ) 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < len/2; j++ ) 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP(idx_[j], idx_[len-j-1], t); 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dptr ) 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvShuffle1D( sptr, idx_, dptr, 1, len, elemSize ); 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &tsrc ); 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &tidx ); 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 1207