selectionTransformTool.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00039 
00040 #include <math.h>
00041 #include <pcl/apps/point_cloud_editor/selectionTransformTool.h>
00042 #include <pcl/apps/point_cloud_editor/cloud.h>
00043 #include <pcl/apps/point_cloud_editor/selection.h>
00044 #include <pcl/apps/point_cloud_editor/transformCommand.h>
00045 #include <pcl/apps/point_cloud_editor/commandQueue.h>
00046 #include <pcl/apps/point_cloud_editor/common.h>
00047 
00048 const float SelectionTransformTool::DEFAULT_TRANSLATE_FACTOR_ = 0.001;
00049 
00050 SelectionTransformTool::SelectionTransformTool (ConstSelectionPtr selection_ptr,
00051                                                 CloudPtr cloud_ptr,
00052                                                 CommandQueuePtr command_queue_ptr)
00053   : selection_ptr_(selection_ptr),
00054     cloud_ptr_(cloud_ptr),
00055     command_queue_ptr_(command_queue_ptr),
00056     translate_factor_(DEFAULT_TRANSLATE_FACTOR_)
00057 {
00058   std::fill_n(center_xyz_, XYZ_SIZE, 0);
00059   setIdentity(transform_matrix_);
00060 }
00061 
00062 void
00063 SelectionTransformTool::start (int x, int y, BitMask modifiers, BitMask buttons)
00064 {
00065   if ((!cloud_ptr_) || (!selection_ptr_) || selection_ptr_->empty())
00066     return;
00067   if (!(buttons & LEFT))
00068     return;
00069   modifiers_ = modifiers;
00070   x_ = x;
00071   y_ = y;
00072   findSelectionCenter();
00073   setIdentity(transform_matrix_);
00074   trackball_.start(x, y);
00075 }
00076 
00077 void
00078 SelectionTransformTool::update (int x, int y, BitMask, BitMask buttons)
00079 {
00080   if (!cloud_ptr_)
00081     return;
00082   if (!(buttons & LEFT))
00083     return;
00084   int dx = (x - x_);
00085   int dy = (y - y_);
00086   if (dx == 0 && dy == 0)
00087     return;
00088   trackball_.update(x, y);
00089 
00090   if (modifiers_ & CTRL)
00091   {
00092     // selection motion is not applied directly (waits for end)
00093     // as such we can not update x and y immediately
00094     float scale = 1.0f / cloud_ptr_->getScalingFactor();
00095     cloud_ptr_->setSelectionTranslation ((float) dx * translate_factor_ * scale,
00096                                          (float) -dy * translate_factor_ * scale,
00097                                          0.0f);
00098     return;
00099   }
00100   else if (modifiers_ & ALT)
00101   {
00102     // selection motion is not applied directly (waits for end)
00103     // as such we can not update x and y immediately
00104     float scale = 1.0f / cloud_ptr_->getScalingFactor();
00105     cloud_ptr_->setSelectionTranslation (0.0f,
00106                                          0.0f,
00107                                          (float) dy * translate_factor_ * scale);
00108     return;
00109   }
00110   float transform[MATRIX_SIZE];
00111   trackball_.getRotationMatrix(transform);
00112   transform_matrix_[12] -= center_xyz_[0];
00113   transform_matrix_[13] -= center_xyz_[1];
00114   transform_matrix_[14] -= center_xyz_[2];
00115   ::multMatrix(transform_matrix_, transform, transform_matrix_);
00116   transform_matrix_[12] += center_xyz_[0];
00117   transform_matrix_[13] += center_xyz_[1];
00118   transform_matrix_[14] += center_xyz_[2];
00119   cloud_ptr_ -> setSelectionRotation(transform_matrix_);
00120 
00121   x_ = x;
00122   y_ = y;
00123 }
00124 
00125 void
00126 SelectionTransformTool::end (int x, int y, BitMask modifiers, BitMask buttons)
00127 {
00128   if (!(buttons & LEFT))
00129     return;
00130 
00131   float scale = 1.0f / cloud_ptr_->getScalingFactor();
00132   int dx = (x - x_);
00133   int dy = (y - y_);
00134   update(x, y, modifiers, buttons);
00135   if (modifiers_ & CTRL)
00136   {
00137     boost::shared_ptr<TransformCommand> c(new TransformCommand(selection_ptr_,
00138       cloud_ptr_, transform_matrix_, (float) dx * translate_factor_ * scale,
00139       (float) -dy * translate_factor_ * scale, 0.0f));
00140     command_queue_ptr_->execute(c);
00141   }
00142   else if (modifiers_ & ALT)
00143   {
00144     boost::shared_ptr<TransformCommand> c(new TransformCommand(selection_ptr_,
00145       cloud_ptr_, transform_matrix_, 0.0f, 0.0f,
00146       (float) dy * translate_factor_ * scale));
00147     command_queue_ptr_->execute(c);
00148   }
00149   else
00150   {
00151     boost::shared_ptr<TransformCommand> c(new TransformCommand(selection_ptr_,
00152       cloud_ptr_, transform_matrix_, 0.0f, 0.0f, 0.0f));
00153     command_queue_ptr_->execute(c);
00154   }
00155   setIdentity(transform_matrix_);
00156   cloud_ptr_->setSelectionRotation(transform_matrix_);
00157   cloud_ptr_->setSelectionTranslation(0.0f, 0.0f, 0.0f);
00158 }
00159 
00160 /*void
00161 SelectionTransformTool::getRotateMatrix (int dx, int dy,
00162                                          float* rotation_matrix_a,
00163                                          float* rotation_matrix_b,
00164                                          BitMask buttons) const
00165 {
00166   setIdentity(rotation_matrix_a);
00167   setIdentity(rotation_matrix_b);
00168   float rad_dx = (float) dx  * M_PI / 180.0f;
00169   float rad_dy = (float) dy  * M_PI / 180.0f;
00170   rotation_matrix_a[5]  = std::cos(rad_dy);
00171   rotation_matrix_a[6]  = std::sin(rad_dy);
00172   rotation_matrix_a[9]  = -std::sin(rad_dy);
00173   rotation_matrix_a[10] = std::cos(rad_dy);
00174   if (buttons & LEFT)
00175   {
00176     rotation_matrix_b[0]  = std::cos(rad_dx);
00177     rotation_matrix_b[2]  = -std::sin(rad_dx);
00178     rotation_matrix_b[8]  = std::sin(rad_dx);
00179     rotation_matrix_b[10] = std::cos(rad_dx);
00180   }
00181   else if (buttons & RIGHT)
00182   {
00183     rotation_matrix_b[0]  = std::cos(rad_dx);
00184     rotation_matrix_b[1]  = std::sin(rad_dx);
00185     rotation_matrix_b[4]  = -std::sin(rad_dx);
00186     rotation_matrix_b[5]  = std::cos(rad_dx);
00187   }
00188 }*/
00189 
00190 void
00191 SelectionTransformTool::findSelectionCenter ()
00192 {
00193   if (!selection_ptr_ || selection_ptr_->empty())
00194     return;
00195   float min_xyz[XYZ_SIZE] = {0.0f};
00196   float max_xyz[XYZ_SIZE] = {0.0f};
00197   Selection::const_iterator it = selection_ptr_->begin();
00198   float *pt = &((cloud_ptr_->getObjectSpacePoint(*it)).data[X]);
00199   std::copy(pt, pt+XYZ_SIZE, max_xyz);
00200   std::copy(max_xyz, max_xyz+XYZ_SIZE, min_xyz);
00201 
00202   for (++it; it != selection_ptr_->end(); ++it)
00203   {
00204     pt = &((cloud_ptr_->getObjectSpacePoint(*it)).data[X]);
00205     for (unsigned int j = 0; j < XYZ_SIZE; ++j)
00206     {
00207       min_xyz[j] = std::min(min_xyz[j], pt[j]);
00208       max_xyz[j] = std::max(max_xyz[j], pt[j]);
00209     }
00210   }
00211   for (unsigned int j = 0; j < XYZ_SIZE; ++j)
00212   {
00213     center_xyz_[j] = 0.5f * (max_xyz[j] + min_xyz[j]);
00214   }
00215 }
00216 
00217 
00218 
00219 
00220 
00221 
00222 
00223 
00224 
00225 
00226 
00227 
00228 
00229 
00230 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:33:22