Start quaternions.

This commit is contained in:
Kyle Isom 2019-08-05 00:12:03 -07:00
parent 3a5dd0490c
commit 4cf4693088
4 changed files with 143 additions and 12 deletions

View File

@ -68,6 +68,7 @@ endmacro()
# define the tests
package_add_gtest(vector_test test/vector_test.cc)
package_add_gtest(orientation_test test/orientation_test.cc)
package_add_gtest(quaternion_test test/quaternion_test.cc)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --verbose DEPENDS ${TEST_EXECS})

View File

@ -0,0 +1,89 @@
#ifndef __WRMATH_QUATERNION_H
#define __WRMATH_QUATERNION_H
#include <cassert>
#include <cmath>
#include <initializer_list>
#include <wrmath/geom/vector.h>
#include <wrmath/math.h>
namespace wr {
namespace geom {
template <typename T>
class Quaternion {
public:
// The default constructor returns the identity quaternion.
Quaternion() : v(Vector<T, 3>()), w(1.0)
{
wr::math::DefaultEpsilon(this->eps);
};
Quaternion(Vector<T, 3> axis, T angle) : v(axis), w(angle)
{
wr::math::DefaultEpsilon(this->eps);
};
Quaternion
operator+(const Quaternion<T> &other) const
{
return Quaternion(this->v + other.v, (this->w + other.w) % this->maxRotation);
}
Quaternion
operator-(const Quaternion<T> &other) const
{
return Quaternion(this->v - other.v, (this->w - other.w) % this->maxRotation);
}
Quaternion
operator*(const Quaternion<T> &other) const
{
T angle = (this->w * other.w) -
(this->v * other.v);
Vector<T, 3> axis = (other.v * this->w) +
(this->v * other.w) +
(this->v.cross(other.v));
return Quaternion(axis, angle);
}
bool
operator==(const Quaternion<T> &other) const
{
return (this->v == other.v) &&
(wr::math::WithinTolerance(this->w, other.w, this->eps));
}
bool
operator!=(const Quaternion<T> &other) const
{
return !(*this == other);
}
private:
static constexpr T maxRotation = 4 * M_PI;
Vector<T, 3> v; // axis of rotation
T w; // angle of rotation
T eps;
};
typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;
} // namespace geom
} // namespace wr
#endif // WRMATH_QUATERNION_H

View File

@ -211,7 +211,7 @@ public:
* @return The cross product vector.
*/
Vector
cross(const Vector<T, N> &other)
cross(const Vector<T, N> &other) const
{
assert(N == 3);
return Vector<T, N> {
@ -228,7 +228,9 @@ public:
* @return A new vector that is the result of adding this and the
* other vector.
*/
Vector operator+(const Vector<T, N> &other) const {
Vector
operator+(const Vector<T, N> &other) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
@ -245,7 +247,9 @@ public:
* @return A new vector that is the result of subtracting the
* other vector from this one.
*/
Vector operator-(const Vector<T, N> &other) const {
Vector
operator-(const Vector<T, N> &other) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
@ -261,7 +265,9 @@ public:
* @param k The scaling value.
* @return A new vector that is this vector scaled by k.
*/
Vector operator*(const T k) const {
Vector
operator*(const T k) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
@ -277,7 +283,9 @@ public:
* @param k The scaling value
* @return A new vector that is this vector scaled by 1/k.
*/
Vector operator/(const T k) const {
Vector
operator/(const T k) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
@ -293,7 +301,9 @@ public:
* @param other The other vector.
* @return A scalar value that is the dot product of the two vectors.
*/
T operator*(const Vector<T, N> &other) const {
T
operator*(const Vector<T, N> &other) const
{
T result = 0;
for (size_t i = 0; i < N; i++) {
@ -310,7 +320,9 @@ public:
* @return Return true if all the components of both vectors are
* within the tolerance value.
*/
bool operator==(const Vector<T, N> &other) const {
bool
operator==(const Vector<T, N> &other) const
{
for (size_t i = 0; i<N; i++) {
if (!wr::math::WithinTolerance(this->arr[i], other.arr[i], this->epsilon)) {
return false;
@ -326,7 +338,9 @@ public:
* @return Return true if any of the components of both vectors are
* not within the tolerance value.
*/
bool operator!=(const Vector<T, N> &other) const {
bool
operator!=(const Vector<T, N> &other) const
{
return !(*this == other);
}
@ -336,7 +350,9 @@ public:
* @param i The component index.
* @return The value of the vector component at i.
*/
T operator[](size_t i) const {
T
operator[](size_t i) const
{
return this->arr[i];
}
@ -347,7 +363,9 @@ public:
* @param vec The vector to be formatted.
* @return The output stream.
*/
friend std::ostream& operator<<(std::ostream& outs, const Vector<T, N>& vec) {
friend std::ostream&
operator<<(std::ostream& outs, const Vector<T, N>& vec)
{
outs << "<";
for (size_t i = 0; i < N; i++) {
outs << vec.arr[i];

23
test/quaternion_test.cc Normal file
View File

@ -0,0 +1,23 @@
#include <gtest/gtest.h>
#include <wrmath/geom/quaternion.h>
using namespace std;
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{-9.0, -2.0, 11.0}, 8.0);
EXPECT_EQ(p * q, expected);
}
int
main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}