Ajout d’une petite simulation physique avec Box2D

include/simul/element.h

@@ -3,6 +3,7 @@
 
 #include "Position.h"
 #include <QPainter>
+#include "Box2D/Box2D.h"
 
 class Element
 {
@@ -17,11 +18,9 @@ public:
 
 	Type type;
 
-	Element(Position p, Type t)
-	{
-		this->p = p;
-		type = t;
-	}
+	b2Body* body;
+
+	Element(b2World & world, Position p, Type t);
 
 	void paint(QPainter & pa)
 	{
@@ -29,8 +28,10 @@ public:
 		pa.setBrush(QBrush(QColor("yellow")));
 		pa.setPen(QBrush(QColor("yellow")));
 
-		pa.drawEllipse(QPoint(p.x.getValueInMillimeters(),p.x.getValueInMillimeters()),150,150);
+		pa.drawEllipse(QPoint(p.x.getValueInMillimeters(),p.y.getValueInMillimeters()),100,100);
 	}
+
+	void updatePos();
 };
 
 #endif //PION_H_INCLUDED

include/simul/robot.h

@@ -5,6 +5,7 @@
 #include <QKeyEvent>
 #include <boost/circular_buffer.hpp>
 #include "PositionPlusAngle.h"
+#include <Box2D/Box2D.h>
 
 class Robot
 {
@@ -19,11 +20,13 @@ public:
 	class OdoRobot* odometrie;
 	class Strategie* strategie;
 	
+	b2Body* body;
 
-	Robot();
+	Robot(b2World &world);
 
 	void paint(QPainter &p, int dt);
 	void keyPressEvent(QKeyEvent* evt,bool press);
+	void updateForces(int dt);
 };
 
 #endif //ROBOT_H_INCLUDED

include/simul/table.h

@@ -3,6 +3,7 @@
 
 #include <QWidget>
 #include "element.h"
+#include <Box2D/Box2D.h>
 
 class Table : public QWidget
 {
@@ -10,6 +11,9 @@ private:
 	int dt;
 	std::vector<class Element*> elements;
 	std::vector<class Robot*> robots;
+
+	b2World world;
+	b2Body* tableBody;
 public:
 
 	static const int tableWidth = 3000;

simulation/Makefile.am

@@ -4,6 +4,7 @@ INCLUDES = -Iinclude -Iimpl -Imoc -I.
 
 
 paprika_SOURCES=\
+src/simul/element.cpp \
 src/simul/main.cpp \
 src/simul/main_window.cpp \
 src/simul/table.cpp \

simulation/aclocal.m4

@@ -1108,3 +1108,4 @@ AC_SUBST([am__untar])
 ]) # _AM_PROG_TAR
 
 m4_include([m4/ax_boost_base.m4])
+m4_include([m4/ax_dep_check.m4])

simulation/configure.ac

@@ -23,6 +23,8 @@ AX_BOOST_BASE([1.40], [AC_MSG_RESULT(yes) ] , [AC_MSG_RESULT(no) ] )
 
 CXXFLAGS=
 
+AX_DEP_CHECK(Box2D, [/usr/lib /usr/local/lib], libBox2D.so,[/usr/include /usr/local/include],Box2D/Box2D.h, -lBox2D)
+
 AC_ARG_ENABLE([debug], AS_HELP_STRING([--enable-debug],[Turn on debugging]), [enable_debug=$enableval],[enable_debug="no"])
 AC_MSG_CHECKING(debug)
 AC_MSG_RESULT($enable_debug)
@@ -30,8 +32,8 @@ if test "x$enable_debug" = "xyes"; then
 	CXXFLAGS="$CXXFLAGS -g3 -ggdb -Wall -DDEBUG"
 fi
 
-CXXFLAGS="$CXXFLAGS   $AM_CXXFLAGS $QT_CFLAGS $BOOST_CFLAGS"
-LDFLAGS="$LDFLAGS $AM_LDFLAGS $QT_LIBS $BOOST_LDFLAGS"
+CXXFLAGS="$CXXFLAGS   $AM_CXXFLAGS $QT_CFLAGS $BOOST_CFLAGS $Box2D_CFLAGS"
+LDFLAGS="$LDFLAGS $AM_LDFLAGS $QT_LIBS $BOOST_LDFLAGS $Box2D_LIBS"
 AC_SUBST([CFLAGS])
 
 AC_HEADER_STDC

simulation/m4/ax_dep_check.m4

