Commit 41cbbff3 authored by Martin Reinecke's avatar Martin Reinecke
Browse files

change quaternion ordering from w,x,y,z to x,y,z,w (for scipy compatibility)

parent 3373e1ec
Pipeline #77204 passed with stages
in 13 minutes and 1 second
......@@ -46,10 +46,10 @@ template<typename T> class PointingProvider
{
MR_assert(quat.shape(0)>=2, "need at least 2 quaternions");
MR_assert(quat.shape(1)==4, "need 4 entries in quaternion");
quat_[0] = quaternion_t<T>(quat(0,3), quat(0,0), quat(0,1), quat(0,2)).normalized();
quat_[0] = quaternion_t<T>(quat(0,0), quat(0,1), quat(0,2), quat(0,3)).normalized();
for (size_t m=0; m<quat_.size()-1; ++m)
{
quat_[m+1] = quaternion_t<T>(quat(m+1,3), quat(m+1,0), quat(m+1,1), quat(m+1,2)).normalized();
quat_[m+1] = quaternion_t<T>(quat(m+1,0), quat(m+1,1), quat(m+1,2), quat(m+1,3)).normalized();
quaternion_t<T> delta(quat_[m+1]*quat_[m].conj());
rotflip[m]=false;
if (delta.w < 0.)
......@@ -64,7 +64,7 @@ template<typename T> class PointingProvider
mav<T,2> &out)
{
MR_assert(rot.shape(0)==4, "need 4 entries in quaternion");
auto rot_ = quaternion_t<T>(rot(3), rot(0), rot(1), rot(2)).normalized();
auto rot_ = quaternion_t<T>(rot(0), rot(1), rot(2), rot(3)).normalized();
MR_assert(out.shape(1)==4, "need 4 entries in quaternion");
double ofs = (t0-t0_)*freq_;
for (size_t i=0; i<out.shape(0); ++i)
......@@ -80,10 +80,10 @@ template<typename T> class PointingProvider
w2 = sin(frac*omega)*xsin;
if (rotflip[idx]) w1=-w1;
const quaternion_t<T> &q1(quat_[idx]), &q2(quat_[idx+1]);
quaternion_t<T> q(w1*q1.w + w2*q2.w,
w1*q1.x + w2*q2.x,
quaternion_t<T> q(w1*q1.x + w2*q2.x,
w1*q1.y + w2*q2.y,
w1*q1.z + w2*q2.z);
w1*q1.z + w2*q2.z,
w1*q1.w + w2*q2.w);
q *= rot_;
out.v(i,0) = q.x;
out.v(i,1) = q.y;
......@@ -137,7 +137,7 @@ freq : float
the frequency at which the provided satellite orientations are sampled
quat : np.ndarray((nval, 4), dtype=np.float64)
the satellite orientation quaternions. Coordinates are expecetd in the order
(w, x, y, z). The quaternions need not be normalized.
(x, y, z, w). The quaternions need not be normalized.
Returns
-------
......@@ -160,7 +160,7 @@ freq : float
rot : np.ndarray((4,), dtype=np.float64)
A single rotation quaternion describing the rotation from the satellite to
the detector reference system. Coordinates are expecetd in the order
(w, x, y, z). The quaternion need not be normalized.
(x, y, z, w). The quaternion need not be normalized.
nval : int
the number of requested quaternions
......
......@@ -56,7 +56,6 @@ def testp2():
t01, f1, size1 = 0., 1., 200
quat1 = rng.uniform(-.5, .5, (size1, 4))
prov = pp.PointingProvider(t01, f1, quat1)
rquat = np.array([1., 0., 0., 0.]) # a non-rotating quaternion
rquat = rng.uniform(-.5, .5, (4,))
t02, f2, size2 = 3.7, 10.2, 300
quat2 = prov.get_rotated_quaternions(t02, f2, rquat, size2)
......
......@@ -40,68 +40,65 @@ using namespace std;
template<typename T> class quaternion_t
{
public:
T w, x, y, z;
T x, y, z, w;
quaternion_t() {}
quaternion_t(T W, T X, T Y, T Z)
: w(W), x(X), y(Y), z(Z) {}
quaternion_t(T X, T Y, T Z, T W)
: x(X), y(Y), z(Z), w(W) {}
quaternion_t(const vec3_t<T> &axis, T angle)
{
angle*=T(0.5);
T sa=sin(angle);
w = cos(angle);
x = sa*axis.x;
y = sa*axis.y;
z = sa*axis.z;
w = cos(angle);
}
void set (T W, T X, T Y, T Z)
{ w=W; x=X; y=Y; z=Z; }
T norm() const
{ return w*w+x*x+y*y+z*z; }
{ return x*x+y*y+z*z+w*w; }
quaternion_t normalized() const
{
T fct = sqrt(T(1)/norm());
return quaternion_t(w*fct ,x*fct, y*fct, z*fct);
return quaternion_t(x*fct, y*fct, z*fct, w*fct);
}
quaternion_t operator-() const
{ return quaternion_t(-w,-x,-y,-z); }
{ return quaternion_t(-x,-y,-z,-w); }
void flip()
{ w=-w; x=-x; y=-y; z=-z; }
{ x=-x; y=-y; z=-z; w=-w; }
quaternion_t conj() const
{ return quaternion_t(w,-x,-y,-z); }
{ return quaternion_t(-x,-y,-z, w); }
quaternion_t inverse() const
{
T fct = T(1)/norm();
return quaternion_t(w*fct ,-x*fct, -y*fct, -z*fct);
return quaternion_t(-x*fct, -y*fct, -z*fct, w*fct);
}
quaternion_t &operator*= (const quaternion_t &b)
{
quaternion_t a=*this;
w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
return *this;
}
quaternion_t operator* (const quaternion_t &b) const
{
return quaternion_t (w*b.w - x*b.x - y*b.y - z*b.z,
w*b.x + x*b.w + y*b.z - z*b.y,
return quaternion_t (w*b.x + x*b.w + y*b.z - z*b.y,
w*b.y - x*b.z + y*b.w + z*b.x,
w*b.z + x*b.y - y*b.x + z*b.w);
w*b.z + x*b.y - y*b.x + z*b.w,
w*b.w - x*b.x - y*b.y - z*b.z);
}
T dot(const quaternion_t &other) const
{ return w*other.w + x*other.x + y*other.y + z*other.z; }
{ return x*other.x + y*other.y + z*other.z + w*other.w; }
auto toAxisAngle() const
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment