void Player::evoluciona(float fdelta) { double delta=fdelta; point p=vector2point(pos); point v=vector2point(speed); point c=vector2point(camaleonPos); if (not licked) { p+=v*delta; pos=point2vector(p); return; } if (not tensioning) { p+=v*delta; pos=point2vector(p); if (prodesc(v,p-c)>=0) tensioning=true; return; } p-=c; double modulov=abs(v); double desp=modulov*delta; double r=abs(p); double alfa=desp/r; int signo=prodvec(p,v)>0?1:-1; p*=std::polar(1.0,signo*alfa); v=p*std::polar(1.0,signo*M_PI/2.0); v=(modulov/abs(v))*v; p+=c; pos=point2vector(p); speed=point2vector(v); }
/* This function will find the peak of energy on the current layer (1st argument). When found, the function will return (2nd to 4th. parameters), the peak index, its real eta and real phi values (relative to the RoI lower left corner). */ void peak_find(const CaloLayer* layer, int* idx, double* eta, double* phi) { /* This is a dummy algorithm */ int i; /* iterator */ Energy temp = 0.; for (i=0; i<layer->NoOfCells; ++i) { if (layer->cell[i].energy > temp) { (*idx) = i; temp = layer->cell[i].energy; } } /* Now I have to translate the cell info */ vector2point(&layer->EtaGran, &layer->PhiGran, idx, eta, phi); return; }
static EControl vscanDetection() { if (_vscan.empty() == true || _closest_waypoint < 0) return KEEP; int decelerate_or_stop = -10000; int decelerate2stop_waypoints = 15; for (int i = _closest_waypoint; i < _closest_waypoint + _search_distance; i++) { g_obstacle.clearStopPoints(); if (!g_obstacle.isDecided()) g_obstacle.clearDeceleratePoints(); decelerate_or_stop++; if (decelerate_or_stop > decelerate2stop_waypoints || (decelerate_or_stop >= 0 && i >= _path_dk.getSize()-1) || (decelerate_or_stop >= 0 && i == _closest_waypoint+_search_distance-1)) return DECELERATE; if (i > _path_dk.getSize() - 1 ) return KEEP; // Detection for cross walk if (i == vmap.getDetectionWaypoint()) { if (CrossWalkDetection(vmap.getDetectionCrossWalkID()) == STOP) { _obstacle_waypoint = i; return STOP; } } // waypoint seen by vehicle geometry_msgs::Point waypoint = calcRelativeCoordinate(_path_dk.getWaypointPosition(i), _current_pose.pose); tf::Vector3 tf_waypoint = point2vector(waypoint); tf_waypoint.setZ(0); int stop_point_count = 0; int decelerate_point_count = 0; for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) { tf::Vector3 vscan_vector((double) item->x, (double) item->y, 0); // for simulation if (g_sim_mode) { tf::Transform transform; tf::poseMsgToTF(_sim_ndt_pose.pose, transform); geometry_msgs::Point world2vscan = vector2point(transform * vscan_vector); vscan_vector = point2vector(calcRelativeCoordinate(world2vscan, _current_pose.pose)); vscan_vector.setZ(0); } // 2D distance between waypoint and vscan points(obstacle) // ---STOP OBSTACLE DETECTION--- double dt = tf::tfDistance(vscan_vector, tf_waypoint); if (dt < _detection_range) { stop_point_count++; geometry_msgs::Point vscan_temp; vscan_temp.x = item->x; vscan_temp.y = item->y; vscan_temp.z = item->z; if (g_sim_mode) g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose)); else g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose)); } if (stop_point_count > _threshold_points) { _obstacle_waypoint = i; return STOP; } // without deceleration range if (_deceleration_range < 0.01) continue; // deceleration search runs "decelerate_search_distance" waypoints from closest if (i > _closest_waypoint+_deceleration_search_distance || decelerate_or_stop >= 0) continue; // ---DECELERATE OBSTACLE DETECTION--- if (dt > _detection_range && dt < _detection_range + _deceleration_range) { bool count_flag = true; // search overlaps between DETECTION range and DECELERATION range for (int waypoint_search = -5; waypoint_search <= 5; waypoint_search++) { if (i+waypoint_search < 0 || i+waypoint_search >= _path_dk.getSize() || !waypoint_search) continue; geometry_msgs::Point temp_waypoint = calcRelativeCoordinate( _path_dk.getWaypointPosition(i+waypoint_search), _current_pose.pose); tf::Vector3 waypoint_vector = point2vector(temp_waypoint); waypoint_vector.setZ(0); // if there is a overlap, give priority to DETECTION range if (tf::tfDistance(vscan_vector, waypoint_vector) < _detection_range) { count_flag = false; break; } } if (count_flag) { decelerate_point_count++; geometry_msgs::Point vscan_temp; vscan_temp.x = item->x; vscan_temp.y = item->y; vscan_temp.z = item->z; if (g_sim_mode) g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose)); else g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose)); } } // found obstacle to DECELERATE if (decelerate_point_count > _threshold_points) { _obstacle_waypoint = i; decelerate_or_stop = 0; // for searching near STOP obstacle g_obstacle.setDecided(true); } } } return KEEP; //no obstacles }