Matlab matrix translation and rotation multiple times

Question

I have a map of individual trees from a forest stored as x,y points in a matrix. I call it fixedPositions. It's cartesian and (0,0) is the origin.

Given a velocity and a heading, i.e. .5 m/s and 60 degrees (2 o'clock equivalent on a watch), how do I rotate the x,y points, so that the new origin is centered at (.5cos(60),.5sin(60)) and 60 degrees is now at the top of the screen?

Then if I were to give you another heading and speed, i.e. 0 degrees and 2m/s, it should calculate it from the last point, not the original fixedPositions origin.

I've wasted my day trying to figure this out. I wish I took matrix algebra but I'm at a loss.

I tried doing cos(30) and even those wouldn't compute correctly, which after an hour I realize were in radians.

Answer 1

You just first translate the coordinates (-0.5cos(60),-0.5sin(60)) to take the origin to your target point.

Then rotate by multiplying the coordinates by a rotation matrix.

Of course, most programming languages use radians as angle units, so that instead of 60 you must enter 60 * PI / 180

Answer 2

I'd try the following: In your object, you already have a property heading. Now you add another property, currentPosition (an maybe rename them to heading_robot and currentPos_robot). heading as well as currentPosition should always be relative to the original coordinate system

Then you add a new method, updatePosition that takes (newHeading, distance) as input. This method will update both heading and currentPosition, by first adding the angle in newHeading to the angle in heading, after which you update currentPosition by adding [distance*cos(heading),distance*sin(heading)] (check for signs of sin/cos here!) to the old value of currentPosition.

Finally, to get the view of the landscape (i.e. apparentPositions), you run bsxfun(@minus,fixedPositions,currentPosition) to move the origin to where the robot is at this moment, and then you multiply with the 2D rotation matrix using the angle stored in heading.