float Raycast(b2Vec2 startPosition, b2Vec2 dir, float distance)
{
b2RayCastInput input;
b2Vec2 p1 = startPosition;
b2Vec2 p2 = p1 + distance * dir;
input.p1 = p1;
input.p2 = p2;
input.maxFraction = 1;
//check every fixture of every body to find closest
float closestFraction = 1; //start with end of line as p2
b2Vec2 intersectionNormal(0, 0);
for (b2Body* b = PHYSX->world->GetBodyList(); b; b = b->GetNext())
{
for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
{
if (f->GetFilterData().categoryBits == CATEGORY_STATIC && !f->IsSensor())
{
b2RayCastOutput output;
if (!f->RayCast(&output, input, 0))
continue;
if (output.fraction < closestFraction)
{
closestFraction = output.fraction;
intersectionNormal = output.normal;
}
}
}
}
// Add raycast to debug renderer
/*DebugLine l;
l.p1 = p1;
l.p2 = p1 + distance * closestFraction * dir;
RENDER->ddraw->debug_lines.push_back(l);
*/
return closestFraction;
}
void Render ()
{
LoadFPS (60);
glClearColor (250/255.0, 245/255.0, 245/255.0, 1);
srand (time (NULL) );
//at global scope
static float currentRayAngle = 0;
currentRayAngle += 6;
//calculate points of ray
b2Vec2 p1 ( b2Vec2 (agent->body->GetPosition ().x,
agent->body->GetPosition ().y) ); //center of scene
b2Vec2 p2 = p1 + rayLength * b2Vec2 (sinf (DEGTORAD * (currentRayAngle ) ),
cosf ( DEGTORAD * (currentRayAngle ) ) );
b2Vec2 p3 ( b2Vec2 (agent->body->GetPosition ().x,
agent->body->GetPosition ().y) );
b2Vec2 p4 = p3 + rayLength * b2Vec2 (sinf (DEGTORAD * (currentRayAngle + 90) ),
cosf ( DEGTORAD * (currentRayAngle + 90) ) );
b2Vec2 p5 ( b2Vec2 (agent->body->GetPosition ().x,
agent->body->GetPosition ().y) ); //center of scene
b2Vec2 p6 = p5 + rayLength * b2Vec2 (sinf (DEGTORAD * (currentRayAngle + 180) ),
cosf ( DEGTORAD * (currentRayAngle + 180) ) );
b2Vec2 p7 ( b2Vec2 (agent->body->GetPosition ().x,
agent->body->GetPosition ().y) );
b2Vec2 p8 = p7 + rayLength * b2Vec2 (sinf (DEGTORAD * (currentRayAngle + 270 ) ),
cosf ( DEGTORAD * (currentRayAngle + 270 ) ) );
b2Vec2 p9 = b2Vec2 (agent->body->GetPosition ().x ,
agent->body->GetPosition ().y - 4 ) ;
b2Vec2 p10 = b2Vec2 (agent->body->GetPosition ().x ,
agent->body->GetPosition ().y - 4 ) + rayLength / 2.0 * b2Vec2 (sinf ( (
agent->angle + 1.57) ),
cosf ( ( agent->angle + 1.57 ) ) );
b2Vec2 p11 = b2Vec2 (agent->body->GetPosition ().x ,
agent->body->GetPosition ().y + 4 ) ;
b2Vec2 p12 = b2Vec2 (agent->body->GetPosition ().x,
agent->body->GetPosition ().y + 4 ) + rayLength / 2.0 * b2Vec2 (sinf ( (
agent->angle + 1.57) ),
cosf ( ( agent->angle + 1.57 ) ) );
b2RayCastInput input;
input.p1 = p1;
input.p2 = p2;
input.maxFraction = rayLength / 50.0;
b2RayCastInput input2;
input2.p1 = p3;
input2.p2 = p4;
input2.maxFraction = rayLength / 50.0;
b2RayCastInput input3;
input3.p1 = p5;
input3.p2 = p6;
input3.maxFraction = rayLength / 50.0 ;
b2RayCastInput input4;
input4.p1 = p7;
input4.p2 = p8;
input4.maxFraction = rayLength / 50.0;
b2RayCastInput input5;
input5.p1 = p9;
input5.p2 = p10;
input5.maxFraction = rayLength / 50.0;
b2RayCastInput input6;
input6.p1 = p11;
input6.p2 = p12;
input6.maxFraction = rayLength / 50.0;
//check every fixture of every body to find closest
float closestFraction = rayLength / 50.0; //start with end of line as p2
float closestFraction2 = rayLength / 50.0 ;
float closestFraction3 = rayLength / 50.0;
float closestFraction4 = rayLength / 50.0;
float closestFraction5 = rayLength / 50.0;
float closestFraction6 = rayLength / 50.0;
static int timer = 0;
timer++;
b2Vec2 intersectionNormal (0, 0);
bool isObstacle[5] = {0} ;
static int freeAngle[5] = {0};
freeAngle[1] = agent->angle;
static int count = 0 ;
/*
* Raycasting is done here, there are 6 ray here
*/
for (b2Body * b = myWorld.GetBodyList (); b; b = b->GetNext () )
{
if (100 == (int) b->GetUserData () )
{ break; }
for (b2Fixture * f = b->GetFixtureList (); f; f = f->GetNext () )
{
b2RayCastOutput output;
if ( f->RayCast ( &output, input, 0 ) )
{
if ( output.fraction < closestFraction )
{
isObstacle[1] = 1;
count = 0 ;
closestFraction = output.fraction;
intersectionNormal = output.normal;
b2Vec2 intersectionPoint = p1 + closestFraction * (p2 - p1);
b2Vec2 diff1 = agent->body->GetPosition () - intersectionPoint;
int x = diff1.x / (800 / 80.0) ;
int y = diff1.y / (600 / 60.0) ;
carCollisionMap[20 - y][20 - x] = 1;
}
}
b2RayCastOutput output2;
if (f->RayCast (&output2, input2, 0) )
{
if (output2.fraction < closestFraction)
{
isObstacle[2] = 1;
closestFraction2 = output2.fraction;
b2Vec2 intersectionPoint2 = p3 + closestFraction2 * (p4 - p3);
b2Vec2 diff2 = agent->body->GetPosition () - intersectionPoint2;
int x = diff2.x / (800 / 80.0) ;
int y = diff2.y / (600 / 60.0) ;
carCollisionMap[20 - y][20 - x] = 1;
}
}
b2RayCastOutput output3;
if (f->RayCast (&output3, input3, 0) )
{
if (output3.fraction < closestFraction3)
{
isObstacle[3] = 1;
closestFraction3 = output3.fraction;
b2Vec2 intersectionPoint3 = p5 + closestFraction3 * (p6 - p5);
b2Vec2 diff2 = agent->body->GetPosition () - intersectionPoint3;
int x = diff2.x / (800 / 80.0) ;
int y = diff2.y / (600 / 60.0) ;
carCollisionMap[20 - y][20 - x] = 1;
}
}
b2RayCastOutput output4;
if (f->RayCast (&output4, input4, 0) )
{
if (output4.fraction < closestFraction4)
{
isObstacle[4] = 1;
closestFraction4 = output4.fraction;
b2Vec2 intersectionPoint4 = p7 + closestFraction4 * (p8 - p7);
b2Vec2 diff2 = agent->body->GetPosition () - intersectionPoint4;
int x = diff2.x / (800 / 80.0) ;
int y = diff2.y / (600 / 60.0) ;
carCollisionMap[20 - y][20 - x] = 1;
}
}
b2RayCastOutput output5;
if (f->RayCast (&output5, input5, 0) )
{
if (output5.fraction < closestFraction5)
{
closestFraction5 = output5.fraction;
b2Vec2 intersectionPoint5 = p9 + closestFraction5 * (p10 - p9);
b2Vec2 diff2 = agent->body->GetPosition () - intersectionPoint5;
int x = diff2.x / (800 / 80.0) ;
int y = diff2.y / (600 / 60.0) ;
carCollisionMap[20 - y][20 - x] = 1;
agent->frontObstacle = 1;
}
}
b2RayCastOutput output6;
if (f->RayCast (&output6, input6, 0) )
{
if (output6.fraction < closestFraction6)
{
closestFraction6 = output6.fraction;
b2Vec2 intersectionPoint6 = p11 + closestFraction6 * (p12 - p11);
b2Vec2 diff2 = agent->body->GetPosition () - intersectionPoint6;
int x = diff2.x / (800 / 80.0) ;
int y = diff2.y / (600 / 60.0) ;
carCollisionMap[20 - y][20 - x] = 1;
agent->frontObstacle = 1;
}
}
}
}
static float timetem= glutGet(GLUT_ELAPSED_TIME);
if(glutGet(GLUT_ELAPSED_TIME)-timetem > 1000)
for (int i = 1 ; i <= 4; ++i)
{
if (isObstacle[i] == false)
{
float tempAngle;
tempAngle = (int) (currentRayAngle + 90 * (i - 1) ) % 360;
if (tempAngle >= converToDegree (agent->angle) && tempAngle <= (int) (converToDegree (agent->angle) + 180) % 360)
{
if (agent->nearestAngle[RIGHT_SIDE] > tempAngle - converToDegree (agent->angle) )
{
agent->nearestAngle[RIGHT_SIDE] = tempAngle - converToDegree (agent->angle) ;
}
else
{ isObstacle[i] = true; }
}
else
{
if (agent->nearestAngle[LEFT_SIDE] > (360 - tempAngle) - converToDegree (agent->angle) )
{ agent->nearestAngle[LEFT_SIDE] = fabs ( (360 - tempAngle) - converToDegree (agent->angle) ); }
else
{ isObstacle[i] = true; }
}
}
timetem= glutGet(GLUT_ELAPSED_TIME);
}
// if no obstacle is infront of behicle and destination is set, then head for the destination
// this is done by changing vehicle angle to face the destination
b2Vec2 intersectionPoint = p1 + closestFraction * (p2 - p1);
b2Vec2 intersectionPoint2 = p3 + closestFraction2 * (p4 - p3);
b2Vec2 intersectionPoint3 = p5 + closestFraction3 * (p6 - p5);
b2Vec2 intersectionPoint4 = p7 + closestFraction4 * (p8 - p7);
b2Vec2 intersectionPoint5 = p9 + closestFraction4 * (p10 - p9);
b2Vec2 intersectionPoint6 = p11 + closestFraction4 * (p12 - p11);
float dist = (agent->body->GetPosition().x - intersectionPoint5.x) * (agent->body->GetPosition().x - intersectionPoint5.x)
+ (agent->body->GetPosition().y - intersectionPoint5.y) * (agent->body->GetPosition().x - intersectionPoint5.y);
//draw a line
glColor3f (255, 0, 0); //white
glBegin (GL_LINES);
glVertex2f ( p1.x, p1.y );
glVertex2f ( intersectionPoint.x, intersectionPoint.y );
glEnd ();
SetRenderColor (0, 0, 0, 255);
if (isObstacle[1] == false)
{ SetRenderColor (255, 0, 255, 255); }
//draw a point at the intersection point
glPointSize (10);
glBegin (GL_POINTS);
glVertex2f ( intersectionPoint.x, intersectionPoint.y );
glEnd ();
SetRenderColor (255, 34, 25, 255);
glBegin (GL_POINTS);
glVertex2f ( destination.x, destination.y );
glEnd ();
glBegin (GL_LINES);
glVertex2f ( p3.x, p3.y );
glVertex2f ( intersectionPoint2.x, intersectionPoint2.y );
glEnd ();
glBegin (GL_LINES);
glVertex2f ( p5.x, p5.y );
glVertex2f ( intersectionPoint3.x, intersectionPoint3.y );
glEnd ();
glBegin (GL_LINES);
glVertex2f ( p7.x, p7.y );
glVertex2f ( intersectionPoint4.x, intersectionPoint4.y );
glEnd ();
glBegin (GL_LINES);
glVertex2f ( p9.x, p9.y );
glVertex2f ( intersectionPoint5.x, intersectionPoint5.y );
glEnd ();
glBegin (GL_POINTS);
glVertex2f ( intersectionPoint5.x, intersectionPoint5.y );
glEnd ();
glBegin (GL_LINES);
glVertex2f ( p11.x, p11.y );
glVertex2f ( intersectionPoint6.x, intersectionPoint6.y );
glEnd ();
glBegin (GL_POINTS);
glVertex2f ( intersectionPoint6.x, intersectionPoint6.y );
glEnd ();
SetRenderColor (0, 0, 0, 255);
if (isObstacle[2] == false)
{ SetRenderColor (255, 0, 255, 255); }
glPointSize (10);
glBegin (GL_POINTS);
glVertex2f ( intersectionPoint2.x, intersectionPoint2.y );
glEnd ();
SetRenderColor (0, 0, 0, 255);
if (isObstacle[3] == false)
{ SetRenderColor (255, 0, 255, 255); }
glBegin (GL_POINTS);
glVertex2f ( intersectionPoint3.x, intersectionPoint3.y );
glEnd ();
SetRenderColor (0, 0, 0, 255);
if (isObstacle[4] == false)
{ SetRenderColor (255, 0, 255, 255); }
glBegin (GL_POINTS);
glVertex2f ( intersectionPoint4.x, intersectionPoint4.y );
glEnd ();
// glBegin (GL_LINES);
// glVertex2f ( agent->body->GetPosition ().x, agent->body->GetPosition ().y );
// glVertex2f ( agent->body->GetPosition ().x + sin (agent->angle + 1.57) * 100,
// agent->body->GetPosition ().y + cos (agent->angle + 1.57) * 100);
// glEnd ();
static float posx = 0;
posx = posx + .1;
// we are drawing collision array here
for (int i = 0; i < 40; ++i)
for (int j = 0; j < 40; ++j)
{
if (carCollisionMap[i][j] == 1)
{
glPointSize (5);
glBegin (GL_POINTS);
glVertex2f (agent->body->GetPosition ().x - (20 - j) * 10 ,
agent->body->GetPosition ().y - (20 - i) * 10 );
glEnd ();
}
}
turnToDestination();
turnToNearestAngle ();
agent->isCollision = 0;
// checking collision here, not so important now
for (int i = 1; i < 15; ++i)
{
float x = 20 + (i * 2) * sinf (agent->angle + 1.57) ;
float y = 20 + (i * 2) * cosf (agent->angle + 1.57);
if (x < 40 && y < 40)
if (carCollisionMap[ (int) y][ (int) x] == 1)
{
// printf ("obstacle found\n");
agent->isCollision = 1;
static float timerT = glutGet (GLUT_ELAPSED_TIME);
bool safe = 1;
safe = rand () % 2;
if (!agent->frontObstacle)
if (agent->nearestAngle[LEFT_SIDE] != 1000 || agent->nearestAngle[RIGHT_SIDE] != 1000)
{
// if (glutGet (GLUT_ELAPSED_TIME) - timerT > 500)
{
if (agent->nearestAngle[LEFT_SIDE] < agent->nearestAngle[RIGHT_SIDE])
{
printf ("Left side wins %f\n", agent->nearestAngle[LEFT_SIDE]);
agent->angle = agent->angle - .2;
agent->foundNearestAngle = true;
// agent->nearestAngle[RIGHT_SIDE] = 1000;
}
else
{
printf ("Right side wins %f\n", agent->nearestAngle[RIGHT_SIDE]);
agent->foundNearestAngle = true;
// agent->nearestAngle[LEFT_SIDE] = 1000;
agent->angle = agent->angle + .2;
}
timerT = glutGet (GLUT_ELAPSED_TIME);
}
}
// if (glutGet (GLUT_ELAPSED_TIME) - timerT > 1000)
// {
//
// if (safe == 1)
// for (int j = 0; j < 10; ++j)
// {
// float x = 20 + ( (j ) * sinf (agent->angle + 1.57 + .3) ) ;
// float y = 20 + ( (j ) * cosf (agent->angle + 1.57 + .3) ) ;
//
// if (carCollisionMap[ (int) y][ (int) x] == 1)
// {
// safe = 0;
// break;
// }
// }
// if (safe == 1)
// { agent->angle = agent->angle + .3; }
// {
// memset (carCollisionMap, 0, sizeof (carCollisionMap) );
// }
// if (safe == 0)
// {
// safe = 1;
// for (int j = 0; j < 10; ++j)
// {
// float x = 20 + ( (j ) * sinf (agent->angle + 1.57 - .3) ) ;
// float y = 20 + ( (j ) * cosf (agent->angle + 1.57 - .3) ) ;
// if (carCollisionMap[ (int) y][ (int) x] == 1)
// {
// safe = 0;
// break;
// }
// }
// if (safe == 1)
// {
// agent->angle = agent->angle - .3;
// memset (carCollisionMap, 0, sizeof (carCollisionMap) );
// }
// }
// timerT = glutGet (GLUT_ELAPSED_TIME);
// }
}
}
agent->moveForward ();
SetRenderColor (0, 0, 0, 255);
RenderBitmapText (60, 550, GLUT_BITMAP_9_BY_15, "ctrl + left click to add obstacle, alt + left click to add a destination");
sprintf (fps, "%f", GetFps () );
SetRenderColor (255, 0, 255, 255);
RenderBitmapText (10, 590, GLUT_BITMAP_9_BY_15, fps);
myWorld.DrawDebugData ();
}