WorldSim - 1.4.5

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phycylinder.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #include "phycylinder.h"
21 #include "private/phyobjectprivate.h"
22 #include "private/worldprivate.h"
23 #include <QtAlgorithms>
24 
25 namespace farsa {
26 
27 PhyCylinder::PhyCylinder( real radius, real height, World* w, QString name, const wMatrix& tm ) :
28  PhyObject( w, name, tm, false ),
29  radiusv(radius),
30  heightv(height),
31  m_upperBaseColor(),
32  m_lowerBaseColor(),
33  m_segmentsColor(),
34  m_uniformColor(),
35  m_graphicalRepresentationNeedsUpdate(),
36  m_oldColorv()
37 {
38  w->pushObject( this );
39  createPrivateObject();
40  changedMatrix();
41 
42  m_uniformColor.append(SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), colorv));
43 }
44 
45 PhyCylinder::~PhyCylinder() {
46 #ifdef WORLDSIM_USE_NEWTON
47  /* nothing to do */
48 #endif
49 }
50 
51 void PhyCylinder::setUpperBaseColor(QColor color)
52 {
53  m_upperBaseColor = color;
54 
55  if (colorv.isValid()) {
56  m_lowerBaseColor = colorv;
57  m_segmentsColor.append(SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), colorv));
58  colorv = QColor();
59  }
60 
61  setUpdateAllGraphicalRepresentation();
62 }
63 
64 void PhyCylinder::setLowerBaseColor(QColor color)
65 {
66  m_lowerBaseColor = color;
67 
68  if (colorv.isValid()) {
69  m_upperBaseColor = colorv;
70  m_segmentsColor.append(SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), colorv));
71  colorv = QColor();
72  }
73 
74  setUpdateAllGraphicalRepresentation();
75 }
76 
77 namespace {
78  // Utility class used in the function PhyCylinder::setSegmentsColor(). This is
79  // used basically to sort section of various color by ascending interval start
80  class SimpleIntervalAndColor {
81  public:
82  SimpleIntervalAndColor() :
83  interval(),
84  color()
85  {
86  }
87 
88  SimpleIntervalAndColor(const SimpleInterval& i, QColor c) :
89  interval(i),
90  color(c)
91  {
92  }
93 
94  bool operator<(const SimpleIntervalAndColor& other) const
95  {
96  return interval < other.interval;
97  }
98 
99  SimpleInterval interval;
100  QColor color;
101  };
102 }
103 
104 void PhyCylinder::setSegmentsColor(QColor base, const QList<SegmentColor>& segmentsColor)
105 {
106  if (segmentsColor.isEmpty()) {
107  m_segmentsColor.clear();
108  m_segmentsColor << SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), base);
109 
110  return;
111  }
112 
113  // The part of the cylinder not in segments of the list (which at the end will have the base color)
114  Intervals remainingArc(SimpleInterval(-PI_GRECO, PI_GRECO));
115 
116  // Adding the first interval. We use a temporary list because the final one will have a different structure
117  QList<SegmentColor> tmpSegments;
118  const Intervals intervalToAdd = segmentsColor[0].intervals & remainingArc;
119  remainingArc -= segmentsColor[0].intervals;
120  tmpSegments << SegmentColor(intervalToAdd, segmentsColor[0].color);
121  for (QList<SegmentColor>::const_iterator it = segmentsColor.begin(); it != segmentsColor.end(); it++) {
122  const Intervals intervalToAdd = it->intervals & remainingArc;
123  remainingArc -= it->intervals;
124  tmpSegments << SegmentColor(intervalToAdd, it->color);
125  }
126  // If the remainingArc is not empty, adding the final SegmentColor
127  if (!remainingArc.isEmpty()) {
128  tmpSegments << SegmentColor(remainingArc, base);
129  }
130 
131  // Now we fill m_segmentsColor so that it respects the format described in segmentsColor(). First we create
132  // a vector of SimpleIntervalAndColor, then sort it and finally use it to populate m_segmentsColor. This procedure
133  // works becayse tmpSegments is made up of intervals that cover the whole circle and without any overlapping
134  // Creating a vector of SimpleIntervalAndColor
135  QList<SimpleIntervalAndColor> simpleIntervals;
136  for (QList<SegmentColor>::const_iterator segmentIt = tmpSegments.constBegin(); segmentIt != tmpSegments.constEnd(); ++segmentIt) {
137  for (Intervals::const_iterator intervalIt = segmentIt->intervals.constBegin(); intervalIt != segmentIt->intervals.constEnd(); ++intervalIt) {
138  simpleIntervals << SimpleIntervalAndColor(*intervalIt, segmentIt->color);
139  }
140  }
141  // Sorting the vector we just generated
142  qSort(simpleIntervals);
143  // Now filling m_segmentsColor
144  m_segmentsColor.clear();
145  for (QList<SimpleIntervalAndColor>::const_iterator it = simpleIntervals.begin(); it != simpleIntervals.end(); ++it) {
146  m_segmentsColor << SegmentColor(it->interval, it->color);
147  }
148 
149  if (colorv.isValid()) {
150  m_upperBaseColor = colorv;
151  m_lowerBaseColor = colorv;
152  colorv = QColor();
153  }
154 
155  setUpdateAllGraphicalRepresentation();
156 }
157 
158 bool PhyCylinder::graphicalRepresentationNeedsUpdate(void* renderer)
159 {
160  // Our current color is not ours if we inherit color from parent! We need to get the color of our parent in that case
161  WObject* obj = this;
162  while (obj->useColorTextureOfOwner() && (obj->owner() != NULL)) {
163  WObject *const owner = dynamic_cast<WObject *>(obj->owner());
164  if (owner != NULL) {
165  obj = owner;
166  } else {
167  break;
168  }
169  }
170  QColor colorToTest = obj->color();
171 
172  // Checking if color has changed
173  if (m_oldColorv != colorToTest) {
174  setUpdateAllGraphicalRepresentation();
175  }
176 
177  if (!m_graphicalRepresentationNeedsUpdate.contains(renderer)) {
178  m_graphicalRepresentationNeedsUpdate[renderer] = true;
179  }
180 
181  const bool ret = m_graphicalRepresentationNeedsUpdate[renderer];
182 
183  m_graphicalRepresentationNeedsUpdate[renderer] = false;
184  m_oldColorv = colorToTest;
185 
186  return ret;
187 }
188 
189 void PhyCylinder::setUpdateAllGraphicalRepresentation()
190 {
191  for (QMap<void*, bool>::iterator it = m_graphicalRepresentationNeedsUpdate.begin(); it != m_graphicalRepresentationNeedsUpdate.end(); ++it) {
192  it.value() = true;
193  }
194 }
195 
196 void PhyCylinder::createPrivateObject() {
197 #ifdef WORLDSIM_USE_NEWTON
198  NewtonCollision* c = NewtonCreateCylinder( worldpriv->world, radiusv, heightv, 1, 0 );
199  wMatrix initialTransformationMatrix = wMatrix::identity(); // The transformation matrix is set in other places
200  priv->body = NewtonCreateBody( worldpriv->world, c, &initialTransformationMatrix[0][0] );
201  priv->collision = c;
202  NewtonBodySetAutoSleep( priv->body, 0 );
203  setMass( 1 );
204  NewtonBodySetUserData( priv->body, this );
205  NewtonBodySetLinearDamping( priv->body, 0.0 );
206  wVector zero = wVector(0,0,0,0);
207  NewtonBodySetAngularDamping( priv->body, &zero[0] );
208  NewtonBodySetAutoSleep( priv->body, 0 );
209  NewtonBodySetFreezeState( priv->body, 0 );
210  // Sets the signal-wrappers callback
211  NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
212  NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
213 #endif
214 }
215 
216 } // end namespace farsa