Wall avoidance for AIWheeledVehicle

#1 Luckily, for what you need to do, you don't need to perform any rotations. You can extract the values you need directly from the transform matrix. Here's a quick rundown:

Each column of the matrix corresponds to the right (column 0), forward (column 1), and up (column 2) vectors for the object. The last column is position. (If you look at the getPosition call, it just grabs the fourth column of the transform matrix and returns it to you). So you should be able to do the following (pseudocode because I'm lazy):

ForwardFeeler = getPosition() + getTransform().getColumn(1); // 1 is forward vector, you may want to scale
RightFeeler = getPosition() + getTransform().getColumn(0); // see comment above about scaling
LeftFeeler = getPosition() + getTransform().getColumn(0) * -1; // see comment above about scaling

That's it! Hope that helps.

#2 Thanks Brian,
I'll give it a try. Apparently it's much more efficient way to find feelers.

Anyway I like to learn if my code is correct.

I used this MatrixF ctor, but I could not exactly understand one thing.
p is a point not a axis how It can rotate it around a point in 3D space.

Does it carry the World Space's orientation to point p and apply Euler transform using world space's orientation
at point p?

in mMatrix.h
/// Create a matrix to rotate about p by e.
/// @see set
MatrixF( const EulerF &e, const Point3F& p);

#3 Feeler. Feeler. Feeler.

Please. :)

#4 Concepts > spelling

#5 I corrected spelling :)

I'm curious to know if I am able to understand the concept of transformations and Euler angles right.

Thanks.

#6 Hi there,

It's me again. I wrote a piece of code to help my wheeled vehicle bot to avoid walls.
Unfurtunately It just dumps into the walls. Me and the my little wheeled bot need some help to avoid walls and calculate
front, left and right feelers.

As the bot speeds up, it prints the following debug info, but left and right feelers I calculated have always same position, I have no idea why?


AIWheeledVehicle::avoidWalls
start = [-212.490997, 797.969971, 0.000000]

front = [-212.490997, 797.969971, 0.000000]

right = [-212.490997, 797.969971, 0.000000]

left = [-212.490997, 797.969971, 0.000000]

-----------------------------------------
AIWheeledVehicle::avoidWalls
start = [-212.489838, 797.976746, 0.027370]

front = [-212.374863, 798.831482, 2.587275]

right = [-212.489838, 797.976746, 0.027370]

left = [-212.489838, 797.976746, 0.027370]

-----------------------------------------
AIWheeledVehicle::avoidWalls
start = [-212.484528, 797.993835, 0.092139]

front = [-212.044830, 799.618530, 4.758163]

right = [-212.484528, 797.993835, 0.092139]

left = [-212.484528, 797.993835, 0.092139]

-----------------------------------------
AIWheeledVehicle::avoidWalls
start = [-212.469681, 798.027954, 0.200115]

front = [-211.591492, 800.503906, 7.424790]

right = [-212.469681, 798.027954, 0.200115]

left = [-212.469681, 798.027954, 0.200115]

-----------------------------------------
AIWheeledVehicle::avoidWalls
start = [-212.446915, 798.074158, 0.350553]

front = [-211.146118, 801.262451, 10.039993]

right = [-212.446915, 798.074158, 0.350553]

left = [-212.446915, 798.074158, 0.350553]




To calculate right feeler i used

//
// Create 3 feelers to find potential collisions ahead
//
Point3F frontFeeler = start + getVelocity() * TWO_SECONDS;

// Matrix to rotate front feeler about y axis by M_PI_F to obtain right feeler
MatrixF matRotateRight( EulerF( 0, 0, M_PI_F / 2.0f), getPosition());
matRotateRight.mul(Point4F(frontFeeler.x, frontFeeler.y, frontFeeler.z, 1));
Point3F rightFeeler = matRotateRight.getPosition();



but to calculate the left feeler I used

// Matrix to rotate front feeler about y axis by M_PI_F to obtain left feeler
MatrixF matRotateLeft( EulerF( 0, 0, -M_PI_F / 2.0f), getPosition());
matRotateLeft.mul(Point4F(frontFeeler.x, frontFeeler.y, frontFeeler.z, 1));
Point3F leftFeeler= matRotateLeft.getPosition();



