rotation-combined.cpp

#include <float.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#define HALF_SQRT_2 0.70710678118654757f

enum Rotation {
    ROTATION_NONE                = 0,
    ROTATION_YAW_45              = 1,
    ROTATION_YAW_90              = 2,
    ROTATION_YAW_135             = 3,
    ROTATION_YAW_180             = 4,
    ROTATION_YAW_225             = 5,
    ROTATION_YAW_270             = 6,
    ROTATION_YAW_315             = 7,
    ROTATION_ROLL_180            = 8,
    ROTATION_ROLL_180_YAW_45     = 9,
    ROTATION_ROLL_180_YAW_90     = 10,
    ROTATION_ROLL_180_YAW_135    = 11,
    ROTATION_PITCH_180           = 12,
    ROTATION_ROLL_180_YAW_225    = 13,
    ROTATION_ROLL_180_YAW_270    = 14,
    ROTATION_ROLL_180_YAW_315    = 15,
    ROTATION_ROLL_90             = 16,
    ROTATION_ROLL_90_YAW_45      = 17,
    ROTATION_ROLL_90_YAW_90      = 18,
    ROTATION_ROLL_90_YAW_135     = 19,
    ROTATION_ROLL_270            = 20,
    ROTATION_ROLL_270_YAW_45     = 21,
    ROTATION_ROLL_270_YAW_90     = 22,
    ROTATION_ROLL_270_YAW_135    = 23,
    ROTATION_PITCH_90            = 24,
    ROTATION_PITCH_270           = 25,
    ROTATION_PITCH_180_YAW_90    = 26,
    ROTATION_PITCH_180_YAW_270   = 27,
    ROTATION_ROLL_90_PITCH_90    = 28,
    ROTATION_ROLL_180_PITCH_90   = 29,
    ROTATION_ROLL_270_PITCH_90   = 30,
    ROTATION_ROLL_90_PITCH_180   = 31,
    ROTATION_ROLL_270_PITCH_180  = 32,
    ROTATION_ROLL_90_PITCH_270   = 33,
    ROTATION_ROLL_180_PITCH_270  = 34,
    ROTATION_ROLL_270_PITCH_270  = 35,
    ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
    ROTATION_ROLL_90_YAW_270     = 37,
    ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
    ROTATION_MAX,
    _ROTATION_NOT_FOUND
};

const char *rotation_str[] = {
    [ROTATION_NONE                ] = "ROTATION_NONE",
    [ROTATION_YAW_45              ] = "ROTATION_YAW_45",
    [ROTATION_YAW_90              ] = "ROTATION_YAW_90",
    [ROTATION_YAW_135             ] = "ROTATION_YAW_135",
    [ROTATION_YAW_180             ] = "ROTATION_YAW_180",
    [ROTATION_YAW_225             ] = "ROTATION_YAW_225",
    [ROTATION_YAW_270             ] = "ROTATION_YAW_270",
    [ROTATION_YAW_315             ] = "ROTATION_YAW_315",
    [ROTATION_ROLL_180            ] = "ROTATION_ROLL_180",
    [ROTATION_ROLL_180_YAW_45     ] = "ROTATION_ROLL_180_YAW_45",
    [ROTATION_ROLL_180_YAW_90     ] = "ROTATION_ROLL_180_YAW_90",
    [ROTATION_ROLL_180_YAW_135    ] = "ROTATION_ROLL_180_YAW_135",
    [ROTATION_PITCH_180           ] = "ROTATION_PITCH_180",
    [ROTATION_ROLL_180_YAW_225    ] = "ROTATION_ROLL_180_YAW_225",
    [ROTATION_ROLL_180_YAW_270    ] = "ROTATION_ROLL_180_YAW_270",
    [ROTATION_ROLL_180_YAW_315    ] = "ROTATION_ROLL_180_YAW_315",
    [ROTATION_ROLL_90             ] = "ROTATION_ROLL_90",
    [ROTATION_ROLL_90_YAW_45      ] = "ROTATION_ROLL_90_YAW_45",
    [ROTATION_ROLL_90_YAW_90      ] = "ROTATION_ROLL_90_YAW_90",
    [ROTATION_ROLL_90_YAW_135     ] = "ROTATION_ROLL_90_YAW_135",
    [ROTATION_ROLL_270            ] = "ROTATION_ROLL_270",
    [ROTATION_ROLL_270_YAW_45     ] = "ROTATION_ROLL_270_YAW_45",
    [ROTATION_ROLL_270_YAW_90     ] = "ROTATION_ROLL_270_YAW_90",
    [ROTATION_ROLL_270_YAW_135    ] = "ROTATION_ROLL_270_YAW_135",
    [ROTATION_PITCH_90            ] = "ROTATION_PITCH_90",
    [ROTATION_PITCH_270           ] = "ROTATION_PITCH_270",
    [ROTATION_PITCH_180_YAW_90    ] = "ROTATION_PITCH_180_YAW_90",
    [ROTATION_PITCH_180_YAW_270   ] = "ROTATION_PITCH_180_YAW_270",
    [ROTATION_ROLL_90_PITCH_90    ] = "ROTATION_ROLL_90_PITCH_90",
    [ROTATION_ROLL_180_PITCH_90   ] = "ROTATION_ROLL_180_PITCH_90",
    [ROTATION_ROLL_270_PITCH_90   ] = "ROTATION_ROLL_270_PITCH_90",
    [ROTATION_ROLL_90_PITCH_180   ] = "ROTATION_ROLL_90_PITCH_180",
    [ROTATION_ROLL_270_PITCH_180  ] = "ROTATION_ROLL_270_PITCH_180",
    [ROTATION_ROLL_90_PITCH_270   ] = "ROTATION_ROLL_90_PITCH_270",
    [ROTATION_ROLL_180_PITCH_270  ] = "ROTATION_ROLL_180_PITCH_270",
    [ROTATION_ROLL_270_PITCH_270  ] = "ROTATION_ROLL_270_PITCH_270",
    [ROTATION_ROLL_90_PITCH_180_YAW_90 ] = "ROTATION_ROLL_90_PITCH_180_YAW_90",
    [ROTATION_ROLL_90_YAW_270     ] = "ROTATION_ROLL_90_YAW_270",
    [ROTATION_ROLL_90_PITCH_68_YAW_293 ] = "ROTATION_ROLL_90_PITCH_68_YAW_293",
};

