Differential-Drive Robot Motion

#pragma once
#include "vec2.h"


class robot
{
public:
	double x, y, theta;
	robot(): x(0), y(0), theta(0) { }
	void setPose(double _x, double _y, double _theta) {
		x = _x; y = _y; theta = _theta;
	}
	void move(const Vec2 &v);
	void draw(CDC &dc);
};

robot.cpp

#include "stdafx.h"
#include <math.h>
#include "robot.h"
#include "vec2.h"
#include "point2.h"

void robot::move(const Vec2 &v)
{
	const double radg = atan(1.0)/45.0;
	const double wp = 0.2*8.0*radg;
	double  dv, dtheta,rtheta;
	double vr = v[0];
	double vl = v[1];

	dtheta = (vr-vl)/wp;
	dv = 0.5*(vr+vl);
	rtheta = radg*theta;
	x += dv*cos(rtheta);
	y += dv*sin(rtheta);
	theta += dtheta;
}
	

void robot::draw(CDC &dc) {
	const double d = 5.0, w = 0.5, h = 2.0;
	Vec2 tmp;
	Vec2 pts[] = { Vec2(-d,d), Vec2(d,d), Vec2(1.4*d,0),Vec2(d,-d), Vec2(-d,-d) };
	Vec2 wheel[] = { Vec2(-h,-w), Vec2(-h,w), Vec2(h,w), Vec2(h,-w) };
	int npts = 5;
	int i;
	CPoint *cpts = new CPoint[npts];
	for (i=0; i<npts; i++) {
		tmp = pts[i];
		tmp.scale(0.2).rotate(theta).translate(x,y);
		point2::convert(tmp,cpts[i]);
	}
	dc.Polygon(cpts,npts);
	npts = 4;
	for (i=0; i<npts; i++) {
		tmp = wheel[i];
		tmp.translate(0.0,-4.0).scale(0.2).rotate(theta).translate(x,y);
		point2::convert(tmp,cpts[i]);
	}
	CBrush brush(RGB(0,0,0));
	CBrush *oldbrush = (CBrush *) dc.SelectObject(&brush);
	dc.Polygon(cpts,npts);
	for (i=0; i<npts; i++) {
		tmp = wheel[i];
		tmp.translate(0.0,4.0).scale(0.2).rotate(theta).translate(x,y);
		point2::convert(tmp,cpts[i]);
	}
	dc.Polygon(cpts,npts);
	dc.SelectObject(oldbrush);
	delete [] cpts;
}

Reference