2D LUA Based Robot Simulator

// Author : Auralius Manurung
// Contact : mr_manurung@yahoo.com

#include "StdAfx.h"
#include "math.h"
#include "Robot.h"

CRobot::CRobot(void)
{
	canResetTime = false;
}

CRobot::~CRobot(void)
{
}

CPoint CRobot::rotate(CPoint centerPt, double distFromCenterPt, double theta)
{
	return CPoint(distFromCenterPt * cos(theta) + centerPt.x, distFromCenterPt * sin(theta) + centerPt.y);
}

double CRobot::getDistance(CPoint pt1, CPoint pt2)
{
	return sqrt(pow((double)(pt2.x - pt1.x), 2) + pow((double)(pt2.y - pt1.y), 2));
}

double CRobot::sensorValue(CDC* pDC, int sensorIndex, double accuracy)
{
	int val = 0;
	pDC->MoveTo(m_robot.pos.x, m_robot.pos.y);

	CPoint pt;
	double dist = 0.0;
	// measure
	while (dist <= MAX_SENSOR_VAL){
		dist = dist + accuracy;
		pt = rotate(m_robot.pos, dist, m_robot.theta + sensorIndex * 2 * M_PI / m_robot.sensorNum);
		dist = getDistance(m_robot.pos, pt);
		m_robot.sensorValue[sensorIndex] = dist;
		if (isObstacleDetected(pDC, pt))
			break;
	}
	CPen sensorPen(PS_DOT, 1, RGB(255, 0, 0));
	CPen *oldPen = pDC->SelectObject(&sensorPen);

	// draw sensor beam
	if (dist < MAX_SENSOR_VAL){
		pDC->MoveTo(m_robot.pos.x, m_robot.pos.y);
		pDC->LineTo(pt.x, pt.y);
		pDC->SelectObject(oldPen);
	}

	return dist;
}

bool CRobot::isObstacleDetected(CDC* pDC, CPoint pt)
{
	COLORREF rgb = pDC->GetPixel(pt.x, pt.y);
	
	//if (rgb > (OBSTACLE - 10000) && rgb < (OBSTACLE + 10000)) // Make it safe
	if (rgb == OBSTACLE)
		return true;

	return false;
}

void CRobot::drawRobot(CDC* pDC)
{
	pDC->MoveTo(m_robot.pos.x, m_robot.pos.y);

	updateSensorValue(pDC);
	
	// draw robot
	CPen robotPen(PS_SOLID, 1, RGB(0, 0, 255));
	CPen *oldPen = pDC->SelectObject(&robotPen);
	pDC->Ellipse(m_robot.pos.x - m_robot.size / 2, m_robot.pos.y - m_robot.size / 2, 
		m_robot.pos.x + m_robot.size / 2, m_robot.pos.y + m_robot.size / 2);	

	// Draw head
	CPoint pt;
	pt = rotate(m_robot.pos, m_robot.size / 2, m_robot.theta);
	pDC->Ellipse(pt.x-2, pt.y-2, pt.x+2, pt.y+2);

	// Draw wheels
	CPoint pt1 = rotate(m_robot.pos, m_robot.size / 2 - 5, m_robot.theta + M_PI/4);
	CPoint pt2 = rotate(m_robot.pos, m_robot.size / 2 - 5, m_robot.theta + M_PI/4 * 3);
	CPoint pt3 = rotate(m_robot.pos, m_robot.size / 2 - 5, m_robot.theta - M_PI/4 );
	CPoint pt4 = rotate(m_robot.pos, m_robot.size / 2 - 5, m_robot.theta - M_PI/4 * 3);

	pDC->MoveTo(pt1.x, pt1.y);
	pDC->LineTo(pt2);
	pDC->MoveTo(pt3.x, pt3.y);
	pDC->LineTo(pt4);

	pDC->SelectObject(oldPen);
}

void CRobot::goRobotGo(double *t)
{
	if (canResetTime){
		*t = 0;
		canResetTime = false;
	}

	int plusFactor = m_robot.rWheelSpeed + m_robot.lWheelSpeed;
	int minusFactor = m_robot.lWheelSpeed - m_robot.rWheelSpeed;

	if (m_robot.lWheelSpeed - m_robot.rWheelSpeed != 0.0){
		m_robot.pos.x = m_pos0.x + m_robot.size / 2 * plusFactor / minusFactor / 2 *(sin(minusFactor * (*t) / m_robot.size / 2 + m_robot.theta) - sin(m_robot.theta));
		m_robot.pos.y = m_pos0.y - m_robot.size / 2 * plusFactor / minusFactor / 2 *(cos(minusFactor * (*t) / m_robot.size / 2 + m_robot.theta) - cos(m_robot.theta));
		m_robot.theta = m_theta0 + minusFactor * (*t) / m_robot.size ;

	}
	else{
		m_robot.pos.x = plusFactor / 2 * cos(m_robot.theta) * (*t) + m_pos0.x;
		m_robot.pos.y = plusFactor / 2 * sin(m_robot.theta) * (*t) + m_pos0.y;
	}
}

void CRobot::setLWheelSpeed(int speed)
{
	// if changing then save previous value
	if (speed != m_robot.lWheelSpeed){
		m_pos0 = m_robot.pos;
		m_theta0 = m_robot.theta;
		canResetTime = true;
	}

	m_robot.lWheelSpeed = speed;
}

void CRobot::setRWheelSpeed(int speed)
{
	// if changing then save previous value
	if (speed != m_robot.rWheelSpeed){
		m_pos0 = m_robot.pos;
		m_theta0 = m_robot.theta;
		canResetTime = true;
	}

	m_robot.rWheelSpeed = speed;
}

void CRobot::setAngle(double theta)
{
	//m_theta0 = m_robot.theta;
	m_robot.theta = theta;
}

bool CRobot::isRobotDetected(CPoint pt)
{
	if (pt.x > m_robot.pos.x - m_robot.size / 2 && pt.x < m_robot.pos.x + m_robot.size / 2
		&& pt.y > m_robot.pos.y - m_robot.size / 2 && pt.y < m_robot.pos.y + m_robot.size / 2)
		return true;
	else
		return false;
}

int CRobot::getSpeed(int index)
{
	if (index = RIGHT)
		return m_robot.rWheelSpeed;
	else if (index = LEFT)
		return m_robot.lWheelSpeed;
	else
		return 0;
}

void CRobot::updateSensorValue(CDC *pDC)
{
	// get sensor values and draw them
	for (int i = 0; i < m_robot.sensorNum; i++)
		sensorValue(pDC, i);
}

double CRobot::getSensorValue(int index)
{

	return m_robot.sensorValue[index];
}