#include?“util.h“

static?const?double?PI=3.1415926535?PIx2=6.283185307;
static?const?double?R2D=57.29577951;

//======================Geometry?functions==============================

//convert?an?angle?to?[02*pi）?rad
double?angle（double?a1）
{
??while（a1>=PIx2）?a1-=PIx2;
??while（a1<=-PIx2）?a1+=PIx2;
??return?a1;
}

//calculate?the?absolute?difference?（d）?between?angles?d=[0pi）
double?absAngleDiff（double?a2?double?a1）
{
??a1=angle（a1）;
??a2=angle（a2）;
??double?ans=fabs（a2-a1）;
??if（ans>PI）?ans=PIx2-ans;
??return?ans;
}

//calculate?the?difference?（d）?between?angles?d=[-pipi）
double?angleDiff（double?a2?double?a1）
{
??a1=angle（a1）;
??a2=angle（a2）;
??double?ans=angle（a2-a1）;
??if（ans>PI）?ans=ans-PIx2;
??return?ans;
}

//?static?double?euclDist（Particle?&a1?Particle?&a2）
//?{
//???return?sqrt（（a1.est.x-a2.est.x）*（a1.est.x-a2.est.x）+（a1.est.y-a2.est.y）*（a1.est.y-a2.est.y））;
//?}

double?euclDist（double?&x1?double?&y1?double?&x2?double?&y2）
{
??return?sqrt（（x1-x2）*（x1-x2）+（y1-y2）*（y1-y2））;
}

//a=sqrt（b^2+c^2-2*b*c*cos（A））
double?cosineRule（double?b?double?c?double?A）
{
??return?sqrt（b*b+c*c-2*b*c*cos（A））;
}

//======================General?functions==============================
double?sign（double?x）
{
??if（x>0.0）?return?1.0;
??return?-1.0;
}



int?minDblVec（vector?&seq?double?&minval）
{
??int?minidx=0;
??minval=seq[0];
??for（size_t?i=1;?i????if（seq[i]??????minval=seq[i];
??????minidx=i;
????}
??}
??return?minidx;
}


//======================Probability?functions==============================

//generate?random?normal?number
//modifed?from?float?gasdev（long?*idum）?in?
//1986-92?Numerical?Recipes?Software
double?randn48（void）
{
??float?ran1（long?*idum）;
??static?int?iset=0;
??static?float?gset;
??float?facrsqv1v2;
??
??if??（iset?==?0）?{
????do?{
??????v1=2.0*drand48（）-1.0;
??????v2=2.0*drand48（）-1.0;
??????rsq=v1*v1+v2*v2;
????}?while?（rsq?>=?1.0?||?rsq?==?0.0）;
????fac=sqrt（-2.0*log（rsq）/rsq）;
????gset=v1*fac;
????iset=1;
????return?v2*fac;
??}?else?{
????iset=0;
????return?gset;
??}
}

double?normalPDF（double?x?double?mu?double?sigma）
{?
??const?double?kNormal=0.39894228;?//1/sqrt（2*pi）
??double?z=（x-mu）/sigma;
??return?kNormal/sqrt（sigma）*exp（-0.5*z*z）;
}

double?stdBiNormalPDF（double?x1?double?x2）
{
?//?1/[2*pi]??const?for?independent?bivariate?distribution?
??const?double?kBiNormal=0.159154943;
??return?kBiNormal*exp（-0.5*（x1*x1+x2*x2））;
}

double?stdTriNormalPDF（double?x1?double?x2?double?x3）
{
??//?1/[2*sqrt（2）*pi^（3/2）]??const?for?independent?trivariate?distribution?
??const?double?kTriNormal=0.06349363592;?
??return?kTriNormal*exp（-0.5*（x1*x1+x2*x2+x3*x3））;
}

double?laplacePDF（double?x?double?m?double?b）
{
??return?0.5/b*exp（-fabs（x-m）/b）;
}

double?cauchyPDF（double?x?double?a?double?b）
{
??double?val=（x-a）/b;
??return?1.0/（b*PI*（1.0+val*val））;
}


double?randNormal（double?m?double?s）
{
??return?randn48（）*s+m;
}

double?randLaplace（double?m?double?b）
{
??double?D=drand48（）;
??

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????2897??2007-08-31?15:55??mpf_slam?final\args.h
?????文件????????5534??2007-12-20?13:44??mpf_slam?final\classes.h
?????文件????????5545??2005-02-03?22:14??mpf_slam?final\gdb_stl_utils
?????文件????????3917??2007-11-09?13:15??mpf_slam?final\gen_util.cpp
?????文件???????68949??2007-12-18?11:42??mpf_slam?final\gmon.out
?????文件?????????264??2008-03-04?12:49??mpf_slam?final\input.txt
?????文件???????????0??2008-03-04?13:19??mpf_slam?final\Localisation.txt
?????文件?????????706??2007-12-18?11:43??mpf_slam?final\Makefile
?????文件???????12984??2008-03-05?15:45??mpf_slam?final\mpfconfig.cpp
?????文件???????28596??2008-03-05?15:49??mpf_slam?final\mpf_cylfeature.cpp
?????文件????????7386??2007-11-08?12:40??mpf_slam?final\particle_util.cpp
?????文件?????1031036??2008-03-04?13:18??mpf_slam?final\playerjoyMPF
?????文件???????20226??2008-03-04?13:18??mpf_slam?final\playerjoyMPF.cc
?????文件???????20259??2008-03-04?12:50??mpf_slam?final\playerjoyMPF.cc~
?????文件????????5809??2008-03-05?15:50??mpf_slam?final\pmpf.cpp
?????文件?????????244??2008-03-05?15:48??mpf_slam?final\Readme.txt
?????文件????????1922??2005-02-03?22:14??mpf_slam?final\StlStdContainers.cc
?????文件????????2335??2007-10-18?11:41??mpf_slam?final\util.h
?????目錄???????????0??2008-07-29?17:01??mpf_slam?final\