band.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/band.h"
18 #include "tango-gl/util.h"
19 
20 namespace tango_gl {
21 
22 // Set band resolution to 0.01m(1cm) when using UpdateVertexArray()
23 static const float kMinDistanceSquared = 0.0001f;
24 
25 Band::Band(const unsigned int max_length)
26  : band_width_(0.2), max_length_(max_length) {
27  SetShader();
28  vertices_v_.reserve(max_length);
29  pivot_left = glm::vec3(0, 0, 0);
30  pivot_right = glm::vec3(0, 0, 0);
31 }
32 
33 void Band::SetWidth(const float width) {
34  band_width_ = width;
35 }
36 
38  // First 2 vertices of a band + 3 arrow head vertices.
39  bool need_to_initialize = (vertices_v_.size() < 5);
40 
41  bool sufficient_delta = false;
42  if (!need_to_initialize) {
43  // Band head is the first two vertices after arrow head.
44  glm::vec3 band_front = 0.5f * (vertices_v_[vertices_v_.size() - 4] +
45  vertices_v_[vertices_v_.size() - 5]);
46  sufficient_delta = kMinDistanceSquared <
48  }
49 
50  if (need_to_initialize || sufficient_delta) {
51  glm::vec3 left = glm::vec3(-band_width_ * 0.5f, 0, 0);
52  glm::vec3 right = glm::vec3(band_width_ * 0.5f, 0, 0);
53  glm::vec3 arrow_left = glm::vec3(-band_width_ * 0.75f, 0, 0);
54  glm::vec3 arrow_right = glm::vec3(band_width_ * 0.75f, 0, 0);
55  glm::vec3 arrow_front = glm::vec3(0, 0, -band_width_ * 0.75f);
56 
57  // If keep right pivot point, or normal mode,
58  // then only update left pivot point.
59  if (mode == BandMode::kNormal || mode == BandMode::kKeepRight) {
61  }
62  // If keep left pivot point, or normal mode,
63  // then only update right pivot point.
64  if (mode == BandMode::kNormal || mode == BandMode::kKeepLeft) {
66  }
67 
68  glm::mat4 head_m = m;
69 
70  if (mode != BandMode::kNormal) {
71  glm::vec3 up = glm::vec3(0, 1.0f, 0);
74  head_m = glm::inverse(glm::lookAt(glm::vec3(0, 0, 0), heading, up));
75  head_m[3][0] = position.x;
76  head_m[3][1] = position.y;
77  head_m[3][2] = position.z;
78  }
79 
80  if (need_to_initialize) {
81  vertices_v_.resize(5);
82  } else {
83  vertices_v_.resize(vertices_v_.size() + 2);
84  }
85 
86  size_t insertion_start = vertices_v_.size() - 5;
87  vertices_v_[insertion_start + 0] = pivot_left;
88  vertices_v_[insertion_start + 1] = pivot_right;
89  vertices_v_[insertion_start + 2] = util::ApplyTransform(head_m, arrow_left);
90  vertices_v_[insertion_start + 3] = util::ApplyTransform(head_m, arrow_right);
91  vertices_v_[insertion_start + 4] = util::ApplyTransform(head_m, arrow_front);
92 
93  if (vertices_v_.size() > max_length_) {
94  vertices_v_.erase(vertices_v_.begin(), vertices_v_.begin() + 2);
95  }
96  }
97 }
98 
100  // Defualt to call update with normal mode.
101  UpdateVertexArray(m, BandMode::kNormal);
102 }
103 
104 void Band::SetVertexArray(const std::vector<glm::vec3>& v,
105  const glm::vec3& up) {
106  vertices_v_.clear();
107  vertices_v_.reserve(2 * v.size());
108  if (v.size() < 2)
109  return;
110 
111  for (size_t i = 0; i < v.size() - 1; ++i) {
112  glm::vec3 gl_p_world_a = v[i];
113  glm::vec3 gl_p_world_b = v[i + 1];
114  glm::vec3 dir = glm::normalize(gl_p_world_b - gl_p_world_a);
115  glm::vec3 left = glm::cross(up, dir);
116  glm::normalize(left);
117 
118  vertices_v_.push_back(gl_p_world_a + (band_width_ / 2.0f * left));
119  vertices_v_.push_back(gl_p_world_a - (band_width_ / 2.0f * left));
120 
121  // Cap the end of the path.
122  if (i == v.size() - 2) {
123  vertices_v_.push_back(gl_p_world_b + (band_width_ / 2.0f * left));
124  vertices_v_.push_back(gl_p_world_b - (band_width_ / 2.0f * left));
125  }
126  }
127 
128 }
129 
131 
132 void Band::Render(const glm::mat4& projection_mat,
133  const glm::mat4& view_mat) const {
134  glUseProgram(shader_program_);
135  glm::mat4 model_mat = GetTransformationMatrix();
136  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
138 
139  glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
140 
141  glEnableVertexAttribArray(attrib_vertices_);
142  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
143  sizeof(glm::vec3), &vertices_v_[0]);
144  glDrawArrays(GL_TRIANGLE_STRIP, 0, vertices_v_.size());
145  glDisableVertexAttribArray(attrib_vertices_);
146  glUseProgram(0);
147 }
148 
149 } // namespace tango_gl
tango_gl::DrawableObject::alpha_
float alpha_
Definition: drawable_object.h:50
tango_gl::DrawableObject::attrib_vertices_
GLuint attrib_vertices_
Definition: drawable_object.h:59
tango_gl::DrawableObject::red_
float red_
Definition: drawable_object.h:47
tango_gl::util::ApplyTransform
glm::vec3 ApplyTransform(const glm::mat4 &mat, const glm::vec3 &vec)
Definition: util.cpp:237
tango_gl::Band::Band
Band(const unsigned int max_legnth)
Definition: band.cpp:25
tango_gl::kMinDistanceSquared
static const float kMinDistanceSquared
Definition: band.cpp:23
util.h
glm::value_ptr
GLM_FUNC_DECL const genType::value_type * value_ptr(genType const &vec)
glm::vec3
highp_vec3 vec3
Definition: type_vec.hpp:392
glm::normalize
GLM_FUNC_DECL genType normalize(genType const &x)
tango_gl::DrawableObject::blue_
float blue_
Definition: drawable_object.h:49
glm::detail::tvec3
Definition: type_mat.hpp:37
glm::detail::tmat4x4
Definition: type_mat.hpp:47
tango_gl::DrawableObject::green_
float green_
Definition: drawable_object.h:48
band.h
mode
const DiscreteKey mode
GL_FALSE
#define GL_FALSE
Definition: dummy.cpp:79
glm::lookAt
GLM_FUNC_DECL detail::tmat4x4< T, P > lookAt(detail::tvec3< T, P > const &eye, detail::tvec3< T, P > const &center, detail::tvec3< T, P > const &up)
tango_gl::Band::SetWidth
void SetWidth(const float width)
Definition: band.cpp:33
tango_gl::DrawableObject::uniform_color_
GLuint uniform_color_
Definition: drawable_object.h:57
tango_gl::Band::pivot_left
glm::vec3 pivot_left
Definition: band.h:51
tango_gl::Band::max_length_
unsigned int max_length_
Definition: band.h:48
m
Matrix3f m
glm::cross
GLM_FUNC_DECL detail::tvec3< T, P > cross(detail::tvec3< T, P > const &x, detail::tvec3< T, P > const &y)
tango_gl::Band::UpdateVertexArray
void UpdateVertexArray(const glm::mat4 m, BandMode mode)
Definition: band.cpp:37
tango_gl::Band::BandMode
BandMode
Definition: band.h:28
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
tango_gl::Band::band_width_
float band_width_
Definition: band.h:47
tango_gl::Band::ClearVertexArray
void ClearVertexArray()
Definition: band.cpp:130
tango_gl::DrawableObject::SetShader
void SetShader()
Definition: drawable_object.cpp:22
v
Array< int, Dynamic, 1 > v
position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
tango_gl::DrawableObject::uniform_mvp_mat_
GLuint uniform_mvp_mat_
Definition: drawable_object.h:58
glUniformMatrix4fv
void glUniformMatrix4fv(GLuint, int, int, float *)
Definition: dummy.cpp:80
tango_gl
Definition: axis.cpp:20
tango_gl::util::GetTranslationFromMatrix
glm::vec3 GetTranslationFromMatrix(const glm::mat4 &mat)
Definition: util.cpp:161
tango_gl::DrawableObject::shader_program_
GLuint shader_program_
Definition: drawable_object.h:56
tango_gl::Transform::GetTransformationMatrix
glm::mat4 GetTransformationMatrix() const
Definition: transform.cpp:67
tango_gl::Band::Render
void Render(const glm::mat4 &projection_mat, const glm::mat4 &view_mat) const
Definition: band.cpp:132
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
i
int i
tango_gl::Band::SetVertexArray
void SetVertexArray(const std::vector< glm::vec3 > &v, const glm::vec3 &up)
Definition: band.cpp:104
tango_gl::Band::pivot_right
glm::vec3 pivot_right
Definition: band.h:52
tango_gl::util::DistanceSquared
float DistanceSquared(const glm::vec3 &v1, const glm::vec3 &v2)
Definition: util.cpp:193
tango_gl::Band::vertices_v_
std::vector< glm::vec3 > vertices_v_
Definition: band.h:49


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