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