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