source: orxonox.OLD/height_map_merge/src/lib/network/converter.cc @ 6501

/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

### File Specific:
   main-programmer: Benjamin Wuest
   co-programmer: ...
*/


/* this is for debug output. It just says, that all calls to PRINT() belong to the DEBUG_MODULE_NETWORK module
   For more information refere to https://www.orxonox.net/cgi-bin/trac.cgi/wiki/DebugOutput
*/
#define DEBUG_MODULE_NETWORK

/* include your own header */
#include "converter.h"


/* using namespace std is default, this needs to be here */
using namespace std;

/*!
 * Standard constructor
 */
Converter::Converter()
{
  /* set the class id for the base object */
  //this->setClassID(CL_ENTITY_MANAGER, "EntityManager");
}

/*!
 * Standard destructor
 */
Converter::~Converter()
{
}

const int sgnadd = 128; // = 2^7

/*!
 * Converts an int into a byte-array
 * @remarks: The int is stored in big-endian
 * @param x: The int which is to convert
 * @return: A byte-array that accords the given int value
 */
byte* Converter::intToByteArray(int x)
{
  const int mod = 256; // = 2^8


  byte* result = new byte[INTSIZE];
  int sgn;
  if (x >= 0)
    sgn = 1;
  else
  {
    sgn = -1;
    x = -x;
  }

  for (int i = 0; i < INTSIZE; i++)
  {
    result[i] = x % mod;
    x /= mod;
  }

  if (sgn == -1)
    result[INTSIZE - 1] += sgnadd;


  return result;
}

/*!
 * Converts an int into a byte-array and stores the result into a given byte-array
 * @remarks: The int is stored in big-endian
 * @param x: The int which is to convert
 * @return: A byte-array that accords the given int value
 */
int Converter::intToByteArray(int x, byte* a, int length)
{
  if (length < INTSIZE)
  {
    PRINTF(1)("byte buffer to short to store an int. Needed length %i. Avaiable length %i", INTSIZE, length);
    return -1;
  }

  const int mod = 256; // = 2^8

  int sgn;
  if (x >= 0)
    sgn = 1;
  else
  {
    sgn = -1;
    x = -x;
  }

  for (int i = 0; i < INTSIZE; i++)
  {
    a[i] = x % mod;
    x /= mod;
  }

  if (sgn == -1)
    a[INTSIZE - 1] += sgnadd;

  return INTSIZE;
}

/*!
 * Converts a byte-array into an int
 * @param a: The byte-array which is to convert
 * @return: An int that accords the given byte-array
 */
int Converter::byteArrayToInt(const byte* a, int* x)
{
  int mult = 1;
  const int step = 256; // = 2 ^ 8
  *x = 0;
  for (int i = 0; i < INTSIZE - 1; i++)
  {
    *x += a[i] * mult;
    mult *= step;
  }

  if (a[INTSIZE - 1] >= sgnadd)
  {
    *x += (a[INTSIZE - 1] - sgnadd) * mult;
    *x *= -1;
  }
  else
    *x += a[INTSIZE - 1] * mult;

  return INTSIZE;
}

/*!
 * Converts a float into a string containing its binary representation
 */
char* Converter::floatToBinString(float x)
{
  char* result = new char[200];
  int pos = 0;

  if (x < 0)
  {
    result[pos++] = '-';
    x = -x;
  }

  float sub = 1;
  while (sub < x)
    sub *= 2;

  while ((x > 0 || sub >= 1) && pos < 200)
  {
    if (x >= sub)
    {
      result[pos] = '1';
      x -= sub;
    }
    else
      result[pos] = '0';
    pos++;
    sub /= 2;

    if (sub == 0.5f)
      result[pos++] = '.';
  }

  return result;
}

const int expmult = 8388608; //2^23

/*!
 * Converts a float value into a byte-array
 * @param x: The float which is to convert
 * @return: A byte-array which accords the given float
 */
byte* Converter::floatToByteArray(float x)
{
  int mantisse = 0;
  int exponent = 128;

  int sgn;
  if (x < 0)
  {
    x = -x;
    sgn = -1;
  }
  else
    sgn = 1;

  float sub = 1;
  while (sub < x)
  {
    sub *= 2;
    exponent++;
  }

  while (x > 0)
  {
    if (x >= sub)
    {
      mantisse += 1;
      x -= sub;
    }

    mantisse *= 2;
    exponent--;
    sub /= 2;
  }
  exponent++;
  mantisse /= 2;
  while (mantisse < expmult)
  {
    mantisse *= 2;
    exponent--;
  }

  //printf("mantisse = %i exponent = %i \n", mantisse, exponent);

  mantisse -= expmult;

  int hx = mantisse + expmult * exponent;
  byte* result = intToByteArray(hx);
  if (sgn == -1)
    result[3] += sgnadd;

  return result;
}


/*!
 * Converts a byte-array into a float value
 * @param a: The byte-array which is to convert
 * @return: A float value which accords the given byte-array
 */
float Converter::byteArrayToFloat(byte* a)
{
  int hmant = a[0] + a[1] * 256 + a[2] * 65536;
  int mantisse = hmant % expmult;
  mantisse += expmult;

  int hexp = a[2] + a[3] * 256;
  int exponent = (hexp / 128) % 256 - 128;

  int sgn;
  if (a[3] >= sgnadd)
    sgn = -1;
  else
    sgn = 1;

  //printf("mantisse = %i exponent = %i \n", mantisse, exponent);

  float emult = 1;
  if (exponent > 0)
    for (int i = 0; i < exponent; i++)
      emult *= 2;
  else if (exponent < 0)
    for (int i = 0; i > exponent; i--)
      emult /= 2;

  float result = mantisse * emult;
  if (sgn == -1)
    result = -result;

  return result;
}

