Army ant simulation
Robot.cpp
1 /*
2  * robot.cpp
3  *
4  * Created on: 26 sept. 2018
5  * Author: lucie
6  *
7  * 29 Jan 2019:
8  * - Add a userData on the robot object in the box2D bodies
9  */
10 
11 #include "Robot.h"
12 
13 #include "Config.h"
14 #include "helpers.h"
15 #include <iostream>
16 
17 Robot::Robot() {
18  // TODO Auto-generated constructor stub
19  //m_robotBody = createBody(world, posX, posY);
20 
21 }
22 Robot::Robot(b2World* world, config::sRobot robotParameters, double posX, double posY, double angle) {
23  // TODO Auto-generated constructor stub
24  //m_robotBody = createBody(world, posX, posY);
25  m_state = WALK;
26  createBody(world, robotParameters, posX, posY, angle);
27 // m_M_TO_PX = config::WINDOW_X_PX / ((2*8+3));
28 
29 }
30 Robot::~Robot() {
31  printf(" destructor called \n");
32  // TODO Auto-generated destructor stub
33 
34  std::cout << "destruction of robot id: " << m_id << std::endl;
35  m_id=-1;
36 
37 // if(m_robotBody){
38  b2World* world = (m_robotBody->GetWorld());
39  m_robotBody->GetWorld()->DestroyBody(m_leftWheel);
40  m_robotBody->GetWorld()->DestroyBody(m_rightWheel);
41  m_robotBody->GetWorld()->DestroyBody(m_robotBody);
42 // }
43 
44  m_robotBody = nullptr;
45  m_leftWheel = nullptr;
46  m_rightWheel = nullptr;
47 
48  m_rightWheelMotor = nullptr;
49  m_leftWheelMotor = nullptr;
50 
51  m_previousGripJoint = nullptr;
52  m_currentGripJoint = nullptr;
53 }
54 
55 void Robot::createBody(b2World* world, config::sRobot robotParameters, double posX, double posY, double angle){
56 
57  m_robotParameters = robotParameters;
58  m_robotParameters.wheel_radius = (m_robotParameters.body_length - 0.02)/4;
59  m_robotParameters.wheel_distance = (m_robotParameters.body_length - 2*m_robotParameters.wheel_radius);
60  m_robotParameters.attach_height = m_robotParameters.wheel_radius/2;
61 
62  /* Attach between the wheel -------------------------- */
63  b2BodyDef BodyDef;
64  BodyDef.position = b2Vec2(posX, posY);
65  BodyDef.angle = angle;
66  BodyDef.type = b2_dynamicBody;
67  BodyDef.bullet = true;
68  m_robotBody = world->CreateBody(&BodyDef);
69  m_robotBody->SetUserData( this );
70 
71  b2PolygonShape boxShape;
72  boxShape.SetAsBox(m_robotParameters.wheel_distance/2,m_robotParameters.attach_height/2);
73  b2FixtureDef boxFixtureDef;
74  boxFixtureDef.shape = &boxShape;
75  boxFixtureDef.density = 2.f;
76  m_robotBody->CreateFixture(&boxFixtureDef);
77 
78  /* Box2D left wheel ---------------------------------*/
79  BodyDef.position = m_robotBody->GetWorldPoint(b2Vec2(-m_robotParameters.wheel_distance/2, 0));
80  BodyDef.fixedRotation = true;
81 // BodyDef.angularDamping = 1.0;
82  m_leftWheel = world->CreateBody(&BodyDef);
83  m_leftWheel->SetUserData( this );
84 
85  // Fixture
86  b2CircleShape mcircleShape;
87  mcircleShape.m_p.Set(0, 0); //position, relative to body position
88  mcircleShape.m_radius = m_robotParameters.wheel_radius; //radius
89  b2FixtureDef leftFixtureDef;
90  leftFixtureDef.density = 1.f;
91  leftFixtureDef.shape = &mcircleShape;
92  m_leftWheel->CreateFixture(&leftFixtureDef);
93 
94  // Joint
95  b2RevoluteJointDef revoluteJointDef;
96  revoluteJointDef.bodyA = m_leftWheel;
97  revoluteJointDef.bodyB = m_robotBody;
98  revoluteJointDef.localAnchorB.Set(-m_robotParameters.wheel_distance/2,0);
99  revoluteJointDef.localAnchorA.Set(0,0);
100 // revoluteJointDef.referenceAngle = 0;
101  revoluteJointDef.collideConnected = false;
102  m_leftWheelMotor = static_cast<b2RevoluteJoint*>(world->CreateJoint( &revoluteJointDef ));
103  m_leftWheelMotor->SetMaxMotorTorque(300.0f);
104  m_leftWheelMotor->EnableMotor(true);
105  m_leftWheelMotor->SetMotorSpeed(0.0);
106 
107  double a = m_leftWheelMotor->GetJointAngle();
108  m_leftWheelMotor->EnableLimit(true);
109  m_leftWheelMotor->SetLimits( a-0.01, a+0.01 );
110 
111  /* Box2D right wheel -----------------------------------*/
112  BodyDef.position = m_robotBody->GetWorldPoint(b2Vec2(m_robotParameters.wheel_distance/2, 0));//b2Vec2(posX+m_robotParameters.wheel_distance/2, posY);
113  m_rightWheel = world->CreateBody(&BodyDef);
114  m_rightWheel->SetUserData( this );
115 
116  // Fixture
117  b2FixtureDef rightFixtureDef;
118  mcircleShape.m_p.Set(0, 0); //left wheel
119  rightFixtureDef.shape = &mcircleShape;
120  rightFixtureDef.density = 1.f;
121  m_rightWheel->CreateFixture(&rightFixtureDef);
122 
123  // Joint
124  b2RevoluteJointDef revoluteJointDef2;
125  revoluteJointDef2.bodyA = m_rightWheel;
126  revoluteJointDef2.bodyB = m_robotBody;
127  revoluteJointDef2.localAnchorB.Set(m_robotParameters.wheel_distance/2,0);
128  revoluteJointDef2.localAnchorA.Set(0,0);
129 // revoluteJointDef2.referenceAngle = 0;
130  revoluteJointDef2.collideConnected = false;
131  m_rightWheelMotor = static_cast<b2RevoluteJoint*>(world->CreateJoint( &revoluteJointDef2 ));
132  m_rightWheelMotor->SetMaxMotorTorque(300.0f);
133  m_rightWheelMotor->EnableMotor(true);
134  m_rightWheelMotor->SetMotorSpeed(0.0);
135 
136  a = m_rightWheelMotor->GetJointAngle();
137  m_rightWheelMotor->EnableLimit(true);
138  m_rightWheelMotor->SetLimits( a-0.01, a+0.01 );
139 
140 }
141 
142 void Robot::drawBody(sf::RenderWindow& window, double m_to_pix){
143 
144  /* Left wheel */
145  sf::CircleShape left_wheelShape(m_to_pix* m_robotParameters.wheel_radius);
146  if (m_state == BRIDGE){
147 // if (m_isGrabbed){
148 // if (m_moving){
149  if(m_movingSide == LEFT){
150  left_wheelShape.setFillColor(sf::Color(237,201,217));
151  }
152  else{
153  left_wheelShape.setFillColor(sf::Color(172,197,217));
154  }
155  }
156  else{
157  if(m_movingSide == LEFT){
158  left_wheelShape.setFillColor(sf::Color(246,125,150));
159  }
160  else{
161  left_wheelShape.setFillColor(sf::Color(58,105,186));
162  }
163  }
164  double bodyAngle = m_leftWheel->GetAngle();
165  double topLeftX = m_leftWheel->GetWorldPoint(b2Vec2(-m_robotParameters.wheel_radius,-m_robotParameters.wheel_radius)).x;
166  double topLeftY = m_leftWheel->GetWorldPoint(b2Vec2(-m_robotParameters.wheel_radius,-m_robotParameters.wheel_radius)).y;
167 
168  left_wheelShape.setPosition(m_to_pix*topLeftX,m_to_pix*topLeftY); //body.getPosition
169  left_wheelShape.setRotation(bodyAngle*RAD_TO_DEG);
170 
171  sf::Vertex angle_left[2];
172  b2Vec2 start = m_leftWheel->GetWorldCenter();
173  b2Vec2 end = m_leftWheel->GetWorldPoint(b2Vec2(0,m_robotParameters.wheel_radius));
174  angle_left[0]= sf::Vertex(sf::Vector2f(start.x*m_to_pix, start.y*m_to_pix), sf::Color::Black);
175  angle_left[1]= sf::Vertex(sf::Vector2f(end.x*m_to_pix, end.y*m_to_pix), sf::Color::Black);
176 
177  /* Right wheel */
178  sf::CircleShape right_wheelShape(m_to_pix* m_robotParameters.wheel_radius);
179 // if (m_isGrabbed){
180  if (m_state == BRIDGE){
181 // if (m_moving){
182  if(m_movingSide == RIGHT){
183  right_wheelShape.setFillColor(sf::Color(237,201,217));
184  }
185  else{
186  right_wheelShape.setFillColor(sf::Color(172,197,217));
187  }
188 
189  }
190  else{
191  if(m_movingSide == RIGHT){
192  right_wheelShape.setFillColor(sf::Color(246,125,150));
193  }
194  else{
195  right_wheelShape.setFillColor(sf::Color(58,105,186));
196  }
197  }
198  bodyAngle = m_rightWheel->GetAngle();
199  topLeftX = m_rightWheel->GetWorldPoint(b2Vec2(-m_robotParameters.wheel_radius,-m_robotParameters.wheel_radius)).x;
200  topLeftY = m_rightWheel->GetWorldPoint(b2Vec2(-m_robotParameters.wheel_radius,-m_robotParameters.wheel_radius)).y;
201 
202  right_wheelShape.setPosition(m_to_pix*topLeftX,m_to_pix*topLeftY); //body.getPosition
203  right_wheelShape.setRotation(bodyAngle*RAD_TO_DEG);
204 
205  sf::Vertex angle_right[2];
206  start = m_rightWheel->GetWorldCenter();
207  end = m_rightWheel->GetWorldPoint(b2Vec2(0, m_robotParameters.wheel_radius));
208  angle_right[0] = sf::Vertex(sf::Vector2f(start.x*m_to_pix, start.y*m_to_pix), sf::Color::Black);
209  angle_right[1] = sf::Vertex(sf::Vector2f(end.x*m_to_pix, end.y*m_to_pix), sf::Color::Black);
210 
211  /* wheel Attach */
212  sf::RectangleShape attachShape(sf::Vector2f(m_robotParameters.wheel_distance*m_to_pix, m_robotParameters.attach_height*m_to_pix));
213  attachShape.setFillColor(sf::Color::Black);
214  bodyAngle = m_robotBody->GetAngle();
215  topLeftX = m_robotBody->GetWorldPoint(b2Vec2(-m_robotParameters.wheel_distance/2,-m_robotParameters.attach_height/2)).x;
216  topLeftY = m_robotBody->GetWorldPoint(b2Vec2(-m_robotParameters.wheel_distance/2,-m_robotParameters.attach_height/2)).y;
217 
218  attachShape.setPosition(m_to_pix*topLeftX,m_to_pix*topLeftY);
219  attachShape.setRotation(bodyAngle*RAD_TO_DEG);
220 
221  window.draw(attachShape);
222 
223  window.draw(left_wheelShape);
224  window.draw(right_wheelShape);
225 
226 // window.draw(angle_right, 2, sf::Lines);
227 // window.draw(angle_left, 2, sf::Lines);
228 
229 // window.draw(m_shapeLeft);
230 // window.draw(m_shapeRight);
231 
232 }
233 void Robot::drawGripJoint(sf::RenderWindow& window, double m_to_px){
234 
235 
236  if(m_previousGripJoint){
237 
238  b2Vec2 pA = m_previousGripJoint->GetAnchorA();
239  float delta = 1/2;
240 
241 
242  sf::Vertex edge[2];
243  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
244 
245  if((m_previousGripJoint->GetBodyA() == m_leftWheel) || (m_previousGripJoint->GetBodyB() == m_leftWheel)){
246  float xB = delta*m_leftWheel->GetLocalPoint(pA).x;
247  float yB = delta*m_leftWheel->GetLocalPoint(pA).y;
248  b2Vec2 pB = m_leftWheel->GetWorldPoint(b2Vec2(xB, yB));
249  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
250  }
251 
252  else if(m_previousGripJoint->GetBodyA() == m_rightWheel || (m_previousGripJoint->GetBodyB() == m_rightWheel)){
253  float xB = delta*m_rightWheel->GetLocalPoint(pA).x;
254  float yB = delta*m_rightWheel->GetLocalPoint(pA).y;
255  b2Vec2 pB = m_rightWheel->GetWorldPoint(b2Vec2(xB, yB));
256  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
257  }
258 
259  sf::CircleShape p1;
260  p1.setRadius(6);
261  p1.setFillColor(sf::Color::Black);
262  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
263 
264  window.draw(edge, 2, sf::Lines);
265  window.draw(p1);
266  }
267 
268 
269  /* Same with current joint -------------------*/
270 
271  if(m_currentGripJoint){
272  b2Vec2 pA = m_currentGripJoint->GetAnchorA();
273  float delta = 1/2;
274 
275  sf::Vertex edge[2];
276  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
277 
278  if((m_currentGripJoint->GetBodyA() == m_leftWheel) || (m_currentGripJoint->GetBodyB() == m_leftWheel)){
279  float xB = delta*m_leftWheel->GetLocalPoint(pA).x;
280  float yB = delta*m_leftWheel->GetLocalPoint(pA).y;
281  b2Vec2 pB = m_leftWheel->GetWorldPoint(b2Vec2(xB, yB));
282  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
283  }
284 
285  else if(m_currentGripJoint->GetBodyA() == m_rightWheel || (m_currentGripJoint->GetBodyB() == m_rightWheel)){
286  float xB = delta*m_rightWheel->GetLocalPoint(pA).x;
287  float yB = delta*m_rightWheel->GetLocalPoint(pA).y;
288  b2Vec2 pB = m_rightWheel->GetWorldPoint(b2Vec2(xB, yB));
289  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
290  }
291 
292  sf::CircleShape p1;
293  p1.setRadius(6);
294  p1.setFillColor(sf::Color::Black);
295  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
296 
297  window.draw(edge, 2, sf::Lines);
298  window.draw(p1);
299  }
300 
301 
302 }
303 void Robot::drawJoint(sf::RenderWindow& window, double m_to_px){
304  b2JointEdge* jointEdge = m_leftWheel->GetJointList();
305 
306  if(jointEdge == nullptr){
307  return;
308  }
309 
310  b2Joint* joint = jointEdge->joint;
311  b2Vec2 pA = joint->GetAnchorA();
312  b2Vec2 pB = joint->GetAnchorB();
313 
314  sf::Vertex edge[2];
315  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
316  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
317 
318  sf::CircleShape p1;
319  sf::CircleShape p2;
320  p1.setRadius(6);
321  p1.setFillColor(sf::Color::Black);
322  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
323  p2.setRadius(6);
324  p2.setFillColor(sf::Color::Black);
325  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
326 
327  window.draw(edge, 2, sf::Lines);
328  window.draw(p1);
329  window.draw(p2);
330 
331  if(!(jointEdge->next == nullptr)){
332  joint = jointEdge->next->joint;
333  pA = joint->GetAnchorA();
334  pB = joint->GetAnchorB();
335 
336  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
337  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
338 
339  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
340  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
341 
342  window.draw(edge, 2, sf::Lines);
343  window.draw(p1);
344  window.draw(p2);
345 
346  if(!(jointEdge->next->next == nullptr)){
347  joint = jointEdge->next->next->joint;
348  pA = joint->GetAnchorA();
349  pB = joint->GetAnchorB();
350 
351  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
352  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
353  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
354  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
355 
356  window.draw(edge, 2, sf::Lines);
357  window.draw(p1);
358  window.draw(p2);
359  }
360  }
361 
362  if(!(jointEdge->prev == nullptr)){
363  joint = jointEdge->prev->joint;
364  pA = joint->GetAnchorA();
365  pB = joint->GetAnchorB();
366 
367  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
368  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
369  window.draw(edge, 2, sf::Lines);
370  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
371  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
372 
373  window.draw(p1);
374  window.draw(p2);
375  }
376 
377  /* Same with right side -------------------*/
378 
379 
380  jointEdge = m_rightWheel->GetJointList();
381  joint = jointEdge->joint;
382  pA = joint->GetAnchorA();
383  pB = joint->GetAnchorB();
384 
385  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
386  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
387 
388  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
389  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
390 
391  window.draw(edge, 2, sf::Lines);
392  window.draw(p1);
393  window.draw(p2);
394 
395  if(!(jointEdge->next == nullptr)){
396  joint = jointEdge->next->joint;
397  pA = joint->GetAnchorA();
398  pB = joint->GetAnchorB();
399 
400  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
401  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
402 
403  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
404  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
405 
406  window.draw(edge, 2, sf::Lines);
407  window.draw(p1);
408  window.draw(p2);
409 
410  if(!(jointEdge->next->next == nullptr)){
411  joint = jointEdge->next->next->joint;
412  pA = joint->GetAnchorA();
413  pB = joint->GetAnchorB();
414 
415  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
416  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
417  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
418  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
419 
420  window.draw(edge, 2, sf::Lines);
421  window.draw(p1);
422  window.draw(p2);
423  }
424  }
425 
426  if(!(jointEdge->prev == nullptr)){
427  joint = jointEdge->prev->joint;
428  pA = joint->GetAnchorA();
429  pB = joint->GetAnchorB();
430 
431  edge[0] = sf::Vertex(sf::Vector2f(pA.x*m_to_px, m_to_px*pA.y), sf::Color::Black);
432  edge[1] = sf::Vertex(sf::Vector2f(m_to_px*pB.x, m_to_px*pB.y), sf::Color::Black);
433  window.draw(edge, 2, sf::Lines);
434  p1.setPosition(m_to_px*pA.x-3,pA.y*m_to_px-3);
435  p2.setPosition(m_to_px*pB.x-3,pB.y*m_to_px-3);
436 
437  window.draw(p1);
438  window.draw(p2);
439  }
440 }
441 
442 void Robot::moveBodyWithMotor(){
443 
444  if (m_movingSide == RIGHT){
445  m_rightWheelMotor->EnableMotor(false);
446  allowMotorRotation(RIGHT);
447 
448  allowMotorRotation(LEFT);
449  m_leftWheelMotor->EnableMotor(true);
450  m_leftWheelMotor->SetMotorSpeed(m_angularSpeed);
451  m_leftWheelMotor->SetMaxMotorTorque(30.0f);
452  }
453 
454  else{
455  m_leftWheelMotor->EnableMotor(false);
456  allowMotorRotation(LEFT);
457 
458  allowMotorRotation(RIGHT);
459  m_rightWheelMotor->EnableMotor(true);
460  m_rightWheelMotor->SetMotorSpeed(m_angularSpeed);
461  m_rightWheelMotor->SetMaxMotorTorque(30.0f);
462  }
463 
464 // m_shape3.setRadius(10);
465 // m_shape3.setFillColor(sf::Color(150, 10, 150));
466 // m_shape3.setPosition(config::M_TO_PX*(m_robotBody->GetWorldCenter()).x-10,(m_robotBody->GetWorldCenter()).y*config::M_TO_PX-10);
467 
468 
469 }
470 void Robot::moveBodyWithForce(){
471  b2Vec2 center(0,0);
472  b2Vec2 strength(0,0);
473  double torque =0 ;
474  double v=6;
475 
476  allowMotorRotation(LEFT);
477  allowMotorRotation(RIGHT);
478 
479  if (m_movingSide == RIGHT){
480  center.Set(m_robotParameters.wheel_distance/2,0);
481  strength.Set((m_robotBody->GetWorldVector(b2Vec2(0,v))).x, (m_robotBody->GetWorldVector(b2Vec2(0,v))).y);
482  m_robotBody->ApplyForceToCenter( strength, true);
483 // torque=m_leftWheelMotor->GetReactionTorque(60.f);
484  }
485 
486  else if (m_movingSide == LEFT){
487 
488  center.Set(-m_robotParameters.wheel_distance/2,0);
489  strength.Set((m_robotBody->GetWorldVector(b2Vec2(0,-v))).x, (m_robotBody->GetWorldVector(b2Vec2(0,-v))).y);
490  m_robotBody->ApplyForceToCenter( strength, true);
491 // torque=m_rightWheelMotor->GetReactionTorque(60.f);
492  }
493 
494 }
495 void Robot::moveBodyWithImpulse(){
496  b2Vec2 center(0,0);
497  b2Vec2 strength(0,0);
498  double torque =0 ;
499  double v=4;
500 
501  allowMotorRotation(LEFT);
502  allowMotorRotation(RIGHT);
503  double vel = m_robotBody->GetAngularVelocity();
504 
505  double velChange = m_angularSpeed - vel;
506  double impulse = m_robotBody->GetInertia() * velChange *10; //disregard time factor
507  /* to reach the target speed, should calculate the real inertia with the wheel attached to the body,
508  * but here act more like a Proportionnal command*/
509 // printf("impulse: %f, \n", impulse);
510  m_robotBody->ApplyAngularImpulse( impulse, true);
511 }
512 
513 void Robot::turnOffMotor(side s){
514  if (s == RIGHT){
515  m_rightWheelMotor->EnableMotor(false);
516  }
517 
518  else{
519  m_leftWheelMotor->EnableMotor(false);
520  }
521 }
522 void Robot::blockMotorRotation(side s){
523  if (s == RIGHT){
524  m_rightWheelMotor->EnableMotor(true);
525  m_rightWheelMotor->SetMotorSpeed(0.0);
526  m_rightWheelMotor->SetMaxMotorTorque(30.0f);
527 
528  double angle = m_rightWheelMotor->GetJointAngle();
529  m_rightWheelMotor->EnableLimit(true);
530  m_rightWheelMotor->SetLimits( angle-0.1, angle+0.1 );
531  }
532 
533  else{
534  m_leftWheelMotor->EnableMotor(true);
535  m_leftWheelMotor->SetMotorSpeed(0.0);
536  m_leftWheelMotor->SetMaxMotorTorque(30.0f);
537 //
538  double angle = m_leftWheelMotor->GetJointAngle();
539  m_leftWheelMotor->EnableLimit(true);
540  m_leftWheelMotor->SetLimits( angle-0.01, angle+0.01 );
541  }
542 }
543 void Robot::limitMotorRotation(side s, double limit_angle_rad){
544  if (s == RIGHT){
545  m_rightWheelMotor->EnableMotor(false);
546 // m_rightWheelMotor->SetMotorSpeed(0.0);
547  m_rightWheelMotor->SetMaxMotorTorque(500.0f);
548 
549  m_referenceAngleJoint = m_rightWheelMotor->GetJointAngle();
550  m_rightWheelMotor->EnableLimit(true);
551  m_rightWheelMotor->SetLimits( m_referenceAngleJoint-limit_angle_rad, m_referenceAngleJoint+limit_angle_rad );
552  }
553 
554  else{
555  m_leftWheelMotor->EnableMotor(false);
556 // m_leftWheelMotor->SetMotorSpeed(0.0);
557  m_leftWheelMotor->SetMaxMotorTorque(500.0f);
558 
559  m_referenceAngleJoint = m_leftWheelMotor->GetJointAngle();
560  m_leftWheelMotor->EnableLimit(true);
561  m_leftWheelMotor->SetLimits( m_referenceAngleJoint-limit_angle_rad, m_referenceAngleJoint+limit_angle_rad );
562  }
563 }
564 void Robot::allowMotorRotation(side s){
565  if (s == RIGHT){
566  m_rightWheelMotor->EnableLimit(false);
567  }
568 
569  else{
570  m_leftWheelMotor->EnableLimit(false);
571  }
572 }
573 
574 
575 void Robot::grip(b2Contact* contact, b2Body* otherBody, double m_to_px){ //ou alors fonction retourne un joint ?
576 
577  b2WorldManifold worldManifold;
578  contact->GetWorldManifold(&worldManifold);
579  b2Body* bodyA = contact->GetFixtureA()->GetBody();
580  b2Body* bodyB = contact->GetFixtureB()->GetBody();
581 // if(bodyA == bodyB){return;}
582 
583  double xB = otherBody->GetLocalPoint(worldManifold.points[0]).x;
584  double yB = otherBody->GetLocalPoint(worldManifold.points[0]).y;
585  //double yB = otherBody->GetLocalPoint(p).y;
586  b2Vec2 wheel_center;
587 
588  if(bodyA==m_leftWheel || bodyB==m_leftWheel)
589  {
590 // printf("left wheel \n");
591  // Create the joint
592  if(!m_movingSide == LEFT){
593  printf("error contact side\n");
594  m_movingSide = LEFT;
595  }
596  wheel_center.Set(0, 0); //in local body coordinates
597  double xA = m_leftWheel->GetLocalPoint(worldManifold.points[0]).x;
598  double yA = m_leftWheel->GetLocalPoint(worldManifold.points[0]).y;
599  m_leftGripJointDef.bodyA = m_leftWheel;
600  m_leftGripJointDef.bodyB = otherBody;
601  m_leftGripJointDef.collideConnected = true;
602  m_leftGripJointDef.localAnchorA.Set(xA,yA);
603  m_leftGripJointDef.localAnchorB.Set(xB,yB);
604 
605  m_shapeLeft.setRadius(10);
606  m_shapeLeft.setFillColor(sf::Color(100, 250, 50));
607  m_shapeLeft.setPosition(m_to_px*worldManifold.points[0].x-10,worldManifold.points[0].y*m_to_px-10);
608  }
609 
610  else if(bodyA == m_rightWheel || bodyB == m_rightWheel)
611  {
612 // printf("right wheel \n");
613  if(m_movingSide == LEFT){
614 // printf("error contact side\n");
615  m_movingSide = RIGHT;
616  }
617 // Create the joint
618  wheel_center.Set(0, 0); //in local body coordinates
619  double xA = m_rightWheel->GetLocalPoint(worldManifold.points[0]).x;
620  double yA = m_rightWheel->GetLocalPoint(worldManifold.points[0]).y;
621  m_rightGripJointDef.bodyA = m_rightWheel;
622  m_rightGripJointDef.bodyB = otherBody;
623  m_rightGripJointDef.collideConnected = true;
624  m_rightGripJointDef.localAnchorA.Set(xA,yA);
625  m_rightGripJointDef.localAnchorB.Set(xB,yB);
626 
627  m_shapeRight.setRadius(10);
628  m_shapeRight.setFillColor(sf::Color(100, 250, 50));
629  m_shapeRight.setPosition(m_to_px*worldManifold.points[0].x-10,worldManifold.points[0].y*m_to_px-10);
630  }
631 
632 //
633 // m_shape2.setRadius(4);
634 // m_shape2.setFillColor(sf::Color(50, 150, 50));
635 // m_shape2.setPosition(m_M_TO_PX*(m_robotBody->GetWorldPoint(wheel_center).x)-2,(m_robotBody->GetWorldPoint(wheel_center).y)*m_M_TO_PX-2);
636 
637  m_normal[0] = sf::Vertex(sf::Vector2f(xB*m_to_px, m_to_px*yB), sf::Color(200, 200, 150));
638  m_normal[1] = sf::Vertex(sf::Vector2f(m_to_px*(xB+(worldManifold.normal).x), m_to_px*(yB+(worldManifold.normal).y)), sf::Color(200, 200, 150));
639 
640 }
641 bool Robot::gripSide(b2Contact* contact, b2Body* otherBody, double m_to_px){ //ou alors fonction retourne un joint ?
642 
643  if(!contactOnGripSide(contact)){
644  return false;
645  }
646 
647  b2WorldManifold worldManifold;
648  contact->GetWorldManifold(&worldManifold);
649  b2Body* bodyA = contact->GetFixtureA()->GetBody();
650  b2Body* bodyB = contact->GetFixtureB()->GetBody();
651 
652  //b2ManifoldPoint p;
653  int i=0;
654  int numPoints = contact->GetManifold()->pointCount;
655 
656 
657  double xB = otherBody->GetLocalPoint(worldManifold.points[0]).x;
658  double yB = otherBody->GetLocalPoint(worldManifold.points[0]).y;
659  //double yB = otherBody->GetLocalPoint(p).y;
660  b2Vec2 wheel_center;
661 
662  b2Body* movingWheel = getWheel(m_movingSide);
663 
664  if(bodyA==movingWheel || bodyB==movingWheel)
665  {
666 // printf("Robot.gripSide : moving side : %d ;0 = right, 1 = left \n", m_movingSide);
667  // Create the joint
668 
669  wheel_center.Set(0, 0); //in local body coordinates
670  double xA = movingWheel->GetLocalPoint(worldManifold.points[0]).x;
671  double yA = movingWheel->GetLocalPoint(worldManifold.points[0]).y;
672 
673  double x = otherBody->GetLocalVector(worldManifold.normal).x;
674  double y = otherBody->GetLocalVector(worldManifold.normal).y;
675 
676  if(m_movingSide == LEFT){
677  m_leftGripJointDef.bodyB = movingWheel;
678  m_leftGripJointDef.bodyA = otherBody;
679  m_leftGripJointDef.collideConnected = true;
680  m_leftGripJointDef.localAnchorB.Set(xA,yA);
681  m_leftGripJointDef.localAnchorA.Set(xB,yB);
682 
683 // m_leftGripJointDef.length = 0.001;
684 // m_leftGripJointDef.frequencyHz = 30.0f;
685 // m_leftGripJointDef.dampingRatio = 1.f;
686 // m_leftGripJointDef.localAxisA.Set(x, y);
687 
688  m_leftGripJointDef.enableLimit = true;
689  m_leftGripJointDef.lowerTranslation = 0;
690  m_leftGripJointDef.upperTranslation = 0.001;
691 
692 // m_referenceAngle = m_robotBody->GetAngle();
693 
694  m_shapeLeft.setRadius(10);
695  m_shapeLeft.setFillColor(sf::Color(100, 250, 50));
696  m_shapeLeft.setPosition(m_to_px*(worldManifold.points[0].x+worldManifold.normal.x)-10,(worldManifold.points[0].y+worldManifold.normal.y)*m_to_px-10);
697 
698  return true;
699  }
700 
701  else{
702  m_rightGripJointDef.bodyB = movingWheel;
703  m_rightGripJointDef.bodyA = otherBody;
704  m_rightGripJointDef.collideConnected = true;
705  m_rightGripJointDef.localAnchorB.Set(xA,yA);
706  m_rightGripJointDef.localAnchorA.Set(xB,yB);
707 
708 // m_rightGripJointDef.length = 0.001;
709 // m_rightGripJointDef.frequencyHz = 30.0f;
710 // m_rightGripJointDef.dampingRatio = 1.f;
711 
712  m_rightGripJointDef.localAxisA.Set(x, y);
713 
714  m_rightGripJointDef.enableLimit = true;
715  m_rightGripJointDef.lowerTranslation = 0;
716  m_rightGripJointDef.upperTranslation = 0.001;
717 
718 // m_referenceAngle = m_robotBody->GetAngle();
719 
720  m_shapeRight.setRadius(10);
721  m_shapeRight.setFillColor(sf::Color(100, 250, 50));
722  m_shapeRight.setPosition(m_to_px*(worldManifold.points[0].x+worldManifold.normal.x)-10,(worldManifold.points[0].y+worldManifold.normal.y)*m_to_px-10);
723 
724  return true;
725  }
726  }
727  return false;
728 }
729 void Robot::gripGroundFromPos(){
730 
731  //left wheel ------------------------------------------
732  b2ContactEdge* contactList = m_leftWheel->GetContactList();
733 
734  if((contactList == nullptr))
735  {
736  return;
737  }
738 
739  b2Contact* contact = contactList->contact;
740 
741  b2Body* bodyA = contact->GetFixtureA()->GetBody();
742  b2Body* bodyB = contact->GetFixtureB()->GetBody();
743  b2World* world = bodyA->GetWorld();
744 
745  if(!(bodyA==m_robotBody || bodyB==m_robotBody ))
746  {
747 
748  b2WorldManifold worldManifold;
749  contact->GetWorldManifold(&worldManifold);
750 
751  double xA = bodyA->GetLocalPoint(worldManifold.points[0]).x;
752  double yA = bodyA->GetLocalPoint(worldManifold.points[0]).y;
753  double xB = bodyB->GetLocalPoint(worldManifold.points[0]).x;
754  double yB = bodyB->GetLocalPoint(worldManifold.points[0]).y;
755  m_leftGripJointDef.bodyA = bodyA;
756  m_leftGripJointDef.bodyB = bodyB;
757  m_leftGripJointDef.collideConnected = true;
758  m_leftGripJointDef.localAnchorA.Set(xA,yA);
759  m_leftGripJointDef.localAnchorB.Set(xB,yB);
760 
761  m_leftGripJointDef.enableLimit = true;
762  m_leftGripJointDef.lowerTranslation = 0;
763  m_leftGripJointDef.upperTranslation = 0.001;
764 
765  m_leftGripJointDef.localAxisA.Set(0.0f, -1.0f);
766  }
767 
768  else {
769  contact = contactList->next->contact;
770 
771  bodyA = contact->GetFixtureA()->GetBody();
772  bodyB = contact->GetFixtureB()->GetBody();
773 
774  if(!(bodyA==m_robotBody || bodyB==m_robotBody ))
775  {
776  b2WorldManifold worldManifold;
777  contact->GetWorldManifold(&worldManifold);
778 
779  double xA = bodyA->GetLocalPoint(worldManifold.points[0]).x;
780  double yA = bodyA->GetLocalPoint(worldManifold.points[0]).y;
781  double xB = bodyB->GetLocalPoint(worldManifold.points[0]).x;
782  double yB = bodyB->GetLocalPoint(worldManifold.points[0]).y;
783  m_leftGripJointDef.bodyA = bodyA;
784  m_leftGripJointDef.bodyB = bodyB;
785  m_leftGripJointDef.collideConnected = true;
786  m_leftGripJointDef.localAnchorA.Set(xA,yA);
787  m_leftGripJointDef.localAnchorB.Set(xB,yB);
788 
789  m_leftGripJointDef.enableLimit = true;
790  m_leftGripJointDef.lowerTranslation = 0;
791  m_leftGripJointDef.upperTranslation = 0.001;
792 
793  m_leftGripJointDef.localAxisA.Set(0.0f, -1.0f);
794  }
795  }
796 
797  if(!(&m_leftGripJointDef==nullptr)){
798  world->CreateJoint( &m_leftGripJointDef );
799  }
800 
801  //right wheel ----------------------------------
802  contactList = m_rightWheel->GetContactList();
803  contact = contactList->contact;
804 
805  bodyA = contact->GetFixtureA()->GetBody();
806  bodyB = contact->GetFixtureB()->GetBody();
807 
808  if(!(bodyA==m_robotBody || bodyB==m_robotBody ))
809  {
810 
811  b2WorldManifold worldManifold;
812  contact->GetWorldManifold(&worldManifold);
813 
814  double xA = bodyA->GetLocalPoint(worldManifold.points[0]).x;
815  double yA = bodyA->GetLocalPoint(worldManifold.points[0]).y;
816  double xB = bodyB->GetLocalPoint(worldManifold.points[0]).x;
817  double yB = bodyB->GetLocalPoint(worldManifold.points[0]).y;
818  m_rightGripJointDef.bodyA = bodyA;
819  m_rightGripJointDef.bodyB = bodyB;
820  m_rightGripJointDef.collideConnected = true;
821  m_rightGripJointDef.localAnchorA.Set(xA,yA);
822  m_rightGripJointDef.localAnchorB.Set(xB,yB);
823 
824  m_rightGripJointDef.enableLimit = true;
825  m_rightGripJointDef.lowerTranslation = 0;
826  m_rightGripJointDef.upperTranslation = 0.001;
827 
828  m_rightGripJointDef.localAxisA.Set(0.0f, -1.0f);
829  }
830 
831  else {
832  contact = contactList->next->contact;
833 
834  bodyA = contact->GetFixtureA()->GetBody();
835  bodyB = contact->GetFixtureB()->GetBody();
836 
837  if(!(bodyA==m_robotBody || bodyB==m_robotBody ))
838  {
839  b2WorldManifold worldManifold;
840  contact->GetWorldManifold(&worldManifold);
841 
842  double xA = bodyA->GetLocalPoint(worldManifold.points[0]).x;
843  double yA = bodyA->GetLocalPoint(worldManifold.points[0]).y;
844  double xB = bodyB->GetLocalPoint(worldManifold.points[0]).x;
845  double yB = bodyB->GetLocalPoint(worldManifold.points[0]).y;
846  m_rightGripJointDef.bodyA = bodyA;
847  m_rightGripJointDef.bodyB = bodyB;
848  m_rightGripJointDef.collideConnected = true;
849  m_rightGripJointDef.localAnchorA.Set(xA,yA);
850  m_rightGripJointDef.localAnchorB.Set(xB,yB);
851 
852  m_rightGripJointDef.enableLimit = true;
853  m_rightGripJointDef.lowerTranslation = 0;
854  m_rightGripJointDef.upperTranslation = 0.001;
855 
856  m_rightGripJointDef.localAxisA.Set(0.0f, -1.0f);
857  }
858  }
859 
860  if(!(&m_rightGripJointDef==nullptr)){
861  world->CreateJoint( &m_rightGripJointDef );
862  }
863 
864 }
865 
866 bool Robot::contactOnGripSide(b2Contact* contact){
867 
868  double angle =(getBody()->GetAngle());//-m_referenceAngle);
869  while(angle < 0){
870  angle += 2*PI;
871  }
872  angle = fmod(angle, (2*PI));
873 
874 // std::cout<<"Before wheel: "<<angle*RAD_TO_DEG<<std::endl;
875  b2WorldManifold worldManifold;
876  contact->GetWorldManifold(&worldManifold);
877  b2Body* bodyA = contact->GetFixtureA()->GetBody();
878  b2Body* bodyB = contact->GetFixtureB()->GetBody();
879 
880  b2Body* movingWheel = getWheel(m_movingSide);
881 
882  if(bodyA==movingWheel || bodyB==movingWheel)
883  {
884 // return true;
885  double x0 = movingWheel->GetLocalPoint(worldManifold.points[0]).x;
886  double y0 = movingWheel->GetLocalPoint(worldManifold.points[0]).y;
887  double y = y0*cos(angle)- x0*sin(angle);
888  if(m_movingSide == RIGHT){
889  if(y>0){
890  return true;
891  }
892  }
893  else{
894  if(y<0){
895  return true;
896  }
897  }
898  }
899 
900  return false;
901 }
902 bool Robot::checkGripp(side s){
903  if(s == RIGHT){
904  b2JointEdge* jointList = m_rightWheel->GetJointList();
905  b2Joint* joint = jointList->joint;
906  for (int i=0; i<6; i++){
907  // printf("Robot.isGrabbed 2 \n");
908  if(!(joint == m_rightWheelMotor)){
909  // printf("Robot.isGrabbed TRUE \n");
910  return true;
911  }
912  if((jointList->next == nullptr)){
913  return false;
914  }
915  jointList = jointList->next;
916  joint = jointList->joint;
917  }
918  return false;
919  }
920  else{
921  b2JointEdge* jointList = m_leftWheel->GetJointList();
922  b2Joint* joint = jointList->joint;
923  for (int i=0; i<6; i++){
924  // printf("Robot.isGrabbed 2 \n");
925  if(!(joint == m_leftWheelMotor)){
926  // printf("Robot.isGrabbed TRUE \n");
927  return true;
928  }
929  if((jointList->next == nullptr)){
930  return false;
931  }
932  jointList = jointList->next;
933  joint = jointList->joint;
934  }
935  return false;
936  }
937 }
938 bool Robot::isGrabbed(){
939 // printf("Robot.isGrabbed \n");
940  bool rightGrabbed = false;
941  bool leftGrabbed = false;
942 
943  b2JointEdge* jointList = m_rightWheel->GetJointList();
944  b2Joint* joint = jointList->joint;
945 
946  for (int i=0; i<6; i++){
947 // printf("Robot.isGrabbed 2 \n");
948  if(!(joint == m_rightWheelMotor || joint == m_currentGripJoint || joint == m_previousGripJoint)){
949 // printf("Robot.isGrabbed TRUE \n");
950  rightGrabbed = true;
951  break;
952  }
953  if((jointList->next == nullptr)){
954  break;
955  }
956  jointList = jointList->next;
957  joint = jointList->joint;
958  }
959 
960 // printf("Robot.isGrabbed \n");
961  jointList = m_leftWheel->GetJointList();
962  joint = jointList->joint;
963 
964  for (int i=0; i<6; i++){
965 // printf("Robot.isGrabbed 2 \n");
966  if(!(joint == m_leftWheelMotor || joint == m_currentGripJoint || joint == m_previousGripJoint)){
967 // printf("Robot.isGrabbed TRUE \n");
968  leftGrabbed = true;
969  break;
970  }
971  if((jointList->next == nullptr)){
972  break;
973  }
974  jointList = jointList->next;
975  joint = jointList->joint;
976  }
977 
978  m_isGrabbed = (leftGrabbed || rightGrabbed);
979  return (leftGrabbed || rightGrabbed);
980 }
981 
982 b2RevoluteJoint* Robot::getMotor(side s){
983  if (s == LEFT){
984  return m_leftWheelMotor;
985  }
986  else{
987  return m_rightWheelMotor;
988  }
989 }
990 
991 int Robot::nbJoint(side s){
992  if (s == LEFT){
993  return m_nbJointLeft;
994  }
995  else{
996  return m_nbJointRight;
997  }
998 }
999 void Robot::incrementNbJoint(side s){
1000  if (s == LEFT){
1001  m_nbJointLeft++;
1002  }
1003  else{
1004  m_nbJointRight++;
1005  }
1006 }
1007 void Robot::resetNbJoint(side s){
1008  if (s == LEFT){
1009  m_nbJointLeft = 0;
1010  }
1011  else{
1012  m_nbJointRight = 0;
1013  }
1014 }
1015 
1016 //void Robot::releaseGrip(b2World* world, b2Joint* gripJoint){
1017 //
1018 //}
1019 
1020 void Robot::setContact(bool contact){
1021  m_contact = contact;
1022 }
1023 void Robot::setDelay(int delay){
1024  //printf("set delay: %d tick \n", delay);
1025  m_delay = delay;
1026 }
1027 void Robot::setId(int id){
1028  m_id = id;
1029 }
1030 void Robot::setSpeed(double speed){
1031  m_angularSpeed = speed;
1032 }
1033 void Robot::setState(e_state state){
1034  m_state = state;
1035 }
1036 
1037 double Robot::getAngle(){
1038  double a = m_robotBody->GetAngle();
1039  return a;
1040 }
1041 b2Body* Robot::getBody(){
1042  return m_robotBody;
1043 }
1044 double Robot::getBodyLength(){
1045  double bl = 2*m_robotParameters.wheel_radius + m_robotParameters.wheel_distance;
1046  return bl;
1047 }
1048 int Robot::getDelay(){
1049 
1050  //printf("m_delai: %d tick \n", m_delay);
1051  return m_delay;
1052 }
1053 int Robot::getId(){
1054  return m_id;
1055 }
1056 b2PrismaticJointDef Robot::getJointDef(side s){
1057  if (s == LEFT){
1058  return m_leftGripJointDef;
1059  }
1060  else{
1061  return m_rightGripJointDef;
1062  }
1063 
1064 }
1065 b2Vec2 Robot::getPosition(){
1066  b2Vec2 pos = m_robotBody->GetPosition();
1067  return pos;
1068 }
1069 e_state Robot::getState(){
1070  return m_state;
1071 }
1072 b2Body* Robot::getWheel(side s){
1073  if (s == LEFT){
1074  return m_leftWheel;
1075  }
1076  else{
1077  return m_rightWheel;
1078  }
1079 }
1080 
1081 
1082 bool Robot::isContact(){
1083  return m_contact;
1084 }
1085 bool Robot::isMoving(){
1086  return m_moving;
1087 }
1088 bool Robot::isReady(){
1089  return m_ready;
1090 }
1091 
1092 
1093 void Robot::destroyBody(){
1094  m_leftWheel->GetWorld()->DestroyBody( m_leftWheel );
1095  m_rightWheel->GetWorld()->DestroyBody( m_rightWheel );
1096  m_robotBody->GetWorld()->DestroyBody( m_robotBody );
1097 }
1098 
Robot::drawGripJoint
void drawGripJoint(sf::RenderWindow &window, double m_to_px)
Definition: Robot.cpp:233
side
side
Definition: Robot.h:30
Robot::isReady
bool isReady()
Definition: Robot.cpp:1088
Robot::isGrabbed
bool isGrabbed()
Definition: Robot.cpp:938
Robot.h
Implement the Robot class and define enum linked to a robot object: e_state and side.
Robot::checkGripp
bool checkGripp(side s)
Definition: Robot.cpp:902
Robot::drawJoint
void drawJoint(sf::RenderWindow &window, double m_to_px)
Definition: Robot.cpp:303
Robot::getBodyLength
double getBodyLength()
Definition: Robot.cpp:1044
Robot::setDelay
void setDelay(int delay)
Definition: Robot.cpp:1023
Robot::getId
int getId()
Definition: Robot.cpp:1053
e_state
e_state
Definition: Robot.h:26
Robot::getAngle
double getAngle()
Definition: Robot.cpp:1037
Robot::allowMotorRotation
void allowMotorRotation(side s)
Definition: Robot.cpp:564
Robot::setSpeed
void setSpeed(double speed)
Definition: Robot.cpp:1030
Robot::blockMotorRotation
void blockMotorRotation(side s)
Definition: Robot.cpp:522
Robot::isMoving
bool isMoving()
Definition: Robot.cpp:1085
Config.h
Implementation of the Configuration class used to parse the configuration file and the sConfig struct...
Robot::getJointDef
b2PrismaticJointDef getJointDef(side s)
Definition: Robot.cpp:1056
Robot::getState
e_state getState()
Definition: Robot.cpp:1069
Robot::getWheel
b2Body * getWheel(side s)
Definition: Robot.cpp:1072
Robot::moveBodyWithImpulse
void moveBodyWithImpulse()
Definition: Robot.cpp:495
Robot::limitMotorRotation
void limitMotorRotation(side s, double limit_angle_rad)
Definition: Robot.cpp:543
Robot::createBody
void createBody(b2World *world, config::sRobot m_robotParameters, double posX, double posY, double angle=0)
Definition: Robot.cpp:55
Robot::resetNbJoint
void resetNbJoint(side s)
Definition: Robot.cpp:1007
Robot::destroyBody
void destroyBody()
Definition: Robot.cpp:1093
Robot::grip
void grip(b2Contact *contact, b2Body *otherBody, double m_to_px)
Definition: Robot.cpp:575
Robot::incrementNbJoint
void incrementNbJoint(side s)
Definition: Robot.cpp:999
Robot::setContact
void setContact(bool contact)
Definition: Robot.cpp:1020
Robot::setId
void setId(int id)
Definition: Robot.cpp:1027
Robot::isContact
bool isContact()
Definition: Robot.cpp:1082
Robot::getMotor
b2RevoluteJoint * getMotor(side s)
Definition: Robot.cpp:982
Robot::gripSide
bool gripSide(b2Contact *contact, b2Body *otherBody, double m_to_px)
Definition: Robot.cpp:641
Robot::gripGroundFromPos
void gripGroundFromPos()
Definition: Robot.cpp:729
Robot::getDelay
int getDelay()
Definition: Robot.cpp:1048
Robot::drawBody
void drawBody(sf::RenderWindow &window, double m_to_pix)
Definition: Robot.cpp:142
Robot::getBody
b2Body * getBody()
Definition: Robot.cpp:1041
Robot::nbJoint
int nbJoint(side s)
Definition: Robot.cpp:991
Robot::setState
void setState(e_state state)
Definition: Robot.cpp:1033
Robot::m_previousGripJoint
b2PrismaticJoint * m_previousGripJoint
Definition: Robot.h:287
Robot::turnOffMotor
void turnOffMotor(side s)
Definition: Robot.cpp:513
Robot::moveBodyWithMotor
void moveBodyWithMotor()
Definition: Robot.cpp:442
Robot::moveBodyWithForce
void moveBodyWithForce()
Definition: Robot.cpp:470
Robot::getPosition
b2Vec2 getPosition()
Definition: Robot.cpp:1065
Generated by   doxygen 1.8.14