1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "RenderTopView.h" 18 #include "VideoTex.h" 19 #include "glError.h" 20 #include "shader.h" 21 #include "shader_simpleTex.h" 22 #include "shader_projectedTex.h" 23 24 #include <log/log.h> 25 #include <math/mat4.h> 26 #include <math/vec3.h> 27 28 29 // Simple aliases to make geometric math using vectors more readable 30 static const unsigned X = 0; 31 static const unsigned Y = 1; 32 static const unsigned Z = 2; 33 //static const unsigned W = 3; 34 35 36 // Since we assume no roll in these views, we can simplify the required math 37 static android::vec3 unitVectorFromPitchAndYaw(float pitch, float yaw) { 38 float sinPitch, cosPitch; 39 sincosf(pitch, &sinPitch, &cosPitch); 40 float sinYaw, cosYaw; 41 sincosf(yaw, &sinYaw, &cosYaw); 42 return android::vec3(cosPitch * -sinYaw, 43 cosPitch * cosYaw, 44 sinPitch); 45 } 46 47 48 // Helper function to set up a perspective matrix with independent horizontal and vertical 49 // angles of view. 50 static android::mat4 perspective(float hfov, float vfov, float near, float far) { 51 const float tanHalfFovX = tanf(hfov * 0.5f); 52 const float tanHalfFovY = tanf(vfov * 0.5f); 53 54 android::mat4 p(0.0f); 55 p[0][0] = 1.0f / tanHalfFovX; 56 p[1][1] = 1.0f / tanHalfFovY; 57 p[2][2] = - (far + near) / (far - near); 58 p[2][3] = -1.0f; 59 p[3][2] = - (2.0f * far * near) / (far - near); 60 return p; 61 } 62 63 64 // Helper function to set up a view matrix for a camera given it's yaw & pitch & location 65 // Yes, with a bit of work, we could use lookAt, but it does a lot of extra work 66 // internally that we can short cut. 67 static android::mat4 cameraLookMatrix(const ConfigManager::CameraInfo& cam) { 68 float sinYaw, cosYaw; 69 sincosf(cam.yaw, &sinYaw, &cosYaw); 70 71 // Construct principal unit vectors 72 android::vec3 vAt = unitVectorFromPitchAndYaw(cam.pitch, cam.yaw); 73 android::vec3 vRt = android::vec3(cosYaw, sinYaw, 0.0f); 74 android::vec3 vUp = -cross(vAt, vRt); 75 android::vec3 eye = android::vec3(cam.position[X], cam.position[Y], cam.position[Z]); 76 77 android::mat4 Result(1.0f); 78 Result[0][0] = vRt.x; 79 Result[1][0] = vRt.y; 80 Result[2][0] = vRt.z; 81 Result[0][1] = vUp.x; 82 Result[1][1] = vUp.y; 83 Result[2][1] = vUp.z; 84 Result[0][2] =-vAt.x; 85 Result[1][2] =-vAt.y; 86 Result[2][2] =-vAt.z; 87 Result[3][0] =-dot(vRt, eye); 88 Result[3][1] =-dot(vUp, eye); 89 Result[3][2] = dot(vAt, eye); 90 return Result; 91 } 92 93 94 RenderTopView::RenderTopView(sp<IEvsEnumerator> enumerator, 95 const std::vector<ConfigManager::CameraInfo>& camList, 96 const ConfigManager& mConfig) : 97 mEnumerator(enumerator), 98 mConfig(mConfig) { 99 100 // Copy the list of cameras we're to employ into our local storage. We'll create and 101 // associate a streaming video texture when we are activated. 102 mActiveCameras.reserve(camList.size()); 103 for (unsigned i=0; i<camList.size(); i++) { 104 mActiveCameras.emplace_back(camList[i]); 105 } 106 } 107 108 109 bool RenderTopView::activate() { 110 // Ensure GL is ready to go... 111 if (!prepareGL()) { 112 ALOGE("Error initializing GL"); 113 return false; 114 } 115 116 // Load our shader programs 117 mPgmAssets.simpleTexture = buildShaderProgram(vtxShader_simpleTexture, 118 pixShader_simpleTexture, 119 "simpleTexture"); 120 if (!mPgmAssets.simpleTexture) { 121 ALOGE("Failed to build shader program"); 122 return false; 123 } 124 mPgmAssets.projectedTexture = buildShaderProgram(vtxShader_projectedTexture, 125 pixShader_projectedTexture, 126 "projectedTexture"); 127 if (!mPgmAssets.projectedTexture) { 128 ALOGE("Failed to build shader program"); 129 return false; 130 } 131 132 133 // Load the checkerboard text image 134 mTexAssets.checkerBoard.reset(createTextureFromPng( 135 "/system/etc/automotive/evs/LabeledChecker.png")); 136 if (!mTexAssets.checkerBoard) { 137 ALOGE("Failed to load checkerboard texture"); 138 return false; 139 } 140 141 // Load the car image 142 mTexAssets.carTopView.reset(createTextureFromPng( 143 "/system/etc/automotive/evs/CarFromTop.png")); 144 if (!mTexAssets.carTopView) { 145 ALOGE("Failed to load carTopView texture"); 146 return false; 147 } 148 149 150 // Set up streaming video textures for our associated cameras 151 for (auto&& cam: mActiveCameras) { 152 cam.tex.reset(createVideoTexture(mEnumerator, cam.info.cameraId.c_str(), sDisplay)); 153 if (!cam.tex) { 154 ALOGE("Failed to set up video texture for %s (%s)", 155 cam.info.cameraId.c_str(), cam.info.function.c_str()); 156 // TODO: For production use, we may actually want to fail in this case, but not yet... 157 // return false; 158 } 159 } 160 161 return true; 162 } 163 164 165 void RenderTopView::deactivate() { 166 // Release our video textures 167 // We can't hold onto it because some other Render object might need the same camera 168 // TODO: If start/stop costs become a problem, we could share video textures 169 for (auto&& cam: mActiveCameras) { 170 cam.tex = nullptr; 171 } 172 } 173 174 175 bool RenderTopView::drawFrame(const BufferDesc& tgtBuffer) { 176 // Tell GL to render to the given buffer 177 if (!attachRenderTarget(tgtBuffer)) { 178 ALOGE("Failed to attached render target"); 179 return false; 180 } 181 182 // Set up our top down projection matrix from car space (world units, Xfwd, Yright, Zup) 183 // to view space (-1 to 1) 184 const float top = mConfig.getDisplayTopLocation(); 185 const float bottom = mConfig.getDisplayBottomLocation(); 186 const float right = mConfig.getDisplayRightLocation(sAspectRatio); 187 const float left = mConfig.getDisplayLeftLocation(sAspectRatio); 188 189 const float near = 10.0f; // arbitrary top of view volume 190 const float far = 0.0f; // ground plane is at zero 191 192 // We can use a simple, unrotated ortho view since the screen and car space axis are 193 // naturally aligned in the top down view. 194 // TODO: Not sure if flipping top/bottom here is "correct" or a double reverse... 195 // orthoMatrix = android::mat4::ortho(left, right, bottom, top, near, far); 196 orthoMatrix = android::mat4::ortho(left, right, top, bottom, near, far); 197 198 199 // Refresh our video texture contents. We do it all at once in hopes of getting 200 // better coherence among images. This does not guarantee synchronization, of course... 201 for (auto&& cam: mActiveCameras) { 202 if (cam.tex) { 203 cam.tex->refresh(); 204 } 205 } 206 207 // Iterate over all the cameras and project their images onto the ground plane 208 for (auto&& cam: mActiveCameras) { 209 renderCameraOntoGroundPlane(cam); 210 } 211 212 // Draw the car image 213 renderCarTopView(); 214 215 // Wait for the rendering to finish 216 glFinish(); 217 218 return true; 219 } 220 221 222 // 223 // Responsible for drawing the car's self image in the top down view. 224 // Draws in car model space (units of meters with origin at center of rear axel) 225 // NOTE: We probably want to eventually switch to using a VertexArray based model system. 226 // 227 void RenderTopView::renderCarTopView() { 228 // Compute the corners of our image footprint in car space 229 const float carLengthInTexels = mConfig.carGraphicRearPixel() - mConfig.carGraphicFrontPixel(); 230 const float carSpaceUnitsPerTexel = mConfig.getCarLength() / carLengthInTexels; 231 const float textureHeightInCarSpace = mTexAssets.carTopView->height() * carSpaceUnitsPerTexel; 232 const float textureAspectRatio = (float)mTexAssets.carTopView->width() / 233 mTexAssets.carTopView->height(); 234 const float pixelsBehindCarInImage = mTexAssets.carTopView->height() - 235 mConfig.carGraphicRearPixel(); 236 const float textureExtentBehindCarInCarSpace = pixelsBehindCarInImage * carSpaceUnitsPerTexel; 237 238 const float btCS = mConfig.getRearLocation() - textureExtentBehindCarInCarSpace; 239 const float tpCS = textureHeightInCarSpace + btCS; 240 const float ltCS = 0.5f * textureHeightInCarSpace * textureAspectRatio; 241 const float rtCS = -ltCS; 242 243 GLfloat vertsCarPos[] = { ltCS, tpCS, 0.0f, // left top in car space 244 rtCS, tpCS, 0.0f, // right top 245 ltCS, btCS, 0.0f, // left bottom 246 rtCS, btCS, 0.0f // right bottom 247 }; 248 // NOTE: We didn't flip the image in the texture, so V=0 is actually the top of the image 249 GLfloat vertsCarTex[] = { 0.0f, 0.0f, // left top 250 1.0f, 0.0f, // right top 251 0.0f, 1.0f, // left bottom 252 1.0f, 1.0f // right bottom 253 }; 254 glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, vertsCarPos); 255 glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 0, vertsCarTex); 256 glEnableVertexAttribArray(0); 257 glEnableVertexAttribArray(1); 258 259 260 glEnable(GL_BLEND); 261 glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 262 263 glUseProgram(mPgmAssets.simpleTexture); 264 GLint loc = glGetUniformLocation(mPgmAssets.simpleTexture, "cameraMat"); 265 glUniformMatrix4fv(loc, 1, false, orthoMatrix.asArray()); 266 glBindTexture(GL_TEXTURE_2D, mTexAssets.carTopView->glId()); 267 268 glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); 269 270 271 glDisable(GL_BLEND); 272 273 glDisableVertexAttribArray(0); 274 glDisableVertexAttribArray(1); 275 } 276 277 278 // NOTE: Might be worth reviewing the ideas at 279 // http://math.stackexchange.com/questions/1691895/inverse-of-perspective-matrix 280 // to see if that simplifies the math, although we'll still want to compute the actual ground 281 // interception points taking into account the pitchLimit as below. 282 void RenderTopView::renderCameraOntoGroundPlane(const ActiveCamera& cam) { 283 // How far is the farthest any camera should even consider projecting it's image? 284 const float visibleSizeV = mConfig.getDisplayTopLocation() - mConfig.getDisplayBottomLocation(); 285 const float visibleSizeH = visibleSizeV * sAspectRatio; 286 const float maxRange = (visibleSizeH > visibleSizeV) ? visibleSizeH : visibleSizeV; 287 288 // Construct the projection matrix (View + Projection) associated with this sensor 289 // TODO: Consider just hard coding the far plane distance as it likely doesn't matter 290 const android::mat4 V = cameraLookMatrix(cam.info); 291 const android::mat4 P = perspective(cam.info.hfov, cam.info.vfov, cam.info.position[Z], maxRange); 292 const android::mat4 projectionMatix = P*V; 293 294 // Just draw the whole darn ground plane for now -- we're wasting fill rate, but so what? 295 // A 2x optimization would be to draw only the 1/2 space of the window in the direction 296 // the sensor is facing. A more complex solution would be to construct the intersection 297 // of the sensor volume with the ground plane and render only that geometry. 298 const float top = mConfig.getDisplayTopLocation(); 299 const float bottom = mConfig.getDisplayBottomLocation(); 300 const float wsHeight = top - bottom; 301 const float wsWidth = wsHeight * sAspectRatio; 302 const float right = wsWidth * 0.5f; 303 const float left = -right; 304 305 const android::vec3 topLeft(left, top, 0.0f); 306 const android::vec3 topRight(right, top, 0.0f); 307 const android::vec3 botLeft(left, bottom, 0.0f); 308 const android::vec3 botRight(right, bottom, 0.0f); 309 310 GLfloat vertsPos[] = { topLeft[X], topLeft[Y], topLeft[Z], 311 topRight[X], topRight[Y], topRight[Z], 312 botLeft[X], botLeft[Y], botLeft[Z], 313 botRight[X], botRight[Y], botRight[Z], 314 }; 315 glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, vertsPos); 316 glEnableVertexAttribArray(0); 317 318 319 glDisable(GL_BLEND); 320 321 glUseProgram(mPgmAssets.projectedTexture); 322 GLint locCam = glGetUniformLocation(mPgmAssets.projectedTexture, "cameraMat"); 323 glUniformMatrix4fv(locCam, 1, false, orthoMatrix.asArray()); 324 GLint locProj = glGetUniformLocation(mPgmAssets.projectedTexture, "projectionMat"); 325 glUniformMatrix4fv(locProj, 1, false, projectionMatix.asArray()); 326 327 GLuint texId; 328 if (cam.tex) { 329 texId = cam.tex->glId(); 330 } else { 331 texId = mTexAssets.checkerBoard->glId(); 332 } 333 glBindTexture(GL_TEXTURE_2D, texId); 334 335 glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); 336 337 338 glDisableVertexAttribArray(0); 339 } 340
