Rotating vectors in space and high precision in C++

Viewed 1698

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*pi like @molbdnilo suggests. Because I have to chain translations and rotations to test if there's a collision.
4 Answers
Related