diff --git a/glm/ext.hpp b/glm/ext.hpp index b0c4f72d..de8150f8 100644 --- a/glm/ext.hpp +++ b/glm/ext.hpp @@ -72,6 +72,7 @@ #include "./gtc/matrix_transform.hpp" #include "./gtc/noise.hpp" #include "./gtc/quaternion.hpp" +#include "./gtc/dual_quaternion.hpp" #include "./gtc/random.hpp" #include "./gtc/reciprocal.hpp" #include "./gtc/swizzle.hpp" diff --git a/glm/gtx/dual_quaternion.hpp b/glm/gtx/dual_quaternion.hpp new file mode 100644 index 00000000..c4d7b267 --- /dev/null +++ b/glm/gtx/dual_quaternion.hpp @@ -0,0 +1,242 @@ +/////////////////////////////////////////////////////////////////////////////////// +/// OpenGL Mathematics (glm.g-truc.net) +/// +/// Copyright (c) 2005 - 2013 G-Truc Creation (www.g-truc.net) +/// Permission is hereby granted, free of charge, to any person obtaining a copy +/// of this software and associated documentation files (the "Software"), to deal +/// in the Software without restriction, including without limitation the rights +/// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +/// copies of the Software, and to permit persons to whom the Software is +/// furnished to do so, subject to the following conditions: +/// +/// The above copyright notice and this permission notice shall be included in +/// all copies or substantial portions of the Software. +/// +/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +/// THE SOFTWARE. +/// +/// @ref gtx_dual_quaternion +/// @file glm/gtx/dual_quaternion.hpp +/// @date 2013-02-10 / 2013-02-20 +/// @author Maksim Vorobiev (msomeone@gmail.com) +/// +/// @see core (dependence) +/// @see gtc_half_float (dependence) +/// @see gtc_constants (dependence) +/// @see gtc_quaternion (dependence) +/// +/// @defgroup gtc_dual_quaternion GLM_GTX_dual_quaternion +/// @ingroup gtc +/// +/// @brief Defines a templated dual-quaternion type and several dual-quaternion operations. +/// +/// need to be included to use these functionalities. +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef GLM_GTX_dual_quaternion +#define GLM_GTX_dual_quaternion GLM_VERSION + +// Dependency: +#include "../glm.hpp" +#include "../gtc/half_float.hpp" +#include "../gtc/constants.hpp" +#include "../gtc/quaternion.hpp" + +#if(defined(GLM_MESSAGES) && !defined(glm_ext)) +# pragma message("GLM: GLM_GTX_dual_quaternion extension included") +#endif + +namespace glm{ +namespace detail +{ + template + struct tdualquat// : public genType + { + enum ctor{null}; + + typedef T value_type; + typedef glm::detail::tquat part_type; + typedef std::size_t size_type; + + public: + glm::detail::tquat real, dual; + + GLM_FUNC_DECL size_type length() const; + + // Constructors + tdualquat(); + explicit tdualquat(tquat const & real); + tdualquat(tquat const & real,tquat const & dual); + tdualquat(tquat const & orientation,tvec3 const& translation); + + ////////////////////////////////////////////////////////////// + // tdualquat conversions + explicit tdualquat(tmat2x4 const & holder_mat); + explicit tdualquat(tmat3x4 const & aug_mat); + + // Accesses + typename part_type & operator[](int i); + typename part_type const & operator[](int i) const; + + // Operators + tdualquat & operator*=(value_type const & s); + tdualquat & operator/=(value_type const & s); + }; + + template + detail::tquat operator- ( + detail::tquat const & q); + + template + detail::tdualquat operator+ ( + detail::tdualquat const & q, + detail::tdualquat const & p); + + template + detail::tdualquat operator* ( + detail::tdualquat const & q, + detail::tdualquat const & p); + + template + detail::tvec3 operator* ( + detail::tquat const & q, + detail::tvec3 const & v); + + template + detail::tvec3 operator* ( + detail::tvec3 const & v, + detail::tquat const & q); + + template + detail::tvec4 operator* ( + detail::tquat const & q, + detail::tvec4 const & v); + + template + detail::tvec4 operator* ( + detail::tvec4 const & v, + detail::tquat const & q); + + template + detail::tdualquat operator* ( + detail::tdualquat const & q, + typename detail::tdualquat::value_type const & s); + + template + detail::tdualquat operator* ( + typename detail::tdualquat::value_type const & s, + detail::tdualquat const & q); + + template + detail::tdualquat operator/ ( + detail::tdualquat const & q, + typename detail::tdualquat::value_type const & s); +} //namespace detail + + /// @addtogroup gtc_dual_quaternion + /// @{ + + /// Returns the normalized quaternion. + /// + /// @see gtc_dual_quaternion + template + detail::tdualquat normalize( + detail::tdualquat const & q); + + /// Returns the linear interpolation of two dual quaternion. + /// + /// @see gtc_dual_quaternion + template + detail::tdualquat lerp ( + detail::tdualquat const & x, + detail::tdualquat const & y, + typename detail::tdualquat::value_type const & a); + + /// Returns the q inverse. + /// + /// @see gtc_dual_quaternion + template + detail::tdualquat inverse( + detail::tdualquat const & q); + + /// Extracts a rotation part from dual-quaternion to a 3 * 3 matrix. + /// + /// @see gtc_dual_quaternion + template + detail::tmat3x3 mat3_cast( + detail::tdualquat const & x); + + /// Converts a quaternion to a 2 * 4 matrix. + /// + /// @see gtc_dual_quaternion + template + detail::tmat2x4 mat2x4_cast( + detail::tdualquat const & x); + + /// Converts a quaternion to a 3 * 4 matrix. + /// + /// @see gtc_dual_quaternion + template + detail::tmat3x4 mat3x4_cast( + detail::tdualquat const & x); + + /// Converts a 2 * 4 matrix (matrix which holds real and dual parts) to a quaternion. + /// + /// @see gtc_dual_quaternion + template + detail::tdualquat dualquat_cast( + detail::tmat2x4 const & x); + + /// Converts a 3 * 4 matrix (augmented matrix rotation + translation) to a quaternion. + /// + /// @see gtc_dual_quaternion + template + detail::tdualquat dualquat_cast( + detail::tmat3x4 const & x); + + /// Dual-quaternion of floating-point numbers. + /// + /// @see gtc_dual_quaternion + typedef detail::tdualquat dualquat; + + /// Dual-quaternion of half-precision floating-point numbers. + /// + /// @see gtc_dual_quaternion + typedef detail::tdualquat hdualquat; + + /// Dual-quaternion of single-precision floating-point numbers. + /// + /// @see gtc_dual_quaternion + typedef detail::tdualquat fdualquat; + + /// Dual-quaternion of double-precision floating-point numbers. + /// + /// @see gtc_dual_quaternion + typedef detail::tdualquat ddualquat; + + /// Dual-quaternion of low precision floating-point numbers. + /// + /// @see gtc_dual_quaternion + typedef detail::tdualquat lowp_dualquat; + + /// Dual-quaternion of medium precision floating-point numbers. + /// + /// @see gtc_dual_quaternion + typedef detail::tdualquat mediump_dualquat; + + /// Dual-quaternion of high precision floating-point numbers. + /// + /// @see gtc_dual_quaternion + typedef detail::tdualquat highp_dualquat; + + /// @} +} //namespace glm + +#include "dual_quaternion.inl" + +#endif//GLM_GTX_dual_quaternion diff --git a/glm/gtx/dual_quaternion.inl b/glm/gtx/dual_quaternion.inl new file mode 100644 index 00000000..65a76401 --- /dev/null +++ b/glm/gtx/dual_quaternion.inl @@ -0,0 +1,426 @@ +/////////////////////////////////////////////////////////////////////////////////// +/// OpenGL Mathematics (glm.g-truc.net) +/// +/// Copyright (c) 2005 - 2013 G-Truc Creation (www.g-truc.net) +/// Permission is hereby granted, free of charge, to any person obtaining a copy +/// of this software and associated documentation files (the "Software"), to deal +/// in the Software without restriction, including without limitation the rights +/// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +/// copies of the Software, and to permit persons to whom the Software is +/// furnished to do so, subject to the following conditions: +/// +/// The above copyright notice and this permission notice shall be included in +/// all copies or substantial portions of the Software. +/// +/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +/// THE SOFTWARE. +/// +/// @ref gtx_quaternion +/// @file glm/gtx/quaternion.inl +/// @date 2013-02-10 / 2013-02-13 +/// @author Maksim Vorobiev (msomeone@gmail.com) +/////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace glm{ +namespace detail +{ + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR typename tdualquat::size_type tdualquat::length() const + { + return 8; + } + + template + GLM_FUNC_QUALIFIER tdualquat::tdualquat() : + real(tquat()), + dual(tquat(tdualquat::value_type(0),tdualquat::value_type(0),tdualquat::value_type(0),tdualquat::value_type(0))) + {} + + template + GLM_FUNC_QUALIFIER tdualquat::tdualquat + ( + tquat const & r + ) : + real(r), + dual(tquat(tdualquat::value_type(0),tdualquat::value_type(0),tdualquat::value_type(0),tdualquat::value_type(0))) + {} + + template + GLM_FUNC_QUALIFIER tdualquat::tdualquat + ( + tquat const & r, + tquat const & d + ) : + real(r), + dual(d) + {} + + template + GLM_FUNC_QUALIFIER tdualquat::tdualquat + ( + tquat const & q, + tvec3 const& p + ) : + real(q), + dual(-0.5f*( p.x*q.x + p.y*q.y + p.z*q.z), + 0.5f*( p.x*q.w + p.y*q.z - p.z*q.y), + 0.5f*(-p.x*q.z + p.y*q.w + p.z*q.x), + 0.5f*( p.x*q.y - p.y*q.x + p.z*q.w)) + {} + + ////////////////////////////////////////////////////////////// + // tdualquat conversions + template + GLM_FUNC_QUALIFIER tdualquat::tdualquat + ( + tmat2x4 const & holder_mat + ) + { + *this = dualquat_cast<> + } + + template + GLM_FUNC_QUALIFIER tdualquat::tdualquat + ( + tmat3x4 const & m + ) + { + *this = dualquat_cast(m); + } + + ////////////////////////////////////////////////////////////// + // tdualquat accesses + + template + GLM_FUNC_QUALIFIER typename tdualquat::part_type & tdualquat::operator [] (int i) + { + return (&real)[i]; + } + + template + GLM_FUNC_QUALIFIER typename tdualquat::part_type const & tdualquat::operator [] (int i) const + { + return (&real)[i]; + } + + ////////////////////////////////////////////////////////////// + // tdualquat operators + + template + GLM_FUNC_QUALIFIER tdualquat & tdualquat::operator *= + ( + value_type const & s + ) + { + this->real *= s; + this->dual *= s; + return *this; + } + + template + GLM_FUNC_QUALIFIER tdualquat & tdualquat::operator /= + ( + value_type const & s + ) + { + this->real /= s; + this->dual /= s; + return *this; + } + + ////////////////////////////////////////////////////////////// + // tquat external operators + + template + GLM_FUNC_QUALIFIER detail::tdualquat operator- + ( + detail::tdualquat const & q + ) + { + return detail::tdualquat(-this->real,-this->dual); + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat operator+ + ( + detail::tdualquat const & q, + detail::tdualquat const & p + ) + { + return detail::tdualquat(q.real + p.real,q.dual + p.dual); + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat operator* + ( + detail::tdualquat const & p, + detail::tdualquat const & o + ) + { + return detail::tdualquat(p.real * o.real,p.real * o.dual + p.dual * o.real); + } + + // Transformation + template + GLM_FUNC_QUALIFIER detail::tvec3 operator* + ( + detail::tdualquat const & q, + detail::tvec3 const & v + ) + { + const detail::tvec3 real_v3(q.real.x,q.real.y,q.real.z); + const detail::tvec3 dual_v3(q.dual.x,q.dual.y,q.dual.z); + return (cross(real_v3, cross(real_v3,v) + v * q.real.w + dual_v3) + dual_v3 * q.real.w - real_v3 * q.dual.w) * detail::tdualquat::value_type(2) + v; + } + + template + GLM_FUNC_QUALIFIER detail::tvec3 operator* + ( + detail::tvec3 const & v, + detail::tdualquat const & q + ) + { + return inverse(q) * v; + } + + template + GLM_FUNC_QUALIFIER detail::tvec4 operator* + ( + detail::tdualquat const & q, + detail::tvec4 const & v + ) + { + return detail::tvec4(q * detail::tvec3(v), v.w); + } + + template + GLM_FUNC_QUALIFIER detail::tvec4 operator* ( + detail::tvec4 const & v, + detail::tdualquat const & q) + { + return inverse(q) * v; + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat operator* + ( + detail::tdualquat const & q, + typename detail::tdualquat::value_type const & s + ) + { + return detail::tdualquat(q.real * s, q.dual * s); + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat operator* + ( + typename detail::tdualquat::value_type const & s, + detail::tdualquat const & q + ) + { + return q * s; + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat operator/ + ( + detail::tdualquat const & q, + typename detail::tdualquat::value_type const & s + ) + { + return detail::tdualquat(q.real / s, q.dual / s); + } + + ////////////////////////////////////// + // Boolean operators + template + GLM_FUNC_QUALIFIER bool operator== + ( + detail::tdualquat const & q1, + detail::tdualquat const & q2 + ) + { + return (q1.real == q2.real) && (q1.dual == q2.dual); + } + + template + GLM_FUNC_QUALIFIER bool operator!= + ( + detail::tdualquat const & q1, + detail::tdualquat const & q2 + ) + { + return (q1.real != q2.dual) || (q1.real != q2.dual); + } +}//namespace detail + + //////////////////////////////////////////////////////// + template + GLM_FUNC_QUALIFIER detail::tdualquat normalize + ( + detail::tdualquat const & q + ) + { + return q / length(q.real); + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat lerp + ( + detail::tdualquat const & x, + detail::tdualquat const & y, + typename detail::tdualquat::value_type const & a + ) + { // Dual Quaternion Linear blend aka DLB: + // Lerp is only defined in [0, 1] + assert(a >= T(0)); + assert(a <= T(1)); + const detail::tdualquat::value_type k = dot(x.real,y.real) < detail::tdualquat::value_type(0) ? -a : a; + const detail::tdualquat::value_type one(1); + return detail::tdualquat(x * (one - a) + y * k); + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat inverse + ( + detail::tdualquat const & q + ) + { + const glm::detail::tquat real = conjugate(q.real); + const glm::detail::tquat dual = conjugate(q.dual); + return detail::tdualquat(real, dual + (real * (-2.0f * dot(real,dual)))); + } + + template + GLM_FUNC_QUALIFIER detail::tmat3x3 mat3_cast + ( + detail::tdualquat const & x + ) + { + } + + template + GLM_FUNC_QUALIFIER detail::tmat2x4 mat2x4_cast + ( + detail::tdualquat const & x + ) + { + return detail::tmat2x4( x[0].x, x[0].y, x[0].z, x[0].w, x[1].x, x[1].y, x[1].z, x[1].w ); + } + + template + GLM_FUNC_QUALIFIER detail::tmat3x4 mat3x4_cast + ( + detail::tdualquat const & x + ) + { + detail::tquat r = x.real / length2(x.real); + + const detail::tquat rr(r.w * x.real.w, r.x * x.real.x, r.y * x.real.y, r.z * x.real.z); + r *= detail::tdualquat::value_type(2); + + const detail::tdualquat::value_type xy = r.x*d.real.y; + const detail::tdualquat::value_type xz = r.x*d.real.z; + const detail::tdualquat::value_type yz = r.y*d.real.z; + const detail::tdualquat::value_type wx = r.w*d.real.x; + const detail::tdualquat::value_type wy = r.w*d.real.y; + const detail::tdualquat::value_type wz = r.w*d.real.z; + + const detail::tvec4 a( + rr.w + rr.x - rr.y - rr.z, + xy - wz, + xz + wy, + -(x.dual.w * r.x - x.dual.x * r.w + x.dual.y * r.z - x.dual.z * r.y) + ); + + const detail::tvec4 b( + xy + wz, + rr.w + rr.y - rr.x - rr.z, + yz - wx, + -(x.dual.w * r.y - x.dual.x * r.z - x.dual.y * r.w + x.dual.z * r.x) + ); + + const detail::tvec4 c( + xz - wy, + yz + wx, + rr.w + rr.z - rr.x - rr.y, + -(x.dual.w * r.z + x.dual.x * r.y - x.dual.y * r.x - x.dual.z * r.w) + ); + + return detail::tmat3x4(a,b,c); + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat dualquat_cast + ( + detail::tmat2x4 const & x + ) + { + return detail::tdualquat ( + detail::tquat ( x[0].w, x[0].x, x[0].y, x[0].z ), + detail::tquat ( x[1].w, x[1].x, x[1].y, x[1].z ) + ); + } + + template + GLM_FUNC_QUALIFIER detail::tdualquat dualquat_cast + ( + detail::tmat3x4 const & x + ) + { + detail::tquat real; + + const detail::tdualquat::value_type trace = x[0].x + x[1].y + x[2].z; + if(trace > detail::tdualquat::value_type(0)) + { + const detail::tdualquat::value_type r = sqrt(detail::tdualquat::value_type(1) + trace); + const detail::tdualquat::value_type invr = detail::tdualquat::value_type(0.5) / r; + real.w = detail::tdualquat::value_type(0.5) * r; + real.x = (x[2].y - x[1].z) * invr; + real.y = (x[0].z - x[2].x) * invr; + real.z = (x[1].x - x[0].y) * invr; + } + else if(x[0].x > x[1].y && x[0].x > x[2].z) + { + const detail::tdualquat::value_type r = sqrt(detail::tdualquat::value_type(1) + x[0].x - x[1].y - x[2].z); + const detail::tdualquat::value_type invr = detail::tdualquat::value_type(0.5) / r; + real.x = detail::tdualquat::value_type(0.5)*r; + real.y = (x[1].x + x[0].y) * invr; + real.z = (x[0].z + x[2].x) * invr; + real.w = (x[2].y - x[1].z) * invr; + } + else if(x[1].y > x[2].z) + { + const detail::tdualquat::value_type r = sqrt(detail::tdualquat::value_type(1) + x[1].y - x[0].x - x[2].z); + const detail::tdualquat::value_type invr = detail::tdualquat::value_type(0.5) / r; + x = (x[1].x + x[0].y) * invr; + y = detail::tdualquat::value_type(0.5) * r; + z = (x[2].y + x[1].z) * invr; + w = (x[0].z - x[2].x) * invr; + } + else + { + const detail::tdualquat::value_type r = sqrt(detail::tdualquat::value_type(1) + x[2].z - x[0].x - x[1].y); + const detail::tdualquat::value_type invr = detail::tdualquat::value_type(0.5) / r; + x = (x[0].z + x[2].x) * invr; + y = (x[2].y + x[1].z) * invr; + z = detail::tdualquat::value_type(0.5) * r; + w = (x[1].x - x[0].y) * invr; + } + + const detail::tquat dual; + dual.x = 0.5f*( x[0].w*real.w + x[1].w*real.z - x[2].w*real.y); + dual.y = 0.5f*(-x[0].w*real.z + x[1].w*real.w + x[2].w*real.x); + dual.z = 0.5f*( x[0].w*real.y - x[1].w*real.x + x[2].w*real.w); + dual.w = -0.5f*( x[0].w*real.x + x[1].w*real.y + x[2].w*real.z); + return detail::tdualquat(real,dual); + } + +}//namespace glm diff --git a/test/gtx/CMakeLists.txt b/test/gtx/CMakeLists.txt index 1beed4d3..e268abc4 100644 --- a/test/gtx/CMakeLists.txt +++ b/test/gtx/CMakeLists.txt @@ -5,6 +5,7 @@ glmCreateTestGTC(gtx_matrix_interpolation) glmCreateTestGTC(gtx_matrix_query) glmCreateTestGTC(gtx_multiple) glmCreateTestGTC(gtx_quaternion) +glmCreateTestGTC(gtx_dual_quaternion) glmCreateTestGTC(gtx_rotate_normalized_axis) glmCreateTestGTC(gtx_rotate_vector) glmCreateTestGTC(gtx_scalar_relational) diff --git a/test/gtx/gtx_dual_quaternion.cpp b/test/gtx/gtx_dual_quaternion.cpp new file mode 100644 index 00000000..ed716334 --- /dev/null +++ b/test/gtx/gtx_dual_quaternion.cpp @@ -0,0 +1,173 @@ +/////////////////////////////////////////////////////////////////////////////////////////////////// +// OpenGL Mathematics Copyright (c) 2005 - 2013 G-Truc Creation (www.g-truc.net) +/////////////////////////////////////////////////////////////////////////////////////////////////// +// Created : 2013-02-10 +// Updated : 2013-02-11 +// Licence : This source is under MIT licence +// File : test/gtc/gtc_dual_quaternion.cpp +/////////////////////////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include + +#include + +int myrand() +{ + static int holdrand = 1; + return (((holdrand = holdrand * 214013L + 2531011L) >> 16) & 0x7fff); +} + +float myfrand() // returns values from -1 to 1 inclusive +{ + return float(double(myrand()) / double( 0x7ffff )) * 2.0f - 1.0f; +} + +int test_dquat_type() +{ + glm::dvec3 vA; + glm::dquat dqA,dqB; + glm::ddualquat C(dqA,dqB); + glm::ddualquat B(dqA); + glm::ddualquat D(dqA,vA); + return 0; +} + +int test_scalars() { + float const Epsilon = 0.0001f; + + int Error(0); + + glm::quat src_q1 = glm::quat(1.0f,2.0f,3.0f,4.0f); + glm::quat src_q2 = glm::quat(5.0f,6.0f,7.0f,8.0f); + glm::dualquat src1(src_q1,src_q2); + + { + glm::dualquat dst1 = src1 * 2.0f; + glm::dualquat dst2 = 2.0f * src1; + glm::dualquat dst3 = src1; + dst3 *= 2.0f; + glm::dualquat dstCmp(src_q1 * 2.0f,src_q2 * 2.0f); + Error += glm::all(glm::epsilonEqual(dst1.real,dstCmp.real, Epsilon)) && glm::all(glm::epsilonEqual(dst1.dual,dstCmp.dual, Epsilon)) ? 0 : 1; + Error += glm::all(glm::epsilonEqual(dst2.real,dstCmp.real, Epsilon)) && glm::all(glm::epsilonEqual(dst2.dual,dstCmp.dual, Epsilon)) ? 0 : 1; + Error += glm::all(glm::epsilonEqual(dst3.real,dstCmp.real, Epsilon)) && glm::all(glm::epsilonEqual(dst3.dual,dstCmp.dual, Epsilon)) ? 0 : 1; + } + + { + glm::dualquat dst1 = src1 / 2.0f; + glm::dualquat dst2 = src1; + dst2 /= 2.0f; + glm::dualquat dstCmp(src_q1 / 2.0f,src_q2 / 2.0f); + Error += glm::all(glm::epsilonEqual(dst1.real,dstCmp.real, Epsilon)) && glm::all(glm::epsilonEqual(dst1.dual,dstCmp.dual, Epsilon)) ? 0 : 1; + Error += glm::all(glm::epsilonEqual(dst2.real,dstCmp.real, Epsilon)) && glm::all(glm::epsilonEqual(dst2.dual,dstCmp.dual, Epsilon)) ? 0 : 1; + } + return Error; +} + +int test_inverse() +{ + int Error(0); + + float const Epsilon = 0.0001f; + + glm::dualquat dqid; + glm::mat4x4 mid(1.0f); + + for (int j = 0; j < 100; ++j) { + glm::mat4x4 rot = glm::yawPitchRoll(myfrand() * 360.0f, myfrand() * 360.0f, myfrand() * 360.0f); + glm::vec3 vt = glm::vec3(myfrand() * 10.0f, myfrand() * 10.0f, myfrand() * 10.0f); + + glm::mat4x4 m = glm::translate(mid, vt) * rot; + + glm::quat qr = glm::quat_cast(m); + + glm::dualquat dq(qr); + + glm::dualquat invdq = glm::inverse(dq); + + glm::dualquat r1 = invdq * dq; + glm::dualquat r2 = dq * invdq; + + Error += glm::all(glm::epsilonEqual(r1.real, dqid.real, Epsilon)) && glm::all(glm::epsilonEqual(r1.dual, dqid.dual, Epsilon)) ? 0 : 1; + Error += glm::all(glm::epsilonEqual(r2.real, dqid.real, Epsilon)) && glm::all(glm::epsilonEqual(r2.dual, dqid.dual, Epsilon)) ? 0 : 1; + + // testing commutative property + glm::dualquat r ( glm::quat( myfrand() * glm::pi() * 2.0f, myfrand(), myfrand(), myfrand() ), + glm::vec3(myfrand() * 10.0f, myfrand() * 10.0f, myfrand() * 10.0f) ); + glm::dualquat riq = (r * invdq) * dq; + glm::dualquat rqi = (r * dq) * invdq; + + Error += glm::all(glm::epsilonEqual(riq.real, rqi.real, Epsilon)) && glm::all(glm::epsilonEqual(riq.dual, rqi.dual, Epsilon)) ? 0 : 1; + } + + return Error; +} + +int test_mul() +{ + int Error(0); + + float const Epsilon = 0.0001f; + + glm::mat4x4 mid(1.0f); + + for (int j = 0; j < 100; ++j) { + // generate random rotations and translations and compare transformed by matrix and dualquats random points + glm::vec3 vt1 = glm::vec3(myfrand() * 10.0f, myfrand() * 10.0f, myfrand() * 10.0f); + glm::vec3 vt2 = glm::vec3(myfrand() * 10.0f, myfrand() * 10.0f, myfrand() * 10.0f); + + glm::mat4x4 rot1 = glm::yawPitchRoll(myfrand() * 360.0f, myfrand() * 360.0f, myfrand() * 360.0f); + glm::mat4x4 rot2 = glm::yawPitchRoll(myfrand() * 360.0f, myfrand() * 360.0f, myfrand() * 360.0f); + glm::mat4x4 m1 = glm::translate(mid, vt1) * rot1; + glm::mat4x4 m2 = glm::translate(mid, vt2) * rot2; + glm::mat4x4 m3 = m2 * m1; + glm::mat4x4 m4 = m1 * m2; + + glm::quat qrot1 = glm::quat_cast(rot1); + glm::quat qrot2 = glm::quat_cast(rot2); + + glm::dualquat dq1 = glm::dualquat(qrot1,vt1); + glm::dualquat dq2 = glm::dualquat(qrot2,vt2); + glm::dualquat dq3 = dq2 * dq1; + glm::dualquat dq4 = dq1 * dq2; + + for (int i = 0; i < 100; ++i) { + glm::vec4 src_pt = glm::vec4(myfrand() * 4.0f, myfrand() * 5.0f, myfrand() * 3.0f,1.0f); + // test both multiplication orders + glm::vec4 dst_pt_m3 = m3 * src_pt; + glm::vec4 dst_pt_dq3 = dq3 * src_pt; + + glm::vec4 dst_pt_m3_i = glm::inverse(m3) * src_pt; + glm::vec4 dst_pt_dq3_i = src_pt * dq3; + + glm::vec4 dst_pt_m4 = m4 * src_pt; + glm::vec4 dst_pt_dq4 = dq4 * src_pt; + + glm::vec4 dst_pt_m4_i = glm::inverse(m4) * src_pt; + glm::vec4 dst_pt_dq4_i = src_pt * dq4; + + Error += glm::all(glm::epsilonEqual(dst_pt_m3, dst_pt_dq3, Epsilon)) ? 0 : 1; + Error += glm::all(glm::epsilonEqual(dst_pt_m4, dst_pt_dq4, Epsilon)) ? 0 : 1; + Error += glm::all(glm::epsilonEqual(dst_pt_m3_i, dst_pt_dq3_i, Epsilon)) ? 0 : 1; + Error += glm::all(glm::epsilonEqual(dst_pt_m4_i, dst_pt_dq4_i, Epsilon)) ? 0 : 1; + } + } + + return Error; +} + +int main() +{ + int Error(0); + + Error += test_dquat_type(); + Error += test_scalars(); + Error += test_inverse(); + Error += test_mul(); + + //std::cout << "Errors count: " << Error << std::endl; + return Error; +}