I am working on a simulation that requires me to have several objects moving around in 3D space (text output of their current position on the grid and heading is fine, I do not need graphics), and I am having some trouble getting objects to move along their relative headings.

I have a basic understanding of vectors and matrices. I am using a vector to represent their position, and I am also using Euler Angles. I can translate one of my entities with a matrix along whatever axis, and I can alter their heading.

For example, if I have an entity at (order is XYZ) 1, 1, 1, with a heading of 0, I can apply a translation matrix to get them to talk to 1, 1, 2 fine. However, if I change their heading to 270, they still walk to 1, 1, 3, instead of 2, 1, 2 as I desire.

I have a feeling that my problem lies in not translating my matrix from world space to object space, but I am not sure how to go about that. How can I do this?

Addition: I am using 3D vectors to represent their current position and their heading (using the three euler angles).

For now, all I want to do is have an entity walk in a square, reporting their current position at each step. So, assuming it starts at 10, 10, 10 I want it to walk as follows:

10,10,10 -> 10, 10, 15
10, 10, 15 -> 5, 10, 15
5, 10, 15 -> 5, 10, 10
5, 10, 10 -> 10, 10, 10

My 1 Z unit translation matrix is as follows:

[1 0 0 0]
[0 1 0 0]
[0 0 1 1]
[0 0 0 1]

My rotation matrix is as follows:

[0 0 1 0]
[0 1 0 0]
[-1 0 0 0]
[0 0 0 1]