Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 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")); |
Scott Randolph | 2060b45 | 2017-10-17 18:48:29 +0100 | [diff] [blame] | 136 | if (!mTexAssets.checkerBoard) { |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 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")); |
Scott Randolph | 2060b45 | 2017-10-17 18:48:29 +0100 | [diff] [blame] | 144 | if (!mTexAssets.carTopView) { |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 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) { |
Changyeon Jo | fbf7279 | 2019-10-12 05:21:00 -0700 | [diff] [blame^] | 152 | cam.tex.reset(createVideoTexture(mEnumerator, cam.info.cameraId.c_str(), sDisplay)); |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 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 |
Haoxiang Li | 43ee1e8 | 2019-05-08 17:16:50 -0700 | [diff] [blame] | 168 | // TODO(b/131492626): investigate whether sharing video textures can save |
| 169 | // the time. |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 170 | for (auto&& cam: mActiveCameras) { |
| 171 | cam.tex = nullptr; |
| 172 | } |
| 173 | } |
| 174 | |
| 175 | |
| 176 | bool RenderTopView::drawFrame(const BufferDesc& tgtBuffer) { |
| 177 | // Tell GL to render to the given buffer |
| 178 | if (!attachRenderTarget(tgtBuffer)) { |
| 179 | ALOGE("Failed to attached render target"); |
| 180 | return false; |
| 181 | } |
| 182 | |
| 183 | // Set up our top down projection matrix from car space (world units, Xfwd, Yright, Zup) |
| 184 | // to view space (-1 to 1) |
| 185 | const float top = mConfig.getDisplayTopLocation(); |
| 186 | const float bottom = mConfig.getDisplayBottomLocation(); |
| 187 | const float right = mConfig.getDisplayRightLocation(sAspectRatio); |
| 188 | const float left = mConfig.getDisplayLeftLocation(sAspectRatio); |
| 189 | |
| 190 | const float near = 10.0f; // arbitrary top of view volume |
| 191 | const float far = 0.0f; // ground plane is at zero |
| 192 | |
| 193 | // We can use a simple, unrotated ortho view since the screen and car space axis are |
| 194 | // naturally aligned in the top down view. |
| 195 | // TODO: Not sure if flipping top/bottom here is "correct" or a double reverse... |
| 196 | // orthoMatrix = android::mat4::ortho(left, right, bottom, top, near, far); |
| 197 | orthoMatrix = android::mat4::ortho(left, right, top, bottom, near, far); |
| 198 | |
| 199 | |
| 200 | // Refresh our video texture contents. We do it all at once in hopes of getting |
| 201 | // better coherence among images. This does not guarantee synchronization, of course... |
| 202 | for (auto&& cam: mActiveCameras) { |
| 203 | if (cam.tex) { |
| 204 | cam.tex->refresh(); |
| 205 | } |
| 206 | } |
| 207 | |
| 208 | // Iterate over all the cameras and project their images onto the ground plane |
| 209 | for (auto&& cam: mActiveCameras) { |
| 210 | renderCameraOntoGroundPlane(cam); |
| 211 | } |
| 212 | |
| 213 | // Draw the car image |
| 214 | renderCarTopView(); |
| 215 | |
Scott Randolph | 788109f | 2018-07-12 13:10:36 -0700 | [diff] [blame] | 216 | // Now that everythign is submitted, release our hold on the texture resource |
| 217 | detachRenderTarget(); |
| 218 | |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 219 | // Wait for the rendering to finish |
| 220 | glFinish(); |
min.yun | 0a07980 | 2018-04-25 16:12:37 +0900 | [diff] [blame] | 221 | detachRenderTarget(); |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 222 | return true; |
| 223 | } |
| 224 | |
| 225 | |
| 226 | // |
| 227 | // Responsible for drawing the car's self image in the top down view. |
| 228 | // Draws in car model space (units of meters with origin at center of rear axel) |
| 229 | // NOTE: We probably want to eventually switch to using a VertexArray based model system. |
| 230 | // |
| 231 | void RenderTopView::renderCarTopView() { |
| 232 | // Compute the corners of our image footprint in car space |
| 233 | const float carLengthInTexels = mConfig.carGraphicRearPixel() - mConfig.carGraphicFrontPixel(); |
| 234 | const float carSpaceUnitsPerTexel = mConfig.getCarLength() / carLengthInTexels; |
| 235 | const float textureHeightInCarSpace = mTexAssets.carTopView->height() * carSpaceUnitsPerTexel; |
| 236 | const float textureAspectRatio = (float)mTexAssets.carTopView->width() / |
| 237 | mTexAssets.carTopView->height(); |
| 238 | const float pixelsBehindCarInImage = mTexAssets.carTopView->height() - |
| 239 | mConfig.carGraphicRearPixel(); |
| 240 | const float textureExtentBehindCarInCarSpace = pixelsBehindCarInImage * carSpaceUnitsPerTexel; |
| 241 | |
| 242 | const float btCS = mConfig.getRearLocation() - textureExtentBehindCarInCarSpace; |
| 243 | const float tpCS = textureHeightInCarSpace + btCS; |
| 244 | const float ltCS = 0.5f * textureHeightInCarSpace * textureAspectRatio; |
| 245 | const float rtCS = -ltCS; |
| 246 | |
| 247 | GLfloat vertsCarPos[] = { ltCS, tpCS, 0.0f, // left top in car space |
| 248 | rtCS, tpCS, 0.0f, // right top |
| 249 | ltCS, btCS, 0.0f, // left bottom |
| 250 | rtCS, btCS, 0.0f // right bottom |
| 251 | }; |
| 252 | // NOTE: We didn't flip the image in the texture, so V=0 is actually the top of the image |
| 253 | GLfloat vertsCarTex[] = { 0.0f, 0.0f, // left top |
| 254 | 1.0f, 0.0f, // right top |
| 255 | 0.0f, 1.0f, // left bottom |
| 256 | 1.0f, 1.0f // right bottom |
| 257 | }; |
| 258 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, vertsCarPos); |
| 259 | glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 0, vertsCarTex); |
| 260 | glEnableVertexAttribArray(0); |
| 261 | glEnableVertexAttribArray(1); |
| 262 | |
| 263 | |
| 264 | glEnable(GL_BLEND); |
| 265 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); |
| 266 | |
| 267 | glUseProgram(mPgmAssets.simpleTexture); |
| 268 | GLint loc = glGetUniformLocation(mPgmAssets.simpleTexture, "cameraMat"); |
| 269 | glUniformMatrix4fv(loc, 1, false, orthoMatrix.asArray()); |
| 270 | glBindTexture(GL_TEXTURE_2D, mTexAssets.carTopView->glId()); |
| 271 | |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 272 | glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); |
| 273 | |
| 274 | |
| 275 | glDisable(GL_BLEND); |
| 276 | |
| 277 | glDisableVertexAttribArray(0); |
| 278 | glDisableVertexAttribArray(1); |
| 279 | } |
| 280 | |
| 281 | |
| 282 | // NOTE: Might be worth reviewing the ideas at |
| 283 | // http://math.stackexchange.com/questions/1691895/inverse-of-perspective-matrix |
| 284 | // to see if that simplifies the math, although we'll still want to compute the actual ground |
| 285 | // interception points taking into account the pitchLimit as below. |
| 286 | void RenderTopView::renderCameraOntoGroundPlane(const ActiveCamera& cam) { |
| 287 | // How far is the farthest any camera should even consider projecting it's image? |
| 288 | const float visibleSizeV = mConfig.getDisplayTopLocation() - mConfig.getDisplayBottomLocation(); |
| 289 | const float visibleSizeH = visibleSizeV * sAspectRatio; |
| 290 | const float maxRange = (visibleSizeH > visibleSizeV) ? visibleSizeH : visibleSizeV; |
| 291 | |
| 292 | // Construct the projection matrix (View + Projection) associated with this sensor |
| 293 | // TODO: Consider just hard coding the far plane distance as it likely doesn't matter |
| 294 | const android::mat4 V = cameraLookMatrix(cam.info); |
| 295 | const android::mat4 P = perspective(cam.info.hfov, cam.info.vfov, cam.info.position[Z], maxRange); |
| 296 | const android::mat4 projectionMatix = P*V; |
| 297 | |
| 298 | // Just draw the whole darn ground plane for now -- we're wasting fill rate, but so what? |
| 299 | // A 2x optimization would be to draw only the 1/2 space of the window in the direction |
| 300 | // the sensor is facing. A more complex solution would be to construct the intersection |
| 301 | // of the sensor volume with the ground plane and render only that geometry. |
| 302 | const float top = mConfig.getDisplayTopLocation(); |
| 303 | const float bottom = mConfig.getDisplayBottomLocation(); |
| 304 | const float wsHeight = top - bottom; |
| 305 | const float wsWidth = wsHeight * sAspectRatio; |
| 306 | const float right = wsWidth * 0.5f; |
| 307 | const float left = -right; |
| 308 | |
| 309 | const android::vec3 topLeft(left, top, 0.0f); |
| 310 | const android::vec3 topRight(right, top, 0.0f); |
| 311 | const android::vec3 botLeft(left, bottom, 0.0f); |
| 312 | const android::vec3 botRight(right, bottom, 0.0f); |
| 313 | |
| 314 | GLfloat vertsPos[] = { topLeft[X], topLeft[Y], topLeft[Z], |
| 315 | topRight[X], topRight[Y], topRight[Z], |
| 316 | botLeft[X], botLeft[Y], botLeft[Z], |
| 317 | botRight[X], botRight[Y], botRight[Z], |
| 318 | }; |
| 319 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, vertsPos); |
| 320 | glEnableVertexAttribArray(0); |
| 321 | |
| 322 | |
| 323 | glDisable(GL_BLEND); |
| 324 | |
| 325 | glUseProgram(mPgmAssets.projectedTexture); |
| 326 | GLint locCam = glGetUniformLocation(mPgmAssets.projectedTexture, "cameraMat"); |
| 327 | glUniformMatrix4fv(locCam, 1, false, orthoMatrix.asArray()); |
| 328 | GLint locProj = glGetUniformLocation(mPgmAssets.projectedTexture, "projectionMat"); |
| 329 | glUniformMatrix4fv(locProj, 1, false, projectionMatix.asArray()); |
| 330 | |
| 331 | GLuint texId; |
| 332 | if (cam.tex) { |
| 333 | texId = cam.tex->glId(); |
| 334 | } else { |
| 335 | texId = mTexAssets.checkerBoard->glId(); |
| 336 | } |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 337 | glBindTexture(GL_TEXTURE_2D, texId); |
| 338 | |
| 339 | glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); |
Scott Randolph | 4a83b5d | 2017-05-16 17:42:19 -0700 | [diff] [blame] | 340 | |
| 341 | |
| 342 | glDisableVertexAttribArray(0); |
| 343 | } |