static int mouse_release(Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3], const GdkEventButton *event) { RendererSimTraffic *self = (RendererSimTraffic*) ehandler->user; if (self->ehandler.picking) { if (!ctrans_have_pose (self->ctrans) || !ctrans_have_gps_to_local (self->ctrans)) { self->ehandler.picking = 0; return 0; } double car_pos[3]; ctrans_local_pos (self->ctrans, car_pos); geom_ray_z_plane_intersect_3d(POINT3D(ray_start), POINT3D(ray_dir), car_pos[2], POINT2D(self->last_xy)); lcmtypes_track_t msg = { .id = self->selected_vehicle_id, .pos = { self->last_xy[0], self->last_xy[1] }, .vel = { 0, 0 }, .size = { 0, 0 }, .size_good = TRUE, .theta = 0, .confidence = 3, }; lcmtypes_track_t_publish (self->lc, "TSIM_MOVE", &msg); } self->ehandler.picking = 0; return 0; } static int mouse_motion (Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3], const GdkEventMotion *event) { RendererSimTraffic *self = (RendererSimTraffic*) ehandler->user; if (!self->ehandler.picking || !event->state & GDK_BUTTON1_MASK) return 0; if (!ctrans_have_pose (self->ctrans) || !ctrans_have_gps_to_local (self->ctrans)) { return 0; } double car_pos[3]; ctrans_local_pos (self->ctrans, car_pos); geom_ray_z_plane_intersect_3d (POINT3D(ray_start), POINT3D(ray_dir), car_pos[2], POINT2D(self->last_xy)); return 1; }
static GList * get_yields (IntersectionState * s, RndfOverlayWaypoint * w, RndfOverlayWaypoint * w2) { GList * yields = NULL; if (w2->type == RNDF_POINT_WAYPOINT && w2->prev && !w2->prev->is_stop && w2->prev != w) { add_yield (&yields, w2, NULL, 0); //printf ("%s %s prev yield\n", w->id_str, w2->id_str); } for (int j = 0; j < w2->num_entrances; j++) if (w2->entrances[j] != w && !w2->entrances[j]->is_stop) { add_yield (&yields, w2->entrances[j], w2, 1); //printf ("%s %s entrance %d yield\n", w->id_str, w2->id_str, j); } SegmentCache segment; make_segment_cache (&s->gpslin, &segment, w, w2); GList * list = find_intersecting_segments (&segment, s->segments_no_stop); for (GList * iter = list; iter; iter = iter->next) { SegmentCache * c = iter->data; double p[2], u; if (geom_line_seg_line_seg_intersect_2d (POINT2D (c->p1), POINT2D (c->p2), POINT2D (segment.p1), POINT2D (segment.p2), POINT2D (p)) == 0) continue; geom_point_line_closest_point_2d (POINT2D (p), POINT2D (c->p1), POINT2D (c->p2), NULL, &u); add_yield (&yields, c->w1, c->w2, u); //printf ("%s %s cross yield %s %s\n", w->id_str, w2->id_str, // c->w1->id_str, c->w2->id_str); } g_list_free (list); return yields; }
static int mouse_press (Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3], const GdkEventButton *event) { RendererGoal *self = (RendererGoal*) ehandler->user; double xy[2]; geom_ray_z_plane_intersect_3d(POINT3D (ray_start), POINT3D (ray_dir), 0, POINT2D (xy)); int consumed = 0; int control = event->state & GDK_CONTROL_MASK; pthread_mutex_lock (&self->mutex); // select a previously selected goal? if (event->button == 1 && ehandler->picking) { self->edit_goal = find_goal(self, xy); consumed = 1; } if (event->button == 1 && control && ehandler->picking && self->edit_goal == NULL) { // create a new goal lcmtypes_goal_t g = { .pos = { xy[0], xy[1] }, .size = { 1, 1 }, .theta = 0,
static int _waypoint_edge_intersect_test (gps_linearize_t *gpslin, RndfOverlayWaypoint *wp_a1, RndfOverlayWaypoint *wp_a2, RndfOverlayWaypoint *wp_b1, RndfOverlayWaypoint *wp_b2) { double latlon_a1[2] = { wp_a1->waypoint->lat, wp_a1->waypoint->lon }; double latlon_a2[2] = { wp_a2->waypoint->lat, wp_a2->waypoint->lon }; double latlon_b1[2] = { wp_b1->waypoint->lat, wp_b1->waypoint->lon }; double latlon_b2[2] = { wp_b2->waypoint->lat, wp_b2->waypoint->lon }; double xy_a1[2], xy_a2[2], xy_b1[2], xy_b2[2]; gps_linearize_to_xy (gpslin, latlon_a1, xy_a1); gps_linearize_to_xy (gpslin, latlon_a2, xy_a2); gps_linearize_to_xy (gpslin, latlon_b1, xy_b1); gps_linearize_to_xy (gpslin, latlon_b2, xy_b2); return geom_line_seg_line_seg_intersect_test_2d (POINT2D (xy_a1), POINT2D (xy_a2), POINT2D (xy_b1), POINT2D (xy_b2)); }
static GList * find_intersecting_segments (SegmentCache * match, GArray * list) { GList * result = NULL; int i; for (i = 0; i < list->len; i++) { SegmentCache * c = &g_array_index (list, SegmentCache, i); if (c->min[0] >= match->max[0] || c->max[0] <= match->min[0] || c->min[1] >= match->max[1] || c->max[1] <= match->min[1]) continue; if (c->w1 == match->w1 || c->w1 == match->w2 || c->w2 == match->w1 || c->w2 == match->w2) continue; if (geom_line_seg_line_seg_intersect_test_2d (POINT2D (c->p1), POINT2D (c->p2), POINT2D (match->p1), POINT2D (match->p2))) result = g_list_prepend (result, c); } return result; }
static int mouse_motion (Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3], const GdkEventMotion *event) { RendererCar *self = (RendererCar*) ehandler->user; if (!viewer->simulation_flag||!self->teleport_car) return 0; if (!self->ehandler.picking) return 0; lcmtypes_pose_t p; if (ctrans_local_pose (self->ctrans, &p) < 0) return 0; double xy[2]; geom_ray_z_plane_intersect_3d(POINT3D(ray_start), POINT3D(ray_dir), p.pos[2], POINT2D(xy)); int shift = event->state & GDK_SHIFT_MASK; if (shift) { // rotate! double theta1 = atan2(xy[1] - p.pos[1], xy[0] - p.pos[0]); // local double dq[4] = { cos (theta1/2), 0, 0, sin(theta1/2) }; memcpy(p.orientation, dq, 4 * sizeof(double)); } else { // translate p.pos[0] = xy[0]; p.pos[1] = xy[1]; } self->last_xy[0] = xy[0]; self->last_xy[1] = xy[1]; lcmtypes_pose_t_publish(self->lc, "SIM_TELEPORT", &p); return 1; }
static int mouse_press (Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3], const GdkEventButton *event) { RendererCar *self = (RendererCar*) ehandler->user; // only handle mouse button 1. if (self->teleport_car_request&&(event->button == 1)) { self->teleport_car = 1; } if (event->type == GDK_2BUTTON_PRESS) { self->display_detail = (self->display_detail + 1) % NUM_DETAILS; viewer_request_redraw(self->viewer); } double carpos[3] = { 0, 0, 0 }; ctrans_local_pos (self->ctrans, carpos); geom_ray_z_plane_intersect_3d(POINT3D(ray_start), POINT3D(ray_dir), carpos[2], POINT2D(self->last_xy)); return 0; }
void Init() { // ::SetTimer(g_hwnd,100,1000,NULL); bool bret = LoadBitmapFile(&bitmap,"wall.bmp"); int bitmapwidth = bitmap.bitmapinfoheader.biWidth; int bitmapheight = bitmap.bitmapinfoheader.biHeight; CRenderObject *pobj = new CRenderObject; std::vector<Vertex> pointlist; pointlist.push_back(Vertex(POINT3D(10,10,10) )); pointlist.push_back(Vertex(POINT3D(10,10,-10))); pointlist.push_back(Vertex(POINT3D(-10,10,-10))); pointlist.push_back(Vertex(POINT3D(-10,10,10))); pointlist.push_back(Vertex(POINT3D(10,-10,10))); pointlist.push_back(Vertex(POINT3D(-10,-10,10))); pointlist.push_back(Vertex(POINT3D(-10,-10,-10))); pointlist.push_back(Vertex(POINT3D(10,-10,-10))); pobj->m_translateverticesList = pointlist; pobj->m_verticesList = pointlist; //Set texture pointlist std::vector<POINT2D> texturelist; texturelist.push_back(POINT2D(0,0)); texturelist.push_back(POINT2D(bitmapwidth-1,0)); texturelist.push_back(POINT2D(bitmapwidth-1,bitmapheight-1)); texturelist.push_back(POINT2D(0,bitmapheight-1)); pobj->m_texturecoordinatesList = texturelist; int tempindices[36]= { 0,1,2, 0,2,3, 0,7,1, 0,4,7, 1,7,6, 1,6,2, 2,6,5, 2,3,5, 0,5,4, 0,3,5, 5,6,7, 4,5,7 }; int textindices[36] = { 2,1,0, 2,0,3, 2,0,1, 2,3,0, 1,0,3, 1,3,0, 0,1,2, 2,3,0, 2,0,1, 2,3,0, 2,1,0, 3,2,0 }; for(int i =0; i <12;++i) { POLYGON temp ; temp.v[0] = tempindices[i*3+0]; temp.v[1] = tempindices[i*3+1]; temp.v[2] = tempindices[i*3 +2]; temp.text[0] = textindices[i*3+0]; temp.text[1] = textindices[i*3+1]; temp.text[2] = textindices[i*3+2]; temp.state |= OBJECT_HAS_TEXTURE|SHADE_MODEL_CONSTANT; temp.color = RGBA(RGB(128,123,140)); temp.texture = &bitmap; temp.pointlist = &pobj->m_verticesList; temp.texturecoordinatelist = &pobj->m_texturecoordinatesList; pobj->m_PolyGonList.push_back(temp); } pobj->world_pos = POINT3D(-10,0,50); CRenderObject *pobj2 = new CRenderObject(*pobj); pobj2->world_pos = POINT3D(15,0,50); CRenderObject* pobj3 =new CRenderObject(*pobj); pobj3->world_pos = POINT3D(0,10,80); CRenderObject *pobj4 = new CRenderObject(*pobj); pobj4->world_pos = POINT3D(20,-10,100); g_RenderManager.Add(pobj); g_RenderManager.Add(pobj2); g_RenderManager.Add(pobj3); g_RenderManager.Add(pobj4); //Set lights CLight *pLight = new CLight; pLight->InitLight(CLight::kAmbientLight,RGBA(RGB(255,0,0)),Black_Color,Black_Color); g_lights.AddLight(pLight); CLight *pLight2 = new CLight; pLight2->InitLight(CLight::kInfiniteLigtht,Black_Color,RGBA(255,0,0),Black_Color,ZeroVector, Vector4D(-1.0,0.0,1.0)); g_lights.AddLight(pLight2); CLight *pLight3 = new CLight; pLight3->InitLight(CLight::kPointLight,Black_Color,RGBA(0,255,0),Black_Color,Vector4D(0,10,0), ZeroVector,1.0,2.0,4.0); g_lights.AddLight(pLight3); CLight *pLight4 = new CLight; pLight4->InitLight(CLight::kSpotLight,Black_Color,RGBA(255,255,255),Black_Color,Vector4D(0,20,10), Vector4D(0.0,0.0,1.0),0.0,0.01,0.0,30.0,60.0,1.0); g_lights.AddLight(pLight4); g_RenderManager.SetCamera(&g_cam); g_RenderManager.SetLightManager(&g_lights); }
double PlannerH::PlanUsingReedSheppWithObstacleDetection(const WayPoint& start, const WayPoint& goal,GridMap& map, vector<WayPoint>& genSmoothedPath, const double pathDensity , const double smoothFactor ) { RSPlanner rs_planner(smoothFactor); int numero = 0; double t=0, u=0 , v=0; rs_planner.PATHDENSITY = pathDensity; genSmoothedPath.clear(); genSmoothedPath.clear(); double length = rs_planner.min_length_rs(start.pos.x, start.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(start.pos.a), goal.pos.x, goal.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(goal.pos.a), numero, t , u , v); rs_planner.constRS(numero, t, u , v, start.pos.x, start.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(start.pos.a), rs_planner.PATHDENSITY, genSmoothedPath); if(genSmoothedPath.size() == 0) return length; CELL_Info* pCellRet = 0; WayPoint p = genSmoothedPath.at(0); int nChanges = 0; double nMinChangeDistance = length; double d = 0; for(unsigned int i=0; i<genSmoothedPath.size(); i++) { if(p.bDir != genSmoothedPath.at(i).bDir) { if(d < nMinChangeDistance) nMinChangeDistance = d; d = 0; nChanges++; } d+= distance2points(p.pos, genSmoothedPath.at(i).pos); p = genSmoothedPath.at(i); // if(map.) // pCellRet = map.GetCellFromPointInnerMap(p.p); // else pCellRet = map.GetCellFromPoint(POINT2D(p.pos.x, p.pos.y)); if(pCellRet) { if(pCellRet->nMovingPoints > 0|| pCellRet->nStaticPoints > 0 || pCellRet->heuristic == map.m_MaxHeuristics) { cout << "\n Obstacle Detected \n"; genSmoothedPath.clear(); return -1; } } else { cout << "\n Outside the Main Grid \n"; genSmoothedPath.clear(); return -1; } } if(nChanges > 3 || nMinChangeDistance < 3.2) { cout << "\n Too much gear changes \n"; genSmoothedPath.clear(); return -1; } // pthread_mutex_lock(&planning_mutex); // m_CurrentPath.assign(genSmoothedPath.begin(), genSmoothedPath.end()); // m_CurrentSmoothPath.clear(); // //m_TotalPath.assign(m_CurrentPath.begin(), m_CurrentPath.end()); // m_CurrentSmoothPath.assign(m_CurrentPath.begin(), m_CurrentPath.end()); // //m_TotalSmoothPath.assign(m_CurrentSmoothPath.begin(), m_CurrentSmoothPath.end()); // pthread_mutex_unlock(&planning_mutex); return length; }