class Vector3 {
public:
	float x, y, z;

	Vector3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
	void rotate(enum Rotation rotation);
	bool is_equal(const Vector3 &v) {
		return fabsf(v.x - x) <= FLT_EPSILON &&
		       fabsf(v.y - y) <= FLT_EPSILON &&
		       fabsf(v.z - z) < FLT_EPSILON;
	}
};

void Vector3::rotate(enum Rotation rotation)
{
    int tmp;
    switch (rotation) {
    case ROTATION_NONE:
    case ROTATION_MAX:
        return;
    case ROTATION_YAW_45: {
        tmp = HALF_SQRT_2*(float)(x - y);
        y   = HALF_SQRT_2*(float)(x + y);
        x = tmp;
        return;
    }
    case ROTATION_YAW_90: {
        tmp = x; x = -y; y = tmp;
        return;
    }
    case ROTATION_YAW_135: {
        tmp = -HALF_SQRT_2*(float)(x + y);
        y   =  HALF_SQRT_2*(float)(x - y);
        x = tmp;
        return;
    }
    case ROTATION_YAW_180:
        x = -x; y = -y;
        return;
    case ROTATION_YAW_225: {
        tmp = HALF_SQRT_2*(float)(y - x);
        y   = -HALF_SQRT_2*(float)(x + y);
        x = tmp;
        return;
    }
    case ROTATION_YAW_270: {
        tmp = x; x = y; y = -tmp;
        return;
    }
    case ROTATION_YAW_315: {
        tmp = HALF_SQRT_2*(float)(x + y);
        y   = HALF_SQRT_2*(float)(y - x);
        x = tmp;
        return;
    }
    case ROTATION_ROLL_180: {
        y = -y; z = -z;
        return;
    }
    case ROTATION_ROLL_180_YAW_45: {
        tmp = HALF_SQRT_2*(float)(x + y);
        y   = HALF_SQRT_2*(float)(x - y);
        x = tmp; z = -z;
        return;
    }
    case ROTATION_ROLL_180_YAW_90: {
        tmp = x; x = y; y = tmp; z = -z;
        return;
    }
    case ROTATION_ROLL_180_YAW_135: {
        tmp = HALF_SQRT_2*(float)(y - x);
        y   = HALF_SQRT_2*(float)(y + x);
        x = tmp; z = -z;
        return;
    }
    case ROTATION_PITCH_180: {
        x = -x; z = -z;
        return;
    }
    case ROTATION_ROLL_180_YAW_225: {
        tmp = -HALF_SQRT_2*(float)(x + y);
        y   =  HALF_SQRT_2*(float)(y - x);
        x = tmp; z = -z;
        return;
    }
    case ROTATION_ROLL_180_YAW_270: {
        tmp = x; x = -y; y = -tmp; z = -z;
        return;
    }
    case ROTATION_ROLL_180_YAW_315: {
        tmp =  HALF_SQRT_2*(float)(x - y);
        y   = -HALF_SQRT_2*(float)(x + y);
        x = tmp; z = -z;
        return;
    }
    case ROTATION_ROLL_90: {
        tmp = z; z = y; y = -tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_45: {
        tmp = z; z = y; y = -tmp;
        tmp = HALF_SQRT_2*(float)(x - y);
        y   = HALF_SQRT_2*(float)(x + y);
        x = tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_90: {
        tmp = z; z = y; y = -tmp;
        tmp = x; x = -y; y = tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_135: {
        tmp = z; z = y; y = -tmp;
        tmp = -HALF_SQRT_2*(float)(x + y);
        y   =  HALF_SQRT_2*(float)(x - y);
        x = tmp;
        return;
    }
    case ROTATION_ROLL_270: {
        tmp = z; z = -y; y = tmp;
        return;
    }
    case ROTATION_ROLL_270_YAW_45: {
        tmp = z; z = -y; y = tmp;
        tmp = HALF_SQRT_2*(float)(x - y);
        y   = HALF_SQRT_2*(float)(x + y);
        x = tmp;
        return;
    }
    case ROTATION_ROLL_270_YAW_90: {
        tmp = z; z = -y; y = tmp;
        tmp = x; x = -y; y = tmp;
        return;
    }
    case ROTATION_ROLL_270_YAW_135: {
        tmp = z; z = -y; y = tmp;
        tmp = -HALF_SQRT_2*(float)(x + y);
        y   =  HALF_SQRT_2*(float)(x - y);
        x = tmp;
        return;
    }
    case ROTATION_PITCH_90: {
        tmp = z; z = -x; x = tmp;
        return;
    }
    case ROTATION_PITCH_270: {
        tmp = z; z = x; x = -tmp;
        return;
    }
    case ROTATION_PITCH_180_YAW_90: {
        z = -z;
        tmp = -x; x = -y; y = tmp;
        return;
    }
    case ROTATION_PITCH_180_YAW_270: {
        x = -x; z = -z;
        tmp = x; x = y; y = -tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_90: {
        tmp = z; z = y; y = -tmp;
        tmp = z; z = -x; x = tmp;
        return;
    }
    case ROTATION_ROLL_180_PITCH_90: {
        y = -y; z = -z;
        tmp = z; z = -x; x = tmp;
        return;
    }
    case ROTATION_ROLL_270_PITCH_90: {
        tmp = z; z = -y; y = tmp;
        tmp = z; z = -x; x = tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_180: {
        tmp = z; z = y; y = -tmp;
        x = -x; z = -z;
        return;
    }
    case ROTATION_ROLL_270_PITCH_180: {
        tmp = z; z = -y; y = tmp;
        x = -x; z = -z;
        return;
    }
    case ROTATION_ROLL_90_PITCH_270: {
        tmp = z; z = y; y = -tmp;
        tmp = z; z = x; x = -tmp;
        return;
    }
    case ROTATION_ROLL_180_PITCH_270: {
        y = -y; z = -z;
        tmp = z; z = x; x = -tmp;
        return;
    }
    case ROTATION_ROLL_270_PITCH_270: {
        tmp = z; z = -y; y = tmp;
        tmp = z; z = x; x = -tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_180_YAW_90: {
        tmp = z; z = y; y = -tmp;
        x = -x; z = -z;
        tmp = x; x = -y; y = tmp;
        return;
    }
    case ROTATION_ROLL_90_YAW_270: {
        tmp = z; z = y; y = -tmp;
        tmp = x; x = y; y = -tmp;
        return;
    }
    case ROTATION_ROLL_90_PITCH_68_YAW_293: {
        float tmpx = x;
        float tmpy = y;
        float tmpz = z;
        x =  0.143039f * tmpx +  0.368776f * tmpy + -0.918446f * tmpz;
        y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz;
        z = -0.932324f * tmpx +  0.361625f * tmpy +  0.000000f * tmpz;
        return;
    }
    }

}

enum Rotation find_rotation(const char *s)
{
	for (unsigned int i = 0; i < ROTATION_MAX; i++) {
		if (strcmp(rotation_str[i], s) == 0)
			return (enum Rotation)i;
	}

	return _ROTATION_NOT_FOUND;
}

int main(int argc, const char *argv[])
{
	Vector3 v1{1, 2, 3};

	if (argc < 3)
		return EXIT_FAILURE;

	enum Rotation r1 = find_rotation(argv[1]);
	if (r1 == _ROTATION_NOT_FOUND)
		return EXIT_FAILURE;

	enum Rotation r2 = find_rotation(argv[2]);
	if (r2 == _ROTATION_NOT_FOUND)
		return EXIT_FAILURE;

	v1.rotate(r1);
	v1.rotate(r2);

	for (unsigned int i = 0; i < ROTATION_MAX; i++) {
		Vector3 v2{1, 2, 3};
		v2.rotate((enum Rotation)i);

		if (v2.is_equal(v1)) {
			printf("%s\n", rotation_str[i]);
			return 0;
		}
	}

	return 0;
}