+
+AC_DEFUN([AX_DEP_CHECK],
+[
+
+libluabindpath="/usr/lib /usr/local/lib"
+libluabindname=libluabind.so
+libluabindheader="/usr/include /usr/local/include"
+
+AC_MSG_CHECKING(for $1 libs)
+
+for l in $2; do
+	if test -f $l/$3; then
+		AC_MSG_RESULT($l/$3)
+		$1_LIBS="-L$l $6"
+		break
+	fi
+done
+
+if test -z "${$1_LIBS}"; then
+	AC_MSG_RESULT(not found)
+	exit 1
+fi
+
+AC_MSG_CHECKING(for $1 headers)
+
+for l in $4; do
+	if test -f $l/$5; then
+		AC_MSG_RESULT($l)
+		$1_CFLAGS="-I$l"
+		break
+	fi
+done
+
+if test -z "${$1_CFLAGS}"; then
+	AC_MSG_RESULT(not found)
+	exit 1
+fi
+
+AC_SUBST($1_CFLAGS)
+AC_SUBST($1_LIBS)
+
+])

src/asservissement.cpp

@@ -62,7 +62,8 @@ Asservissement::Asservissement(Odometrie* _odometrie) :
     en_mouvement = false;
     Asservissement::asservissement = this;
 
-        *((uint32_t *)(STK_CTRL_ADDR)) = 0x03; // CLKSOURCE:0 ; TICKINT: 1 ; ENABLE:1
+#ifdef ROBOTHW
+    *((uint32_t *)(STK_CTRL_ADDR)) = 0x03; // CLKSOURCE:0 ; TICKINT: 1 ; ENABLE:1
     *((uint32_t *)(STK_LOAD_ADDR)) = 9000*nb_ms_between_updates; // valeur en ms*9000 (doit etre inférieur à 0x00FFFFFF=16 777 215)
 
     NVIC_InitTypeDef SysTick_IRQ;
@@ -72,15 +73,18 @@ Asservissement::Asservissement(Odometrie* _odometrie) :
     SysTick_IRQ.NVIC_IRQChannelPreemptionPriority = 0;
     SysTick_IRQ.NVIC_IRQChannelSubPriority = 1;
     NVIC_Init(&SysTick_IRQ);
+#endif
 }
 
 int asserCount = 0;
 
 void Asservissement::update(void)
 {
+/*#ifdef ROBOTHW
     roues.gauche.tourne(0.1);
     roues.droite.tourne(0.1);
     return;
+#endif*/
     //PositionPlusAngle before(odometrie.positionPlusAngle);
     asserCount++;
 

src/roue.cpp

@@ -29,6 +29,7 @@ Roue::Roue(unsigned char OCx, GPIO_TypeDef * GPIOx_Sens, uint16_t GPIO_Pin_Sens)
 
 void Roue::tourne(float rapport)
 {
+    rapport = 0;
     if(rapport >= 0){
         GPIO_WriteBit(GPIOx_Sens, GPIO_Pin_Sens, Bit_RESET);
     }

src/simul/element.cpp

+#include "simul/element.h"
+
+Element::Element(b2World & world, Position p, Type t)
+{
+	this->p = p;
+	type = t;
+
+	b2BodyDef bodyDef;
+	bodyDef.type = b2_dynamicBody;
+	bodyDef.position.Set(p.x.getValueInMillimeters()/100., p.y.getValueInMillimeters()/100.);
+	
+	body = world.CreateBody(&bodyDef);
+
+	b2CircleShape circle;
+	circle.m_p.Set(0.,0.);
+	circle.m_radius = 1.0f;
+
+	b2FixtureDef fixtureDef;
+	fixtureDef.shape = &circle;
+	fixtureDef.density = 1.0f;
+	fixtureDef.friction = 0; //0.3f;
+	body->CreateFixture(&fixtureDef);
+}
+
+void Element::updatePos()
+{
+	p.x = 100.*body->GetPosition().x;
+	p.y = 100.*body->GetPosition().y;
+}

src/simul/robot.cpp

@@ -23,12 +23,12 @@ public:
 		return robot->pos;
 	}
 
-	Distance vitesseLineaire()
+	Distance getVitesseLineaire()
 	{
 		return robot->deriv.position.getNorme();
 	}
 
-	Angle vitesseAngulaire()
+	Angle getVitesseAngulaire()
 	{
 		return robot->deriv.angle;
 	}
@@ -40,11 +40,8 @@ public:
 };
 
 
