Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- union {
- struct
- {
- float _11, _12, _13, _14;
- float _21, _22, _23, _24;
- float _31, _32, _33, _34;
- float _41, _42, _43, _44;
- };
- float m[4][4];
- float m2[16];
- };
- inline void GetRotation(float& Yaw, float& Pitch, float& Roll) const
- {
- if (_11 == 1.0f)
- {
- Yaw = atan2f(_13, _34);
- Pitch = 0;
- Roll = 0;
- }else if (_11 == -1.0f)
- {
- Yaw = atan2f(_13, _34);
- Pitch = 0;
- Roll = 0;
- }else
- {
- Yaw = atan2(-_31,_11);
- Pitch = asin(_21);
- Roll = atan2(-_23,_22);
- }
- }
- ┌ cosθ -sinθ 0 ┐
- Rz = │ sinθ cosθ 0 │
- └ 0 0 1 ┘
- ┌ 1 0 0 ┐
- Rx = │ 0 cosθ -sinθ │
- └ 0 sinθ cosθ ┘
- ┌ cosθ 0 sinθ ┐
- Ry = │ 0 1 0 │
- └ -sinθ 0 cosθ ┘
- ┌ CyCz -CySz Sy ┐
- RxRyRz = │ SxSyCz + CxSz -SxSySz + CxCz -SxCy │
- └ -CxSyCz + SxSz CxSySz + SxCz CxCy ┘
- x = atan2(-M[1][2], M[2][2])
- cosY = sqrt(1 - M[0][2])
- y = atan2(M[0][2], cosY)
- ┌ Cy 0 Sy ┐
- RxRy = │ SxSy Cx -SxCy │
- └ -CxSy Sx CxCy ┘
- M = RxRy * Rz
- inverse(RxRy) * M = Rz
- ┌ Cy SxSy -CxSy ┐┌M00 M01 M02┐ ┌ cosZ -sinZ 0 ┐
- │ 0 Cx Sx ││M10 M11 M12│ = │ sinZ cosZ 0 │
- └ Sy -SxCy CxCy ┘└M20 M21 M22┘ └ 0 0 1 ┘
- sinZ = cosX * M[1][0] + sinX * M[2][0]
- cosZ = coxX * M[1][1] + sinX * M[2][1]
- z = atan2(sinZ, cosZ)
- #include <iostream>
- #include <cmath>
- class Vec4 {
- public:
- Vec4(float x, float y, float z, float w) :
- x(x), y(y), z(z), w(w) {}
- float dot(const Vec4& other) const {
- return x * other.x +
- y * other.y +
- z * other.z +
- w * other.w;
- };
- float x, y, z, w;
- };
- class Mat4x4 {
- public:
- Mat4x4() {}
- Mat4x4(float v00, float v01, float v02, float v03,
- float v10, float v11, float v12, float v13,
- float v20, float v21, float v22, float v23,
- float v30, float v31, float v32, float v33) {
- values[0] = v00;
- values[1] = v01;
- values[2] = v02;
- values[3] = v03;
- values[4] = v10;
- values[5] = v11;
- values[6] = v12;
- values[7] = v13;
- values[8] = v20;
- values[9] = v21;
- values[10] = v22;
- values[11] = v23;
- values[12] = v30;
- values[13] = v31;
- values[14] = v32;
- values[15] = v33;
- }
- Vec4 row(const int row) const {
- return Vec4(
- values[row*4],
- values[row*4+1],
- values[row*4+2],
- values[row*4+3]
- );
- }
- Vec4 column(const int column) const {
- return Vec4(
- values[column],
- values[column + 4],
- values[column + 8],
- values[column + 12]
- );
- }
- Mat4x4 multiply(const Mat4x4& other) const {
- Mat4x4 result;
- for (int row = 0; row < 4; ++row) {
- for (int column = 0; column < 4; ++column) {
- result.values[row*4+column] = this->row(row).dot(other.column(column));
- }
- }
- return result;
- }
- void extractEulerAngleXYZ(float& rotXangle, float& rotYangle, float& rotZangle) const {
- rotXangle = atan2(-row(1).z, row(2).z);
- float cosYangle = sqrt(pow(row(0).x, 2) + pow(row(0).y, 2));
- rotYangle = atan2(row(0).z, cosYangle);
- float sinXangle = sin(rotXangle);
- float cosXangle = cos(rotXangle);
- rotZangle = atan2(cosXangle * row(1).x + sinXangle * row(2).x, cosXangle * row(1).y + sinXangle * row(2).y);
- }
- float values[16];
- };
- float toRadians(float degrees) {
- return degrees * (M_PI / 180);
- }
- float toDegrees(float radians) {
- return radians * (180 / M_PI);
- }
- int main() {
- float rotXangle = toRadians(15);
- float rotYangle = toRadians(30);
- float rotZangle = toRadians(60);
- Mat4x4 rotX(
- 1, 0, 0, 0,
- 0, cos(rotXangle), -sin(rotXangle), 0,
- 0, sin(rotXangle), cos(rotXangle), 0,
- 0, 0, 0, 1
- );
- Mat4x4 rotY(
- cos(rotYangle), 0, sin(rotYangle), 0,
- 0, 1, 0, 0,
- -sin(rotYangle), 0, cos(rotYangle), 0,
- 0, 0, 0, 1
- );
- Mat4x4 rotZ(
- cos(rotZangle), -sin(rotZangle), 0, 0,
- sin(rotZangle), cos(rotZangle), 0, 0,
- 0, 0, 1, 0,
- 0, 0, 0, 1
- );
- Mat4x4 concatenatedRotationMatrix =
- rotX.multiply(rotY.multiply(rotZ));
- float extractedXangle = 0, extractedYangle = 0, extractedZangle = 0;
- concatenatedRotationMatrix.extractEulerAngleXYZ(
- extractedXangle, extractedYangle, extractedZangle
- );
- std::cout << toDegrees(extractedXangle) << ' ' <<
- toDegrees(extractedYangle) << ' ' <<
- toDegrees(extractedZangle) << std::endl;
- return 0;
- }
Add Comment
Please, Sign In to add comment