int wb_tree_probe(wb_tree *tree, void *key, void **dat) { int rv = 0; wb_node *node, *parent = NULL; float wbal; ASSERT(tree != NULL); node = tree->root; while (node) { rv = tree->key_cmp(key, node->key); if (rv < 0) parent = node, node = node->llink; else if (rv > 0) parent = node, node = node->rlink; else { *dat = node->dat; return 0; } } if ((node = node_new(key, *dat)) == NULL) return -1; if ((node->parent = parent) == NULL) { ASSERT(tree->count == 0); tree->root = node; tree->count = 1; return 0; } if (rv < 0) parent->llink = node; else parent->rlink = node; while ((node = parent) != NULL) { parent = node->parent; node->weight++; wbal = WEIGHT(node->llink) / (float)node->weight; if (wbal < ALPHA_0) { wbal = WEIGHT(node->rlink->llink) / (float)node->rlink->weight; if (wbal < ALPHA_3) { rot_left(tree, node); } else { rot_right(tree, node->rlink); rot_left(tree, node); } } else if (wbal > ALPHA_1) { wbal = WEIGHT(node->llink->llink) / (float)node->llink->weight; if (wbal > ALPHA_2) { rot_right(tree, node); } else { rot_left(tree, node->llink); rot_right(tree, node); } } } tree->count++; return 1; }
int hb_tree_probe(hb_tree *tree, void *key, void **dat) { int rv = 0; hb_node *node, *parent = NULL, *q = NULL; ASSERT(tree != NULL); node = tree->root; while (node) { rv = tree->key_cmp(key, node->key); if (rv < 0) parent = node, node = node->llink; else if (rv > 0) parent = node, node = node->rlink; else { *dat = node->dat; return 0; } if (parent->bal) q = parent; } if ((node = node_new(key, *dat)) == NULL) return -1; if ((node->parent = parent) == NULL) { tree->root = node; ASSERT(tree->count == 0); tree->count = 1; return 1; } if (rv < 0) parent->llink = node; else parent->rlink = node; while (parent != q) { parent->bal = (parent->rlink == node) * 2 - 1; node = parent; parent = parent->parent; } if (q) { if (q->llink == node) { if (--q->bal == -2) { if (q->llink->bal > 0) rot_left(tree, q->llink); rot_right(tree, q); } } else { if (++q->bal == +2) { if (q->rlink->bal < 0) rot_right(tree, q->rlink); rot_left(tree, q); } } } tree->count++; return 1; }
static void delete_node (BiNode_s **np) { static int odd = 0; BiNode_s *node; for (;;) { node = *np; if (!node->right) { *np = node->left; break; } if (!node->left) { *np = node->right; break; } if (odd & 1) { rot_left(np); np = &((*np)->left); } else { rot_right(np); np = &((*np)->right); } ++odd; } free(node); }
TranslationRotation3D &TranslationRotation3D:: operator*=(const TranslationRotation3D &rhs) { // Fl = Fl*Fr; Eigen::Map<Eigen::Vector3d> tra_left(T_); Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_left(R_mat_); double T_rhs[3]; rhs.getT(T_rhs); Eigen::Map<Eigen::Vector3d> tra_right(T_rhs); double R_mat_rhs[9]; rhs.getR_mat(R_mat_rhs); Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_right( R_mat_rhs); tra_left = rot_left * tra_right + tra_left; rot_left *= rot_right; updateR(); // convert back to rotation matrix to increase numerical stability updateR_mat(); // apply logical AND to validity setValid(isValid() && rhs.isValid()); return *this; }
void recover_msg( uint32_t *const msg1, uint32_t *const msg2, const uint32_t *const state1, const uint32_t *const state2, const size_t i) { msg1[i] = rot_right(state1[i] - state1[i - 1], md5_shift[i]) - (*md5_round_func[i])(state1[i - 1], state1[i - 2], state1[i - 3]) - state1[i - 4] - md5_add[i]; msg2[i] = rot_right(state2[i] - state2[i - 1], md5_shift[i]) - (*md5_round_func[i])(state2[i - 1], state2[i - 2], state2[i - 3]) - state2[i - 4] - md5_add[i]; }
int RBST<KEY, VAL, COMP_FUNC, LT_FUNC>::insert_at_root( int root_n, int prev, KEY key, VAL value, int count ) { if ( root_n == -1 ) { if ( prev == -1 ) { int new_free = backing_array[free]->left; delete backing_array[free]; root = free; backing_array[root] = new pair(key, value); free = new_free; ++current_size; return count; } else if ( lt( key, backing_array[prev]->key ) ) { int new_free = backing_array[free]->left; backing_array[prev]->left = free; delete backing_array[free]; backing_array[free] = new pair(key, value); free = new_free; ++current_size; return count; } else { int new_free = backing_array[free]->left; backing_array[prev]->right = free; delete backing_array[free]; backing_array[free] = new pair(key, value); free = new_free; ++current_size; return count; } } else if ( lt( key, backing_array[root_n]->key ) ) { insert_at_root( backing_array[root_n]->left, root_n, key, value, count + 1 ); rot_right(root_n); } else { insert_at_root( backing_array[root_n]->right, root_n, key, value, count + 1 ); rot_left(root_n); } }
int key_hook(int keycode, t_rtv *rtv) { if (keycode == 53) { destroy_and_exit(rtv); } if (keycode == 126) { rot_up(rtv); } if (keycode == 125) { rot_down(rtv); } if (keycode == 124) { rot_right(rtv); } if (keycode == 123) { rot_left(rtv); } return (0); }
int hb_tree_remove(hb_tree *tree, const void *key, int del) { int rv, left; hb_node *node, *out, *parent = NULL; void *tmp; ASSERT(tree != NULL); node = tree->root; while (node) { rv = tree->key_cmp(key, node->key); if (rv == 0) break; parent = node; node = rv < 0 ? node->llink : node->rlink; } if (node == NULL) return -1; if (node->llink && node->rlink) { for (out = node->rlink; out->llink; out = out->llink) /* void */; SWAP(node->key, out->key, tmp); SWAP(node->dat, out->dat, tmp); node = out; parent = out->parent; } out = node->llink ? node->llink : node->rlink; FREE_NODE(node); if (out) out->parent = parent; if (parent == NULL) { tree->root = out; tree->count--; return 0; } left = parent->llink == node; if (left) parent->llink = out; else parent->rlink = out; for (;;) { if (left) { if (++parent->bal == 0) { node = parent; goto higher; } if (parent->bal == +2) { ASSERT(parent->rlink != NULL); if (parent->rlink->bal < 0) { rot_right(tree, parent->rlink); rot_left(tree, parent); } else { ASSERT(parent->rlink->rlink != NULL); if (rot_left(tree, parent) == 0) break; } } else { break; } } else { if (--parent->bal == 0) { node = parent; goto higher; } if (parent->bal == -2) { ASSERT(parent->llink != NULL); if (parent->llink->bal > 0) { rot_left(tree, parent->llink); rot_right(tree, parent); } else { ASSERT(parent->llink->llink != NULL); if (rot_right(tree, parent) == 0) break; } } else { break; } } /* Only get here on double rotations or single rotations that changed * subtree height - in either event, `parent->parent' is positioned * where `parent' was positioned before any rotations. */ node = parent->parent; higher: if ((parent = node->parent) == NULL) break; left = parent->llink == node; } tree->count--; return 0; }
int wb_tree_remove(wb_tree *tree, const void *key, int del) { int rv; wb_node *node, *temp, *out = NULL; /* ergh @ GCC unitializated warning */ ASSERT(tree != NULL); ASSERT(key != NULL); node = tree->root; while (node) { rv = tree->key_cmp(key, node->key); if (rv) { node = rv < 0 ? node->llink : node->rlink; continue; } if (node->llink == NULL) { temp = node; out = node->rlink; if (out) out->parent = node->parent; if (del) { if (tree->key_del) tree->key_del(node->key); if (tree->dat_del) tree->dat_del(node->dat); } if (node->parent) { if (node->parent->llink == node) node->parent->llink = out; else node->parent->rlink = out; } else { tree->root = out; } FREE(node); out = temp; } else if (node->rlink == NULL) { temp = node; out = node->llink; if (out) out->parent = node->parent; if (del) { if (tree->key_del) tree->key_del(node->key); if (tree->dat_del) tree->dat_del(node->dat); } if (node->parent) { if (node->parent->llink == node) node->parent->llink = out; else node->parent->rlink = out; } else { tree->root = out; } FREE(node); out = temp; } else if (WEIGHT(node->llink) > WEIGHT(node->rlink)) { if (WEIGHT(node->llink->llink) < WEIGHT(node->llink->rlink)) rot_left(tree, node->llink); out = node->llink; rot_right(tree, node); node = out->rlink; continue; } else { if (WEIGHT(node->rlink->rlink) < WEIGHT(node->rlink->llink)) rot_right(tree, node->rlink); out = node->rlink; rot_left(tree, node); node = out->llink; continue; } if (--tree->count) { while (out) { out->weight--; out = out->parent; } } return 0; } return -1; }
int wb_tree_insert(wb_tree *tree, void *key, void *dat, int overwrite) { int rv = 0; wb_node *node, *parent = NULL; float wbal; ASSERT(tree != NULL); node = tree->root; while (node) { rv = tree->key_cmp(key, node->key); if (rv < 0) parent = node, node = node->llink; else if (rv > 0) parent = node, node = node->rlink; else { if (overwrite == 0) return 1; if (tree->key_del) tree->key_del(node->key); if (tree->dat_del) tree->dat_del(node->dat); node->key = key; node->dat = dat; return 0; } } if ((node = node_new(key, dat)) == NULL) return -1; if ((node->parent = parent) == NULL) { ASSERT(tree->count == 0); tree->root = node; tree->count = 1; return 0; } if (rv < 0) parent->llink = node; else parent->rlink = node; while ((node = parent) != NULL) { parent = node->parent; node->weight++; wbal = WEIGHT(node->llink) / (float)node->weight; if (wbal < ALPHA_0) { wbal = WEIGHT(node->rlink->llink) / (float)node->rlink->weight; if (wbal < ALPHA_3) { /* LL */ rot_left(tree, node); } else { /* RL */ rot_right(tree, node->rlink); rot_left(tree, node); } } else if (wbal > ALPHA_1) { wbal = WEIGHT(node->llink->llink) / (float)node->llink->weight; if (wbal > ALPHA_2) { /* RR */ rot_right(tree, node); } else { /* LR */ rot_left(tree, node->llink); rot_right(tree, node); } } } tree->count++; return 0; }
static void vtree_rebalance(node_t *root, node_t *prev, vtree_slot_t *prev_slot, int direction, vtree_pthread_data_t *vtree_data) { node_t *node, *sib_node, *tmp1_node, *tmp2_node; vtree_slot_t *sib_slot, *tmp1_slot, *tmp2_slot; node = NULL; loop: /* * loop invariants: * - node is black (or NULL on 1st iteration) * - node is not roiot (parent is not NULL) * -ALL leaf paths going through parent and node have * black node count is 1 lower than other leaf path */ sib_node = prev_slot->child[!direction]; if ((sib_slot = get_slot_from_rec_new(sib_node, vtree_data)) == NULL) sib_slot = read_slot(sib_node, vtree_data); if (get_node_color(sib_slot->parent) == RB_RED) { if (direction == 0) { tmp1_node = sib_slot->child[0]; rot_left(root, prev, prev_slot, vtree_data); } else { tmp1_node = sib_slot->child[1]; rot_right(root, prev, prev_slot, vtree_data); } tmp1_slot = get_slot_from_rec_new(tmp1_node, vtree_data); prev_slot->parent = set_node_red(prev_slot->parent); sib_slot = get_slot_from_rec_new(sib_node, vtree_data); sib_slot->parent = set_node_black(sib_slot->parent); sib_node = tmp1_node; sib_slot = tmp1_slot; } // now node and sib are both black tmp1_node = sib_slot->child[!direction]; if (tmp1_node != NULL) if ((tmp1_slot = get_slot_from_rec_new(tmp1_node, vtree_data)) == NULL) tmp1_slot = read_slot(tmp1_node, vtree_data); if (tmp1_node == NULL || get_node_color(tmp1_slot->parent) == RB_BLACK) { tmp2_node = sib_slot->child[direction]; if (tmp2_node != NULL) if ((tmp2_slot = get_slot_from_rec_new(tmp2_node, vtree_data)) == NULL) tmp2_slot = read_slot(tmp2_node, vtree_data); if (tmp2_node == NULL || get_node_color(tmp2_slot->parent) == RB_BLACK) { // sibling color flip to red // this violate rbtree variant, flip parent to black if it was red, or recurse add_or_change_slot(sib_node, sib_slot->child[0], sib_slot->child[1], set_node_red(sib_slot->parent), vtree_data); if (get_node_color(prev_slot->parent) == RB_RED) prev_slot->parent = set_node_black(prev_slot->parent); else { node = prev; prev = get_node_parent(prev_slot->parent); if (prev) { if ((prev_slot = get_slot_from_rec_new(prev, vtree_data)) == NULL) { prev_slot = read_slot(prev, vtree_data); prev_slot = add_slot(prev, prev_slot->child[0], prev_slot->child[1], prev_slot->parent, vtree_data); } direction = (prev_slot->child[1] == node); goto loop; } } goto out; } if (direction == 0) { rot_right(root, sib_node, sib_slot, vtree_data); } else { rot_left(root, sib_node, sib_slot, vtree_data); } sib_slot = get_slot_from_rec_new(sib_node, vtree_data); sib_slot->parent = set_node_red(sib_slot->parent); tmp2_slot = get_slot_from_rec_new(tmp2_node, vtree_data); tmp2_slot->parent = set_node_black(tmp2_slot->parent); tmp1_node = sib_node; tmp1_slot = sib_slot; sib_node = tmp2_node; } if (direction == 0) { rot_left(root, prev, prev_slot, vtree_data); } else { rot_right(root, prev, prev_slot, vtree_data); } sib_slot = get_slot_from_rec_new(sib_node, vtree_data); sib_slot->parent = change_node_parent(prev_slot->parent, get_node_parent(sib_slot->parent)); prev_slot->parent = set_node_black(prev_slot->parent); add_or_change_slot(tmp1_node, tmp1_slot->child[0], tmp1_slot->child[1], set_node_black(tmp1_slot->parent), vtree_data); out: return; }
int list_ins(int key, pthread_data_t *data) { vtree_tree_t *tree = (vtree_tree_t *)data->list; vtree_pthread_data_t *vtree_data = (vtree_pthread_data_t *)data->ds_data; node_t *prev, *cur, *node, *new_node, *gprev, *prev_sib; vtree_slot_t *slot, *prev_slot, *gprev_slot, *prev_sib_slot; int prev_direction, direction, ret, val; new_node = vtree_new_node(key); assert(new_node != NULL); vtree_write_cs_enter(vtree_data); do { data->nr_txn++; prev = tree->root; prev_slot = read_slot(prev, vtree_data); cur = prev_slot->child[0]; rbtree_check(cur, NULL, RB_RED, vtree_data); direction = 0; while (cur != NULL) { val = cur->value; if (val > key) { prev_slot = read_slot(cur, vtree_data); direction = 0; prev = cur; cur = prev_slot->child[0]; } else if (val < key) { prev_slot = read_slot(cur, vtree_data); direction = 1; prev = cur; cur = prev_slot->child[1]; } else break; } ret = (cur == NULL); if (!ret) goto out; node = new_node; if (direction == 0) prev_slot = add_slot(prev, node, prev_slot->child[1], prev_slot->parent, vtree_data); else prev_slot = add_slot(prev, prev_slot->child[0], node, prev_slot->parent, vtree_data); if (prev == tree->root) { slot = add_slot(node, NULL, NULL, set_node_black(NULL), vtree_data); goto out; } // newly inserted node is red to maintain invariant slot = add_slot(node, NULL, NULL, set_node_red(prev), vtree_data); loop: if (prev == NULL) { // we can always set root to black slot->parent = set_node_black(slot->parent); goto out; } if (get_node_color(prev_slot->parent) == RB_BLACK) { // parent is black, no invariant violated goto out; } // now both node and parent are red, check grandparent, // since parent is red, there must be grandparent and it is black gprev = get_node_parent(prev_slot->parent); gprev_slot = read_slot(gprev, vtree_data); prev_direction = (gprev_slot->child[1] == prev); prev_sib = gprev_slot->child[!prev_direction]; if (prev_sib) { prev_sib_slot = read_slot(prev_sib, vtree_data); if (get_node_color(prev_sib_slot->parent) == RB_RED) { // if sib is red, change parent and sib to black // and move upwards prev_slot->parent = set_node_black(prev_slot->parent); add_slot(prev_sib, prev_sib_slot->child[0], prev_sib_slot->child[1], set_node_black(prev_sib_slot->parent), vtree_data); prev = get_node_parent(gprev_slot->parent); slot = add_slot(gprev, gprev_slot->child[0], gprev_slot->child[1], set_node_red(prev), vtree_data); node = gprev; if (prev) { prev_slot = read_slot(prev, vtree_data); direction = (prev_slot->child[1] == node); prev_slot = add_slot(prev, prev_slot->child[0], prev_slot->child[1], prev_slot->parent, vtree_data); } goto loop; } } // now, either there is no sib of parent or it is black if (prev_direction == 0) { if (direction == 1) { rot_left(tree->root, prev, prev_slot, vtree_data); gprev_slot = get_slot_from_rec_new(gprev, vtree_data); prev = node; prev_slot = get_slot_from_rec_new(node, vtree_data); } rot_right(tree->root, gprev, gprev_slot, vtree_data); gprev_slot = get_slot_from_rec_new(gprev, vtree_data); gprev_slot->parent = set_node_red(gprev_slot->parent); prev_slot->parent = set_node_black(prev_slot->parent); } else { if (direction == 0) { rot_right(tree->root, prev, prev_slot, vtree_data); gprev_slot = get_slot_from_rec_new(gprev, vtree_data); prev = node; prev_slot = get_slot_from_rec_new(node, vtree_data); } rot_left(tree->root, gprev, gprev_slot, vtree_data); gprev_slot = get_slot_from_rec_new(gprev, vtree_data); gprev_slot->parent = set_node_red(gprev_slot->parent); prev_slot->parent = set_node_black(prev_slot->parent); } out: ; } while (vtree_write_cs_exit(vtree_data)); if (ret == 0) vtree_free_node_later(new_node, vtree_data); vtree_maybe_quiescent(vtree_data); return ret; }
void generateVisualizationMarker(double steering_angle_rad, visualization_msgs::MarkerArray& marker_array) { double turn_radius = 0.0; if (std::abs(steering_angle_rad) > 0.000001){ // use cotangent //turn_radius = std::tan((M_PI*0.5 - steering_angle_rad) * p_wheel_base_); turn_radius = (1.0/std::tan(steering_angle_rad)) * p_wheel_base_; } // (0,0) is rear axle middle // x axis towards front, y axis to left of car Eigen::Vector2d icc (0.0, turn_radius); Eigen::Vector2d left_wheel (p_wheel_base_, p_wheel_track_*0.5); Eigen::Vector2d right_wheel(p_wheel_base_, -p_wheel_track_*0.5); double dist_left = (left_wheel - icc).norm(); double dist_right = (right_wheel - icc).norm(); double steer_angle_left = std::asin(p_wheel_base_/dist_left); double steer_angle_right = std::asin(p_wheel_base_/dist_right); if (turn_radius < 0){ steer_angle_left = -steer_angle_left; steer_angle_right = -steer_angle_right; } ROS_DEBUG("turn radius: %f dist left: %f right: %f", turn_radius, dist_left, dist_right); ROS_DEBUG("steer angle left: %f right: %f", steer_angle_left, steer_angle_right); marker_array.markers[0].points.resize(40); marker_array.markers[1].points.resize(40); std::vector<geometry_msgs::Point>& point_vector_left = marker_array.markers[0].points; std::vector<geometry_msgs::Point>& point_vector_right = marker_array.markers[1].points; marker_array.markers[1].color.r = 0.0; marker_array.markers[1].color.b = 1.0; marker_array.markers[1].id = 1; Eigen::Affine3d rot_left (Eigen::AngleAxisd(M_PI*0.5, Eigen::Vector3d::UnitX())* Eigen::AngleAxisd(steer_angle_left, Eigen::Vector3d::UnitY())); tf::quaternionEigenToMsg(Eigen::Quaterniond(rot_left.rotation()), marker_array_.markers[LEFT_FRONT_WHEEL].pose.orientation); Eigen::Affine3d rot_right (Eigen::AngleAxisd(M_PI*0.5, Eigen::Vector3d::UnitX())* Eigen::AngleAxisd(steer_angle_right, Eigen::Vector3d::UnitY())); tf::quaternionEigenToMsg(Eigen::Quaterniond(rot_right.rotation()), marker_array_.markers[RIGHT_FRONT_WHEEL].pose.orientation); //marker_array_.markers[LEFT_FRONT_WHEEL].pose.orientation; // = marker; //marker_array_.markers[RIGHT_FRONT_WHEEL].pose.orientation; // = marker; Eigen::Vector3d rotation_vector( Eigen::Vector3d::UnitZ() ); if (turn_radius > 0.0){ rotation_vector = -rotation_vector; } //std::cout << "rotation_vector:\n" << rotation_vector << "\n"; for (size_t i = 0; i < 40; ++i) { Eigen::Affine3d o_t_i (Eigen::Affine3d::Identity()); o_t_i.translation() = Eigen::Vector3d(icc.x(), -icc.y(), 0.0); //Eigen::Rotation2Dd rotation(steer_angle_left + static_cast<double>(i) * 0.05); Eigen::Affine3d rotation_left (Eigen::AngleAxisd(static_cast<double>(i) * 0.05, rotation_vector)); //Eigen::Affine3d rotation_left (Eigen::AngleAxisd( static_cast<double>(i) * 0.05, // (turn_radius > 0.0) ? -(Eigen::Vector3d::UnitZ()) : Eigen::Vector3d::UnitZ() )); //Eigen::Vector2d tmp(o_t_i * rotation * left_wheel); //Eigen::Vector2d tmp(o_t_i * rotation * left_wheel).translation(); Eigen::Vector3d tmp(o_t_i * rotation_left *Eigen::Vector3d(p_wheel_base_, (turn_radius)-p_wheel_track_*0.5, 0.0)); point_vector_left[i].x = tmp.x(); point_vector_left[i].y = -tmp.y(); Eigen::Affine3d rotation_right (Eigen::AngleAxisd(static_cast<double>(i) * 0.05, rotation_vector)); //Eigen::Affine3d rotation_right (Eigen::AngleAxisd( static_cast<double>(i) * 0.05, // (turn_radius > 0.0) ? -(Eigen::Vector3d::UnitZ()) : Eigen::Vector3d::UnitZ() )); tmp = o_t_i * rotation_right*Eigen::Vector3d(p_wheel_base_, (turn_radius)+p_wheel_track_*0.5, 0.0); point_vector_right[i].x = tmp.x(); point_vector_right[i].y = -tmp.y(); } }