More work on quaternions.
This commit is contained in:
parent
4cf4693088
commit
de1c4e6109
|
|
@ -4,7 +4,7 @@
|
|||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <initializer_list>
|
||||
#include <ostream>
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <wrmath/math.h>
|
||||
|
||||
|
|
@ -16,30 +16,93 @@ namespace geom {
|
|||
template <typename T>
|
||||
class Quaternion {
|
||||
public:
|
||||
// The default constructor returns the identity quaternion.
|
||||
/**
|
||||
* The default Quaternion constructor returns an identity quaternion.
|
||||
*/
|
||||
Quaternion() : v(Vector<T, 3>()), w(1.0)
|
||||
{
|
||||
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);
|
||||
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
|
||||
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
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
friend std::ostream&
|
||||
operator<<(std::ostream& outs, const Quaternion<T>& q)
|
||||
{
|
||||
outs << q.w << " + " << q.v;
|
||||
return outs;
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr T maxRotation = 4 * M_PI;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
#include <sstream>
|
||||
#include <gtest/gtest.h>
|
||||
#include <wrmath/geom/quaternion.h>
|
||||
|
||||
|
|
@ -6,6 +7,28 @@ using namespace wr;
|
|||
|
||||
|
||||
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 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
|
||||
main(int argc, char **argv)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
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::Vector4f b {6.404, -9.144, 2.759, 8.718};
|
||||
geom::Vector4f c {1.9685, -2.8108, 0.8481, 2.6798};
|
||||
geom::Vector4f d {1.0405, -3.3612, 2.8439, -5.1898};
|
||||
geom::Vector3f a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
|
||||
geom::Vector3f b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
|
||||
geom::Vector3f c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
|
||||
geom::Vector3f d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
|
||||
|
||||
ASSERT_EQ(a.projectParallel(b), c);
|
||||
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::Vector4d b {6.404, -9.144, 2.759, 8.718};
|
||||
geom::Vector4d c {1.9685, -2.8108, 0.8481, 2.6798};
|
||||
geom::Vector4d d {1.0405, -3.3612, 2.8439, -5.1898};
|
||||
geom::Vector3d a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
|
||||
geom::Vector3d b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
|
||||
geom::Vector3d c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
|
||||
geom::Vector3d d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
|
||||
|
||||
ASSERT_EQ(a.projectParallel(b), c);
|
||||
ASSERT_EQ(a.projectOrthogonal(b), d);
|
||||
|
|
|
|||
Loading…
Reference in New Issue