fnmath.cpp

在人脸检测的基础之上,对嘴部的运动表情进行分析,进行语音模拟.

// FnMath.cpp: implementation of the FnMath class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "SeqProcess.h"
#include "FnMath.h"
#include <stdlib.h>
#include <stdio.h>
#include <time.h>


#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

FnMath::FnMath()
{

}

FnMath::~FnMath()
{

}

//------------------------------------------------------------------------
double FnMath::Sqr( double a)
{
	return ( (a==0.0)? 0: a*a );
}

//------------------------------------------------------------------------
int FnMath::Sqr( int a)
{
	return ( (a==0)? 0: a*a );
}


//------------------------------------------------------------------------
double FnMath::Min( double a, double b)
{
	return ( (a<=b)? a: b );
}

//------------------------------------------------------------------------
double FnMath::Max( double a, double b)
{
	return ( (a<=b)? b: a );
}

//------------------------------------------------------------------------
int FnMath::Min( int a, int b)
{
	return ( (a<=b)? a: b );
}

//------------------------------------------------------------------------
int FnMath::Max( int a, int b)
{
	return ( (a<=b)? b: a );
}

//------------------------------------------------------------------------
int FnMath::Mod( int i, int n)
{
  int m;
  return (((m = i%n)>=0)? m : n+m);
}

//------------------------------------------------------------------------
int FnMath::Sign( double a)
{
  return (a == 0)? 0: ( ( a > 0)? 1: -1);
}
//------------------------------------------------------------------------
int FnMath::Sign( int a)
{
  return (a == 0)? 0: ( ( a > 0)? 1: -1);
}

//------------------------------------------------------------------------
double FnMath::Sign( double a, double b)
{
  return (b >= 0)? fabs(a): -fabs(a);
}

//------------------------------------------------------------------------
int FnMath::SignE( double a)
{
  return (fabs(a) < EPSILON)? 0: ( ( a > 0)? 1: -1);
}

//------------------------------------------------------------------------
int FnMath::Sign1at0( double a)
{
  return (( a >= 0)? 1: -1);
}

//------------------------------------------------------------------------
double FnMath::nPower( double a, int n)
{
	double p = 1;
	int i;

	if( n > 0)
	{
		for( i = 0; i < n; i++)
		{
			p *= a;
		}
	}
	else if( n < 0)
	{
		ASSERT( fabs(a) > TINY);
		for( i = 0; i > n; i--)
		{
			p /= a;
		}
	}

	return p;
}

//-------------------------------------------------------------------------
// A routine to sort numbers in ascending numerical order.
// Alters the original array.
//-------------------------------------------------------------------------
void FnMath::sortNumbers( double* arr, int* krr, int n)
{
  int i, j;
  double a; 
  int k;

  for( j = 1; j < n; j++)
  {
    a = arr[j];
    k = krr[j];
    i = j - 1;
    while( i >= 0 && arr[i] > a)
    {
      arr[i+1] = arr[i];
      krr[i+1] = krr[i];
      i--;
    }
    arr[i+1] = a;
    krr[i+1] = k;
  }
}

//-------------------------------------------------------------------------
// A routine to sort numbers in ascending numerical order.
// Alters the original array.
//-------------------------------------------------------------------------
void FnMath::sortNumbers2( int* arr, int* krr, int n)
{
  int i, j;
  int a; 
  int k;

  for( j = 1; j < n; j++)
  {
    a = arr[j];
    k = krr[j];
    i = j - 1;
    while( i >= 0 && arr[i] > a)
    {
      arr[i+1] = arr[i];
      krr[i+1] = krr[i];
      i--;
    }
    arr[i+1] = a;
    krr[i+1] = k;
  }
}

