00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00035 #include <string.h>
00036 #include "lcmtypes_pose_t.h"
00037
00038 static int __lcmtypes_pose_t_hash_computed;
00039 static int64_t __lcmtypes_pose_t_hash;
00040
00041 int64_t __lcmtypes_pose_t_hash_recursive(const __lcm_hash_ptr *p)
00042 {
00043 const __lcm_hash_ptr *fp;
00044 for (fp = p; fp != NULL; fp = fp->parent)
00045 if (fp->v == __lcmtypes_pose_t_get_hash)
00046 return 0;
00047
00048 const __lcm_hash_ptr cp = { p, (void*)__lcmtypes_pose_t_get_hash };
00049 (void) cp;
00050
00051 int64_t hash = 0x170b77d82958082fLL
00052 + __int64_t_hash_recursive(&cp)
00053 + __double_hash_recursive(&cp)
00054 + __double_hash_recursive(&cp)
00055 + __double_hash_recursive(&cp)
00056 + __double_hash_recursive(&cp)
00057 + __double_hash_recursive(&cp)
00058 ;
00059
00060 return (hash<<1) + ((hash>>63)&1);
00061 }
00062
00063 int64_t __lcmtypes_pose_t_get_hash(void)
00064 {
00065 if (!__lcmtypes_pose_t_hash_computed) {
00066 __lcmtypes_pose_t_hash = __lcmtypes_pose_t_hash_recursive(NULL);
00067 __lcmtypes_pose_t_hash_computed = 1;
00068 }
00069
00070 return __lcmtypes_pose_t_hash;
00071 }
00072
00073 int __lcmtypes_pose_t_encode_array(void *buf, int offset, int maxlen, const lcmtypes_pose_t *p, int elements)
00074 {
00075 int pos = 0, thislen, element;
00076
00077 for (element = 0; element < elements; element++) {
00078
00079 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].utime), 1);
00080 if (thislen < 0) return thislen; else pos += thislen;
00081
00082 thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].pos, 3);
00083 if (thislen < 0) return thislen; else pos += thislen;
00084
00085 thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].vel, 3);
00086 if (thislen < 0) return thislen; else pos += thislen;
00087
00088 thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].orientation, 4);
00089 if (thislen < 0) return thislen; else pos += thislen;
00090
00091 thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].rotation_rate, 3);
00092 if (thislen < 0) return thislen; else pos += thislen;
00093
00094 thislen = __double_encode_array(buf, offset + pos, maxlen - pos, p[element].accel, 3);
00095 if (thislen < 0) return thislen; else pos += thislen;
00096
00097 }
00098 return pos;
00099 }
00100
00101 int lcmtypes_pose_t_encode(void *buf, int offset, int maxlen, const lcmtypes_pose_t *p)
00102 {
00103 int pos = 0, thislen;
00104 int64_t hash = __lcmtypes_pose_t_get_hash();
00105
00106 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
00107 if (thislen < 0) return thislen; else pos += thislen;
00108
00109 thislen = __lcmtypes_pose_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
00110 if (thislen < 0) return thislen; else pos += thislen;
00111
00112 return pos;
00113 }
00114
00115 int __lcmtypes_pose_t_encoded_array_size(const lcmtypes_pose_t *p, int elements)
00116 {
00117 int size = 0, element;
00118 for (element = 0; element < elements; element++) {
00119
00120 size += __int64_t_encoded_array_size(&(p[element].utime), 1);
00121
00122 size += __double_encoded_array_size(p[element].pos, 3);
00123
00124 size += __double_encoded_array_size(p[element].vel, 3);
00125
00126 size += __double_encoded_array_size(p[element].orientation, 4);
00127
00128 size += __double_encoded_array_size(p[element].rotation_rate, 3);
00129
00130 size += __double_encoded_array_size(p[element].accel, 3);
00131
00132 }
00133 return size;
00134 }
00135
00136 int lcmtypes_pose_t_encoded_size(const lcmtypes_pose_t *p)
00137 {
00138 return 8 + __lcmtypes_pose_t_encoded_array_size(p, 1);
00139 }
00140
00141 int __lcmtypes_pose_t_decode_array(const void *buf, int offset, int maxlen, lcmtypes_pose_t *p, int elements)
00142 {
00143 int pos = 0, thislen, element;
00144
00145 for (element = 0; element < elements; element++) {
00146
00147 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].utime), 1);
00148 if (thislen < 0) return thislen; else pos += thislen;
00149
00150 thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].pos, 3);
00151 if (thislen < 0) return thislen; else pos += thislen;
00152
00153 thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].vel, 3);
00154 if (thislen < 0) return thislen; else pos += thislen;
00155
00156 thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].orientation, 4);
00157 if (thislen < 0) return thislen; else pos += thislen;
00158
00159 thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].rotation_rate, 3);
00160 if (thislen < 0) return thislen; else pos += thislen;
00161
00162 thislen = __double_decode_array(buf, offset + pos, maxlen - pos, p[element].accel, 3);
00163 if (thislen < 0) return thislen; else pos += thislen;
00164
00165 }
00166 return pos;
00167 }
00168
00169 int __lcmtypes_pose_t_decode_array_cleanup(lcmtypes_pose_t *p, int elements)
00170 {
00171 int element;
00172 for (element = 0; element < elements; element++) {
00173
00174 __int64_t_decode_array_cleanup(&(p[element].utime), 1);
00175
00176 __double_decode_array_cleanup(p[element].pos, 3);
00177
00178 __double_decode_array_cleanup(p[element].vel, 3);
00179
00180 __double_decode_array_cleanup(p[element].orientation, 4);
00181
00182 __double_decode_array_cleanup(p[element].rotation_rate, 3);
00183
00184 __double_decode_array_cleanup(p[element].accel, 3);
00185
00186 }
00187 return 0;
00188 }
00189
00190 int lcmtypes_pose_t_decode(const void *buf, int offset, int maxlen, lcmtypes_pose_t *p)
00191 {
00192 int pos = 0, thislen;
00193 int64_t hash = __lcmtypes_pose_t_get_hash();
00194
00195 int64_t this_hash;
00196 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
00197 if (thislen < 0) return thislen; else pos += thislen;
00198 if (this_hash != hash) return -1;
00199
00200 thislen = __lcmtypes_pose_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
00201 if (thislen < 0) return thislen; else pos += thislen;
00202
00203 return pos;
00204 }
00205
00206 int lcmtypes_pose_t_decode_cleanup(lcmtypes_pose_t *p)
00207 {
00208 return __lcmtypes_pose_t_decode_array_cleanup(p, 1);
00209 }
00210
00211 int __lcmtypes_pose_t_clone_array(const lcmtypes_pose_t *p, lcmtypes_pose_t *q, int elements)
00212 {
00213 int element;
00214 for (element = 0; element < elements; element++) {
00215
00216 __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1);
00217
00218 __double_clone_array(p[element].pos, q[element].pos, 3);
00219
00220 __double_clone_array(p[element].vel, q[element].vel, 3);
00221
00222 __double_clone_array(p[element].orientation, q[element].orientation, 4);
00223
00224 __double_clone_array(p[element].rotation_rate, q[element].rotation_rate, 3);
00225
00226 __double_clone_array(p[element].accel, q[element].accel, 3);
00227
00228 }
00229 return 0;
00230 }
00231
00232 lcmtypes_pose_t *lcmtypes_pose_t_copy(const lcmtypes_pose_t *p)
00233 {
00234 lcmtypes_pose_t *q = (lcmtypes_pose_t*) malloc(sizeof(lcmtypes_pose_t));
00235 __lcmtypes_pose_t_clone_array(p, q, 1);
00236 return q;
00237 }
00238
00239 void lcmtypes_pose_t_destroy(lcmtypes_pose_t *p)
00240 {
00241 __lcmtypes_pose_t_decode_array_cleanup(p, 1);
00242 free(p);
00243 }