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;
}
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();
}
}