//-------------------------------------------------------------------------
// A routine to sort numbers in ascending numerical order.
// Does not alter the original array.
//-------------------------------------------------------------------------
void FnMath::sortNumbers( int* crr, int* krr, int n)
{
  int i, j;
  int a; 
  int k;
  int* arr = (int*) new int[n];
  for( i = 0; i < n; i++)
    arr[i] = crr[i];  // Do not alter the original array crr.

  for( j = 1; j < n; j++)
  {
    a = arr[j];
    k = krr[j];
    i = j - 1;
    while( i >= 0 && arr[i] > a)
    {
      arr[i+1] = arr[i];
      krr[i+1] = krr[i];
      i--;
    }
    arr[i+1] = a;
    krr[i+1] = k;
  }
}

//------------------------------------------------------------------------
#define SWAP(a,b) temp=(a);(a)=(b);(b)=temp;

// Sorts an array arr0[1..n] into ascending numerical order
// using the quicksort algorithm. n is input; arr is replaced on output by its 
// sorted arrangement. 
// Numerical Recipes in C  
void FnMath::sort(int n, double* arr0)
{
	const int NSTACK = 50;
	const int M = 7;

	double* arr = new double[n+1];

	for (int u = 0; u < n; ++u)
	{
		arr[u+1] = arr0[u];
	}

	unsigned long i,ir=n,j,k,l=1;
	int *istack,jstack=0;
	double a,temp;

	istack= new int[ NSTACK + 1];
	for (;;) // Insertion sort when subarray small enough
	{
		if (ir-l < M) 
		{
			for (j=l+1;j<=ir;j++) 
			{
				a=arr[j];
				for (i=j-1;i>=1;i--) 
				{
					if (arr[i] <= a) break;
					arr[i+1]=arr[i];
				}
				arr[i+1]=a;
			}
			if (!jstack) 
			{
				delete[] istack;
				break; // exit the loop "for (;;)"
			}
			ir=istack[jstack];  // pop stack and begin a new round of partitioning 
			l=istack[jstack-1];
			jstack -= 2;
		} 
		else 
		{
			k=(l+ir) >> 1;       // choose median of left, center and right elements as partitioning 
			SWAP(arr[k],arr[l+1])// elelement a. Also rearrange so that a[1]<=a[l+1]<=a[ir].
			if (arr[l+1] > arr[ir]) 
			{
				SWAP(arr[l+1],arr[ir])
			}
			if (arr[l] > arr[ir]) 
			{
				SWAP(arr[l],arr[ir])
			}
			if (arr[l+1] > arr[l]) 
			{
				SWAP(arr[l+1],arr[l])
			}
			i=l+1;    // initialize pointers for partitioning 
			j=ir;
			a=arr[l]; // partitioning element
			for (;;)  // beginning of innermost loop 
			{
				do i++; while (arr[i] < a);  // scan up to find an element > a
				do j--; while (arr[j] > a);  // scan down to find an element < a
				if (j < i) break;            // pointers crossed. partitioning complete. 
				SWAP(arr[i],arr[j])          // exchange elements
			}								 // end of innermost loop 
			arr[l]=arr[j];					 // insert partitioning element
			arr[j]=a;
			jstack += 2;  // push pointers to larger subarray on stack, process smaller subarray 
						 // immediately 
			ASSERT(jstack <= NSTACK);
			if (ir-i+1 >= j-l) 
			{
				istack[jstack]=ir;
				istack[jstack-1]=i;
				ir=j-1;
			} else 
			{
				istack[jstack]=j-1;
				istack[jstack-1]=l;
				l=i;
			}
		}
	}

	for (u = 0; u < n; ++u)
	{
		arr0[n-1-u] = arr[u+1];
	}

	delete[] arr;

}
//--------------------------------------------------------------------------
// Sorts an array arr[1..n] into ascending order using Quicksort, 
// while making the corresponding 
// rearrangement of the array brr[1..n]
// Numerical Recipes in C 
void FnMath::sort2(int n, double* arr0, double* brr0)
{
	ASSERT( n > 1);
	const int NSTACK = 50;
	const int M = 7;

	double* arr = new double[n+1];
	double* brr = new double[n+1];

	for (int u = 0; u < n; ++u)
	{
		arr[u+1] = arr0[u];
		brr[u+1] = brr0[u];
	}

	unsigned long i,ir=n,j,k,l=1;
	int *istack,jstack=0;
	double a,b,temp;

	istack= new int[ NSTACK + 1];
	for (;;)   // Insertion sort when subarray small enough
	{
		if (ir-l < M) 
		{
			for (j=l+1;j<=ir;j++) 
			{
				a=arr[j];
				b=brr[j];
				for (i=j-1;i>=1;i--) 
				{
					if (arr[i] <= a) break;
					arr[i+1]=arr[i];
					brr[i+1]=brr[i];
				}
				arr[i+1]=a;
				brr[i+1]=b;
			}
			if (!jstack) 
			{
				delete[] istack;
				break; // exit the loop "for (;;)"
			}
			ir=istack[jstack];  // pop stack and begin a new round of partitioning 
			l=istack[jstack-1];
			jstack -= 2;
		} 
		else 
		{
			k=(l+ir) >> 1; // choose median of left, center and right elements as partitioning 
			SWAP(arr[k],arr[l+1]) // element a. Also rearrange so that a[1]<=a[l+1]<=a[ir].
			SWAP(brr[k],brr[l+1])
			if (arr[l+1] > arr[ir]) 
			{
				SWAP(arr[l+1],arr[ir])
				SWAP(brr[l+1],brr[ir])
			}
			if (arr[l] > arr[ir]) 
			{
				SWAP(arr[l],arr[ir])
				SWAP(brr[l],brr[ir])
			}
			if (arr[l+1] > arr[l]) 
			{
				SWAP(arr[l+1],arr[l])
				SWAP(brr[l+1],brr[l])
			}
			i=l+1;  // initialize pointers for partitioning 
			j=ir;
			a=arr[l]; // partitioning element
			b=brr[l]; 
			for (;;) // beginning of innermost loop 
			{
				do i++; while (arr[i] < a); // scan up to find an element > a
				do j--; while (arr[j] > a); // scan down to find an element < a
				if (j < i) break;			// pointers crossed. partitioning complete. 
				SWAP(arr[i],arr[j])			// exchange elements of both arrays
				SWAP(brr[i],brr[j])
			} 	 // end of innermost loop 
			arr[l]=arr[j]; // insert partitioning element in both arrays 
			arr[j]=a;
			brr[l]=brr[j];
			brr[j]=b;
			jstack += 2;   // push pointers to larger subarray on stack, process smaller subarray 
			ASSERT(jstack <= NSTACK);  // immediately 
			if (ir-i+1 >= j-l) 
			{
				istack[jstack]=ir;
				istack[jstack-1]=i;
				ir=j-1;
			} else 
			{
				istack[jstack]=j-1;
				istack[jstack-1]=l;
				l=i;
			}
		}
	}

	for (u = 0; u < n; ++u)
	{
		arr0[n-1-u] = arr[u+1];
		brr0[n-1-u] = brr[u+1];
	}

	delete[] arr;
	delete[] brr;

}

#undef SWAP
/* (C) Copr. 1986-92 Numerical Recipes Software -. */
//-------------------------------------------------------------------------
// From Numerical Recipes
//-------------------------------------------------------------------------
double FnMath::dpythag( double a, double b)
{
	double absa,absb;
	absa=fabs(a);
	absb=fabs(b);
	if (absa > absb) return absa*sqrt(1.0+Sqr(absb/absa));
	else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+Sqr(absa/absb)));
}

//---------------------------
double FnMath::random( double max)
//----------------------------
{

 	double a;

	a = rand()*1.0/RAND_MAX;
	a *= max;

    return a;
}
//---------------------------
double FnMath::random( int max)
//----------------------------
{

 	double a;

	a = rand()*1.0/RAND_MAX;
	a *= max;

    return a;

}
//---------------------------
int FnMath::round( double d)
//---------------------------
{
	// round to the nearest integer
	int rounded;

	rounded = (int) (d + 0.5*Sign(d));

	return rounded;
}