More work on quaternions.

This commit is contained in:
Kyle Isom 2019-08-05 09:46:08 -07:00
parent 4cf4693088
commit de1c4e6109
3 changed files with 164 additions and 35 deletions

View File

@ -4,7 +4,7 @@
#include <cassert> #include <cassert>
#include <cmath> #include <cmath>
#include <initializer_list> #include <ostream>
#include <wrmath/geom/vector.h> #include <wrmath/geom/vector.h>
#include <wrmath/math.h> #include <wrmath/math.h>
@ -16,30 +16,93 @@ namespace geom {
template <typename T> template <typename T>
class Quaternion { class Quaternion {
public: public:
// The default constructor returns the identity quaternion. /**
* The default Quaternion constructor returns an identity quaternion.
*/
Quaternion() : v(Vector<T, 3>()), w(1.0) Quaternion() : v(Vector<T, 3>()), w(1.0)
{ {
wr::math::DefaultEpsilon(this->eps); wr::math::DefaultEpsilon(this->eps);
this->w = std::fmod(this->w, this->maxRotation);
}; };
Quaternion(Vector<T, 3> axis, T angle) : v(axis), w(angle) /**
* A Quaternion may be initialised with a Vector<T, 3> axis of rotation
* and an angle of rotation.
* @param _axis A three-dimensional vector of the same type as the Quaternion.
* @param _angle The angle of rotation about the axis of rotation.
*/
Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle)
{ {
wr::math::DefaultEpsilon(this->eps); wr::math::DefaultEpsilon(this->eps);
this->w = std::fmod(this->w, this->maxRotation);
}; };
/**
* Return the axis of rotation of this quaternion.
* @return The axis of rotation of this quaternion.
*/
Vector<T, 3>
axis() const
{
return this->v;
}
/**
* Return the angle of rotation of this quaternion.
* @return the angle of rotation of this quaternion.
*/
T
angle() const
{
return this->w;
}
/**
* Compute the norm of a quaternion. Treating the Quaternion as a
* Vector<T, 4>, it's the same as computing the magnitude.
* @return A non-negative real number.
*/
T
norm()
{
T n = 0;
n += (this->v[0] * this->v[0]);
n += (this->v[1] * this->v[1]);
n += (this->v[2] * this->v[2]);
n += (this->w * this->w);
return std::sqrt(n);
}
Vector<T, 4>
asVector()
{
return Vector<T, 4> {this->v[0], this->v[1], this->v[2], this->w};
}
/**
* Perform quaternion addition with another quaternion.
* @param other The quaternion to be added with this one.
* @return
*/
Quaternion Quaternion
operator+(const Quaternion<T> &other) const operator+(const Quaternion<T> &other) const
{ {
return Quaternion(this->v + other.v, (this->w + other.w) % this->maxRotation); return Quaternion(this->v + other.v, this->w + other.w);
} }
Quaternion Quaternion
operator-(const Quaternion<T> &other) const operator-(const Quaternion<T> &other) const
{ {
return Quaternion(this->v - other.v, (this->w - other.w) % this->maxRotation); return Quaternion(this->v - other.v, this->w - other.w);
} }
@ -69,6 +132,14 @@ public:
return !(*this == other); return !(*this == other);
} }
friend std::ostream&
operator<<(std::ostream& outs, const Quaternion<T>& q)
{
outs << q.w << " + " << q.v;
return outs;
}
private: private:
static constexpr T maxRotation = 4 * M_PI; static constexpr T maxRotation = 4 * M_PI;

View File