-Robot::Robot() : olds(10000)
+Robot::Robot(b2World & world) : olds(10000)
 {
-	pos.position.x=1000;
-	pos.position.y=500;
-	pos.angle=0;
 
 	deriv.position.x = 0;
 	deriv.position.y = 0;
@@ -55,22 +52,72 @@ Robot::Robot() : olds(10000)
 	odometrie = new OdoRobot(this);
 	asservissement = new Asservissement(odometrie);
 	strategie = new Strategie(true, asservissement);
+
+	pos.position.x=1000.;
+	pos.position.y=500.;
+	pos.angle=0;
+
 	asservissement->strategie = strategie;
+
+	b2BodyDef bodyDef;
+	bodyDef.type = b2_dynamicBody;
+	bodyDef.position.Set(pos.position.x.getValueInMillimeters()/100., pos.position.y.getValueInMillimeters()/100.);
+	bodyDef.angle = pos.angle.getValueInRadian();
+	
+	body = world.CreateBody(&bodyDef);
+
+	b2PolygonShape box;
+	box.SetAsBox(1.,1., b2Vec2(1,1),0);
+
+	b2FixtureDef fixtureDef;
+	fixtureDef.shape = &box;
+	fixtureDef.density = 10.0f;
+	fixtureDef.friction = 0.f;
+	body->CreateFixture(&fixtureDef);
+}
+
+void Robot::updateForces(int dt)
+{
+	if(dt == 0)
+		return;
+
+	Position impulse;
+	impulse.x = (deriv.position.x*cos(pos.angle.getValueInRadian()) - deriv.position.y*sin(pos.angle.getValueInRadian()));
+	impulse.y = (deriv.position.x*sin(pos.angle.getValueInRadian()) + deriv.position.y*cos(pos.angle.getValueInRadian()));
+
+	float32 rdt = 1000./(float)dt;
+
+	b2Vec2 bimpulse = 10*(0.01*rdt*b2Vec2(impulse.x.getValueInMillimeters(),impulse.y.getValueInMillimeters()) - body->GetLinearVelocity());
+	//body->SetLinearVelocity(bimpulse);
+	body->ApplyForce(body->GetMass()*bimpulse, body->GetWorldCenter());
+	//body->SetAngularVelocity(deriv.angle.getValueInRadian()*(1000./(float)dt));
+	body->ApplyTorque((deriv.angle.getValueInRadian()*rdt - body->GetAngularVelocity())*body->GetInertia());
+
 }
 
+
 void Robot::paint(QPainter &p, int dt)
 {
-	if(manual)
-	{
-		keyPressEvent(NULL,false);
-		deriv.position.x = deriv.position.x* 0.99;
-		deriv.angle = deriv.angle * 0.9;
-	}
-	else
+	if(dt)
 	{
-		Asservissement::asservissement->update();
-		deriv.position.x = asservissement->vitesse_lineaire_a_atteindre;
-		deriv.angle = asservissement->vitesse_angulaire_a_atteindre;
+		pos.position.x = 100*body->GetWorldCenter().x;
+		pos.position.y = 100*body->GetWorldCenter().y;
+		pos.angle = body->GetAngle();
+
+		olds.push_back(pos);
+		if(manual)
+		{
+			keyPressEvent(NULL,false);
+			deriv.position.x = deriv.position.x* 0.97;
+			deriv.angle = deriv.angle * 0.9;
+		}
+		else
+		{
+			Asservissement::asservissement->update();
+			deriv.position.x = asservissement->vitesse_lineaire_a_atteindre;
+			deriv.position.y = 0;
+			deriv.angle = asservissement->vitesse_angulaire_a_atteindre;
+		}
 	}
 
 	p.setWorldTransform(QTransform().translate(pos.position.x.getValueInMillimeters(),pos.position.y.getValueInMillimeters()).rotateRadians(pos.angle.getValueInRadian()));
@@ -87,16 +134,9 @@ void Robot::paint(QPainter &p, int dt)
 	p.drawLine(0,100*pos.angle.getValueInRadian(),0,0);
 	p.setWorldTransform(QTransform());
 
-	if(dt)
-	{
-		olds.push_back(pos);
-		pos.position.x += (deriv.position.x*cos(pos.angle.getValueInRadian()) - deriv.position.y*sin(pos.angle.getValueInRadian()));
-		pos.position.y += (deriv.position.x*sin(pos.angle.getValueInRadian()) + deriv.position.y*cos(pos.angle.getValueInRadian()));
-		pos.angle += deriv.angle;
-	}
 
 	p.setPen(QColor(Qt::green));
-	for(unsigned int i=0; i < olds.size()-1; i++)
+	for(unsigned int i=0; i+1 < olds.size(); i++)
 		p.drawLine(olds[i].position.x.getValueInMillimeters(), olds[i].position.y.getValueInMillimeters(), olds[i+1].position.x.getValueInMillimeters(), olds[i+1].position.y.getValueInMillimeters());	
 }
 
