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 #include "platform_tools/android/apps/arcore/src/main/cpp/util.h" 17 18 #include "include/core/SkMatrix44.h" 19 #include <gtx/string_cast.inl> 20 #include <sstream> 21 #include <string> 22 #include <unistd.h> 23 24 #include "platform_tools/android/apps/arcore/src/main/cpp/jni_interface.h" 25 26 namespace hello_ar { 27 namespace util { 28 CheckGlError(const char * operation)29 void CheckGlError(const char *operation) { 30 bool anyError = false; 31 for (GLint error = glGetError(); error; error = glGetError()) { 32 LOGE("after %s() glError (0x%x)\n", operation, error); 33 anyError = true; 34 } 35 if (anyError) { 36 abort(); 37 } 38 } 39 40 // Convenience function used in CreateProgram below. LoadShader(GLenum shader_type,const char * shader_source)41 static GLuint LoadShader(GLenum shader_type, const char *shader_source) { 42 GLuint shader = glCreateShader(shader_type); 43 if (!shader) { 44 return shader; 45 } 46 47 glShaderSource(shader, 1, &shader_source, nullptr); 48 glCompileShader(shader); 49 GLint compiled = 0; 50 glGetShaderiv(shader, GL_COMPILE_STATUS, &compiled); 51 52 if (!compiled) { 53 GLint info_len = 0; 54 55 glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &info_len); 56 if (!info_len) { 57 return shader; 58 } 59 60 char *buf = reinterpret_cast<char *>(malloc(info_len)); 61 if (!buf) { 62 return shader; 63 } 64 65 glGetShaderInfoLog(shader, info_len, nullptr, buf); 66 LOGE("hello_ar::util::Could not compile shader %d:\n%s\n", shader_type, 67 buf); 68 free(buf); 69 glDeleteShader(shader); 70 shader = 0; 71 } 72 73 return shader; 74 } 75 CreateProgram(const char * vertex_source,const char * fragment_source)76 GLuint CreateProgram(const char *vertex_source, const char *fragment_source) { 77 GLuint vertexShader = LoadShader(GL_VERTEX_SHADER, vertex_source); 78 if (!vertexShader) { 79 return 0; 80 } 81 82 GLuint fragment_shader = LoadShader(GL_FRAGMENT_SHADER, fragment_source); 83 if (!fragment_shader) { 84 return 0; 85 } 86 87 GLuint program = glCreateProgram(); 88 if (program) { 89 glAttachShader(program, vertexShader); 90 CheckGlError("hello_ar::util::glAttachShader"); 91 glAttachShader(program, fragment_shader); 92 CheckGlError("hello_ar::util::glAttachShader"); 93 glLinkProgram(program); 94 GLint link_status = GL_FALSE; 95 glGetProgramiv(program, GL_LINK_STATUS, &link_status); 96 if (link_status != GL_TRUE) { 97 GLint buf_length = 0; 98 glGetProgramiv(program, GL_INFO_LOG_LENGTH, &buf_length); 99 if (buf_length) { 100 char *buf = reinterpret_cast<char *>(malloc(buf_length)); 101 if (buf) { 102 glGetProgramInfoLog(program, buf_length, nullptr, buf); 103 LOGE("hello_ar::util::Could not link program:\n%s\n", buf); 104 free(buf); 105 } 106 } 107 glDeleteProgram(program); 108 program = 0; 109 } 110 } 111 return program; 112 } 113 LoadPngFromAssetManager(int target,const std::string & path)114 bool LoadPngFromAssetManager(int target, const std::string &path) { 115 JNIEnv *env = GetJniEnv(); 116 117 // Put all the JNI values in a structure that is statically initalized on the 118 // first call to this method. This makes it thread safe in the unlikely case 119 // of multiple threads calling this method. 120 static struct JNIData { 121 jclass helper_class; 122 jmethodID load_image_method; 123 jmethodID load_texture_method; 124 } jniIds = [env]() -> JNIData { 125 constexpr char kHelperClassName[] = 126 "org/skia/arcore/JniInterface"; 127 constexpr char kLoadImageMethodName[] = "loadImage"; 128 constexpr char kLoadImageMethodSignature[] = 129 "(Ljava/lang/String;)Landroid/graphics/Bitmap;"; 130 constexpr char kLoadTextureMethodName[] = "loadTexture"; 131 constexpr char kLoadTextureMethodSignature[] = 132 "(ILandroid/graphics/Bitmap;)V"; 133 jclass helper_class = FindClass(kHelperClassName); 134 if (helper_class) { 135 helper_class = static_cast<jclass>(env->NewGlobalRef(helper_class)); 136 jmethodID load_image_method = env->GetStaticMethodID( 137 helper_class, kLoadImageMethodName, kLoadImageMethodSignature); 138 jmethodID load_texture_method = env->GetStaticMethodID( 139 helper_class, kLoadTextureMethodName, kLoadTextureMethodSignature); 140 return {helper_class, load_image_method, load_texture_method}; 141 } 142 LOGE("hello_ar::util::Could not find Java helper class %s", 143 kHelperClassName); 144 return {}; 145 }(); 146 147 if (!jniIds.helper_class) { 148 return false; 149 } 150 151 jstring j_path = env->NewStringUTF(path.c_str()); 152 153 jobject image_obj = env->CallStaticObjectMethod( 154 jniIds.helper_class, jniIds.load_image_method, j_path); 155 156 if (j_path) { 157 env->DeleteLocalRef(j_path); 158 } 159 160 env->CallStaticVoidMethod(jniIds.helper_class, jniIds.load_texture_method, 161 target, image_obj); 162 return true; 163 } 164 GetTransformMatrixFromPose(ArSession * ar_session,const ArPose * ar_pose,glm::mat4 * out_model_mat)165 void GetTransformMatrixFromPose(ArSession *ar_session, 166 const ArPose *ar_pose, 167 glm::mat4 *out_model_mat) { 168 if (out_model_mat == nullptr) { 169 LOGE("util::GetTransformMatrixFromPose model_mat is null."); 170 return; 171 } 172 ArPose_getMatrix(ar_session, ar_pose, 173 glm::value_ptr(*out_model_mat)); 174 } 175 GetPlaneNormal(const ArSession * ar_session,const ArPose & plane_pose)176 glm::vec3 GetPlaneNormal(const ArSession *ar_session, 177 const ArPose &plane_pose) { 178 float plane_pose_raw[7] = {0.f}; 179 ArPose_getPoseRaw(ar_session, &plane_pose, plane_pose_raw); 180 glm::quat plane_quaternion(plane_pose_raw[3], plane_pose_raw[0], 181 plane_pose_raw[1], plane_pose_raw[2]); 182 // Get normal vector, normal is defined to be positive Y-position in local 183 // frame. 184 return glm::rotate(plane_quaternion, glm::vec3(0., 1.f, 0.)); 185 } 186 CalculateDistanceToPlane(const ArSession * ar_session,const ArPose & plane_pose,const ArPose & camera_pose)187 float CalculateDistanceToPlane(const ArSession *ar_session, 188 const ArPose &plane_pose, 189 const ArPose &camera_pose) { 190 float plane_pose_raw[7] = {0.f}; 191 ArPose_getPoseRaw(ar_session, &plane_pose, plane_pose_raw); 192 glm::vec3 plane_position(plane_pose_raw[4], plane_pose_raw[5], 193 plane_pose_raw[6]); 194 glm::vec3 normal = GetPlaneNormal(ar_session, plane_pose); 195 196 float camera_pose_raw[7] = {0.f}; 197 ArPose_getPoseRaw(ar_session, &camera_pose, camera_pose_raw); 198 glm::vec3 camera_P_plane(camera_pose_raw[4] - plane_position.x, 199 camera_pose_raw[5] - plane_position.y, 200 camera_pose_raw[6] - plane_position.z); 201 return glm::dot(normal, camera_P_plane); 202 } 203 GetCameraRotationMatrix(float cameraOutRaw[])204 glm::mat4 GetCameraRotationMatrix(float cameraOutRaw[]) { 205 glm::mat4 cameraRotation(1); 206 glm::quat cameraQuat = glm::quat(cameraOutRaw[0], cameraOutRaw[1], cameraOutRaw[2], 207 cameraOutRaw[3]); 208 cameraRotation = glm::toMat4(cameraQuat); 209 glm::vec4 temp = cameraRotation[0]; 210 cameraRotation[0] = cameraRotation[2]; 211 cameraRotation[2] = temp; 212 return cameraRotation; 213 } 214 GetCameraInfo(ArSession * arSession,ArFrame * arFrame,glm::vec3 & cameraPos,glm::mat4 & cameraRotation)215 void GetCameraInfo(ArSession* arSession, ArFrame* arFrame, glm::vec3& cameraPos, glm::mat4& cameraRotation) { 216 //Acquire camera 217 ArCamera *ar_camera; 218 ArFrame_acquireCamera(arSession, arFrame, &ar_camera); 219 220 //Get camera pose 221 ArPose *camera_pose = nullptr; 222 ArPose_create(arSession, nullptr, &camera_pose); 223 ArCamera_getDisplayOrientedPose(arSession, ar_camera, camera_pose); 224 225 //Get camera raw info 226 float outCameraRaw[] = {0, 0, 0, 0, 0, 0, 0}; 227 ArPose_getPoseRaw(arSession, camera_pose, outCameraRaw); 228 ArPose_destroy(camera_pose); 229 230 //Write to out variables 231 cameraPos = glm::vec3(outCameraRaw[4], outCameraRaw[5], outCameraRaw[6]); 232 cameraRotation = util::GetCameraRotationMatrix(outCameraRaw); 233 234 //Release camera 235 ArCamera_release(ar_camera); 236 } 237 GlmMatToSkMat(const glm::mat4 m)238 SkMatrix44 GlmMatToSkMat(const glm::mat4 m) { 239 SkMatrix44 skMat = SkMatrix44::kIdentity_Constructor; 240 for (int i = 0; i < 4; i++) { 241 for (int j = 0; j < 4; j++) { 242 skMat.set(j, i, m[i][j]); 243 } 244 } 245 return skMat; 246 } 247 SkMatToGlmMat(const SkMatrix44 m)248 glm::mat4 SkMatToGlmMat(const SkMatrix44 m) { 249 glm::mat4 glmMat(1); 250 for (int i = 0; i < 4; i++) { 251 for (int j = 0; j < 4; j++) { 252 glmMat[i][j] = m.get(j, i); 253 } 254 } 255 return glmMat; 256 } 257 Log4x4Matrix(float raw_matrix[16])258 void Log4x4Matrix(float raw_matrix[16]) { 259 LOGI( 260 "%f, %f, %f, %f\n" 261 "%f, %f, %f, %f\n" 262 "%f, %f, %f, %f\n" 263 "%f, %f, %f, %f\n", 264 raw_matrix[0], raw_matrix[1], raw_matrix[2], raw_matrix[3], raw_matrix[4], 265 raw_matrix[5], raw_matrix[6], raw_matrix[7], raw_matrix[8], raw_matrix[9], 266 raw_matrix[10], raw_matrix[11], raw_matrix[12], raw_matrix[13], 267 raw_matrix[14], raw_matrix[15]); 268 } 269 LogGlmMat(glm::mat4 m,char * type)270 void LogGlmMat(glm::mat4 m, char *type) { 271 std::string str = glm::to_string(m); 272 LOGE("glm Matrix - %s: %s\n", type, str.c_str()); 273 } 274 LogSkMat44(SkMatrix44 m,char * type)275 void LogSkMat44(SkMatrix44 m, char *type) { 276 LOGE("SkMatrix - %s: [%g, %g, %g, %g] || [%g, %g, %g, %g] || [%g, %g, %g, %g] || [%g, %g, %g, %g] \n", 277 type, 278 m.get(0, 0), m.get(1, 0), m.get(2, 0), m.get(3, 0), 279 m.get(0, 1), m.get(1, 1), m.get(2, 1), m.get(3, 1), 280 m.get(0, 2), m.get(1, 2), m.get(2, 2), m.get(3, 2), 281 m.get(0, 3), m.get(1, 3), m.get(2, 3), m.get(3, 3) 282 ); 283 } 284 LogSkMat(SkMatrix m,char * type)285 void LogSkMat(SkMatrix m, char *type) { 286 LOGE("SkMatrix - %s: [%g, %g, %g] || [%g, %g, %g] || [%g, %g, %g] \n", type, 287 m.get(0), m.get(3), m.get(6), 288 m.get(1), m.get(4), m.get(7), 289 m.get(2), m.get(5), m.get(8) 290 ); 291 } 292 LogOrientation(float rotationDirection,float angleRad,char * type)293 void LogOrientation(float rotationDirection, float angleRad, char *type) { 294 LOGI("Plane orientation: %s", type); 295 LOGI("Cross dotted with zDir:", rotationDirection); 296 if (rotationDirection == -1) { 297 LOGI("Counter Clockwise %.6f degrees rotation: ", glm::degrees(angleRad)); 298 } else { 299 LOGI("Clockwise %.6f degrees rotation: ", glm::degrees(angleRad)); 300 } 301 } 302 Dot(glm::vec3 u,glm::vec3 v)303 float Dot(glm::vec3 u, glm::vec3 v) { 304 float result = u.x * v.x + u.y * v.y + u.z * v.z; 305 return result; 306 } 307 Magnitude(glm::vec3 u)308 float Magnitude(glm::vec3 u) { 309 float result = u.x * u.x + u.y * u.y + u.z * u.z; 310 return sqrt(result); 311 } 312 AngleRad(glm::vec3 u,glm::vec3 v)313 float AngleRad(glm::vec3 u, glm::vec3 v) { 314 float dot = util::Dot(u, v); 315 float scale = (util::Magnitude(u) * util::Magnitude(v)); 316 float cosine = dot / scale; 317 float acosine = acos(cosine); 318 return acosine; 319 } 320 ProjectOntoPlane(glm::vec3 in,glm::vec3 normal)321 glm::vec3 ProjectOntoPlane(glm::vec3 in, glm::vec3 normal) { 322 float dot = util::Dot(in, normal); 323 float multiplier = dot / (util::Magnitude(normal) * util::Magnitude(normal)); 324 glm::vec3 out = in - multiplier * normal; 325 return out; 326 } 327 328 } // namespace util 329 } // namespace hello_ar 330