band.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2014 Google Inc. All Rights Reserved.
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "tango-gl/band.h"
00018 #include "tango-gl/util.h"
00019 
00020 namespace tango_gl {
00021 
00022 // Set band resolution to 0.01m(1cm) when using UpdateVertexArray()
00023 static const float kMinDistanceSquared = 0.0001f;
00024 
00025 Band::Band(const unsigned int max_length)
00026     : band_width_(0.2), max_length_(max_length) {
00027   SetShader();
00028   vertices_v_.reserve(max_length);
00029   pivot_left = glm::vec3(0, 0, 0);
00030   pivot_right = glm::vec3(0, 0, 0);
00031 }
00032 
00033 void Band::SetWidth(const float width) {
00034   band_width_ = width;
00035 }
00036 
00037 void Band::UpdateVertexArray(const glm::mat4 m, BandMode mode) {
00038   // First 2 vertices of a band + 3 arrow head vertices.
00039   bool need_to_initialize = (vertices_v_.size() < 5);
00040 
00041   bool sufficient_delta = false;
00042   if (!need_to_initialize) {
00043     // Band head is the first two vertices after arrow head.
00044     glm::vec3 band_front = 0.5f * (vertices_v_[vertices_v_.size() - 4] +
00045                                    vertices_v_[vertices_v_.size() - 5]);
00046     sufficient_delta = kMinDistanceSquared <
00047         util::DistanceSquared(band_front, util::GetTranslationFromMatrix(m));
00048   }
00049 
00050   if (need_to_initialize || sufficient_delta) {
00051     glm::vec3 left  = glm::vec3(-band_width_ * 0.5f, 0, 0);
00052     glm::vec3 right = glm::vec3(band_width_ * 0.5f, 0, 0);
00053     glm::vec3 arrow_left = glm::vec3(-band_width_ * 0.75f, 0, 0);
00054     glm::vec3 arrow_right = glm::vec3(band_width_ * 0.75f, 0, 0);
00055     glm::vec3 arrow_front = glm::vec3(0, 0, -band_width_ * 0.75f);
00056 
00057     // If keep right pivot point, or normal mode,
00058     // then only update left pivot point.
00059     if (mode == BandMode::kNormal || mode == BandMode::kKeepRight) {
00060       pivot_left = util::ApplyTransform(m, left);
00061     }
00062     // If keep left pivot point, or normal mode,
00063     // then only update right pivot point.
00064     if (mode == BandMode::kNormal || mode == BandMode::kKeepLeft) {
00065       pivot_right = util::ApplyTransform(m, right);
00066     }
00067 
00068     glm::mat4 head_m = m;
00069 
00070     if (mode != BandMode::kNormal) {
00071       glm::vec3 up = glm::vec3(0, 1.0f, 0);
00072       glm::vec3 position = 0.5f * (pivot_left + pivot_right);
00073       glm::vec3 heading = glm::cross(up, pivot_right-pivot_left);
00074       head_m = glm::inverse(glm::lookAt(glm::vec3(0, 0, 0), heading, up));
00075       head_m[3][0] = position.x;
00076       head_m[3][1] = position.y;
00077       head_m[3][2] = position.z;
00078     }
00079 
00080     if (need_to_initialize) {
00081       vertices_v_.resize(5);
00082     } else {
00083       vertices_v_.resize(vertices_v_.size() + 2);
00084     }
00085 
00086     size_t insertion_start = vertices_v_.size() - 5;
00087     vertices_v_[insertion_start + 0] = pivot_left;
00088     vertices_v_[insertion_start + 1] = pivot_right;
00089     vertices_v_[insertion_start + 2] = util::ApplyTransform(head_m, arrow_left);
00090     vertices_v_[insertion_start + 3] = util::ApplyTransform(head_m, arrow_right);
00091     vertices_v_[insertion_start + 4] = util::ApplyTransform(head_m, arrow_front);
00092 
00093     if (vertices_v_.size() > max_length_) {
00094       vertices_v_.erase(vertices_v_.begin(), vertices_v_.begin() + 2);
00095     }
00096   }
00097 }
00098 
00099 void Band::UpdateVertexArray(const glm::mat4 m) {
00100   // Defualt to call update with normal mode.
00101   UpdateVertexArray(m, BandMode::kNormal);
00102 }
00103 
00104 void Band::SetVertexArray(const std::vector<glm::vec3>& v,
00105                           const glm::vec3& up) {
00106   vertices_v_.clear();
00107   vertices_v_.reserve(2 * v.size());
00108   if (v.size() < 2)
00109     return;
00110 
00111   for (size_t i = 0; i < v.size() - 1; ++i) {
00112     glm::vec3 gl_p_world_a = v[i];
00113     glm::vec3 gl_p_world_b = v[i + 1];
00114     glm::vec3 dir = glm::normalize(gl_p_world_b - gl_p_world_a);
00115     glm::vec3 left = glm::cross(up, dir);
00116     glm::normalize(left);
00117 
00118     vertices_v_.push_back(gl_p_world_a + (band_width_ / 2.0f * left));
00119     vertices_v_.push_back(gl_p_world_a - (band_width_ / 2.0f * left));
00120 
00121     // Cap the end of the path.
00122     if (i == v.size() - 2) {
00123       vertices_v_.push_back(gl_p_world_b + (band_width_ / 2.0f * left));
00124       vertices_v_.push_back(gl_p_world_b - (band_width_ / 2.0f * left));
00125     }
00126   }
00127 
00128 }
00129 
00130 void Band::ClearVertexArray() { vertices_v_.clear(); }
00131 
00132 void Band::Render(const glm::mat4& projection_mat,
00133                   const glm::mat4& view_mat) const {
00134   glUseProgram(shader_program_);
00135   glm::mat4 model_mat = GetTransformationMatrix();
00136   glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
00137   glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
00138 
00139   glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
00140 
00141   glEnableVertexAttribArray(attrib_vertices_);
00142   glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
00143                         sizeof(glm::vec3), &vertices_v_[0]);
00144   glDrawArrays(GL_TRIANGLE_STRIP, 0, vertices_v_.size());
00145   glDisableVertexAttribArray(attrib_vertices_);
00146   glUseProgram(0);
00147 }
00148 
00149 }  // namespace tango_gl


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:18