As you see, I used different Euler angles to find rotation matrix. But the result is turned out to be same?

Any help is really appreciated.

Cheers

--tunca

#7

F32 AIWheeledVehicle::avoidWalls()
{
	F32 TWO_SECONDS = 2.0;
	F32 steering = 0.0;
	F32 safeDistance = (getVelocity() * TWO_SECONDS).magnitudeSafe();
	F32 maxSteeringAngle;

	VehicleData *vd= (VehicleData*) getDataBlock();
	maxSteeringAngle = vd->maxSteeringAngle;

	Point3F start = getPosition();
	//
	// Create 3 feelers to find potential collisions ahead
	//
	Point3F frontFeeler = start + getVelocity() * TWO_SECONDS;

	// Matrix to rotate front feeler about y axis by M_PI_F to obtain right feeler
	MatrixF matRotateRight( EulerF( 0, 0, M_PI_F / 2.0f), getPosition());
	matRotateRight.mul(Point4F(frontFeeler.x, frontFeeler.y, frontFeeler.z, 1));
	Point3F rightFeeler = matRotateRight.getPosition();

	// Matrix to rotate front feeler about y axis by M_PI_F to obtain left feeler
	MatrixF matRotateLeft( EulerF( 0, 0, -M_PI_F / 2.0f), getPosition());
	matRotateLeft.mul(Point4F(frontFeeler.x, frontFeeler.y, frontFeeler.z, 1));
	Point3F leftFeeler= matRotateLeft.getPosition();

	// Debug
	Con::printf("AIWheeledVehicle::avoidWalls");
	Con::printf(" start = [%f, %f, %f] \n", start.x, start.y, start.z);
	Con::printf(" front = [%f, %f, %f] \n", frontFeeler.x, frontFeeler.y, frontFeeler.z);
	Con::printf(" right = [%f, %f, %f] \n", rightFeeler.x, rightFeeler.y, rightFeeler.z);
	Con::printf(" left  = [%f, %f, %f] \n", leftFeeler.x, leftFeeler.y, leftFeeler.z);
	Con::printf("-----------------------------------------");

	// Find closest intersection with wall (static shape) line if any
	bool intersectionFound = false;
	F32 closestDis;
	RayInfo closestRay;
	Point3F closestFeeler;
	FeelerState closestFeelerState;

	RayInfo rayInfo_1;
	if (getContainer()->collideBox( start, frontFeeler, StaticShapeObjectType , &rayInfo_1 ))
	{
		intersectionFound = true;
		closestRay = rayInfo_1;
		closestFeeler = frontFeeler;
		closestFeelerState = FrontFeeler;
	}

	RayInfo rayInfo_2;
	if (getContainer()->collideBox( start, rightFeeler, StaticShapeObjectType , &rayInfo_2 ))
	{
		intersectionFound = true;
		if (rayInfo_2.distance < closestRay.distance)
		{
			closestRay = rayInfo_2;
			closestFeeler = rightFeeler;
			closestFeelerState = RightFeeler;
		}		
	}

	RayInfo rayInfo_3;
	if (getContainer()->collideBox( start, leftFeeler, StaticShapeObjectType , &rayInfo_3 ))
	{
		intersectionFound = true;
		if (rayInfo_3.distance < closestRay.distance)
		{
			closestRay = rayInfo_3;
			closestFeeler = leftFeeler;
			closestFeelerState = LeftFeeler;
		}		
	}

	if (!intersectionFound)
	{
		return 0.0;
	}
	else
	{
		switch (closestFeelerState){
			case FrontFeeler:
			case LeftFeeler:
				steering = maxSteeringAngle / (closestRay.distance / safeDistance);
				break;
			case RightFeeler:
				steering = -maxSteeringAngle / (closestRay.distance / safeDistance);
		}
		
	}

	Con::printf("AIWheeledVehicle::avoidWalls Return steering = [%f]", steering);
	return steering;
	
}

#8 Juan,

I'm guessing your code to createFeelers() should look a little more like this. It's similar to what I was doing in my resource.

