Use quaternion multiplaction for quaternion derivative.

This commit is contained in:
James Goppert
2016-08-16 01:03:23 -04:00
parent c563c41cd4
commit e6a6b4680c
3 changed files with 8 additions and 15 deletions
+1 -1
View File
@@ -278,7 +278,7 @@ public:
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
snprintf(buf + strlen(buf), n - strlen(buf), "\t%.2g", double(self(i, j))); // directly append to the string buffer
snprintf(buf + strlen(buf), n - strlen(buf), "\t%g", double(self(i, j))); // directly append to the string buffer
}
snprintf(buf + strlen(buf), n - strlen(buf), "\n");
}
+2 -13
View File
@@ -238,19 +238,8 @@ public:
Matrix41 derivative(const Matrix31 &w) const
{
const Quaternion &q = *this;
Type dataQ[] = {
q(0), -q(1), -q(2), -q(3),
q(1), q(0), -q(3), q(2),
q(2), q(3), q(0), -q(1),
q(3), -q(2), q(1), q(0)
};
Matrix<Type, 4, 4> Q(dataQ);
Vector<Type, 4> v;
v(0) = 0;
v(1) = w(0, 0);
v(2) = w(1, 0);
v(3) = w(2, 0);
return Q * v * Type(0.5);
Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
return v * q * Type(0.5);
}
/**
+5 -1
View File
@@ -157,7 +157,11 @@ int main()
TEST(isEqual(q_from_m, m4));
// quaternion derivate
Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
Quatf q1(0, 1, 0, 0);
Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
float data_q_dot_check[] = {-0.5f, 0.0f, -1.5f, 1.0f};
Vector<float, 4> q1_dot_check(data_q_dot_check);
TEST(isEqual(q1_dot, q1_dot_check));
// quaternion product
Quatf q_prod_check(