// get_euler returns a vector containing the Euler angles in the format // (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last // (following the convention they are commonly defined in the literature). // // The current implementation uses XYZ convention (Z is the first rotation), // so euler.z is the angle of the (first) rotation around Z axis and so on, // // And thus, assuming the matrix is a rotation matrix, this function returns // the angles in the decomposition R = X(a1).Y(a2).Z(a3) where Z(a) rotates // around the z-axis by a and so on. Vector3 Basis::get_euler() const { // Euler angles in XYZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // // rot = cy*cz -cy*sz sy // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy Vector3 euler; ERR_FAIL_COND_V(is_rotation() == false, euler); euler.y = Math::asin(elements[0][2]); if ( euler.y < Math_PI*0.5) { if ( euler.y > -Math_PI*0.5) { euler.x = Math::atan2(-elements[1][2],elements[2][2]); euler.z = Math::atan2(-elements[0][1],elements[0][0]); } else { real_t r = Math::atan2(elements[1][0],elements[1][1]); euler.z = 0.0; euler.x = euler.z - r; } } else { real_t r = Math::atan2(elements[0][1],elements[1][1]); euler.z = 0; euler.x = r - euler.z; } return euler; }
int main(int argc, const char *argv[]){ //char s1[]="hello", s2[]="llohe"; if (is_rotation(argv[1], argv[2])) puts("is a rotation"); else puts("not a rotation"); return 0; }
int main(int argc, char** argv) { assert(is_rotation("abc", "abc")); assert(is_rotation("abc", string("abc"))); assert(!is_rotation(u8"aaa", u8"aa")); assert(is_rotation(u"abc", u"bca")); assert(is_rotation(L"abc", wstring(L"cab"))); assert(is_rotation(U"waterbottle", U"erbottlewat")); assert(!is_rotation(R"(waterbottle)", R"(rbottlewat)")); assert(!is_rotation(uR"(waterbottle)", uR"(Erbottlewat)")); cout << "All tests passed!\n"; }
// get_euler_xyz returns a vector containing the Euler angles in the format // (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last // (following the convention they are commonly defined in the literature). // // The current implementation uses XYZ convention (Z is the first rotation), // so euler.z is the angle of the (first) rotation around Z axis and so on, // // And thus, assuming the matrix is a rotation matrix, this function returns // the angles in the decomposition R = X(a1).Y(a2).Z(a3) where Z(a) rotates // around the z-axis by a and so on. Vector3 Basis::get_euler_xyz() const { // Euler angles in XYZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // // rot = cy*cz -cy*sz sy // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy Vector3 euler; #ifdef MATH_CHECKS ERR_FAIL_COND_V(!is_rotation(), euler); #endif real_t sy = elements[0][2]; if (sy < 1.0) { if (sy > -1.0) { // is this a pure Y rotation? if (elements[1][0] == 0.0 && elements[0][1] == 0.0 && elements[1][2] == 0 && elements[2][1] == 0 && elements[1][1] == 1) { // return the simplest form (human friendlier in editor and scripts) euler.x = 0; euler.y = atan2(elements[0][2], elements[0][0]); euler.z = 0; } else { euler.x = Math::atan2(-elements[1][2], elements[2][2]); euler.y = Math::asin(sy); euler.z = Math::atan2(-elements[0][1], elements[0][0]); } } else { euler.x = -Math::atan2(elements[0][1], elements[1][1]); euler.y = -Math_PI / 2.0; euler.z = 0.0; } } else { euler.x = Math::atan2(elements[0][1], elements[1][1]); euler.y = Math_PI / 2.0; euler.z = 0.0; } return euler; }
Quat Basis::get_quat() const { #ifdef MATH_CHECKS if (!is_rotation()) { ERR_EXPLAIN("Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead."); ERR_FAIL_V(Quat()); } #endif /* Allow getting a quaternion from an unnormalized transform */ Basis m = *this; real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; real_t temp[4]; if (trace > 0.0) { real_t s = Math::sqrt(trace + 1.0); temp[3] = (s * 0.5); s = 0.5 / s; temp[0] = ((m.elements[2][1] - m.elements[1][2]) * s); temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s); temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s); } else { int i = m.elements[0][0] < m.elements[1][1] ? (m.elements[1][1] < m.elements[2][2] ? 2 : 1) : (m.elements[0][0] < m.elements[2][2] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0); temp[i] = s * 0.5; s = 0.5 / s; temp[3] = (m.elements[k][j] - m.elements[j][k]) * s; temp[j] = (m.elements[j][i] + m.elements[i][j]) * s; temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; } return Quat(temp[0], temp[1], temp[2], temp[3]); }
static void vpp_get_min_max_variant(struct decon_win_config *config, u32 *max_src, u32 *min_src_w, u32 *min_src_h, u32 *max_dst, u32 *min_dst_w, u32 *min_dst_h) { if (is_rotation(config)) { if (is_yuv(config)) { *max_src = 2560; *max_dst = 4096; *min_src_w = 32; *min_src_h = 64; *min_dst_w = 16; *min_dst_h = 8; } else { *max_src = 2560; *max_dst = 4096; *min_src_w = 16; *min_src_h = 32; *min_dst_w = 16; *min_dst_h = 8; } } else { if (is_yuv(config)) { *max_src = 4096; *max_dst = 4096; *min_src_w = 64; *min_src_h = 32; *min_dst_w = 16; *min_dst_h = 8; } else { *max_src = 4096; *max_dst = 4096; *min_src_w = 32; *min_src_h = 16; *min_dst_w = 16; *min_dst_h = 8; } } }
static void vpp_get_align_variant(struct decon_win_config *config, u32 *offs, u32 *src_f, u32 *src_cr, u32 *dst_cr) { if (is_rotation(config)) { if (is_yuv(config)) { *offs = *src_f = 4; *src_cr = 2; *dst_cr = 1; } else { *offs = *src_f = 2; *src_cr = *dst_cr = 1; } } else { if (is_yuv(config)) { *offs = *src_f = 2; *src_cr = 2; *dst_cr = 1; } else { *offs = *src_f = 1; *src_cr = *dst_cr = 1; } } }
Basis::operator Quat() const { ERR_FAIL_COND_V(is_rotation() == false, Quat()); real_t trace = elements[0][0] + elements[1][1] + elements[2][2]; real_t temp[4]; if (trace > 0.0) { real_t s = Math::sqrt(trace + 1.0); temp[3]=(s * 0.5); s = 0.5 / s; temp[0]=((elements[2][1] - elements[1][2]) * s); temp[1]=((elements[0][2] - elements[2][0]) * s); temp[2]=((elements[1][0] - elements[0][1]) * s); } else { int i = elements[0][0] < elements[1][1] ? (elements[1][1] < elements[2][2] ? 2 : 1) : (elements[0][0] < elements[2][2] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; real_t s = Math::sqrt(elements[i][i] - elements[j][j] - elements[k][k] + 1.0); temp[i] = s * 0.5; s = 0.5 / s; temp[3] = (elements[k][j] - elements[j][k]) * s; temp[j] = (elements[j][i] + elements[i][j]) * s; temp[k] = (elements[k][i] + elements[i][k]) * s; } return Quat(temp[0],temp[1],temp[2],temp[3]); }
void Basis::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const { ERR_FAIL_COND(is_rotation() == false); double angle,x,y,z; // variables for result double epsilon = 0.01; // margin to allow for rounding errors double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees if ( (Math::abs(elements[1][0]-elements[0][1])< epsilon) && (Math::abs(elements[2][0]-elements[0][2])< epsilon) && (Math::abs(elements[2][1]-elements[1][2])< epsilon)) { // singularity found // first check for identity matrix which must have +1 for all terms // in leading diagonaland zero in other terms if ((Math::abs(elements[1][0]+elements[0][1]) < epsilon2) && (Math::abs(elements[2][0]+elements[0][2]) < epsilon2) && (Math::abs(elements[2][1]+elements[1][2]) < epsilon2) && (Math::abs(elements[0][0]+elements[1][1]+elements[2][2]-3) < epsilon2)) { // this singularity is identity matrix so angle = 0 r_axis=Vector3(0,1,0); r_angle=0; return; } // otherwise this singularity is angle = 180 angle = Math_PI; double xx = (elements[0][0]+1)/2; double yy = (elements[1][1]+1)/2; double zz = (elements[2][2]+1)/2; double xy = (elements[1][0]+elements[0][1])/4; double xz = (elements[2][0]+elements[0][2])/4; double yz = (elements[2][1]+elements[1][2])/4; if ((xx > yy) && (xx > zz)) { // elements[0][0] is the largest diagonal term if (xx< epsilon) { x = 0; y = 0.7071; z = 0.7071; } else { x = Math::sqrt(xx); y = xy/x; z = xz/x; } } else if (yy > zz) { // elements[1][1] is the largest diagonal term if (yy< epsilon) { x = 0.7071; y = 0; z = 0.7071; } else { y = Math::sqrt(yy); x = xy/y; z = yz/y; } } else { // elements[2][2] is the largest diagonal term so base result on this if (zz< epsilon) { x = 0.7071; y = 0.7071; z = 0; } else { z = Math::sqrt(zz); x = xz/z; y = yz/z; } } r_axis=Vector3(x,y,z); r_angle=angle; return; } // as we have reached here there are no singularities so we can handle normally double s = Math::sqrt((elements[1][2] - elements[2][1])*(elements[1][2] - elements[2][1]) +(elements[2][0] - elements[0][2])*(elements[2][0] - elements[0][2]) +(elements[0][1] - elements[1][0])*(elements[0][1] - elements[1][0])); // s=|axis||sin(angle)|, used to normalise angle = Math::acos(( elements[0][0] + elements[1][1] + elements[2][2] - 1)/2); if (angle < 0) s = -s; x = (elements[2][1] - elements[1][2])/s; y = (elements[0][2] - elements[2][0])/s; z = (elements[1][0] - elements[0][1])/s; r_axis=Vector3(x,y,z); r_angle=angle; }