Following another object that is rotating

Question

I have three objects: marshal, sergeant, soldier. All of them are of Human type:

class Human{
public: //just to make question less complicated,
//let's assume all members are public
   DirectX::XMFLOAT3 position; //pivot point
   DirectX::XMFLOAT3 rotation;

   Human * follow;
   std::vector<Human *> followers;
   DirectX::XMFLOAT3 distanceToFollow;

   ...
   void render(){
      ...
      DirectX::XMMATRIX objectWorldMatrix = DirectX::XMMatrixIdentity();
      DirectX::XMMATRIX rotationMatrix = 
         DirectX::XMMatrixRotationX(rotation.x) * 
         DirectX::XMMatrixRotationY(rotation.y) * 
         DirectX::XMMatrixRotationZ(rotation.z)
      );
      ...
      DirectX::XMMATRIX translationMatrix = DirectX::XMMatrixTranslation(position.x, position.y, position.z);
      objectWorldMatrix =  scaleMatrix * rotationMatrix * translationMatrix;
      ...
   }

   void follow(Human * h){
       follow = h;
       h.followers.push_back(this);
       distanceToFollow = h.position - position;
   }
};

The relation between them looks like that:

soldier.follow(sergeant);
sergeant.follow(marshal);

What I want now is to make soldier following sergeant and sergeant following the marshal. So when, for instance, marshal moves, everybody moves and keep their distances (as well as relative angles) to each other (the "visualization" is 2D for simplify, but I am working with 3D models):

For position changes, that will do:

void Human::setPosition(XMFFLOAT3 newPosition){
    position = newPosition;
    for(auto &follower : followers){
        follower.setPosition(newPosition - follower.distanceToFollow);
        //keep the distance, soldier!
    }
}

But I cannot figure out, how to make the same for rotation?.

When marshal rotates, sergeant will change both its position and rotation.

I am after few failures. During my last try, when I rotated marshal by [40, 0, 0], then [0, 30, 0], then [-40, 0, 0] and [0, -30, 0], sergeant ended in place different then before all those rotations (marshal backed to place as it should, with rotation [0, 0, 0] - the relative distance has been broken). Maybe it's because of order of calculations.

Anyway, I don't want to suggest the wrong way of solving the problem, so will just ask you: how to write the Human::setRotation(XMFFLOAT3 newRotation) method?

Note: There is no interpolations/animations for now (from one state to another), so I guess that quaternions won't be necessary.

---

My attempt

At first I did not want to post it. It's because I think it contains an mistakes and I do not want to suggest them to you (I though it easier to compare with right solution than to find the bug in the wrong one). But if you want to check my buggy code:

Human::setRotation(XMFFLOAT3 newRotation){
    XMFFLOAT3 oldRotation = rotation;
    rotation = newRotation;
    for(auto &follower : followers){
        follower.rotateAround(newRotation - oldRotation, rotationDiffrenceToFollowTarget, position);
        //keep the relative angle, soldier!
    }
}

Human::rotateAround(XMFFLOAT3 radians, XMFFLOAT3 origin){
    DirectX::XMMATRIX rotX = DirectX::XMMatrixRotationX(radians.x);
    DirectX::XMMATRIX rotY = DirectX::XMMatrixRotationY(radians.y);
    DirectX::XMMATRIX rotZ = DirectX::XMMatrixRotationZ(radians.z);

    DirectX::XMMATRIX worldM = rotX * rotY * rotZ;

    DirectX::XMFLOAT3 positionTemp = DirectX::XMFLOAT3(
        position.x - origin.x, position.y - origin.y, position.z - origin.z);
    DirectX::XMVECTOR pos = XMLoadFloat3(&positionTemp);
    pos = XMVector3Transform(pos, worldM);
    XMStoreFloat3(&positionTemp, pos);

    positionTemp.x = positionTemp.x + origin.x;
    positionTemp.y = positionTemp.y + origin.y;
    positionTemp.z = positionTemp.z + origin.z;

    setPosition(positionTemp); //so followers will get noticed
    rotation += radians;
}

And the rotationDiffrenceToFollowTarget is computed as:

Human::void follow(Human * h){
    follow = h;
    h.followers.push_back(this);
    distanceToFollow = h.position - position;
    rotationDiffrenceToFollowTarget = h.rotation - rotation;
}

Answer 1

You can't subtract a set of three rotation angles and expect sense because 3D rotations don't commute, and that pretty much wrecks your approach. Your three angles area not much use for anything - it's an all-in-one matrix you want to store.

The way to do this is to express each individual movement of the leader as a single matrix and apply exactly that matrix to the followers as well. For translations this matrix is just a translation, and for rotations it's a translation of the leader to the origin followed by a simple rotation that reflects the UI action that caused it, followed by the reverse translation. When I say "followed by" I mean "premultiplied by".

If that UI action is supposed to be character-centric, i.e. if turning left feels the same whichever way up you are, then you'll also have to unwind the current rotation before applying the change and re-apply it afterwards, but the translations are still outermost. Without this follow-the-leader feature that would boil down to simply post-multiplying the current matrix by the change, but for the follower, you'll apply the inverse of the leader's current matrix, then the change, then the old current matrix.

This doesn't work with your multi-stage following thing, so you'll have to express that as both followers following the one leader directly.

Another option is to find a matrix that transforms the leader's entire state onto the follower's and when the leader moves around you postmultiply his state by that matrix to get the follower's state. By "state" I mean position and orientation expressed as a single matrix. How do you find that matrix in the first place? If it's called M while L and F are the two states, then L.M=F so M=Inv(L).F

Is that specific enough? ;-)