xsens_list.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

/*! \file
    \brief Implementations of Cmtmath class functions.

    For information about objects in this file, see the appropriate header:
    \ref Cmtmath.h

    \section FileCopyright Copyright Notice
    Copyright (C) Xsens Technologies B.V., 2006.  All rights reserved.

    This source code is intended for use only by Xsens Technologies BV and
    those that have explicit written permission to use it from
    Xsens Technologies BV.

    THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
    KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
    IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
    PARTICULAR PURPOSE.

    \section FileSwitches Switches
    \li \c _XSENS_MATH_RANGE_CHECKS Check whether indices fall in the valid
            range(s) of the object when set to 1 (in CmtMath.h).

    \section FileChangelog  Changelog
    \par 2006-05-31, v0.0.1
    \li Job Mulder: Created

    \par 2006-06-08, v0.0.2
    \li Job Mulder: Moved implementation of List to Cmtlist.hpp
    \li Job Mulder: Removed Matlab class and integrated save functions in math
   classes

    \par 2006-07-21, v0.1.0
    \li Job Mulder: Updated file for release 0.1.0
*/

#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif

#include <string.h>
#include <ctype.h>
#include <stdio.h>

#include "xsens_list.hpp"

namespace xsens
{
int32_t IntList::deserialize(const char* str)
{
    uint32_t tmp = *((const uint32_t*)str);
    resize(tmp);

    memcpy(m_data, str + 4, tmp * 4);
    m_count = tmp;
    return (int32_t)(4 + tmp * 4);
}

int32_t IntList::serialize(char* buffer) const
{
    if (buffer != nullptr)
    {
        *((uint32_t*)buffer) = m_count;
        memcpy(buffer + 4, m_data, m_count * 4);
    }
    return (int32_t)(4 + m_count * 4);
}

void IntList::setIncremental(
    const uint32_t start, const uint32_t end, const int32_t step)
{
    if (step == 0) return;
    int32_t size;
    size = 1 + (((int32_t)end - (int32_t)start) / step);

    if ((uint32_t)size > m_max) resize(size);

    uint32_t astep = (uint32_t)step;
    m_count = 0;
    if (step > 0)
        for (uint32_t i = start; i < end; i += astep) m_data[m_count++] = i;
    else
        for (uint32_t i = start; i > end; i += astep) m_data[m_count++] = i;
}

int32_t IntList::readFromString(const char* str)
{
    long int rd = 0;
    long unsigned sz = 0;
    const char* start = str;

    if (sscanf(str, "%lu:%ln", &sz, &rd) != 1) return 0;
    str += rd;
    resize(sz);
    m_count = sz;

    for (uint32_t i = 0; i < m_count; ++i)
    {
        long int auxInt;
        if (sscanf(str, "%li%ln", &auxInt, &rd) != 1) return 0;
        m_data[i] = auxInt;
        str += rd;
    }
    return (int32_t)(str - start);
}

int32_t IntList::writeToString(char* buffer) const
{
    char* buf = buffer;
    char fake[128];
    uint32_t written = 0;
    if (buffer == nullptr) buf = fake;
    written = sprintf(buf, "%lu:", (long unsigned)m_count);
    if (buf != fake) buf += written;
    for (uint32_t i = 0; i < m_count; ++i)
    {
        written += sprintf(buf, " %lu", static_cast<long unsigned>(m_data[i]));
        if (buf != fake) buf = buffer + written;
    }
    return (int32_t)written;
}

int32_t IntList::writeToStringHex(char* buffer) const
{
    char* buf = buffer;
    char fake[128];
    uint32_t written = 0;
    if (buffer == nullptr) buf = fake;
    written = sprintf(buf, "%lu:", static_cast<long unsigned>(m_count));
    if (buf != fake) buf += written;
    for (uint32_t i = 0; i < m_count; ++i)
    {
        written +=
            sprintf(buf, " 0x%lX", static_cast<long unsigned>(m_data[i]));
        if (buf != fake) buf = buffer + written;
    }
    return (int32_t)written;
}

void IntList::addValue(int32_t value)
{
    uint32_t add = *((uint32_t*)&value);
    for (uint32_t i = 0; i < m_count; ++i) m_data[i] += add;
}

bool IntList::operator==(const IntList& lst)
{
    if (m_count != lst.m_count) return false;
    for (uint32_t i = 0; i < m_count; ++i)
        if (m_data[i] != lst.m_data[i]) return false;
    return true;
}

}  // end of xsens namespace