1 /* 2 * Copyright 2017 Google Inc. All Rights Reserved. 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 "platform_tools/android/apps/arcore/src/main/cpp/hello_ar_application.h" 18 #include <gtx/string_cast.hpp> 19 20 #include <math.h> /* acos */ 21 #include "include/core/SkCanvas.h" 22 #include "include/core/SkFontStyle.h" 23 #include "include/core/SkMatrix.h" 24 #include "include/core/SkMatrix44.h" 25 #include "include/core/SkPoint3.h" 26 #include "include/core/SkStream.h" 27 #include "include/core/SkSurface.h" 28 #include "include/core/SkTextBlob.h" 29 #include "include/core/SkTypeface.h" 30 #include "include/gpu/GrBackendSurface.h" 31 #include "include/gpu/GrContext.h" 32 #include "include/gpu/gl/GrGLTypes.h" 33 #include "include/utils/Sk3D.h" 34 #include "modules/skottie/include/Skottie.h" 35 #include "modules/skshaper/include/SkShaper.h" 36 #include "platform_tools/android/apps/arcore/src/main/cpp/anchor_wrapper.h" 37 #include "platform_tools/android/apps/arcore/src/main/cpp/glm.h" 38 #include "platform_tools/android/apps/arcore/src/main/cpp/pending_anchor.h" 39 #include "platform_tools/android/apps/arcore/src/main/cpp/plane_renderer.h" 40 #include "platform_tools/android/apps/arcore/src/main/cpp/util.h" 41 #include "tools/Resources.h" 42 43 namespace hello_ar { 44 namespace { 45 constexpr size_t kMaxNumberOfAndroidsToRender = 1; 46 constexpr int32_t kPlaneColorRgbaSize = 16; 47 48 const glm::vec3 kWhite = {255, 255, 255}; 49 50 constexpr std::array<uint32_t, kPlaneColorRgbaSize> kPlaneColorRgba = { 51 {0xFFFFFFFF, 0xF44336FF, 0xE91E63FF, 0x9C27B0FF, 0x673AB7FF, 0x3F51B5FF, 52 0x2196F3FF, 0x03A9F4FF, 0x00BCD4FF, 0x009688FF, 0x4CAF50FF, 0x8BC34AFF, 53 0xCDDC39FF, 0xFFEB3BFF, 0xFFC107FF, 0xFF9800FF}}; 54 GetRandomPlaneColor()55 inline glm::vec3 GetRandomPlaneColor() { 56 const int32_t colorRgba = kPlaneColorRgba[std::rand() % kPlaneColorRgbaSize]; 57 return glm::vec3(((colorRgba >> 24) & 0xff) / 255.0f, 58 ((colorRgba >> 16) & 0xff) / 255.0f, 59 ((colorRgba >> 8) & 0xff) / 255.0f); 60 } 61 } // namespace 62 HelloArApplication(AAssetManager * asset_manager)63 HelloArApplication::HelloArApplication(AAssetManager *asset_manager) 64 : asset_manager_(asset_manager) { 65 LOGI("OnCreate()"); 66 } 67 ~HelloArApplication()68 HelloArApplication::~HelloArApplication() { 69 if (ar_session_ != nullptr) { 70 ArSession_destroy(ar_session_); 71 ArFrame_destroy(ar_frame_); 72 } 73 } 74 OnPause()75 void HelloArApplication::OnPause() { 76 LOGI("OnPause()"); 77 if (ar_session_ != nullptr) { 78 ArSession_pause(ar_session_); 79 } 80 } 81 OnResume(void * env,void * context,void * activity)82 void HelloArApplication::OnResume(void *env, void *context, void *activity) { 83 LOGI("OnResume()"); 84 85 if (ar_session_ == nullptr) { 86 ArInstallStatus install_status; 87 // If install was not yet requested, that means that we are resuming the 88 // activity first time because of explicit user interaction (such as 89 // launching the application) 90 bool user_requested_install = !install_requested_; 91 92 // === ATTENTION! ATTENTION! ATTENTION! === 93 // This method can and will fail in user-facing situations. Your 94 // application must handle these cases at least somewhat gracefully. See 95 // HelloAR Java sample code for reasonable behavior. 96 CHECK(ArCoreApk_requestInstall(env, activity, user_requested_install, 97 &install_status) == AR_SUCCESS); 98 99 switch (install_status) { 100 case AR_INSTALL_STATUS_INSTALLED: 101 break; 102 case AR_INSTALL_STATUS_INSTALL_REQUESTED: 103 install_requested_ = true; 104 return; 105 } 106 107 // === ATTENTION! ATTENTION! ATTENTION! === 108 // This method can and will fail in user-facing situations. Your 109 // application must handle these cases at least somewhat gracefully. See 110 // HelloAR Java sample code for reasonable behavior. 111 CHECK(ArSession_create(env, context, &ar_session_) == AR_SUCCESS); 112 CHECK(ar_session_); 113 114 ArFrame_create(ar_session_, &ar_frame_); 115 CHECK(ar_frame_); 116 117 ArSession_setDisplayGeometry(ar_session_, display_rotation_, width_, 118 height_); 119 } 120 121 const ArStatus status = ArSession_resume(ar_session_); 122 CHECK(status == AR_SUCCESS); 123 } 124 OnSurfaceCreated()125 void HelloArApplication::OnSurfaceCreated() { 126 LOGI("OnSurfaceCreated()"); 127 128 background_renderer_.InitializeGlContent(); 129 point_cloud_renderer_.InitializeGlContent(); 130 plane_renderer_.InitializeGlContent(asset_manager_); 131 } 132 OnDisplayGeometryChanged(int display_rotation,int width,int height)133 void HelloArApplication::OnDisplayGeometryChanged(int display_rotation, 134 int width, int height) { 135 LOGI("OnSurfaceChanged(%d, %d)", width, height); 136 glViewport(0, 0, width, height); 137 display_rotation_ = display_rotation; 138 width_ = width; 139 height_ = height; 140 141 if (ar_session_ != nullptr) { 142 ArSession_setDisplayGeometry(ar_session_, display_rotation, width, height); 143 } 144 } 145 OnObjectRotationChanged(int rotation)146 void HelloArApplication::OnObjectRotationChanged(int rotation) { 147 LOGI("OnObjectRotationChanged(%d)", rotation); 148 currentObjectRotation = rotation; 149 } 150 OnAction(float value)151 void HelloArApplication::OnAction(float value) { 152 LOGI("OnAction(%.6f)", value); 153 currentValue = value; 154 } 155 DrawText(SkCanvas * canvas,SkPaint * paint,const char text[])156 void DrawText(SkCanvas *canvas, SkPaint *paint, const char text[]) { 157 float spacing = 0.05; 158 for (int i = 0; i < sizeof(text) / sizeof(text[0]); i++) { 159 const char letter[] = {text[i]}; 160 size_t byteLength = strlen(static_cast<const char *>(letter)); 161 canvas->drawText(letter, byteLength, spacing * i, 0, *paint); 162 } 163 } 164 DrawAxes(SkCanvas * canvas,SkMatrix44 m)165 void DrawAxes(SkCanvas *canvas, SkMatrix44 m) { 166 SkPaint p; 167 p.setStrokeWidth(10); 168 SkPoint3 src[4] = { 169 {0, 0, 0}, 170 {0.2, 0, 0}, 171 {0, 0.2, 0}, 172 {0, 0, 0.2}, 173 }; 174 SkPoint dst[4]; 175 Sk3MapPts(dst, m, src, 4); 176 177 const char str[] = "XYZ"; 178 p.setColor(SK_ColorRED); 179 canvas->drawLine(dst[0], dst[1], p); 180 181 p.setColor(SK_ColorGREEN); 182 canvas->drawLine(dst[0], dst[2], p); 183 184 p.setColor(SK_ColorBLUE); 185 canvas->drawLine(dst[0], dst[3], p); 186 } 187 DrawVector(SkCanvas * canvas,SkMatrix44 m,glm::vec3 begin,glm::vec3 end,SkColor c)188 void DrawVector(SkCanvas *canvas, SkMatrix44 m, glm::vec3 begin, glm::vec3 end, SkColor c) { 189 SkPaint p; 190 p.setStrokeWidth(15); 191 SkPoint3 src[2] = { 192 {begin.x, begin.y, begin.z}, 193 {end.x, end.y, end.z} 194 }; 195 SkPoint dst[2]; 196 Sk3MapPts(dst, m, src, 2); 197 198 const char str[] = "XYZ"; 199 p.setColor(c); 200 canvas->drawLine(dst[0], dst[1], p); 201 } 202 DrawBoundingBox(SkCanvas * canvas)203 void DrawBoundingBox(SkCanvas* canvas) { 204 SkPaint paint; 205 paint.setColor(SK_ColorYELLOW); 206 SkIRect bounds = canvas->getDeviceClipBounds(); 207 SkRect b = SkRect::Make(bounds); 208 209 canvas->drawRect(b, paint); 210 } 211 OnDrawFrame()212 void HelloArApplication::OnDrawFrame() { 213 grContext = GrContext::MakeGL(); 214 215 GrBackendRenderTarget target; 216 sk_sp<SkSurface> surface = nullptr; 217 GrGLFramebufferInfo framebuffer_info; 218 framebuffer_info.fFBOID = 0; 219 framebuffer_info.fFormat = 0x8058; 220 221 222 glClearColor(0.9f, 0.9f, 0.9f, 1.0f); 223 glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT); 224 225 glEnable(GL_CULL_FACE); 226 glEnable(GL_DEPTH_TEST); 227 glEnable(GL_BLEND); 228 glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 229 230 if (ar_session_ == nullptr) return; 231 232 ArSession_setCameraTextureName(ar_session_, 233 background_renderer_.GetTextureId()); 234 235 // Update session to get current frame and render camera background. 236 if (ArSession_update(ar_session_, ar_frame_) != AR_SUCCESS) { 237 LOGE("HelloArApplication::OnDrawFrame ArSession_update error"); 238 } 239 240 // GET CAMERA INFO 241 ArCamera *ar_camera; 242 ArFrame_acquireCamera(ar_session_, ar_frame_, &ar_camera); 243 244 glm::mat4 view_mat; 245 glm::mat4 projection_mat; 246 ArCamera_getViewMatrix(ar_session_, ar_camera, glm::value_ptr(view_mat)); 247 ArCamera_getProjectionMatrix(ar_session_, ar_camera, 248 /*near=*/0.1f, /*far=*/100.f, 249 glm::value_ptr(projection_mat)); 250 251 ArTrackingState camera_tracking_state; 252 ArCamera_getTrackingState(ar_session_, ar_camera, &camera_tracking_state); 253 ArCamera_release(ar_camera); 254 255 background_renderer_.Draw(ar_session_, ar_frame_); 256 257 // If the camera isn't tracking don't bother rendering other objects. 258 if (camera_tracking_state != AR_TRACKING_STATE_TRACKING) { 259 return; 260 } 261 262 // Get light estimation value. 263 ArLightEstimate *ar_light_estimate; 264 ArLightEstimateState ar_light_estimate_state; 265 ArLightEstimate_create(ar_session_, &ar_light_estimate); 266 267 ArFrame_getLightEstimate(ar_session_, ar_frame_, ar_light_estimate); 268 ArLightEstimate_getState(ar_session_, ar_light_estimate, 269 &ar_light_estimate_state); 270 271 // Set light intensity to default. Intensity value ranges from 0.0f to 1.0f. 272 // The first three components are color scaling factors. 273 // The last one is the average pixel intensity in gamma space. 274 float color_correction[4] = {1.f, 1.f, 1.f, 1.f}; 275 if (ar_light_estimate_state == AR_LIGHT_ESTIMATE_STATE_VALID) { 276 ArLightEstimate_getColorCorrection(ar_session_, ar_light_estimate, 277 color_correction); 278 } 279 280 ArLightEstimate_destroy(ar_light_estimate); 281 ar_light_estimate = nullptr; 282 SkMatrix44 skProj; 283 SkMatrix44 skView; 284 SkMatrix skViewport; 285 286 skProj = util::GlmMatToSkMat(projection_mat); 287 skView = util::GlmMatToSkMat(view_mat); 288 skViewport.setScale(width_ / 2, -height_ / 2); 289 skViewport.postTranslate(width_ / 2, height_ / 2); 290 target = GrBackendRenderTarget(width_, height_, 0, 0, framebuffer_info); 291 surface = SkSurface::MakeFromBackendRenderTarget(grContext.get(), 292 target, 293 kBottomLeft_GrSurfaceOrigin, 294 kRGBA_8888_SkColorType, 295 nullptr, nullptr); 296 297 // Render Andy objects. 298 std::vector<SkMatrix44> models; 299 //glm::mat4 model_mat(1.0f); 300 for (const auto &obj_iter : tracked_obj_set_) { 301 ArTrackingState tracking_state = AR_TRACKING_STATE_STOPPED; 302 ArAnchor_getTrackingState(ar_session_, obj_iter, &tracking_state); 303 if (tracking_state == AR_TRACKING_STATE_TRACKING) { 304 // Render object only if the tracking state is AR_TRACKING_STATE_TRACKING. 305 //util::GetTransformMatrixFromAnchor(ar_session_, obj_iter, &model_mat); 306 //DRAW ANDY 307 //andy_renderer_.Draw(glm::mat4(1), glm::mat4(1), model_mat, color_correction); 308 309 //PREPARE SKIA MATS 310 311 SkMatrix44 skModel; 312 313 switch (currentObjectRotation) { 314 case 0: { 315 auto iter = anchor_skmat4_axis_aligned_map_.find(obj_iter); 316 if (iter != anchor_skmat4_axis_aligned_map_.end()) { 317 skModel = iter->second; 318 models.push_back(skModel); 319 } 320 } 321 break; 322 case 1: { 323 auto iter = anchor_skmat4_camera_aligned_map_.find(obj_iter); 324 if (iter != anchor_skmat4_camera_aligned_map_.end()) { 325 skModel = iter->second; 326 models.push_back(skModel); 327 } 328 } 329 break; 330 case 2: { 331 auto iter = anchor_skmat4_snap_aligned_map_.find(obj_iter); 332 if (iter != anchor_skmat4_snap_aligned_map_.end()) { 333 skModel = iter->second; 334 models.push_back(skModel); 335 } 336 } 337 break; 338 default: { 339 auto iter = anchor_skmat4_axis_aligned_map_.find(obj_iter); 340 if (iter != anchor_skmat4_axis_aligned_map_.end()) { 341 skModel = iter->second; 342 models.push_back(skModel); 343 } 344 } 345 break; 346 } 347 348 } 349 } 350 351 // Update and render planes. 352 ArTrackableList *plane_list = nullptr; 353 ArTrackableList_create(ar_session_, &plane_list); 354 CHECK(plane_list != nullptr); 355 356 ArTrackableType plane_tracked_type = AR_TRACKABLE_PLANE; 357 ArSession_getAllTrackables(ar_session_, plane_tracked_type, plane_list); 358 359 int32_t plane_list_size = 0; 360 ArTrackableList_getSize(ar_session_, plane_list, &plane_list_size); 361 plane_count_ = plane_list_size; 362 363 for (int i = 0; i < plane_list_size; ++i) { 364 ArTrackable *ar_trackable = nullptr; 365 ArTrackableList_acquireItem(ar_session_, plane_list, i, &ar_trackable); 366 ArPlane *ar_plane = ArAsPlane(ar_trackable); 367 ArTrackingState out_tracking_state; 368 ArTrackable_getTrackingState(ar_session_, ar_trackable, 369 &out_tracking_state); 370 371 ArPlane *subsume_plane; 372 ArPlane_acquireSubsumedBy(ar_session_, ar_plane, &subsume_plane); 373 if (subsume_plane != nullptr) { 374 ArTrackable_release(ArAsTrackable(subsume_plane)); 375 continue; 376 } 377 378 if (ArTrackingState::AR_TRACKING_STATE_TRACKING != out_tracking_state) { 379 continue; 380 } 381 382 ArTrackingState plane_tracking_state; 383 ArTrackable_getTrackingState(ar_session_, ArAsTrackable(ar_plane), 384 &plane_tracking_state); 385 if (plane_tracking_state == AR_TRACKING_STATE_TRACKING) { 386 const auto iter = plane_color_map_.find(ar_plane); 387 glm::vec3 color; 388 if (iter != plane_color_map_.end()) { 389 color = iter->second; 390 391 // If this is an already observed trackable release it so it doesn't 392 // leave aof placing objects on surfaces (n additional reference dangling. 393 ArTrackable_release(ar_trackable); 394 } else { 395 // The first plane is always white. 396 if (!first_plane_has_been_found_) { 397 first_plane_has_been_found_ = true; 398 color = kWhite; 399 } else { 400 color = GetRandomPlaneColor(); 401 } 402 plane_color_map_.insert({ar_plane, color}); 403 } 404 405 plane_renderer_.Draw(projection_mat, view_mat, ar_session_, ar_plane, 406 color); 407 } 408 } 409 410 ArTrackableList_destroy(plane_list); 411 plane_list = nullptr; 412 413 // Update and render point cloud. 414 ArPointCloud *ar_point_cloud = nullptr; 415 ArStatus point_cloud_status = 416 ArFrame_acquirePointCloud(ar_session_, ar_frame_, &ar_point_cloud); 417 if (point_cloud_status == AR_SUCCESS) { 418 point_cloud_renderer_.Draw(projection_mat * view_mat, ar_session_, 419 ar_point_cloud); 420 ArPointCloud_release(ar_point_cloud); 421 } 422 SkMatrix44 i = SkMatrix44::kIdentity_Constructor; 423 424 if (surface != nullptr) { 425 SkCanvas *canvas = surface->getCanvas(); 426 SkAutoCanvasRestore acr(canvas, true); 427 SkMatrix44 vpv = skViewport * skProj * skView; 428 for(SkMatrix44 skModel: models) { 429 SkMatrix44 i = SkMatrix44::kIdentity_Constructor; 430 canvas->setMatrix(i); 431 SkMatrix44 mvpv = skViewport * skProj * skView * skModel; 432 433 //Draw XYZ axes 434 DrawAxes(canvas, mvpv); 435 //Drawing camera orientation 436 /* DrawVector(canvas, vpv, begins[0], ends[0], SK_ColorMAGENTA); 437 DrawVector(canvas, vpv, begins[0], ends[1], SK_ColorYELLOW); 438 DrawVector(canvas, vpv, begins[0], ends[2], SK_ColorCYAN);*/ 439 440 canvas->concat(mvpv); 441 SkPaint paint; 442 443 //Draw Circle 444 paint.setColor(0x80700000); 445 canvas->drawCircle(0, 0, 0.1, paint); 446 447 //Draw Text 448 paint.setColor(SK_ColorBLUE); 449 if (currentValue != 0) { 450 paint.setTextSize(currentValue); 451 } else { 452 paint.setTextSize(0.1); 453 } 454 455 paint.setAntiAlias(true); 456 const char text[] = "SkAR"; 457 size_t byteLength = strlen(static_cast<const char *>(text)); 458 SkShaper shaper(nullptr); 459 SkTextBlobBuilder builder; 460 SkPoint p = SkPoint::Make(0, 0); 461 shaper.shape(&builder, paint, text, byteLength, true, p, 10); 462 canvas->drawTextBlob(builder.make(), 0, 0, paint); 463 464 //DrawBoundingBox(canvas); 465 } 466 canvas->flush(); 467 } 468 } 469 470 OnTouchedFirst(float x,float y,int drawMode)471 bool HelloArApplication::OnTouchedFirst(float x, float y, int drawMode) { 472 LOGI("Entered OnTouchedFirst"); 473 if (pendingAnchor != nullptr) { 474 delete pendingAnchor; 475 } 476 SkPoint p = SkPoint::Make(x,y); 477 pendingAnchor = new PendingAnchor(p); 478 bool editAnchor = false; 479 480 if (ar_frame_ != nullptr && ar_session_ != nullptr) { 481 ArHitResultList *hit_result_list = nullptr; 482 ArHitResultList_create(ar_session_, &hit_result_list); 483 CHECK(hit_result_list); 484 ArFrame_hitTest(ar_session_, ar_frame_, x, y, hit_result_list); 485 486 int32_t hit_result_list_size = 0; 487 ArHitResultList_getSize(ar_session_, hit_result_list, &hit_result_list_size); 488 ArHitResult *ar_hit_result = nullptr; 489 ArPose *out_pose = nullptr; 490 ArPlane* hitPlane = nullptr; 491 for (int32_t i = 0; i < hit_result_list_size; ++i) { 492 ArHitResult *ar_hit = nullptr; 493 ArPose *created_out_pose = nullptr; 494 ArHitResult_create(ar_session_, &ar_hit); 495 ArHitResultList_getItem(ar_session_, hit_result_list, i, ar_hit); 496 497 if (ar_hit == nullptr) { 498 LOGE("HelloArApplication::OnTouched ArHitResultList_getItem error"); 499 return editAnchor; 500 } 501 502 ArTrackable *ar_trackable = nullptr; 503 ArHitResult_acquireTrackable(ar_session_, ar_hit, &ar_trackable); 504 ArTrackableType ar_trackable_type = AR_TRACKABLE_NOT_VALID; 505 ArTrackable_getType(ar_session_, ar_trackable, &ar_trackable_type); 506 // Creates an anchor if a plane or an oriented point was hit. 507 if (AR_TRACKABLE_PLANE == ar_trackable_type) { 508 ArPose *hit_pose = nullptr; 509 ArPose_create(ar_session_, nullptr, &hit_pose); 510 ArHitResult_getHitPose(ar_session_, ar_hit, hit_pose); 511 int32_t in_polygon = 0; 512 ArPlane *ar_plane = ArAsPlane(ar_trackable); 513 ArPlane_isPoseInPolygon(ar_session_, ar_plane, hit_pose, &in_polygon); 514 515 { 516 // Use hit pose and camera pose to check if hittest is from the 517 // back of the plane, if it is, no need to create the anchor. 518 ArPose *camera_pose = nullptr; 519 ArPose_create(ar_session_, nullptr, &camera_pose); 520 ArCamera *ar_camera; 521 ArFrame_acquireCamera(ar_session_, ar_frame_, &ar_camera); 522 ArCamera_getPose(ar_session_, ar_camera, camera_pose); 523 float normal_distance_to_plane = util::CalculateDistanceToPlane( 524 ar_session_, *hit_pose, *camera_pose); 525 526 if (!in_polygon || normal_distance_to_plane < 0) { 527 ArPose_destroy(camera_pose); 528 continue; 529 } 530 ArPose_destroy(camera_pose); 531 ArCamera_release(ar_camera); 532 } 533 534 //Raw pose of hit location 535 float out_hit_raw[] = {0, 0, 0, 0, 0, 0, 0}; 536 ArPose_getPoseRaw(ar_session_, hit_pose, out_hit_raw); 537 ArPose_destroy(hit_pose); 538 539 //Position of anchor 540 glm::vec4 pendingAnchorPos(out_hit_raw[4], out_hit_raw[5], out_hit_raw[6], 1); 541 pendingAnchor->SetContainingPlane(ar_plane); 542 543 //Check if plane contains approx the same anchor 544 auto planeAnchors = plane_anchors_map_.find(ar_plane); 545 if (planeAnchors != plane_anchors_map_.end()) { 546 //other anchors existed on this plane 547 std::vector<ArAnchor*> anchors = planeAnchors->second; 548 int i = 0; 549 LOGI("Size of anchor list: %d", (int) anchors.size()); 550 for(ArAnchor* const& anchor: anchors) { 551 //Get anchor's pose 552 i++; 553 LOGI("CHECKING: Anchor #%d", i); 554 ArPose *anchor_pose = nullptr; 555 ArPose_create(ar_session_, nullptr, &anchor_pose); 556 ArAnchor_getPose(ar_session_, anchor, anchor_pose); 557 float out_anchor_raw[] = {0, 0, 0, 0, 0, 0, 0}; 558 ArPose_getPoseRaw(ar_session_, anchor_pose, out_anchor_raw); 559 ArPose_destroy(anchor_pose); 560 glm::vec4 oldAnchorPos(out_anchor_raw[4], out_anchor_raw[5], out_anchor_raw[6], 1); 561 oldAnchorPos = oldAnchorPos - pendingAnchorPos; 562 float distance = util::Magnitude(glm::vec3(oldAnchorPos)); 563 if (distance < 0.1f) { 564 LOGI("TouchFirst: Editing old anchor!"); 565 editAnchor = true; 566 pendingAnchor->SetArAnchor(anchor); 567 pendingAnchor->SetEditMode(true); 568 569 ArHitResult_destroy(ar_hit); 570 ArHitResultList_destroy(hit_result_list); 571 LOGI("TouchFirst: Edit %d", editAnchor); 572 return editAnchor; 573 } 574 } 575 } 576 577 //actual hit result, and containing plane 578 ar_hit_result = ar_hit; 579 hitPlane = ar_plane; 580 581 //new anchor pos 582 float wanted_raw_pose[] = {0, 0, 0, 0, out_hit_raw[4], out_hit_raw[5], out_hit_raw[6]}; 583 ArPose_create(ar_session_, wanted_raw_pose, &created_out_pose); 584 out_pose = created_out_pose; 585 break; 586 } 587 } 588 589 590 if (ar_hit_result) { 591 LOGI("TouchFirst: Adding new anchor!"); 592 ArAnchor *anchor = nullptr; 593 pendingAnchor->SetEditMode(false); 594 595 if (ArSession_acquireNewAnchor(ar_session_, out_pose, &anchor) != AR_SUCCESS) { 596 LOGE("HelloArApplication::OnTouched ArHitResult_acquireNewAnchor error"); 597 LOGI("TouchFirst: Failed to acquire new anchor"); 598 delete hitPlane; 599 delete pendingAnchor; 600 pendingAnchor = nullptr; 601 LOGI("TouchFirst: Edit %d", editAnchor); 602 return editAnchor; 603 } 604 pendingAnchor->SetArAnchor(anchor); 605 606 ArHitResult_destroy(ar_hit_result); 607 ArHitResultList_destroy(hit_result_list); 608 ArPose_destroy(out_pose); 609 hit_result_list = nullptr; 610 LOGI("TouchFirst: Edit %d", editAnchor); 611 return editAnchor; 612 } 613 614 LOGI("TouchFirst: didn't hit anything"); 615 delete hitPlane; 616 delete pendingAnchor; 617 pendingAnchor = nullptr; 618 LOGI("TouchFirst: Edit %d", editAnchor); 619 return editAnchor; 620 } 621 } 622 AddAnchor(ArAnchor * anchor,ArPlane * containingPlane)623 void HelloArApplication::AddAnchor(ArAnchor* anchor, ArPlane* containingPlane) { 624 //delete anchor from matrices maps 625 //releasing the anchor if it is not tracking anymore 626 ArTrackingState tracking_state = AR_TRACKING_STATE_STOPPED; 627 ArAnchor_getTrackingState(ar_session_, anchor, &tracking_state); 628 if (tracking_state != AR_TRACKING_STATE_TRACKING) { 629 RemoveAnchor(anchor); 630 return; 631 } 632 633 //releasing the first anchor if we exceeded maximum number of objects to be rendered 634 if (tracked_obj_set_.size() >= kMaxNumberOfAndroidsToRender) { 635 RemoveAnchor(tracked_obj_set_[0]); 636 } 637 638 //updating the containing plane with a new anchor 639 auto planeAnchors = plane_anchors_map_.find(containingPlane); 640 if (planeAnchors != plane_anchors_map_.end()) { 641 //other anchors existed on this plane 642 LOGI("TouchFinal: ADDING TO OLD ANCHORS"); 643 std::vector<ArAnchor*> anchors = planeAnchors->second; 644 anchors.push_back(anchor); 645 plane_anchors_map_[containingPlane] = anchors; 646 anchor_plane_map_.insert({anchor, containingPlane}); 647 } else { 648 LOGI("TouchFinal: NEW SET OF ANCHORS"); 649 std::vector<ArAnchor*> anchors; 650 anchors.push_back(anchor); 651 plane_anchors_map_.insert({containingPlane, anchors}); 652 anchor_plane_map_.insert({anchor, containingPlane}); 653 } 654 655 tracked_obj_set_.push_back(anchor); 656 } 657 OnTouchTranslate(float x,float y)658 void HelloArApplication::OnTouchTranslate(float x, float y) { 659 LOGI("Entered On Edit Touched"); 660 ArAnchor *anchor = pendingAnchor->GetArAnchor(); 661 glm::mat4 matrix = util::SkMatToGlmMat( 662 anchor_skmat4_axis_aligned_map_.find(anchor)->second); 663 664 if (ar_frame_ != nullptr && ar_session_ != nullptr) { 665 ArHitResultList *hit_result_list = nullptr; 666 ArHitResultList_create(ar_session_, &hit_result_list); 667 CHECK(hit_result_list); 668 ArFrame_hitTest(ar_session_, ar_frame_, x, y, hit_result_list); 669 670 int32_t hit_result_list_size = 0; 671 ArHitResultList_getSize(ar_session_, hit_result_list, &hit_result_list_size); 672 ArHitResult *ar_hit_result = nullptr; 673 ArPose *out_pose = nullptr; 674 ArPlane *hitPlane = nullptr; 675 for (int32_t i = 0; i < hit_result_list_size; ++i) { 676 ArHitResult *ar_hit = nullptr; 677 ArPose *created_out_pose = nullptr; 678 ArHitResult_create(ar_session_, &ar_hit); 679 ArHitResultList_getItem(ar_session_, hit_result_list, i, ar_hit); 680 681 if (ar_hit == nullptr) { 682 LOGE("HelloArApplication::OnTouched ArHitResultList_getItem error"); 683 return; 684 } 685 686 ArTrackable *ar_trackable = nullptr; 687 ArHitResult_acquireTrackable(ar_session_, ar_hit, &ar_trackable); 688 ArTrackableType ar_trackable_type = AR_TRACKABLE_NOT_VALID; 689 ArTrackable_getType(ar_session_, ar_trackable, &ar_trackable_type); 690 // Creates an anchor if a plane or an oriented point was hit. 691 if (AR_TRACKABLE_PLANE == ar_trackable_type) { 692 ArPose *hit_pose = nullptr; 693 ArPose_create(ar_session_, nullptr, &hit_pose); 694 ArHitResult_getHitPose(ar_session_, ar_hit, hit_pose); 695 int32_t in_polygon = 0; 696 ArPlane *ar_plane = ArAsPlane(ar_trackable); 697 ArPlane_isPoseInPolygon(ar_session_, ar_plane, hit_pose, &in_polygon); 698 699 { 700 // Use hit pose and camera pose to check if hittest is from the 701 // back of the plane, if it is, no need to create the anchor. 702 ArPose *camera_pose = nullptr; 703 ArPose_create(ar_session_, nullptr, &camera_pose); 704 ArCamera *ar_camera; 705 ArFrame_acquireCamera(ar_session_, ar_frame_, &ar_camera); 706 ArCamera_getPose(ar_session_, ar_camera, camera_pose); 707 float normal_distance_to_plane = util::CalculateDistanceToPlane( 708 ar_session_, *hit_pose, *camera_pose); 709 710 if (!in_polygon || normal_distance_to_plane < 0) { 711 ArPose_destroy(camera_pose); 712 continue; 713 } 714 ArPose_destroy(camera_pose); 715 ArCamera_release(ar_camera); 716 } 717 718 //Raw pose of hit location 719 float out_hit_raw[] = {0, 0, 0, 0, 0, 0, 0}; 720 ArPose_getPoseRaw(ar_session_, hit_pose, out_hit_raw); 721 ArPose_destroy(hit_pose); 722 723 //Translate by new amount 724 glm::vec4 newPos(out_hit_raw[4], out_hit_raw[5], out_hit_raw[6], 1); 725 glm::vec4 oldPos = pendingAnchor->GetAnchorPos(ar_session_); 726 glm::vec3 movement = glm::vec3(newPos - oldPos); 727 728 729 //CAMERA SETTINGS 730 glm::mat4 backToOrigin(1); 731 backToOrigin = glm::translate(backToOrigin, -glm::vec3(oldPos)); 732 glm::mat4 backToPlane(1); 733 backToPlane = glm::translate(backToPlane, glm::vec3(oldPos)); 734 735 //Axes of Skia object: start with XYZ, totate to get X(-Z)Y, paste on plane, go back to origin --> plane orientation but on origin 736 glm::vec3 objX = glm::normalize(glm::vec3( 737 backToOrigin * matrix * 738 glm::vec4(1, 0, 0, 1))); //X still X 739 glm::vec3 objY = glm::normalize(glm::vec3( 740 backToOrigin * matrix * 741 glm::vec4(0, 1, 0, 1))); //Y is now Z 742 glm::vec3 objZ = glm::normalize(glm::vec3( 743 backToOrigin * matrix * 744 glm::vec4(0, 0, 1, 1))); //Z is now Y 745 746 747 glm::mat4 translate(1); 748 translate = glm::translate(translate, movement); 749 matrix = translate * matrix; 750 RemoveAnchor(anchor); 751 752 753 754 //new anchor pos 755 float wanted_raw_pose[] = {0, 0, 0, 0, out_hit_raw[4], out_hit_raw[5], 756 out_hit_raw[6]}; 757 ArPose_create(ar_session_, wanted_raw_pose, &created_out_pose); 758 out_pose = created_out_pose; 759 ar_hit_result = ar_hit; 760 break; 761 } 762 } 763 764 if (ar_hit_result) { 765 LOGI("TouchFirst: Adding new anchor!"); 766 ArAnchor *anchor = nullptr; 767 pendingAnchor->SetEditMode(false); 768 769 if (ArSession_acquireNewAnchor(ar_session_, out_pose, &anchor) != AR_SUCCESS) { 770 LOGE("HelloArApplication::OnTouched ArHitResult_acquireNewAnchor error"); 771 LOGI("TouchFirst: Failed to acquire new anchor"); 772 delete hitPlane; 773 delete pendingAnchor; 774 pendingAnchor = nullptr; 775 return; 776 } 777 pendingAnchor->SetArAnchor(anchor); 778 anchor_skmat4_axis_aligned_map_[anchor] = util::GlmMatToSkMat(matrix); 779 780 //Add anchor 781 AddAnchor(anchor, pendingAnchor->GetContainingPlane()); 782 783 784 ArHitResult_destroy(ar_hit_result); 785 ArHitResultList_destroy(hit_result_list); 786 ArPose_destroy(out_pose); 787 hit_result_list = nullptr; 788 return; 789 } 790 } 791 } 792 RemoveAnchor(ArAnchor * anchor)793 void HelloArApplication::RemoveAnchor(ArAnchor* anchor) { 794 //delete anchor from matrices maps 795 anchor_skmat4_axis_aligned_map_.erase(anchor); 796 anchor_skmat4_camera_aligned_map_.erase(anchor); 797 anchor_skmat4_snap_aligned_map_.erase(anchor); 798 799 auto containingPlaneIter = anchor_plane_map_.find(anchor); 800 if (containingPlaneIter != anchor_plane_map_.end()) { 801 ArPlane* containingPlane = containingPlaneIter->second; 802 auto planeAnchors = plane_anchors_map_.find(containingPlane); 803 if (planeAnchors != plane_anchors_map_.end()) { 804 //delete this anchor from the list of anchors associated with its plane 805 std::vector<ArAnchor*> anchors = planeAnchors->second; 806 anchors.erase(std::remove(anchors.begin(), anchors.end(), anchor), anchors.end()); 807 plane_anchors_map_[planeAnchors->first] = anchors; 808 809 //delete anchor from map of anchor to plane 810 anchor_plane_map_.erase(anchor); 811 } 812 } 813 //delete anchor from list of tracked objects 814 tracked_obj_set_.erase(std::remove(tracked_obj_set_.begin(), tracked_obj_set_.end(), anchor), tracked_obj_set_.end()); 815 ArAnchor_release(anchor); 816 } 817 UpdateMatrixMaps(ArAnchor * anchorKey,glm::mat4 aaMat,glm::mat4 caMat,glm::mat4 snapMat)818 void HelloArApplication::UpdateMatrixMaps(ArAnchor* anchorKey, glm::mat4 aaMat, glm::mat4 caMat, glm::mat4 snapMat) { 819 anchor_skmat4_axis_aligned_map_.insert({anchorKey, util::GlmMatToSkMat(aaMat)}); 820 anchor_skmat4_camera_aligned_map_.insert({anchorKey, util::GlmMatToSkMat(caMat)}); 821 anchor_skmat4_snap_aligned_map_.insert({anchorKey, util::GlmMatToSkMat(snapMat)}); 822 } 823 SetSkiaInitialRotation(glm::mat4 & initRotation)824 void SetSkiaInitialRotation(glm::mat4& initRotation) { 825 initRotation = glm::rotate(initRotation, SK_ScalarPI / 2, glm::vec3(1, 0, 0)); 826 } 827 SetSkiaObjectAxes(glm::vec3 & x,glm::vec3 & y,glm::vec3 & z,glm::mat4 transform)828 void SetSkiaObjectAxes(glm::vec3& x, glm::vec3& y, glm::vec3& z, glm::mat4 transform) { 829 x = glm::normalize(glm::vec3(transform * glm::vec4(1, 0, 0, 1))); //X still X 830 y = glm::normalize(glm::vec3(transform * glm::vec4(0, 1, 0, 1))); //Y is now Z 831 z = glm::normalize(glm::vec3(transform * glm::vec4(0, 0, 1, 1))); //Z is now Y 832 } 833 SetCameraAlignedRotation(glm::mat4 & rotateTowardsCamera,float & rotationDirection,const glm::vec3 & toProject,const glm::vec3 & skiaY,const glm::vec3 & skiaZ)834 void SetCameraAlignedRotation(glm::mat4& rotateTowardsCamera, float& rotationDirection, const glm::vec3& toProject, const glm::vec3& skiaY, const glm::vec3& skiaZ) { 835 glm::vec3 hitLookProj = -util::ProjectOntoPlane(toProject, skiaZ); 836 float angleRad = util::AngleRad(skiaY, hitLookProj); 837 glm::vec3 cross = glm::normalize(glm::cross(skiaY, hitLookProj)); 838 839 //outs 840 rotationDirection = util::Dot(cross, skiaZ); 841 rotateTowardsCamera = glm::rotate(rotateTowardsCamera, angleRad, rotationDirection * skiaZ); 842 } 843 844 struct CameraAlignmentInfo { 845 glm::vec3& skiaY, skiaZ; 846 glm::mat4& preRot, postRot; 847 CameraAlignmentInfohello_ar::CameraAlignmentInfo848 CameraAlignmentInfo(glm::vec3& skiaY, glm::vec3& skiaZ, glm::mat4 preRot, glm::mat4 postRot) 849 : skiaY(skiaY), skiaZ(skiaZ), preRot(preRot), postRot(postRot) {} 850 }; 851 SetCameraAlignedVertical(glm::mat4 & caMat,const glm::mat4 & camRot,const CameraAlignmentInfo & camAlignInfo)852 void SetCameraAlignedVertical(glm::mat4& caMat, const glm::mat4& camRot, const CameraAlignmentInfo& camAlignInfo) { 853 //Camera axes 854 glm::vec3 xCamera = glm::vec3(glm::vec4(1, 0, 0, 1) * camRot); 855 glm::vec3 yCamera = glm::vec3(glm::vec4(0, 1, 0, 1) * camRot); 856 glm::vec3 zCamera = glm::vec3(glm::vec4(0, 0, -1, 1) * camRot); 857 858 //Get matrix that rotates object from plane towards the wanted angle 859 glm::mat4 rotateTowardsCamera(1); 860 float rotationDirection = 1; 861 SetCameraAlignedRotation(rotateTowardsCamera, rotationDirection, yCamera, camAlignInfo.skiaY, camAlignInfo.skiaZ); 862 863 //LogOrientation(dot, angleRad, "Vertical/Wall"); 864 glm::mat4 flip(1); 865 flip = glm::rotate(flip, SK_ScalarPI, rotationDirection * camAlignInfo.skiaZ); 866 caMat = camAlignInfo.postRot * flip * rotateTowardsCamera * camAlignInfo.preRot; 867 } 868 SetCameraAlignedHorizontal(glm::mat4 & caMat,ArPlaneType planeType,const glm::vec3 hitLook,const CameraAlignmentInfo & camAlignInfo)869 void SetCameraAlignedHorizontal(glm::mat4& caMat, ArPlaneType planeType, const glm::vec3 hitLook, const CameraAlignmentInfo& camAlignInfo) { 870 //Ceiling or Floor: follow hit location 871 //Get matrix that rotates object from plane towards the wanted angle 872 glm::mat4 rotateTowardsCamera(1); 873 float rotationDirection = 1; 874 SetCameraAlignedRotation(rotateTowardsCamera, rotationDirection, hitLook, camAlignInfo.skiaY, camAlignInfo.skiaZ); 875 876 if (planeType == ArPlaneType::AR_PLANE_HORIZONTAL_DOWNWARD_FACING) { 877 //ceiling 878 //LogOrientation(dot, angleRad, "Ceiling"); 879 glm::mat4 flip(1); 880 flip = glm::rotate(flip, SK_ScalarPI, rotationDirection * camAlignInfo.skiaZ); 881 caMat = camAlignInfo.postRot * flip * rotateTowardsCamera * camAlignInfo.preRot; 882 } else { 883 //floor or tabletop 884 //LogOrientation(dot, angleRad, "Floor"); 885 caMat = camAlignInfo.postRot * rotateTowardsCamera * camAlignInfo.preRot; 886 } 887 } 888 889 890 SetCameraAlignedMatrix(glm::mat4 & caMat,glm::vec3 hitPos,glm::mat4 & planeModel,const glm::mat4 & initRotation)891 void HelloArApplication::SetCameraAlignedMatrix(glm::mat4& caMat, glm::vec3 hitPos, glm::mat4& planeModel, const glm::mat4& initRotation) { 892 //Translation matrices: from plane to origin, and from origin to plane 893 glm::mat4 backToOrigin(1); 894 backToOrigin = glm::translate(backToOrigin, -hitPos); 895 glm::mat4 backToPlane(1); 896 backToPlane = glm::translate(backToPlane, hitPos); 897 898 //Axes of Skia object: start with XYZ, totate to get X(-Z)Y, paste on plane, go back to origin --> plane orientation but on origin 899 glm::vec3 skiaX, skiaY, skiaZ; 900 SetSkiaObjectAxes(skiaX, skiaY, skiaZ, backToOrigin * planeModel * initRotation); 901 902 //Get camera position & rotation 903 glm::vec3 cameraPos; 904 glm::mat4 cameraRotationMatrix; 905 util::GetCameraInfo(ar_session_, ar_frame_, cameraPos, cameraRotationMatrix); 906 907 //Set matrix depending on type of surface 908 ArPlaneType planeType = AR_PLANE_VERTICAL; 909 ArPlane_getType(ar_session_, pendingAnchor->GetContainingPlane(), &planeType); 910 911 //Set CamerAlignmentInfo 912 CameraAlignmentInfo camAlignInfo(skiaY, skiaZ, backToOrigin * planeModel * initRotation, backToPlane); 913 914 if (planeType == ArPlaneType::AR_PLANE_VERTICAL) { 915 //Wall: follow phone orientation 916 SetCameraAlignedVertical(caMat, cameraRotationMatrix, camAlignInfo); 917 } else { 918 //Ceiling or Floor: follow hit location 919 glm::vec3 hitLook(hitPos - cameraPos); 920 SetCameraAlignedHorizontal(caMat, planeType, hitLook, camAlignInfo); 921 } 922 } 923 924 SetModelMatrices(glm::mat4 & aaMat,glm::mat4 & caMat,glm::mat4 & snapMat,const glm::mat4 & planeModel)925 void HelloArApplication::SetModelMatrices(glm::mat4& aaMat, glm::mat4& caMat, glm::mat4& snapMat, const glm::mat4& planeModel) { 926 //Brings Skia world to ARCore world 927 glm::mat4 initRotation(1); 928 SetSkiaInitialRotation(initRotation); 929 930 //Copy plane model for editing 931 glm::mat4 copyPlaneModel(planeModel); 932 933 //Set snap matrix 934 //snapMat = copyPlaneModel * initRotation; 935 936 //Set axis-aligned matrix 937 glm::vec4 anchorPos = pendingAnchor->GetAnchorPos(ar_session_); 938 copyPlaneModel[3] = anchorPos; 939 aaMat = planeModel * initRotation; 940 941 //Set camera-aligned matrix 942 //SetCameraAlignedMatrix(caMat, glm::vec3(anchorPos), copyPlaneModel, initRotation); 943 } 944 GetPlaneModelMatrix(glm::mat4 & planeModel,ArSession * arSession,ArPlane * arPlane)945 void GetPlaneModelMatrix(glm::mat4& planeModel, ArSession* arSession, ArPlane* arPlane) { 946 ArPose *plane_pose = nullptr; 947 ArPose_create(arSession, nullptr, &plane_pose); 948 ArPlane_getCenterPose(arSession, arPlane, plane_pose); 949 util::GetTransformMatrixFromPose(arSession, plane_pose, &planeModel); 950 ArPose_destroy(plane_pose); 951 } 952 OnTouchedFinal(int type)953 void HelloArApplication::OnTouchedFinal(int type) { 954 LOGI("Entered OnTouchedFinal"); 955 if (pendingAnchor == nullptr) { 956 LOGI("WARNING: Entered OnTouchedFinal but no pending anchor.."); 957 return; 958 } 959 960 if (pendingAnchor->GetEditMode()) { 961 LOGI("WARNING: Editing old anchor in OnTouchedFinal!"); 962 } 963 964 //Get necessary pending anchor info 965 ArPlane* containingPlane = pendingAnchor->GetContainingPlane(); 966 glm::vec4 pendingAnchorPos = pendingAnchor->GetAnchorPos(ar_session_); 967 ArAnchor* actualAnchor = pendingAnchor->GetArAnchor(); 968 969 //Plane model matrix 970 glm::mat4 planeModel(1); 971 GetPlaneModelMatrix(planeModel, ar_session_, containingPlane); 972 973 //Setup skia object model matrices 974 glm::mat4 matrixAxisAligned(1); 975 glm::mat4 matrixCameraAligned(1); 976 glm::mat4 matrixSnapAligned(1); 977 SetModelMatrices(matrixAxisAligned, matrixCameraAligned, matrixSnapAligned, planeModel); 978 979 //Update anchor -> model matrix datastructures 980 UpdateMatrixMaps(actualAnchor, matrixAxisAligned, matrixCameraAligned, matrixSnapAligned); 981 982 //Add anchor to aux datastructures 983 AddAnchor(actualAnchor, containingPlane); 984 } 985 986 } // namespace hello_ar 987