This is my function to compute 3D rotation in C++ defined by an angle in radiant around axis.
Vector rotate(const Vector& axis, const Vector& input, const double angle) {
double norm = 1/axis.norm();
if(norm != 1)
axis *= norm;
double cos = std::cos(angle);
double mcos = 1 - cos;
double sin = std::sin(angle);
double r1[3];
double r2[3];
double r3[3];
double t_x, t_ym t_z;
r1[0] = cos + std::pow(axis.x, 2)*mcos;
r1[1] = axis.x*axis.y*mcos - axis.z * sin;
r1[2] = axis.x*axis.z*mcos - axis.y * sin;
r2[0] = axis.x*axis.y*mcos + axis.z*sin;
r2[1] = cos + std::pow(axis.y, 2)*mcos;
r2[2] = axis.x*axis.z*mcos - axis.x * sin;
r3[0] = axis.x*axis.z*mcos - axis.y * sin;
r3[1] = axis.z*axis.y*mcos - axis.x * sin;
r3[2] = cos - std::pow(axis.z, 2) * mcos;
return Vector(t_x, t_y, t_z);
}
The thing is if you try yourself to rotate the vector a n times pi/4 where n multiple of 4 (so you do complete revolution around the axis by doing four quarter of rotations) the error will propagate pretty fast.
Example (where err = input-output):
input: (1.265, 3.398, 3.333)
rotation axis: (2.33, 0.568, 2.689)
n: 8 (so two completes revolutions)
output: (1.301967, 1.533389, 4.138940)
error: (0.038697, -0.864611, 0.805940)
n: 400 (so 100 completes revolutions)
error: (472..., 166..., 673...)
What can I do ?
Constraints:
- Rotations are not predictable so not possible to do something like
angle = pi/4 *n % 2*pilike @molbdnilo suggests. Because I have to chain translations and rotations to test if there's a collision.