@ -1,3 +1,4 @@
#include <sstream>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <wrmath/geom/quaternion.h> #include <wrmath/geom/quaternion.h>
@ -6,6 +7,28 @@ using namespace wr;
TEST(Quaterniond, Addition) TEST(Quaterniond, Addition)
{
geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0);
geom::Quaterniond q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0);
geom::Quaterniond expected(geom::Vector3d{0.0, 0.0, 4.0}, 5.0);
EXPECT_EQ(p + q, expected);
EXPECT_EQ(expected - q, p);
EXPECT_NE(expected - q, q); // exercise !=
}
TEST(Quaterniond, Norm)
{
geom::Quaterniond p(geom::Vector3d {0.9899139811480784, 9.387110042325054, 6.161341707794767},
5.563199889674063);
double norm = 12.57016663729933;
EXPECT_DOUBLE_EQ(p.norm(), norm);
}
TEST(Quaterniond, Product)
{ {
geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0); geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0);
geom::Quaterniond q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0); geom::Quaterniond q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0);
@ -15,6 +38,61 @@ TEST(Quaterniond, Addition)
} }
TEST(Quaterniond, Identity)
{
geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0);
geom::Quaterniond q;
EXPECT_EQ(p * q, p);
}
TEST(Quaternionf, Norm)
{
geom::Quaternionf p(geom::Vector3f {0.9899139811480784, 9.387110042325054, 6.161341707794767},
5.563199889674063);
float norm = 12.57016663729933;
EXPECT_DOUBLE_EQ(p.norm(), norm);
}
TEST(Quaternionf, Product)
{
geom::Quaternionf p(geom::Vector3f {1.0, -2.0, 1.0}, 3.0);
geom::Quaternionf q(geom::Vector3f {-1.0, 2.0, 3.0}, 2.0);
geom::Quaternionf expected(geom::Vector3f{-9.0, -2.0, 11.0}, 8.0);
EXPECT_EQ(p * q, expected);
}
TEST(QuaternionMiscellaneous, SanityChecks)
{
geom::Vector3d v {1.0, 2.0, 3.0};
double w = 4.0;
geom::Quaterniond p(v, w);
EXPECT_EQ(p.axis(), v);
EXPECT_DOUBLE_EQ(p.angle(), w);
}
TEST(QuaternionMiscellaneous, OutputStream)
{
geom::Quaternionf p(geom::Vector3f {1.0, 2.0, 3.0}, 4.0);
geom::Quaterniond q(geom::Vector3d {1.0, 2.0, 3.0}, 4.0);
stringstream ss;
ss << p;
EXPECT_EQ(ss.str(), "4 + <1, 2, 3>");
ss.str("");
ss << q;
EXPECT_EQ(ss.str(), "4 + <1, 2, 3>");
}
int int
main(int argc, char **argv) main(int argc, char **argv)
{ {

View File

@ -26,26 +26,6 @@ TEST(Vector3Miscellaneous, ExtractionOperator3f)
} }
TEST(Vector3Miscellaneous, ExtractionOperator4d)
{
geom::Vector4d vec {1.0, 2.0, 3.0, 4.0};
stringstream vecBuffer;
vecBuffer << vec;
EXPECT_EQ(vecBuffer.str(), "<1, 2, 3, 4>");
}
TEST(Vector3Miscellaneous, ExtractionOperator4f)
{
geom::Vector4f vec {1.0, 2.0, 3.0, 4.0};
stringstream vecBuffer;
vecBuffer << vec;
EXPECT_EQ(vecBuffer.str(), "<1, 2, 3, 4>");
}
TEST(Vector3Miscellaneous, SetEpsilon) TEST(Vector3Miscellaneous, SetEpsilon)
{ {
geom::Vector3f a {1.0, 1.0, 1.0}; geom::Vector3f a {1.0, 1.0, 1.0};
@ -172,12 +152,12 @@ TEST(Vector3FloatTests, ParallelOrthogonalVectors)
} }
TEST(Vector4FloatTests, Projections) TEST(Vector3FloatTests, Projections)
{ {
geom::Vector4f a {3.009, -6.172, 3.692, -2.510}; geom::Vector3f a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
geom::Vector4f b {6.404, -9.144, 2.759, 8.718}; geom::Vector3f b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
geom::Vector4f c {1.9685, -2.8108, 0.8481, 2.6798}; geom::Vector3f c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
geom::Vector4f d {1.0405, -3.3612, 2.8439, -5.1898}; geom::Vector3f d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
ASSERT_EQ(a.projectParallel(b), c); ASSERT_EQ(a.projectParallel(b), c);
ASSERT_EQ(a.projectOrthogonal(b), d); ASSERT_EQ(a.projectOrthogonal(b), d);
@ -313,12 +293,12 @@ TEST(Vector3DoubleTests, ParallelOrthogonalVectors)
} }
TEST(Vector4DoubleTests, Projections) TEST(Vector3DoubleTests, Projections)
{ {
geom::Vector4d a {3.009, -6.172, 3.692, -2.510}; geom::Vector3d a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
geom::Vector4d b {6.404, -9.144, 2.759, 8.718}; geom::Vector3d b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
geom::Vector4d c {1.9685, -2.8108, 0.8481, 2.6798}; geom::Vector3d c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
geom::Vector4d d {1.0405, -3.3612, 2.8439, -5.1898}; geom::Vector3d d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
ASSERT_EQ(a.projectParallel(b), c); ASSERT_EQ(a.projectParallel(b), c);
ASSERT_EQ(a.projectOrthogonal(b), d); ASSERT_EQ(a.projectOrthogonal(b), d);