Rotating coordinates around an axis

Answer 1

Where you do your rotation in rotateByMatrix, you compute the new pos[0], but then feed that into the next line for computing the new pos[1]. So the pos[0] you're using to compute the new pos[1] is not the input, but the output. Store the result in a temp var and return that.

Coord<float> tmp;
tmp[0] = (cosf(zrot) * pos[0] - (sinf(zrot) * pos[1]));
tmp[1] = (sinf(zrot) * pos[0] + cosf(zrot) * pos[1]);   
return tmp;

Also, pass the pos into the function as a const reference.

const Coord<float> &pos

Plus you should compute the sin and cos values once, store them in temporaries and reuse them.