C++ documentation C documentation

Support for two dimensional (2D) vectors.

A 2D vector just has an X and a Y value and no Z value - ie no 'height'. For land vehicles, or water surface vessels, this is normally enough as the height is out of your control. The height will always be 'sea level' or whatever the ground height is at X,Y. Tanks can't fly !!

A 2D vector is the distance in X and Y between two points. For example: if you plot the point (10,12) on a piece of graph paper then it is 10 units to the right of the origin and 12 units above the origin. The two points are the origin (0,0) and the point (10,12). So a Point is the 2D vector from the origin to the point - ie draw a line from the origin to the point and that is the vector.

Vectors can also be used to store a heading. In this case the first point is the current location of the vehicle and the second point is the location that the vehicle will have reached after a given time interval. For example: a vector of (1,0) indicates that the vehicle is heading to the right and its x position will increase by 1 every time interval. However a vector of (3,0) indicates that the vehicle is still heading to the right but it is now travelling 3 times faster.

So a vehicle could use two vectors: one to hold its point position from the origin, and a second to hold the current heading.

Since a vector is a line between two points then it has a finite length. Vectors can be 'normalised' which means that the direction of the line stays the same but the length is always equal to one. Consider the above examples of a vehicle heading of (1,0) and one of (3,0). They are both heading in the same direction but at different speeds. The length of the first heading is SQRT(1x1 + 0x0) = 1 and the length of the second is SQRT(3x3 + 0x0) = 3. We can normalise these headings by dividing the X,Y values by the length ie (1,0)=>(1/1,0/1) = (1,0) and the second is (3,0)=>(3/3,0/3) = (1,0). Their normalised values are the same (1,0). This means that you can store a heading and a speed where the heading is a normalised vector and the speed is a scaling factor.

How can I use this in practice?

Well lets assume you have a GPS and the previous position is (10,8) and the next position is (13,8) then the heading is the difference between the two ie new position - old position = (13,8) - (10,8) = (3,0). So the speed (length of 3,0) is 3 and the normalised heading is (1,0).

Vectors also have some other useful functions. The most obvious being that you can very easily/quickly calculate the angle between two vectors. Lets assume our robot is at RobotX,RobotY and is heading in direction (1,0) and that there is another object located at OtherX,OtherY. We can create a vector to the other object relative to the robot which will be (OtherX-RobotX, OtherY-RobotY). We now have two vectors both of which have the robot as origin: ie the robots heading and a line from the robot to other thing. We can now use the supplied function to calculate the angle between the two vectors. If this angle is 0° then the object is 'dead-ahead'; but if it is less than -90° or greater than 90° then the other object is 'behind' the robot.

Of course the other object may be another robot with its own heading and speed. By using vectors we can also find out if, and when, the two robots are going to collide if they continue at their current heading and speed. This allows your robot to start taking evasive action now.

When you get into the mind set of having multiple robots - where each broadcasts its current position, heading and speed to the other robots then it opens all sort of possibilities. For example: how about some quadro-copters that fly in a flock like birds - ie they all follow the leader but as they twist and turn to avoid obstacles then the leader changes. Or a fixed leader robot that other robots follow.

Such 'behaviour' routines are well documented and may well get into WebbotLib at some stage.

In summary: vectors are very simple to use and can achieve complex tasks

A C++ implementation is available via the Vector2D class.

Function

- MAKE_VECTOR2D - Construct a new vector with the given x,y values.
- vector2d_GetX - Returns the X value of the vector.
- vector2d_GetY - Returns the Y value of the vector.
- vector2d_SetX - Set the X value of an exiting vector.
- vector2d_SetY - Set the Y value of an exiting vector.
- vector2d_Set - Overwrite the vector with new values.
- vector2d_Length - Get the length of the vector.
- vector2d_LengthSquared - Return the length squared of the vector.
- vector2d_Normalise - Normalise the vector so that it has a length of 1.0
- vector2d_Add - Add two vectors ie dst = dst + src
- vector2d_Subtract - Subtract two vectors. dst = dst - src;
- vector2d_Copy - Set a vector to be a copy of another vector. dst = src.
- vector2d_Scale - Multiply both the X and Y values of the vector by the given scale factor.
- vector2d_DotProduct - Return the dot product of the two vectors 'v1' and 'v2'.
- vector2d_AngleRadians - Return the angle, in radians, between two vectors.

Construct a new vector with the given x,y values.

For example to create a vector with the values (10,5) then:-

VECTOR2D myVector = MAKE_VECTOR2D(10,5);

Returns the X value of the vector.

Example:

VECTOR2D myVector = MAKE_VECTOR2D(10,5); // Create vector

double x = vector2d_GetX(&myVector); // Get x value ie 10 in this case

Returns the Y value of the vector.

Example:

VECTOR2D myVector = MAKE_VECTOR2D(10,5); // Create vector

double y = vector2d_GetY(&myVector); // Get y value ie 5 in this case

Set the X value of an exiting vector.

Example:

VECTOR2D myVector = MAKE_VECTOR2D(10,5);

vector2d_SetX(&myVector, 12); // Change to (12,5)

Set the Y value of an exiting vector.

Example:

VECTOR2D myVector = MAKE_VECTOR2D(10,5);

vector2d_SetY(&myVector, 6); // Change to (10,6)

Overwrite the vector with new values.

Example:

VECTOR2D myVector; // Create vector with undefined values

vector2d_Set(&myVector, 10, 5);// Vector is now (10,5)

Get the length of the vector.

Mathematically this is SQRT(x*x + y*y)

Return the length squared of the vector.

Mathematically this is: x*x + y*y

Sometimes you may only be comparing relative lengths of different vectors - in which case you can use this function instead of the real length and thus avoid the overhead of the additional square root function to convert to the real length.

Normalise the vector so that it has a length of 1.0

The first parameter specifies the vector to store the result, and the second parameter specifies the vector to be normalised. Both parameters can refer to the same vector if you are happy to loose the details of the original vector.

Add two vectors ie dst = dst + src

Subtract two vectors. dst = dst - src;

Set a vector to be a copy of another vector. dst = src.

Example:

VECTOR2D vec1 = MAKE_VECTOR2D(10,5); // Init vec1

VECTOR2D vec2; // Create vec2 with unkown values

vector2d_Copy(&vec2, &vec1); // Set vec2 to vec1 ie (10,5)

Multiply both the X and Y values of the vector by the given scale factor.

Example:

Assuming we know that

VECTOR2D robotPosition = MAKE_VECTOR2D(10,5); // current location

VECTOR2D robotHeading = MAKE_VECTOR2D(1,0); // The normalised heading

double speed = 7; // The speed of the robot in, say, cm/second

We can estimate what the robot position will be in 5 seconds using:

VECTOR2D newPosition; // Create a new vector

VECTOR2D movement; // Create a new vector

vector2d_Copy(&newPosition, &robotPosition); // new = current

vector2d_Copy(&movement, &robotHeading); // Get heading

vector2d_Scale(&movement, speed * 5); // Heading = Heading * speed * 5 seconds

vector2d_Add(&newPosition, &movement); // Estimated position after 5 seconds

Return the dot product of the two vectors 'v1' and 'v2'.

The dot product is: Length(v1) * Length(v2) * cos(angle between them)

If 'v1' and 'v2' have been normalised then their lengths are 1.0 and so the above simplifies to: cos(angle between them)

Return the angle, in radians, between two vectors.

The returned value will be in the range 0 to PI.

To convert the radians value into degrees then multiply it by (180.0 / PI)