2D LUA Based Robot Simulator

		// Author : Auralius Manurung
// Contact : auralius@lavabit.com

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

///////////////////////////////////////////////////////////////////////////////////////////////////////
// CONSTRUCTORS AND DESTRUCTORS ///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////

CRobot::CRobot(void)
{
	// Default setting when CRobot is created

	mCanDrawTrajectory = false;
	mZoom = 1;

	SetSensorNum(6); // 6 sensors
	SetRadiationCone();
	SetSensorRange();
	SetNoiseProperties(0.0, 0.0);
	SetSimulationTimeStep();

	AddRobot(CPoint(200, 200));
	SetActiveRobot(0);
}

CRobot::~CRobot(void)
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
// PRIVATE MEMBERS/////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////

inline float CRobot::UniformNoise()
{
	return ((float)rand()/(float)(RAND_MAX+1));
}

float CRobot::GaussianNoise(const float &mean, const float &deviation)
{
	// Press W.H., Teukolsky S.A., Vetterling W.T., Flannery B.P. Numerical recipes in C, pp 289-290
	if (mean == 0.0 && deviation == 0.0)
		return 0.0;

	float x,v1,v2,r;
	if (mNoiseTime == 0.0){
		do {
			v1 = 2.0 * UniformNoise() - 1.0;
			v2 = 2.0 * UniformNoise() - 1.0;
			r = v1 * v1 + v2 * v2;
		} while (r >= 1.0);

		r = sqrt((-2.0 * log(r)) / r);
		mNoiseTime = v2 * r;
		return (mean + v1 * r * deviation);
	}
		else{
			x = mNoiseTime;
			mNoiseTime = 0.0;
			return (mean + x * deviation);
		}
}

inline CPoint CRobot::Rotate(CPoint center_pt, float dist_from_center_pt, float theta)
{
	return CPoint(dist_from_center_pt * cos(theta) + center_pt.x, dist_from_center_pt * sin(theta) + center_pt.y);
}

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

void CRobot::UpdateSensorValue(CDC* pDC, ROBOT *robot, float accuracy)
{
	int pos_x = robot->XPos;
	int pos_y = robot->YPos;

	float sensor_dist = robot->Size / 2 - 3;
	float step = 2 * M_PI / mSensorNum;

	for (int sensor_index = 0; sensor_index < mSensorNum; sensor_index++){
		pDC->MoveTo(pos_x, pos_y);
		float min_dist = mSensorRange[MAX_VAL];
		float pivot_angle = robot->Theta + sensor_index * step;

		// Calculate sensor position 
		CPoint sensor_position = Rotate(CPoint(pos_x, pos_y), sensor_dist, pivot_angle);
			
		// measure
		for (int i = 0; i <= 2; i++){
			float dist = 0.0;
			while (dist <= mSensorRange[MAX_VAL]){
				dist = dist + accuracy;
				CPoint pt = Rotate(sensor_position, dist, pivot_angle - mRadiationCone + i * mRadiationCone);
				dist = CalcDistance(sensor_position, pt);
				if (IsObstacleDetected(pDC, pt)){
					if (dist > 5) // minimum sensor reading
						break;
				}
			}
			if (dist < min_dist)
				min_dist = dist;
		}

		// Should not smaller than desired minimum value
		if (min_dist >=  mSensorRange[MIN_VAL])
			robot->SensorValue[sensor_index] = min_dist + GaussianNoise(mMeanNoise,mDevNoise);
		else
			robot->SensorValue[sensor_index] = mSensorRange[MIN_VAL];

		CPen sensor_pen(PS_DOT, 1, RGB(255, 0, 0));
		CPen *old_pen = pDC->SelectObject(&sensor_pen);

		// draw sensor beam
		if (min_dist < mSensorRange[MAX_VAL]){
			pDC->MoveTo(sensor_position);
			pDC->LineTo(Rotate(sensor_position, min_dist, pivot_angle - mRadiationCone));
			pDC->LineTo(Rotate(sensor_position, min_dist, pivot_angle + mRadiationCone));
			pDC->LineTo(sensor_position);
		}

		pDC->SelectObject(old_pen);
	}
}

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

	return false;
}

void CRobot::UpdateTrajectoryData(ROBOT *robot)
{
	CPoint pos = CPoint(robot->XPos, robot->YPos);
	UINT size = robot->Trail.size();
	
	if (size > 0){
		if ( CalcDistance(pos, robot->Trail.at(size - 1)) > 5.0)
			robot->Trail.push_back(pos);
	}
	else
		robot->Trail.push_back(pos);
}

