60#ifndef WHITEBOARD_POSTER_STRING_CONVERSION
61#define WHITEBOARD_POSTER_STRING_CONVERSION
72#pragma clang diagnostic push
73#pragma clang diagnostic ignored "-Wunused-macros"
77#elif defined(__APPLE__)
78# include <machine/endian.h>
79# include <architecture/byte_order.h>
80# define bswap_16(x) NXSwapShort(x)
81# define bswap_32(x) NXSwapInt(x)
82# define bswap_64(x) NXSwapLongLong(x)
84# define bswap_16(x) __builtin_bswap16(x)
85# define bswap_32(x) __builtin_bswap32(x)
86# define bswap_64(x) __builtin_bswap64(x)
91#if (!defined(__BYTE_ORDER) && !defined(__LITTLE_ENDIAN)) || (defined(__BYTE_ORDER) && defined(__LITTLE_ENDIAN) && __BYTE_ORDER == __LITTLE_ENDIAN)
92# if !defined(htonll) && !defined(ntohll)
93# define htonll(x) bswap_64(x)
94# define ntohll(x) bswap_64(x)
96# if !defined(htonl) && !defined(ntohl)
97# define htonl(x) bswap_32(x)
98# define ntohl(x) bswap_32(x)
100# if !defined(htons) && !defined(ntohs)
101# define htons(x) bswap_16(x)
102# define ntohs(x) bswap_16(x)
105# if !defined(htonll) && !defined(ntohll)
106# define htonll(x) (x)
107# define ntohll(x) (x)
109# if !defined(htonl) && !defined(ntohl)
113# if !defined(htons) && !defined(ntohs)
118#pragma clang diagnostic pop
120#pragma clang diagnostic push
121#pragma clang diagnostic ignored "-Wunused-function"
122#pragma clang diagnostic ignored "-Wc++-compat"
123#pragma clang diagnostic ignored "-Wdouble-promotion"
124#pragma clang diagnostic ignored "-Wunused-parameter"
151 int image_width = 640;
152 int image_height = 480;
167 double imageAngleY = ((double)(image_height-y)/image_height)*
CAMERA_VFOV;
169 double angleY = cameraBottomVFOV + imageAngleY;
171 double frontDistance = camHeight*tan(angleY);
173 int im_width_2 = (image_width+1)/2;
174 double imageAngleX = ((double)((x-im_width_2))/im_width_2)*
CAMERA_HFOV/2;
176 *distance = frontDistance/cos(imageAngleX);
177 if (angleY > M_PI_2) {
178 *distance = INFINITY;
181 *angle = -(imageAngleX-joints->
HeadYaw);
185 int image_width = 640;
186 int image_height = 480;
193 int im_width_2 = (image_width+1)/2;
194 double imageAngleX = angle +joints->
HeadYaw;
196 *sx = (int)((imageAngleX/(
CAMERA_HFOV/2))*im_width_2)+im_width_2;
198 double frontDistance = distance*cos(imageAngleX);
200 double angleY = atan(frontDistance/camHeight);
202 double imageAngleY = angleY - cameraBottomVFOV;
204 *sy = (int)(image_height-(imageAngleY*image_height/
CAMERA_VFOV));
218#pragma clang diagnostic pop
227#pragma clang diagnostic push
228#pragma clang diagnostic ignored "-Wunused-variable"
230 if (len >= bufferSize) {
233 len += snprintf(descString + len, bufferSize - len,
"i=%d", self->i);
235#pragma clang diagnostic pop
243#pragma clang diagnostic push
244#pragma clang diagnostic ignored "-Wunused-variable"
246 if (len >= bufferSize) {
249 len += snprintf(toString + len, bufferSize - len,
"%d", self->i);
251#pragma clang diagnostic pop
259 size_t temp_length = strlen(str);
260 int length = (temp_length <= INT_MAX) ? ((
int)((ssize_t)temp_length)) : -1;
265 char* var_str = &var_str_buffer[0];
267 char* key = &key_buffer[0];
274 if (index == 0 && str[0] ==
'{') {
280 for (
int i = index;
i < length;
i++) {
282 if (bracecount == 0 && str[
i] ==
'=') {
287 if (bracecount == 0 && isspace(str[
i])) {
294 if (bracecount == 0 && str[
i] ==
',') {
304 if (bracecount < 0) {
309 if (
i == length - 1) {
313 if (endKey >= startKey && endKey - startKey < length) {
314 strncpy(key, str + startKey, ((
size_t)(endKey - startKey) + 1));
315 key[(endKey - startKey) + 1] = 0;
319 strncpy(var_str, str + startVar, ((
size_t)(index - startVar) + 1));
320 var_str[(index - startVar) + 1] = 0;
326 if (strlen(key) > 0) {
327 if (0 == strcmp(
"i", key)) {
337 self->i = ((int)atoi(var_str));
344 }
while(index < length);
355 uint16_t bit_offset = 0;
356 int i_nbo =
htonl(self->i);
359 for (b = (32 - 1); b >= 0; b--) {
361 uint16_t
byte = bit_offset / 8;
362 uint16_t bit = 7 - (bit_offset % 8);
363 unsigned long newbit = !!((i_nbo >> b) & 1U);
364 dst[byte] ^= (-newbit ^ dst[byte]) & (1UL << bit);
365 bit_offset = bit_offset + 1;
380 uint16_t bit_offset = 0;
383 for (b = (32 - 1); b >= 0; b--) {
385 uint16_t
byte = bit_offset / 8;
386 uint16_t bit = 7 - (bit_offset % 8);
387 char dataByte = src[byte];
388 unsigned char bitValue = (dataByte >> bit) & 1U;
389 dst->
i ^= (-bitValue ^ dst->
i) & (1UL << b);
390 bit_offset = bit_offset + 1;
WHITEBOARD_POSTER_STRING_CONVERSION.
WHITEBOARD_POSTER_STRING_CONVERSION.
float RKneePitch
pitch of the knee joint (right)
float LKneePitch
pitch of the knee joint (left)
WHITEBOARD_POSTER_STRING_CONVERSION.
float HeadYaw
Head pos in RAD.
float HeadPitch
Head pos in RAD.
WHITEBOARD_POSTER_STRING_CONVERSION.
const char * wb_pixel_to_robot_relative_coord_to_string(const struct wb_pixel_to_robot_relative_coord *self, char *toString, size_t bufferSize)
Convert to a string.
void pixel_to_rr_coord_no_conv(int32_t sx, int32_t sy, enum VisionCamera camera, struct wb_vision_control_status *vs, struct wb_sensors_torsojointsensors *joints, double *distance, double *angle)
size_t wb_pixel_to_robot_relative_coord_from_network_serialised(const char *src, struct wb_pixel_to_robot_relative_coord *dst)
Convert from a compressed, serialised, network byte order byte stream.
const char * wb_pixel_to_robot_relative_coord_description(const struct wb_pixel_to_robot_relative_coord *self, char *descString, size_t bufferSize)
Convert to a description string.
void rr_to_pixel_coord(double angle, double distance, enum VisionCamera camera, struct wb_vision_control_status *vs, struct wb_sensors_torsojointsensors *joints, int32_t *sx, int32_t *sy, bool kneeling)
void pixel_to_rr_coord_kneeling(int32_t sx, int32_t sy, enum VisionCamera camera, struct wb_vision_control_status *vs, struct wb_sensors_torsojointsensors *joints, double *distance, double *angle, bool kneeling)
double get_camera_height(enum VisionCamera camera, struct wb_sensors_torsojointsensors *joints, bool kneeling)
void pixel_to_rr_coord(int32_t sx, int32_t sy, enum VisionCamera camera, struct wb_vision_control_status *vs, struct wb_sensors_torsojointsensors *joints, double *distance, double *angle)
void pixel_to_rr_coord_kneeling_conv(int32_t sx, int32_t sy, enum VisionCamera camera, struct wb_vision_control_status *vs, struct wb_sensors_torsojointsensors *joints, double *distance, double *angle, bool kneeling, bool conv)
void pixel_to_rr_coord_legs_no_conv(int32_t sx, int32_t sy, enum VisionCamera camera, struct wb_vision_control_status *vs, struct wb_sensors_torsojointsensors *joints, struct wb_sensors_legjointsensors *leg_sensors, double *distance, double *angle)
struct wb_pixel_to_robot_relative_coord * wb_pixel_to_robot_relative_coord_from_string(struct wb_pixel_to_robot_relative_coord *self, const char *str)
Convert from a string.
void pixel_to_rr_coord_legs(int32_t sx, int32_t sy, enum VisionCamera camera, struct wb_vision_control_status *vs, struct wb_sensors_torsojointsensors *joints, struct wb_sensors_legjointsensors *leg_sensors, double *distance, double *angle)
size_t wb_pixel_to_robot_relative_coord_to_network_serialised(const struct wb_pixel_to_robot_relative_coord *self, char *dst)
Convert to a compressed, serialised, network byte order byte stream.
#define CONV_TO_IMG(x, r)
#define BOTTOM_CAMERA_ANGLE
#define BOTTOM_CAMERA_OFFSET_Z
#define TOP_CAMERA_OFFSET_X
#define BOTTOM_CAMERA_OFFSET_X
#define HEAD_BASE_HEIGHT_KNEELING
#define TOP_CAMERA_OFFSET_Z
#define PIXEL_TO_ROBOT_RELATIVE_COORD_DESC_BUFFER_SIZE
VisionCamera
Enum of available camera's that can be used by vision.