text: /*M///////////////////////////////////////////////////////////////////////////////////////
  //
  //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
  //
  //  By downloading, copying, installing or using the software you agree to this license.
  //  If you do not agree to this license, do not download, install,
  //  copy or use the software.
  //
  //
  //                        Intel License Agreement
  //                For Open Source Computer Vision Library
  //
  // Copyright (C) 2000, Intel Corporation, all rights reserved.
  // Third party copyrights are property of their respective owners.
  //
  // Redistribution and use in source and binary forms, with or without modification,
  // are permitted provided that the following conditions are met:
  //
  //   * Redistribution's of source code must retain the above copyright notice,
  //     this list of conditions and the following disclaimer.
  //
  //   * Redistribution's in binary form must reproduce the above copyright notice,
  //     this list of conditions and the following disclaimer in the documentation
  //     and/or other materials provided with the distribution.
  //
  //   * The name of Intel Corporation may not be used to endorse or promote products
  //     derived from this software without specific prior written permission.
  //
  // This software is provided by the copyright holders and contributors "as is" and
  // any express or implied warranties, including, but not limited to, the implied
  // warranties of merchantability and fitness for a particular purpose are disclaimed.
  // In no event shall the Intel Corporation or contributors be liable for any direct,
  // indirect, incidental, special, exemplary, or consequential damages
  // (including, but not limited to, procurement of substitute goods or services;
  // loss of use, data, or profits; or business interruption) however caused
  // and on any theory of liability, whether in contract, strict liability,
  // or tort (including negligence or otherwise) arising in any way out of
  // the use of this software, even if advised of the possibility of such damage.
  //
  //M*/
  
  /****************************************************************************************\
        Contour-based face feature tracking
        The code was created by Tatiana Cherepanova (tata@sl.iae.nsk.su)
  \****************************************************************************************/
  
  #include "precomp.hpp"
  #include "_vectrack.h"
  
  #define NUM_FACE_ELEMENTS   3
  enum 
  {
      MOUTH = 0,
      LEYE = 1,
      REYE = 2,
  };
  
  #define MAX_LAYERS      64
  
  const double pi = 3.1415926535;
  
  struct CvFaceTracker;
  struct CvTrackingRect;
  class CvFaceElement;
  
  void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/);
  int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy);
  int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel);
  inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl);
  inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element);
  inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints);
  inline double CalculateTransformationLMS3( CvPoint* pTemplPoints, 
                                     CvPoint* pSrcPoints,
                                     double*       pdbAverageScale,
                                     double*       pdbAverageRotate,
                                     double*       pdbAverageShiftX,
                                     double*       pdbAverageShiftY );
  
  struct CvTrackingRect
  {
      CvRect r;
      CvPoint ptCenter;
      int iColor;
      int iEnergy;
      int nRectsInThis;
      int nRectsOnLeft;
      int nRectsOnRight;
      int nRectsOnTop;
      int nRectsOnBottom;
      CvTrackingRect() { memset(this, 0, sizeof(CvTrackingRect)); };
      int Energy(const CvTrackingRect& prev)
      {
          int prev_color = 0 == prev.iColor ? iColor : prev.iColor;
          iEnergy =	1 * pow2(r.width - prev.r.width) + 
              1 * pow2(r.height - prev.r.height) + 
              1 * pow2(iColor - prev_color) / 4 + 
              - 1 * nRectsInThis + 
              - 0 * nRectsOnTop + 
              + 0 * nRectsOnLeft + 
              + 0 * nRectsOnRight + 
              + 0 * nRectsOnBottom;
          return iEnergy;
      }
  };
  
  struct CvFaceTracker
  {
      CvTrackingRect face[NUM_FACE_ELEMENTS];
      int iTrackingFaceType;
      double dbRotateDelta;
      double dbRotateAngle;
      CvPoint ptRotate;
      
      CvPoint ptTempl[NUM_FACE_ELEMENTS];
      CvRect rTempl[NUM_FACE_ELEMENTS];
      
      IplImage* imgGray;
      IplImage* imgThresh;
      CvMemStorage* mstgContours;
      CvFaceTracker()
      {
          ptRotate.x = 0;
          ptRotate.y = 0;
          dbRotateDelta = 0;
          dbRotateAngle = 0;
          iTrackingFaceType = -1;
          imgThresh = NULL;
          imgGray = NULL;
          mstgContours = NULL;
      };
      ~CvFaceTracker()
      {
          if (NULL != imgGray)
              delete imgGray;
          if (NULL != imgThresh)
              delete imgThresh;
          if (NULL != mstgContours)
              cvReleaseMemStorage(&mstgContours);
      };
      int Init(CvRect* pRects, IplImage* imgGray)
      {
          for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
          {
              face[i].r = pRects[i];
              face[i].ptCenter = Center(face[i].r);
              ptTempl[i] = face[i].ptCenter;
              rTempl[i] = face[i].r;
          }
          imgGray = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
          imgThresh = cvCreateImage(cvSize(imgGray->width, imgGray->height), 8, 1);
          mstgContours = cvCreateMemStorage();
          if ((NULL == imgGray) || 
              (NULL == imgThresh) || 
              (NULL == mstgContours))
              return FALSE;
          return TRUE;
      };
      int InitNextImage(IplImage* img)
      {
          CvSize sz = {img->width, img->height};
          ReallocImage(&imgGray, sz, 1);
          ReallocImage(&imgThresh, sz, 1);
          ptRotate = face[MOUTH].ptCenter;
          float m[6];
          CvMat mat = cvMat( 2, 3, CV_32FC1, m ); 
  
          if (NULL == imgGray || NULL == imgThresh)
              return FALSE;
          
          /*m[0] = (float)cos(-dbRotateAngle*CV_PI/180.);
          m[1] = (float)sin(-dbRotateAngle*CV_PI/180.);
          m[2] = (float)ptRotate.x;
          m[3] = -m[1];
          m[4] = m[0];
          m[5] = (float)ptRotate.y;*/
          cv2DRotationMatrix( cvPointTo32f(ptRotate), -dbRotateAngle, 1., &mat );
          cvWarpAffine( img, imgGray, &mat );
          
          if (NULL == mstgContours)
              mstgContours = cvCreateMemStorage();
          else
              cvClearMemStorage(mstgContours);
          if (NULL == mstgContours)
              return FALSE;
          return TRUE;
      }
  };
  
  class CvFaceElement
  {
  public:
      CvSeq* m_seqRects;
      CvMemStorage* m_mstgRects;
      CvRect m_rROI;
      CvTrackingRect m_trPrev;
      inline CvFaceElement()
      {
          m_seqRects = NULL;
          m_mstgRects = NULL;
          m_rROI.x = 0;
          m_rROI.y = 0;
          m_rROI.width = 0;
          m_rROI.height = 0;
      };
      inline int Init(const CvRect& roi, const CvTrackingRect& prev, CvMemStorage* mstg = NULL)
      {
          m_rROI = roi;
          m_trPrev = prev;
          if (NULL != mstg)
              m_mstgRects = mstg;
          if (NULL == m_mstgRects)
              return FALSE;
          if (NULL == m_seqRects)
              m_seqRects = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvTrackingRect), m_mstgRects);
          else
              cvClearSeq(m_seqRects);
          if (NULL == m_seqRects)
              return FALSE;
          return TRUE;
      };
      void FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
  protected:
      void FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize);
      void MergeRects(int d);
      void Energy();
  }; //class CvFaceElement
  
  int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
  {
      return ((CvTrackingRect*)el1)->iEnergy - ((CvTrackingRect*)el2)->iEnergy;
  }// int CV_CDECL CompareEnergy(const void* el1, const void* el2, void*)
  
  void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
  {
      FindContours(img, thresh, nLayers, dMinSize / 4);
      if (0 == m_seqRects->total)
          return;
      Energy();
      cvSeqSort(m_seqRects, CompareEnergy, NULL);
      CvTrackingRect* pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
      if (m_seqRects->total < 32)
      {
          MergeRects(dMinSize / 8);
          Energy();
          cvSeqSort(m_seqRects, CompareEnergy, NULL);
      }
      pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
      if ((pR->iEnergy > 100 && m_seqRects->total < 32) || (m_seqRects->total < 16))
      {
          MergeRects(dMinSize / 4);
          Energy();
          cvSeqSort(m_seqRects, CompareEnergy, NULL);
      }
      pR = (CvTrackingRect*)cvGetSeqElem(m_seqRects, 0);
      if ((pR->iEnergy > 100 && m_seqRects->total < 16) || (pR->iEnergy > 200 && m_seqRects->total < 32))
      {
          MergeRects(dMinSize / 2);
          Energy();
          cvSeqSort(m_seqRects, CompareEnergy, NULL);
      }
  
  }// void CvFaceElement::FindRects(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
  
  void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers, int dMinSize)
  {
      CvSeq* seq;
      CvRect roi = m_rROI;
      Extend(roi, 1);
      cvSetImageROI(img, roi);
      cvSetImageROI(thresh, roi);
      // layers
      int colors[MAX_LAYERS] = {0};
      int iMinLevel = 0, iMaxLevel = 255;
      float step, power;
      ThresholdingParam(img, nLayers / 2, iMinLevel, iMaxLevel, step, power, 4);
      int iMinLevelPrev = iMinLevel;
      int iMaxLevelPrev = iMinLevel;
      if (m_trPrev.iColor != 0)
      {
          iMinLevelPrev = m_trPrev.iColor - nLayers / 2;
          iMaxLevelPrev = m_trPrev.iColor + nLayers / 2;
      }
      if (iMinLevelPrev < iMinLevel)
      {
          iMaxLevelPrev += iMinLevel - iMinLevelPrev;
          iMinLevelPrev = iMinLevel;
      }
      if (iMaxLevelPrev > iMaxLevel)
      {
          iMinLevelPrev -= iMaxLevelPrev - iMaxLevel;
          if (iMinLevelPrev < iMinLevel)
              iMinLevelPrev = iMinLevel;
          iMaxLevelPrev = iMaxLevel;
      }
      int n = nLayers;
      n -= (iMaxLevelPrev - iMinLevelPrev + 1) / 2;
      step = float(iMinLevelPrev - iMinLevel + iMaxLevel - iMaxLevelPrev) / float(n);
      int j = 0;
      float level;
      for (level = (float)iMinLevel; level < iMinLevelPrev && j < nLayers; level += step, j++)
          colors[j] = int(level + 0.5);
      for (level = (float)iMinLevelPrev; level < iMaxLevelPrev && j < nLayers; level += 2.0, j++)
          colors[j] = int(level + 0.5);
      for (level = (float)iMaxLevelPrev; level < iMaxLevel && j < nLayers; level += step, j++)
          colors[j] = int(level + 0.5);
      //
      for (int i = 0; i < nLayers; i++)
      {
          cvThreshold(img, thresh, colors[i], 255.0, CV_THRESH_BINARY);
          if (cvFindContours(thresh, m_mstgRects, &seq, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE))
          {
              CvTrackingRect cr;
              for (CvSeq* external = seq; external; external = external->h_next)
              {
                  cr.r = cvContourBoundingRect(external);
                  Move(cr.r, roi.x, roi.y);
                  if (RectInRect(cr.r, m_rROI) && cr.r.width > dMinSize  && cr.r.height > dMinSize)
                  {
                      cr.ptCenter = Center(cr.r);
                      cr.iColor = colors[i];
                      cvSeqPush(m_seqRects, &cr);
                  }
                  for (CvSeq* internal = external->v_next; internal; internal = internal->h_next)
                  {
                      cr.r = cvContourBoundingRect(internal);	  
                      Move(cr.r, roi.x, roi.y);
                      if (RectInRect(cr.r, m_rROI) && cr.r.width > dMinSize  && cr.r.height > dMinSize)
                      {
                          cr.ptCenter = Center(cr.r);
                          cr.iColor = colors[i];
                          cvSeqPush(m_seqRects, &cr);
                      }
                  }
              }
              cvClearSeq(seq);
          }
      }
      cvResetImageROI(img);
      cvResetImageROI(thresh);
  }//void CvFaceElement::FindContours(IplImage* img, IplImage* thresh, int nLayers)
  
  void CvFaceElement::MergeRects(int d)
  {
      int nRects = m_seqRects->total;
      CvSeqReader reader, reader2;
      cvStartReadSeq( m_seqRects, &reader );
      int i, j;
      for (i = 0; i < nRects; i++)
      {
          CvTrackingRect* pRect1 = (CvTrackingRect*)(reader.ptr);
          cvStartReadSeq( m_seqRects, &reader2 );
          cvSetSeqReaderPos(&reader2, i + 1);
          for (j = i + 1; j < nRects; j++)
          {
              CvTrackingRect* pRect2 = (CvTrackingRect*)(reader2.ptr);
              if (abs(pRect1->ptCenter.y - pRect2->ptCenter.y) < d && 
                  abs(pRect1->r.height - pRect2->r.height) < d)
              {
                  CvTrackingRect rNew;
                  rNew.iColor = (pRect1->iColor + pRect2->iColor + 1) / 2;
                  rNew.r.x = min(pRect1->r.x, pRect2->r.x);
                  rNew.r.y = min(pRect1->r.y, pRect2->r.y);
                  rNew.r.width = max(pRect1->r.x + pRect1->r.width, pRect2->r.x + pRect2->r.width) - rNew.r.x;
                  rNew.r.height = min(pRect1->r.y + pRect1->r.height, pRect2->r.y + pRect2->r.height) - rNew.r.y;
                  if (rNew.r != pRect1->r && rNew.r != pRect2->r)
                  {
                      rNew.ptCenter = Center(rNew.r);
                      cvSeqPush(m_seqRects, &rNew);
                  }
              }
              CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
          }
          CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
      }
      // delete equal rects
      for (i = 0; i < m_seqRects->total; i++)
      {
          CvTrackingRect* pRect1 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, i);
          int j_begin = i + 1;
          for (j = j_begin; j < m_seqRects->total;)
          {
              CvTrackingRect* pRect2 = (CvTrackingRect*)cvGetSeqElem(m_seqRects, j);
              if (pRect1->r == pRect2->r)
                  cvSeqRemove(m_seqRects, j);
              else
                  j++;
          }
      }
  
  }//void CvFaceElement::MergeRects(int d)
  
  void CvFaceElement::Energy()
  {
      CvSeqReader reader, reader2;
      cvStartReadSeq( m_seqRects, &reader );
      for (int i = 0; i < m_seqRects->total; i++)
      {
          CvTrackingRect* pRect = (CvTrackingRect*)(reader.ptr);
          // outside and inside rects
          cvStartReadSeq( m_seqRects, &reader2 );
          for (int j = 0; j < m_seqRects->total; j++)
          {
              CvTrackingRect* pRect2 = (CvTrackingRect*)(reader2.ptr);
              if (i != j)
              {
                  if (RectInRect(pRect2->r, pRect->r))
                      pRect->nRectsInThis ++;
                  else if (pRect2->r.y + pRect2->r.height <= pRect->r.y)
                      pRect->nRectsOnTop ++;
                  else if (pRect2->r.y >= pRect->r.y + pRect->r.height)
                      pRect->nRectsOnBottom ++;
                  else if (pRect2->r.x + pRect2->r.width <= pRect->r.x)
                      pRect->nRectsOnLeft ++;
                  else if (pRect2->r.x >= pRect->r.x + pRect->r.width)
                      pRect->nRectsOnRight ++;
              }
              CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader2 );
          }
          // energy
          pRect->Energy(m_trPrev);
          CV_NEXT_SEQ_ELEM( sizeof(CvTrackingRect), reader );
      }
  }//void CvFaceElement::Energy()
  
  CV_IMPL CvFaceTracker*
  cvInitFaceTracker(CvFaceTracker* pFaceTracker, const IplImage* imgGray, CvRect* pRects, int nRects)
  {
      assert(NULL != imgGray);
      assert(NULL != pRects);
      assert(nRects >= NUM_FACE_ELEMENTS);
      if ((NULL == imgGray) ||
          (NULL == pRects) ||
          (nRects < NUM_FACE_ELEMENTS))
          return NULL;
      
      //int new_face = FALSE;
      CvFaceTracker* pFace = pFaceTracker;
      if (NULL == pFace)
      {
          pFace = new CvFaceTracker;
          if (NULL == pFace)
              return NULL;
          //new_face = TRUE;
      }
      pFace->Init(pRects, (IplImage*)imgGray);
      return pFace;
  }//CvFaceTracker* InitFaceTracker(IplImage* imgGray, CvRect* pRects, int nRects)
  
  CV_IMPL void
  cvReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
  {
      if (NULL == *ppFaceTracker)
          return;
      delete *ppFaceTracker;
      *ppFaceTracker = NULL;
  }//void ReleaseFaceTracker(CvFaceTracker** ppFaceTracker)
  
  
  CV_IMPL int
  cvTrackFace(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint* ptRotate, double* dbAngleRotate)
  {
      assert(NULL != pFaceTracker);
      assert(NULL != imgGray);
      assert(NULL != pRects && nRects >= NUM_FACE_ELEMENTS);
      if ((NULL == pFaceTracker) ||
          (NULL == imgGray))
          return FALSE;
      pFaceTracker->InitNextImage(imgGray);
      *ptRotate = pFaceTracker->ptRotate;
      *dbAngleRotate = pFaceTracker->dbRotateAngle;
      
      int nElements = 16;
      double dx = pFaceTracker->face[LEYE].ptCenter.x - pFaceTracker->face[REYE].ptCenter.x;
      double dy = pFaceTracker->face[LEYE].ptCenter.y - pFaceTracker->face[REYE].ptCenter.y;
      double d_eyes = sqrt(dx*dx + dy*dy);
      int d = cvRound(0.25 * d_eyes);
      int dMinSize = d;
      int nRestarts = 0;
      
      int elem;
      
      CvFaceElement big_face[NUM_FACE_ELEMENTS];
  START:
      // init
      for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
      {
          CvRect r = pFaceTracker->face[elem].r;
          Extend(r, d);
          if (r.width < 4*d)
          {
              r.x -= (4*d - r.width) / 2;
              r.width += 4*d - r.width;
          }
          if (r.height < 3*d)
          {
              r.y -= (3*d - r.height) / 2;
              r.height += 3*d - r.height;
          }
          if (r.x < 1)
              r.x = 1;
          if (r.y < 1)
              r.y = 1;
          if (r.x + r.width > pFaceTracker->imgGray->width - 2)
              r.width = pFaceTracker->imgGray->width - 2 - r.x;
          if (r.y + r.height > pFaceTracker->imgGray->height - 2)
              r.height = pFaceTracker->imgGray->height - 2 - r.y;
          if (!big_face[elem].Init(r, pFaceTracker->face[elem], pFaceTracker->mstgContours))
              return FALSE;
      }
      // find contours
      for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
          big_face[elem].FindRects(pFaceTracker->imgGray, pFaceTracker->imgThresh, 32, dMinSize);
      // candidats
      CvTrackingRect new_face[NUM_FACE_ELEMENTS];
      int new_energy = 0;
      int found = ChoiceTrackingFace3(pFaceTracker, nElements, big_face, new_face, new_energy);
      int restart = FALSE;
      int find2 = FALSE;
      int noel = -1;
      if (found)
      {
          if (new_energy > 100000 && -1 != pFaceTracker->iTrackingFaceType)
              find2 = TRUE;
          else if (new_energy > 150000)
          {
              int elements = 0;
              for (int el = 0; el < NUM_FACE_ELEMENTS; el++)
              {
                  if (big_face[el].m_seqRects->total > 16 || (big_face[el].m_seqRects->total > 8 && new_face[el].iEnergy < 100))
                      elements++;
                  else
                      noel = el;
              }
              if (2 == elements)
                  find2 = TRUE;
              else 
                  restart = TRUE;
          }
      }
      else
      {
          if (-1 != pFaceTracker->iTrackingFaceType)
              find2 = TRUE;
          else
              restart = TRUE;
      }
  RESTART:
      if (restart)
      {
          if (nRestarts++ < 2)
          {
              d = d + d/4;
              goto START;
          }
      }
      else if (find2)
      {
          if (-1 != pFaceTracker->iTrackingFaceType)
              noel = pFaceTracker->iTrackingFaceType;
          int found2 = ChoiceTrackingFace2(pFaceTracker, nElements, big_face, new_face, new_energy, noel);
          if (found2 && new_energy < 100000)
          {
              pFaceTracker->iTrackingFaceType = noel;
              found = TRUE;
          }
          else 
          {
              restart = TRUE;
              goto RESTART;
          }
      }
      
      if (found)
      {
          // angle by mouth & eyes
          double vx_prev = double(pFaceTracker->face[LEYE].ptCenter.x + pFaceTracker->face[REYE].ptCenter.x) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.x;
          double vy_prev = double(pFaceTracker->face[LEYE].ptCenter.y + pFaceTracker->face[REYE].ptCenter.y) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.y;
          double vx_prev1 = vx_prev * cos(pFaceTracker->dbRotateDelta) - vy_prev * sin(pFaceTracker->dbRotateDelta);
          double vy_prev1 = vx_prev * sin(pFaceTracker->dbRotateDelta) + vy_prev * cos(pFaceTracker->dbRotateDelta);
          vx_prev = vx_prev1;
          vy_prev = vy_prev1;
          for (elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
              pFaceTracker->face[elem] = new_face[elem];
          double vx = double(pFaceTracker->face[LEYE].ptCenter.x + pFaceTracker->face[REYE].ptCenter.x) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.x;
          double vy = double(pFaceTracker->face[LEYE].ptCenter.y + pFaceTracker->face[REYE].ptCenter.y) / 2.0 - pFaceTracker->face[MOUTH].ptCenter.y;
          pFaceTracker->dbRotateDelta = 0;
          double n1_n2 = (vx * vx + vy * vy) * (vx_prev * vx_prev + vy_prev * vy_prev);
          if (n1_n2 != 0)
              pFaceTracker->dbRotateDelta = asin((vx * vy_prev - vx_prev * vy) / sqrt(n1_n2));
          pFaceTracker->dbRotateAngle -= pFaceTracker->dbRotateDelta;
      }
      else
      {
          pFaceTracker->dbRotateDelta = 0;
          pFaceTracker->dbRotateAngle = 0;
      }
      if ((pFaceTracker->dbRotateAngle >= pi/2 && pFaceTracker->dbRotateAngle > 0) ||
          (pFaceTracker->dbRotateAngle <= -pi/2 && pFaceTracker->dbRotateAngle < 0))
      {
          pFaceTracker->dbRotateDelta = 0;
          pFaceTracker->dbRotateAngle = 0;
          found = FALSE;
      }
      if (found)
      {
          for (int i = 0; i < NUM_FACE_ELEMENTS && i < nRects; i++)
              pRects[i] = pFaceTracker->face[i].r;
      }
      return found;
  }//int FindFaceTracker(CvFaceTracker* pFaceTracker, IplImage* imgGray, CvRect* pRects, int nRects, CvPoint& ptRotate, double& dbAngleRotate)
  
  void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, float &step, float& power, int iHistMin /*= HIST_MIN*/)
  {
      assert(imgGray != NULL);
      assert(imgGray->nChannels == 1);
      int i, j; 
      // create histogram
      int histImg[256] = {0};
      uchar* buffImg = (uchar*)imgGray->imageData;
      CvRect rROI = cvGetImageROI(imgGray);
      buffImg += rROI.y * imgGray->widthStep + rROI.x;
      for (j = 0; j < rROI.height; j++)
      {
          for (i = 0; i < rROI.width; i++)
              histImg[buffImg[i]] ++;
          buffImg += imgGray->widthStep;
      }
      // params
      for (i = 0; i < 256; i++)
      {
          if (histImg[i] > iHistMin)
              break;
      }
      iMinLevel = i;
      for (i = 255; i >= 0; i--)
      {
          if (histImg[i] > iHistMin)
              break;
      }
      iMaxLevel = i;
      if (iMaxLevel <= iMinLevel)
      {
          iMaxLevel = 255;
          iMinLevel = 0;
      }
      // power
      double black = 1;
      double white = 1;
      for (i = iMinLevel; i < (iMinLevel + iMaxLevel) / 2; i++)
          black += histImg[i];
      for (i = (iMinLevel + iMaxLevel) / 2; i < iMaxLevel; i++)
          white += histImg[i];
      power = float(black) / float(2 * white);
      //
      step = float(iMaxLevel - iMinLevel) / float(iNumLayers);
      if (step < 1.0)
          step = 1.0;
  }// void ThresholdingParam(IplImage *imgGray, int iNumLayers, int &iMinLevel, int &iMaxLevel, int &iStep)
  
  int ChoiceTrackingFace3(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy)
  {
      CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
      CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
      new_energy = 0x7fffffff;
      int curr_energy = 0x7fffffff;
      int found = FALSE;
      int N = 0;
      CvSeqReader reader_m, reader_l, reader_r;
      cvStartReadSeq( big_face[MOUTH].m_seqRects, &reader_m );
      for (int i_mouth = 0; i_mouth < big_face[MOUTH].m_seqRects->total && i_mouth < nElements; i_mouth++)
      {
          curr_face[MOUTH] = (CvTrackingRect*)(reader_m.ptr);
          cvStartReadSeq( big_face[LEYE].m_seqRects, &reader_l );
          for (int i_left = 0; i_left < big_face[LEYE].m_seqRects->total && i_left < nElements; i_left++)
          {
              curr_face[LEYE] = (CvTrackingRect*)(reader_l.ptr);
              if (curr_face[LEYE]->r.y + curr_face[LEYE]->r.height < curr_face[MOUTH]->r.y)
              {
                  cvStartReadSeq( big_face[REYE].m_seqRects, &reader_r );
                  for (int i_right = 0; i_right < big_face[REYE].m_seqRects->total && i_right < nElements; i_right++)
                  {
                      curr_face[REYE] = (CvTrackingRect*)(reader_r.ptr);
                      if (curr_face[REYE]->r.y + curr_face[REYE]->r.height < curr_face[MOUTH]->r.y &&
                          curr_face[REYE]->r.x > curr_face[LEYE]->r.x + curr_face[LEYE]->r.width)
                      {
                          curr_energy = GetEnergy(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl);
                          if (curr_energy < new_energy)
                          {
                              for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
                                  new_face[elem] = curr_face[elem];
                              new_energy = curr_energy;
                              found = TRUE;
                          }
                          N++;
                      }
                  }
              }
          }
      }
      if (found)
      {
          for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
              face[elem] = *(new_face[elem]);
      }
      return found;
  } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
  
  int ChoiceTrackingFace2(CvFaceTracker* pTF, const int nElements, const CvFaceElement* big_face, CvTrackingRect* face, int& new_energy, int noel)
  {
      int element[NUM_FACE_ELEMENTS];
      for (int i = 0, elem = 0; i < NUM_FACE_ELEMENTS; i++)
      {
          if (i != noel)
          {
              element[elem] = i;
              elem ++;
          }
          else
              element[2] = i;
      }
      CvTrackingRect* curr_face[NUM_FACE_ELEMENTS] = {NULL};
      CvTrackingRect* new_face[NUM_FACE_ELEMENTS] = {NULL};
      new_energy = 0x7fffffff;
      int curr_energy = 0x7fffffff;
      int found = FALSE;
      int N = 0;
      CvSeqReader reader0, reader1;
      cvStartReadSeq( big_face[element[0]].m_seqRects, &reader0 );
      for (int i0 = 0; i0 < big_face[element[0]].m_seqRects->total && i0 < nElements; i0++)
      {
          curr_face[element[0]] = (CvTrackingRect*)(reader0.ptr);
          cvStartReadSeq( big_face[element[1]].m_seqRects, &reader1 );
          for (int i1 = 0; i1 < big_face[element[1]].m_seqRects->total && i1 < nElements; i1++)
          {
              curr_face[element[1]] = (CvTrackingRect*)(reader1.ptr);
              curr_energy = GetEnergy2(curr_face, pTF->face, pTF->ptTempl, pTF->rTempl, element);
              if (curr_energy < new_energy)
              {
                  for (int elem = 0; elem < NUM_FACE_ELEMENTS; elem++)
                      new_face[elem] = curr_face[elem];
                  new_energy = curr_energy;
                  found = TRUE;
              }
              N++;
          }
      }
      if (found)
      {
          face[element[0]] = *(new_face[element[0]]);
          face[element[1]] = *(new_face[element[1]]);
          // 3 element find by template
          CvPoint templ_v01 = {pTF->ptTempl[element[1]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[1]].y - pTF->ptTempl[element[0]].y};
          CvPoint templ_v02 = {pTF->ptTempl[element[2]].x - pTF->ptTempl[element[0]].x, pTF->ptTempl[element[2]].y - pTF->ptTempl[element[0]].y};
          CvPoint prev_v01 = {pTF->face[element[1]].ptCenter.x - pTF->face[element[0]].ptCenter.x, pTF->face[element[1]].ptCenter.y - pTF->face[element[0]].ptCenter.y};
          CvPoint prev_v02 = {pTF->face[element[2]].ptCenter.x - pTF->face[element[0]].ptCenter.x, pTF->face[element[2]].ptCenter.y - pTF->face[element[0]].ptCenter.y};
          CvPoint new_v01 = {new_face[element[1]]->ptCenter.x - new_face[element[0]]->ptCenter.x, new_face[element[1]]->ptCenter.y - new_face[element[0]]->ptCenter.y};
          double templ_d01 = sqrt((double)templ_v01.x*templ_v01.x + templ_v01.y*templ_v01.y);
          double templ_d02 = sqrt((double)templ_v02.x*templ_v02.x + templ_v02.y*templ_v02.y);
          double prev_d01 = sqrt((double)prev_v01.x*prev_v01.x + prev_v01.y*prev_v01.y);
          double prev_d02 = sqrt((double)prev_v02.x*prev_v02.x + prev_v02.y*prev_v02.y);
          double new_d01 = sqrt((double)new_v01.x*new_v01.x + new_v01.y*new_v01.y);
          double scale = templ_d01 / new_d01;
          double new_d02 = templ_d02 / scale; 
          double sin_a = double(prev_v01.x * prev_v02.y - prev_v01.y * prev_v02.x) / (prev_d01 * prev_d02);
          double cos_a = cos(asin(sin_a));
          double x = double(new_v01.x) * cos_a - double(new_v01.y) * sin_a;
          double y = double(new_v01.x) * sin_a + double(new_v01.y) * cos_a;
          x = x * new_d02 / new_d01;
          y = y * new_d02 / new_d01;
          CvPoint new_v02 = {int(x + 0.5), int(y + 0.5)};
          face[element[2]].iColor = 0;
          face[element[2]].iEnergy = 0;
          face[element[2]].nRectsInThis = 0;
          face[element[2]].nRectsOnBottom = 0;
          face[element[2]].nRectsOnLeft = 0;
          face[element[2]].nRectsOnRight = 0;
          face[element[2]].nRectsOnTop = 0;
          face[element[2]].ptCenter.x = new_v02.x + new_face[element[0]]->ptCenter.x;
          face[element[2]].ptCenter.y = new_v02.y + new_face[element[0]]->ptCenter.y;
          face[element[2]].r.width = int(double(pTF->rTempl[element[2]].width) / (scale) + 0.5);
          face[element[2]].r.height = int(double(pTF->rTempl[element[2]].height) / (scale) + 0.5);
          face[element[2]].r.x = face[element[2]].ptCenter.x - (face[element[2]].r.width + 1) / 2;
          face[element[2]].r.y = face[element[2]].ptCenter.y - (face[element[2]].r.height + 1) / 2;
          assert(face[LEYE].r.x + face[LEYE].r.width <= face[REYE].r.x);
      }
      return found;
  } // int ChoiceTrackingFace3(const CvTrackingRect* tr_face, CvTrackingRect* new_face, int& new_energy)
  
  inline int GetEnergy(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl)
  {
      int energy = 0;
      CvPoint ptNew[NUM_FACE_ELEMENTS];
      CvPoint ptPrev[NUM_FACE_ELEMENTS];
      for (int i = 0; i < NUM_FACE_ELEMENTS; i++)
      {
          ptNew[i] = ppNew[i]->ptCenter;
          ptPrev[i] = pPrev[i].ptCenter;
          energy += ppNew[i]->iEnergy - 2 * ppNew[i]->nRectsInThis;
      }
      double dx = 0, dy = 0, scale = 1, rotate = 0;
      double e_templ = CalculateTransformationLMS3(ptTempl, ptNew, &scale, &rotate, &dx, &dy);
      double e_prev = CalculateTransformationLMS3_0(ptPrev, ptNew);
      double w_eye = double(ppNew[LEYE]->r.width + ppNew[REYE]->r.width) * scale / 2.0;
      double h_eye = double(ppNew[LEYE]->r.height + ppNew[REYE]->r.height) * scale / 2.0;
      double w_mouth = double(ppNew[MOUTH]->r.width) * scale;
      double h_mouth = double(ppNew[MOUTH]->r.height) * scale;
      energy +=
          int(512.0 * (e_prev + 16.0 * e_templ)) +
          4 * pow2(ppNew[LEYE]->r.width - ppNew[REYE]->r.width) + 
          4 * pow2(ppNew[LEYE]->r.height - ppNew[REYE]->r.height) + 
          4 * (int)pow(w_eye - double(rTempl[LEYE].width + rTempl[REYE].width) / 2.0, 2) + 
          2 * (int)pow(h_eye - double(rTempl[LEYE].height + rTempl[REYE].height) / 2.0, 2) + 
          1 * (int)pow(w_mouth - double(rTempl[MOUTH].width), 2) + 
          1 * (int)pow(h_mouth - double(rTempl[MOUTH].height), 2) + 
          0;
      return energy;
  }
  
  inline int GetEnergy2(CvTrackingRect** ppNew, const CvTrackingRect* pPrev, CvPoint* ptTempl, CvRect* rTempl, int* element)
  {
      CvPoint new_v = {ppNew[element[0]]->ptCenter.x - ppNew[element[1]]->ptCenter.x,
          ppNew[element[0]]->ptCenter.y - ppNew[element[1]]->ptCenter.y};
      CvPoint prev_v = {pPrev[element[0]].ptCenter.x - pPrev[element[1]].ptCenter.x,
          pPrev[element[0]].ptCenter.y - pPrev[element[1]].ptCenter.y};
      double new_d = sqrt((double)new_v.x*new_v.x + new_v.y*new_v.y);
      double prev_d = sqrt((double)prev_v.x*prev_v.x + prev_v.y*prev_v.y);
      double dx = ptTempl[element[0]].x - ptTempl[element[1]].x;
      double dy = ptTempl[element[0]].y - ptTempl[element[1]].y;
      double templ_d = sqrt(dx*dx + dy*dy);
      double scale_templ = new_d / templ_d;
      double w0 = (double)ppNew[element[0]]->r.width * scale_templ;
      double h0 = (double)ppNew[element[0]]->r.height * scale_templ;
      double w1 = (double)ppNew[element[1]]->r.width * scale_templ;
      double h1 = (double)ppNew[element[1]]->r.height * scale_templ;
      
      int energy = ppNew[element[0]]->iEnergy + ppNew[element[1]]->iEnergy +
          - 2 * (ppNew[element[0]]->nRectsInThis - ppNew[element[1]]->nRectsInThis) + 
          (int)pow(w0 - (double)rTempl[element[0]].width, 2) +
          (int)pow(h0 - (double)rTempl[element[0]].height, 2) +
          (int)pow(w1 - (double)rTempl[element[1]].width, 2) +
          (int)pow(h1 - (double)rTempl[element[1]].height, 2) +
          (int)pow(new_d - prev_d, 2) +
          0;
      
      return energy;
  }
  
  inline double CalculateTransformationLMS3( CvPoint* pTemplPoints, 
                                     CvPoint* pSrcPoints,
                                     double*       pdbAverageScale,
                                     double*       pdbAverageRotate,
                                     double*       pdbAverageShiftX,
                                     double*       pdbAverageShiftY )
  {
  //    double WS = 0;
      double dbAverageScale = 1;
      double dbAverageRotate = 0;
      double dbAverageShiftX = 0;
      double dbAverageShiftY = 0;
      double dbLMS = 0;
  
      assert( NULL != pTemplPoints);
      assert( NULL != pSrcPoints);
  
      double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
      double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
      double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
      double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
      
      double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
      double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
      
      double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
      double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
      
      double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x + 
          pTemplPoints[1].x * pSrcPoints[1].x + 
          pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
      double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y + 
          pTemplPoints[1].y * pSrcPoints[1].y + 
          pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
      
      double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y + 
          pTemplPoints[1].x * pSrcPoints[1].y + 
          pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
      double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x + 
          pTemplPoints[1].y * pSrcPoints[1].x + 
          pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
      
      dbXtXt -= dbXt * dbXt;
      dbYtYt -= dbYt * dbYt;
      
      dbXsXs -= dbXs * dbXs;
      dbYsYs -= dbYs * dbYs;
      
      dbXtXs -= dbXt * dbXs;
      dbYtYs -= dbYt * dbYs;
      
      dbXtYs -= dbXt * dbYs;
      dbYtXs -= dbYt * dbXs;
      
      dbAverageRotate = atan2( dbXtYs - dbYtXs, dbXtXs + dbYtYs );
      
      double cosR = cos(dbAverageRotate);
      double sinR = sin(dbAverageRotate);
      double del = dbXsXs + dbYsYs;
      if( del != 0 )
      {
          dbAverageScale = (double(dbXtXs + dbYtYs) * cosR + double(dbXtYs - dbYtXs) * sinR) / del;
          dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
      }
      
      dbAverageShiftX = double(dbXt) - dbAverageScale * (double(dbXs) * cosR + double(dbYs) * sinR);
      dbAverageShiftY = double(dbYt) - dbAverageScale * (double(dbYs) * cosR - double(dbXs) * sinR);
      
      if( pdbAverageScale != NULL ) *pdbAverageScale = dbAverageScale;
      if( pdbAverageRotate != NULL ) *pdbAverageRotate = dbAverageRotate;
      if( pdbAverageShiftX != NULL ) *pdbAverageShiftX = dbAverageShiftX;
      if( pdbAverageShiftY != NULL ) *pdbAverageShiftY = dbAverageShiftY;
      
      assert(dbLMS >= 0);
      return dbLMS;
  }
  
  inline double CalculateTransformationLMS3_0( CvPoint* pTemplPoints, CvPoint* pSrcPoints)
  {
      double dbLMS = 0;
  
      assert( NULL != pTemplPoints);
      assert( NULL != pSrcPoints);
  
      double dbXt = double(pTemplPoints[0].x + pTemplPoints[1].x + pTemplPoints[2].x) / 3.0;
      double dbYt = double(pTemplPoints[0].y + pTemplPoints[1].y + pTemplPoints[2].y ) / 3.0;
      double dbXs = double(pSrcPoints[0].x + pSrcPoints[1].x + pSrcPoints[2].x) / 3.0;
      double dbYs = double(pSrcPoints[0].y + pSrcPoints[1].y + pSrcPoints[2].y) / 3.0;
      
      double dbXtXt = double(pow2(pTemplPoints[0].x) + pow2(pTemplPoints[1].x) + pow2(pTemplPoints[2].x)) / 3.0;
      double dbYtYt = double(pow2(pTemplPoints[0].y) + pow2(pTemplPoints[1].y) + pow2(pTemplPoints[2].y)) / 3.0;
      
      double dbXsXs = double(pow2(pSrcPoints[0].x) + pow2(pSrcPoints[1].x) + pow2(pSrcPoints[2].x)) / 3.0;
      double dbYsYs = double(pow2(pSrcPoints[0].y) + pow2(pSrcPoints[1].y) + pow2(pSrcPoints[2].y)) / 3.0;
      
      double dbXtXs = double(pTemplPoints[0].x * pSrcPoints[0].x + 
          pTemplPoints[1].x * pSrcPoints[1].x + 
          pTemplPoints[2].x * pSrcPoints[2].x) / 3.0;
      double dbYtYs = double(pTemplPoints[0].y * pSrcPoints[0].y + 
          pTemplPoints[1].y * pSrcPoints[1].y + 
          pTemplPoints[2].y * pSrcPoints[2].y) / 3.0;
      
      double dbXtYs = double(pTemplPoints[0].x * pSrcPoints[0].y + 
          pTemplPoints[1].x * pSrcPoints[1].y + 
          pTemplPoints[2].x * pSrcPoints[2].y) / 3.0;
      double dbYtXs = double(pTemplPoints[0].y * pSrcPoints[0].x + 
          pTemplPoints[1].y * pSrcPoints[1].x + 
          pTemplPoints[2].y * pSrcPoints[2].x ) / 3.0;
      
      dbXtXt -= dbXt * dbXt;
      dbYtYt -= dbYt * dbYt;
      
      dbXsXs -= dbXs * dbXs;
      dbYsYs -= dbYs * dbYs;
      
      dbXtXs -= dbXt * dbXs;
      dbYtYs -= dbYt * dbYs;
      
      dbXtYs -= dbXt * dbYs;
      dbYtXs -= dbYt * dbXs;
      
      double del = dbXsXs + dbYsYs;
      if( del != 0 )
          dbLMS = dbXtXt + dbYtYt - ((double)pow(dbXtXs + dbYtYs,2) + (double)pow(dbXtYs - dbYtXs,2)) / del;
      return dbLMS;
  }
  
  