diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index 0b2587f5..d0e1ae61 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -47,14 +47,28 @@ Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), } -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(decimal angleX, decimal angleY, decimal angleZ) { - initWithEulerAngles(angleX, angleY, angleZ); +// Constructor with the component w and the vector v=(x y z) +Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { + } -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(const Vector3& eulerAngles) { - initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); +// Return a quaternion constructed from Euler angles (in radians) +Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) { + + Quaternion quaternion; + quaternion.initWithEulerAngles(angleX, angleY, angleZ); + + return quaternion; +} + + +// Return a quaternion constructed from Euler angles (in radians) +Quaternion Quaternion::fromEulerAngles(const Vector3& eulerAngles) { + + Quaternion quaternion; + quaternion.initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); + + return quaternion; } // Copy-constructor @@ -72,10 +86,10 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { decimal r; decimal s; - if (trace < 0.0) { + if (trace < decimal(0.0)) { if (matrix[1][1] > matrix[0][0]) { if(matrix[2][2] > matrix[1][1]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); + r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -85,7 +99,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { w = (matrix[1][0] - matrix[0][1]) * s; } else { - r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0)); + r = std::sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -96,7 +110,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { } } else if (matrix[2][2] > matrix[0][0]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); + r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -106,7 +120,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { w = (matrix[1][0] - matrix[0][1]) * s; } else { - r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0)); + r = std::sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -117,7 +131,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { } } else { - r = sqrt(trace + decimal(1.0)); + r = std::sqrt(trace + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -132,22 +146,12 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { /// This method is used to get the rotation angle (in radian) and the unit /// rotation axis of an orientation quaternion. void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const { - Quaternion quaternion; - - // If the quaternion is unit - if (length() == 1.0) { - quaternion = *this; - } - else { - // We compute the unit quaternion - quaternion = getUnit(); - } // Compute the roation angle - angle = acos(quaternion.w) * decimal(2.0); + angle = std::acos(w) * decimal(2.0); // Compute the 3D rotation axis - Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z); + Vector3 rotationAxis(x, y, z); // Normalize the rotation axis rotationAxis = rotationAxis.getUnit(); @@ -162,7 +166,7 @@ Matrix3x3 Quaternion::getMatrix() const { decimal nQ = x*x + y*y + z*z + w*w; decimal s = 0.0; - if (nQ > 0.0) { + if (nQ > decimal(0.0)) { s = decimal(2.0) / nQ; } @@ -190,7 +194,7 @@ Matrix3x3 Quaternion::getMatrix() const { /// The t argument has to be such that 0 <= t <= 1. This method is static. Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, decimal t) { - assert(t >= 0.0 && t <= 1.0); + assert(t >= decimal(0.0) && t <= decimal(1.0)); decimal invert = 1.0; @@ -198,7 +202,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, decimal cosineTheta = quaternion1.dot(quaternion2); // Take care of the sign of cosineTheta - if (cosineTheta < 0.0) { + if (cosineTheta < decimal(0.0)) { cosineTheta = -cosineTheta; invert = -1.0; } @@ -212,14 +216,14 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, } // Compute the theta angle - decimal theta = acos(cosineTheta); + decimal theta = std::acos(cosineTheta); // Compute sin(theta) - decimal sineTheta = sin(theta); + decimal sineTheta = std::sin(theta); // Compute the two coefficients that are in the spherical linear interpolation formula - decimal coeff1 = sin((decimal(1.0)-t)*theta) / sineTheta; - decimal coeff2 = sin(t*theta) / sineTheta * invert; + decimal coeff1 = std::sin((decimal(1.0)-t)*theta) / sineTheta; + decimal coeff2 = std::sin(t*theta) / sineTheta * invert; // Compute and return the interpolated quaternion return quaternion1 * coeff1 + quaternion2 * coeff2; diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index b8e75306..74415eb8 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -69,11 +69,8 @@ struct Quaternion { /// Constructor with the component w and the vector v=(x y z) Quaternion(decimal newW, const Vector3& v); - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(decimal angleX, decimal angleY, decimal angleZ); - - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(const Vector3& eulerAngles); + /// Constructor with the component w and the vector v=(x y z) + Quaternion(const Vector3& v, decimal newW); /// Copy-constructor Quaternion(const Quaternion& quaternion); @@ -123,6 +120,12 @@ struct Quaternion { /// Return the identity quaternion static Quaternion identity(); + /// Return a quaternion constructed from Euler angles (in radians) + static Quaternion fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ); + + /// Return a quaternion constructed from Euler angles (in radians) + static Quaternion fromEulerAngles(const Vector3& eulerAngles); + /// Dot product between two quaternions decimal dot(const Quaternion& quaternion) const; @@ -228,7 +231,6 @@ inline void Quaternion::inverse() { x = -x; y = -y; z = -z; - w = w; } // Return the unit quaternion diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 608b439b..afdb415c 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -93,7 +93,7 @@ class TestPointInside : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -108,7 +108,7 @@ class TestPointInside : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space @@ -165,7 +165,7 @@ class TestPointInside : public Test { // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index c62ca179..97dd002a 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -169,7 +169,7 @@ class TestRaycast : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -185,7 +185,7 @@ class TestRaycast : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space @@ -246,7 +246,7 @@ class TestRaycast : public Test { // Compound shape is a cylinder and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundCapsuleProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 5fe0ccd0..dfc65955 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -94,7 +94,7 @@ class TestQuaternion : public Test { const decimal PI_OVER_2 = PI * decimal(0.5); const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5); - Quaternion quaternion5(PI_OVER_2, 0, 0); + Quaternion quaternion5 = Quaternion::fromEulerAngles(PI_OVER_2, 0, 0); Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); quaternionTest5.normalize(); test(approxEqual(quaternion5.x, quaternionTest5.x)); @@ -102,7 +102,7 @@ class TestQuaternion : public Test { test(approxEqual(quaternion5.z, quaternionTest5.z)); test(approxEqual(quaternion5.w, quaternionTest5.w)); - Quaternion quaternion6(0, PI_OVER_2, 0); + Quaternion quaternion6 = Quaternion::fromEulerAngles(0, PI_OVER_2, 0); Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); quaternionTest6.normalize(); test(approxEqual(quaternion6.x, quaternionTest6.x)); @@ -110,7 +110,7 @@ class TestQuaternion : public Test { test(approxEqual(quaternion6.z, quaternionTest6.z)); test(approxEqual(quaternion6.w, quaternionTest6.w)); - Quaternion quaternion7(Vector3(0, 0, PI_OVER_2)); + Quaternion quaternion7 = Quaternion::fromEulerAngles(Vector3(0, 0, PI_OVER_2)); Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); quaternionTest7.normalize(); test(approxEqual(quaternion7.x, quaternionTest7.x)); @@ -124,7 +124,7 @@ class TestQuaternion : public Test { // Test method that returns the length Quaternion quaternion(2, 3, -4, 5); - test(approxEqual(quaternion.length(), sqrt(decimal(54.0)))); + test(approxEqual(quaternion.length(), std::sqrt(decimal(54.0)))); // Test method that returns a unit quaternion test(approxEqual(quaternion.getUnit().length(), 1.0)); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 1241b701..6f2c65ae 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -282,32 +282,32 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i } else if (key == GLFW_KEY_A && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, stepAngle, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_D && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, -stepAngle, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_W && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(stepAngle, 0, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_S && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(-stepAngle, 0, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_F && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, stepAngle) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_G && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, -stepAngle) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); }