void AIWheeledVehicle::createFeelers()
{
    F32 FOV = M_PI_F / 3;
    F32 FEELER_STEP = FOV / NUM_FEELERS;
    Point3F startPos = getPosition() + Point3F(0.0, 0.0, 2.0);
    for(U32 i=0; i< NUM_FEELERS; i++)
    {
        Point3F feeler;
        if(i < NUM_FEELERS / 2)
        {
            feeler = /*mLastValidVelocity*/getVelocity() * (i+1);
        }
        else
        {
            feeler = /*mLastValidVelocity*/getVelocity() * (NUM_FEELERS - i + 1);
        }

        EulerF rot;
        MatrixF mat = getTransform();
        rot = extractEuler(mat);
        MatrixF transform(rot);

        F32 rotation = (-FOV/2) + FEELER_STEP * i; 
        Point3F offset(0.0,rotation,0.0);
        transform.mulV(offset, &feeler);
        mFeelers[i] = startPos + feeler;
    }
}

And be careful of calculating things in the renderImage() thread that should be calculated in the in the AIPlayer thread. It can cause jitters because renderImage is asynchonous to the other thread.

In particular, startPos in

Point3F startPos = getPosition() + Point3F(0.0, 0.0, 2.0);

should be stored in the mDataBlock when you do createFeelers and then loaded from there when in renderImage().

#9 Well, it seems a little better. :)

I couldn't actually test it for you though. I'll add your feelers to my robots and debug it some. My bots could benefit from not bumping into things. And the code I suggested has a couple of flaws that I see already.

Did you want two feelers on each side spread 90 degrees apart? Looks like your code was to generate N feelers at FOV/N degrees apart, spread horizontally.

#10 Juan, I did a brief test with the following code. It isn't exactly what you described but it is very close. It produces a spread of feelers in front of the vehicle from the vehicle center, a 180 degree sweep.

#define NUM_FEELERS 8

   MatrixF mat = getTransform();
   MatrixF transform(extractEuler(mat));

   MatrixF wt = this->getWorldTransform(); 

   F32 FOV = M_PI;
   F32 FEELER_STEP = FOV / NUM_FEELERS;

   Point3F velocity = getVelocity();
   F32 speed = velocity.x*velocity.x+velocity.y*velocity.y+velocity.z+velocity.z;
   speed = sqrt( (speed<1.0)?1.0:speed);  // minimum speed (length) will be 1.0 m/sec
   vehiclePosition = getPosition();
   for(U32 i=0; i<= NUM_FEELERS; i++)
   {
      Point3F feeler,temp;
      F32 angle = FEELER_STEP * i;
      temp.x = speed * cos(angle);
      temp.y = speed * sin(angle);
      temp.z = .5;
      transform.mulV(temp, &feeler);
      mDataBlock->mFeelers[i] = feeler + offset;
//   wt.mulP(mFeelers[i]);
   }

Maybe you can modify this to suit your needs. At least it illustrates how to position and rotate the corrdinates. Good luck.

Edit: Put some checks around the sqrt() to produce a minimum length (and avoid 1.#IND issues).

Use the //wt.mulP(mFeelers[i]); only if you are using mFeelers result for world calculations rather than rendering; otherwise it conflicts with the world transform setup in the renderImage routine. You'll have to modify to suit your application.

I added an offset to the feeler to move them towards the front of the vehicle.

#11 I just made an edit. See above....

#12 I was using an offset based on the distance from the center of my object to the front axle. But any Point3F like (0.0,0.5,0.0) would probably do fine. That moves the feeler array forward by .5 meters.

Actually I was thinkng of making the offset based on velocity also and wrapping the feelers all the way around the vehicle. That way when the vehicle isn't moving the feelers are centered around the vehicle (or bike).

Or how about making the feelers extended forward by speed and a fixed distance behind. that way a rider could sense when a bike is about to collide from behind and adjust have his forced steering a little to the right or left. Just to make the ride a little more exciting.

Edit: Oh, I think there is still something wrong. I just tried actually driving around and see that the feelers are biased to one side of the other when steering. Rats!

#13 Ok, but I think the feelers are swinging around slightly out of line with the vehicle heading for some reason. But if you are using 2*PI, it doesn't matter as long as you not using the order of feelers somehow in the raycasting operation (which I haven't implemented).

Sound like things are getting better.

#14 Yes, I think your bicycle application is really excellent and interesting from a control point of view. There are many things about what your cyclists need to do that are the same AI that my balancing robots do. Keep up the good work and post pictures.