@@ -111,8 +151,8 @@ void Robot::keyPressEvent(QKeyEvent* evt, bool press)
 
 	if(manual)
 	{
-		float dinc = 0.1;
-		float ainc = 0.003;
+		float dinc = .5;
+		float ainc = 0.005;
 
 		IF_KEYSWITCH(avant,evt->key() == Qt::Key_Up)
 			deriv.position.x += dinc;
@@ -122,7 +162,6 @@ void Robot::keyPressEvent(QKeyEvent* evt, bool press)
 			deriv.angle += ainc;
 		IF_KEYSWITCH(droite,evt->key() == Qt::Key_Left)
 			deriv.angle -= ainc;
-		std::cout << deriv.position.x.getValueInMillimeters() << " & " << deriv.angle.getValueInRadian() << std::endl;
 	}
 }
 

src/simul/table.cpp

@@ -3,7 +3,7 @@
 #include <iostream>
 #include <QPainter>
 
-Table::Table(QWidget* parent) : QWidget(parent)
+Table::Table(QWidget* parent) : QWidget(parent), world(b2Vec2(0.f,0.f), false)
 {
 	dt=0;
 	setAutoFillBackground(true);
@@ -12,9 +12,56 @@ Table::Table(QWidget* parent) : QWidget(parent)
 	p.setColor(QPalette::Window,QColor(Qt::darkGray));
 	setPalette(p);
 
-	robots.push_back(new Robot());
+	robots.push_back(new Robot(world));
 
-	elements.push_back(new Element(Position(300,300),Element::Pion));
+	elements.push_back(new Element(world,Position(900,900),Element::Pion));
+
+	//Geometry
+	b2BodyDef bodyDef;
+	bodyDef.position.Set(0., 0.);
+	
+	tableBody = world.CreateBody(&bodyDef);
+
+	
+	b2PolygonShape box;
+	b2FixtureDef fixtureDef;
+	fixtureDef.shape = &box;
+	fixtureDef.friction = 0;
+	fixtureDef.density = 0;
+
+	box.SetAsBox(30,1, b2Vec2(0,-1),0);
+	tableBody->CreateFixture(&fixtureDef);
+
+	box.SetAsBox(1,21, b2Vec2(-1,0),0);
+	tableBody->CreateFixture(&fixtureDef);
+
+	box.SetAsBox(1,21, b2Vec2(31,0),0);
+	tableBody->CreateFixture(&fixtureDef);
+
+	box.SetAsBox(30.,1., b2Vec2(0,22),0);
+	tableBody->CreateFixture(&fixtureDef);
+
+	//Starting zones borders
+	box.SetAsBox(4.00,.11, b2Vec2(0.,4.), 0.);
+	tableBody->CreateFixture(&fixtureDef);
+	box.SetAsBox(4.00,.11, b2Vec2(30.,4.), 0.);
+	tableBody->CreateFixture(&fixtureDef);
+
+	//Blocked zones
+	box.SetAsBox(3.50,1.20, b2Vec2(8,21), 0.);
+	tableBody->CreateFixture(&fixtureDef);
+	box.SetAsBox(3.50,1.20, b2Vec2(22,21), 0.);
+	tableBody->CreateFixture(&fixtureDef);
+
+	box.SetAsBox(.11,0.65, b2Vec2(4.61,19.15), 0.);
+	tableBody->CreateFixture(&fixtureDef);
+	box.SetAsBox(.11,0.65, b2Vec2(18.61,19.15), 0.);
+	tableBody->CreateFixture(&fixtureDef);
+
+	box.SetAsBox(.11,0.65, b2Vec2(11.39,19.15), 0.);
+	tableBody->CreateFixture(&fixtureDef);
+	box.SetAsBox(.11,0.64, b2Vec2(25.39,19.15), 0.);
+	tableBody->CreateFixture(&fixtureDef);
 }
 
 Table::~Table()
@@ -24,6 +71,13 @@ Table::~Table()
 void Table::update(int dt)
 {
 	this->dt = dt;
+	for(unsigned int i=0; i < robots.size(); i++)
+		robots[i]->updateForces(dt);
+
+	world.Step((float)dt/1000., 10, 10);
+	world.ClearForces();
+	for(unsigned int i=0; i < elements.size(); i++)
+		elements[i]->updatePos();
 	repaint();
 }