snark
ShapeWithId.h
1 // This file is part of snark, a generic and flexible library
2 // for robotics research.
3 //
4 // Copyright (C) 2011 The University of Sydney
5 //
6 // snark is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // snark is distributed in the hope that it will be useful, but WITHOUT ANY
12 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
13 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
14 // for more details.
15 //
16 // You should have received a copy of the GNU Lesser General Public
17 // License along with snark. If not, see <http://www.gnu.org/licenses/>.
18 
20 
21 #ifndef SNARK_GRAPHICS_APPLICATIONS_VIEWPOINTS_SHAPEWITHID_H_
22 #define SNARK_GRAPHICS_APPLICATIONS_VIEWPOINTS_SHAPEWITHID_H_
23 
24 #include <boost/foreach.hpp>
25 #include <boost/optional.hpp>
26 #include <comma/base/types.h>
27 #include <comma/visiting/traits.h>
28 #include <snark/math/interval.h>
29 #include <snark/graphics/qt3d/rotation_matrix.h>
30 #include <snark/graphics/qt3d/vertex_buffer.h>
31 #include <Qt3D/qglnamespace.h>
32 #include <Qt3D/qglpainter.h>
33 
34 namespace snark { namespace graphics { namespace View {
35 
36 template < class S >
37 struct ShapeWithId // quick and dirty
38 {
39  typedef S Shape;
40  ShapeWithId() : id( 0 ), block( 0 ) {}
41  S shape;
42  comma::uint32 id;
43  comma::uint32 block;
44  QColor4ub color;
45  std::string label;
46  double scalar;
47 };
48 
49 
50 template < class S >
51 struct Shapetraits {}; // quick and dirty
52 
53 template<>
54 struct Shapetraits< Eigen::Vector3d >
55 {
56  static const QGL::DrawingMode drawingMode = QGL::Points;
57  static const unsigned int size = 1;
58 
59  static void update( const Eigen::Vector3d& p, const Eigen::Vector3d& offset, const QColor4ub& color, unsigned int block, qt3d::vertex_buffer& buffer, boost::optional< snark::math::closed_interval< float, 3 > >& extents )
60  {
61  Eigen::Vector3d point = p - offset;
62  buffer.addVertex( QVector3D( point.x(), point.y(), point.z() ), color, block );
63  if( extents )
64  {
65  extents = extents->hull( point.cast< float >() );
66  }
67  else
68  {
69  extents = snark::math::closed_interval< float, 3 >( point.cast< float >() );
70  }
71  }
72 
73  static void draw( QGLPainter* painter, unsigned int size, unsigned int index )
74  {
75  painter->draw( QGL::Points, size, index );
76  }
77 
78  static const Eigen::Vector3d& somePoint( const Eigen::Vector3d& point ) { return point; }
79 
80  static const Eigen::Vector3d& center( const Eigen::Vector3d& point ) { return point; }
81 };
82 
83 template<>
84 struct Shapetraits< snark::math::closed_interval< double, 3 > >
85 {
86  static const unsigned int size = 8;
87  static void update( const snark::math::closed_interval< double, 3 >& e, const Eigen::Vector3d& offset, const QColor4ub& color, unsigned int block, qt3d::vertex_buffer& buffer, boost::optional< snark::math::closed_interval< float, 3 > >& extents )
88  {
89  Eigen::Vector3f min = ( e.min() - offset ).cast< float >();
90  Eigen::Vector3f max = ( e.max() - offset ).cast< float >();
91  buffer.addVertex( QVector3D( min.x(), min.y(), min.z() ), color, block );
92  buffer.addVertex( QVector3D( min.x(), min.y(), max.z() ), color, block );
93  buffer.addVertex( QVector3D( min.x(), max.y(), max.z() ), color, block );
94  buffer.addVertex( QVector3D( min.x(), max.y(), min.z() ), color, block );
95  buffer.addVertex( QVector3D( max.x(), min.y(), min.z() ), color, block );
96  buffer.addVertex( QVector3D( max.x(), min.y(), max.z() ), color, block );
97  buffer.addVertex( QVector3D( max.x(), max.y(), max.z() ), color, block );
98  buffer.addVertex( QVector3D( max.x(), max.y(), min.z() ), color, block );
99 
100  if( extents )
101  {
102  extents = extents->hull( snark::math::closed_interval< float, 3 >( min, max ) );
103  }
104  else
105  {
106  extents = snark::math::closed_interval< float, 3 >( min, max );
107  }
108  }
109 
110  static void draw( QGLPainter* painter, unsigned int size, unsigned int index )
111  {
112  const boost::array< unsigned short, 8 > baseIndices = { { 0, 4, 1, 5, 2, 6, 3, 7 } };
113  for( unsigned int i = 0; i < size; i += 8 )
114  {
115  // 2 line loops
116  painter->draw( QGL::LineLoop, 4, index + i );
117  painter->draw( QGL::LineLoop, 4, index + i + 4 );
118 
119  // 4 lines
120  boost::array< unsigned short, 8 > lineIndices = baseIndices;
121  BOOST_FOREACH( unsigned short& j, lineIndices )
122  {
123  j += index + i;
124  }
125  painter->draw( QGL::Lines, &lineIndices[0], 8 );
126  }
127  }
128 
129  static const Eigen::Vector3d& somePoint( const snark::math::closed_interval< double, 3 >& extents ) { return extents.min(); }
130 
131  static Eigen::Vector3d center( const snark::math::closed_interval< double, 3 >& extents ) { return ( extents.min() + extents.max() ) / 2; }
132 };
133 
134 template<>
135 struct Shapetraits< std::pair< Eigen::Vector3d, Eigen::Vector3d > >
136 {
137  static const unsigned int size = 2;
138  static void update( const std::pair< Eigen::Vector3d, Eigen::Vector3d >& p, const Eigen::Vector3d& offset, const QColor4ub& color, unsigned int block, qt3d::vertex_buffer& buffer, boost::optional< snark::math::closed_interval< float, 3 > >& extents )
139  {
140  Eigen::Vector3f first = ( p.first - offset ).cast< float >();
141  Eigen::Vector3f second = ( p.second - offset ).cast< float >();
142  buffer.addVertex( QVector3D( first.x(), first.y(), first.z() ), color, block );
143  buffer.addVertex( QVector3D( second.x(), second.y(), second.z() ), color, block );
144  if( extents )
145  {
146  extents = extents->hull( snark::math::closed_interval< float, 3 >( first, second ) );
147  }
148  else
149  {
150  extents = snark::math::closed_interval< float, 3 >( first, second );
151  }
152  }
153 
154  static void draw( QGLPainter* painter, unsigned int size, unsigned int index )
155  {
156  painter->draw( QGL::Lines, size, index );
157  }
158 
159  static const Eigen::Vector3d& somePoint( const std::pair< Eigen::Vector3d, Eigen::Vector3d >& line ) { return line.first; }
160 
161  static Eigen::Vector3d center( const std::pair< Eigen::Vector3d, Eigen::Vector3d >& line ) { return ( line.first + line.second ) / 2; }
162 };
163 
164 template < std::size_t Size >
165 struct Ellipse
166 {
167  Eigen::Vector3d center;
168  Eigen::Vector3d orientation;
169  double major;
170  double minor;
171 };
172 
173 template < std::size_t Size >
174 struct Shapetraits< Ellipse< Size > >
175 {
176  static const unsigned int size = Size;
177  static void update( const Ellipse< Size >& ellipse, const Eigen::Vector3d& offset, const QColor4ub& color, unsigned int block, qt3d::vertex_buffer& buffer, boost::optional< snark::math::closed_interval< float, 3 > >& extents )
178  {
179  Eigen::Vector3d c = ellipse.center - offset;
180  const Eigen::Matrix3d& r = rotation_matrix::rotation( ellipse.orientation );
181  static const double step = 3.14159265358979323846l * 2 / Size;
182  double angle = 0;
183  // todo: use native opengl rotation and normals instead
184  for( std::size_t i = 0; i < Size; ++i, angle += step )
185  {
186  Eigen::Vector3d v = r * Eigen::Vector3d( std::cos( angle ) * ellipse.major, std::sin( angle ) * ellipse.minor, 0 );
187  Eigen::Vector3d p( v.x(), v.y(), v.z() );
188  Eigen::Vector3f point = ( p + c ).cast< float >();
189  buffer.addVertex( QVector3D( point.x(), point.y(), point.z() ), color, block );
190  if( extents )
191  {
192  extents = extents->hull( point );
193  }
194  else
195  {
196  extents = snark::math::closed_interval< float, 3 >( point );
197  }
198  }
199  }
200 
201  static void draw( QGLPainter* painter, unsigned int size, unsigned int index )
202  {
203  for( unsigned int i = 0; i < size; i += Size )
204  {
205  painter->draw( QGL::LineLoop, Size, index + i );
206  }
207  }
208 
209  static const Eigen::Vector3d& somePoint( const Ellipse< Size >& ellipse ) { return ellipse.center; }
210 
211  static Eigen::Vector3d center( const Ellipse< Size >& ellipse ) { return ellipse.center; }
212 };
213 
214 } } }
215 
216 namespace comma { namespace visiting {
217 
218 template <> struct traits< QColor4ub >
219 {
220  template < typename Key, class Visitor >
221  static void visit( Key, QColor4ub& p, Visitor& v )
222  {
223  unsigned char red = 0;
224  unsigned char green = 0;
225  unsigned char blue = 0;
226  unsigned char alpha = 255;
227  v.apply( "r", red );
228  v.apply( "g", green );
229  v.apply( "b", blue );
230  v.apply( "a", alpha );
231  p = QColor4ub( red, green, blue, alpha );
232  }
233 
234  template < typename Key, class Visitor >
235  static void visit( Key, const QColor4ub& p, Visitor& v )
236  {
237  v.apply( "r", p.red() );
238  v.apply( "g", p.green() );
239  v.apply( "b", p.blue() );
240  v.apply( "a", p.alpha() );
241  }
242 };
243 
244 template < typename S > struct traits< snark::graphics::View::ShapeWithId< S > >
245 {
246  template < typename Key, class Visitor >
247  static void visit( Key, snark::graphics::View::ShapeWithId< S >& p, Visitor& v )
248  {
249  v.apply( "shape", p.shape );
250  v.apply( "id", p.id );
251  v.apply( "block", p.block );
252  v.apply( "colour", p.color );
253  v.apply( "label", p.label );
254  v.apply( "scalar", p.scalar );
255  }
256 
257  template < typename Key, class Visitor >
258  static void visit( Key, const snark::graphics::View::ShapeWithId< S >& p, Visitor& v )
259  {
260  v.apply( "shape", p.shape );
261  v.apply( "id", p.id );
262  v.apply( "block", p.block );
263  v.apply( "colour", p.color );
264  v.apply( "label", p.label );
265  v.apply( "scalar", p.scalar );
266  }
267 };
268 
269 template < std::size_t Size > struct traits< snark::graphics::View::Ellipse< Size > >
270 {
271  template < typename Key, class Visitor >
272  static void visit( Key, snark::graphics::View::Ellipse< Size >& p, Visitor& v )
273  {
274  v.apply( "center", p.center );
275  v.apply( "orientation", p.orientation );
276  v.apply( "major", p.major );
277  v.apply( "minor", p.minor );
278  }
279 
280  template < typename Key, class Visitor >
281  static void visit( Key, const snark::graphics::View::Ellipse< Size >& p, Visitor& v )
282  {
283  v.apply( "center", p.center );
284  v.apply( "orientation", p.orientation );
285  v.apply( "major", p.major );
286  v.apply( "minor", p.minor );
287  }
288 };
289 
290 template < typename T > struct traits< snark::math::closed_interval< T, 3 > >
291 {
292  template < typename Key, class Visitor >
293  static void visit( Key, snark::math::closed_interval< T, 3 >& p, Visitor& v )
294  {
295  Eigen::Matrix< T, 3, 1 > min;
296  Eigen::Matrix< T, 3, 1 > max;
297  v.apply( "min", min );
298  v.apply( "max", max );
299  p = snark::math::closed_interval< T, 3 >( min, max );
300  }
301 
302  template < typename Key, class Visitor >
303  static void visit( Key, const snark::math::closed_interval< T, 3 >& p, Visitor& v )
304  {
305  v.apply( "min", p.min() );
306  v.apply( "max", p.max() );
307  }
308 };
309 
310 } } // namespace comma { namespace visiting {
311 
312 #endif /*SNARK_GRAPHICS_APPLICATIONS_VIEWPOINTS_SHAPEWITHID_H_*/
Generated on Fri Nov 16 2012 12:42:42 for snark by   doxygen 1.8.1.2