lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add quaternion derivative to angular momentum transform matrix

Browse source
This commit is contained in:
Nikita Lisitsa 2022-01-07 23:49:14 +03:00
parent 385efe5ff9
commit 1f0e4e545e
1 changed files with 20 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
libs/geom/include/psemek/geom/quaternion.hpp
View file

@ -240,4 +240,24 @@ namespace psemek::geom
return os << q.coords;
}
// Returns a matrix M such that the angular gradient of function f(q) is M * grad(f)
template <typename T>
geom::matrix<T, 3, 4> angular_gradient(quaternion<T> const & q)
{
geom::matrix<T, 3, 4> result;
result[0][0] = q[3];
result[0][1] = - q[2];
result[0][2] = q[1];
result[0][3] = - q[0];
result[1][0] = q[2];
result[1][1] = q[3];
result[1][2] = - q[0];
result[1][3] = - q[1];
result[2][0] = - q[1];
result[2][1] = q[0];
result[2][2] = q[3];
result[2][3] = - q[2];
return result;
}
}