lcmtypes_pose_t.cpp

Go to the documentation of this file.

/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted
 provided that the following conditions are met:

  *Redistributions of source code must retain the above copyright notice, this list of
   conditions and the following disclaimer.
  *Redistributions in binary form must reproduce the above copyright notice, this list of
   conditions and the following disclaimer in the documentation and/or other materials provided
   with the distribution.
  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
   endorse or promote products derived from this software without specific prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************************/
#include <string.h>
#include "lcmtypes_pose_t.h"

static int __lcmtypes_pose_t_hash_computed;
static int64_t __lcmtypes_pose_t_hash;
 
int64_t __lcmtypes_pose_t_hash_recursive(const __lcm_hash_ptr *p)
{
    const __lcm_hash_ptr *fp;
    for (fp = p; fp != NULL; fp = fp->parent)
        if (fp->v == __lcmtypes_pose_t_get_hash)
            return 0;
 
    const __lcm_hash_ptr cp = { p, (void*)__lcmtypes_pose_t_get_hash };
    (void) cp;
 
    int64_t hash = 0x170b77d82958082fLL
         + __int64_t_hash_recursive(&cp)
         + __double_hash_recursive(&cp)
         + __double_hash_recursive(&cp)
         + __double_hash_recursive(&cp)
         + __double_hash_recursive(&cp)
         + __double_hash_recursive(&cp)
        ;
 
    return (hash<<1) + ((hash>>63)&1);
}
 
int64_t __lcmtypes_pose_t_get_hash(void)
{
    if (!__lcmtypes_pose_t_hash_computed) {
        __lcmtypes_pose_t_hash = __lcmtypes_pose_t_hash_recursive(NULL);
        __lcmtypes_pose_t_hash_computed = 1;
    }
 
    return __lcmtypes_pose_t_hash;
}
 
int __lcmtypes_pose_t_encode_array(void *buf, int offset, int maxlen, const lcmtypes_pose_t *p, int elements)
{
    int pos = 0, thislen, element;
 
    for (element = 0; element < elements; element++) {
 
        thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].utime), 1);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].pos, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].vel, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].orientation, 4);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].rotation_rate, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].accel, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
    }
    return pos;
}
 
int lcmtypes_pose_t_encode(void *buf, int offset, int maxlen, const lcmtypes_pose_t *p)
{
    int pos = 0, thislen;
    int64_t hash = __lcmtypes_pose_t_get_hash();
 
    thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
    if (thislen < 0) return thislen; else pos += thislen;
 
    thislen = __lcmtypes_pose_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
    if (thislen < 0) return thislen; else pos += thislen;
 
    return pos;
}
 
int __lcmtypes_pose_t_encoded_array_size(const lcmtypes_pose_t *p, int elements)
{
    int size = 0, element;
    for (element = 0; element < elements; element++) {
 
        size += __int64_t_encoded_array_size(&(p[element].utime), 1);
 
        size += __double_encoded_array_size(p[element].pos, 3);
 
        size += __double_encoded_array_size(p[element].vel, 3);
 
        size += __double_encoded_array_size(p[element].orientation, 4);
 
        size += __double_encoded_array_size(p[element].rotation_rate, 3);
 
        size += __double_encoded_array_size(p[element].accel, 3);
 
    }
    return size;
}
 
int lcmtypes_pose_t_encoded_size(const lcmtypes_pose_t *p)
{
    return 8 + __lcmtypes_pose_t_encoded_array_size(p, 1);
}
 
int __lcmtypes_pose_t_decode_array(const void *buf, int offset, int maxlen, lcmtypes_pose_t *p, int elements)
{
    int pos = 0, thislen, element;
 
    for (element = 0; element < elements; element++) {
 
        thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].utime), 1);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].pos, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].vel, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].orientation, 4);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].rotation_rate, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
        thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].accel, 3);
        if (thislen < 0) return thislen; else pos += thislen;
 
    }
    return pos;
}
 
int __lcmtypes_pose_t_decode_array_cleanup(lcmtypes_pose_t *p, int elements)
{
    int element;
    for (element = 0; element < elements; element++) {
 
        __int64_t_decode_array_cleanup(&(p[element].utime), 1);
 
        __double_decode_array_cleanup(p[element].pos, 3);
 
        __double_decode_array_cleanup(p[element].vel, 3);
 
        __double_decode_array_cleanup(p[element].orientation, 4);
 
        __double_decode_array_cleanup(p[element].rotation_rate, 3);
 
        __double_decode_array_cleanup(p[element].accel, 3);
 
    }
    return 0;
}
 
int lcmtypes_pose_t_decode(const void *buf, int offset, int maxlen, lcmtypes_pose_t *p)
{
    int pos = 0, thislen;
    int64_t hash = __lcmtypes_pose_t_get_hash();
 
    int64_t this_hash;
    thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
    if (thislen < 0) return thislen; else pos += thislen;
    if (this_hash != hash) return -1;
 
    thislen = __lcmtypes_pose_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
    if (thislen < 0) return thislen; else pos += thislen;
 
    return pos;
}
 
int lcmtypes_pose_t_decode_cleanup(lcmtypes_pose_t *p)
{
    return __lcmtypes_pose_t_decode_array_cleanup(p, 1);
}
 
int __lcmtypes_pose_t_clone_array(const lcmtypes_pose_t *p, lcmtypes_pose_t *q, int elements)
{
    int element;
    for (element = 0; element < elements; element++) {
 
        __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1);
 
        __double_clone_array(p[element].pos, q[element].pos, 3);
 
        __double_clone_array(p[element].vel, q[element].vel, 3);
 
        __double_clone_array(p[element].orientation, q[element].orientation, 4);
 
        __double_clone_array(p[element].rotation_rate, q[element].rotation_rate, 3);
 
        __double_clone_array(p[element].accel, q[element].accel, 3);
 
    }
    return 0;
}
 
lcmtypes_pose_t *lcmtypes_pose_t_copy(const lcmtypes_pose_t *p)
{
    lcmtypes_pose_t *q = (lcmtypes_pose_t*) malloc(sizeof(lcmtypes_pose_t));
    __lcmtypes_pose_t_clone_array(p, q, 1);
    return q;
}
 
void lcmtypes_pose_t_destroy(lcmtypes_pose_t *p)
{
    __lcmtypes_pose_t_decode_array_cleanup(p, 1);
    free(p);
}