mathutil.cpp

这是个人脸识别程序

// $mat\mathutil.cpp 1.5 milbo$ mathematics utilities
// Warning: this is raw research code -- expect it to be quite messy.

#include <stdio.h>
#include <float.h>
#pragma warning(disable:4786)		// MSC compiler: disable warnings about extremely long variable names in STL
#include "cvec.hpp"
#include "mcommon.hpp"
#include "mat.hpp"
#include "matview.hpp"
using namespace GslMat;
#include "mchol.hpp"
#include "mathutil.hpp"
#include "regex.h"
#include "util.hpp"
#include "err.hpp"
#include "memstate.hpp"

//-----------------------------------------------------------------------------
// true if x is a "normal" IEEE number

bool fIeeeNormal (double x)
{
int c = _fpclass(x);

// Negative normal | -0 | +0 | Positive normal

return (c & (_FPCLASS_NN|_FPCLASS_NZ|_FPCLASS_PZ|_FPCLASS_PN)) != 0;
}

//-----------------------------------------------------------------------------
// This function compares two floats to see if they equal or almost so.
//
// maxUlps is the maximum error in terms of ULPS "units in the last place".
// This specifies how big an error we are willing to accept in terms of the
// value of the least significant digit of the floating point number抯
// representation. maxUlps can also be interpreted in terms of how many
// representable floats we are willing to accept between A and B.
//
// This function has the following good characteristics:
//
//	  Measures whether two floats are "close" to each other, where
//	  close is defined in terms of ULPS.
//
//	  Treats infinity as being close to FLT_MAX
//
//	  Treats NANs as being four million ulps away from everything
//	  (assuming the default NAN value for x87), except other NANs
//
//	  Accepts greater relative error as numbers gradually underflow
//    to subnormals
//
//	  Treats tiny negative numbers as being close to tiny
//	  positive numbers
//
// Lifted from "Comparing floating point numbers" by Bruce Dawson (web document)
//
// This function uses floats.  Converting to doubles is a bit tricky and
// hasn't been done.  You need 64 bit unsigned longs.

bool fFloatsAlmostEqual (float A, float B, int maxUlps)
{
// Make sure maxUlps is non-negative and small enough that the
// default NAN won't compare as equal to anything.

if (!(maxUlps > 0 && maxUlps < 4 * 1024 * 1024))
	SysErr("FloatsAlmostEqual maxUlps %x\n", maxUlps);

int aInt = *(int*)&A;

// make aInt lexicographically ordered as a twos-complement int

if (aInt < 0)
	aInt = 0x80000000 - aInt;

int bInt = *(int*)&B;
if (bInt < 0)
	bInt = 0x80000000 - bInt;
int intDiff = abs(aInt - bInt);
if (intDiff <= maxUlps)
	return true;
return false;
}

//-----------------------------------------------------------------------------
// Return normalized vector bisector of three ordered points.
// We rotate the vector by OffsetAngle (in radians).  Make OffsetAngle=0 to get standard bisector.
// TODO not tested in test.cpp

Vec GetBisector (const Vec &Prev, const Vec &This, const Vec &Next, double OffsetAngle)	// in
{
static Vec p(2, ROWVEC), q(2, ROWVEC);	// static to reduce number of matrix allocations, this routine is called a lot

#if GSL_RANGE_CHECK
DASSERT(!(Prev(VX) == 0 && Prev(VY) == 0));
DASSERT(!(This(VX) == 0 && This(VY) == 0));
DASSERT(!(Next(VX) == 0 && Next(VY) == 0));
#endif

static Vec d0; d0.assign(This); d0 -= Prev; d0.normalizeMe();
p(VX) = sin(OffsetAngle)  * d0(VX) + cos(OffsetAngle) * d0(VY);		// rotate 90 degrees + OffsetAngle
p(VY) = -cos(OffsetAngle) * d0(VX) + sin(OffsetAngle) * d0(VY);

static Vec d1; d1.assign(Next); d1 -=This; d1.normalizeMe();
q(VX) = sin(OffsetAngle)  * d1(VX) + cos(OffsetAngle) * d1(VY);
q(VY) = -cos(OffsetAngle) * d1(VX) + sin(OffsetAngle) * d1(VY);

Vec d(p); d += q; d.normalizeMe();

if (fabs(d(VX)) < 1e-10 && fabs(d(VY)) < 1e-10)	// are points Prev and Next in same line?
	return (This - Prev).normalize();			// yes, do it this way to avoid numerical issues

return d;
}