util.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014 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 "tango-gl/util.h"
18 #include <rtabmap/utilite/ULogger.h>
19 
20 namespace tango_gl {
21 
22 namespace {
23 int NormalizedColorCameraRotation(int camera_rotation) {
24  int camera_n = 0;
25  switch (camera_rotation) {
26  case 90:
27  camera_n = 1;
28  break;
29  case 180:
30  camera_n = 2;
31  break;
32  case 270:
33  camera_n = 3;
34  break;
35  default:
36  camera_n = 0;
37  break;
38  }
39  return camera_n;
40 }
41 } // annonymous namespace
42 
43 void util::CheckGlError(const char* operation) {
44  for (GLint error = glGetError(); error; error = glGetError()) {
45  LOGE("after %s() glError (0x%x)\n", operation, error);
46  }
47 }
48 
49 // Convenience function used in CreateProgram below.
50 static GLuint LoadShader(GLenum shader_type, const char* shader_source) {
51  GLuint shader = glCreateShader(shader_type);
52  if (shader) {
53  glShaderSource(shader, 1, &shader_source, NULL);
54  glCompileShader(shader);
55  GLint compiled = 0;
56  glGetShaderiv(shader, GL_COMPILE_STATUS, &compiled);
57  if (!compiled) {
58  GLint info_len = 0;
59  glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &info_len);
60  if (info_len) {
61  char* buf = (char*) malloc(info_len);
62  if (buf) {
63  glGetShaderInfoLog(shader, info_len, NULL, buf);
64  LOGE("Could not compile shader %d:\n%s\n", shader_type, buf);
65  free(buf);
66  }
67  glDeleteShader(shader);
68  shader = 0;
69  }
70  }
71  }
72  return shader;
73 }
74 
75 GLuint util::CreateProgram(const char* vertex_source,
76  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("glAttachShader");
91  glAttachShader(program, fragment_shader);
92  CheckGlError("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 = (char*) malloc(buf_length);
101  if (buf) {
102  glGetProgramInfoLog(program, buf_length, NULL, buf);
103  LOGE("Could not link program:\n%s\n", buf);
104  free(buf);
105  }
106  }
107  glDeleteProgram(program);
108  program = 0;
109  }
110  }
111  CheckGlError("CreateProgram");
112  return program;
113 }
114 
115 void util::DecomposeMatrix (const glm::mat4& transform_mat,
116  glm::vec3& translation,
117  glm::quat& rotation,
118  glm::vec3& scale) {
119  float scale_x = glm::length( glm::vec3( transform_mat[0][0], transform_mat[1][0], transform_mat[2][0] ) );
120  float scale_y = glm::length( glm::vec3( transform_mat[0][1], transform_mat[1][1], transform_mat[2][1] ) );
121  float scale_z = glm::length( glm::vec3( transform_mat[0][2], transform_mat[1][2], transform_mat[2][2] ) );
122 
123 
124  float determinant = glm::determinant( transform_mat );
125  if( determinant < 0.0 )
126  scale_x = -scale_x;
127 
128  translation.x = transform_mat[3][0];
129  translation.y = transform_mat[3][1];
130  translation.z = transform_mat[3][2];
131 
132  float inverse_scale_x = 1.0 / scale_x;
133  float inverse_scale_y = 1.0 / scale_y;
134  float inverse_scale_z = 1.0 / scale_z;
135 
136  glm::mat4 transform_unscaled = transform_mat;
137 
138  transform_unscaled[0][0] *= inverse_scale_x;
139  transform_unscaled[1][0] *= inverse_scale_x;
140  transform_unscaled[2][0] *= inverse_scale_x;
141 
142  transform_unscaled[0][1] *= inverse_scale_y;
143  transform_unscaled[1][1] *= inverse_scale_y;
144  transform_unscaled[2][1] *= inverse_scale_y;
145 
146  transform_unscaled[0][2] *= inverse_scale_z;
147  transform_unscaled[1][2] *= inverse_scale_z;
148  transform_unscaled[2][2] *= inverse_scale_z;
149 
150  rotation = glm::quat_cast( transform_mat );
151 
152  scale.x = scale_x;
153  scale.y = scale_y;
154  scale.z = scale_z;
155 }
156 
157 glm::vec3 util::GetColumnFromMatrix(const glm::mat4& mat, const int col) {
158  return glm::vec3(mat[col][0], mat[col][1], mat[col][2]);
159 }
160 
161 glm::vec3 util::GetTranslationFromMatrix(const glm::mat4& mat) {
162  return glm::vec3(mat[3][0], mat[3][1], mat[3][2]);
163 }
164 
165 float util::Clamp(float value, float min, float max) {
166  return value < min ? min : (value > max ? max : value);
167 }
168 
169 // Print out a column major matrix.
170 void util::PrintMatrix(const glm::mat4& matrix) {
171  int i;
172  for (i = 0; i < 4; i++) {
173  LOGI("[ %f, %f, %f, %f ]", matrix[0][i], matrix[1][i], matrix[2][i],
174  matrix[3][i]);
175  }
176  LOGI(" ");
177 }
178 
179 void util::PrintVector(const glm::vec3& vector) {
180  LOGI("[ %f, %f, %f ]", vector[0], vector[1], vector[2]);
181  LOGI(" ");
182 }
183 
184 void util::PrintQuaternion(const glm::quat& quat) {
185  LOGI("[ %f, %f, %f, %f ]", quat[0], quat[1], quat[2], quat[3]);
186  LOGI(" ");
187 }
188 
189 glm::vec3 util::LerpVector(const glm::vec3& x, const glm::vec3& y, float a) {
190  return x * (1.0f - a) + y * a;
191 }
192 
193 float util::DistanceSquared(const glm::vec3& v1, const glm::vec3& v2) {
194  glm::vec3 delta = v2 - v1;
195  return glm::dot(delta, delta);
196 }
197 
198 bool util::SegmentAABBIntersect(const glm::vec3& aabb_min,
199  const glm::vec3& aabb_max,
200  const glm::vec3& start,
201  const glm::vec3& end) {
202  float tmin, tmax, tymin, tymax, tzmin, tzmax;
203  glm::vec3 direction = end - start;
204  if (direction.x >= 0) {
205  tmin = (aabb_min.x - start.x) / direction.x;
206  tmax = (aabb_max.x - start.x) / direction.x;
207  } else {
208  tmin = (aabb_max.x - start.x) / direction.x;
209  tmax = (aabb_min.x - start.x) / direction.x;
210  }
211  if (direction.y >= 0) {
212  tymin = (aabb_min.y - start.y) / direction.y;
213  tymax = (aabb_max.y - start.y) / direction.y;
214  } else {
215  tymin = (aabb_max.y - start.y) / direction.y;
216  tymax = (aabb_min.y - start.y) / direction.y;
217  }
218  if ((tmin > tymax) || (tymin > tmax)) return false;
219 
220  if (tymin > tmin) tmin = tymin;
221  if (tymax < tmax) tmax = tymax;
222  if (direction.z >= 0) {
223  tzmin = (aabb_min.z - start.z) / direction.z;
224  tzmax = (aabb_max.z - start.z) / direction.z;
225  } else {
226  tzmin = (aabb_max.z - start.z) / direction.z;
227  tzmax = (aabb_min.z - start.z) / direction.z;
228  }
229  if ((tmin > tzmax) || (tzmin > tmax)) return false;
230 
231  if (tzmin > tmin) tmin = tzmin;
232  if (tzmax < tmax) tmax = tzmax;
233  // Use the full length of the segment.
234  return ((tmin < 1.0f) && (tmax > 0));
235 }
236 
237 glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
238  return glm::vec3(mat * glm::vec4(vec, 1.0f));
239 }
240 
241 } // namespace tango_gl
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::NormalizedColorCameraRotation
int NormalizedColorCameraRotation(int camera_rotation)
Definition: util.h:211
tango_gl::util::PrintQuaternion
void PrintQuaternion(const glm::quat &quat)
Definition: util.cpp:184
tango_gl::util::PrintVector
void PrintVector(const glm::vec3 &vector)
Definition: util.cpp:179
tango_gl::util::DecomposeMatrix
void DecomposeMatrix(const glm::mat4 &transform_mat, glm::vec3 &translation, glm::quat &rotation, glm::vec3 &scale)
Definition: util.cpp:115
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
v1
v1
tango_gl::util::ApplyTransform
glm::vec3 ApplyTransform(const glm::mat4 &mat, const glm::vec3 &vec)
Definition: util.cpp:237
glm::quat
highp_quat quat
Quaternion of default single-precision floating-point numbers.
Definition: fwd.hpp:68
tango_gl::util::CheckGlError
void CheckGlError(const char *operation)
Definition: util.cpp:43
end
end
col
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ColXpr col(Index i) const
util.h
glm::vec3
highp_vec3 vec3
Definition: type_vec.hpp:392
y
Matrix3f y
tango_gl::util::PrintMatrix
void PrintMatrix(const glm::mat4 &matrix)
Definition: util.cpp:170
glm::quat_cast
GLM_FUNC_DECL detail::tquat< T, P > quat_cast(detail::tmat3x3< T, P > const &x)
glm::determinant
GLM_FUNC_DECL T determinant(matType< T, P > const &m)
glm::detail::tquat
Definition: fwd.hpp:41
glm::detail::tvec3
Definition: type_mat.hpp:37
glm::detail::tmat4x4
Definition: type_mat.hpp:47
determinant
void determinant(const MatrixType &m)
LOGE
#define LOGE(...)
Definition: tango-gl/include/tango-gl/util.h:61
glm::dot
GLM_FUNC_DECL T dot(vecType< T, P > const &x, vecType< T, P > const &y)
GL_FALSE
#define GL_FALSE
Definition: dummy.cpp:79
glm::detail::tvec3::z
T z
Definition: type_vec3.hpp:86
tango_gl::util::SegmentAABBIntersect
bool SegmentAABBIntersect(const glm::vec3 &aabb_min, const glm::vec3 &aabb_max, const glm::vec3 &start, const glm::vec3 &end)
Definition: util.cpp:198
translation
translation
delta
def delta(g0, g1)
GLuint
unsigned int GLuint
Definition: dummy.cpp:78
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
tango_gl::util::CreateProgram
GLuint CreateProgram(const char *vertex_source, const char *fragment_source)
Definition: util.cpp:75
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
glm::detail::tvec4
Definition: type_mat.hpp:38
glm::detail::tvec3::y
T y
Definition: type_vec3.hpp:85
x
x
error
void error(const char *str)
glm::detail::tvec3::x
T x
Definition: type_vec3.hpp:84
LOGI
#define LOGI(...)
Definition: tango-gl/include/tango-gl/util.h:53
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
tango_gl::util::Clamp
float Clamp(float value, float min, float max)
Definition: util.cpp:165
NULL
#define NULL
tango_gl
Definition: axis.cpp:20
matrix
Map< const Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(const T *data, int rows, int cols, int stride)
tango_gl::util::GetTranslationFromMatrix
glm::vec3 GetTranslationFromMatrix(const glm::mat4 &mat)
Definition: util.cpp:161
value
value
tango_gl::util::GetColumnFromMatrix
glm::vec3 GetColumnFromMatrix(const glm::mat4 &mat, const int col)
Definition: util.cpp:157
i
int i
tango_gl::util::DistanceSquared
float DistanceSquared(const glm::vec3 &v1, const glm::vec3 &v2)
Definition: util.cpp:193
mat
else mat
tango_gl::util::LerpVector
glm::vec3 LerpVector(const glm::vec3 &x, const glm::vec3 &y, float a)
Definition: util.cpp:189
tango_gl::LoadShader
static GLuint LoadShader(GLenum shader_type, const char *shader_source)
Definition: util.cpp:50
v2
v2

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:23