/* * Copyright 2017 Google Inc. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "hello_ar_application.h" #include #include "anchor_wrapper.h" #include "plane_renderer.h" #include "pending_anchor.h" #include "util.h" #include "SkCanvas.h" #include "GrContext.h" #include "gl/GrGLTypes.h" #include "SkSurface.h" #include "SkTypeface.h" #include "SkFontStyle.h" #include "GrBackendSurface.h" #include "SkMatrix44.h" #include "SkMatrix.h" #include "SkTextBlob.h" #include "glm.h" #include "SkPoint3.h" #include "Sk3D.h" #include /* acos */ #include "SkShaper.h" #include "Skottie.h" #include "SkAnimTimer.h" #include "Resources.h" #include "SkStream.h" namespace hello_ar { namespace { constexpr size_t kMaxNumberOfAndroidsToRender = 1; constexpr int32_t kPlaneColorRgbaSize = 16; const glm::vec3 kWhite = {255, 255, 255}; constexpr std::array kPlaneColorRgba = { {0xFFFFFFFF, 0xF44336FF, 0xE91E63FF, 0x9C27B0FF, 0x673AB7FF, 0x3F51B5FF, 0x2196F3FF, 0x03A9F4FF, 0x00BCD4FF, 0x009688FF, 0x4CAF50FF, 0x8BC34AFF, 0xCDDC39FF, 0xFFEB3BFF, 0xFFC107FF, 0xFF9800FF}}; inline glm::vec3 GetRandomPlaneColor() { const int32_t colorRgba = kPlaneColorRgba[std::rand() % kPlaneColorRgbaSize]; return glm::vec3(((colorRgba >> 24) & 0xff) / 255.0f, ((colorRgba >> 16) & 0xff) / 255.0f, ((colorRgba >> 8) & 0xff) / 255.0f); } } // namespace HelloArApplication::HelloArApplication(AAssetManager *asset_manager) : asset_manager_(asset_manager) { LOGI("OnCreate()"); } HelloArApplication::~HelloArApplication() { if (ar_session_ != nullptr) { ArSession_destroy(ar_session_); ArFrame_destroy(ar_frame_); } } void HelloArApplication::OnPause() { LOGI("OnPause()"); if (ar_session_ != nullptr) { ArSession_pause(ar_session_); } } void HelloArApplication::OnResume(void *env, void *context, void *activity) { LOGI("OnResume()"); if (ar_session_ == nullptr) { ArInstallStatus install_status; // If install was not yet requested, that means that we are resuming the // activity first time because of explicit user interaction (such as // launching the application) bool user_requested_install = !install_requested_; // === ATTENTION! ATTENTION! ATTENTION! === // This method can and will fail in user-facing situations. Your // application must handle these cases at least somewhat gracefully. See // HelloAR Java sample code for reasonable behavior. CHECK(ArCoreApk_requestInstall(env, activity, user_requested_install, &install_status) == AR_SUCCESS); switch (install_status) { case AR_INSTALL_STATUS_INSTALLED: break; case AR_INSTALL_STATUS_INSTALL_REQUESTED: install_requested_ = true; return; } // === ATTENTION! ATTENTION! ATTENTION! === // This method can and will fail in user-facing situations. Your // application must handle these cases at least somewhat gracefully. See // HelloAR Java sample code for reasonable behavior. CHECK(ArSession_create(env, context, &ar_session_) == AR_SUCCESS); CHECK(ar_session_); ArFrame_create(ar_session_, &ar_frame_); CHECK(ar_frame_); ArSession_setDisplayGeometry(ar_session_, display_rotation_, width_, height_); } const ArStatus status = ArSession_resume(ar_session_); CHECK(status == AR_SUCCESS); } void HelloArApplication::OnSurfaceCreated() { LOGI("OnSurfaceCreated()"); background_renderer_.InitializeGlContent(); point_cloud_renderer_.InitializeGlContent(); plane_renderer_.InitializeGlContent(asset_manager_); } void HelloArApplication::OnDisplayGeometryChanged(int display_rotation, int width, int height) { LOGI("OnSurfaceChanged(%d, %d)", width, height); glViewport(0, 0, width, height); display_rotation_ = display_rotation; width_ = width; height_ = height; if (ar_session_ != nullptr) { ArSession_setDisplayGeometry(ar_session_, display_rotation, width, height);; } } void HelloArApplication::OnObjectRotationChanged(int rotation) { LOGI("OnObjectRotationChanged(%d)", rotation); currentObjectRotation = rotation; } void HelloArApplication::OnAction(float value) { LOGI("OnAction(%.6f)", value); currentValue = value; } void DrawText(SkCanvas *canvas, SkPaint *paint, const char text[]) { float spacing = 0.05; for (int i = 0; i < sizeof(text) / sizeof(text[0]); i++) { const char letter[] = {text[i]}; size_t byteLength = strlen(static_cast(letter)); canvas->drawText(letter, byteLength, spacing * i, 0, *paint); } } void DrawAxes(SkCanvas *canvas, SkMatrix44 m) { SkPaint p; p.setStrokeWidth(10); SkPoint3 src[4] = { {0, 0, 0}, {0.2, 0, 0}, {0, 0.2, 0}, {0, 0, 0.2}, }; SkPoint dst[4]; Sk3MapPts(dst, m, src, 4); const char str[] = "XYZ"; p.setColor(SK_ColorRED); canvas->drawLine(dst[0], dst[1], p); p.setColor(SK_ColorGREEN); canvas->drawLine(dst[0], dst[2], p); p.setColor(SK_ColorBLUE); canvas->drawLine(dst[0], dst[3], p); } void DrawVector(SkCanvas *canvas, SkMatrix44 m, glm::vec3 begin, glm::vec3 end, SkColor c) { SkPaint p; p.setStrokeWidth(15); SkPoint3 src[2] = { {begin.x, begin.y, begin.z}, {end.x, end.y, end.z} }; SkPoint dst[2]; Sk3MapPts(dst, m, src, 2); const char str[] = "XYZ"; p.setColor(c); canvas->drawLine(dst[0], dst[1], p); } void DrawBoundingBox(SkCanvas* canvas) { SkPaint paint; paint.setColor(SK_ColorYELLOW); SkIRect bounds = canvas->getDeviceClipBounds(); SkRect b = SkRect::Make(bounds); canvas->drawRect(b, paint); } void HelloArApplication::OnDrawFrame() { grContext = GrContext::MakeGL(); GrBackendRenderTarget target; sk_sp surface = nullptr; GrGLFramebufferInfo framebuffer_info; framebuffer_info.fFBOID = 0; framebuffer_info.fFormat = 0x8058; glClearColor(0.9f, 0.9f, 0.9f, 1.0f); glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT); glEnable(GL_CULL_FACE); glEnable(GL_DEPTH_TEST); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); if (ar_session_ == nullptr) return; ArSession_setCameraTextureName(ar_session_, background_renderer_.GetTextureId()); // Update session to get current frame and render camera background. if (ArSession_update(ar_session_, ar_frame_) != AR_SUCCESS) { LOGE("HelloArApplication::OnDrawFrame ArSession_update error"); } // GET CAMERA INFO ArCamera *ar_camera; ArFrame_acquireCamera(ar_session_, ar_frame_, &ar_camera); glm::mat4 view_mat; glm::mat4 projection_mat; ArCamera_getViewMatrix(ar_session_, ar_camera, glm::value_ptr(view_mat)); ArCamera_getProjectionMatrix(ar_session_, ar_camera, /*near=*/0.1f, /*far=*/100.f, glm::value_ptr(projection_mat)); ArTrackingState camera_tracking_state; ArCamera_getTrackingState(ar_session_, ar_camera, &camera_tracking_state); ArCamera_release(ar_camera); background_renderer_.Draw(ar_session_, ar_frame_); // If the camera isn't tracking don't bother rendering other objects. if (camera_tracking_state != AR_TRACKING_STATE_TRACKING) { return; } // Get light estimation value. ArLightEstimate *ar_light_estimate; ArLightEstimateState ar_light_estimate_state; ArLightEstimate_create(ar_session_, &ar_light_estimate); ArFrame_getLightEstimate(ar_session_, ar_frame_, ar_light_estimate); ArLightEstimate_getState(ar_session_, ar_light_estimate, &ar_light_estimate_state); // Set light intensity to default. Intensity value ranges from 0.0f to 1.0f. // The first three components are color scaling factors. // The last one is the average pixel intensity in gamma space. float color_correction[4] = {1.f, 1.f, 1.f, 1.f}; if (ar_light_estimate_state == AR_LIGHT_ESTIMATE_STATE_VALID) { ArLightEstimate_getColorCorrection(ar_session_, ar_light_estimate, color_correction); } ArLightEstimate_destroy(ar_light_estimate); ar_light_estimate = nullptr; SkMatrix44 skProj; SkMatrix44 skView; SkMatrix skViewport; skProj = util::GlmMatToSkMat(projection_mat); skView = util::GlmMatToSkMat(view_mat); skViewport.setScale(width_ / 2, -height_ / 2); skViewport.postTranslate(width_ / 2, height_ / 2); target = GrBackendRenderTarget(width_, height_, 0, 0, framebuffer_info); surface = SkSurface::MakeFromBackendRenderTarget(grContext.get(), target, kBottomLeft_GrSurfaceOrigin, kRGBA_8888_SkColorType, nullptr, nullptr); // Render Andy objects. std::vector models; //glm::mat4 model_mat(1.0f); for (const auto &obj_iter : tracked_obj_set_) { ArTrackingState tracking_state = AR_TRACKING_STATE_STOPPED; ArAnchor_getTrackingState(ar_session_, obj_iter, &tracking_state); if (tracking_state == AR_TRACKING_STATE_TRACKING) { // Render object only if the tracking state is AR_TRACKING_STATE_TRACKING. //util::GetTransformMatrixFromAnchor(ar_session_, obj_iter, &model_mat); //DRAW ANDY //andy_renderer_.Draw(glm::mat4(1), glm::mat4(1), model_mat, color_correction); //PREPARE SKIA MATS SkMatrix44 skModel; switch (currentObjectRotation) { case 0: { auto iter = anchor_skmat4_axis_aligned_map_.find(obj_iter); if (iter != anchor_skmat4_axis_aligned_map_.end()) { skModel = iter->second; models.push_back(skModel); } } break; case 1: { auto iter = anchor_skmat4_camera_aligned_map_.find(obj_iter); if (iter != anchor_skmat4_camera_aligned_map_.end()) { skModel = iter->second; models.push_back(skModel); } } break; case 2: { auto iter = anchor_skmat4_snap_aligned_map_.find(obj_iter); if (iter != anchor_skmat4_snap_aligned_map_.end()) { skModel = iter->second; models.push_back(skModel); } } break; default: { auto iter = anchor_skmat4_axis_aligned_map_.find(obj_iter); if (iter != anchor_skmat4_axis_aligned_map_.end()) { skModel = iter->second; models.push_back(skModel); } } break; } } } // Update and render planes. ArTrackableList *plane_list = nullptr; ArTrackableList_create(ar_session_, &plane_list); CHECK(plane_list != nullptr); ArTrackableType plane_tracked_type = AR_TRACKABLE_PLANE; ArSession_getAllTrackables(ar_session_, plane_tracked_type, plane_list); int32_t plane_list_size = 0; ArTrackableList_getSize(ar_session_, plane_list, &plane_list_size); plane_count_ = plane_list_size; for (int i = 0; i < plane_list_size; ++i) { ArTrackable *ar_trackable = nullptr; ArTrackableList_acquireItem(ar_session_, plane_list, i, &ar_trackable); ArPlane *ar_plane = ArAsPlane(ar_trackable); ArTrackingState out_tracking_state; ArTrackable_getTrackingState(ar_session_, ar_trackable, &out_tracking_state); ArPlane *subsume_plane; ArPlane_acquireSubsumedBy(ar_session_, ar_plane, &subsume_plane); if (subsume_plane != nullptr) { ArTrackable_release(ArAsTrackable(subsume_plane)); continue; } if (ArTrackingState::AR_TRACKING_STATE_TRACKING != out_tracking_state) { continue; } ArTrackingState plane_tracking_state; ArTrackable_getTrackingState(ar_session_, ArAsTrackable(ar_plane), &plane_tracking_state); if (plane_tracking_state == AR_TRACKING_STATE_TRACKING) { const auto iter = plane_color_map_.find(ar_plane); glm::vec3 color; if (iter != plane_color_map_.end()) { color = iter->second; // If this is an already observed trackable release it so it doesn't // leave aof placing objects on surfaces (n additional reference dangling. ArTrackable_release(ar_trackable); } else { // The first plane is always white. if (!first_plane_has_been_found_) { first_plane_has_been_found_ = true; color = kWhite; } else { color = GetRandomPlaneColor(); } plane_color_map_.insert({ar_plane, color}); } plane_renderer_.Draw(projection_mat, view_mat, ar_session_, ar_plane, color); } } ArTrackableList_destroy(plane_list); plane_list = nullptr; // Update and render point cloud. ArPointCloud *ar_point_cloud = nullptr; ArStatus point_cloud_status = ArFrame_acquirePointCloud(ar_session_, ar_frame_, &ar_point_cloud); if (point_cloud_status == AR_SUCCESS) { point_cloud_renderer_.Draw(projection_mat * view_mat, ar_session_, ar_point_cloud); ArPointCloud_release(ar_point_cloud); } SkMatrix44 i = SkMatrix44::kIdentity_Constructor; if (surface != nullptr) { SkCanvas *canvas = surface->getCanvas(); SkAutoCanvasRestore acr(canvas, true); SkMatrix44 vpv = skViewport * skProj * skView; for(SkMatrix44 skModel: models) { SkMatrix44 i = SkMatrix44::kIdentity_Constructor; canvas->setMatrix(i); SkMatrix44 mvpv = skViewport * skProj * skView * skModel; //Draw XYZ axes DrawAxes(canvas, mvpv); //Drawing camera orientation /* DrawVector(canvas, vpv, begins[0], ends[0], SK_ColorMAGENTA); DrawVector(canvas, vpv, begins[0], ends[1], SK_ColorYELLOW); DrawVector(canvas, vpv, begins[0], ends[2], SK_ColorCYAN);*/ canvas->concat(mvpv); SkPaint paint; //Draw Circle paint.setColor(0x80700000); canvas->drawCircle(0, 0, 0.1, paint); //Draw Text paint.setColor(SK_ColorBLUE); if (currentValue != 0) { paint.setTextSize(currentValue); } else { paint.setTextSize(0.1); } paint.setAntiAlias(true); const char text[] = "SkAR"; size_t byteLength = strlen(static_cast(text)); SkShaper shaper(nullptr); SkTextBlobBuilder builder; SkPoint p = SkPoint::Make(0, 0); shaper.shape(&builder, paint, text, byteLength, true, p, 10); canvas->drawTextBlob(builder.make(), 0, 0, paint); //DrawBoundingBox(canvas); } canvas->flush(); } } bool HelloArApplication::OnTouchedFirst(float x, float y, int drawMode) { LOGI("Entered OnTouchedFirst"); if (pendingAnchor != nullptr) { delete pendingAnchor; } SkPoint p = SkPoint::Make(x,y); pendingAnchor = new PendingAnchor(p); bool editAnchor = false; if (ar_frame_ != nullptr && ar_session_ != nullptr) { ArHitResultList *hit_result_list = nullptr; ArHitResultList_create(ar_session_, &hit_result_list); CHECK(hit_result_list); ArFrame_hitTest(ar_session_, ar_frame_, x, y, hit_result_list); int32_t hit_result_list_size = 0; ArHitResultList_getSize(ar_session_, hit_result_list, &hit_result_list_size); ArHitResult *ar_hit_result = nullptr; ArPose *out_pose = nullptr; ArPlane* hitPlane = nullptr; for (int32_t i = 0; i < hit_result_list_size; ++i) { ArHitResult *ar_hit = nullptr; ArPose *created_out_pose = nullptr; ArHitResult_create(ar_session_, &ar_hit); ArHitResultList_getItem(ar_session_, hit_result_list, i, ar_hit); if (ar_hit == nullptr) { LOGE("HelloArApplication::OnTouched ArHitResultList_getItem error"); return editAnchor; } ArTrackable *ar_trackable = nullptr; ArHitResult_acquireTrackable(ar_session_, ar_hit, &ar_trackable); ArTrackableType ar_trackable_type = AR_TRACKABLE_NOT_VALID; ArTrackable_getType(ar_session_, ar_trackable, &ar_trackable_type); // Creates an anchor if a plane or an oriented point was hit. if (AR_TRACKABLE_PLANE == ar_trackable_type) { ArPose *hit_pose = nullptr; ArPose_create(ar_session_, nullptr, &hit_pose); ArHitResult_getHitPose(ar_session_, ar_hit, hit_pose); int32_t in_polygon = 0; ArPlane *ar_plane = ArAsPlane(ar_trackable); ArPlane_isPoseInPolygon(ar_session_, ar_plane, hit_pose, &in_polygon); { // Use hit pose and camera pose to check if hittest is from the // back of the plane, if it is, no need to create the anchor. ArPose *camera_pose = nullptr; ArPose_create(ar_session_, nullptr, &camera_pose); ArCamera *ar_camera; ArFrame_acquireCamera(ar_session_, ar_frame_, &ar_camera); ArCamera_getPose(ar_session_, ar_camera, camera_pose); float normal_distance_to_plane = util::CalculateDistanceToPlane( ar_session_, *hit_pose, *camera_pose); if (!in_polygon || normal_distance_to_plane < 0) { ArPose_destroy(camera_pose); continue; } ArPose_destroy(camera_pose); ArCamera_release(ar_camera); } //Raw pose of hit location float out_hit_raw[] = {0, 0, 0, 0, 0, 0, 0}; ArPose_getPoseRaw(ar_session_, hit_pose, out_hit_raw); ArPose_destroy(hit_pose); //Position of anchor glm::vec4 pendingAnchorPos(out_hit_raw[4], out_hit_raw[5], out_hit_raw[6], 1); pendingAnchor->SetContainingPlane(ar_plane); //Check if plane contains approx the same anchor auto planeAnchors = plane_anchors_map_.find(ar_plane); if (planeAnchors != plane_anchors_map_.end()) { //other anchors existed on this plane std::vector anchors = planeAnchors->second; int i = 0; LOGI("Size of anchor list: %d", (int) anchors.size()); for(ArAnchor* const& anchor: anchors) { //Get anchor's pose i++; LOGI("CHECKING: Anchor #%d", i); ArPose *anchor_pose = nullptr; ArPose_create(ar_session_, nullptr, &anchor_pose); ArAnchor_getPose(ar_session_, anchor, anchor_pose); float out_anchor_raw[] = {0, 0, 0, 0, 0, 0, 0}; ArPose_getPoseRaw(ar_session_, anchor_pose, out_anchor_raw); ArPose_destroy(anchor_pose); glm::vec4 oldAnchorPos(out_anchor_raw[4], out_anchor_raw[5], out_anchor_raw[6], 1); oldAnchorPos = oldAnchorPos - pendingAnchorPos; float distance = util::Magnitude(glm::vec3(oldAnchorPos)); if (distance < 0.1f) { LOGI("TouchFirst: Editing old anchor!"); editAnchor = true; pendingAnchor->SetArAnchor(anchor); pendingAnchor->SetEditMode(true); ArHitResult_destroy(ar_hit); ArHitResultList_destroy(hit_result_list); LOGI("TouchFirst: Edit %d", editAnchor); return editAnchor; } } } //actual hit result, and containing plane ar_hit_result = ar_hit; hitPlane = ar_plane; //new anchor pos float wanted_raw_pose[] = {0, 0, 0, 0, out_hit_raw[4], out_hit_raw[5], out_hit_raw[6]}; ArPose_create(ar_session_, wanted_raw_pose, &created_out_pose); out_pose = created_out_pose; break; } } if (ar_hit_result) { LOGI("TouchFirst: Adding new anchor!"); ArAnchor *anchor = nullptr; pendingAnchor->SetEditMode(false); if (ArSession_acquireNewAnchor(ar_session_, out_pose, &anchor) != AR_SUCCESS) { LOGE("HelloArApplication::OnTouched ArHitResult_acquireNewAnchor error"); LOGI("TouchFirst: Failed to acquire new anchor"); delete hitPlane; delete pendingAnchor; pendingAnchor = nullptr; LOGI("TouchFirst: Edit %d", editAnchor); return editAnchor; } pendingAnchor->SetArAnchor(anchor); ArHitResult_destroy(ar_hit_result); ArHitResultList_destroy(hit_result_list); ArPose_destroy(out_pose); hit_result_list = nullptr; LOGI("TouchFirst: Edit %d", editAnchor); return editAnchor; } LOGI("TouchFirst: didn't hit anything"); delete hitPlane; delete pendingAnchor; pendingAnchor = nullptr; LOGI("TouchFirst: Edit %d", editAnchor); return editAnchor; } } void HelloArApplication::AddAnchor(ArAnchor* anchor, ArPlane* containingPlane) { //delete anchor from matrices maps //releasing the anchor if it is not tracking anymore ArTrackingState tracking_state = AR_TRACKING_STATE_STOPPED; ArAnchor_getTrackingState(ar_session_, anchor, &tracking_state); if (tracking_state != AR_TRACKING_STATE_TRACKING) { RemoveAnchor(anchor); return; } //releasing the first anchor if we exceeded maximum number of objects to be rendered if (tracked_obj_set_.size() >= kMaxNumberOfAndroidsToRender) { RemoveAnchor(tracked_obj_set_[0]); } //updating the containing plane with a new anchor auto planeAnchors = plane_anchors_map_.find(containingPlane); if (planeAnchors != plane_anchors_map_.end()) { //other anchors existed on this plane LOGI("TouchFinal: ADDING TO OLD ANCHORS"); std::vector anchors = planeAnchors->second; anchors.push_back(anchor); plane_anchors_map_[containingPlane] = anchors; anchor_plane_map_.insert({anchor, containingPlane}); } else { LOGI("TouchFinal: NEW SET OF ANCHORS"); std::vector anchors; anchors.push_back(anchor); plane_anchors_map_.insert({containingPlane, anchors}); anchor_plane_map_.insert({anchor, containingPlane}); } tracked_obj_set_.push_back(anchor); } void HelloArApplication::OnTouchTranslate(float x, float y) { LOGI("Entered On Edit Touched"); ArAnchor *anchor = pendingAnchor->GetArAnchor(); glm::mat4 matrix = util::SkMatToGlmMat( anchor_skmat4_axis_aligned_map_.find(anchor)->second); if (ar_frame_ != nullptr && ar_session_ != nullptr) { ArHitResultList *hit_result_list = nullptr; ArHitResultList_create(ar_session_, &hit_result_list); CHECK(hit_result_list); ArFrame_hitTest(ar_session_, ar_frame_, x, y, hit_result_list); int32_t hit_result_list_size = 0; ArHitResultList_getSize(ar_session_, hit_result_list, &hit_result_list_size); ArHitResult *ar_hit_result = nullptr; ArPose *out_pose = nullptr; ArPlane *hitPlane = nullptr; for (int32_t i = 0; i < hit_result_list_size; ++i) { ArHitResult *ar_hit = nullptr; ArPose *created_out_pose = nullptr; ArHitResult_create(ar_session_, &ar_hit); ArHitResultList_getItem(ar_session_, hit_result_list, i, ar_hit); if (ar_hit == nullptr) { LOGE("HelloArApplication::OnTouched ArHitResultList_getItem error"); return; } ArTrackable *ar_trackable = nullptr; ArHitResult_acquireTrackable(ar_session_, ar_hit, &ar_trackable); ArTrackableType ar_trackable_type = AR_TRACKABLE_NOT_VALID; ArTrackable_getType(ar_session_, ar_trackable, &ar_trackable_type); // Creates an anchor if a plane or an oriented point was hit. if (AR_TRACKABLE_PLANE == ar_trackable_type) { ArPose *hit_pose = nullptr; ArPose_create(ar_session_, nullptr, &hit_pose); ArHitResult_getHitPose(ar_session_, ar_hit, hit_pose); int32_t in_polygon = 0; ArPlane *ar_plane = ArAsPlane(ar_trackable); ArPlane_isPoseInPolygon(ar_session_, ar_plane, hit_pose, &in_polygon); { // Use hit pose and camera pose to check if hittest is from the // back of the plane, if it is, no need to create the anchor. ArPose *camera_pose = nullptr; ArPose_create(ar_session_, nullptr, &camera_pose); ArCamera *ar_camera; ArFrame_acquireCamera(ar_session_, ar_frame_, &ar_camera); ArCamera_getPose(ar_session_, ar_camera, camera_pose); float normal_distance_to_plane = util::CalculateDistanceToPlane( ar_session_, *hit_pose, *camera_pose); if (!in_polygon || normal_distance_to_plane < 0) { ArPose_destroy(camera_pose); continue; } ArPose_destroy(camera_pose); ArCamera_release(ar_camera); } //Raw pose of hit location float out_hit_raw[] = {0, 0, 0, 0, 0, 0, 0}; ArPose_getPoseRaw(ar_session_, hit_pose, out_hit_raw); ArPose_destroy(hit_pose); //Translate by new amount glm::vec4 newPos(out_hit_raw[4], out_hit_raw[5], out_hit_raw[6], 1); glm::vec4 oldPos = pendingAnchor->GetAnchorPos(ar_session_); glm::vec3 movement = glm::vec3(newPos - oldPos); //CAMERA SETTINGS glm::mat4 backToOrigin(1); backToOrigin = glm::translate(backToOrigin, -glm::vec3(oldPos)); glm::mat4 backToPlane(1); backToPlane = glm::translate(backToPlane, glm::vec3(oldPos)); //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 glm::vec3 objX = glm::normalize(glm::vec3( backToOrigin * matrix * glm::vec4(1, 0, 0, 1))); //X still X glm::vec3 objY = glm::normalize(glm::vec3( backToOrigin * matrix * glm::vec4(0, 1, 0, 1))); //Y is now Z glm::vec3 objZ = glm::normalize(glm::vec3( backToOrigin * matrix * glm::vec4(0, 0, 1, 1))); //Z is now Y glm::mat4 translate(1); translate = glm::translate(translate, movement); matrix = translate * matrix; RemoveAnchor(anchor); //new anchor pos float wanted_raw_pose[] = {0, 0, 0, 0, out_hit_raw[4], out_hit_raw[5], out_hit_raw[6]}; ArPose_create(ar_session_, wanted_raw_pose, &created_out_pose); out_pose = created_out_pose; ar_hit_result = ar_hit; break; } } if (ar_hit_result) { LOGI("TouchFirst: Adding new anchor!"); ArAnchor *anchor = nullptr; pendingAnchor->SetEditMode(false); if (ArSession_acquireNewAnchor(ar_session_, out_pose, &anchor) != AR_SUCCESS) { LOGE("HelloArApplication::OnTouched ArHitResult_acquireNewAnchor error"); LOGI("TouchFirst: Failed to acquire new anchor"); delete hitPlane; delete pendingAnchor; pendingAnchor = nullptr; return; } pendingAnchor->SetArAnchor(anchor); anchor_skmat4_axis_aligned_map_[anchor] = util::GlmMatToSkMat(matrix); //Add anchor AddAnchor(anchor, pendingAnchor->GetContainingPlane()); ArHitResult_destroy(ar_hit_result); ArHitResultList_destroy(hit_result_list); ArPose_destroy(out_pose); hit_result_list = nullptr; return; } } } void HelloArApplication::RemoveAnchor(ArAnchor* anchor) { //delete anchor from matrices maps anchor_skmat4_axis_aligned_map_.erase(anchor); anchor_skmat4_camera_aligned_map_.erase(anchor); anchor_skmat4_snap_aligned_map_.erase(anchor); auto containingPlaneIter = anchor_plane_map_.find(anchor); if (containingPlaneIter != anchor_plane_map_.end()) { ArPlane* containingPlane = containingPlaneIter->second; auto planeAnchors = plane_anchors_map_.find(containingPlane); if (planeAnchors != plane_anchors_map_.end()) { //delete this anchor from the list of anchors associated with its plane std::vector anchors = planeAnchors->second; anchors.erase(std::remove(anchors.begin(), anchors.end(), anchor), anchors.end()); plane_anchors_map_[planeAnchors->first] = anchors; //delete anchor from map of anchor to plane anchor_plane_map_.erase(anchor); } } //delete anchor from list of tracked objects tracked_obj_set_.erase(std::remove(tracked_obj_set_.begin(), tracked_obj_set_.end(), anchor), tracked_obj_set_.end()); ArAnchor_release(anchor); } void HelloArApplication::UpdateMatrixMaps(ArAnchor* anchorKey, glm::mat4 aaMat, glm::mat4 caMat, glm::mat4 snapMat) { anchor_skmat4_axis_aligned_map_.insert({anchorKey, util::GlmMatToSkMat(aaMat)}); anchor_skmat4_camera_aligned_map_.insert({anchorKey, util::GlmMatToSkMat(caMat)}); anchor_skmat4_snap_aligned_map_.insert({anchorKey, util::GlmMatToSkMat(snapMat)}); } void SetSkiaInitialRotation(glm::mat4& initRotation) { initRotation = glm::rotate(initRotation, SK_ScalarPI / 2, glm::vec3(1, 0, 0)); } void SetSkiaObjectAxes(glm::vec3& x, glm::vec3& y, glm::vec3& z, glm::mat4 transform) { x = glm::normalize(glm::vec3(transform * glm::vec4(1, 0, 0, 1))); //X still X y = glm::normalize(glm::vec3(transform * glm::vec4(0, 1, 0, 1))); //Y is now Z z = glm::normalize(glm::vec3(transform * glm::vec4(0, 0, 1, 1))); //Z is now Y } void SetCameraAlignedRotation(glm::mat4& rotateTowardsCamera, float& rotationDirection, const glm::vec3& toProject, const glm::vec3& skiaY, const glm::vec3& skiaZ) { glm::vec3 hitLookProj = -util::ProjectOntoPlane(toProject, skiaZ); float angleRad = util::AngleRad(skiaY, hitLookProj); glm::vec3 cross = glm::normalize(glm::cross(skiaY, hitLookProj)); //outs rotationDirection = util::Dot(cross, skiaZ); rotateTowardsCamera = glm::rotate(rotateTowardsCamera, angleRad, rotationDirection * skiaZ); } struct CameraAlignmentInfo { glm::vec3& skiaY, skiaZ; glm::mat4& preRot, postRot; CameraAlignmentInfo(glm::vec3& skiaY, glm::vec3& skiaZ, glm::mat4 preRot, glm::mat4 postRot) : skiaY(skiaY), skiaZ(skiaZ), preRot(preRot), postRot(postRot) {} }; void SetCameraAlignedVertical(glm::mat4& caMat, const glm::mat4& camRot, const CameraAlignmentInfo& camAlignInfo) { //Camera axes glm::vec3 xCamera = glm::vec3(glm::vec4(1, 0, 0, 1) * camRot); glm::vec3 yCamera = glm::vec3(glm::vec4(0, 1, 0, 1) * camRot); glm::vec3 zCamera = glm::vec3(glm::vec4(0, 0, -1, 1) * camRot); //Get matrix that rotates object from plane towards the wanted angle glm::mat4 rotateTowardsCamera(1); float rotationDirection = 1; SetCameraAlignedRotation(rotateTowardsCamera, rotationDirection, yCamera, camAlignInfo.skiaY, camAlignInfo.skiaZ); //LogOrientation(dot, angleRad, "Vertical/Wall"); glm::mat4 flip(1); flip = glm::rotate(flip, SK_ScalarPI, rotationDirection * camAlignInfo.skiaZ); caMat = camAlignInfo.postRot * flip * rotateTowardsCamera * camAlignInfo.preRot; } void SetCameraAlignedHorizontal(glm::mat4& caMat, ArPlaneType planeType, const glm::vec3 hitLook, const CameraAlignmentInfo& camAlignInfo) { //Ceiling or Floor: follow hit location //Get matrix that rotates object from plane towards the wanted angle glm::mat4 rotateTowardsCamera(1); float rotationDirection = 1; SetCameraAlignedRotation(rotateTowardsCamera, rotationDirection, hitLook, camAlignInfo.skiaY, camAlignInfo.skiaZ); if (planeType == ArPlaneType::AR_PLANE_HORIZONTAL_DOWNWARD_FACING) { //ceiling //LogOrientation(dot, angleRad, "Ceiling"); glm::mat4 flip(1); flip = glm::rotate(flip, SK_ScalarPI, rotationDirection * camAlignInfo.skiaZ); caMat = camAlignInfo.postRot * flip * rotateTowardsCamera * camAlignInfo.preRot; } else { //floor or tabletop //LogOrientation(dot, angleRad, "Floor"); caMat = camAlignInfo.postRot * rotateTowardsCamera * camAlignInfo.preRot; } } void HelloArApplication::SetCameraAlignedMatrix(glm::mat4& caMat, glm::vec3 hitPos, glm::mat4& planeModel, const glm::mat4& initRotation) { //Translation matrices: from plane to origin, and from origin to plane glm::mat4 backToOrigin(1); backToOrigin = glm::translate(backToOrigin, -hitPos); glm::mat4 backToPlane(1); backToPlane = glm::translate(backToPlane, hitPos); //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 glm::vec3 skiaX, skiaY, skiaZ; SetSkiaObjectAxes(skiaX, skiaY, skiaZ, backToOrigin * planeModel * initRotation); //Get camera position & rotation glm::vec3 cameraPos; glm::mat4 cameraRotationMatrix; util::GetCameraInfo(ar_session_, ar_frame_, cameraPos, cameraRotationMatrix); //Set matrix depending on type of surface ArPlaneType planeType = AR_PLANE_VERTICAL; ArPlane_getType(ar_session_, pendingAnchor->GetContainingPlane(), &planeType); //Set CamerAlignmentInfo CameraAlignmentInfo camAlignInfo(skiaY, skiaZ, backToOrigin * planeModel * initRotation, backToPlane); if (planeType == ArPlaneType::AR_PLANE_VERTICAL) { //Wall: follow phone orientation SetCameraAlignedVertical(caMat, cameraRotationMatrix, camAlignInfo); } else { //Ceiling or Floor: follow hit location glm::vec3 hitLook(hitPos - cameraPos); SetCameraAlignedHorizontal(caMat, planeType, hitLook, camAlignInfo); } } void HelloArApplication::SetModelMatrices(glm::mat4& aaMat, glm::mat4& caMat, glm::mat4& snapMat, const glm::mat4& planeModel) { //Brings Skia world to ARCore world glm::mat4 initRotation(1); SetSkiaInitialRotation(initRotation); //Copy plane model for editing glm::mat4 copyPlaneModel(planeModel); //Set snap matrix //snapMat = copyPlaneModel * initRotation; //Set axis-aligned matrix glm::vec4 anchorPos = pendingAnchor->GetAnchorPos(ar_session_); copyPlaneModel[3] = anchorPos; aaMat = planeModel * initRotation; //Set camera-aligned matrix //SetCameraAlignedMatrix(caMat, glm::vec3(anchorPos), copyPlaneModel, initRotation); } void GetPlaneModelMatrix(glm::mat4& planeModel, ArSession* arSession, ArPlane* arPlane) { ArPose *plane_pose = nullptr; ArPose_create(arSession, nullptr, &plane_pose); ArPlane_getCenterPose(arSession, arPlane, plane_pose); util::GetTransformMatrixFromPose(arSession, plane_pose, &planeModel); ArPose_destroy(plane_pose); } void HelloArApplication::OnTouchedFinal(int type) { LOGI("Entered OnTouchedFinal"); if (pendingAnchor == nullptr) { LOGI("WARNING: Entered OnTouchedFinal but no pending anchor.."); return; } if (pendingAnchor->GetEditMode()) { LOGI("WARNING: Editing old anchor in OnTouchedFinal!"); } //Get necessary pending anchor info ArPlane* containingPlane = pendingAnchor->GetContainingPlane(); glm::vec4 pendingAnchorPos = pendingAnchor->GetAnchorPos(ar_session_); ArAnchor* actualAnchor = pendingAnchor->GetArAnchor(); //Plane model matrix glm::mat4 planeModel(1); GetPlaneModelMatrix(planeModel, ar_session_, containingPlane); //Setup skia object model matrices glm::mat4 matrixAxisAligned(1); glm::mat4 matrixCameraAligned(1); glm::mat4 matrixSnapAligned(1); SetModelMatrices(matrixAxisAligned, matrixCameraAligned, matrixSnapAligned, planeModel); //Update anchor -> model matrix datastructures UpdateMatrixMaps(actualAnchor, matrixAxisAligned, matrixCameraAligned, matrixSnapAligned); //Add anchor to aux datastructures AddAnchor(actualAnchor, containingPlane); } } // namespace hello_ar