modules/legacy/src/lee.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 /* Reconstruction of contour skeleton */
  43 #include "precomp.hpp"
  44 #include <time.h>
  45
  46 #define NEXT_SEQ(seq,seq_first) ((seq) == (seq_first) ? seq->v_next : seq->h_next)
  47 #define SIGN(x) ( x<0 ? -1:( x>0 ? 1:0 ) )
  48
  49 const float LEE_CONST_ZERO = 1e-6f;
  50 const float LEE_CONST_DIFF_POINTS = 1e-2f;
  51 const float LEE_CONST_ACCEPTABLE_ERROR = 1e-4f;
  52
  53 /****************************************************************************************\
  54 *                                    Auxiliary struct definitions                                 *
  55 \****************************************************************************************/
  56
  57 template<class T>
  58 struct CvLeePoint
  59 {
  60     T x,y;
  61 };
  62
  63 typedef CvLeePoint<float> CvPointFloat;
  64 typedef CvLeePoint<float> CvDirection;
  65
  66 struct CvVoronoiSiteInt;
  67 struct CvVoronoiEdgeInt;
  68 struct CvVoronoiNodeInt;
  69 struct CvVoronoiParabolaInt;
  70 struct CvVoronoiChainInt;
  71 struct CvVoronoiHoleInt;
  72
  73 struct CvVoronoiDiagramInt
  74 {
  75     CvSeq* SiteSeq;
  76     CvSeq* EdgeSeq;
  77     CvSeq* NodeSeq;
  78     CvSeq* ChainSeq;
  79     CvSeq* ParabolaSeq;
  80     CvSeq* DirectionSeq;
  81     CvSeq* HoleSeq;
  82     CvVoronoiSiteInt* reflex_site;
  83     CvVoronoiHoleInt* top_hole;
  84 };
  85
  86 struct CvVoronoiStorageInt
  87 {
  88     CvMemStorage* SiteStorage;
  89     CvMemStorage* EdgeStorage;
  90     CvMemStorage* NodeStorage;
  91     CvMemStorage* ChainStorage;
  92     CvMemStorage* ParabolaStorage;
  93     CvMemStorage* DirectionStorage;
  94     CvMemStorage* HoleStorage;
  95 };
  96
  97 struct CvVoronoiNodeInt
  98 {
  99     CvPointFloat  node;
 100     float         radius;
 101 };
 102
 103 struct CvVoronoiSiteInt
 104 {
 105     CvVoronoiNodeInt* node1;
 106     CvVoronoiNodeInt* node2;
 107     CvVoronoiEdgeInt* edge1;
 108     CvVoronoiEdgeInt* edge2;
 109     CvVoronoiSiteInt* next_site;
 110     CvVoronoiSiteInt* prev_site;
 111     CvDirection* direction;
 112 };
 113
 114 struct CvVoronoiEdgeInt
 115 {
 116     CvVoronoiNodeInt* node1;
 117     CvVoronoiNodeInt* node2;
 118     CvVoronoiSiteInt* site;
 119     CvVoronoiEdgeInt* next_edge;
 120     CvVoronoiEdgeInt* prev_edge;
 121     CvVoronoiEdgeInt* twin_edge;
 122     CvVoronoiParabolaInt* parabola;
 123     CvDirection*  direction;
 124 };
 125
 126 struct CvVoronoiParabolaInt
 127 {
 128     float map[6];
 129     float a;
 130     CvVoronoiNodeInt* focus;
 131     CvVoronoiSiteInt* directrice;
 132 };
 133
 134 struct CvVoronoiChainInt
 135 {
 136     CvVoronoiSiteInt * first_site;
 137     CvVoronoiSiteInt * last_site;
 138     CvVoronoiChainInt* next_chain;
 139 };
 140
 141 struct CvVoronoiHoleInt
 142 {
 143     CvSeq* SiteSeq;
 144     CvSeq* ChainSeq;
 145     CvVoronoiSiteInt* site_top;
 146     CvVoronoiSiteInt* site_nearest;
 147     CvVoronoiSiteInt* site_opposite;
 148     CvVoronoiNodeInt* node;
 149     CvVoronoiHoleInt* next_hole;
 150     bool error;
 151     float x_coord;
 152 };
 153
 154 typedef CvVoronoiSiteInt* pCvVoronoiSite;
 155 typedef CvVoronoiEdgeInt* pCvVoronoiEdge;
 156 typedef CvVoronoiNodeInt* pCvVoronoiNode;
 157 typedef CvVoronoiParabolaInt* pCvVoronoiParabola;
 158 typedef CvVoronoiChainInt* pCvVoronoiChain;
 159 typedef CvVoronoiHoleInt* pCvVoronoiHole;
 160 typedef CvPointFloat* pCvPointFloat;
 161 typedef CvDirection* pCvDirection;
 162
 163 /****************************************************************************************\
 164 *                                    Function definitions                                *
 165 \****************************************************************************************/
 166
 167 /*F///////////////////////////////////////////////////////////////////////////////////////
 168 //    Author:  Andrey Sobolev
 169 //    Name:    _cvLee
 170 //    Purpose: Compute Voronoi Diagram for one given polygon with holes
 171 //    Context:
 172 //    Parameters:
 173 //      ContourSeq : in, vertices of polygon.
 174 //      VoronoiDiagramInt : in&out, pointer to struct, which contains the
 175 //                       description of Voronoi Diagram.
 176 //      VoronoiStorage: in, storage for Voronoi Diagram.
 177 //      contour_type: in, type of vertices.
 178 //                    The possible values are CV_LEE_INT,CV_LEE_FLOAT,CV_LEE_DOUBLE.
 179 //      contour_orientation: in, orientation of polygons.
 180 //                           = 1, if contour is left - oriented in left coordinat system
 181 //                           =-1, if contour is left - oriented in right coordinat system
 182 //      attempt_number: in, number of unsuccessful attemts made by program to compute
 183 //                          the Voronoi Diagram befor return the error
 184 //
 185 //    Returns: 1, if Voronoi Diagram was succesfully computed
 186 //             0, if some error occures
 187 //F*/
 188 static int  _cvLee(CvSeq* ContourSeq,
 189                       CvVoronoiDiagramInt* pVoronoiDiagramInt,
 190                       CvMemStorage* VoronoiStorage,
 191                       CvLeeParameters contour_type,
 192                       int contour_orientation,
 193                       int attempt_number);
 194
 195 /*F///////////////////////////////////////////////////////////////////////////////////////
 196 //    Author:  Andrey Sobolev
 197 //    Name:    _cvConstuctSites
 198 //    Purpose : Compute sites for given polygon with holes
 199 //                     (site is an edge of polygon or a reflex vertex).
 200 //    Context:
 201 //    Parameters:
 202 //            ContourSeq : in, vertices of polygon
 203 //       pVoronoiDiagram : in, pointer to struct, which contains the
 204 //                          description of Voronoi Diagram
 205 //           contour_type: in, type of vertices.  The possible values are
 206 //                          CV_LEE_INT,CV_LEE_FLOAT,CV_LEE_DOUBLE.
 207 //    contour_orientation: in, orientation of polygons.
 208 //                           = 1, if contour is left - oriented in left coordinat system
 209 //                           =-1, if contour is left - oriented in right coordinat system
 210 //     Return: 1, if sites were succesfully constructed
 211 //             0, if some error occures
 212 //F*/
 213 static int _cvConstuctSites(CvSeq* ContourSeq,
 214                             CvVoronoiDiagramInt* pVoronoiDiagram,
 215                             CvLeeParameters contour_type,
 216                             int contour_orientation);
 217
 218 /*F///////////////////////////////////////////////////////////////////////////////////////
 219 //    Author:  Andrey Sobolev
 220 //    Name:    _cvConstructChains
 221 //    Purpose : Compute chains for given polygon with holes.
 222 //    Context:
 223 //    Parameters:
 224 //       pVoronoiDiagram : in, pointer to struct, which contains the
 225 //                          description of Voronoi Diagram
 226 //     Return: 1, if chains were succesfully constructed
 227 //             0, if some error occures
 228 //F*/
 229 static int _cvConstructChains(CvVoronoiDiagramInt* pVoronoiDiagram);
 230
 231 /*F///////////////////////////////////////////////////////////////////////////////////////
 232 //    Author:  Andrey Sobolev
 233 //    Name:    _cvConstructSkeleton
 234 //    Purpose: Compute skeleton for given collection of sites, using Lee algorithm
 235 //    Context:
 236 //    Parameters:
 237 //      VoronoiDiagram : in, pointer to struct, which contains the
 238 //                       description of Voronoi Diagram.
 239 //    Returns: 1, if skeleton was succesfully computed
 240 //             0, if some error occures
 241 //F*/
 242 static int _cvConstructSkeleton(CvVoronoiDiagramInt* pVoronoiDiagram);
 243
 244 /*F///////////////////////////////////////////////////////////////////////////////////////
 245 //    Author:  Andrey Sobolev
 246 //    Name:    _cvConstructSiteTree
 247 //    Purpose: Construct tree of sites (by analogy with contour tree).
 248 //    Context:
 249 //    Parameters:
 250 //      VoronoiDiagram : in, pointer to struct, which contains the
 251 //                       description of Voronoi Diagram.
 252 //    Returns:
 253 //F*/
 254 static void _cvConstructSiteTree(CvVoronoiDiagramInt* pVoronoiDiagram);
 255
 256 /*F///////////////////////////////////////////////////////////////////////////////////////
 257 //    Author:   Andrey Sobolev
 258 //    Name:     _cvReleaseVoronoiStorage
 259 //    Purpose : Function realease storages.
 260 //                  The storages are divided into two groups:
 261 //                  SiteStorage, EdgeStorage, NodeStorage form the first group;
 262 //                  ChainStorage,ParabolaStorage,DirectionStorage,HoleStorage form the second group.
 263 //    Context:
 264 //    Parameters:
 265 //        pVoronoiStorage: in,
 266 //        group1,group2: in, if group1<>0 then storages from first group released
 267 //                           if group2<>0 then storages from second group released
 268 //    Return    :
 269 //F*/
 270 static void _cvReleaseVoronoiStorage(CvVoronoiStorageInt* pVoronoiStorage, int group1, int group2);
 271
 272 /*F///////////////////////////////////////////////////////////////////////////////////////
 273 //  Author:  Andrey Sobolev
 274 //  Name:  _cvConvert
 275 //  Purpose :  Function convert internal representation of VD (via
 276 //                  structs CvVoronoiSiteInt, CvVoronoiEdgeInt,CvVoronoiNodeInt) into
 277 //                  external representation of VD (via structs CvVoronoiSite2D, CvVoronoiEdge2D,
 278 //                  CvVoronoiNode2D)
 279 //    Context:
 280 //    Parameters:
 281 //        VoronoiDiagram: in
 282 //        VoronoiStorage: in
 283 //        change_orientation: in, if = -1 then the convertion is accompanied with change
 284 //                            of orientation
 285 //
 286 //     Return: 1, if convertion was succesfully completed
 287 //             0, if some error occures
 288 //F*/
 289 /*
 290 static int _cvConvert(CvVoronoiDiagram2D* VoronoiDiagram,
 291                       CvMemStorage* VoronoiStorage,
 292                       int change_orientation);
 293 */
 294 static int _cvConvert(CvVoronoiDiagram2D* VoronoiDiagram,
 295                        CvVoronoiDiagramInt VoronoiDiagramInt,
 296                        CvSet* &NewSiteSeqPrev,
 297                        CvSeqWriter &NodeWriter,
 298                        CvSeqWriter &EdgeWriter,
 299                        CvMemStorage* VoronoiStorage,
 300                        int change_orientation);
 301
 302 /*F///////////////////////////////////////////////////////////////////////////////////////
 303 //  Author:  Andrey Sobolev
 304 //  Name:  _cvConvertSameOrientation
 305 //  Purpose :  Function convert internal representation of VD (via
 306 //                  structs CvVoronoiSiteInt, CvVoronoiEdgeInt,CvVoronoiNodeInt) into
 307 //                  external representation of VD (via structs CvVoronoiSite2D, CvVoronoiEdge2D,
 308 //                  CvVoronoiNode2D) without change of orientation
 309 //    Context:
 310 //    Parameters:
 311 //        VoronoiDiagram: in
 312 //        VoronoiStorage: in
 313 /
 314 //     Return: 1, if convertion was succesfully completed
 315 //             0, if some error occures
 316 //F*/
 317 /*
 318 static int _cvConvertSameOrientation(CvVoronoiDiagram2D* VoronoiDiagram,
 319                                       CvMemStorage* VoronoiStorage);
 320 */
 321 static int _cvConvertSameOrientation(CvVoronoiDiagram2D* VoronoiDiagram,
 322                        CvVoronoiDiagramInt VoronoiDiagramInt,
 323                        CvSet* &NewSiteSeqPrev,
 324                        CvSeqWriter &NodeWriter,
 325                        CvSeqWriter &EdgeWriter,
 326                        CvMemStorage* VoronoiStorage);
 327
 328 /*F///////////////////////////////////////////////////////////////////////////////////////
 329 //  Author:  Andrey Sobolev
 330 //  Name:  _cvConvertChangeOrientation
 331 //  Purpose :  Function convert internal representation of VD (via
 332 //                  structs CvVoronoiSiteInt, CvVoronoiEdgeInt,CvVoronoiNodeInt) into
 333 //                  external representation of VD (via structs CvVoronoiSite2D, CvVoronoiEdge2D,
 334 //                  CvVoronoiNode2D) with change of orientation
 335 //    Context:
 336 //    Parameters:
 337 //        VoronoiDiagram: in
 338 //        VoronoiStorage: in
 339 /
 340 //     Return: 1, if convertion was succesfully completed
 341 //             0, if some error occures
 342 //F*/
 343 /*
 344 static int _cvConvertChangeOrientation(CvVoronoiDiagram2D* VoronoiDiagram,
 345                                       CvMemStorage* VoronoiStorage);
 346                                       */
 347 static int _cvConvertChangeOrientation(CvVoronoiDiagram2D* VoronoiDiagram,
 348                        CvVoronoiDiagramInt VoronoiDiagramInt,
 349                        CvSet* &NewSiteSeqPrev,
 350                        CvSeqWriter &NodeWriter,
 351                        CvSeqWriter &EdgeWriter,
 352                        CvMemStorage* VoronoiStorage);
 353
 354 /*--------------------------------------------------------------------------
 355     Author      : Andrey Sobolev
 356     Description : Compute  sites for external polygon.
 357     Arguments
 358      pVoronoiDiagram : in, pointer to struct, which contains the
 359                         description of Voronoi Diagram
 360      ContourSeq : in, vertices of polygon
 361      pReflexSite: out, pointer to reflex site,if any exist,else NULL
 362      orientation: in, orientation of contour ( = 1 or = -1)
 363      type:        in, type of vertices. The possible values are (int)1,
 364                    (float)1,(double)1.
 365      Return:    1, if sites were succesfully constructed
 366                 0, if some error occures    :
 367     --------------------------------------------------------------------------*/
 368 template<class T>
 369 int _cvConstructExtSites(CvVoronoiDiagramInt* pVoronoiDiagram,
 370                          CvSeq* ContourSeq,
 371                          int orientation,
 372                          T /*type*/);
 373
 374 /*--------------------------------------------------------------------------
 375     Author      : Andrey Sobolev
 376     Description : Compute  sites for internal polygon (for hole).
 377     Arguments
 378      pVoronoiDiagram : in, pointer to struct, which contains the
 379                         description of Voronoi Diagram
 380      CurrSiteSeq: in, the sequence for sites to be constructed
 381      CurrContourSeq : in, vertices of polygon
 382      pTopSite: out, pointer to the most left site of polygon (it is the most left
 383                 vertex of polygon)
 384      orientation: in, orientation of contour ( = 1 or = -1)
 385      type:        in, type of vertices. The possible values are (int)1,
 386                    (float)1,(double)1.
 387      Return:    1, if sites were succesfully constructed
 388                 0, if some error occures    :
 389     --------------------------------------------------------------------------*/
 390 template<class T>
 391 int _cvConstructIntSites(CvVoronoiDiagramInt* pVoronoiDiagram,
 392                          CvSeq* CurrSiteSeq,
 393                          CvSeq* CurrContourSeq,
 394                          pCvVoronoiSite &pTopSite,
 395                          int orientation,
 396                          T /*type*/);
 397
 398 /*--------------------------------------------------------------------------
 399     Author      : Andrey Sobolev
 400     Description : Compute the simple chains of sites for external polygon.
 401     Arguments
 402      pVoronoiDiagram : in&out, pointer to struct, which contains the
 403                         description of Voronoi Diagram
 404
 405     Return: 1, if chains were succesfully constructed
 406             0, if some error occures
 407     --------------------------------------------------------------------------*/
 408 static int _cvConstructExtChains(CvVoronoiDiagramInt* pVoronoiDiagram);
 409
 410 /*--------------------------------------------------------------------------
 411     Author      : Andrey Sobolev
 412     Description : Compute the simple chains of sites for internal polygon (= hole)
 413     Arguments
 414     pVoronoiDiagram : in, pointer to struct, which contains the
 415                         description of Voronoi Diagram
 416     CurrSiteSeq : in, the sequence of sites
 417    CurrChainSeq : in,the sequence for chains to be constructed
 418        pTopSite : in, the most left site of hole
 419
 420       Return    :
 421     --------------------------------------------------------------------------*/
 422 static void _cvConstructIntChains(CvVoronoiDiagramInt* pVoronoiDiagram,
 423                                   CvSeq* CurrChainSeq,
 424                                   CvSeq* CurrSiteSeq,
 425                                   pCvVoronoiSite pTopSite);
 426
 427 /*--------------------------------------------------------------------------
 428     Author      : Andrey Sobolev
 429     Description : Compute the initial Voronoi Diagram for single site
 430     Arguments
 431      pVoronoiDiagram : in, pointer to struct, which contains the
 432                         description of Voronoi Diagram
 433      pSite: in, pointer to site
 434
 435      Return :
 436     --------------------------------------------------------------------------*/
 437 static void _cvConstructEdges(pCvVoronoiSite pSite,CvVoronoiDiagramInt* pVoronoiDiagram);
 438
 439 /*--------------------------------------------------------------------------
 440     Author      : Andrey Sobolev
 441     Description : Function moves each node on small random value. The nodes are taken
 442                     from pVoronoiDiagram->NodeSeq.
 443     Arguments
 444      pVoronoiDiagram : in, pointer to struct, which contains the
 445                         description of Voronoi Diagram
 446      begin,end: in, the first and the last nodes in pVoronoiDiagram->NodeSeq,
 447                     which moves
 448      shift: in, the value of maximal shift.
 449      Return :
 450     --------------------------------------------------------------------------*/
 451 static void _cvRandomModification(CvVoronoiDiagramInt* pVoronoiDiagram, int begin, int end, float shift);
 452
 453 /*--------------------------------------------------------------------------
 454     Author      : Andrey Sobolev
 455     Description : Compute the internal Voronoi Diagram for external polygon.
 456     Arguments
 457      pVoronoiDiagram : in, pointer to struct, which contains the
 458                         description of Voronoi Diagram
 459      Return     : 1, if VD was constructed succesfully
 460                   0, if some error occure
 461     --------------------------------------------------------------------------*/
 462 static int _cvConstructExtVD(CvVoronoiDiagramInt* pVoronoiDiagram);
 463
 464 /*--------------------------------------------------------------------------
 465     Author      : Andrey Sobolev
 466     Description : Compute the external Voronoi Diagram for each internal polygon (hole).
 467     Arguments
 468      pVoronoiDiagram : in, pointer to struct, which contains the
 469                         description of Voronoi Diagram
 470      Return     :
 471     --------------------------------------------------------------------------*/
 472 static void _cvConstructIntVD(CvVoronoiDiagramInt* pVoronoiDiagram);
 473
 474 /*--------------------------------------------------------------------------
 475     Author      : Andrey Sobolev
 476     Description : Function joins the Voronoi Diagrams of different
 477                     chains into one Voronoi Diagram
 478     Arguments
 479      pVoronoiDiagram : in, pointer to struct, which contains the
 480                         description of Voronoi Diagram
 481      pChain1,pChain1: in, given chains
 482      Return     : 1, if joining was succesful
 483                   0, if some error occure
 484     --------------------------------------------------------------------------*/
 485 static int _cvJoinChains(pCvVoronoiChain pChain1,
 486                          pCvVoronoiChain pChain2,
 487                          CvVoronoiDiagramInt* pVoronoiDiagram);
 488
 489 /*--------------------------------------------------------------------------
 490     Author      : Andrey Sobolev
 491     Description : Function finds the nearest site for top vertex
 492                  (= the most left vertex) of each hole
 493     Arguments
 494         pVoronoiDiagram : in, pointer to struct, which contains the
 495                          description of Voronoi Diagram
 496      Return     :
 497     --------------------------------------------------------------------------*/
 498 static void _cvFindNearestSite(CvVoronoiDiagramInt* pVoronoiDiagram);
 499
 500 /*--------------------------------------------------------------------------
 501     Author      : Andrey Sobolev
 502     Description : Function seeks for site, which has common bisector in
 503                   final VD with top vertex of given hole. It stores in pHole->opposite_site.
 504                    The search begins from  Hole->nearest_site and realizes in clockwise
 505                    direction around the top vertex of given hole.
 506     Arguments
 507         pVoronoiDiagram : in, pointer to struct, which contains the
 508                           description of Voronoi Diagram
 509           pHole : in, given hole
 510      Return     : 1, if the search was succesful
 511                   0, if some error occure
 512     --------------------------------------------------------------------------*/
 513 static int _cvFindOppositSiteCW(pCvVoronoiHole pHole, CvVoronoiDiagramInt* pVoronoiDiagram);
 514
 515 /*--------------------------------------------------------------------------
 516     Author      : Andrey Sobolev
 517     Description : Function seeks for site, which has common bisector in
 518                   final VD with top vertex of given hole. It stores in pHole->opposite_site.
 519                    The search begins from  Hole->nearest_site and realizes in counterclockwise
 520                    direction around the top vertex of given hole.
 521     Arguments
 522         pVoronoiDiagram : in, pointer to struct, which contains the
 523                         description of Voronoi Diagram
 524           pHole : in, given hole
 525      Return     : 1, if the search was succesful
 526                   0, if some error occure
 527     --------------------------------------------------------------------------*/
 528 static int _cvFindOppositSiteCCW(pCvVoronoiHole pHole,CvVoronoiDiagramInt* pVoronoiDiagram);
 529
 530 /*--------------------------------------------------------------------------
 531     Author      : Andrey Sobolev
 532     Description : Function merges external VD of hole and internal VD, which was
 533                   constructed ealier.
 534     Arguments
 535 pVoronoiDiagram : in, pointer to struct, which contains the
 536                         description of Voronoi Diagram
 537           pHole : in, given hole
 538      Return     : 1, if merging was succesful
 539                   0, if some error occure
 540     --------------------------------------------------------------------------*/
 541 static int _cvMergeVD(pCvVoronoiHole pHole,CvVoronoiDiagramInt* pVoronoiDiagram);
 542
 543
 544 /* ///////////////////////////////////////////////////////////////////////////////////////
 545 //                               Computation of bisectors                               //
 546 /////////////////////////////////////////////////////////////////////////////////////// */
 547
 548 /*--------------------------------------------------------------------------
 549     Author      : Andrey Sobolev
 550     Description : Compute the bisector of two sites
 551     Arguments
 552      pSite_left,pSite_right: in, given sites
 553      pVoronoiDiagram : in, pointer to struct, which contains the
 554                         description of Voronoi Diagram
 555      pEdge      : out, bisector
 556      Return     :
 557     --------------------------------------------------------------------------*/
 558 void _cvCalcEdge(pCvVoronoiSite pSite_left,
 559                 pCvVoronoiSite pSite_right,
 560                 pCvVoronoiEdge pEdge,
 561                 CvVoronoiDiagramInt* pVoronoiDiagram);
 562
 563 /*--------------------------------------------------------------------------
 564     Author      : Andrey Sobolev
 565     Description : Compute the bisector of point and site
 566     Arguments
 567      pSite      : in, site
 568      pNode      : in, point
 569      pVoronoiDiagram : in, pointer to struct, which contains the
 570                         description of Voronoi Diagram
 571      pEdge      : out, bisector
 572      Return     :
 573     --------------------------------------------------------------------------*/
 574 void _cvCalcEdge(pCvVoronoiSite pSite,
 575                 pCvVoronoiNode pNode,
 576                 pCvVoronoiEdge pEdge,
 577                 CvVoronoiDiagramInt* pVoronoiDiagram);
 578
 579 /*--------------------------------------------------------------------------
 580     Author      : Andrey Sobolev
 581     Description : Compute the bisector of point and site
 582     Arguments
 583      pSite      : in, site
 584      pNode      : in, point
 585      pVoronoiDiagram : in, pointer to struct, which contains the
 586                         description of Voronoi Diagram
 587      pEdge      : out, bisector
 588      Return     :
 589     --------------------------------------------------------------------------*/
 590 void _cvCalcEdge(pCvVoronoiNode pNode,
 591                 pCvVoronoiSite pSite,
 592                 pCvVoronoiEdge pEdge,
 593                 CvVoronoiDiagramInt* pVoronoiDiagram);
 594
 595 /*--------------------------------------------------------------------------
 596     Author      : Andrey Sobolev
 597     Description : Compute the direction of bisector of two segments
 598     Arguments
 599      pDirection1: in, direction of first segment
 600      pDirection2: in, direction of second segment
 601      pVoronoiDiagram : in, pointer to struct, which contains the
 602                         description of Voronoi Diagram
 603      pEdge      : out, bisector
 604      Return     :
 605     --------------------------------------------------------------------------*/
 606 CV_INLINE
 607 void _cvCalcEdgeLL(pCvDirection pDirection1,
 608                   pCvDirection pDirection2,
 609                   pCvVoronoiEdge pEdge,
 610                   CvVoronoiDiagramInt* pVoronoiDiagram);
 611
 612 /*--------------------------------------------------------------------------
 613     Author      : Andrey Sobolev
 614     Description : Compute the bisector of two points
 615     Arguments
 616      pPoint1, pPoint2: in, given points
 617      pVoronoiDiagram : in, pointer to struct, which contains the
 618                         description of Voronoi Diagram
 619      pEdge      : out, bisector
 620      Return     :
 621     --------------------------------------------------------------------------*/
 622 CV_INLINE
 623 void _cvCalcEdgePP(pCvPointFloat pPoint1,
 624                   pCvPointFloat pPoint2,
 625                   pCvVoronoiEdge pEdge,
 626                   CvVoronoiDiagramInt* pVoronoiDiagram);
 627
 628 /*--------------------------------------------------------------------------
 629     Author      : Andrey Sobolev
 630     Description : Compute the bisector of segment and point. Since
 631                     it is parabola, it is defined by its focus (site - point)
 632                     and directrice(site-segment)
 633     Arguments
 634      pFocus    : in, point, which defines the focus of parabola
 635      pDirectrice: in, site - segment, which defines the directrice of parabola
 636      pVoronoiDiagram : in, pointer to struct, which contains the
 637                         description of Voronoi Diagram
 638      pEdge      : out, bisector
 639      Return     :
 640     --------------------------------------------------------------------------*/
 641 CV_INLINE
 642 void _cvCalcEdgePL(pCvVoronoiNode pFocus,
 643                   pCvVoronoiSite pDirectrice,
 644                   pCvVoronoiEdge pEdge,
 645                   CvVoronoiDiagramInt* pVoronoiDiagram);
 646
 647 /*--------------------------------------------------------------------------
 648     Author      : Andrey Sobolev
 649     Description : Compute the bisector of segment and point. Since
 650                     it is parabola, it is defined by its focus (site - point)
 651                     and directrice(site-segment)
 652     Arguments
 653      pFocus    : in, point, which defines the focus of parabola
 654      pDirectrice: in, site - segment, which defines the directrice of parabola
 655      pVoronoiDiagram : in, pointer to struct, which contains the
 656                         description of Voronoi Diagram
 657      pEdge      : out, bisector
 658      Return     :
 659     --------------------------------------------------------------------------*/
 660 CV_INLINE
 661 void _cvCalcEdgeLP(pCvVoronoiSite pDirectrice,
 662                   pCvVoronoiNode pFocus,
 663                   pCvVoronoiEdge pEdge,
 664                   CvVoronoiDiagramInt* pVoronoiDiagram);
 665
 666 /* ///////////////////////////////////////////////////////////////////////////////////////
 667 //                  Computation of intersections of bisectors                           //
 668 /////////////////////////////////////////////////////////////////////////////////////// */
 669
 670 /*--------------------------------------------------------------------------
 671     Author      : Andrey Sobolev
 672     Description : Function computes intersection of two edges. Intersection
 673                     must be the nearest to the marked point on pEdge1
 674                     (this marked point is pEdge1->node1->node).
 675     Arguments
 676     pEdge1,pEdge2: in, two edges
 677         pPoint: out, intersection of pEdge1 and pEdge2
 678         Radius: out, distance between pPoint and sites, assosiated
 679                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 680                     distance from site, assosiated with pEdge1 and from
 681                     site,assosiated with pEdge2)
 682       Return    : distance between pPoint and marked point on pEdge1 or
 683                 : -1, if edges have no intersections
 684     --------------------------------------------------------------------------*/
 685 static
 686 float _cvCalcEdgeIntersection(pCvVoronoiEdge pEdge1,
 687                               pCvVoronoiEdge pEdge2,
 688                               CvPointFloat* pPoint,
 689                               float &Radius);
 690
 691 /*--------------------------------------------------------------------------
 692     Author      : Andrey Sobolev
 693     Description : Function computes intersection of two edges. Intersection
 694                     must be the nearest to the marked point on pEdge1
 695                     (this marked point is pEdge1->node1->node).
 696     Arguments
 697     pEdge1 : in, straight ray
 698     pEdge2: in, straight ray or segment
 699         pPoint: out, intersection of pEdge1 and pEdge2
 700         Radius: out, distance between pPoint and sites, assosiated
 701                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 702                     distance from site, assosiated with pEdge1 and from
 703                     site,assosiated with pEdge2)
 704      Return : distance between pPoint and marked point on pEdge1 or
 705                 : -1, if edges have no intersections
 706     --------------------------------------------------------------------------*/
 707 static
 708 float _cvLine_LineIntersection(pCvVoronoiEdge pEdge1,
 709                                 pCvVoronoiEdge pEdge2,
 710                                 pCvPointFloat  pPoint,
 711                                 float &Radius);
 712
 713 /*--------------------------------------------------------------------------
 714     Author      : Andrey Sobolev
 715     Description : Function computes intersection of two edges. Intersection
 716                     must be the nearest to the marked point on pEdge1
 717                     (this marked point is pEdge1->node1->node).
 718     Arguments
 719     pEdge1 : in, straight ray
 720     pEdge2: in, parabolic ray or segment
 721         pPoint: out, intersection of pEdge1 and pEdge2
 722         Radius: out, distance between pPoint and sites, assosiated
 723                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 724                     distance from site, assosiated with pEdge1 and from
 725                     site,assosiated with pEdge2)
 726       Return    : distance between pPoint and marked point on pEdge1 or
 727                 : -1, if edges have no intersections
 728     --------------------------------------------------------------------------*/
 729 static
 730 float _cvLine_ParIntersection(pCvVoronoiEdge pEdge1,
 731                                 pCvVoronoiEdge pEdge2,
 732                                 pCvPointFloat  pPoint,
 733                                 float &Radius);
 734
 735 /*--------------------------------------------------------------------------
 736     Author      : Andrey Sobolev
 737     Description : Function computes intersection of two edges. Intersection
 738                     must be the nearest to the marked point on pEdge1
 739                     (this marked point is pEdge1->node1->node).
 740     Arguments
 741     pEdge1 : in, straight ray
 742     pEdge2: in, parabolic segment
 743         pPoint: out, intersection of pEdge1 and pEdge2
 744         Radius: out, distance between pPoint and sites, assosiated
 745                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 746                     distance from site, assosiated with pEdge1 and from
 747                     site,assosiated with pEdge2)
 748       Return    : distance between pPoint and marked point on pEdge1 or
 749                 : -1, if edges have no intersections
 750     --------------------------------------------------------------------------*/
 751 static
 752 float _cvLine_CloseParIntersection(pCvVoronoiEdge pEdge1,
 753                                 pCvVoronoiEdge pEdge2,
 754                                 pCvPointFloat  pPoint,
 755                                 float &Radius);
 756
 757 /*--------------------------------------------------------------------------
 758     Author      : Andrey Sobolev
 759     Description : Function computes intersection of two edges. Intersection
 760                     must be the nearest to the marked point on pEdge1
 761                     (this marked point is pEdge1->node1->node).
 762     Arguments
 763     pEdge1 : in, straight ray
 764     pEdge2: in, parabolic ray
 765         pPoint: out, intersection of pEdge1 and pEdge2
 766         Radius: out, distance between pPoint and sites, assosiated
 767                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 768                     distance from site, assosiated with pEdge1 and from
 769                     site,assosiated with pEdge2)
 770       Return    : distance between pPoint and marked point on pEdge1 or
 771                 : -1, if edges have no intersections
 772     --------------------------------------------------------------------------*/
 773 static
 774 float _cvLine_OpenParIntersection(pCvVoronoiEdge pEdge1,
 775                                 pCvVoronoiEdge pEdge2,
 776                                 pCvPointFloat  pPoint,
 777                                 float &Radius);
 778
 779 /*--------------------------------------------------------------------------
 780     Author      : Andrey Sobolev
 781     Description : Function computes intersection of two edges. Intersection
 782                     must be the nearest to the marked point on pEdge1
 783                     (this marked point is pEdge1->node1->node).
 784     Arguments
 785     pEdge1 : in,  parabolic ray
 786     pEdge2: in,  straight ray or segment
 787         pPoint: out, intersection of pEdge1 and pEdge2
 788         Radius: out, distance between pPoint and sites, assosiated
 789                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 790                     distance from site, assosiated with pEdge1 and from
 791                     site,assosiated with pEdge2)
 792       Return    : distance between pPoint and marked point on pEdge1 or
 793                 : -1, if edges have no intersections
 794     --------------------------------------------------------------------------*/
 795 static
 796 float _cvPar_LineIntersection(pCvVoronoiEdge pEdge1,
 797                                 pCvVoronoiEdge pEdge2,
 798                                 pCvPointFloat  pPoint,
 799                                 float &Radius);
 800 /*--------------------------------------------------------------------------
 801     Author      : Andrey Sobolev
 802     Description : Function computes intersection of two edges. Intersection
 803                     must be the nearest to the marked point on pEdge1
 804                     (this marked point is pEdge1->node1->node).
 805     Arguments
 806     pEdge1 : in,  parabolic ray
 807     pEdge2: in,  straight ray
 808         pPoint: out, intersection of pEdge1 and pEdge2
 809         Radius: out, distance between pPoint and sites, assosiated
 810                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 811                     distance from site, assosiated with pEdge1 and from
 812                     site,assosiated with pEdge2)
 813       Return    : distance between pPoint and marked point on pEdge1 or
 814                 : -1, if edges have no intersections
 815     --------------------------------------------------------------------------*/
 816 static
 817 float _cvPar_OpenLineIntersection(pCvVoronoiEdge pEdge1,
 818                                 pCvVoronoiEdge pEdge2,
 819                                 pCvPointFloat  pPoint,
 820                                 float &Radius);
 821
 822 /*--------------------------------------------------------------------------
 823     Author      : Andrey Sobolev
 824     Description : Function computes intersection of two edges. Intersection
 825                     must be the nearest to the marked point on pEdge1
 826                     (this marked point is pEdge1->node1->node).
 827     Arguments
 828     pEdge1 : in,  parabolic ray
 829     pEdge2: in,  straight segment
 830         pPoint: out, intersection of pEdge1 and pEdge2
 831         Radius: out, distance between pPoint and sites, assosiated
 832                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 833                     distance from site, assosiated with pEdge1 and from
 834                     site,assosiated with pEdge2)
 835       Return    : distance between pPoint and marked point on pEdge1 or
 836                 : -1, if edges have no intersections
 837     --------------------------------------------------------------------------*/
 838 static
 839 float _cvPar_CloseLineIntersection(pCvVoronoiEdge pEdge1,
 840                                     pCvVoronoiEdge pEdge2,
 841                                     pCvPointFloat  pPoint,
 842                                     float &Radius);
 843
 844 /*--------------------------------------------------------------------------
 845     Author      : Andrey Sobolev
 846     Description : Function computes intersection of two edges. Intersection
 847                     must be the nearest to the marked point on pEdge1
 848                     (this marked point is pEdge1->node1->node).
 849     Arguments
 850     pEdge1 : in,  parabolic ray
 851     pEdge2: in,  parabolic ray or segment
 852         pPoint: out, intersection of pEdge1 and pEdge2
 853         Radius: out, distance between pPoint and sites, assosiated
 854                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 855                     distance from site, assosiated with pEdge1 and from
 856                     site,assosiated with pEdge2)
 857       Return    : distance between pPoint and marked point on pEdge1 or
 858                 : -1, if edges have no intersections
 859     --------------------------------------------------------------------------*/
 860 static
 861 float _cvPar_ParIntersection(pCvVoronoiEdge pEdge1,
 862                                 pCvVoronoiEdge pEdge2,
 863                                 pCvPointFloat  pPoint,
 864                                 float &Radius);
 865
 866
 867 /*--------------------------------------------------------------------------
 868     Author      : Andrey Sobolev
 869     Description : Function computes intersection of two edges. Intersection
 870                     must be the nearest to the marked point on pEdge1
 871                     (this marked point is pEdge1->node1->node).
 872     Arguments
 873     pEdge1 : in,  parabolic ray
 874     pEdge2: in,  parabolic ray
 875         pPoint: out, intersection of pEdge1 and pEdge2
 876         Radius: out, distance between pPoint and sites, assosiated
 877                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 878                     distance from site, assosiated with pEdge1 and from
 879                     site,assosiated with pEdge2)
 880       Return    : distance between pPoint and marked point on pEdge1 or
 881                 : -1, if edges have no intersections
 882     --------------------------------------------------------------------------*/
 883 static
 884 float _cvPar_OpenParIntersection(pCvVoronoiEdge pEdge1,
 885                             pCvVoronoiEdge pEdge2,
 886                             pCvPointFloat  pPoint,
 887                             float &Radius);
 888
 889 /*--------------------------------------------------------------------------
 890     Author      : Andrey Sobolev
 891     Description : Function computes intersection of two edges. Intersection
 892                     must be the nearest to the marked point on pEdge1
 893                     (this marked point is pEdge1->node1->node).
 894     Arguments
 895     pEdge1 : in,  parabolic ray
 896     pEdge2: in,  parabolic segment
 897         pPoint: out, intersection of pEdge1 and pEdge2
 898         Radius: out, distance between pPoint and sites, assosiated
 899                     with pEdge1 and pEdge2 (pPoint is situated on the equal
 900                     distance from site, assosiated with pEdge1 and from
 901                     site,assosiated with pEdge2)
 902       Return    : distance between pPoint and marked point on pEdge1 or
 903                 : -1, if edges have no intersections
 904     --------------------------------------------------------------------------*/
 905 static
 906 float _cvPar_CloseParIntersection(pCvVoronoiEdge pEdge1,
 907                             pCvVoronoiEdge pEdge2,
 908                             pCvPointFloat  pPoint,
 909                             float &Radius);
 910
 911 /* ///////////////////////////////////////////////////////////////////////////////////////
 912 //                           Subsidiary functions                                       //
 913 /////////////////////////////////////////////////////////////////////////////////////// */
 914
 915 /*--------------------------------------------------------------------------
 916     Author      : Andrey Sobolev
 917     Description :
 918     Arguments
 919         pEdge1  : in
 920         pEdge2  : out
 921      Return     :
 922     --------------------------------------------------------------------------*/
 923 CV_INLINE
 924 void _cvMakeTwinEdge(pCvVoronoiEdge pEdge2,
 925                      pCvVoronoiEdge pEdge1);
 926
 927 /*--------------------------------------------------------------------------
 928     Author      : Andrey Sobolev
 929     Description :
 930     Arguments
 931         pEdge   : in&out
 932         pEdge_left_prev : in&out
 933         pSite_left : in&out
 934      Return     :
 935     --------------------------------------------------------------------------*/
 936 CV_INLINE
 937 void _cvStickEdgeLeftBegin(pCvVoronoiEdge pEdge,
 938                           pCvVoronoiEdge pEdge_left_prev,
 939                           pCvVoronoiSite pSite_left);
 940
 941 /*--------------------------------------------------------------------------
 942     Author      : Andrey Sobolev
 943     Description :
 944     Arguments
 945         pEdge   : in&out
 946         pEdge_right_next : in&out
 947         pSite_right : in&out
 948      Return     :
 949     --------------------------------------------------------------------------*/
 950 CV_INLINE
 951 void _cvStickEdgeRightBegin(pCvVoronoiEdge pEdge,
 952                           pCvVoronoiEdge pEdge_right_next,
 953                           pCvVoronoiSite pSite_right);
 954
 955 /*--------------------------------------------------------------------------
 956     Author      : Andrey Sobolev
 957     Description :
 958     Arguments
 959         pEdge   : in&out
 960         pEdge_left_next : in&out
 961         pSite_left : in&out
 962      Return     :
 963     --------------------------------------------------------------------------*/
 964 CV_INLINE
 965 void _cvStickEdgeLeftEnd(pCvVoronoiEdge pEdge,
 966                         pCvVoronoiEdge pEdge_left_next,
 967                         pCvVoronoiSite pSite_left);
 968
 969 /*--------------------------------------------------------------------------
 970     Author      : Andrey Sobolev
 971     Description :
 972     Arguments
 973         pEdge   : in&out
 974         pEdge_right_prev : in&out
 975         pSite_right : in&out
 976      Return     :
 977     --------------------------------------------------------------------------*/
 978 CV_INLINE
 979 void _cvStickEdgeRightEnd(pCvVoronoiEdge pEdge,
 980                          pCvVoronoiEdge pEdge_right_prev,
 981                          pCvVoronoiSite pSite_right);
 982
 983 /*--------------------------------------------------------------------------
 984     Author      : Andrey Sobolev
 985     Description :
 986     Arguments
 987         pEdge_left_cur  : in
 988         pEdge_left : in
 989      Return     :
 990     --------------------------------------------------------------------------*/
 991 CV_INLINE
 992 void _cvTwinNULLLeft(pCvVoronoiEdge pEdge_left_cur,
 993                     pCvVoronoiEdge pEdge_left);
 994
 995 /*--------------------------------------------------------------------------
 996     Author      : Andrey Sobolev
 997     Description :
 998     Arguments
 999         pEdge_right_cur : in
1000         pEdge_right : in
1001      Return     :
1002     --------------------------------------------------------------------------*/
1003 CV_INLINE
1004 void _cvTwinNULLRight(pCvVoronoiEdge pEdge_right_cur,
1005                      pCvVoronoiEdge pEdge_right);
1006
1007
1008 /*--------------------------------------------------------------------------
1009     Author      : Andrey Sobolev
1010     Description : function initializes the struct CvVoronoiNode
1011     Arguments
1012         pNode   : out
1013          pPoint : in,
1014         radius  : in
1015      Return     :
1016     --------------------------------------------------------------------------*/
1017 template <class T> CV_INLINE
1018 void _cvInitVoronoiNode(pCvVoronoiNode pNode,
1019                        T pPoint, float radius = 0);
1020
1021 /*--------------------------------------------------------------------------
1022     Author      : Andrey Sobolev
1023     Description : function initializes the struct CvVoronoiSite
1024     Arguments
1025         pSite   : out
1026          pNode1,pNode2,pPrev_site : in
1027      Return     :
1028     --------------------------------------------------------------------------*/
1029 CV_INLINE
1030 void _cvInitVoronoiSite(pCvVoronoiSite pSite,
1031                        pCvVoronoiNode pNode1,
1032                        pCvVoronoiNode pNode2,
1033                        pCvVoronoiSite pPrev_site);
1034
1035 /*--------------------------------------------------------------------------
1036     Author      : Andrey Sobolev
1037     Description : function pushs the element in the end of the sequence
1038                     end returns its adress
1039     Arguments
1040             Seq : in, pointer to the sequence
1041            Elem : in, element
1042      Return     : pointer to the element in the sequence
1043     --------------------------------------------------------------------------*/
1044 template <class T> CV_INLINE
1045 T _cvSeqPush(CvSeq* Seq, T pElem);
1046
1047 /*--------------------------------------------------------------------------
1048     Author      : Andrey Sobolev
1049     Description : function pushs the element in the begin of the sequence
1050                     end returns its adress
1051     Arguments
1052             Seq : in, pointer to the sequence
1053            Elem : in, element
1054      Return     : pointer to the element in the sequence
1055     --------------------------------------------------------------------------*/
1056 template <class T> CV_INLINE
1057 T _cvSeqPushFront(CvSeq* Seq, T pElem);
1058
1059 /*--------------------------------------------------------------------------
1060     Author      : Andrey Sobolev
1061     Description : function pushs the element pHole in pHoleHierarchy->HoleSeq
1062                      so as all elements in this sequence would be normalized
1063                      according to field .x_coord of element. pHoleHierarchy->TopHole
1064                      points to hole with smallest x_coord.
1065     Arguments
1066 pHoleHierarchy  : in, pointer to the structur
1067           pHole : in, element
1068      Return     : pointer to the element in the sequence
1069     --------------------------------------------------------------------------*/
1070 CV_INLINE
1071 void _cvSeqPushInOrder(CvVoronoiDiagramInt* pVoronoiDiagram, pCvVoronoiHole pHole);
1072
1073 /*--------------------------------------------------------------------------
1074     Author      : Andrey Sobolev
1075     Description : function intersects given edge pEdge (and his twin edge)
1076                     by point pNode on two parts
1077     Arguments
1078         pEdge   : in, given edge
1079           pNode : in, given point
1080         EdgeSeq : in
1081      Return     : one of parts
1082     --------------------------------------------------------------------------*/
1083 CV_INLINE
1084 pCvVoronoiEdge _cvDivideRightEdge(pCvVoronoiEdge pEdge,
1085                                  pCvVoronoiNode pNode,
1086                                  CvSeq* EdgeSeq);
1087
1088 /*--------------------------------------------------------------------------
1089     Author      : Andrey Sobolev
1090     Description : function intersects given edge pEdge (and his twin edge)
1091                     by point pNode on two parts
1092     Arguments
1093         pEdge   : in, given edge
1094           pNode : in, given point
1095         EdgeSeq : in
1096      Return     : one of parts
1097     --------------------------------------------------------------------------*/
1098 CV_INLINE
1099 pCvVoronoiEdge _cvDivideLeftEdge(pCvVoronoiEdge pEdge,
1100                                 pCvVoronoiNode pNode,
1101                                 CvSeq* EdgeSeq);
1102
1103 /*--------------------------------------------------------------------------
1104     Author      : Andrey Sobolev
1105     Description : function pushs the element in the end of the sequence
1106                     end returns its adress
1107     Arguments
1108           writer: in, writer associated with sequence
1109           pElem : in, element
1110      Return     : pointer to the element in the sequence
1111     --------------------------------------------------------------------------*/
1112 template<class T> CV_INLINE
1113 T _cvWriteSeqElem(T pElem, CvSeqWriter &writer);
1114
1115 /* ///////////////////////////////////////////////////////////////////////////////////////
1116 //                           Mathematical functions                                     //
1117 /////////////////////////////////////////////////////////////////////////////////////// */
1118
1119 /*--------------------------------------------------------------------------
1120     Author      : Andrey Sobolev
1121     Description : Function changes x and y
1122     Arguments
1123             x,y : in&out
1124       Return    :
1125     --------------------------------------------------------------------------*/
1126 template <class T> CV_INLINE
1127 void _cvSwap(T &x, T &y);
1128
1129 /*--------------------------------------------------------------------------
1130     Author      : Andrey Sobolev
1131     Description : Function computes the inverse map to the
1132                     given ortogonal affine map
1133     Arguments
1134             A   : in, given ortogonal affine map
1135             B   : out, inverse map
1136       Return    : 1, if inverse map exist
1137                   0, else
1138     --------------------------------------------------------------------------*/
1139 template <class T> CV_INLINE
1140 int _cvCalcOrtogInverse(T* B, T* A);
1141
1142 /*--------------------------------------------------------------------------
1143     Author      : Andrey Sobolev
1144     Description : Function computes the composition of two affine maps
1145     Arguments
1146             A,B : in, given affine maps
1147             Result: out, composition of A and B (Result = AB)
1148       Return    :
1149     --------------------------------------------------------------------------*/
1150 template <class T> CV_INLINE
1151 void _cvCalcComposition(T* Result,T* A,T* B);
1152
1153 /*--------------------------------------------------------------------------
1154     Author      : Andrey Sobolev
1155     Description : Function computes the image of point under
1156                     given affin map
1157     Arguments
1158             A   : in, affine maps
1159         pPoint  : in, pointer to point
1160         pImgPoint:out, pointer to image of point
1161       Return    :
1162     --------------------------------------------------------------------------*/
1163 template<class T> CV_INLINE
1164 void _cvCalcPointImage(pCvPointFloat pImgPoint,pCvPointFloat pPoint,T* A);
1165
1166 /*--------------------------------------------------------------------------
1167     Author      : Andrey Sobolev
1168     Description : Function computes the image of vector under
1169                     given affin map
1170     Arguments
1171             A   : in, affine maps
1172         pVector : in, pointer to vector
1173         pImgVector:out, pointer to image of vector
1174       Return    :
1175     --------------------------------------------------------------------------*/
1176 template<class T> CV_INLINE
1177 void _cvCalcVectorImage(pCvDirection pImgVector,pCvDirection pVector,T* A);
1178
1179 /*--------------------------------------------------------------------------
1180     Author      : Andrey Sobolev
1181     Description : Function computes the distance between the point
1182                     and site. Internal function.
1183     Arguments
1184         pPoint  : in, point
1185         pSite   : in, site
1186       Return    : distance
1187     --------------------------------------------------------------------------*/
1188 CV_INLINE
1189 float _cvCalcDist(pCvPointFloat pPoint, pCvVoronoiSite pSite);
1190
1191 /*--------------------------------------------------------------------------
1192     Author      : Andrey Sobolev
1193     Description : Function computes the distance between two points
1194     Arguments
1195     pPoint1,pPoint2 : in, two points
1196       Return    : distance
1197     --------------------------------------------------------------------------*/
1198 CV_INLINE
1199 float _cvPPDist(pCvPointFloat pPoint1,pCvPointFloat pPoint2);
1200
1201 /*--------------------------------------------------------------------------
1202     Author      : Andrey Sobolev
1203     Description : Function computes the distance betwin point and
1204                     segment. Internal function.
1205     Arguments
1206           pPoint: in, point
1207     pPoint1,pPoint2 : in, segment [pPoint1,pPoint2]
1208        Return   : distance
1209     --------------------------------------------------------------------------*/
1210 CV_INLINE
1211 float _cvPLDist(pCvPointFloat pPoint,pCvPointFloat pPoint1,pCvDirection pDirection);
1212
1213 /*--------------------------------------------------------------------------
1214     Author      : Andrey Sobolev
1215     Description : Function solves the squar equation with real coefficients
1216                     T - float or double
1217     Arguments
1218      c2,c1,c0: in, real coefficients of polynom
1219                X: out, array of roots
1220      Return     : number of roots
1221     --------------------------------------------------------------------------*/
1222 template <class T>
1223 int _cvSolveEqu2thR(T c2, T c1, T c0, T* X);
1224
1225 /*--------------------------------------------------------------------------
1226     Author      : Andrey Sobolev
1227     Description : Function solves the linear equation with real or complex coefficients
1228                     T - float or double or complex
1229     Arguments
1230         c1,c0: in, real or complex coefficients of polynom
1231                X: out, array of roots
1232      Return     : number of roots
1233     --------------------------------------------------------------------------*/
1234 template <class T> CV_INLINE
1235 int _cvSolveEqu1th(T c1, T c0, T* X);
1236
1237 /****************************************************************************************\
1238 *                             Storage Block Increase                                    *
1239 \****************************************************************************************/
1240 /*--------------------------------------------------------------------------
1241     Author      : Andrey Sobolev
1242     Description : For each sequence function creates the memory block sufficient to store
1243                   all elements of sequnce
1244     Arguments
1245         pVoronoiDiagramInt: in, pointer to struct, which contains the
1246                             description of Voronoi Diagram.
1247         vertices_number: in, number of vertices in polygon
1248      Return     :
1249     --------------------------------------------------------------------------*/
1250 static void _cvSetSeqBlockSize(CvVoronoiDiagramInt* pVoronoiDiagramInt,int vertices_number)
1251 {
1252     int N = 2*vertices_number;
1253     cvSetSeqBlockSize(pVoronoiDiagramInt->SiteSeq,N*pVoronoiDiagramInt->SiteSeq->elem_size);
1254     cvSetSeqBlockSize(pVoronoiDiagramInt->EdgeSeq,3*N*pVoronoiDiagramInt->EdgeSeq->elem_size);
1255     cvSetSeqBlockSize(pVoronoiDiagramInt->NodeSeq,5*N*pVoronoiDiagramInt->NodeSeq->elem_size);
1256     cvSetSeqBlockSize(pVoronoiDiagramInt->ParabolaSeq,N*pVoronoiDiagramInt->ParabolaSeq->elem_size);
1257     cvSetSeqBlockSize(pVoronoiDiagramInt->DirectionSeq,3*N*pVoronoiDiagramInt->DirectionSeq->elem_size);
1258     cvSetSeqBlockSize(pVoronoiDiagramInt->ChainSeq,N*pVoronoiDiagramInt->DirectionSeq->elem_size);
1259     cvSetSeqBlockSize(pVoronoiDiagramInt->HoleSeq,100*pVoronoiDiagramInt->HoleSeq->elem_size);
1260 }
1261
1262 /****************************************************************************************\
1263 *                                    Function realization                               *
1264 \****************************************************************************************/
1265
1266
1267 CV_IMPL int   cvVoronoiDiagramFromContour(CvSeq* ContourSeq,
1268                                            CvVoronoiDiagram2D** VoronoiDiagram,
1269                                            CvMemStorage* VoronoiStorage,
1270                                            CvLeeParameters contour_type,
1271                                            int contour_orientation,
1272                                            int attempt_number)
1273 {
1274     CV_FUNCNAME( "cvVoronoiDiagramFromContour" );
1275
1276     __BEGIN__;
1277
1278     CvSet* SiteSeq = NULL;
1279     CvSeq* CurContourSeq = NULL;
1280     CvVoronoiDiagramInt VoronoiDiagramInt;
1281     CvSeqWriter NodeWriter, EdgeWriter;
1282     CvMemStorage* storage;
1283
1284     memset( &VoronoiDiagramInt, 0, sizeof(VoronoiDiagramInt) );
1285
1286     if( !ContourSeq )
1287         CV_ERROR( CV_StsBadArg,"Contour sequence is empty" );
1288
1289     if(!VoronoiStorage)
1290         CV_ERROR( CV_StsBadArg,"Storage is not initialized" );
1291
1292     if( contour_type < 0 || contour_type > 2)
1293         CV_ERROR( CV_StsBadArg,"Unsupported parameter: type" );
1294
1295     if( contour_orientation != 1 &&  contour_orientation != -1)
1296         CV_ERROR( CV_StsBadArg,"Unsupported parameter: orientation" );
1297
1298     storage = cvCreateChildMemStorage(VoronoiStorage);
1299     (*VoronoiDiagram) = (CvVoronoiDiagram2D*)cvCreateSet(0,sizeof(CvVoronoiDiagram2D),sizeof(CvVoronoiNode2D), storage);
1300     storage = cvCreateChildMemStorage(VoronoiStorage);
1301     (*VoronoiDiagram)->edges = cvCreateSet(0,sizeof(CvSet),sizeof(CvVoronoiEdge2D), storage);
1302     cvStartAppendToSeq((CvSeq*)(*VoronoiDiagram)->edges, &EdgeWriter);
1303     cvStartAppendToSeq((CvSeq*)(*VoronoiDiagram), &NodeWriter);
1304
1305         for(CurContourSeq = ContourSeq;\
1306             CurContourSeq != NULL;\
1307             CurContourSeq = CurContourSeq->h_next)
1308         {
1309             if(_cvLee(CurContourSeq, &VoronoiDiagramInt,VoronoiStorage,contour_type,contour_orientation,attempt_number))
1310
1311             {
1312                 if(!_cvConvert(*VoronoiDiagram,VoronoiDiagramInt,SiteSeq,NodeWriter,EdgeWriter,VoronoiStorage,contour_orientation))
1313                     goto exit;
1314             }
1315             else if(CurContourSeq->total >= 3)
1316                 goto exit;
1317         }
1318
1319         cvEndWriteSeq(&EdgeWriter);
1320         cvEndWriteSeq(&NodeWriter);
1321         if(SiteSeq != NULL)
1322             return 1;
1323
1324
1325     __END__;
1326     return 0;
1327 }//end of cvVoronoiDiagramFromContour
1328
1329 CV_IMPL int   cvVoronoiDiagramFromImage(IplImage* pImage,
1330                                          CvSeq** ContourSeq,
1331                                          CvVoronoiDiagram2D** VoronoiDiagram,
1332                                          CvMemStorage* VoronoiStorage,
1333                                          CvLeeParameters regularization_method,
1334                                          float approx_precision)
1335 {
1336     CV_FUNCNAME( "cvVoronoiDiagramFromContour" );
1337     int RESULT = 0;
1338
1339     __BEGIN__;
1340
1341     IplImage* pWorkImage = NULL;
1342     CvSize image_size;
1343     int i, multiplicator = 3;
1344
1345     int approx_method;
1346     CvMemStorage* ApproxContourStorage = NULL;
1347     CvSeq* ApproxContourSeq = NULL;
1348
1349     if( !ContourSeq )
1350         CV_ERROR( CV_StsBadArg,"Contour sequence is not initialized" );
1351
1352     if( (*ContourSeq)->total != 0)
1353         CV_ERROR( CV_StsBadArg,"Contour sequence is not empty" );
1354
1355     if(!VoronoiStorage)
1356         CV_ERROR( CV_StsBadArg,"Storage is not initialized" );
1357
1358     if(!pImage)
1359         CV_ERROR( CV_StsBadArg,"Image is not initialized" );
1360
1361     if(pImage->nChannels != 1 || pImage->depth != 8)
1362         CV_ERROR( CV_StsBadArg,"Unsupported image format" );
1363
1364     if(approx_precision<0 && approx_precision != CV_LEE_AUTO)
1365         CV_ERROR( CV_StsBadArg,"Unsupported presision value" );
1366
1367     switch(regularization_method)
1368     {
1369         case CV_LEE_ERODE:  image_size.width = pImage->width;
1370                             image_size.height = pImage->height;
1371                             pWorkImage = cvCreateImage(image_size,8,1);
1372                             cvErode(pImage,pWorkImage,0,1);
1373                             approx_method = CV_CHAIN_APPROX_TC89_L1;
1374                             break;
1375         case CV_LEE_ZOOM:   image_size.width = multiplicator*pImage->width;
1376                             image_size.height = multiplicator*pImage->height;
1377                             pWorkImage = cvCreateImage(image_size,8,1);
1378                             cvResize(pImage, pWorkImage, CV_INTER_NN);
1379                             approx_method = CV_CHAIN_APPROX_TC89_L1;
1380                             break;
1381         case CV_LEE_NON:    pWorkImage = pImage;
1382                             approx_method = CV_CHAIN_APPROX_TC89_L1;
1383                             break;
1384         default:            CV_ERROR( CV_StsBadArg,"Unsupported regularisation method" );
1385                             break;
1386
1387     }
1388
1389     cvFindContours(pWorkImage, (*ContourSeq)->storage, ContourSeq, \
1390                             sizeof(CvContour), CV_RETR_CCOMP, approx_method );
1391
1392     if(pWorkImage && pWorkImage != pImage )
1393         cvReleaseImage(&pWorkImage);
1394
1395     ApproxContourStorage = cvCreateMemStorage(0);
1396     if(approx_precision > 0)
1397     {
1398         ApproxContourSeq = cvApproxPoly(*ContourSeq, sizeof(CvContour), ApproxContourStorage,\
1399                                         CV_POLY_APPROX_DP,approx_precision,1);
1400
1401         RESULT = cvVoronoiDiagramFromContour(ApproxContourSeq,VoronoiDiagram,VoronoiStorage,CV_LEE_INT,-1,10);
1402     }
1403     else if(approx_precision == CV_LEE_AUTO)
1404     {
1405         ApproxContourSeq = *ContourSeq;
1406         for(i = 1; i < 50; i++)
1407         {
1408             RESULT = cvVoronoiDiagramFromContour(ApproxContourSeq,VoronoiDiagram,VoronoiStorage,CV_LEE_INT,-1,1);
1409             if(RESULT)
1410                 break;
1411             ApproxContourSeq = cvApproxPoly(ApproxContourSeq, sizeof(CvContour),ApproxContourStorage,\
1412                                             CV_POLY_APPROX_DP,(float)i,1);
1413         }
1414     }
1415     else
1416         RESULT = cvVoronoiDiagramFromContour(*ContourSeq,VoronoiDiagram,VoronoiStorage,CV_LEE_INT,-1,10);
1417 /*
1418     if(ApproxContourSeq)
1419     {
1420         cvClearMemStorage( (*ContourSeq)->storage );
1421         *ContourSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvPoint),(*ContourSeq)->storage);
1422         CvSeqReader reader;
1423         CvSeqWriter writer;
1424         CvPoint Point;
1425         cvStartAppendToSeq(*ContourSeq, &writer);
1426         cvStartReadSeq(ApproxContourSeq, &reader);
1427         for(int i = 0;i < ApproxContourSeq->total;i++)
1428         {
1429             CV_READ_SEQ_ELEM(Point,reader);
1430             Point.y = 600 - Point.y;
1431             CV_WRITE_SEQ_ELEM(Point,writer);
1432         }
1433         cvEndWriteSeq(&writer);
1434     }
1435     */
1436
1437     cvReleaseMemStorage(&ApproxContourStorage);
1438
1439
1440     __END__;
1441     return RESULT;
1442 }//end of cvVoronoiDiagramFromImage
1443
1444 CV_IMPL void cvReleaseVoronoiStorage(CvVoronoiDiagram2D* VoronoiDiagram,
1445                                      CvMemStorage** pVoronoiStorage)
1446 {
1447     /*CV_FUNCNAME( "cvReleaseVoronoiStorage" );*/
1448     __BEGIN__;
1449
1450     CvSeq* Seq;
1451
1452     if(VoronoiDiagram->storage)
1453         cvReleaseMemStorage(&VoronoiDiagram->storage);
1454     for(Seq = (CvSeq*)VoronoiDiagram->sites; Seq != NULL; Seq = Seq->h_next)
1455         if(Seq->storage)
1456             cvReleaseMemStorage(&Seq->storage);
1457     for(Seq = (CvSeq*)VoronoiDiagram->edges; Seq != NULL; Seq = Seq->h_next)
1458         if(Seq->storage)
1459             cvReleaseMemStorage(&Seq->storage);
1460
1461     if(*pVoronoiStorage)
1462         cvReleaseMemStorage(pVoronoiStorage);
1463
1464     __END__;
1465 }//end of cvReleaseVoronoiStorage
1466
1467 static int  _cvLee(CvSeq* ContourSeq,
1468                     CvVoronoiDiagramInt* pVoronoiDiagramInt,
1469                     CvMemStorage* VoronoiStorage,
1470                     CvLeeParameters contour_type,
1471                     int contour_orientation,
1472                     int attempt_number)
1473 {
1474     //orientation = 1 for left coordinat system
1475     //orientation = -1 for right coordinat system
1476     if(ContourSeq->total<3)
1477         return 0;
1478
1479     int attempt = 0;
1480     CvVoronoiStorageInt VoronoiStorageInt;
1481
1482     srand((int)cvGetTickCount());
1483
1484 NEXTATTEMPT:
1485     VoronoiStorageInt.SiteStorage = cvCreateChildMemStorage(VoronoiStorage);
1486     VoronoiStorageInt.NodeStorage = cvCreateChildMemStorage(VoronoiStorage);
1487     VoronoiStorageInt.EdgeStorage = cvCreateChildMemStorage(VoronoiStorage);
1488     VoronoiStorageInt.ParabolaStorage = cvCreateMemStorage(0);
1489     VoronoiStorageInt.ChainStorage = cvCreateMemStorage(0);
1490     VoronoiStorageInt.DirectionStorage = cvCreateMemStorage(0);
1491     VoronoiStorageInt.HoleStorage = cvCreateMemStorage(0);
1492
1493     pVoronoiDiagramInt->SiteSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiSiteInt),VoronoiStorageInt.SiteStorage);
1494     pVoronoiDiagramInt->NodeSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiNodeInt),VoronoiStorageInt.NodeStorage);
1495     pVoronoiDiagramInt->EdgeSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiEdgeInt),VoronoiStorageInt.EdgeStorage);
1496     pVoronoiDiagramInt->ChainSeq  = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiChainInt),VoronoiStorageInt.ChainStorage);
1497     pVoronoiDiagramInt->DirectionSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvDirection),VoronoiStorageInt.DirectionStorage);
1498     pVoronoiDiagramInt->ParabolaSeq =  cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiParabolaInt),VoronoiStorageInt.ParabolaStorage);
1499     pVoronoiDiagramInt->HoleSeq =  cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiHoleInt),VoronoiStorageInt.HoleStorage);
1500
1501     _cvSetSeqBlockSize(pVoronoiDiagramInt,ContourSeq->total);
1502
1503     if(!_cvConstuctSites(ContourSeq, pVoronoiDiagramInt, contour_type,contour_orientation))
1504     {
1505         attempt = attempt_number;
1506         goto FAULT;
1507     }
1508     _cvRandomModification(pVoronoiDiagramInt, 0,pVoronoiDiagramInt->NodeSeq->total,0.2f);
1509
1510     if(!_cvConstructChains(pVoronoiDiagramInt))
1511     {
1512         attempt = attempt_number;
1513         goto FAULT;
1514     }
1515
1516     if(!_cvConstructSkeleton(pVoronoiDiagramInt))
1517         goto FAULT;
1518
1519     _cvConstructSiteTree(pVoronoiDiagramInt);
1520
1521 //SUCCESS:
1522     _cvReleaseVoronoiStorage(&VoronoiStorageInt,0,1);
1523     return 1;
1524
1525 FAULT:
1526     _cvReleaseVoronoiStorage(&VoronoiStorageInt,1,1);
1527     if(++attempt < attempt_number)
1528         goto NEXTATTEMPT;
1529
1530     return 0;
1531 }// end of _cvLee
1532
1533 static int _cvConstuctSites(CvSeq* ContourSeq,
1534                             CvVoronoiDiagramInt* pVoronoiDiagram,
1535                             CvLeeParameters contour_type,
1536                             int contour_orientation)
1537 {
1538     pVoronoiDiagram->reflex_site = NULL;
1539     pVoronoiDiagram->top_hole = NULL;
1540     int result = 0;
1541
1542     switch(contour_type)
1543     {
1544         case CV_LEE_INT :    result = _cvConstructExtSites(pVoronoiDiagram,ContourSeq,contour_orientation,(int)1);
1545                              break;
1546         case CV_LEE_FLOAT :  result = _cvConstructExtSites(pVoronoiDiagram,ContourSeq,contour_orientation,(float)1);
1547                              break;
1548         case CV_LEE_DOUBLE : result = _cvConstructExtSites(pVoronoiDiagram,ContourSeq,contour_orientation,(double)1);
1549                              break;
1550         default:             return 0;
1551     }
1552
1553     if(!result)
1554         return 0;
1555
1556     CvSeq* CurSiteSeq;
1557     CvVoronoiHoleInt Hole = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,false,0};
1558     pCvVoronoiSite pTopSite = 0;
1559     for(CvSeq* CurContourSeq = ContourSeq->v_next;\
1560         CurContourSeq != NULL;\
1561         CurContourSeq = CurContourSeq->h_next)
1562     {
1563         if(CurContourSeq->total == 0)
1564             continue;
1565
1566         CurSiteSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiSiteInt),pVoronoiDiagram->SiteSeq->storage);
1567         switch(contour_type)
1568         {
1569             case CV_LEE_INT :   result = _cvConstructIntSites(pVoronoiDiagram,CurSiteSeq,CurContourSeq,pTopSite,contour_orientation,(int)1);
1570                                 break;
1571             case CV_LEE_FLOAT : result = _cvConstructIntSites(pVoronoiDiagram,CurSiteSeq,CurContourSeq,pTopSite,contour_orientation,(float)1);
1572                                 break;
1573             case CV_LEE_DOUBLE :result = _cvConstructIntSites(pVoronoiDiagram,CurSiteSeq,CurContourSeq,pTopSite,contour_orientation,(double)1);
1574                                 break;
1575             default:            result = 0;
1576         }
1577         if(!result)
1578             continue;
1579
1580         Hole.SiteSeq = CurSiteSeq;
1581         Hole.site_top = pTopSite;
1582         Hole.x_coord = pTopSite->node1->node.x;
1583         Hole.error = false;
1584         _cvSeqPushInOrder(pVoronoiDiagram, &Hole);
1585     }
1586     return 1;
1587 }//end of _cvConstuctSites
1588
1589 static int _cvConstructChains(CvVoronoiDiagramInt* pVoronoiDiagram)
1590 {
1591     if(!_cvConstructExtChains(pVoronoiDiagram))
1592         return 0;
1593
1594     CvSeq* CurrChainSeq;
1595     for(pCvVoronoiHole pHole = pVoronoiDiagram->top_hole;\
1596         pHole != NULL; \
1597         pHole = pHole->next_hole)
1598         {
1599             pHole->error = false;
1600             CurrChainSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(CvVoronoiChainInt),pVoronoiDiagram->ChainSeq->storage);
1601             _cvConstructIntChains(pVoronoiDiagram,CurrChainSeq,pHole->SiteSeq,pHole->site_top);
1602             pHole->ChainSeq = CurrChainSeq;
1603         }
1604     return 1;
1605 }//end of _cvConstructChains
1606
1607 static int _cvConstructSkeleton(CvVoronoiDiagramInt* pVoronoiDiagram)
1608 {
1609     if(!_cvConstructExtVD(pVoronoiDiagram))
1610         return 0;
1611     _cvConstructIntVD(pVoronoiDiagram);
1612     _cvFindNearestSite(pVoronoiDiagram);
1613
1614     float dx,dy;
1615     int result;
1616     for(pCvVoronoiHole pHole = pVoronoiDiagram->top_hole;\
1617         pHole != NULL; pHole = pHole->next_hole)
1618     {
1619         if(pHole->error)
1620             continue;
1621         dx = pHole->node->node.x - pHole->site_top->node1->node.x;
1622         dy = pHole->node->node.y - pHole->site_top->node1->node.y;
1623
1624         if(fabs(dy) < 0.01 && dx < 0)
1625             pHole->site_opposite = pHole->site_nearest;
1626         else
1627         {
1628             if(dy > 0)
1629                 result = _cvFindOppositSiteCCW(pHole,pVoronoiDiagram);
1630             else
1631                 result = _cvFindOppositSiteCW(pHole,pVoronoiDiagram);
1632
1633             if(!result)
1634             {
1635                 pHole->error = true;
1636                 continue;
1637             }
1638         }
1639
1640         if(!_cvMergeVD(pHole,pVoronoiDiagram))
1641             return 0;
1642     }
1643     return 1;
1644 }//end of _cvConstructSkeleton
1645
1646 static void _cvConstructSiteTree(CvVoronoiDiagramInt* pVoronoiDiagram)
1647 {
1648     CvSeq* CurSeq = pVoronoiDiagram->SiteSeq;
1649     for(pCvVoronoiHole pHole = pVoronoiDiagram->top_hole;\
1650         pHole != NULL; pHole = pHole->next_hole)
1651     {
1652         if(pHole->error)
1653             continue;
1654         if(CurSeq == pVoronoiDiagram->SiteSeq)
1655         {
1656             CurSeq->v_next = pHole->SiteSeq;
1657             pHole->SiteSeq->v_prev = CurSeq;
1658         }
1659         else
1660         {
1661             CurSeq->h_next = pHole->SiteSeq;
1662             pHole->SiteSeq->h_prev = CurSeq;
1663             pHole->SiteSeq->v_prev = pVoronoiDiagram->SiteSeq;
1664         }
1665         CurSeq = pHole->SiteSeq;
1666     }
1667     CurSeq->h_next = NULL;
1668 }//end of _cvConstructSiteTree
1669
1670 static void _cvRandomModification(CvVoronoiDiagramInt* pVoronoiDiagram, int begin, int end, float shift)
1671 {
1672     CvSeqReader Reader;
1673     pCvVoronoiNode pNode;
1674     const float rnd_const = shift/RAND_MAX;
1675     int i;
1676
1677     cvStartReadSeq(pVoronoiDiagram->NodeSeq, &Reader,0);
1678     for( i = begin; i < end; i++)
1679     {
1680         pNode = (pCvVoronoiNode)Reader.ptr;
1681         pNode->node.x = (float)cvFloor(pNode->node.x) + rand()*rnd_const;
1682         pNode->node.y = (float)cvFloor(pNode->node.y) + rand()*rnd_const;
1683         CV_NEXT_SEQ_ELEM( sizeof(CvVoronoiNodeInt), Reader );
1684     }
1685
1686     for(pCvVoronoiHole pHole = pVoronoiDiagram->top_hole;\
1687         pHole != NULL;\
1688         pHole = pHole->next_hole)
1689     {
1690         pHole->site_top->node1->node.x = (float)cvFloor(pHole->site_top->node1->node.x);
1691     }
1692
1693 }//end of _cvRandomModification
1694
1695 static void _cvReleaseVoronoiStorage(CvVoronoiStorageInt* pVoronoiStorage, int group1, int group2)
1696 {
1697     //if group1 = 1 then SiteSeq, NodeSeq, EdgeSeq released
1698     //if group2 = 1 then DirectionSeq, ParabolaSeq, ChainSeq, HoleSeq released
1699     if(group1 == 1)
1700     {
1701         if(pVoronoiStorage->SiteStorage!=NULL)
1702             cvReleaseMemStorage(&pVoronoiStorage->SiteStorage);
1703         if(pVoronoiStorage->EdgeStorage!=NULL)
1704             cvReleaseMemStorage(&pVoronoiStorage->EdgeStorage);
1705         if(pVoronoiStorage->NodeStorage!=NULL)
1706             cvReleaseMemStorage(&pVoronoiStorage->NodeStorage);
1707     }
1708     if(group2 == 1)
1709     {
1710         if(pVoronoiStorage->ParabolaStorage!=NULL)
1711             cvReleaseMemStorage(&pVoronoiStorage->ParabolaStorage);
1712         if(pVoronoiStorage->ChainStorage!=NULL)
1713             cvReleaseMemStorage(&pVoronoiStorage->ChainStorage);
1714         if(pVoronoiStorage->DirectionStorage!=NULL)
1715             cvReleaseMemStorage(&pVoronoiStorage->DirectionStorage);
1716         if(pVoronoiStorage->HoleStorage != NULL)
1717             cvReleaseMemStorage(&pVoronoiStorage->HoleStorage);
1718     }
1719 }// end of _cvReleaseVoronoiStorage
1720
1721 static int _cvConvert(CvVoronoiDiagram2D* VoronoiDiagram,
1722                        CvVoronoiDiagramInt VoronoiDiagramInt,
1723                        CvSet* &SiteSeq,
1724                        CvSeqWriter &NodeWriter,
1725                        CvSeqWriter &EdgeWriter,
1726                        CvMemStorage* VoronoiStorage,
1727                        int contour_orientation)
1728 {
1729     if(contour_orientation == 1)
1730         return _cvConvertSameOrientation(VoronoiDiagram,VoronoiDiagramInt,SiteSeq,NodeWriter,
1731                                         EdgeWriter,VoronoiStorage);
1732     else
1733         return _cvConvertChangeOrientation(VoronoiDiagram,VoronoiDiagramInt,SiteSeq,NodeWriter,
1734                                         EdgeWriter,VoronoiStorage);
1735 }// end of _cvConvert
1736
1737 static int _cvConvertSameOrientation(CvVoronoiDiagram2D* VoronoiDiagram,
1738                                       CvVoronoiDiagramInt VoronoiDiagramInt,
1739                                       CvSet* &NewSiteSeqPrev,
1740                                       CvSeqWriter &NodeWriter,
1741                                       CvSeqWriter &EdgeWriter,
1742                                       CvMemStorage* VoronoiStorage)
1743 {
1744     CvSeq* SiteSeq = VoronoiDiagramInt.SiteSeq;
1745     CvSeq* EdgeSeq = VoronoiDiagramInt.EdgeSeq;
1746     CvSeq* NodeSeq = VoronoiDiagramInt.NodeSeq;
1747
1748
1749     CvMemStorage *NewSiteStorage = cvCreateChildMemStorage(VoronoiStorage);
1750     CvSet *NewSiteSeq = NULL,*CurrNewSiteSeq = NULL, *PrevNewSiteSeq = NULL;;
1751     CvSeqWriter SiteWriter;
1752
1753     CvVoronoiSite2D NewSite = {{0,0},{0,0},{0,0}},NewSite_prev = {{0,0},{0,0},{0,0}};
1754     CvVoronoiSite2D *pNewSite, *pNewSite_prev = &NewSite_prev;
1755     pCvVoronoiSite pSite,pFirstSite;
1756
1757     CvVoronoiEdge2D NewEdge = {{0,0},{0,0},{0,0,0,0}};
1758     CvVoronoiEdge2D *pNewEdge1, *pNewEdge2;
1759     pCvVoronoiEdge pEdge;
1760
1761     CvVoronoiNode2D* pNode1, *pNode2;
1762     CvVoronoiNode2D Node;
1763     Node.next_free = NULL;
1764
1765     for(CvSeq* CurrSiteSeq = SiteSeq;\
1766         CurrSiteSeq != NULL;\
1767         CurrSiteSeq = NEXT_SEQ(CurrSiteSeq,SiteSeq))
1768     {
1769         CurrNewSiteSeq = cvCreateSet(0,sizeof(CvSet),sizeof(CvVoronoiSite2D), NewSiteStorage);
1770         if(!NewSiteSeq)
1771             NewSiteSeq = PrevNewSiteSeq = CurrNewSiteSeq;
1772         else if(PrevNewSiteSeq->v_prev == NULL)
1773         {
1774             PrevNewSiteSeq->v_next = (CvSeq*)CurrNewSiteSeq;
1775             CurrNewSiteSeq->v_prev = (CvSeq*)PrevNewSiteSeq;
1776         }
1777         else
1778         {
1779             PrevNewSiteSeq->h_next = (CvSeq*)CurrNewSiteSeq;
1780             CurrNewSiteSeq->h_prev = (CvSeq*)PrevNewSiteSeq;
1781             CurrNewSiteSeq->v_prev = (CvSeq*)PrevNewSiteSeq->v_prev;
1782         }
1783         PrevNewSiteSeq = CurrNewSiteSeq;
1784
1785         pSite = pFirstSite = (pCvVoronoiSite)cvGetSeqElem(CurrSiteSeq, 0);
1786         while(pSite->prev_site->node1 == pSite->prev_site->node2)\
1787             pSite = pSite->next_site;
1788         pFirstSite = pSite;
1789
1790         pNewSite_prev = &NewSite_prev;
1791         cvStartAppendToSeq((CvSeq*)CurrNewSiteSeq, &SiteWriter);
1792         do
1793         {
1794             pNewSite = _cvWriteSeqElem(&NewSite,SiteWriter);
1795             pNewSite->next[0] = pNewSite_prev;
1796             pNewSite_prev->next[1] = pNewSite;
1797             pEdge = pSite->edge1;
1798             if(!pEdge || !pEdge->node1 || !pEdge->node2)
1799                 return 0;
1800
1801             if(pEdge->site == NULL)
1802             {
1803                 pNewEdge1 = (CvVoronoiEdge2D*)pEdge->twin_edge;
1804                 pNewEdge1->site[1] = pNewSite;
1805                 pNewSite->node[0] = pNewEdge1->node[0];
1806             }
1807             else
1808             {
1809                 pNewEdge1 = _cvWriteSeqElem(&NewEdge,EdgeWriter);
1810                 pNewEdge1->site[0] = pNewSite;
1811
1812                 pNode1 = _cvWriteSeqElem(&Node,NodeWriter);
1813                 pNode2 = _cvWriteSeqElem(&Node,NodeWriter);
1814                 pNode1->pt.x = pEdge->node1->node.x;
1815                 pNode1->pt.y = pEdge->node1->node.y;
1816                 pNode1->radius = pEdge->node1->radius;
1817                 pNode2->pt.x = pEdge->node2->node.x;
1818                 pNode2->pt.y = pEdge->node2->node.y;
1819                 pNode2->radius = pEdge->node2->radius;
1820                 pNewEdge1->node[0] = pNode1;
1821                 pNewEdge1->node[1] = pNode2;
1822
1823                 pNewSite->node[0] = pNewEdge1->node[1];
1824
1825                 if(!pNewEdge1->node[0] || !pNewEdge1->node[1])
1826                     return 0;
1827                 pEdge->twin_edge->site = NULL;
1828                 pEdge->twin_edge->twin_edge = (pCvVoronoiEdge)pNewEdge1;
1829             }
1830             pNewSite->edge[1] = pNewEdge1;
1831             pEdge = pEdge->prev_edge;
1832             while((pEdge != NULL && CurrSiteSeq->total>1) ||
1833                   (pEdge != pSite->edge2 && CurrSiteSeq->total == 1))
1834             {
1835                 if(pEdge->site == NULL)
1836                 {
1837                     pNewEdge2 = (CvVoronoiEdge2D*)pEdge->twin_edge;
1838                     pNewEdge2->site[1] = pNewSite;
1839                     if(CV_VORONOIEDGE2D_BEGINNODE(pNewEdge1,pNewSite) != pNewEdge2->node[0])
1840                     {
1841                         cvFlushSeqWriter(&NodeWriter);
1842 //                      cvSetRemove((CvSet*)VoronoiDiagram,VoronoiDiagram->total-1);
1843                         pNewEdge1->node[0] = pNewEdge2->node[0];
1844                     }
1845                 }
1846                 else
1847                 {
1848                     pNewEdge2 = _cvWriteSeqElem(&NewEdge,EdgeWriter);
1849                     pNewEdge2->site[0] = pNewSite;
1850
1851                     pNode1 = _cvWriteSeqElem(&Node,NodeWriter);
1852                     pNode1->pt.x = pEdge->node1->node.x;
1853                     pNode1->pt.y = pEdge->node1->node.y;
1854                     pNode1->radius = pEdge->node1->radius;
1855                     pNewEdge2->node[0] = pNode1;
1856
1857                     if(pNewEdge1->site[0] == pNewSite)
1858                         pNewEdge2->node[1] = pNewEdge1->node[0];
1859                     else
1860                         pNewEdge2->node[1] = pNewEdge1->node[1];
1861
1862                     if(!pNewEdge1->node[0] || !pNewEdge1->node[1])
1863                         return 0;
1864                     pEdge->twin_edge->site = NULL;
1865                     pEdge->twin_edge->twin_edge = (pCvVoronoiEdge)pNewEdge2;
1866                 }
1867                 if(pNewEdge1->site[0] == pNewSite)
1868                     pNewEdge1->next[2] = pNewEdge2;
1869                 else
1870                     pNewEdge1->next[3] = pNewEdge2;
1871
1872                 if(pNewEdge2->site[0] == pNewSite)
1873                     pNewEdge2->next[0] = pNewEdge1;
1874                 else
1875                     pNewEdge2->next[1] = pNewEdge1;
1876
1877                 pEdge = pEdge->prev_edge;
1878                 pNewEdge1 = pNewEdge2;
1879             }
1880             pNewSite->edge[0] = pNewEdge1;
1881             pNewSite->node[1] = pNewEdge1->node[0];
1882
1883             if(pSite->node1 == pSite->node2 && pSite != pSite->next_site && pNewEdge1->node[0] != pNewEdge1->node[1])
1884             {
1885                 cvFlushSeqWriter(&NodeWriter);
1886 //              cvSetRemove((CvSet*)VoronoiDiagram,VoronoiDiagram->total-1);
1887                 pNewSite->node[1] = pNewEdge1->node[0] = pNewSite->node[0];
1888             }
1889
1890             pNewSite_prev = pNewSite;
1891             pSite = pSite->next_site;
1892         }while(pSite != pFirstSite);
1893         pNewSite->node[1] = pNewEdge1->node[1];
1894         if(pSite == pSite->next_site)
1895         {
1896             Node.pt.x = pSite->node1->node.x;
1897             Node.pt.y = pSite->node1->node.y;
1898             Node.radius = 0;
1899             pNewSite->node[0] = pNewSite->node[1] = _cvWriteSeqElem(&Node,NodeWriter);
1900         }
1901
1902         cvEndWriteSeq(&SiteWriter);
1903         pNewSite = (CvVoronoiSite2D*)cvGetSetElem(CurrNewSiteSeq, 0);
1904         pNewSite->next[0] = pNewSite_prev;
1905         pNewSite_prev->next[1] = pNewSite;
1906     }
1907
1908     cvReleaseMemStorage(&(SiteSeq->storage));
1909     cvReleaseMemStorage(&(EdgeSeq->storage));
1910     cvReleaseMemStorage(&(NodeSeq->storage));
1911
1912     if(NewSiteSeqPrev == NULL)
1913         VoronoiDiagram->sites = NewSiteSeq;
1914     else
1915     {
1916         NewSiteSeqPrev->h_next = (CvSeq*)NewSiteSeq;
1917         NewSiteSeq->h_prev = (CvSeq*)NewSiteSeqPrev;
1918     }
1919
1920     NewSiteSeqPrev = NewSiteSeq;
1921     return 1;
1922 }//end of _cvConvertSameOrientation
1923
1924 static int _cvConvertChangeOrientation(CvVoronoiDiagram2D* VoronoiDiagram,
1925                                         CvVoronoiDiagramInt VoronoiDiagramInt,
1926                                         CvSet* &NewSiteSeqPrev,
1927                                         CvSeqWriter &NodeWriter,
1928                                         CvSeqWriter &EdgeWriter,
1929                                         CvMemStorage* VoronoiStorage)
1930 {
1931     // pNewSite->edge[1] = pSite->edge2
1932     // pNewSite->edge[0] = pSite->edge1
1933
1934     CvSeq* SiteSeq = VoronoiDiagramInt.SiteSeq;
1935     CvSeq* EdgeSeq = VoronoiDiagramInt.EdgeSeq;
1936     CvSeq* NodeSeq = VoronoiDiagramInt.NodeSeq;
1937
1938
1939     CvMemStorage *NewSiteStorage = cvCreateChildMemStorage(VoronoiStorage);
1940     CvSet *NewSiteSeq = NULL,*CurrNewSiteSeq = NULL, *PrevNewSiteSeq = NULL;;
1941     CvSeqWriter SiteWriter;
1942
1943     CvVoronoiSite2D NewSite = {{0,0},{0,0},{0,0}},NewSite_prev = {{0,0},{0,0},{0,0}};
1944     CvVoronoiSite2D *pNewSite, *pNewSite_prev = &NewSite_prev;
1945     pCvVoronoiSite pSite,pFirstSite;
1946
1947     CvVoronoiEdge2D NewEdge = {{0,0},{0,0},{0,0,0,0}};
1948     CvVoronoiEdge2D *pNewEdge1, *pNewEdge2;
1949     pCvVoronoiEdge pEdge;
1950
1951     CvVoronoiNode2D* pNode1, *pNode2;
1952     CvVoronoiNode2D Node;
1953     Node.next_free = NULL;
1954
1955     for(CvSeq* CurrSiteSeq = SiteSeq;\
1956         CurrSiteSeq != NULL;\
1957         CurrSiteSeq = NEXT_SEQ(CurrSiteSeq,SiteSeq))
1958     {
1959         CurrNewSiteSeq = cvCreateSet(0,sizeof(CvSet),sizeof(CvVoronoiSite2D), NewSiteStorage);
1960         if(!NewSiteSeq)
1961             NewSiteSeq = PrevNewSiteSeq = CurrNewSiteSeq;
1962         else if(PrevNewSiteSeq->v_prev == NULL)
1963         {
1964             PrevNewSiteSeq->v_next = (CvSeq*)CurrNewSiteSeq;
1965             CurrNewSiteSeq->v_prev = (CvSeq*)PrevNewSiteSeq;
1966         }
1967         else
1968         {
1969             PrevNewSiteSeq->h_next = (CvSeq*)CurrNewSiteSeq;
1970             CurrNewSiteSeq->h_prev = (CvSeq*)PrevNewSiteSeq;
1971             CurrNewSiteSeq->v_prev = (CvSeq*)PrevNewSiteSeq->v_prev;
1972         }
1973         PrevNewSiteSeq = CurrNewSiteSeq;
1974
1975         pSite = (pCvVoronoiSite)cvGetSeqElem(CurrSiteSeq, 0);
1976         while(pSite->next_site->node1 == pSite->next_site->node2)\
1977             pSite = pSite->next_site;
1978         pFirstSite = pSite;
1979
1980         pNewSite_prev = &NewSite_prev;
1981         cvStartAppendToSeq((CvSeq*)CurrNewSiteSeq, &SiteWriter);
1982         do
1983         {
1984             pNewSite = _cvWriteSeqElem(&NewSite,SiteWriter);
1985             pNewSite->next[0] = pNewSite_prev;
1986             pNewSite_prev->next[1] = pNewSite;
1987
1988             pEdge = pSite->edge2;
1989             if(!pEdge || !pEdge->node1 || !pEdge->node2)
1990                 return 0;
1991
1992             if(pEdge->site == NULL)
1993             {
1994                 pNewEdge1 = (CvVoronoiEdge2D*)pEdge->twin_edge;
1995                 pNewEdge1->site[1] = pNewSite;
1996                 pNewSite->node[0] = pNewEdge1->node[0];
1997             }
1998             else
1999             {
2000                 pNewEdge1 = _cvWriteSeqElem(&NewEdge,EdgeWriter);
2001                 pNewEdge1->site[0] = pNewSite;
2002
2003                 pNode1 = _cvWriteSeqElem(&Node,NodeWriter);
2004                 pNode2 = _cvWriteSeqElem(&Node,NodeWriter);
2005                 pNode1->pt.x = pEdge->node1->node.x;
2006                 pNode1->pt.y = pEdge->node1->node.y;
2007                 pNode1->radius = pEdge->node1->radius;
2008                 pNode2->pt.x = pEdge->node2->node.x;
2009                 pNode2->pt.y = pEdge->node2->node.y;
2010                 pNode2->radius = pEdge->node2->radius;
2011                 pNewEdge1->node[0] = pNode2;
2012                 pNewEdge1->node[1] = pNode1;
2013
2014                 pNewSite->node[0] = pNewEdge1->node[1];
2015
2016                 if(!pNewEdge1->node[0] || !pNewEdge1->node[1])
2017                     return 0;
2018                 pEdge->twin_edge->site = NULL;
2019                 pEdge->twin_edge->twin_edge = (pCvVoronoiEdge)pNewEdge1;
2020             }
2021             pNewSite->edge[1] = pNewEdge1;
2022
2023
2024             pEdge = pEdge->next_edge;
2025             while((pEdge != NULL && CurrSiteSeq->total>1) ||
2026                   (pEdge != pSite->edge1 && CurrSiteSeq->total == 1))
2027             {
2028                 if(pEdge->site == NULL)
2029                 {
2030                     pNewEdge2 = (CvVoronoiEdge2D*)pEdge->twin_edge;
2031                     pNewEdge2->site[1] = pNewSite;
2032                     if(CV_VORONOIEDGE2D_BEGINNODE(pNewEdge1,pNewSite) != pNewEdge2->node[0])
2033                     {
2034                         cvFlushSeqWriter(&NodeWriter);
2035 //                      cvSetRemove((CvSet*)VoronoiDiagram,VoronoiDiagram->total-1);
2036                         pNewEdge1->node[0] = pNewEdge2->node[0];
2037                     }
2038                 }
2039                 else
2040                 {
2041                     pNewEdge2 = _cvWriteSeqElem(&NewEdge,EdgeWriter);
2042                     pNewEdge2->site[0] = pNewSite;
2043
2044                     pNode2 = _cvWriteSeqElem(&Node,NodeWriter);
2045                     pNode2->pt.x = pEdge->node2->node.x;
2046                     pNode2->pt.y = pEdge->node2->node.y;
2047                     pNode2->radius = pEdge->node2->radius;
2048                     pNewEdge2->node[0] = pNode2;
2049
2050                     if(pNewEdge1->site[0] == pNewSite)
2051                         pNewEdge2->node[1] = pNewEdge1->node[0];
2052                     else
2053                         pNewEdge2->node[1] = pNewEdge1->node[1];
2054
2055                     if(!pNewEdge1->node[0] || !pNewEdge1->node[1])
2056                         return 0;
2057                     pEdge->twin_edge->site = NULL;
2058                     pEdge->twin_edge->twin_edge = (pCvVoronoiEdge)pNewEdge2;
2059                 }
2060                 if(pNewEdge1->site[0] == pNewSite)
2061                     pNewEdge1->next[2] = pNewEdge2;
2062                 else
2063                     pNewEdge1->next[3] = pNewEdge2;
2064
2065                 if(pNewEdge2->site[0] == pNewSite)
2066                     pNewEdge2->next[0] = pNewEdge1;
2067                 else
2068                     pNewEdge2->next[1] = pNewEdge1;
2069
2070                 pEdge = pEdge->next_edge;
2071                 pNewEdge1 = pNewEdge2;
2072             }
2073             pNewSite->edge[0] = pNewEdge1;
2074             pNewSite->node[1] = pNewEdge1->node[0];
2075
2076             if(pSite->node1 == pSite->node2 && pSite != pSite->next_site && pNewEdge1->node[0] != pNewEdge1->node[1])
2077             {
2078                 cvFlushSeqWriter(&NodeWriter);
2079 //              cvSetRemove((CvSet*)VoronoiDiagram,VoronoiDiagram->total-1);
2080                 pNewSite->node[1] = pNewEdge1->node[0] = pNewSite->node[0];
2081             }
2082
2083             pNewSite_prev = pNewSite;
2084             pSite = pSite->prev_site;
2085         }while(pSite != pFirstSite);
2086         pNewSite->node[1] = pNewEdge1->node[1];
2087         if(pSite == pSite->next_site)
2088         {
2089             Node.pt.x = pSite->node1->node.x;
2090             Node.pt.y = pSite->node1->node.y;
2091             Node.radius = 0;
2092             pNewSite->node[0] = pNewSite->node[1] = _cvWriteSeqElem(&Node,NodeWriter);
2093         }
2094
2095         cvEndWriteSeq(&SiteWriter);
2096         pNewSite = (CvVoronoiSite2D*)cvGetSetElem(CurrNewSiteSeq, 0);
2097         pNewSite->next[0] = pNewSite_prev;
2098         pNewSite_prev->next[1] = pNewSite;
2099     }
2100
2101     cvReleaseMemStorage(&(SiteSeq->storage));
2102     cvReleaseMemStorage(&(EdgeSeq->storage));
2103     cvReleaseMemStorage(&(NodeSeq->storage));
2104
2105     if(NewSiteSeqPrev == NULL)
2106         VoronoiDiagram->sites = NewSiteSeq;
2107     else
2108     {
2109         NewSiteSeqPrev->h_next = (CvSeq*)NewSiteSeq;
2110         NewSiteSeq->h_prev = (CvSeq*)NewSiteSeqPrev;
2111     }
2112     NewSiteSeqPrev = NewSiteSeq;
2113     return 1;
2114 }//end of _cvConvert
2115
2116 template<class T>
2117 int _cvConstructExtSites(CvVoronoiDiagramInt* pVoronoiDiagram,
2118                          CvSeq* ContourSeq,
2119                          int orientation,
2120                          T /*type*/)
2121 {
2122     const double angl_eps = 0.03;
2123     CvSeq* SiteSeq = pVoronoiDiagram->SiteSeq;
2124     CvSeq* NodeSeq = pVoronoiDiagram->NodeSeq;
2125     //CvSeq* DirectionSeq = pVoronoiDiagram->DirectionSeq;
2126     CvPointFloat Vertex1,Vertex2,Vertex3;
2127     CvLeePoint<T> VertexT1,VertexT2,VertexT3;
2128
2129     CvSeqReader ContourReader;
2130     CvVoronoiSiteInt Site = {NULL,NULL,NULL,NULL,NULL,NULL,NULL};
2131     CvVoronoiSiteInt SiteTemp = {NULL,NULL,NULL,NULL,NULL,NULL,NULL};
2132     CvVoronoiNodeInt Node;
2133     pCvVoronoiNode pNode1,pNode2;
2134     pCvVoronoiSite pSite = &SiteTemp,pSite_prev = &SiteTemp;
2135     pCvVoronoiSite pReflexSite = NULL;
2136     int NReflexSite = 0;
2137
2138     float delta_x_rc, delta_x_cl, delta_y_rc, delta_y_cl;
2139     float norm_cl,norm_rc, mult_cl_rc;
2140     float _sin, _cos;
2141     int i;
2142
2143     if(orientation == 1)
2144     {
2145         cvStartReadSeq(ContourSeq, &ContourReader,0);
2146         CV_READ_SEQ_ELEM(VertexT1,ContourReader);
2147         CV_READ_SEQ_ELEM(VertexT2,ContourReader);
2148     }
2149     else
2150     {
2151         cvStartReadSeq(ContourSeq, &ContourReader,1);
2152         CV_REV_READ_SEQ_ELEM(VertexT1,ContourReader);
2153         CV_REV_READ_SEQ_ELEM(VertexT2,ContourReader);
2154     }
2155
2156     Vertex1.x = (float)VertexT1.x;
2157     Vertex1.y = (float)VertexT1.y;
2158     Vertex2.x = (float)VertexT2.x;
2159     Vertex2.y = (float)VertexT2.y;
2160
2161     _cvInitVoronoiNode(&Node, &Vertex2);
2162     pNode1 = _cvSeqPush(NodeSeq, &Node);
2163
2164     delta_x_cl = Vertex2.x - Vertex1.x;
2165     delta_y_cl = Vertex2.y - Vertex1.y;
2166     norm_cl = (float)sqrt((double)delta_x_cl*delta_x_cl + delta_y_cl*delta_y_cl);
2167
2168     for( i = 0;i<ContourSeq->total;i++)
2169     {
2170         if(orientation == 1)
2171         {
2172             CV_READ_SEQ_ELEM(VertexT3,ContourReader);
2173         }
2174         else
2175         {
2176             CV_REV_READ_SEQ_ELEM(VertexT3,ContourReader);
2177         }
2178
2179         Vertex3.x = (float)VertexT3.x;
2180         Vertex3.y = (float)VertexT3.y;
2181
2182         _cvInitVoronoiNode(&Node, &Vertex3);
2183         pNode2 = _cvSeqPush(NodeSeq, &Node);
2184
2185         delta_x_rc = Vertex3.x - Vertex2.x;
2186         delta_y_rc = Vertex3.y - Vertex2.y;
2187         norm_rc = (float)sqrt((double)delta_x_rc*delta_x_rc + delta_y_rc*delta_y_rc);
2188         if(norm_rc==0)
2189             continue;
2190
2191         mult_cl_rc = norm_cl*norm_rc;
2192         _sin = (delta_y_rc* delta_x_cl - delta_x_rc* delta_y_cl)/mult_cl_rc;
2193         _cos = -(delta_x_cl*delta_x_rc + delta_y_cl*delta_y_rc)/mult_cl_rc;
2194
2195         if((_sin > angl_eps) || (_sin > 0 && _cos > 0))
2196         {
2197             pSite = _cvSeqPush(SiteSeq, &Site);
2198             _cvInitVoronoiSite(pSite,pNode1,pNode2,pSite_prev);
2199             pSite_prev->next_site = pSite;
2200         }
2201         else if((_sin < -angl_eps) || (_sin < 0 && _cos > 0))
2202         {
2203             pSite = _cvSeqPush(SiteSeq, &Site);
2204             _cvInitVoronoiSite(pSite,pNode1,pNode1,pSite_prev);
2205             pReflexSite = pSite;
2206             NReflexSite++;
2207             pSite_prev->next_site = pSite;
2208
2209             pSite_prev = pSite;
2210             pSite = _cvSeqPush(SiteSeq, &Site);
2211             _cvInitVoronoiSite(pSite,pNode1,pNode2,pSite_prev);
2212             pSite_prev->next_site = pSite;
2213         }
2214         else
2215         {
2216             Vertex2 = Vertex3;
2217             delta_y_rc = delta_y_cl + delta_y_rc;
2218             delta_x_rc = delta_x_cl + delta_x_rc;
2219             pSite->node2 = pNode2;
2220
2221             norm_rc = (float)sqrt((double)delta_y_rc*delta_y_rc + delta_x_rc*delta_x_rc);
2222         }
2223         Vertex2=Vertex3;
2224         delta_y_cl= delta_y_rc;
2225         delta_x_cl= delta_x_rc;
2226         norm_cl = norm_rc;
2227         pSite_prev = pSite;
2228         pNode1 = pNode2;
2229     }
2230
2231     if(SiteTemp.next_site==NULL)
2232         return 0;
2233
2234     if(ContourSeq->total - NReflexSite<2)
2235         return 0;
2236
2237     if(SiteSeq->total<3)
2238         return 0;
2239
2240     pSite->node2 = SiteTemp.next_site->node1;
2241     pSite->next_site = SiteTemp.next_site;
2242     SiteTemp.next_site->prev_site = pSite;
2243
2244     i = 0;
2245     if(pReflexSite!=NULL)
2246         for(i=0; i<SiteSeq->total; i++)
2247         {
2248             if(pReflexSite->next_site->next_site->node1 !=
2249               pReflexSite->next_site->next_site->node2)
2250               break;
2251             else
2252                 pReflexSite = pReflexSite->next_site->next_site;
2253         }
2254     pVoronoiDiagram->reflex_site = pReflexSite;
2255     return (i<SiteSeq->total);
2256 }//end of _cvConstructExtSites
2257
2258 template<class T>
2259 int _cvConstructIntSites(CvVoronoiDiagramInt* pVoronoiDiagram,
2260                                  CvSeq* CurrSiteSeq,
2261                                  CvSeq* CurrContourSeq,
2262                                  pCvVoronoiSite &pTopSite,
2263                                  int orientation,
2264                                  T /*type*/)
2265 {
2266     const double angl_eps = 0.03;
2267     float min_x = (float)999999999;
2268     CvSeq* SiteSeq = CurrSiteSeq;
2269     CvSeq* NodeSeq = pVoronoiDiagram->NodeSeq;
2270     //CvSeq* DirectionSeq = pVoronoiDiagram->DirectionSeq;
2271     CvPointFloat Vertex1,Vertex2,Vertex3;
2272     CvLeePoint<T> VertexT1,VertexT2,VertexT3;
2273
2274     CvSeqReader ContourReader;
2275     CvVoronoiSiteInt Site = {NULL,NULL,NULL,NULL,NULL,NULL,NULL};
2276     CvVoronoiSiteInt SiteTemp = {NULL,NULL,NULL,NULL,NULL,NULL,NULL};
2277     CvVoronoiNodeInt Node;
2278     pCvVoronoiNode pNode1,pNode2;
2279     pCvVoronoiSite pSite = &SiteTemp,pSite_prev = &SiteTemp;
2280     pTopSite = NULL;
2281     int NReflexSite = 0;
2282
2283     if(CurrContourSeq->total == 1)
2284     {
2285         cvStartReadSeq(CurrContourSeq, &ContourReader,0);
2286         CV_READ_SEQ_ELEM(VertexT1,ContourReader);
2287         Vertex1.x = (float)VertexT1.x;
2288         Vertex1.y = (float)VertexT1.y;
2289
2290         _cvInitVoronoiNode(&Node, &Vertex1);
2291         pNode1 = _cvSeqPush(NodeSeq, &Node);
2292         pTopSite = pSite = _cvSeqPush(SiteSeq, &Site);
2293         _cvInitVoronoiSite(pSite,pNode1,pNode1,pSite);
2294         pSite->next_site = pSite;
2295         return 1;
2296     }
2297
2298     float delta_x_rc, delta_x_cl, delta_y_rc, delta_y_cl;
2299     float norm_cl,norm_rc, mult_cl_rc;
2300     float _sin, _cos;
2301     int i;
2302
2303     if(orientation == 1)
2304     {
2305         cvStartReadSeq(CurrContourSeq, &ContourReader,0);
2306         CV_READ_SEQ_ELEM(VertexT1,ContourReader);
2307         CV_READ_SEQ_ELEM(VertexT2,ContourReader);
2308     }
2309     else
2310     {
2311         cvStartReadSeq(CurrContourSeq, &ContourReader,1);
2312         CV_REV_READ_SEQ_ELEM(VertexT1,ContourReader);
2313         CV_REV_READ_SEQ_ELEM(VertexT2,ContourReader);
2314     }
2315
2316     Vertex1.x = (float)VertexT1.x;
2317     Vertex1.y = (float)VertexT1.y;
2318     Vertex2.x = (float)VertexT2.x;
2319     Vertex2.y = (float)VertexT2.y;
2320
2321     _cvInitVoronoiNode(&Node, &Vertex2);
2322     pNode1 = _cvSeqPush(NodeSeq, &Node);
2323
2324     delta_x_cl = Vertex2.x - Vertex1.x;
2325     delta_y_cl = Vertex2.y - Vertex1.y;
2326     norm_cl = (float)sqrt((double)delta_x_cl*delta_x_cl + delta_y_cl*delta_y_cl);
2327     for( i = 0;i<CurrContourSeq->total;i++)
2328     {
2329         if(orientation == 1)
2330         {
2331             CV_READ_SEQ_ELEM(VertexT3,ContourReader);
2332         }
2333         else
2334         {
2335             CV_REV_READ_SEQ_ELEM(VertexT3,ContourReader);
2336         }
2337         Vertex3.x = (float)VertexT3.x;
2338         Vertex3.y = (float)VertexT3.y;
2339
2340         _cvInitVoronoiNode(&Node, &Vertex3);
2341         pNode2 = _cvSeqPush(NodeSeq, &Node);
2342
2343         delta_x_rc = Vertex3.x - Vertex2.x;
2344         delta_y_rc = Vertex3.y - Vertex2.y;
2345         norm_rc = (float)sqrt((double)delta_x_rc*delta_x_rc + delta_y_rc*delta_y_rc);
2346         if(norm_rc==0)
2347             continue;
2348
2349         mult_cl_rc = norm_cl*norm_rc;
2350         _sin = (delta_y_rc* delta_x_cl - delta_x_rc* delta_y_cl)/mult_cl_rc;
2351         _cos = -(delta_x_cl*delta_x_rc + delta_y_cl*delta_y_rc)/mult_cl_rc;
2352         if((_sin > angl_eps) || (_sin > 0 && _cos > 0))
2353         {
2354             pSite = _cvSeqPush(SiteSeq, &Site);
2355             _cvInitVoronoiSite(pSite,pNode1,pNode2,pSite_prev);
2356             pSite_prev->next_site = pSite;
2357         }
2358         else if((_sin < -angl_eps) || (_sin < 0 && _cos > 0) || (_sin == 0 && CurrContourSeq->total == 2))
2359         {
2360             pSite = _cvSeqPush(SiteSeq, &Site);
2361             _cvInitVoronoiSite(pSite,pNode1,pNode1,pSite_prev);
2362             if(pNode1->node.x < min_x)
2363             {
2364                 min_x = pNode1->node.x;
2365                 pTopSite = pSite;
2366             }
2367             NReflexSite++;
2368             pSite_prev->next_site = pSite;
2369
2370             pSite_prev = pSite;
2371             pSite = _cvSeqPush(SiteSeq, &Site);
2372             _cvInitVoronoiSite(pSite,pNode1,pNode2,pSite_prev);
2373             pSite_prev->next_site = pSite;
2374         }
2375         else
2376         {
2377             Vertex2 = Vertex3;
2378             delta_y_rc = delta_y_cl + delta_y_rc;
2379             delta_x_rc = delta_x_cl + delta_x_rc;
2380             norm_rc = (float)sqrt((double)delta_y_rc*delta_y_rc + delta_x_rc*delta_x_rc);
2381             pSite->node2 = pNode2;
2382         }
2383
2384         Vertex1=Vertex2;
2385         Vertex2=Vertex3;
2386         delta_y_cl= delta_y_rc;
2387         delta_x_cl= delta_x_rc;
2388         norm_cl = norm_rc;
2389         pSite_prev = pSite;
2390         pNode1 = pNode2;
2391     }
2392
2393     if(SiteTemp.next_site==NULL)
2394         return 0;
2395
2396     if((NReflexSite < 3 && CurrContourSeq->total > 2) || NReflexSite < 2)
2397         return 0;
2398
2399     pSite->node2 = SiteTemp.next_site->node1;
2400     pSite->next_site = SiteTemp.next_site;
2401     SiteTemp.next_site->prev_site = pSite;
2402
2403     return 1;
2404 }//end of _cvConstructIntSites
2405
2406 static int _cvConstructExtChains(CvVoronoiDiagramInt* pVoronoiDiagram)
2407 {
2408     CvSeq* SiteSeq = pVoronoiDiagram->SiteSeq;
2409     CvSeq* ChainSeq = pVoronoiDiagram->ChainSeq;
2410
2411     CvVoronoiChainInt Chain;
2412     pCvVoronoiChain pChain,pChainFirst;
2413     pCvVoronoiSite pSite, pSite_prev, pSiteFirst,pReflexSite = pVoronoiDiagram->reflex_site;
2414
2415     if(pReflexSite==NULL)
2416         pSite = pSiteFirst = (pCvVoronoiSite)cvGetSeqElem(SiteSeq, 0);
2417     else
2418     {
2419         while(pReflexSite->next_site->next_site->node1==
2420               pReflexSite->next_site->next_site->node2)
2421             pReflexSite = pReflexSite->next_site->next_site;
2422
2423         pSite = pSiteFirst = pReflexSite->next_site;
2424     }
2425
2426     Chain.last_site = pSite;
2427     _cvConstructEdges(pSite,pVoronoiDiagram);
2428     pSite_prev = pSite;
2429     pSite = pSite->prev_site;
2430     do
2431     {
2432         if(pSite->node1!=pSite->node2)
2433         {
2434             Chain.first_site = pSite_prev;
2435             pChain = _cvSeqPushFront(ChainSeq,&Chain);
2436
2437             _cvConstructEdges(pSite,pVoronoiDiagram);
2438             Chain.last_site = pSite;
2439             Chain.next_chain = pChain;
2440         }
2441         else
2442         {
2443             pSite=pSite->prev_site;
2444             _cvConstructEdges(pSite,pVoronoiDiagram);
2445             _cvConstructEdges(pSite->next_site,pVoronoiDiagram);
2446         }
2447         pSite_prev = pSite;
2448         pSite = pSite->prev_site;
2449     }while(pSite!=pSiteFirst);
2450
2451     Chain.first_site = pSite_prev;
2452     pChain = _cvSeqPushFront(ChainSeq,&Chain);
2453     pChainFirst = (pCvVoronoiChain)cvGetSeqElem(ChainSeq,ChainSeq->total - 1);
2454     pChainFirst->next_chain = pChain;
2455     if(ChainSeq->total < 3)
2456         return 0;
2457     else
2458         return 1;
2459 }// end of _cvConstructExtChains
2460
2461 static void _cvConstructIntChains(CvVoronoiDiagramInt* pVoronoiDiagram,
2462                                    CvSeq* CurrChainSeq,
2463                                    CvSeq* CurrSiteSeq,
2464                                    pCvVoronoiSite pTopSite)
2465 {
2466     CvSeq* ChainSeq = CurrChainSeq;
2467
2468     if(CurrSiteSeq->total == 1)
2469         return;
2470
2471     CvVoronoiChainInt Chain = {NULL,NULL,NULL};
2472     pCvVoronoiChain pChain,pChainFirst;;
2473     pCvVoronoiSite pSite, pSite_prev, pSiteFirst;
2474     pSite = pSiteFirst = pTopSite->next_site;
2475
2476     Chain.last_site = pSite;
2477     _cvConstructEdges(pSite,pVoronoiDiagram);
2478     pSite_prev = pSite;
2479     pSite = pSite->prev_site;
2480     do
2481     {
2482         if(pSite->node1!=pSite->node2)
2483         {
2484             Chain.first_site = pSite_prev;
2485             pChain = _cvSeqPushFront(ChainSeq,&Chain);
2486
2487             _cvConstructEdges(pSite,pVoronoiDiagram);
2488             Chain.last_site = pSite;
2489             Chain.next_chain = pChain;
2490         }
2491         else
2492         {
2493             pSite=pSite->prev_site;
2494             if(pSite != pSiteFirst)
2495                 _cvConstructEdges(pSite,pVoronoiDiagram);
2496             _cvConstructEdges(pSite->next_site,pVoronoiDiagram);
2497         }
2498         pSite_prev = pSite;
2499         pSite = pSite->prev_site;
2500     }while(pSite!=pSiteFirst && pSite!= pSiteFirst->prev_site);
2501
2502     if(pSite == pSiteFirst->prev_site && ChainSeq->total == 0)
2503         return;
2504
2505     Chain.first_site = pSite_prev;
2506     if(pSite == pSiteFirst->prev_site)
2507     {
2508         pChainFirst = (pCvVoronoiChain)cvGetSeqElem(ChainSeq,ChainSeq->total - 1);
2509         pChainFirst->last_site = Chain.last_site;
2510         pChainFirst->next_chain = Chain.next_chain;
2511         return;
2512     }
2513     else
2514     {
2515         pChain = _cvSeqPushFront(ChainSeq,&Chain);
2516         pChainFirst = (pCvVoronoiChain)cvGetSeqElem(ChainSeq,ChainSeq->total - 1);
2517         pChainFirst->next_chain = pChain;
2518         return;
2519     }
2520 }// end of _cvConstructIntChains
2521
2522 CV_INLINE void _cvConstructEdges(pCvVoronoiSite pSite,CvVoronoiDiagramInt* pVoronoiDiagram)
2523 {
2524     CvSeq* EdgeSeq = pVoronoiDiagram->EdgeSeq;
2525     CvSeq* DirectionSeq = pVoronoiDiagram->DirectionSeq;
2526     CvVoronoiEdgeInt Edge = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL};
2527     pCvVoronoiEdge pEdge1,pEdge2;
2528     CvDirection EdgeDirection,SiteDirection;
2529     float site_lengh;
2530
2531     Edge.site = pSite;
2532     if(pSite->node1!=pSite->node2)
2533     {
2534         SiteDirection.x = pSite->node2->node.x - pSite->node1->node.x;
2535         SiteDirection.y = pSite->node2->node.y - pSite->node1->node.y;
2536         site_lengh = (float)sqrt((double)SiteDirection.x*SiteDirection.x + SiteDirection.y * SiteDirection.y);
2537         SiteDirection.x /= site_lengh;
2538         SiteDirection.y /= site_lengh;
2539         EdgeDirection.x = -SiteDirection.y;
2540         EdgeDirection.y = SiteDirection.x;
2541         Edge.direction = _cvSeqPush(DirectionSeq,&EdgeDirection);
2542         pSite->direction = _cvSeqPush(DirectionSeq,&SiteDirection);
2543
2544         pEdge1 = _cvSeqPush(EdgeSeq,&Edge);
2545         pEdge2 = _cvSeqPush(EdgeSeq,&Edge);
2546     }
2547     else
2548     {
2549         pCvVoronoiSite pSite_prev = pSite->prev_site;
2550         pCvVoronoiSite pSite_next = pSite->next_site;
2551
2552         pEdge1 = _cvSeqPush(EdgeSeq,&Edge);
2553         pEdge2 = _cvSeqPush(EdgeSeq,&Edge);
2554
2555         pEdge2->direction = pSite_next->edge1->direction;
2556         pEdge2->twin_edge = pSite_next->edge1;
2557         pSite_next->edge1->twin_edge = pEdge2;
2558
2559         pEdge1->direction = pSite_prev->edge2->direction;
2560         pEdge1->twin_edge = pSite_prev->edge2;
2561         pSite_prev->edge2->twin_edge = pEdge1;
2562     }
2563
2564         pEdge2->node1 = pSite->node2;
2565         pEdge1->node2 = pSite->node1;
2566         pSite->edge1 = pEdge1;
2567         pSite->edge2 = pEdge2;
2568         pEdge2->next_edge = pEdge1;
2569         pEdge1->prev_edge = pEdge2;
2570 }// end of _cvConstructEdges
2571
2572 static int _cvConstructExtVD(CvVoronoiDiagramInt* pVoronoiDiagram)
2573 {
2574     pCvVoronoiSite pSite_right = 0,pSite_left = 0;
2575     pCvVoronoiEdge pEdge_left,pEdge_right;
2576     pCvVoronoiChain pChain1, pChain2;
2577
2578     pChain1 = (pCvVoronoiChain)cvGetSeqElem(pVoronoiDiagram->ChainSeq,0);
2579     do
2580     {
2581         pChain2 = pChain1->next_chain;
2582         if(pChain2->next_chain==pChain1)
2583         {
2584             pSite_right = pChain1->first_site;
2585             pSite_left = pChain2->last_site;
2586             pEdge_left = pSite_left->edge2->next_edge;
2587             pEdge_right = pSite_right->edge1->prev_edge;
2588             pEdge_left->prev_edge = NULL;
2589             pEdge_right->next_edge = NULL;
2590             pSite_right->edge1 = NULL;
2591             pSite_left->edge2 = NULL;
2592         }
2593
2594         if(!_cvJoinChains(pChain1,pChain2,pVoronoiDiagram))
2595             return 0;
2596
2597         pChain1->last_site = pChain2->last_site;
2598         pChain1->next_chain = pChain2->next_chain;
2599         pChain1 = pChain1->next_chain;
2600     }while(pChain1->next_chain != pChain1);
2601
2602     pCvVoronoiNode pEndNode = pSite_left->node2;
2603     if(pSite_right->edge1==NULL)
2604         return 0;
2605     else
2606         pSite_right->edge1->node2 = pEndNode;
2607
2608     if(pSite_left->edge2==NULL)
2609         return 0;
2610     else
2611         pSite_left->edge2->node1 = pEndNode;
2612
2613     return 1;
2614 }//end of _cvConstructExtVD
2615
2616 static int _cvJoinChains(pCvVoronoiChain pChain1,
2617                           pCvVoronoiChain pChain2,
2618                           CvVoronoiDiagramInt* pVoronoiDiagram)
2619 {
2620     const double dist_eps = 0.05;
2621     if(pChain1->first_site == NULL || pChain1->last_site == NULL ||
2622         pChain2->first_site == NULL || pChain2->last_site == NULL)
2623         return 0;
2624
2625     CvVoronoiEdgeInt EdgeNULL = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL};
2626
2627     CvSeq* NodeSeq = pVoronoiDiagram->NodeSeq;
2628     CvSeq* EdgeSeq = pVoronoiDiagram->EdgeSeq;
2629
2630     pCvVoronoiSite pSite_left = pChain1->last_site;
2631     pCvVoronoiSite pSite_right = pChain2->first_site;
2632
2633     pCvVoronoiEdge pEdge_left = pSite_left->edge2->next_edge;
2634     pCvVoronoiEdge pEdge_right = pSite_right->edge1->prev_edge;
2635
2636     pCvVoronoiEdge pEdge_left_cur = pEdge_left;
2637     pCvVoronoiEdge pEdge_right_cur = pEdge_right;
2638
2639     pCvVoronoiEdge pEdge_left_prev = NULL;
2640     pCvVoronoiEdge pEdge_right_next = NULL;
2641
2642     pCvVoronoiNode pNode_siteleft = pChain1->first_site->node1;
2643     pCvVoronoiNode pNode_siteright = pChain2->last_site->node2;
2644     /*CvVoronoiSiteInt Site_left_chain = {pNode_siteleft,pNode_siteleft,NULL,NULL,NULL,NULL};
2645     CvVoronoiSiteInt Site_right_chain = {pNode_siteright,pNode_siteright,NULL,NULL,NULL,NULL};*/
2646
2647     pCvVoronoiEdge pEdge1,pEdge2;
2648     CvPointFloat Point1 = {0,0}, Point2 = {0,0};
2649
2650     float radius1,radius2,dist1,dist2;
2651     bool left = true,right = true;
2652
2653     CvVoronoiNodeInt Node;
2654     pCvVoronoiNode pNode_begin = pSite_left->node2;
2655
2656     pEdge1 = pSite_left->edge2;
2657     pEdge1->node2 = NULL;
2658     pEdge2 = pSite_right->edge1;
2659     pEdge2->node1 = NULL;
2660
2661     for(;;)
2662     {
2663
2664         if(left)
2665             pEdge1->node1 = pNode_begin;
2666         if(right)
2667             pEdge2->node2 = pNode_begin;
2668
2669         pEdge_left = pEdge_left_cur;
2670         pEdge_right = pEdge_right_cur;
2671
2672         if(left&&right)
2673         {
2674             _cvCalcEdge(pSite_left,pSite_right,pEdge1,pVoronoiDiagram);
2675             _cvMakeTwinEdge(pEdge2,pEdge1);
2676             _cvStickEdgeLeftBegin(pEdge1,pEdge_left_prev,pSite_left);
2677             _cvStickEdgeRightBegin(pEdge2,pEdge_right_next,pSite_right);
2678         }
2679         else if(!left)
2680         {
2681             _cvCalcEdge(pNode_siteleft,pSite_right,pEdge2,pVoronoiDiagram);
2682             _cvStickEdgeRightBegin(pEdge2,pEdge_right_next,pSite_right);
2683         }
2684         else if(!right)
2685         {
2686             _cvCalcEdge(pSite_left,pNode_siteright,pEdge1,pVoronoiDiagram);
2687             _cvStickEdgeLeftBegin(pEdge1,pEdge_left_prev,pSite_left);
2688         }
2689
2690         dist1=dist2=-1;
2691         radius1 = -1; radius2 = -2;
2692
2693         while(pEdge_left!=NULL)
2694         {
2695             if(pEdge_left->node2 == NULL)
2696             {
2697                 pEdge_left_cur = pEdge_left = pEdge_left->next_edge;
2698                 if(pEdge_left == NULL)
2699                     break;
2700             }
2701
2702             if(left)
2703                 dist1 = _cvCalcEdgeIntersection(pEdge1, pEdge_left, &Point1,radius1);
2704             else
2705                 dist1 = _cvCalcEdgeIntersection(pEdge2, pEdge_left, &Point1,radius1);
2706
2707             if(dist1>=0)
2708                 break;
2709
2710             pEdge_left = pEdge_left->next_edge;
2711         }
2712
2713         while(pEdge_right!=NULL)
2714         {
2715             if(pEdge_right->node1 == NULL)
2716             {
2717                 pEdge_right_cur = pEdge_right = pEdge_right->prev_edge;
2718                 if(pEdge_right == NULL)
2719                     break;
2720             }
2721
2722             if(left)
2723                 dist2 = _cvCalcEdgeIntersection(pEdge1, pEdge_right, &Point2, radius2);
2724             else
2725                 dist2 = _cvCalcEdgeIntersection(pEdge2, pEdge_right, &Point2, radius2);
2726
2727             if(dist2>=0)
2728                 break;
2729
2730             pEdge_right = pEdge_right->prev_edge;
2731         }
2732
2733         if(dist1<0&&dist2<0)
2734         {
2735             if(left)
2736             {
2737                 pEdge_left = pSite_left->edge1;
2738                 if(pEdge_left==NULL)
2739                     _cvStickEdgeLeftEnd(pEdge1,NULL,pSite_left);
2740                 else
2741                 {
2742                     while(pEdge_left->node1!=NULL
2743                         &&pEdge_left->node1==pEdge_left->prev_edge->node2)
2744                     {
2745                         pEdge_left = pEdge_left->prev_edge;
2746                         if(pEdge_left==NULL || pEdge_left->prev_edge == NULL)
2747                             return 0;
2748                     }
2749                     _cvStickEdgeLeftEnd(pEdge1,pEdge_left,pSite_left);
2750                 }
2751             }
2752             if(right)
2753             {
2754                 pEdge_right = pSite_right->edge2;
2755                 if(pEdge_right==NULL)
2756                     _cvStickEdgeRightEnd(pEdge2,NULL,pSite_right);
2757                 else
2758                 {
2759                     while(pEdge_right->node2!=NULL
2760                         &&  pEdge_right->node2==pEdge_right->next_edge->node1)
2761                     {
2762                         pEdge_right = pEdge_right->next_edge;
2763                         if(pEdge_right==NULL || pEdge_right->next_edge == NULL )
2764                             return 0;
2765                     }
2766                     _cvStickEdgeRightEnd(pEdge2,pEdge_right,pSite_right);
2767                 }
2768             }
2769             return 1;
2770         }
2771
2772         if(fabs(dist1 - dist2)<dist_eps)
2773         {
2774             pNode_begin = _cvSeqPush(NodeSeq,&Node);
2775             _cvInitVoronoiNode(pNode_begin, &Point2,radius2);
2776
2777             pEdge1->node2 = pNode_begin;
2778             pEdge2->node1 = pNode_begin;
2779
2780             _cvStickEdgeLeftEnd(pEdge1,pEdge_left,pSite_left);
2781             _cvTwinNULLLeft(pEdge_left_cur,pEdge_left);
2782
2783             _cvStickEdgeRightEnd(pEdge2,pEdge_right,pSite_right);
2784             _cvTwinNULLRight(pEdge_right_cur,pEdge_right);
2785
2786             if(pEdge_left->twin_edge!=NULL&&pEdge_right->twin_edge!=NULL)
2787             {
2788                 pEdge_left_prev = pEdge_left->twin_edge;
2789                 if(!pEdge_left_prev)
2790                     return 0;
2791                 pEdge_left_cur = pEdge_left_prev->next_edge;
2792                 pEdge_right_next = pEdge_right->twin_edge;
2793                 if(!pEdge_right_next)
2794                     return 0;
2795                 pEdge_right_cur = pEdge_right_next->prev_edge;
2796                 pSite_right = pEdge_right_next->site;
2797                 pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2798                 pSite_left = pEdge_left_prev->site;
2799                 pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2800                 continue;
2801             }
2802
2803             if(pEdge_left->twin_edge==NULL&&pEdge_right->twin_edge!=NULL)
2804             {
2805                 pEdge_right_next = pEdge_right->twin_edge;
2806                 if(!pEdge_right_next)
2807                     return 0;
2808                 pEdge_right_cur = pEdge_right_next->prev_edge;
2809                 pSite_right = pEdge_right_next->site;
2810                 pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2811                 pEdge_left_cur = NULL;
2812                 left = false;
2813                 continue;
2814             }
2815
2816             if(pEdge_left->twin_edge!=NULL&&pEdge_right->twin_edge==NULL)
2817             {
2818                 pEdge_left_prev = pEdge_left->twin_edge;
2819                 if(!pEdge_left_prev)
2820                     return 0;
2821                 pEdge_left_cur = pEdge_left_prev->next_edge;
2822                 pSite_left = pEdge_left_prev->site;
2823                 pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2824                 pEdge_right_cur = NULL;
2825                 right = false;
2826                 continue;
2827             }
2828             if(pEdge_left->twin_edge==NULL&&pEdge_right->twin_edge==NULL)
2829                 return 1;
2830         }
2831
2832         if((dist1<dist2&&dist1>=0)||(dist1>=0&&dist2<0))
2833         {
2834
2835             pNode_begin = _cvSeqPush(NodeSeq,&Node);
2836             _cvInitVoronoiNode(pNode_begin, &Point1,radius1);
2837             pEdge1->node2 = pNode_begin;
2838             _cvTwinNULLLeft(pEdge_left_cur,pEdge_left);
2839             _cvStickEdgeLeftEnd(pEdge1,pEdge_left,pSite_left);
2840             if(right)
2841             {
2842                 pEdge2->node1 = pNode_begin;
2843                 pEdge_right_next = pEdge2;
2844                 pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2845                 if(pEdge_left->twin_edge!=NULL)
2846                 {
2847                     pEdge_left_prev = pEdge_left->twin_edge;
2848                     if(!pEdge_left_prev)
2849                         return 0;
2850                     pEdge_left_cur = pEdge_left_prev->next_edge;
2851                     pSite_left = pEdge_left_prev->site;
2852                     pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2853                     continue;
2854                 }
2855                 else
2856                 {
2857                     pEdge_left_cur = NULL;
2858                     left = false;
2859                     continue;
2860                 }
2861             }
2862             else
2863             {
2864                 if(pEdge_left->twin_edge!=NULL)
2865                 {
2866                     pEdge_left_prev = pEdge_left->twin_edge;
2867                     if(!pEdge_left_prev)
2868                         return 0;
2869                     pEdge_left_cur = pEdge_left_prev->next_edge;
2870                     pSite_left = pEdge_left_prev->site;
2871                     pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2872                     continue;
2873                 }
2874                 else
2875                     return 1;
2876
2877             }
2878
2879         }
2880
2881         if((dist1>dist2&&dist2>=0)||(dist1<0&&dist2>=0))
2882         {
2883             pNode_begin = _cvSeqPush(NodeSeq,&Node);
2884             _cvInitVoronoiNode(pNode_begin, &Point2,radius2);
2885             pEdge2->node1 = pNode_begin;
2886             _cvTwinNULLRight(pEdge_right_cur,pEdge_right);
2887             _cvStickEdgeRightEnd(pEdge2,pEdge_right,pSite_right);
2888             if(left)
2889             {
2890                 pEdge1->node2 = pNode_begin;
2891                 pEdge_left_prev = pEdge1;
2892                 pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2893                 if(pEdge_right->twin_edge!=NULL)
2894                 {
2895                     pEdge_right_next = pEdge_right->twin_edge;
2896                     if(!pEdge_right_next)
2897                         return 0;
2898                     pEdge_right_cur = pEdge_right_next->prev_edge;
2899                     pSite_right = pEdge_right_next->site;
2900                     pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2901                     continue;
2902                 }
2903                 else
2904                 {
2905                     pEdge_right_cur = NULL;
2906                     right = false;
2907                     continue;
2908                 }
2909             }
2910             else
2911             {
2912                 if(pEdge_right->twin_edge!=NULL)
2913                 {
2914                     pEdge_right_next = pEdge_right->twin_edge;
2915                     if(!pEdge_right_next)
2916                         return 0;
2917                     pEdge_right_cur = pEdge_right_next->prev_edge;
2918                     pSite_right = pEdge_right_next->site;
2919                     pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
2920                     continue;
2921                 }
2922                 else
2923                     return 1;
2924             }
2925
2926         }
2927     }
2928
2929 }// end of _cvJoinChains
2930
2931 static void _cvFindNearestSite(CvVoronoiDiagramInt* pVoronoiDiagram)
2932 {
2933     pCvVoronoiHole pCurrHole, pHole = pVoronoiDiagram->top_hole;
2934     pCvPointFloat pTopPoint,pPoint1,pPoint2;
2935     CvPointFloat Direction;
2936     pCvVoronoiSite pSite;
2937     CvVoronoiNodeInt Node;
2938     CvSeq* CurrSeq;
2939     float min_distance,distance;
2940     int i;
2941     for(;pHole != NULL; pHole = pHole->next_hole)
2942     {
2943         if(pHole->error)
2944             continue;
2945         pTopPoint = &pHole->site_top->node1->node;
2946         pCurrHole = NULL;
2947         CurrSeq = pVoronoiDiagram->SiteSeq;
2948         min_distance = (float)3e+34;
2949         while(pCurrHole != pHole)
2950         {
2951             if(pCurrHole && pCurrHole->error)
2952                 continue;
2953             pSite = (pCvVoronoiSite)cvGetSeqElem(CurrSeq,0);
2954             if(CurrSeq->total == 1)
2955             {
2956                 distance = _cvCalcDist(pTopPoint, pSite);
2957                 if(distance < min_distance)
2958                 {
2959                     min_distance = distance;
2960                     pHole->site_nearest = pSite;
2961                 }
2962             }
2963             else
2964             {
2965                 for(i = 0; i < CurrSeq->total;i++, pSite = pSite->next_site)
2966                 {
2967                     if(pSite->node1 != pSite->node2)
2968                     {
2969                         pPoint1 = &pSite->node1->node;
2970                         pPoint2 = &pSite->node2->node;
2971
2972                         Direction.x = -pSite->direction->y;
2973                         Direction.y = pSite->direction->x;
2974
2975                         if(
2976                                  (pTopPoint->x - pPoint2->x)*Direction.y -
2977                                  (pTopPoint->y - pPoint2->y)*Direction.x > 0
2978                             ||
2979                                  (pTopPoint->x - pPoint1->x)*Direction.y -
2980                                  (pTopPoint->y - pPoint1->y)*Direction.x < 0
2981                             ||
2982                                  (pTopPoint->x - pPoint1->x)*pSite->direction->y -
2983                                  (pTopPoint->y - pPoint1->y)*pSite->direction->x > 0
2984                            )
2985                                 continue;
2986
2987                         distance = _cvCalcDist(pTopPoint, pSite);
2988                     }
2989                     else
2990                     {
2991                         pPoint1 = &pSite->node1->node;
2992                         if(
2993                                  (pTopPoint->x - pPoint1->x)*pSite->edge2->direction->y -
2994                                  (pTopPoint->y - pPoint1->y)*pSite->edge2->direction->x > 0
2995                             ||
2996                                  (pTopPoint->x - pPoint1->x)*pSite->edge1->direction->y -
2997                                  (pTopPoint->y - pPoint1->y)*pSite->edge1->direction->x < 0
2998                            )
2999                                 continue;
3000
3001                         distance = _cvCalcDist(pTopPoint, pSite);
3002                     }
3003
3004
3005                     if(distance < min_distance)
3006                     {
3007                         min_distance = distance;
3008                         pHole->site_nearest = pSite;
3009                     }
3010                 }
3011             }
3012
3013             if(pCurrHole == NULL)
3014                 pCurrHole = pVoronoiDiagram->top_hole;
3015             else
3016                 pCurrHole = pCurrHole->next_hole;
3017
3018             CurrSeq = pCurrHole->SiteSeq;
3019         }
3020         pHole->x_coord = min_distance;
3021
3022         if(pHole->site_nearest->node1 == pHole->site_nearest->node2)
3023         {
3024             Direction.x = (pHole->site_nearest->node1->node.x - pHole->site_top->node1->node.x)/2;
3025             Direction.y = (pHole->site_nearest->node1->node.y - pHole->site_top->node1->node.y)/2;
3026         }
3027         else
3028         {
3029
3030             Direction.x = pHole->site_nearest->direction->y * min_distance / 2;
3031             Direction.y = - pHole->site_nearest->direction->x * min_distance / 2;
3032         }
3033
3034         Node.node.x = pHole->site_top->node1->node.x + Direction.x;
3035         Node.node.y = pHole->site_top->node1->node.y + Direction.y;
3036         pHole->node = _cvSeqPush(pVoronoiDiagram->NodeSeq, &Node);
3037     }
3038 }//end of _cvFindNearestSite
3039
3040 static void _cvConstructIntVD(CvVoronoiDiagramInt* pVoronoiDiagram)
3041 {
3042     pCvVoronoiChain pChain1, pChain2;
3043     pCvVoronoiHole pHole;
3044     int i;
3045
3046     pHole = pVoronoiDiagram->top_hole;
3047
3048     for(;pHole != NULL; pHole = pHole->next_hole)
3049     {
3050         if(pHole->ChainSeq->total == 0)
3051             continue;
3052         pChain1 = (pCvVoronoiChain)cvGetSeqElem(pHole->ChainSeq,0);
3053         for(i = pHole->ChainSeq->total; i > 0;i--)
3054         {
3055             pChain2 = pChain1->next_chain;
3056             if(!_cvJoinChains(pChain1,pChain2,pVoronoiDiagram))
3057             {
3058                 pHole->error = true;
3059                 break;
3060             }
3061
3062             pChain1->last_site = pChain2->last_site;
3063             pChain1->next_chain = pChain2->next_chain;
3064             pChain1 = pChain1->next_chain;
3065         }
3066     }
3067 }// end of _cvConstructIntVD
3068
3069 static int _cvFindOppositSiteCW(pCvVoronoiHole pHole, CvVoronoiDiagramInt* pVoronoiDiagram)
3070 {
3071     pCvVoronoiSite pSite_left = pHole->site_nearest;
3072     pCvVoronoiSite pSite_right = pHole->site_top;
3073     pCvVoronoiNode pNode = pHole->node;
3074
3075     CvDirection Direction = {-1,0};
3076     CvVoronoiEdgeInt Edge_right = {NULL,pSite_right->node1,pSite_right,NULL,NULL,NULL,NULL,&Direction};
3077
3078     pCvVoronoiEdge pEdge_left = pSite_left->edge2->next_edge;
3079     pCvVoronoiEdge pEdge_right = &Edge_right;
3080
3081     CvVoronoiEdgeInt Edge     = {NULL,pNode,pSite_right,NULL,NULL,NULL,NULL,NULL};
3082     CvVoronoiEdgeInt Edge_cur = {NULL,NULL, NULL,       NULL,NULL,NULL,NULL,NULL};
3083     pCvVoronoiEdge pEdge = &Edge;
3084
3085     float radius1, radius2,dist1, dist2;
3086     CvPointFloat Point1 = {0,0}, Point2 = {0,0};
3087
3088     for(;;)
3089     {
3090         pEdge->direction = NULL;
3091         pEdge->parabola = NULL;
3092         _cvCalcEdge(pSite_left,pSite_right,pEdge,pVoronoiDiagram);
3093
3094         dist1=dist2=-1;
3095         radius1 = -1; radius2 = -2;
3096         while(pEdge_left!=NULL)
3097         {
3098             dist1 = _cvCalcEdgeIntersection(pEdge, pEdge_left, &Point1,radius1);
3099             if(dist1>=0)
3100                 break;
3101             pEdge_left = pEdge_left->next_edge;
3102         }
3103
3104         dist2 = _cvCalcEdgeIntersection(pEdge, pEdge_right, &Point2, radius2);
3105         if(dist2>=0 && dist1 >= dist2)
3106         {
3107             pHole->site_opposite = pSite_left;
3108             pNode->node = Point2;
3109             return 1;
3110         }
3111
3112         if(dist1<0)
3113             return 0;
3114
3115         Edge_cur = *pEdge_left->twin_edge;
3116         Edge_cur.node1 = pNode;
3117         pEdge_left = &Edge_cur;
3118
3119         pSite_left = pEdge_left->site;
3120         pNode->node = Point1;
3121     }
3122 }//end of _cvFindOppositSiteCW
3123
3124 static int _cvFindOppositSiteCCW(pCvVoronoiHole pHole,CvVoronoiDiagramInt* pVoronoiDiagram)
3125 {
3126     pCvVoronoiSite pSite_right = pHole->site_nearest;
3127     pCvVoronoiSite pSite_left = pHole->site_top;
3128     pCvVoronoiNode pNode = pHole->node;
3129
3130     CvDirection Direction = {-1,0};
3131     CvVoronoiEdgeInt Edge_left = {pSite_left->node1,NULL,pSite_left,NULL,NULL,NULL, NULL, &Direction};
3132
3133     pCvVoronoiEdge pEdge_left = &Edge_left;
3134     pCvVoronoiEdge pEdge_right = pSite_right->edge1->prev_edge;
3135
3136     CvVoronoiEdgeInt Edge     = {NULL,pNode,pSite_left,NULL,NULL,NULL,NULL,NULL};
3137     CvVoronoiEdgeInt Edge_cur = {NULL,NULL, NULL,      NULL,NULL,NULL,NULL,NULL};
3138     pCvVoronoiEdge pEdge = &Edge;
3139
3140     double dist1, dist2;
3141     float radius1, radius2;
3142     CvPointFloat Point1 = {0,0}, Point2 = {0,0};
3143
3144     for(;;)
3145     {
3146         pEdge->direction = NULL;
3147         pEdge->parabola = NULL;
3148         _cvCalcEdge(pSite_left,pSite_right,pEdge,pVoronoiDiagram);
3149
3150         dist1=dist2=-1;
3151         radius1 = -1; radius2 = -2;
3152         while(pEdge_right!=NULL)
3153         {
3154             dist1 = _cvCalcEdgeIntersection(pEdge, pEdge_right, &Point1,radius2);
3155             if(dist1>=0)
3156                 break;
3157             pEdge_right = pEdge_right->prev_edge;
3158         }
3159
3160         dist2 = _cvCalcEdgeIntersection(pEdge, pEdge_left, &Point2, radius1);
3161         if(dist2>=0 && dist1 > dist2)
3162         {
3163             pHole->site_opposite = pSite_right;
3164             pNode->node = Point2;
3165             return 1;
3166         }
3167
3168         if(dist1<0)
3169             return 0;
3170
3171         Edge_cur = *pEdge_right->twin_edge;
3172         Edge_cur.node2 = pNode;
3173         pEdge_right = &Edge_cur;
3174
3175         pSite_right = pEdge_right->site;
3176         pNode->node = Point1;
3177     }
3178 }//end of _cvFindOppositSiteCCW
3179
3180 static int _cvMergeVD(pCvVoronoiHole pHole,CvVoronoiDiagramInt* pVoronoiDiagram)
3181 {
3182     pCvVoronoiSite pSite_left_first = pHole->site_top;
3183     pCvVoronoiSite pSite_right_first = pHole->site_opposite;
3184     pCvVoronoiNode pNode_begin = pHole->node;
3185     if(pSite_left_first == NULL || pSite_right_first == NULL || pNode_begin == NULL)
3186         return 0;
3187
3188     pCvVoronoiSite pSite_left = pSite_left_first;
3189     pCvVoronoiSite pSite_right = pSite_right_first;
3190
3191     const double dist_eps = 0.05;
3192     CvVoronoiEdgeInt EdgeNULL = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL};
3193
3194     CvSeq* NodeSeq = pVoronoiDiagram->NodeSeq;
3195     CvSeq* EdgeSeq = pVoronoiDiagram->EdgeSeq;
3196
3197     pCvVoronoiEdge pEdge_left = NULL;
3198     if(pSite_left->edge2 != NULL)
3199         pEdge_left = pSite_left->edge2->next_edge;
3200
3201     pCvVoronoiEdge pEdge_right = pSite_right->edge1;
3202     pCvVoronoiEdge pEdge_left_cur = pEdge_left;
3203     pCvVoronoiEdge pEdge_right_cur = pEdge_right;
3204
3205     pCvVoronoiEdge pEdge_left_prev = NULL;
3206     pCvVoronoiEdge pEdge_right_next = NULL;
3207
3208     pCvVoronoiEdge pEdge1,pEdge2,pEdge1_first, pEdge2_first;
3209     CvPointFloat Point1 = {0,0}, Point2 = {0,0};
3210
3211     float radius1,radius2,dist1,dist2;
3212
3213     CvVoronoiNodeInt Node;
3214
3215     pEdge1_first = pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);;
3216     pEdge2_first = pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);;
3217     pEdge1->site = pSite_left_first;
3218     pEdge2->site = pSite_right_first;
3219
3220     do
3221     {
3222         pEdge1->node1 = pEdge2->node2 = pNode_begin;
3223
3224         pEdge_left = pEdge_left_cur;
3225         pEdge_right = pEdge_right_cur->prev_edge;
3226
3227         _cvCalcEdge(pSite_left,pSite_right,pEdge1,pVoronoiDiagram);
3228         _cvMakeTwinEdge(pEdge2,pEdge1);
3229
3230         if(pEdge_left_prev != NULL)
3231             _cvStickEdgeLeftBegin(pEdge1,pEdge_left_prev,pSite_left);
3232         if(pEdge_right_next != NULL)
3233             _cvStickEdgeRightBegin(pEdge2,pEdge_right_next,pSite_right);
3234
3235         dist1=dist2=-1;
3236         radius1 = -1; radius2 = -2;
3237
3238 //LEFT:
3239         while(pEdge_left!=NULL)
3240         {
3241             if(pEdge_left->node2 == NULL)
3242                 pEdge_left_cur = pEdge_left = pEdge_left->next_edge;
3243
3244             dist1 = _cvCalcEdgeIntersection(pEdge1, pEdge_left, &Point1,radius1);
3245             if(dist1>=0)
3246                 goto RIGHT;
3247             pEdge_left = pEdge_left->next_edge;
3248         }
3249
3250 RIGHT:
3251         while(pEdge_right!=NULL)
3252         {
3253             dist2 = _cvCalcEdgeIntersection(pEdge1, pEdge_right, &Point2,radius2);
3254             if(dist2>=0)
3255                 goto RESULTHANDLING;
3256
3257             pEdge_right = pEdge_right->prev_edge;
3258         }
3259         pEdge_right = pEdge_right_cur;
3260         dist2 = _cvCalcEdgeIntersection(pEdge1, pEdge_right, &Point2, radius2);
3261
3262 RESULTHANDLING:
3263         if(dist1<0&&dist2<0)
3264             return 0;
3265
3266         if(fabs(dist1 - dist2)<dist_eps)
3267         {
3268             pNode_begin = _cvSeqPush(NodeSeq,&Node);
3269             _cvInitVoronoiNode(pNode_begin, &Point2,radius2);
3270
3271             pEdge1->node2 = pNode_begin;
3272             pEdge2->node1 = pNode_begin;
3273
3274             pEdge_right_cur = _cvDivideRightEdge(pEdge_right,pNode_begin,EdgeSeq);
3275
3276             _cvStickEdgeLeftEnd(pEdge1,pEdge_left,pSite_left);
3277             _cvStickEdgeRightEnd(pEdge2,pEdge_right,pSite_right);
3278
3279             pEdge_left_prev = pEdge_left->twin_edge;
3280             if(!pEdge_left_prev)
3281                 return 0;
3282             pEdge_left_cur = pEdge_left_prev->next_edge;
3283             pSite_left = pEdge_left_prev->site;
3284             pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
3285
3286             pEdge_right_next = pEdge_right->twin_edge;
3287             if(!pEdge_right_next)
3288                 return 0;
3289             pSite_right = pEdge_right_next->site;
3290             pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
3291
3292             continue;
3293         }
3294
3295         if((dist1<dist2&&dist1>=0)||(dist1>=0&&dist2<0))
3296         {
3297             pNode_begin = _cvSeqPush(NodeSeq,&Node);
3298             _cvInitVoronoiNode(pNode_begin, &Point1,radius1);
3299
3300             pEdge1->node2 = pNode_begin;
3301             _cvStickEdgeLeftEnd(pEdge1,pEdge_left,pSite_left);
3302
3303             pEdge2->node1 = pNode_begin;
3304             pEdge_right_next = pEdge2;
3305             pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
3306
3307             pEdge_left_prev = pEdge_left->twin_edge;
3308             if(!pEdge_left_prev)
3309                 return 0;
3310             pEdge_left_cur = pEdge_left_prev->next_edge;
3311             pSite_left = pEdge_left_prev->site;
3312             pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
3313
3314             continue;
3315         }
3316
3317         if((dist1>dist2&&dist2>=0)||(dist1<0&&dist2>=0))
3318         {
3319             pNode_begin = _cvSeqPush(NodeSeq,&Node);
3320             _cvInitVoronoiNode(pNode_begin, &Point2,radius2);
3321
3322             pEdge_right_cur = _cvDivideRightEdge(pEdge_right,pNode_begin,EdgeSeq);
3323
3324             pEdge2->node1 = pNode_begin;
3325             _cvStickEdgeRightEnd(pEdge2,pEdge_right,pSite_right);
3326
3327             pEdge1->node2 = pNode_begin;
3328             pEdge_left_prev = pEdge1;
3329             pEdge1 = _cvSeqPush(EdgeSeq, &EdgeNULL);
3330
3331             pEdge_right_next = pEdge_right->twin_edge;
3332             if(!pEdge_right_next)
3333                 return 0;
3334             pSite_right = pEdge_right_next->site;
3335             pEdge2 = _cvSeqPush(EdgeSeq, &EdgeNULL);
3336
3337             continue;
3338         }
3339
3340     }while(!(pSite_left == pSite_left_first && pSite_right == pSite_right_first));
3341
3342         pEdge1_first->node1 = pNode_begin;
3343         pEdge2_first->node2 = pNode_begin;
3344         _cvStickEdgeLeftBegin(pEdge1_first,pEdge_left_prev,pSite_left_first);
3345         _cvStickEdgeRightBegin(pEdge2_first,pEdge_right_next,pSite_right_first);
3346
3347         if(pSite_left_first->edge2 == NULL)
3348             pSite_left_first->edge2 = pSite_left_first->edge1 = pEdge1_first;
3349         return 1;
3350 }// end of _cvMergeVD
3351
3352
3353 /* ///////////////////////////////////////////////////////////////////////////////////////
3354 //                               Computation of bisectors                               //
3355 /////////////////////////////////////////////////////////////////////////////////////// */
3356
3357 void _cvCalcEdge(pCvVoronoiSite pSite_left,
3358                  pCvVoronoiSite pSite_right,
3359                  pCvVoronoiEdge pEdge,
3360                  CvVoronoiDiagramInt* pVoronoiDiagram)
3361 {
3362     if((pSite_left->node1!=pSite_left->node2)&&
3363         (pSite_right->node1!=pSite_right->node2))
3364         _cvCalcEdgeLL(pSite_left->direction,
3365                      pSite_right->direction,
3366                      pEdge,pVoronoiDiagram);
3367
3368     else if((pSite_left->node1!=pSite_left->node2)&&
3369             (pSite_right->node1 == pSite_right->node2))
3370         _cvCalcEdgeLP(pSite_left,pSite_right->node1,pEdge,pVoronoiDiagram);
3371
3372     else if((pSite_left->node1==pSite_left->node2)&&
3373             (pSite_right->node1!=pSite_right->node2))
3374         _cvCalcEdgePL(pSite_left->node1,pSite_right,pEdge,pVoronoiDiagram);
3375
3376     else
3377         _cvCalcEdgePP(&(pSite_left->node1->node),
3378                      &(pSite_right->node1->node),
3379                      pEdge,pVoronoiDiagram);
3380 }//end of _cvCalcEdge
3381
3382 void _cvCalcEdge(pCvVoronoiSite pSite,
3383                  pCvVoronoiNode pNode,
3384                  pCvVoronoiEdge pEdge,
3385                  CvVoronoiDiagramInt* pVoronoiDiagram)
3386 {
3387     if(pSite->node1!=pSite->node2)
3388         _cvCalcEdgeLP(pSite, pNode, pEdge,pVoronoiDiagram);
3389     else
3390         _cvCalcEdgePP(&(pSite->node1->node),
3391                      &pNode->node,
3392                      pEdge,pVoronoiDiagram);
3393 }//end of _cvCalcEdge
3394
3395 void _cvCalcEdge(pCvVoronoiNode pNode,
3396                          pCvVoronoiSite pSite,
3397                          pCvVoronoiEdge pEdge,
3398                          CvVoronoiDiagramInt* pVoronoiDiagram)
3399 {
3400     if(pSite->node1!=pSite->node2)
3401         _cvCalcEdgePL(pNode,pSite,pEdge,pVoronoiDiagram);
3402     else
3403         _cvCalcEdgePP(&pNode->node,&pSite->node1->node,pEdge,pVoronoiDiagram);
3404 }//end of _cvCalcEdge
3405
3406 CV_INLINE
3407 void _cvCalcEdgeLL(pCvDirection pDirection1,
3408                   pCvDirection pDirection2,
3409                   pCvVoronoiEdge pEdge,
3410                   CvVoronoiDiagramInt* pVoronoiDiagram)
3411 {
3412     CvDirection Direction = {pDirection2->x - pDirection1->x, pDirection2->y - pDirection1->y};
3413     if((fabs(Direction.x)<LEE_CONST_ZERO)&&(fabs(Direction.y)<LEE_CONST_ZERO))
3414             Direction = *pDirection2;
3415     pEdge->direction = _cvSeqPush(pVoronoiDiagram->DirectionSeq,&Direction);;
3416 }//end of _cvCalcEdgeLL
3417
3418 CV_INLINE
3419 void _cvCalcEdgePP(pCvPointFloat pPoint1,
3420                   pCvPointFloat pPoint2,
3421                   pCvVoronoiEdge pEdge,
3422                   CvVoronoiDiagramInt* pVoronoiDiagram)
3423 {
3424     CvDirection Direction = {pPoint1->y - pPoint2->y,pPoint2->x - pPoint1->x};
3425     pEdge->direction = _cvSeqPush(pVoronoiDiagram->DirectionSeq,&Direction);
3426 }//end of _cvCalcEdgePP
3427
3428 CV_INLINE
3429 void _cvCalcEdgePL(pCvVoronoiNode pFocus,
3430                   pCvVoronoiSite pDirectrice,
3431                   pCvVoronoiEdge pEdge,
3432                   CvVoronoiDiagramInt* pVoronoiDiagram)
3433 {
3434     pCvPointFloat pPoint0 = &pFocus->node;
3435     pCvPointFloat pPoint1 = &pDirectrice->node1->node;
3436
3437     CvDirection Vector01 = {pPoint0->x - pPoint1->x,pPoint0->y - pPoint1->y};
3438     float half_h = (Vector01.y*pDirectrice->direction->x - Vector01.x*pDirectrice->direction->y)/2;
3439     CvDirection Normal = {-pDirectrice->direction->y,pDirectrice->direction->x};
3440     if(half_h < LEE_CONST_ZERO)
3441     {
3442         pEdge->direction = _cvSeqPush(pVoronoiDiagram->DirectionSeq,&Normal);
3443         return;
3444     }
3445     CvVoronoiParabolaInt Parabola;
3446     pCvVoronoiParabola  pParabola = _cvSeqPush(pVoronoiDiagram->ParabolaSeq,&Parabola);
3447     float* map = pParabola->map;
3448
3449     map[1] = Normal.x;
3450     map[4] = Normal.y;
3451     map[0] = Normal.y;
3452     map[3] = -Normal.x;
3453     map[2] = pPoint0->x - Normal.x*half_h;
3454     map[5] = pPoint0->y - Normal.y*half_h;
3455
3456     pParabola->a = 1/(4*half_h);
3457     pParabola->focus = pFocus;
3458     pParabola->directrice = pDirectrice;
3459     pEdge->parabola = pParabola;
3460 }//end of _cvCalcEdgePL
3461
3462 CV_INLINE
3463 void _cvCalcEdgeLP(pCvVoronoiSite pDirectrice,
3464                   pCvVoronoiNode pFocus,
3465                   pCvVoronoiEdge pEdge,
3466                   CvVoronoiDiagramInt* pVoronoiDiagram)
3467 {
3468     pCvPointFloat pPoint0 = &pFocus->node;
3469     pCvPointFloat pPoint1 = &pDirectrice->node1->node;
3470
3471     CvDirection Vector01 = {pPoint0->x - pPoint1->x,pPoint0->y - pPoint1->y};
3472     float half_h = (Vector01.y*pDirectrice->direction->x - Vector01.x*pDirectrice->direction->y)/2;
3473     CvDirection Normal = {-pDirectrice->direction->y,pDirectrice->direction->x};
3474     if(half_h < LEE_CONST_ZERO)
3475     {
3476         pEdge->direction = _cvSeqPush(pVoronoiDiagram->DirectionSeq,&Normal);
3477         return;
3478     }
3479     CvVoronoiParabolaInt Parabola;
3480     pCvVoronoiParabola  pParabola = _cvSeqPush(pVoronoiDiagram->ParabolaSeq,&Parabola);
3481     float* map = pParabola->map;
3482
3483     map[1] = Normal.x;
3484     map[4] = Normal.y;
3485     map[0] = -Normal.y;
3486     map[3] = Normal.x;
3487     map[2] = pPoint0->x - Normal.x*half_h;
3488     map[5] = pPoint0->y - Normal.y*half_h;
3489
3490     pParabola->a = 1/(4*half_h);
3491     pParabola->focus = pFocus;
3492     pParabola->directrice = pDirectrice;
3493     pEdge->parabola = pParabola;
3494 }//end of _cvCalcEdgeLP
3495
3496 /* ///////////////////////////////////////////////////////////////////////////////////////
3497 //                  Computation of intersections of bisectors                           //
3498 /////////////////////////////////////////////////////////////////////////////////////// */
3499
3500 static
3501 float _cvCalcEdgeIntersection(pCvVoronoiEdge pEdge1,
3502                               pCvVoronoiEdge pEdge2,
3503                               CvPointFloat* pPoint,
3504                               float &Radius)
3505 {
3506     if((pEdge1->parabola==NULL)&&(pEdge2->parabola==NULL))
3507         return _cvLine_LineIntersection(pEdge1,pEdge2,pPoint,Radius);
3508     if((pEdge1->parabola==NULL)&&(pEdge2->parabola!=NULL))
3509         return _cvLine_ParIntersection(pEdge1,pEdge2,pPoint,Radius);
3510     if((pEdge1->parabola!=NULL)&&(pEdge2->parabola==NULL))
3511         return _cvPar_LineIntersection(pEdge1,pEdge2,pPoint,Radius);
3512     //if((pEdge1->parabola!=NULL)&&(pEdge2->parabola!=NULL))
3513         return _cvPar_ParIntersection(pEdge1,pEdge2,pPoint,Radius);
3514     //return -1;
3515 }//end of _cvCalcEdgeIntersection
3516
3517 static
3518 float _cvLine_LineIntersection(pCvVoronoiEdge pEdge1,
3519                                 pCvVoronoiEdge pEdge2,
3520                                 pCvPointFloat  pPoint,
3521                                 float &Radius)
3522 {
3523     if(((pEdge1->node1 == pEdge2->node1 ||
3524         pEdge1->node1 == pEdge2->node2) &&
3525         pEdge1->node1 != NULL)||
3526        ((pEdge1->node2 == pEdge2->node1 ||
3527         pEdge1->node2 == pEdge2->node2) &&
3528         pEdge1->node2 != NULL))
3529         return -1;
3530
3531     CvPointFloat Point1,Point3;
3532     float det;
3533     float k,m;
3534     float x21,x43,y43,y21,x31,y31;
3535
3536     if(pEdge1->node1!=NULL)
3537     {
3538         Point1.x = pEdge1->node1->node.x;
3539         Point1.y = pEdge1->node1->node.y;
3540     }
3541     else
3542     {
3543         Point1.x = pEdge1->node2->node.x;
3544         Point1.y = pEdge1->node2->node.y;
3545     }
3546     x21 = pEdge1->direction->x;
3547     y21 = pEdge1->direction->y;
3548
3549     if(pEdge2->node2==NULL)
3550     {
3551         Point3.x = pEdge2->node1->node.x;
3552         Point3.y = pEdge2->node1->node.y;
3553         x43 = pEdge2->direction->x;
3554         y43 = pEdge2->direction->y;
3555
3556     }
3557     else if(pEdge2->node1==NULL)
3558     {
3559         Point3.x = pEdge2->node2->node.x;
3560         Point3.y = pEdge2->node2->node.y;
3561         x43 = pEdge2->direction->x;
3562         y43 = pEdge2->direction->y;
3563     }
3564     else
3565     {
3566         Point3.x = pEdge2->node1->node.x;
3567         Point3.y = pEdge2->node1->node.y;
3568         x43 = pEdge2->node2->node.x - Point3.x;
3569         y43 = pEdge2->node2->node.y - Point3.y;
3570     }
3571
3572     x31 = Point3.x - Point1.x;
3573     y31 = Point3.y - Point1.y;
3574
3575     det = y21*x43 - x21*y43;
3576     if(fabs(det) < LEE_CONST_ZERO)
3577         return -1;
3578
3579     k = (x43*y31 - y43*x31)/det;
3580     m = (x21*y31 - y21*x31)/det;
3581
3582     if(k<-LEE_CONST_ACCEPTABLE_ERROR||m<-LEE_CONST_ACCEPTABLE_ERROR)
3583         return -1;
3584     if(((pEdge1->node2!=NULL)&&(pEdge1->node1!=NULL))&&(k>1.f+LEE_CONST_ACCEPTABLE_ERROR))
3585         return -1;
3586     if(((pEdge2->node2!=NULL)&&(pEdge2->node1!=NULL))&&(m>1.f+LEE_CONST_ACCEPTABLE_ERROR))
3587         return -1;
3588
3589     pPoint->x = (float)(k*x21) + Point1.x;
3590     pPoint->y = (float)(k*y21) + Point1.y;
3591
3592     Radius = _cvCalcDist(pPoint,pEdge1->site);
3593     return _cvPPDist(pPoint,&Point1);;
3594 }//end of _cvLine_LineIntersection
3595
3596 static
3597 float _cvLine_ParIntersection(pCvVoronoiEdge pEdge1,
3598                                 pCvVoronoiEdge pEdge2,
3599                                 pCvPointFloat  pPoint,
3600                                 float &Radius)
3601 {
3602     if(pEdge2->node1==NULL||pEdge2->node2==NULL)
3603         return _cvLine_OpenParIntersection(pEdge1,pEdge2,pPoint,Radius);
3604     return _cvLine_CloseParIntersection(pEdge1,pEdge2,pPoint,Radius);
3605 }//end of _cvLine_ParIntersection
3606
3607 static
3608 float _cvLine_OpenParIntersection(pCvVoronoiEdge pEdge1,
3609                                 pCvVoronoiEdge pEdge2,
3610                                 pCvPointFloat  pPoint,
3611                                 float &Radius)
3612 {
3613     int IntersectionNumber = 1;
3614     if(((pEdge1->node1 == pEdge2->node1 ||
3615         pEdge1->node1 == pEdge2->node2) &&
3616         pEdge1->node1 != NULL)||
3617        ((pEdge1->node2 == pEdge2->node1 ||
3618         pEdge1->node2 == pEdge2->node2) &&
3619         pEdge1->node2 != NULL))
3620         IntersectionNumber = 2;
3621
3622     pCvPointFloat pRayPoint1;
3623     if(pEdge1->node1!=NULL)
3624         pRayPoint1 = &(pEdge1->node1->node);
3625     else
3626         pRayPoint1 = &(pEdge1->node2->node);
3627
3628     pCvDirection pDirection = pEdge1->direction;
3629     float* Parabola = pEdge2->parabola->map;
3630
3631     pCvPointFloat pParPoint1;
3632     if(pEdge2->node1==NULL)
3633         pParPoint1 = &(pEdge2->node2->node);
3634     else
3635         pParPoint1 = &(pEdge2->node1->node);
3636
3637     float InversParabola[6]={0,0,0,0,0,0};
3638     _cvCalcOrtogInverse(InversParabola, Parabola);
3639
3640     CvPointFloat  Point,ParPoint1_img,RayPoint1_img;
3641     CvDirection Direction_img;
3642     _cvCalcPointImage(&RayPoint1_img, pRayPoint1, InversParabola);
3643     _cvCalcVectorImage(&Direction_img,pDirection, InversParabola);
3644
3645     float c2 = pEdge2->parabola->a*Direction_img.x;
3646     float c1 = -Direction_img.y;
3647     float c0 = Direction_img.y* RayPoint1_img.x - Direction_img.x*RayPoint1_img.y;
3648     float X[2];
3649     int N = _cvSolveEqu2thR(c2,c1,c0,X);
3650     if(N==0)
3651         return -1;
3652
3653     _cvCalcPointImage(&ParPoint1_img, pParPoint1, InversParabola);
3654     int sign_x = SIGN(Direction_img.x);
3655     int sign_y = SIGN(Direction_img.y);
3656     if(N==1)
3657     {
3658         if(X[0]<ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
3659             return -1;
3660         float pr0 = (X[0]-RayPoint1_img.x)*sign_x + \
3661                     (pEdge2->parabola->a*X[0]*X[0]-RayPoint1_img.y)*sign_y;
3662         if(pr0 <= -LEE_CONST_ACCEPTABLE_ERROR)
3663             return -1;
3664     }
3665     else
3666     {
3667         if(X[1]<ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
3668             return -1;
3669         float pr0 = (X[0]-RayPoint1_img.x)*sign_x + \
3670                         (pEdge2->parabola->a*X[0]*X[0]-RayPoint1_img.y)*sign_y;
3671         float pr1 = (X[1]-RayPoint1_img.x)*sign_x + \
3672                         (pEdge2->parabola->a*X[1]*X[1]-RayPoint1_img.y)*sign_y;
3673
3674         if(pr0 <= -LEE_CONST_ACCEPTABLE_ERROR &&pr1 <= -LEE_CONST_ACCEPTABLE_ERROR)
3675             return -1;
3676
3677         if(pr0 >- LEE_CONST_ACCEPTABLE_ERROR && pr1 >- LEE_CONST_ACCEPTABLE_ERROR)
3678         {
3679             if(X[0] >= ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
3680             {
3681                 if(pr0>pr1)
3682                     _cvSwap(X[0],X[1]);
3683             }
3684             else
3685             {
3686                 N=1;
3687                 X[0] = X[1];
3688             }
3689         }
3690         else if(pr0 >- LEE_CONST_ACCEPTABLE_ERROR)
3691         {
3692             N = 1;
3693             if(X[0] < ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
3694                 return -1;
3695         }
3696         else if(pr1 >- LEE_CONST_ACCEPTABLE_ERROR)
3697         {
3698             N=1;
3699             X[0] = X[1];
3700         }
3701         else
3702             return -1;
3703     }
3704
3705     Point.x = X[(N-1)*(IntersectionNumber - 1)];
3706     Point.y = pEdge2->parabola->a*Point.x*Point.x;
3707
3708     Radius = Point.y + 1.f/(4*pEdge2->parabola->a);
3709     _cvCalcPointImage(pPoint,&Point,Parabola);
3710     float dist = _cvPPDist(pPoint, pRayPoint1);
3711     if(IntersectionNumber == 2 && dist < LEE_CONST_DIFF_POINTS)
3712         return -1;
3713     else
3714         return dist;
3715 }// end of _cvLine_OpenParIntersection
3716
3717 static
3718 float _cvLine_CloseParIntersection(pCvVoronoiEdge pEdge1,
3719                                 pCvVoronoiEdge pEdge2,
3720                                 pCvPointFloat  pPoint,
3721                                 float &Radius)
3722 {
3723     int IntersectionNumber = 1;
3724     if(((pEdge1->node1 == pEdge2->node1 ||
3725         pEdge1->node1 == pEdge2->node2) &&
3726         pEdge1->node1 != NULL)||
3727        ((pEdge1->node2 == pEdge2->node1 ||
3728         pEdge1->node2 == pEdge2->node2) &&
3729         pEdge1->node2 != NULL))
3730         IntersectionNumber = 2;
3731
3732     pCvPointFloat pRayPoint1;
3733     if(pEdge1->node1!=NULL)
3734         pRayPoint1 = &(pEdge1->node1->node);
3735     else
3736         pRayPoint1 = &(pEdge1->node2->node);
3737
3738     pCvDirection pDirection = pEdge1->direction;
3739     float* Parabola = pEdge2->parabola->map;
3740
3741     pCvPointFloat pParPoint1,pParPoint2;
3742     pParPoint2 = &(pEdge2->node1->node);
3743     pParPoint1 = &(pEdge2->node2->node);
3744
3745
3746     float InversParabola[6]={0,0,0,0,0,0};
3747     _cvCalcOrtogInverse(InversParabola, Parabola);
3748
3749     CvPointFloat  Point,ParPoint1_img,ParPoint2_img,RayPoint1_img;
3750     CvDirection Direction_img;
3751     _cvCalcPointImage(&RayPoint1_img, pRayPoint1, InversParabola);
3752     _cvCalcVectorImage(&Direction_img,pDirection, InversParabola);
3753
3754     float c2 = pEdge2->parabola->a*Direction_img.x;
3755     float c1 = -Direction_img.y;
3756     float c0 = Direction_img.y* RayPoint1_img.x - Direction_img.x*RayPoint1_img.y;
3757     float X[2];
3758     int N = _cvSolveEqu2thR(c2,c1,c0,X);
3759     if(N==0)
3760         return -1;
3761
3762     _cvCalcPointImage(&ParPoint1_img, pParPoint1, InversParabola);
3763     _cvCalcPointImage(&ParPoint2_img, pParPoint2, InversParabola);
3764     if(ParPoint1_img.x>ParPoint2_img.x)
3765         _cvSwap(ParPoint1_img,ParPoint2_img);
3766     int sign_x = SIGN(Direction_img.x);
3767     int sign_y = SIGN(Direction_img.y);
3768     if(N==1)
3769     {
3770         if((X[0]<ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR) ||
3771            (X[0]>ParPoint2_img.x + LEE_CONST_ACCEPTABLE_ERROR))
3772             return -1;
3773         float pr0 = (X[0]-RayPoint1_img.x)*sign_x + \
3774                     (pEdge2->parabola->a*X[0]*X[0]-RayPoint1_img.y)*sign_y;
3775         if(pr0 <= -LEE_CONST_ACCEPTABLE_ERROR)
3776             return -1;
3777     }
3778     else
3779     {
3780         if((X[1]<ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR) ||
3781            (X[0]>ParPoint2_img.x + LEE_CONST_ACCEPTABLE_ERROR))
3782             return -1;
3783
3784         if((X[0]<ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR) &&
3785            (X[1]>ParPoint2_img.x + LEE_CONST_ACCEPTABLE_ERROR))
3786             return -1;
3787
3788         float pr0 = (X[0]-RayPoint1_img.x)*sign_x + \
3789                     (pEdge2->parabola->a*X[0]*X[0]-RayPoint1_img.y)*sign_y;
3790         float pr1 = (X[1]-RayPoint1_img.x)*sign_x + \
3791                     (pEdge2->parabola->a*X[1]*X[1]-RayPoint1_img.y)*sign_y;
3792
3793         if(pr0 <= -LEE_CONST_ACCEPTABLE_ERROR && pr1 <= -LEE_CONST_ACCEPTABLE_ERROR)
3794             return -1;
3795
3796         if(pr0 > -LEE_CONST_ACCEPTABLE_ERROR && pr1 > -LEE_CONST_ACCEPTABLE_ERROR)
3797         {
3798             if(X[0] >= ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
3799             {
3800                 if(X[1] <= ParPoint2_img.x + LEE_CONST_ACCEPTABLE_ERROR)
3801                 {
3802                     if(pr0>pr1)
3803                         _cvSwap(X[0], X[1]);
3804                 }
3805                 else
3806                     N=1;
3807             }
3808             else
3809             {
3810                 N=1;
3811                 X[0] = X[1];
3812             }
3813         }
3814         else if(pr0 > -LEE_CONST_ACCEPTABLE_ERROR)
3815         {
3816
3817             if(X[0] >= ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
3818                 N=1;
3819             else
3820                 return -1;
3821         }
3822         else if(pr1 > -LEE_CONST_ACCEPTABLE_ERROR)
3823         {
3824             if(X[1] <= ParPoint2_img.x + LEE_CONST_ACCEPTABLE_ERROR)
3825             {
3826                 N=1;
3827                 X[0] = X[1];
3828             }
3829             else
3830                 return -1;
3831         }
3832         else
3833             return -1;
3834     }
3835
3836     Point.x = X[(N-1)*(IntersectionNumber - 1)];
3837     Point.y = pEdge2->parabola->a*Point.x*Point.x;
3838     Radius = Point.y + 1.f/(4*pEdge2->parabola->a);
3839     _cvCalcPointImage(pPoint,&Point,Parabola);
3840     float dist = _cvPPDist(pPoint, pRayPoint1);
3841     if(IntersectionNumber == 2 && dist < LEE_CONST_DIFF_POINTS)
3842         return -1;
3843     else
3844         return dist;
3845 }// end of _cvLine_CloseParIntersection
3846
3847 static
3848 float _cvPar_LineIntersection(pCvVoronoiEdge pEdge1,
3849                                 pCvVoronoiEdge pEdge2,
3850                                 pCvPointFloat  pPoint,
3851                                 float &Radius)
3852 {
3853     if(pEdge2->node1==NULL||pEdge2->node2==NULL)
3854         return _cvPar_OpenLineIntersection(pEdge1,pEdge2,pPoint,Radius);
3855     return _cvPar_CloseLineIntersection(pEdge1,pEdge2,pPoint,Radius);
3856 }//end _cvPar_LineIntersection
3857
3858 static
3859 float _cvPar_OpenLineIntersection(pCvVoronoiEdge pEdge1,
3860                                 pCvVoronoiEdge pEdge2,
3861                                 pCvPointFloat  pPoint,
3862                                 float &Radius)
3863 {
3864     int i, IntersectionNumber = 1;
3865     if(((pEdge1->node1 == pEdge2->node1 ||
3866         pEdge1->node1 == pEdge2->node2) &&
3867         pEdge1->node1 != NULL)||
3868        ((pEdge1->node2 == pEdge2->node1 ||
3869         pEdge1->node2 == pEdge2->node2) &&
3870         pEdge1->node2 != NULL))
3871         IntersectionNumber = 2;
3872
3873     float* Parabola = pEdge1->parabola->map;
3874     pCvPointFloat pParPoint1;
3875     if(pEdge1->node1!=NULL)
3876         pParPoint1 = &(pEdge1->node1->node);
3877     else
3878         pParPoint1 = &(pEdge1->node2->node);
3879
3880     pCvPointFloat pRayPoint1;
3881     if(pEdge2->node1==NULL)
3882         pRayPoint1 = &(pEdge2->node2->node);
3883     else
3884         pRayPoint1 = &(pEdge2->node1->node);
3885     pCvDirection pDirection = pEdge2->direction;
3886
3887
3888     float InversParabola[6]={0,0,0,0,0,0};
3889     _cvCalcOrtogInverse(InversParabola, Parabola);
3890
3891     CvPointFloat  Point = {0,0},ParPoint1_img,RayPoint1_img;
3892     CvDirection Direction_img;
3893     _cvCalcVectorImage(&Direction_img,pDirection, InversParabola);
3894     _cvCalcPointImage(&RayPoint1_img, pRayPoint1, InversParabola);
3895
3896
3897     float q = RayPoint1_img.y - pEdge1->parabola->a*RayPoint1_img.x*RayPoint1_img.x;
3898     if((pEdge2->site->node1 == pEdge2->site->node2 && q < 0) ||
3899         (pEdge2->site->node1 != pEdge2->site->node2 && q > 0))
3900         return -1;
3901
3902     float c2 = pEdge1->parabola->a*Direction_img.x;
3903     float c1 = -Direction_img.y;
3904     float c0 = Direction_img.y* RayPoint1_img.x - Direction_img.x*RayPoint1_img.y;
3905     float X[2];
3906     int N = _cvSolveEqu2thR(c2,c1,c0,X);
3907     if(N==0)
3908         return -1;
3909
3910     _cvCalcPointImage(&ParPoint1_img, pParPoint1, InversParabola);
3911     int sign_x = SIGN(Direction_img.x);
3912     int sign_y = SIGN(Direction_img.y);
3913     float pr;
3914
3915     if(N==2 && IntersectionNumber == 2)
3916         _cvSwap(X[0], X[1]);
3917
3918     for( i=0;i<N;i++)
3919     {
3920         if(X[i]<=ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
3921             continue;
3922         pr = (X[i]-RayPoint1_img.x)*sign_x +
3923                         (pEdge1->parabola->a*X[i]*X[i]-RayPoint1_img.y)*sign_y;
3924         if(pr <= -LEE_CONST_ACCEPTABLE_ERROR)
3925             continue;
3926         else
3927         {
3928             Point.x = X[i];
3929             break;
3930         }
3931     }
3932
3933     if(i==N)
3934         return -1;
3935
3936     Point.y = pEdge1->parabola->a*Point.x*Point.x;
3937     Radius = Point.y + 1.f/(4*pEdge1->parabola->a);
3938     _cvCalcPointImage(pPoint,&Point,Parabola);
3939     float dist = Point.x - ParPoint1_img.x;
3940     if(IntersectionNumber == 2 && dist < LEE_CONST_DIFF_POINTS)
3941         return -1;
3942     else
3943         return dist;
3944 }// end of _cvPar_OpenLineIntersection
3945
3946 static
3947 float _cvPar_CloseLineIntersection(pCvVoronoiEdge pEdge1,
3948                                     pCvVoronoiEdge pEdge2,
3949                                     pCvPointFloat  pPoint,
3950                                     float &Radius)
3951 {
3952     int i, IntersectionNumber = 1;
3953     if(((pEdge1->node1 == pEdge2->node1 ||
3954         pEdge1->node1 == pEdge2->node2) &&
3955         pEdge1->node1 != NULL)||
3956        ((pEdge1->node2 == pEdge2->node1 ||
3957         pEdge1->node2 == pEdge2->node2) &&
3958         pEdge1->node2 != NULL))
3959         IntersectionNumber = 2;
3960
3961     float* Parabola = pEdge1->parabola->map;
3962     pCvPointFloat pParPoint1;
3963     if(pEdge1->node1!=NULL)
3964         pParPoint1 = &(pEdge1->node1->node);
3965     else
3966         pParPoint1 = &(pEdge1->node2->node);
3967
3968     pCvPointFloat pRayPoint1,pRayPoint2;
3969     pRayPoint2 = &(pEdge2->node1->node);
3970     pRayPoint1 = &(pEdge2->node2->node);
3971
3972     pCvDirection pDirection = pEdge2->direction;
3973     float InversParabola[6]={0,0,0,0,0,0};
3974     _cvCalcOrtogInverse(InversParabola, Parabola);
3975
3976     CvPointFloat  Point={0,0},ParPoint1_img,RayPoint1_img,RayPoint2_img;
3977     CvDirection Direction_img;
3978     _cvCalcPointImage(&RayPoint1_img, pRayPoint1, InversParabola);
3979     _cvCalcPointImage(&RayPoint2_img, pRayPoint2, InversParabola);
3980
3981     float q;
3982     if(Radius == -1)
3983     {
3984          q = RayPoint1_img.y - pEdge1->parabola->a*RayPoint1_img.x*RayPoint1_img.x;
3985          if((pEdge2->site->node1 == pEdge2->site->node2 && q < 0) ||
3986             (pEdge2->site->node1 != pEdge2->site->node2 && q > 0))
3987                 return -1;
3988     }
3989     if(Radius == -2)
3990     {
3991          q = RayPoint2_img.y - pEdge1->parabola->a*RayPoint2_img.x*RayPoint2_img.x;
3992         if((pEdge2->site->node1 == pEdge2->site->node2 && q < 0) ||
3993             (pEdge2->site->node1 != pEdge2->site->node2 && q > 0))
3994                 return -1;
3995     }
3996
3997     _cvCalcPointImage(&ParPoint1_img, pParPoint1, InversParabola);
3998     _cvCalcVectorImage(&Direction_img,pDirection, InversParabola);
3999
4000     float c2 = pEdge1->parabola->a*Direction_img.x;
4001     float c1 = -Direction_img.y;
4002     float c0 = Direction_img.y* RayPoint1_img.x - Direction_img.x*RayPoint1_img.y;
4003     float X[2];
4004     int N = _cvSolveEqu2thR(c2,c1,c0,X);
4005     if(N==0)
4006         return -1;
4007     int sign_x = SIGN(RayPoint2_img.x - RayPoint1_img.x);
4008     int sign_y = SIGN(RayPoint2_img.y - RayPoint1_img.y);
4009     float pr_dir = (RayPoint2_img.x - RayPoint1_img.x)*sign_x +
4010                    (RayPoint2_img.y - RayPoint1_img.y)*sign_y;
4011     float pr;
4012
4013     if(N==2 && IntersectionNumber == 2)
4014         _cvSwap(X[0], X[1]);
4015
4016     for( i =0;i<N;i++)
4017     {
4018         if(X[i] <= ParPoint1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
4019             continue;
4020         pr = (X[i]-RayPoint1_img.x)*sign_x + \
4021              (pEdge1->parabola->a*X[i]*X[i]-RayPoint1_img.y)*sign_y;
4022         if(pr <= -LEE_CONST_ACCEPTABLE_ERROR || pr>=pr_dir + LEE_CONST_ACCEPTABLE_ERROR)
4023             continue;
4024         else
4025         {
4026             Point.x = X[i];
4027             break;
4028         }
4029     }
4030
4031     if(i==N)
4032         return -1;
4033
4034     Point.y = pEdge1->parabola->a*Point.x*Point.x;
4035     Radius = Point.y + 1.f/(4*pEdge1->parabola->a);
4036     _cvCalcPointImage(pPoint,&Point,Parabola);
4037     float dist = Point.x - ParPoint1_img.x;
4038     if(IntersectionNumber == 2 && dist < LEE_CONST_DIFF_POINTS)
4039         return -1;
4040     else
4041         return dist;
4042 }// end of _cvPar_CloseLineIntersection
4043
4044 static
4045 float _cvPar_ParIntersection(pCvVoronoiEdge pEdge1,
4046                             pCvVoronoiEdge pEdge2,
4047                             pCvPointFloat  pPoint,
4048                             float &Radius)
4049 {
4050     if(pEdge2->node1==NULL||pEdge2->node2==NULL)
4051         return _cvPar_OpenParIntersection(pEdge1,pEdge2,pPoint,Radius);
4052     return _cvPar_CloseParIntersection(pEdge1,pEdge2,pPoint,Radius);
4053 }// end of _cvPar_ParIntersection
4054
4055 static
4056 float _cvPar_OpenParIntersection(pCvVoronoiEdge pEdge1,
4057                             pCvVoronoiEdge pEdge2,
4058                             pCvPointFloat  pPoint,
4059                             float &Radius)
4060 {
4061     int i, IntersectionNumber = 1;
4062     if(((pEdge1->node1 == pEdge2->node1 ||
4063         pEdge1->node1 == pEdge2->node2) &&
4064         pEdge1->node1 != NULL)||
4065        ((pEdge1->node2 == pEdge2->node1 ||
4066         pEdge1->node2 == pEdge2->node2) &&
4067         pEdge1->node2 != NULL))
4068         IntersectionNumber = 2;
4069
4070     float* Parabola1 = pEdge1->parabola->map;
4071     pCvPointFloat pPar1Point1;
4072     if(pEdge1->node1!=NULL)
4073         pPar1Point1 = &(pEdge1->node1->node);
4074     else
4075         pPar1Point1 = &(pEdge1->node2->node);
4076
4077     float* Parabola2 = pEdge2->parabola->map;
4078     pCvPointFloat pPar2Point1;
4079     if(pEdge2->node1!=NULL)
4080         pPar2Point1 = &(pEdge2->node1->node);
4081     else
4082         pPar2Point1 = &(pEdge2->node2->node);
4083
4084     CvPointFloat Point;
4085     CvDirection Direction;
4086     if(pEdge1->parabola->directrice==pEdge2->parabola->directrice)  //common site is segment -> different focuses
4087     {
4088         pCvPointFloat pFocus1 = &(pEdge1->parabola->focus->node);
4089         pCvPointFloat pFocus2 = &(pEdge2->parabola->focus->node);
4090
4091         Point.x = (pFocus1->x + pFocus2->x)/2;
4092         Point.y = (pFocus1->y + pFocus2->y)/2;
4093         Direction.x = pFocus1->y - pFocus2->y;
4094         Direction.y = pFocus2->x - pFocus1->x;
4095     }
4096     else//common site is focus -> different directrices
4097     {
4098         pCvVoronoiSite pDirectrice1 = pEdge1->parabola->directrice;
4099         pCvPointFloat pPoint1 = &(pDirectrice1->node1->node);
4100         pCvDirection pVector21 = pDirectrice1->direction;
4101
4102         pCvVoronoiSite pDirectrice2 = pEdge2->parabola->directrice;
4103         pCvPointFloat pPoint3 = &(pDirectrice2->node1->node);
4104         pCvDirection pVector43 = pDirectrice2->direction;
4105
4106         Direction.x = pVector43->x - pVector21->x;
4107         Direction.y = pVector43->y - pVector21->y;
4108
4109         if((fabs(Direction.x) < LEE_CONST_ZERO) &&
4110            (fabs(Direction.y) < LEE_CONST_ZERO))
4111                 Direction = *pVector43;
4112
4113         float det = pVector21->y * pVector43->x - pVector21->x * pVector43->y;
4114         if(fabs(det) < LEE_CONST_ZERO)
4115         {
4116             Point.x = (pPoint1->x + pPoint3->x)/2;
4117             Point.y = (pPoint1->y + pPoint3->y)/2;
4118         }
4119         else
4120         {
4121             float d1 = pVector21->y*pPoint1->x - pVector21->x*pPoint1->y;
4122             float d2 = pVector43->y*pPoint3->x - pVector43->x*pPoint3->y;
4123             Point.x = (float)((pVector43->x*d1 - pVector21->x*d2)/det);
4124             Point.y = (float)((pVector43->y*d1 - pVector21->y*d2)/det);
4125         }
4126     }
4127
4128     float InversParabola2[6]={0,0,0,0,0,0};
4129     _cvCalcOrtogInverse(InversParabola2, Parabola2);
4130
4131     CvPointFloat  Par2Point1_img,Point_img;
4132     CvDirection Direction_img;
4133     _cvCalcVectorImage(&Direction_img,&Direction, InversParabola2);
4134     _cvCalcPointImage(&Point_img, &Point, InversParabola2);
4135
4136     float a1 = pEdge1->parabola->a;
4137     float a2 = pEdge2->parabola->a;
4138     float c2 = a2*Direction_img.x;
4139     float c1 = -Direction_img.y;
4140     float c0 = Direction_img.y* Point_img.x - Direction_img.x*Point_img.y;
4141     float X[2];
4142     int N = _cvSolveEqu2thR(c2,c1,c0,X);
4143
4144     if(N==0)
4145         return -1;
4146
4147     _cvCalcPointImage(&Par2Point1_img, pPar2Point1, InversParabola2);
4148
4149     if(X[N-1]<Par2Point1_img.x)
4150         return -1;
4151
4152     if(X[0]<Par2Point1_img.x)
4153     {
4154         X[0] = X[1];
4155         N=1;
4156     }
4157
4158     float InversParabola1[6]={0,0,0,0,0,0};
4159     CvPointFloat Par1Point1_img;
4160     _cvCalcOrtogInverse(InversParabola1, Parabola1);
4161     _cvCalcPointImage(&Par1Point1_img, pPar1Point1, InversParabola1);
4162     float InvPar1_Par2[6];
4163     _cvCalcComposition(InvPar1_Par2,InversParabola1,Parabola2);
4164     for(i=0;i<N;i++)
4165         X[i] = (InvPar1_Par2[1]*a2*X[i] + InvPar1_Par2[0])*X[i] +  InvPar1_Par2[2];
4166
4167     if(N!=1)
4168     {
4169         if((X[0]>X[1] && IntersectionNumber == 1)||
4170             (X[0]<X[1] && IntersectionNumber == 2))
4171             _cvSwap(X[0], X[1]);
4172     }
4173
4174     for(i = 0;i<N;i++)
4175     {
4176         Point.x = X[i];
4177         Point.y = a1*Point.x*Point.x;
4178         if(Point.x < Par1Point1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
4179             continue;
4180         else
4181             break;
4182     }
4183
4184     if(i==N)
4185         return -1;
4186
4187     Radius = Point.y + 1.f/(4*pEdge1->parabola->a);
4188     _cvCalcPointImage(pPoint,&Point,Parabola1);
4189     float dist = Point.x - Par1Point1_img.x;
4190     if(IntersectionNumber == 2 && dist < LEE_CONST_DIFF_POINTS)
4191         return -1;
4192     else
4193         return dist;
4194 }// end of _cvPar_OpenParIntersection
4195
4196 static
4197 float _cvPar_CloseParIntersection(pCvVoronoiEdge pEdge1,
4198                                   pCvVoronoiEdge pEdge2,
4199                                   pCvPointFloat  pPoint,
4200                                   float &Radius)
4201 {
4202     int i, IntersectionNumber = 1;
4203     if(((pEdge1->node1 == pEdge2->node1 ||
4204         pEdge1->node1 == pEdge2->node2) &&
4205         pEdge1->node1 != NULL)||
4206        ((pEdge1->node2 == pEdge2->node1 ||
4207         pEdge1->node2 == pEdge2->node2) &&
4208         pEdge1->node2 != NULL))
4209         IntersectionNumber = 2;
4210
4211     float* Parabola1 = pEdge1->parabola->map;
4212     float* Parabola2 = pEdge2->parabola->map;
4213     pCvPointFloat pPar1Point1;
4214     if(pEdge1->node1!=NULL)
4215         pPar1Point1 = &(pEdge1->node1->node);
4216     else
4217         pPar1Point1 = &(pEdge1->node2->node);
4218
4219     pCvPointFloat pPar2Point1 = &(pEdge2->node1->node);
4220     pCvPointFloat pPar2Point2 = &(pEdge2->node2->node);
4221
4222     CvPointFloat Point;
4223     CvDirection Direction;
4224     if(pEdge1->parabola->directrice==pEdge2->parabola->directrice)  //common site is segment -> different focuses
4225     {
4226         pCvPointFloat pFocus1 = &(pEdge1->parabola->focus->node);
4227         pCvPointFloat pFocus2 = &(pEdge2->parabola->focus->node);
4228
4229         Point.x = (pFocus1->x + pFocus2->x)/2;
4230         Point.y = (pFocus1->y + pFocus2->y)/2;
4231         Direction.x = pFocus1->y - pFocus2->y;
4232         Direction.y = pFocus2->x - pFocus1->x;
4233     }
4234     else//common site is focus -> different directrices
4235     {
4236         pCvVoronoiSite pDirectrice1 = pEdge1->parabola->directrice;
4237         pCvPointFloat pPoint1 = &(pDirectrice1->node1->node);
4238         pCvDirection pVector21 = pDirectrice1->direction;
4239
4240         pCvVoronoiSite pDirectrice2 = pEdge2->parabola->directrice;
4241         pCvPointFloat pPoint3 = &(pDirectrice2->node1->node);
4242         pCvDirection pVector43 = pDirectrice2->direction;
4243
4244         Direction.x = pVector43->x - pVector21->x;
4245         Direction.y = pVector43->y - pVector21->y;
4246
4247         if((fabs(Direction.x) < LEE_CONST_ZERO) &&
4248            (fabs(Direction.y) < LEE_CONST_ZERO))
4249                 Direction = *pVector43;
4250
4251         float det = pVector21->y * pVector43->x - pVector21->x * pVector43->y;
4252         if(fabs(det) < LEE_CONST_ZERO)
4253         {
4254             Point.x = (pPoint1->x + pPoint3->x)/2;
4255             Point.y = (pPoint1->y + pPoint3->y)/2;
4256         }
4257         else
4258         {
4259             float d1 = pVector21->y*pPoint1->x - pVector21->x*pPoint1->y;
4260             float d2 = pVector43->y*pPoint3->x - pVector43->x*pPoint3->y;
4261             Point.x = (float)((pVector43->x*d1 - pVector21->x*d2)/det);
4262             Point.y = (float)((pVector43->y*d1 - pVector21->y*d2)/det);
4263         }
4264     }
4265
4266
4267
4268     float InversParabola2[6]={0,0,0,0,0,0};
4269     _cvCalcOrtogInverse(InversParabola2, Parabola2);
4270
4271     CvPointFloat  Par2Point1_img,Par2Point2_img,Point_img;
4272     CvDirection Direction_img;
4273     _cvCalcVectorImage(&Direction_img,&Direction, InversParabola2);
4274     _cvCalcPointImage(&Point_img, &Point, InversParabola2);
4275
4276     float a1 = pEdge1->parabola->a;
4277     float a2 = pEdge2->parabola->a;
4278     float c2 = a2*Direction_img.x;
4279     float c1 = -Direction_img.y;
4280     float c0 = Direction_img.y* Point_img.x - Direction_img.x*Point_img.y;
4281     float X[2];
4282     int N = _cvSolveEqu2thR(c2,c1,c0,X);
4283
4284     if(N==0)
4285         return -1;
4286
4287     _cvCalcPointImage(&Par2Point1_img, pPar2Point1, InversParabola2);
4288     _cvCalcPointImage(&Par2Point2_img, pPar2Point2, InversParabola2);
4289     if(Par2Point1_img.x>Par2Point2_img.x)
4290         _cvSwap(Par2Point1_img,Par2Point2_img);
4291
4292     if(X[0]>Par2Point2_img.x||X[N-1]<Par2Point1_img.x)
4293         return -1;
4294
4295     if(X[0]<Par2Point1_img.x)
4296     {
4297         if(X[1]<Par2Point2_img.x)
4298         {
4299             X[0] = X[1];
4300             N=1;
4301         }
4302         else
4303             return -1;
4304     }
4305     else if(X[N-1]>Par2Point2_img.x)
4306             N=1;
4307
4308     float InversParabola1[6]={0,0,0,0,0,0};
4309     CvPointFloat Par1Point1_img;
4310     _cvCalcOrtogInverse(InversParabola1, Parabola1);
4311     _cvCalcPointImage(&Par1Point1_img, pPar1Point1, InversParabola1);
4312     float InvPar1_Par2[6];
4313     _cvCalcComposition(InvPar1_Par2,InversParabola1,Parabola2);
4314     for(i=0;i<N;i++)
4315         X[i] = (InvPar1_Par2[1]*a2*X[i] + InvPar1_Par2[0])*X[i] +  InvPar1_Par2[2];
4316
4317     if(N!=1)
4318     {
4319         if((X[0]>X[1] && IntersectionNumber == 1)||
4320             (X[0]<X[1] && IntersectionNumber == 2))
4321             _cvSwap(X[0], X[1]);
4322     }
4323
4324
4325     for(i = 0;i<N;i++)
4326     {
4327         Point.x = (float)X[i];
4328         Point.y = (float)a1*Point.x*Point.x;
4329         if(Point.x < Par1Point1_img.x - LEE_CONST_ACCEPTABLE_ERROR)
4330             continue;
4331         else
4332             break;
4333     }
4334
4335     if(i==N)
4336         return -1;
4337
4338     Radius = Point.y + 1.f/(4*a1);
4339     _cvCalcPointImage(pPoint,&Point,Parabola1);
4340     float dist = Point.x - Par1Point1_img.x;
4341     if(IntersectionNumber == 2 && dist < LEE_CONST_DIFF_POINTS)
4342         return -1;
4343     else
4344         return dist;
4345 }// end of _cvPar_CloseParIntersection
4346
4347 /* ///////////////////////////////////////////////////////////////////////////////////////
4348 //                           Subsidiary functions                                       //
4349 /////////////////////////////////////////////////////////////////////////////////////// */
4350
4351 CV_INLINE
4352 void _cvMakeTwinEdge(pCvVoronoiEdge pEdge2,
4353                     pCvVoronoiEdge pEdge1)
4354 {
4355     pEdge2->direction = pEdge1->direction;
4356     pEdge2->parabola = pEdge1->parabola;
4357     pEdge2->node1 = pEdge1->node2;
4358     pEdge2->twin_edge = pEdge1;
4359     pEdge1->twin_edge = pEdge2;
4360 }//end of _cvMakeTwinEdge
4361
4362 CV_INLINE
4363 void _cvStickEdgeLeftBegin(pCvVoronoiEdge pEdge,
4364                           pCvVoronoiEdge pEdge_left_prev,
4365                           pCvVoronoiSite pSite_left)
4366 {
4367     pEdge->prev_edge = pEdge_left_prev;
4368     pEdge->site = pSite_left;
4369     if(pEdge_left_prev == NULL)
4370         pSite_left->edge2 = pEdge;
4371     else
4372     {
4373         pEdge_left_prev->node2 = pEdge->node1;
4374         pEdge_left_prev->next_edge = pEdge;
4375     }
4376 }//end of _cvStickEdgeLeftBegin
4377
4378 CV_INLINE
4379 void _cvStickEdgeRightBegin(pCvVoronoiEdge pEdge,
4380                           pCvVoronoiEdge pEdge_right_next,
4381                           pCvVoronoiSite pSite_right)
4382 {
4383     pEdge->next_edge = pEdge_right_next;
4384     pEdge->site = pSite_right;
4385     if(pEdge_right_next == NULL)
4386         pSite_right->edge1 = pEdge;
4387     else
4388     {
4389         pEdge_right_next->node1 = pEdge->node2;
4390         pEdge_right_next->prev_edge = pEdge;
4391     }
4392 }// end of _cvStickEdgeRightBegin
4393
4394 CV_INLINE
4395 void _cvStickEdgeLeftEnd(pCvVoronoiEdge pEdge,
4396                         pCvVoronoiEdge pEdge_left_next,
4397                         pCvVoronoiSite pSite_left)
4398 {
4399     pEdge->next_edge = pEdge_left_next;
4400     if(pEdge_left_next == NULL)
4401         pSite_left->edge1 = pEdge;
4402     else
4403     {
4404         pEdge_left_next->node1 = pEdge->node2;
4405         pEdge_left_next->prev_edge = pEdge;
4406     }
4407 }//end of _cvStickEdgeLeftEnd
4408
4409 CV_INLINE
4410 void _cvStickEdgeRightEnd(pCvVoronoiEdge pEdge,
4411                          pCvVoronoiEdge pEdge_right_prev,
4412                          pCvVoronoiSite pSite_right)
4413 {
4414     pEdge->prev_edge = pEdge_right_prev;
4415     if(pEdge_right_prev == NULL)
4416         pSite_right->edge2 = pEdge;
4417     else
4418     {
4419         pEdge_right_prev->node2 = pEdge->node1;
4420         pEdge_right_prev->next_edge = pEdge;
4421     }
4422 }//end of _cvStickEdgeRightEnd
4423
4424 template <class T> CV_INLINE
4425 void _cvInitVoronoiNode(pCvVoronoiNode pNode,
4426                         T pPoint,
4427                         float radius)
4428 {
4429     pNode->node.x = (float)pPoint->x;
4430     pNode->node.y = (float)pPoint->y;
4431     pNode->radius = radius;
4432 }//end of _cvInitVoronoiNode
4433
4434 CV_INLINE
4435 void _cvInitVoronoiSite(pCvVoronoiSite pSite,
4436                        pCvVoronoiNode pNode1,
4437                        pCvVoronoiNode pNode2,
4438                        pCvVoronoiSite pPrev_site)
4439 {
4440     pSite->node1 = pNode1;
4441     pSite->node2 = pNode2;
4442     pSite->prev_site = pPrev_site;
4443 }//end of _cvInitVoronoiSite
4444
4445 template <class T> CV_INLINE
4446 T _cvSeqPush(CvSeq* Seq, T pElem)
4447 {
4448     cvSeqPush(Seq, pElem);
4449     return (T)(Seq->ptr - Seq->elem_size);
4450 //  return (T)cvGetSeqElem(Seq, Seq->total - 1,NULL);
4451 }//end of _cvSeqPush
4452
4453 template <class T> CV_INLINE
4454 T _cvSeqPushFront(CvSeq* Seq, T pElem)
4455 {
4456     cvSeqPushFront(Seq,pElem);
4457     return (T)Seq->first->data;
4458 //  return (T)cvGetSeqElem(Seq,0,NULL);
4459 }//end of _cvSeqPushFront
4460
4461 CV_INLINE
4462 void _cvTwinNULLLeft(pCvVoronoiEdge pEdge_left_cur,
4463                     pCvVoronoiEdge pEdge_left)
4464 {
4465     while(pEdge_left_cur!=pEdge_left)
4466     {
4467         if(pEdge_left_cur->twin_edge!=NULL)
4468             pEdge_left_cur->twin_edge->twin_edge = NULL;
4469         pEdge_left_cur = pEdge_left_cur->next_edge;
4470     }
4471 }//end of _cvTwinNULLLeft
4472
4473 CV_INLINE
4474 void _cvTwinNULLRight(pCvVoronoiEdge pEdge_right_cur,
4475                      pCvVoronoiEdge pEdge_right)
4476 {
4477     while(pEdge_right_cur!=pEdge_right)
4478     {
4479         if(pEdge_right_cur->twin_edge!=NULL)
4480             pEdge_right_cur->twin_edge->twin_edge = NULL;
4481         pEdge_right_cur = pEdge_right_cur->prev_edge;
4482     }
4483 }//end of _cvTwinNULLRight
4484
4485 CV_INLINE
4486 void _cvSeqPushInOrder(CvVoronoiDiagramInt* pVoronoiDiagram, pCvVoronoiHole pHole)
4487 {
4488     pHole = _cvSeqPush(pVoronoiDiagram->HoleSeq, pHole);
4489     if(pVoronoiDiagram->HoleSeq->total == 1)
4490     {
4491         pVoronoiDiagram->top_hole = pHole;
4492         return;
4493     }
4494
4495     pCvVoronoiHole pTopHole = pVoronoiDiagram->top_hole;
4496     pCvVoronoiHole pCurrHole;
4497     if(pTopHole->x_coord >= pHole->x_coord)
4498     {
4499         pHole->next_hole = pTopHole;
4500         pVoronoiDiagram->top_hole = pHole;
4501         return;
4502     }
4503
4504     for(pCurrHole = pTopHole; \
4505         pCurrHole->next_hole != NULL; \
4506         pCurrHole = pCurrHole->next_hole)
4507         if(pCurrHole->next_hole->x_coord >= pHole->x_coord)
4508             break;
4509     pHole->next_hole = pCurrHole->next_hole;
4510     pCurrHole->next_hole = pHole;
4511 }//end of _cvSeqPushInOrder
4512
4513 CV_INLINE
4514 pCvVoronoiEdge _cvDivideRightEdge(pCvVoronoiEdge pEdge,pCvVoronoiNode pNode, CvSeq* EdgeSeq)
4515 {
4516     CvVoronoiEdgeInt Edge1 = *pEdge;
4517     CvVoronoiEdgeInt Edge2 = *pEdge->twin_edge;
4518     pCvVoronoiEdge pEdge1, pEdge2;
4519
4520     pEdge1 = _cvSeqPush(EdgeSeq, &Edge1);
4521     pEdge2 = _cvSeqPush(EdgeSeq, &Edge2);
4522
4523     if(pEdge1->next_edge != NULL)
4524         pEdge1->next_edge->prev_edge = pEdge1;
4525     pEdge1->prev_edge = NULL;
4526
4527     if(pEdge2->prev_edge != NULL)
4528         pEdge2->prev_edge->next_edge = pEdge2;
4529     pEdge2->next_edge = NULL;
4530
4531     pEdge1->node1 = pEdge2->node2= pNode;
4532     pEdge1->twin_edge = pEdge2;
4533     pEdge2->twin_edge = pEdge1;
4534     return pEdge2;
4535 }//end of _cvDivideRightEdge
4536
4537 CV_INLINE
4538 pCvVoronoiEdge _cvDivideLeftEdge(pCvVoronoiEdge pEdge,pCvVoronoiNode pNode, CvSeq* EdgeSeq)
4539 {
4540     CvVoronoiEdgeInt Edge1 = *pEdge;
4541     CvVoronoiEdgeInt Edge2 = *pEdge->twin_edge;
4542     pCvVoronoiEdge pEdge1, pEdge2;
4543
4544     pEdge1 = _cvSeqPush(EdgeSeq, &Edge1);
4545     pEdge2 = _cvSeqPush(EdgeSeq, &Edge2);
4546
4547     if(pEdge2->next_edge != NULL)
4548         pEdge2->next_edge->prev_edge = pEdge2;
4549     pEdge2->prev_edge = NULL;
4550
4551     if(pEdge1->prev_edge != NULL)
4552         pEdge1->prev_edge->next_edge = pEdge1;
4553     pEdge1->next_edge = NULL;
4554
4555     pEdge1->node2 = pEdge2->node1= pNode;
4556     pEdge1->twin_edge = pEdge2;
4557     pEdge2->twin_edge = pEdge1;
4558     return pEdge2;
4559 }//end of _cvDivideLeftEdge
4560
4561 template<class T> CV_INLINE
4562 T _cvWriteSeqElem(T pElem, CvSeqWriter &writer)
4563 {
4564     if( writer.ptr >= writer.block_max )
4565           cvCreateSeqBlock( &writer);
4566
4567     T ptr = (T)writer.ptr;
4568     memcpy(writer.ptr, pElem, sizeof(*pElem));
4569     writer.ptr += sizeof(*pElem);
4570     return ptr;
4571 }//end of _cvWriteSeqElem
4572
4573 /* ///////////////////////////////////////////////////////////////////////////////////////
4574 //                           Mathematical functions                                     //
4575 /////////////////////////////////////////////////////////////////////////////////////// */
4576
4577 template<class T> CV_INLINE
4578 void _cvCalcPointImage(pCvPointFloat pImgPoint,pCvPointFloat pPoint,T* A)
4579 {
4580     pImgPoint->x = (float)(A[0]*pPoint->x + A[1]*pPoint->y + A[2]);
4581     pImgPoint->y = (float)(A[3]*pPoint->x + A[4]*pPoint->y + A[5]);
4582 }//end of _cvCalcPointImage
4583
4584 template <class T> CV_INLINE
4585 void _cvSwap(T &x, T &y)
4586 {
4587     T z; z=x; x=y; y=z;
4588 }//end of _cvSwap
4589
4590 template <class T> CV_INLINE
4591 int _cvCalcOrtogInverse(T* B, T* A)
4592 {
4593     int sign_det = SIGN(A[0]*A[4] - A[1]*A[3]);
4594
4595     if(sign_det)
4596     {
4597         B[0] =  A[4]*sign_det;
4598         B[1] = -A[1]*sign_det;
4599         B[3] = -A[3]*sign_det;
4600         B[4] =  A[0]*sign_det;
4601         B[2] = - (B[0]*A[2]+B[1]*A[5]);
4602         B[5] = - (B[3]*A[2]+B[4]*A[5]);
4603         return 1;
4604     }
4605     else
4606         return 0;
4607 }//end of _cvCalcOrtogInverse
4608
4609 template<class T> CV_INLINE
4610 void _cvCalcVectorImage(pCvDirection pImgVector,pCvDirection pVector,T* A)
4611 {
4612     pImgVector->x = A[0]*pVector->x + A[1]*pVector->y;
4613     pImgVector->y = A[3]*pVector->x + A[4]*pVector->y;
4614 }//end of _cvCalcVectorImage
4615
4616 template <class T> CV_INLINE
4617 void _cvCalcComposition(T* Result,T* A,T* B)
4618 {
4619     Result[0] = A[0]*B[0] + A[1]*B[3];
4620     Result[1] = A[0]*B[1] + A[1]*B[4];
4621     Result[3] = A[3]*B[0] + A[4]*B[3];
4622     Result[4] = A[3]*B[1] + A[4]*B[4];
4623     Result[2] = A[0]*B[2] + A[1]*B[5] + A[2];
4624     Result[5] = A[3]*B[2] + A[4]*B[5] + A[5];
4625 }//end of _cvCalcComposition
4626
4627 CV_INLINE
4628 float _cvCalcDist(pCvPointFloat pPoint, pCvVoronoiSite pSite)
4629 {
4630     if(pSite->node1==pSite->node2)
4631         return _cvPPDist(pPoint,&(pSite->node1->node));
4632     else
4633         return _cvPLDist(pPoint,&(pSite->node1->node),pSite->direction);
4634 }//end of _cvCalcComposition
4635
4636 CV_INLINE
4637 float _cvPPDist(pCvPointFloat pPoint1,pCvPointFloat pPoint2)
4638 {
4639     float delta_x = pPoint1->x - pPoint2->x;
4640     float delta_y = pPoint1->y - pPoint2->y;
4641     return (float)sqrt((double)delta_x*delta_x + delta_y*delta_y);
4642 }//end of _cvPPDist
4643
4644
4645 CV_INLINE
4646 float _cvPLDist(pCvPointFloat pPoint,pCvPointFloat pPoint1,pCvDirection pDirection)
4647 {
4648     return (float)fabs(pDirection->x*(pPoint->y - pPoint1->y) -
4649                      pDirection->y*(pPoint->x - pPoint1->x));
4650 }//end of _cvPLDist
4651
4652 template <class T>
4653 int _cvSolveEqu2thR(T c2, T c1, T c0, T* X)
4654 {
4655     const T eps = (T)1e-6;
4656     if(fabs(c2)<eps)
4657         return _cvSolveEqu1th(c1,c0,X);
4658
4659     T Discr = c1*c1 - c2*c0*4;
4660     if(Discr<-eps)
4661         return 0;
4662     Discr = (T)sqrt((double)fabs(Discr));
4663
4664     if(fabs(Discr)<eps)
4665     {
4666         X[0] = -c1/(c2*2);
4667         if(fabs(X[0])<eps)
4668             X[0]=0;
4669         return 1;
4670     }
4671     else
4672     {
4673         if(c1 >=0)
4674         {
4675             if(c2>0)
4676             {
4677                 X[0] = (-c1 - Discr)/(2*c2);
4678                 X[1] = -2*c0/(c1+Discr);
4679                 return 2;
4680             }
4681             else
4682             {
4683                 X[1] = (-c1 - Discr)/(2*c2);
4684                 X[0] = -2*c0/(c1+Discr);
4685                 return 2;
4686             }
4687         }
4688         else
4689         {
4690             if(c2>0)
4691             {
4692                 X[1] = (-c1 + Discr)/(2*c2);
4693                 X[0] = -2*c0/(c1-Discr);
4694                 return 2;
4695             }
4696             else
4697             {
4698                 X[0] = (-c1 + Discr)/(2*c2);
4699                 X[1] = -2*c0/(c1-Discr);
4700                 return 2;
4701             }
4702         }
4703     }
4704 }//end of _cvSolveEqu2thR
4705
4706 template <class T> CV_INLINE
4707 int _cvSolveEqu1th(T c1, T c0, T* X)
4708 {
4709     const T eps = (T)1e-6;
4710     if(fabs(c1)<eps)
4711         return 0;
4712
4713     X[0] = -c0/c1;
4714     return 1;
4715 }//end of _cvSolveEqu1th