00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <utils/math/angle.h>
00025 #include <cstdio>
00026
00027 using namespace fawkes;
00028
00029 int
00030 main(int argc, char **argv)
00031 {
00032 float f = -2 * M_PI;
00033 float fnm = normalize_mirror_rad(f);
00034 float expd = 0;
00035 printf("f=%f normalize_mirror_rad(f)=%f expected=%f\n", f, fnm, expd);
00036
00037 f = 2 * M_PI;
00038 fnm = normalize_mirror_rad(f);
00039 expd = 0;
00040 printf("f=%f normalize_mirror_rad(f)=%f expected=%f\n", f, fnm, expd);
00041
00042
00043 f = 2 * M_PI + 1;
00044 fnm = normalize_mirror_rad(f);
00045 expd = 1;
00046 printf("f=%f normalize_mirror_rad(f)=%f expected=%f\n", f, fnm, expd);
00047
00048
00049 f = - 2 * M_PI - 1.4;
00050 fnm = normalize_mirror_rad(f);
00051 expd = -1.4;
00052 printf("f=%f normalize_mirror_rad(f)=%f expected=%f\n", f, fnm, expd);
00053
00054
00055 f = - 2 * M_PI - 2.9;
00056 fnm = normalize_mirror_rad(f);
00057 expd = -2.9;
00058 printf("f=%f normalize_mirror_rad(f)=%f expected=%f\n", f, fnm, expd);
00059
00060
00061 f = - 3 * M_PI - 1;
00062 fnm = normalize_mirror_rad(f);
00063 expd = f + 4 * M_PI;
00064 printf("f=%f normalize_mirror_rad(f)=%f expected=%f\n", f, fnm, expd);
00065
00066
00067 f = -M_PI;
00068 float fnr = normalize_rad(f);
00069 expd = M_PI;
00070 printf("f=%f normalize_rad(f)=%f expected=%f\n", f, fnr, expd);
00071
00072 f = 3 * M_PI;
00073 fnr = normalize_rad(f);
00074 expd = M_PI;
00075 printf("f=%f normalize_rad(f)=%f expected=%f\n", f, fnr, expd);
00076
00077 f = - 3 * M_PI;
00078 fnr = normalize_rad(f);
00079 expd = M_PI;
00080 printf("f=%f normalize_rad(f)=%f expected=%f\n", f, fnr, expd);
00081
00082 f = - 2 * M_PI - 1;
00083 fnr = normalize_rad(f);
00084 expd = 2 * M_PI - 1;
00085 printf("f=%f normalize_rad(f)=%f expected=%f\n", f, fnr, expd);
00086
00087 f = 10 * M_PI;
00088 fnr = normalize_rad(f);
00089 expd = 0;
00090 printf("f=%f normalize_rad(f)=%f expected=%f\n", f, fnr, expd);
00091
00092 return 0;
00093 }