void CRobot::DrawTrajectory(CDC *pDC, ROBOT *robot)
{
	UINT size = robot->Trail.size();

	// Draw if not empty
	if (size > 1)
	{
		CPen trail_pen(PS_DOT, 2, robot->RobotColor);
		CPen *old_pen = pDC->SelectObject(&trail_pen);
		for (UINT count = 1; count < size; count++)
		{
			CPoint start = robot->Trail.at(count-1);
			CPoint end = robot->Trail.at(count);

			pDC->MoveTo(start);
			if (CalcDistance(start, end) < 10)
				pDC->LineTo(end);
		}
		pDC->SelectObject(old_pen);
	}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
// PUBLIC MEMBERS//////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////

void CRobot::AddRobot(CPoint initial_pos)
{
	ROBOT new_robot;
	new_robot.XPos = initial_pos.x;
	new_robot.YPos = initial_pos.y;
	new_robot.Theta = 0;
	new_robot.Size = 40 * mZoom;
	new_robot.LWheelSpeed = 0;
	new_robot.RWheelSpeed = 0;
	new_robot.HittingWall = false;

	// Random initial color
	COLORREF random_color;
	srand(mVectRobot.size()+time(0));
	do{
		random_color = RGB(UniformNoise()*255, UniformNoise()*255, UniformNoise()*255);
		
	} while (random_color < RGB(255,255,255)/2); // Get rid off dark colors

	new_robot.RobotColor = random_color;
	mVectRobot.push_back(new_robot);
}

void CRobot::RemoveRobot()
{
	if (mVectRobot.size() > 1)
		mVectRobot.erase(mVectRobot.end()-1);
}

void CRobot::SetActiveRobot(UINT index)
{
	if (index < mVectRobot.size()){
		mActiveRobot = index;
		mRobot = &mVectRobot.at(index);
	}
}

void CRobot::DrawRobot(CDC* pDC)
{
	const float deg45 = M_PI / 4;
	const float deg135 = M_PI / 4 * 3;
	const float deg15 = M_PI / 180 * 15;

	for (UINT count = 0; count < mVectRobot.size(); count++){
		ROBOT *robot = &mVectRobot.at(count);
		float robot_radius = robot->Size / 2;
		CPoint pos = CPoint(robot->XPos, robot->YPos);

		pDC->MoveTo(pos);

		// Draw robot
		CPen robot_pen(PS_SOLID, 2, OBSTACLE);
		CPen *old_pen = pDC->SelectObject(&robot_pen);

		CBrush brush;
		brush.CreateSolidBrush(robot->RobotColor);
		CBrush *old_brush = pDC->SelectObject(&brush);

		pDC->Ellipse(robot->XPos - robot_radius, robot->YPos - robot_radius, robot->XPos + robot_radius, robot->YPos + robot_radius);	

		pDC->SelectObject(old_brush);
		
		int l = robot_radius - 4;

		// For wheels
		CPoint pt1 = Rotate(pos, l, robot->Theta + deg45);
		CPoint pt2 = Rotate(pos, l, robot->Theta + deg135);
		CPoint pt3 = Rotate(pos, l, robot->Theta - deg45 );
		CPoint pt4 = Rotate(pos, l, robot->Theta - deg135);

		// For Head
		CPoint pt0 = Rotate(pos, robot_radius, robot->Theta);
		CPoint pt5 = Rotate(pos, l, robot->Theta + deg15 );
		CPoint pt6 = Rotate(pos, l, robot->Theta - deg15);
		
		// Draw head
		pDC->MoveTo(pt5);
		pDC->LineTo(pt0);
		pDC->LineTo(pt6);

		// Draw wheels
		pDC->MoveTo(pt1);
		pDC->LineTo(pt2);
		pDC->MoveTo(pt3);
		pDC->LineTo(pt4);

		pDC->SelectObject(old_pen);
	}

	for (UINT count = 0; count < mVectRobot.size(); count++){
		ROBOT *robot = &mVectRobot.at(count);
		DrawTrajectory(pDC, robot);
		robot->HittingWall = SimpleCollisionDetection(robot, pDC);
		UpdateSensorValue(pDC, robot);
	}
}

void CRobot::SetDrawTrajectoryStatus(bool status)
{
	mCanDrawTrajectory = status;

	if (!status)
	for (UINT count = 0; count < mVectRobot.size(); count++){
		ROBOT *robot = &mVectRobot.at(count);
		robot->Trail.clear();
	}
}

void CRobot::SetSimulationTimeStep(float time_step)
{
	mSimulationTimeStep = time_step;
}

void CRobot::GoRobotGo()
{
	for (UINT count = 0; count < mVectRobot.size(); count++){
		ROBOT *robot = &mVectRobot.at(count);
		if (!robot->HittingWall){
			float plus_factor = robot->RWheelSpeed + robot->LWheelSpeed;
			float minus_factor = robot->LWheelSpeed - robot->RWheelSpeed;
			float half_plus_factor = plus_factor / 2;
			float a = robot->Size / 2 * plus_factor / minus_factor;
			float b = robot->Theta + minus_factor * mSimulationTimeStep / robot->Size;
			float cos_theta = cos(robot->Theta);
			float sin_theta = sin(robot->Theta);

			if (minus_factor != 0.0){
				robot->Theta =  b;
				robot->XPos = robot->XPos + a * (sin(b) - sin_theta);
				robot->YPos = robot->YPos - a * (cos(b) - cos_theta);
			}
			else{
				robot->XPos = half_plus_factor * cos_theta * mSimulationTimeStep + robot->XPos;
				robot->YPos = half_plus_factor * sin_theta * mSimulationTimeStep + robot->YPos;
			}

			if (mCanDrawTrajectory)
				UpdateTrajectoryData(robot);
		}
	}
}

void CRobot::SetSpeed(float l_speed, float r_speed)
{
		mRobot->LWheelSpeed = l_speed * mZoom;
		mRobot->RWheelSpeed = r_speed * mZoom;
}

void CRobot::PrepareToRun()
{
	// Random seed
	srand(time(0));
	mNoiseTime = 0.0;
}

void CRobot::SetPosition(CPoint pt)
{
	mRobot->XPos = pt.x;
	mRobot->YPos = pt.y;
}

void CRobot::ResetPosition()
{
	 // Line up the robot horizontally
	for (UINT count = 0; count < mVectRobot.size(); count++){
		ROBOT *robot = &mVectRobot.at(count);
		robot->XPos = 100 + (robot->Size + 10) * count;
		robot->YPos = 100;
	}
}

void CRobot::SetAngle(float theta)
{
	mRobot->Theta = theta;
}

void CRobot::SetRadiationCone(float cone_angle) 
{
	mRadiationCone = cone_angle / 2;
}

void CRobot::SetNoiseProperties(float mean, float dev)
{
	mMeanNoise = mean;
	mDevNoise = dev;
}

void CRobot::SetSensorRange(float min_dist, float max_dist)
{
	if (min_dist >= 0 && max_dist <= 1000){
		mSensorRange[MIN_VAL] = min_dist;
		mSensorRange[MAX_VAL] = max_dist;
	}
}

bool CRobot::IsRobotDetected(CPoint pt)
{
	for (UINT count = 0; count < mVectRobot.size(); count++){
		mRobot = &mVectRobot.at(count);
		float robot_radius = mRobot->Size / 2;

		if (pt.x > mRobot->XPos - robot_radius && pt.x < mRobot->XPos + robot_radius
			&& pt.y > mRobot->YPos - robot_radius && pt.y < mRobot->YPos + robot_radius){
				SetActiveRobot(count);
				return true;
		}
	}

	return false;
}

void CRobot::Zoom(float zoom_factor)
{
	mZoom = mZoom * zoom_factor;
	for (UINT count = 0; count < mVectRobot.size(); count++){
		ROBOT *robot = &mVectRobot.at(count);
		SetSize(robot->Size * zoom_factor);
		SetPosition(CPoint((int)robot->XPos * zoom_factor + 0.5, (int)robot->YPos * zoom_factor + 0.5));
		SetSensorRange(mSensorRange[MIN_VAL] * zoom_factor, mSensorRange[MAX_VAL] * zoom_factor);
	}
}

bool CRobot::SimpleCollisionDetection(ROBOT *robot, CDC *pDC)
{
	float step = M_PI / 10;
	float half_pi = M_PI / 2; 
	float dist = robot->Size / 2 + 5;
	CPoint pos = CPoint(robot->XPos, robot->YPos);

	// Head as normal
	if(robot->LWheelSpeed >= 0 && robot->RWheelSpeed >= 0){
		float count = -half_pi;
		while(count <= half_pi){
			CPoint pt = Rotate(pos, dist, robot->Theta + count);
			if (IsObstacleDetected(pDC, pt))
				return true;
			count = count + step;
		}
	}
	
	// Tail as normal
	else if (robot->LWheelSpeed <= 0 && robot->RWheelSpeed <= 0){
		float count = half_pi;
		while(count <= 3 * half_pi){
			CPoint pt = Rotate(pos, dist, robot->Theta + count);
			if (IsObstacleDetected(pDC, pt))
				return true;
			count = count + step;
		}
	}

	return false;
}

float CRobot::GetRadiationCone()
{
	return 2 * mRadiationCone;
}

float CRobot::GetSensorRange(int index)
{
	return mSensorRange[index] / mZoom;
}

void CRobot::GetNoiseProperties(float &mean, float &dev)
{
	mean = mMeanNoise;
	dev = mMeanNoise;
}

float CRobot::GetSpeed(int index)
{
	if (index = RIGHT)
		return mRobot->RWheelSpeed / mZoom;
	else if (index = LEFT)
		return mRobot->LWheelSpeed / mZoom;
	else
		return 0;
}

COLORREF CRobot::GetRobotColor(UINT index)
{
	return mVectRobot.at(index).RobotColor;
}

float CRobot::GetSensorValue(int index)
{
	return mRobot->SensorValue[index] / mZoom;
}