coding.cpp
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <cstdint>
00003 #include <cmath>
00004 
00005 using std::printf;
00006 
00007 void encodePoint(const double x, const double y, uint32_t& code)
00008 {
00009         if (((x * 100) < (((double) INT16_MIN) + 1)) ||
00010                         ((x * 100) > (((double) INT16_MAX) - 1)) ||
00011                         ((y * 100) < (((double) INT16_MIN) + 1)) ||
00012                         ((y * 100) > (((double) INT16_MAX) - 1)))
00013         {
00014                 fprintf(stderr, "Point (%f, %f) cannot be encoded\n", x, y);
00015         }
00016         int16_t xint = (int16_t) std::lround(x * 100);
00017         int16_t yint = (int16_t) std::lround(y * 100);
00018         code = (xint << 16);
00019         code |= (yint & 0x0000FFFF);
00020 }
00021 
00022 void decodePoint(const uint32_t code, double& x, double& y)
00023 {
00024         int16_t xint = (int16_t) (code >> 16);
00025         int16_t yint = (int16_t) code;
00026         x = ((double) xint) / 100;
00027         y = ((double) yint) / 100;
00028 }
00029 
00030 int main(int argc, char** argv)
00031 {
00032         double xin = 2.546;
00033         double yin = 1.20;
00034         double xout;
00035         double yout;
00036         uint32_t code;
00037 
00038         encodePoint(xin, yin, code);
00039         decodePoint(code, xout, yout);
00040         printf("encoded x: %f, y: %f\n", xin, yin);
00041         printf("decoded x: %f, y: %f\n", xout, yout);
00042         printf("\n");
00043 
00044         xin = -0.02;
00045         encodePoint(xin, yin, code);
00046         decodePoint(code, xout, yout);
00047         printf("encoded x: %f, y: %f\n", xin, yin);
00048         printf("decoded x: %f, y: %f\n", xout, yout);
00049         printf("\n");
00050         
00051         yin = -57.893;
00052         encodePoint(xin, yin, code);
00053         decodePoint(code, xout, yout);
00054         printf("encoded x: %f, y: %f\n", xin, yin);
00055         printf("decoded x: %f, y: %f\n", xout, yout);
00056         printf("\n");
00057 
00058         xin = -1.984;
00059         encodePoint(xin, yin, code);
00060         decodePoint(code, xout, yout);
00061         printf("encoded x: %f, y: %f\n", xin, yin);
00062         printf("decoded x: %f, y: %f\n", xout, yout);
00063         printf("\n");
00064 
00065         xin = -700.984;
00066         encodePoint(xin, yin, code);
00067         decodePoint(code, xout, yout);
00068         printf("encoded x: %f, y: %f\n", xin, yin);
00069         printf("decoded x: %f, y: %f\n", xout, yout);
00070         printf("\n");
00071 
00072         xin = 0.184421;
00073         yin = 2.10794;
00074         encodePoint(xin, yin, code);
00075         decodePoint(code, xout, yout);
00076         printf("encoded x: %f, y: %f\n", xin, yin);
00077         printf("code: %d (%#010x)\n", code, code);
00078         printf("decoded x: %f, y: %f\n", xout, yout);
00079         printf("\n");
00080 
00081 }


nj_costmap
Author(s): Gaƫl Ecorchard
autogenerated on Sat Jun 8 2019 20:58:46