void GLScenePane::drawPoints() {
//draw current map points
for (size_t i = 0; i < curMapPoints.size(); i++) {
const MapPoint* p = curMapPoints[i];
drawCurMapPoint(p, m_scale, m_pointSize, true); //!MyApp::bStop);
}
glPointSize(3.0 * m_pointSize);
//draw active map points
glBegin(GL_POINTS);
for (size_t i = 0; i < actMapPoints.size(); i++) {
const MapPoint* p = actMapPoints[i];
if (p->isCertainStatic()) {
double r = p->color[0] / 255.0;
double g = p->color[1] / 255.0;
double b = p->color[2] / 255.0;
glColor3d(r, g, b);
glVertex3d(p->x, p->y, p->z);
}
}
glEnd();
glPointSize(2.0 * m_pointSize);
// //draw inactive map points
// for (size_t i = 0; i < iactMapPoints.size(); i++) {
// const MapPoint* p = iactMapPoints[i];
// if (p->type == TYPE_MAPPOINT_STATIC) {
// //glColor3f(PT_COLOR[0], PT_COLOR[1], PT_COLOR[2]);
// glColor3f(0.35f, 0.35f, 0.35f);
// glTranslatef(p->x, p->y, p->z);
// //glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? m_range * 0.018 : m_range * 0.018 * 5, 12, 12);
// glutSolidSphere(0.004 * m_range * p->numVisCam, 12, 12);
// // glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? 0.004 * m_range * p->numVisCam : m_range * 0.004 * 5, 12,
// // 12);
// glTranslatef(-p->x, -p->y, -p->z);
// }
// }
//glDisable(GL_LIGHT1);
//glDisable(GL_LIGHTING);
//draw inactive map points
glBegin(GL_POINTS);
for (size_t i = 0; i < iactMapPoints.size(); i++) {
const MapPoint* p = iactMapPoints[i];
if (p->isGlobalStatic()) {
double r = p->color[0] / 255.0;
double g = p->color[1] / 255.0;
double b = p->color[2] / 255.0;
glColor3d(r, g, b);
glVertex3d(p->x, p->y, p->z);
}
}
glEnd();
//draw dynamic trajectories
if (m_drawDynTrj) {
// glPointSize(0.5);
// glBegin(GL_POINTS);
// int tf = dynMapPoints.size();
// int f0 = tf - m_nTrjLen < 0 ? 0 : tf - m_nTrjLen;
// int numFrms = tf - f0;
// double dalpha = 0.3 / (numFrms + 1.0);
// for (int f = f0; f < tf; f++) {
// glColor4d(0, 0, 1, 0.0 + (f - f0) * dalpha);
// for (int i = 0; i < dynMapPoints[f].m; i++) {
// glVertex3d(dynMapPoints[f].data[3 * i], dynMapPoints[f].data[3 * i + 1], dynMapPoints[f].data[3 * i + 2]);
// }
// }
// glEnd();
for (size_t n = 0; n < dynTracks.size(); n++) {
glLineWidth(0.5);
glBegin(GL_LINE_STRIP);
double alpha = 1.0 / dynTracks[n].size();
for (size_t i = 0; i < dynTracks[n].size(); i++) {
glColor4d(0, 0, 1, 1.0 - i * alpha);
glVertex3d(dynTracks[n][i].x, dynTracks[n][i].y,
dynTracks[n][i].z);
}
glEnd();
}
}
}
void GLScenePane::drawPoints() {
//draw current map points
for (size_t i = 0; i < curMapPoints.size(); i++) {
const MapPoint* p = curMapPoints[i];
drawCurMapPoint(p, m_scale, m_pointSize, true); //!MyApp::bStop);
}
glPointSize(8.0 * m_pointSize);
//draw active map points
// glBegin(GL_POINTS);
// for (size_t i = 0; i < actMapPoints.size(); i++) {
// const MapPoint* p = actMapPoints[i];
// if (p->isCertainStatic()) {
// double r = p->color[0] / 255.0;
// double g = p->color[1] / 255.0;
// double b = p->color[2] / 255.0;
// glColor3d(0, 1, 0);
// glVertex3d(p->x, p->y, p->z);
// }
// }
// glEnd();
glPointSize(2.0 * m_pointSize);
// //draw inactive map points
// for (size_t i = 0; i < iactMapPoints.size(); i++) {
// const MapPoint* p = iactMapPoints[i];
// if (p->type == TYPE_MAPPOINT_STATIC) {
// //glColor3f(PT_COLOR[0], PT_COLOR[1], PT_COLOR[2]);
// glColor3f(0.35f, 0.35f, 0.35f);
// glTranslatef(p->x, p->y, p->z);
// //glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? m_range * 0.018 : m_range * 0.018 * 5, 12, 12);
// glutSolidSphere(0.004 * m_range * p->numVisCam, 12, 12);
// // glutSolidSphere(p->flag < FLAG_MAPPOINT_TEST1 ? 0.004 * m_range * p->numVisCam : m_range * 0.004 * 5, 12,
// // 12);
// glTranslatef(-p->x, -p->y, -p->z);
// }
// }
//glDisable(GL_LIGHT1);
//glDisable(GL_LIGHTING);
//draw inactive map points
// glBegin(GL_POINTS);
// for (size_t i = 0; i < iactMapPoints.size(); i++) {
// const MapPoint* p = iactMapPoints[i];
// if (p->isGlobalStatic()) {
// double r = p->color[0] / 255.0;
// double g = p->color[1] / 255.0;
// double b = p->color[2] / 255.0;
// glColor3d(r, g, b);
//
// glVertex3d(p->x, p->y, p->z);
// }
// }
// glEnd();
if (mDynObjPresent){
glPointSize(30.0 * m_pointSize);
glBegin(GL_POINTS);
glColor3d(1.0, 0, 0);
glVertex3d(mDynObjPos[0], mDynObjPos[1], mDynObjPos[2]);
glEnd();
// glLineWidth(60);
// glBegin(GL_LINE_STRIP);
// glPointSize(10.0 * m_pointSize);
// glBegin(GL_POINTS);
// double alpha = 1.0 / mDynObjPosVec.size();
// for (size_t i = 0; i < mDynObjPosVec.size(); i = i + 3) {
// glColor3d(1, 0, 0);
// glVertex3d(mDynObjPosVec[i],mDynObjPosVec[i+1],mDynObjPosVec[i+2]);
// }
// glEnd();
}
//draw dynamic trajectories
if (m_drawDynTrj) {
// glPointSize(0.5);
// glBegin(GL_POINTS);
// int tf = dynMapPoints.size();
// int f0 = tf - m_nTrjLen < 0 ? 0 : tf - m_nTrjLen;
// int numFrms = tf - f0;
// double dalpha = 0.3 / (numFrms + 1.0);
// for (int f = f0; f < tf; f++) {
// glColor4d(0, 0, 1, 0.0 + (f - f0) * dalpha);
// for (int i = 0; i < dynMapPoints[f].m; i++) {
// glVertex3d(dynMapPoints[f].data[3 * i], dynMapPoints[f].data[3 * i + 1], dynMapPoints[f].data[3 * i + 2]);
// }
// }
// glEnd();
// Simple Clustering
// vector<cv::Point3f> dynVel;
// dynVel.clear();
// for (size_t n = 0; n < dynTracks.size(); n++) {
// dynVel.push_back(cv::Point3f(dynTracks[n][0].x - dynTracks[n][1].x,
// dynTracks[n][0].y - dynTracks[n][1].y,
// dynTracks[n][0].z - dynTracks[n][1].z));
// }
//
// vector<double> numLike;
// vector<vector<int> > idLike;
// int maxId = -1;
// int maxNum = 0;
//
// for (int i = 0; i < dynVel.size(); i++){
// vector<int> ids;
// for (int j = 0; j < dynVel.size(); j++){
//// if(j != i){
// double lengthDiff = 0;
// double lengthI = sqrt(pow(dynVel[i].x,2) + pow(dynVel[i].y,2) + pow(dynVel[i].z,2));
// double lengthJ = sqrt(pow(dynVel[j].x,2) + pow(dynVel[j].y,2) + pow(dynVel[j].z,2));
// lengthDiff = abs(lengthI - lengthJ);
// double dotVec = dynVel[i].dot(dynVel[j]);
// double theta = acos(dotVec / (lengthI * lengthJ)) * 180 / 3.1415926;
// if (theta < 30){
// ids.push_back(j);
// }
//// }
// }
// idLike.push_back(ids);
// if (ids.size() > maxNum){
// maxId = i;
// maxNum = ids.size();
// }
// }
// for (size_t n = 0; n < dynTracks.size(); n++) {
// glLineWidth(0.5);
// glBegin(GL_LINE_STRIP);
//
// double alpha = 1.0 / dynTracks[n].size();
// for (size_t i = 0; i < dynTracks[n].size(); i++) {
// glColor4d(0, 0, 1, 1.0 - i * alpha);
// glVertex3d(dynTracks[n][i].x, dynTracks[n][i].y,
// dynTracks[n][i].z);
// }
// glEnd();
// }
//
// if (maxId > -1){
//// for (size_t ii = 0; ii < idLike[maxId].size(); ii++) {
//// int n = idLike[maxId][ii];
//// glLineWidth(1.5);
//// glBegin(GL_LINE_STRIP);
////
//// double alpha = 1.0 / dynTracks[n].size();
//// for (size_t i = 0; i < dynTracks[n].size(); i++) {
//// glColor4d(1, 0, 0, 1.0 - i * alpha);
//// glVertex3d(dynTracks[n][i].x, dynTracks[n][i].y,
//// dynTracks[n][i].z);
//// }
//// glEnd();
//// }
// for (size_t ii = 0; ii < idLike[maxId].size(); ii++){
// int n = idLike[maxId][ii];
// glPointSize(10.0 * m_pointSize);
// glBegin(GL_POINTS);
// glColor3d(1.0, 0, 0);
// glVertex3d(dynTracks[n][0].x, dynTracks[n][0].y,
// dynTracks[n][0].z);
// glEnd();
// }
// }
}
}