/*!
 * Converts a float value into a byte-array
 * @param x: The float which is to convert
 * @return: A byte-array which accords the given float
 */
byte* Converter::_floatToByteArray(float x)
{
  byte* p = (byte*)&x;
  byte* result = new byte[4];
  for (int i = 0; i < 4; i++)
    result[i] = p[i];
  return result;
}


/*!
 * Converts a byte-array into a float value
 * @param a: The byte-array which is to convert
 * @return: A float value which accords the given byte-array
 */
float Converter::_byteArrayToFloat(byte* a)
{
  float* p = (float*)a;
  float result = *p;
  return result;
}

/*!
 * Converts a float value into a byte-array and stores the result into a given byte-array
 * @param x: The float which is to convert
 * @return: A byte-array which accords the given float
 */
int Converter::floatToByteArray(float x, byte* a, int length)
{
  if (length < FLOATSIZE)
  {
    PRINTF(1)("byte buffer to short to store a float. Needed length %i. Avaiable length %i", FLOATSIZE, length);
    return -1;
  }

  //handle 0 else function will loop for ever
  if ( x == 0 )
  {
    for ( int i = 0; i<FLOATSIZE; i++)
      a[i] = 0;
    return FLOATSIZE;
  }

  int mantisse = 0;
  int exponent = 128;

  int sgn;
  if (x < 0)
  {
    x = -x;
    sgn = -1;
  }
  else
    sgn = 1;

  float sub = 1;
  while (sub < x)
  {
    sub *= 2;
    exponent++;
  }

  while (x > 0)
  {
    if (x >= sub)
    {
      mantisse += 1;
      x -= sub;
    }

    mantisse *= 2;
    exponent--;
    sub /= 2;
  }
  exponent++;
  mantisse /= 2;
  while (mantisse < expmult)
  {
    mantisse *= 2;
    exponent--;
  }

  //printf("mantisse = %i exponent = %i \n", mantisse, exponent);

  mantisse -= expmult;

  int hx = mantisse + expmult * exponent;
  int result = intToByteArray(hx, a, length);
  if (sgn == -1)
    a[3] += sgnadd;

  return result;
}


/*!
 * Converts a byte-array into a float value
 * @param a: The byte-array which is to convert
 * @return: A float value which accords the given byte-array
 */
int Converter::byteArrayToFloat(const byte* a, float* x)
{
  //handle 0
  for (int i = 0; i<FLOATSIZE; i++)
  {
    if (a[i]!=0)
      break;
    if ( i==FLOATSIZE-1 )
    {
      *x = 0.0f;
      return FLOATSIZE;
    }
  }


  int hmant = a[0] + a[1] * 256 + a[2] * 65536;
  int mantisse = hmant % expmult;
  mantisse += expmult;

  int hexp = a[2] + a[3] * 256;
  int exponent = (hexp / 128) % 256 - 128;

  int sgn;
  if (a[3] >= sgnadd)
    sgn = -1;
  else
    sgn = 1;

  //printf("mantisse = %i exponent = %i \n", mantisse, exponent);

  float emult = 1;
  if (exponent > 0)
    for (int i = 0; i < exponent; i++)
      emult *= 2;
  else if (exponent < 0)
    for (int i = 0; i > exponent; i--)
      emult /= 2;

  *x = mantisse * emult;
  if (sgn == -1)
    *x = -  *x;

  return FLOATSIZE;
}

/**
 * copies a strint to a byte array
 * @param s: string to copy
 * @param a: byte array
 * @param length: string length
 * @return: the used number of bytes in byte array
 */
int Converter::stringToByteArray( const char * s, byte * a, int length, int maxLength )
{
  if ( length+INTSIZE > maxLength )
  {
    PRINTF(1)("Byte array is too small (%d) to store %d bytes\n", maxLength, length+INTSIZE);
    return -1;
  }

  int n = Converter::intToByteArray( length, a, maxLength );

  memcpy( a+INTSIZE, s, length );

  return length + INTSIZE;
}

/**
 * reads a string out of a byte array
 * @param a: byte array
 * @param s: string
 * @param maxLength: max bytes to copy
 * @return: the number of read bytes in byte array
 */
int Converter::byteArrayToString( const byte * a, char * s, int maxLength )
{
  int length;

  int n = Converter::byteArrayToInt( a, &length );


  if ( length+1 > maxLength )
  {
    PRINTF(1)("There is not enough space in string (%d) to store %d bytes\n", maxLength, length+1 );
    strncpy(s,"",maxLength);
    return -1;
  }

  memcpy( s, a+n, length );
  s[length] = '\0';

  return n+length;
}

/**
 * reads a string out of a byte array and allocates memory for string
 * @param a: byte array
 * @param s: string
 * @param maxLength: max bytes to copy
 * @return: the number of read bytes in byte array
 */
int Converter::byteArrayToStringM( const byte * a, char*& s )
{
  int length;

  int n = Converter::byteArrayToInt( a, &length );

  s = new char[length+1];

  if ( !s )
  {
    PRINTF(1)("Could not allocate memory!\n" );
    return -1;
  }

  memcpy( s, a+n, length );
  s[length] = '\0';

  return n+length;
}