Polygon PointSensor::updateFOV(const Point3 &translationGlobal, const Point3 &rotationGlobal) {
// 1. Rotate the translation of the sensor w.r.t. the global rotation of the vehicle.
m_sensorPosition = m_translation;
m_sensorPosition.rotateZ(rotationGlobal.getAngleXY());
m_sensorPosition += translationGlobal;
m_totalRotation = rotationGlobal.getAngleXY() + m_rotZ*cartesian::Constants::DEG2RAD;
// 2. Construct the FOV in the origin w.r.t. the global rotation of the vehicle.
Point3 leftBoundaryFOV(m_distanceFOV, 0, 0);
leftBoundaryFOV.rotateZ(m_totalRotation + (m_angleFOV/2.0)*cartesian::Constants::DEG2RAD);
Point3 rightBoundaryFOV(m_distanceFOV, 0, 0);
rightBoundaryFOV.rotateZ(m_totalRotation - (m_angleFOV/2.0)*cartesian::Constants::DEG2RAD);
// 3. Translate the FOV to the sensor's position.
leftBoundaryFOV += m_sensorPosition;
rightBoundaryFOV += m_sensorPosition;
// Iterate through all available polygons and intersect FOV-polygon with polygon.
Polygon FOV;
FOV.add(m_sensorPosition);
FOV.add(leftBoundaryFOV);
FOV.add(rightBoundaryFOV);
FOV.add(m_sensorPosition);
m_FOV = FOV;
return m_FOV;
}
void CamGen::display() {
glClearColor(0, 0.58, 0.78, 0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
Point3 dir = m_egoState.getRotation();
Point3 target(15, 0, 0);
target.rotateZ(dir.getAngleXY());
target += m_egoState.getPosition();
// cerr << "Look from: " << m_egoState.getPosition().toString() << "/angle: " << dir.getAngleXY() << endl;
// cerr << "Look to: " << target.toString() << endl << endl;
gluLookAt(m_egoState.getPosition().getX(), m_egoState.getPosition().getY(), 2.8,
target.getX(), target.getY(), 0,
0, 0, 1);
// First, apply the translation.
glTranslatef(m_translationX, m_translationY, 0);
// Second, rotate the model.
glRotatef(m_theta, 1, 0, 0);
glRotatef(m_phi, 0, 1, 0);
// Third, scale the model.
glScalef(m_scale, m_scale, m_scale);
glPushMatrix();
drawScene();
glPopMatrix();
glutSwapBuffers();
}
