You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
45 lines
1.4 KiB
45 lines
1.4 KiB
#pragma once
|
|
#include "real.hpp"
|
|
#include "uvector.hpp"
|
|
#include "xarray.hpp"
|
|
|
|
namespace algoim
|
|
{
|
|
|
|
class Quaternion
|
|
{
|
|
public:
|
|
real aw, ai, aj, ak;
|
|
|
|
Quaternion(real aw_, real ai_, real aj_, real ak_) : aw(aw_), ai(ai_), aj(aj_), ak(ak_) {}
|
|
|
|
Quaternion(real ai_, real aj_, real ak_) : aw(0), ai(ai_), aj(aj_), ak(ak_) {}
|
|
|
|
Quaternion(const uvector3& axis, real angle)
|
|
{
|
|
uvector3 axisNorm = axis / norm(axis);
|
|
real halfAngle = angle / 2;
|
|
aw = cos(halfAngle);
|
|
real sinHalfAngle = sin(halfAngle);
|
|
ai = axisNorm(0) * sinHalfAngle;
|
|
aj = axisNorm(1) * sinHalfAngle;
|
|
ak = axisNorm(2) * sinHalfAngle;
|
|
}
|
|
|
|
void getRotation(tensor2& rotation) const
|
|
{
|
|
assert(all(rotation.ext() == uvector2(3)));
|
|
rotation.m(uvector2(0, 0)) = aw * aw + ai * ai - aj * aj - ak * ak;
|
|
rotation.m(uvector2(0, 1)) = 2 * (ai * aj - aw * ak);
|
|
rotation.m(uvector2(0, 2)) = 2 * (ai * ak + aw * aj);
|
|
|
|
rotation.m(uvector2(1, 0)) = 2 * (ai * aj + aw * ak);
|
|
rotation.m(uvector2(1, 1)) = aw * aw - ai * ai + aj * aj - ak * ak;
|
|
rotation.m(uvector2(1, 2)) = 2 * (aj * ak - aw * ai);
|
|
|
|
rotation.m(uvector2(2, 0)) = 2 * (ai * ak - aw * aj);
|
|
rotation.m(uvector2(2, 1)) = 2 * (aj * ak + aw * ai);
|
|
rotation.m(uvector2(2, 2)) = aw * aw - ai * ai - aj * aj + ak * ak;
|
|
}
|
|
};
|
|
} // namespace algoim
|
|
|