版权声明：, https://blog.csdn.net/u011760195/article/details/85382640
前言
同样的，跟前面一眼四元数的乘法也代表了旋转，四元数的积有多种定义，这里我们只讲被用在旋转操作的上积 —— 矢量积
矢量积
对于两个四元数q，p：
我们来看q右乘p，按照分配律
按照同样算法算pq结果是等于-qp的
也就是
旋转
重新审视qp
（1）q右乘p，我们将q作为一个旋转操作，作用于某个值p上
qp即为：p被q按照右手法则变换
（2）p左乘q，我们将p作为一个旋转操作，作用于某个值q上
qp即为：q被p按照左手法则变换
如何旋转一个三维空间的点呢？
现在，假设我们这里有一个旋转操作q，和一个三维坐标v
我们将v看作w = 0处的一个四元数p
记
经过一次旋转后
不为0了，这在三维空间看起来已经变形了
为什么呢，因为一次旋转涉及i，j，k三个轴的变化，他们是联动的，
当你从i轴向j轴转动时，k轴向ij向量积的方向转动
你可能会问一个轴怎么转动向他所指的方向转动？
事实上这个轴是投影在三维空间的一个半径为无穷大的圆...
在这里有一个3Blue1Brown的交互性视频，如果你无法理解我的概念可以自己去试着操作一下超球（Hypersphere）
https://eater.net/quaternions/video/quatmult
看，这是旋转之前的投影
现在我们绕k轴旋转
即
模长1，确保四维超球不会变形
我们看
可以看到确实旋转了，我们来看看k轴的情况
球体变形了，他膨胀了！
为什么？我们明明限制了
的模长1
根据运算结果，w不为0时偏离了我们的三维空间，导致超球在三维的投影变形了
如何去修复他？
我们左乘一个
，使得其按左手法则，向相反的方向旋转k轴，将超球拉回w=0处
当然，为了不变形超球，
模长 = 1，
我们做稍稍调整，
并用
左乘p，这样便可以抵消w的偏移
你可以同时拿出你的右手和左手，让食指从i轴的方向旋至j轴的方向，一个对应q1，另一个对应q2
两根拇指的方向相反，但是食指的转动的方向相同
也就是说：为了调整偏离w = 0的操作我们旋过了两倍
的操作。
或者说，要旋转
欧拉角，我们应该使q1等于其一半
之后
即可
至此，四元数的三维空间旋转应用已经结束，下面给出C++代码
C++代码
quaternion.h
// 四元数
// Ayww
// 2018年12月30日15:10:11
#ifndef _QUATERNION_
#define _QUATERNION_
#define _USE_MATH_DEFINES
#include <cmath>
#include <memory>
#include <tuple>
class quaternion {
public:
#ifdef QUATERNION_DOUBLE
using _Myvt = double;
#else
using _Myvt = float;
#endif
using vec3 = std::tuple<_Myvt, _Myvt, _Myvt>;
_Myvt w;
_Myvt x;
_Myvt y;
_Myvt z;
public:
explicit quaternion(_Myvt a = 0, _Myvt b = 0, _Myvt c = 0, _Myvt d = 0) :w(a), x(b), y(c), z(d) {}
~quaternion(){}
quaternion::quaternion(const quaternion &r) : w(r.w), x(r.x), y(r.y), z(r.z) {}
quaternion::quaternion(quaternion &&r) : w(r.w), x(r.x), y(r.y), z(r.z) {}
quaternion& operator=(quaternion const&);
quaternion& operator=(quaternion &&);
quaternion operator*(quaternion const &r);
quaternion operator+(quaternion const &r);
// 求模的平方
_Myvt norm_square(void);
// 求模
auto norm(void)->decltype(sqrt(w));
// 共轭
quaternion conjugate(void);
// 逆
quaternion inv(void);
// 加
quaternion& add(quaternion const& qr);
// 右乘
quaternion& multi(quaternion const& qr);
// 到欧拉角
vec3 to_euler(void);
// 欧拉角转换到四元数
quaternion& from_euler(_Myvt, _Myvt, _Myvt);
// 到三维向量
vec3 to_vector3(void);
// 三维向量扩充到四元数
quaternion& from_vector3(_Myvt, _Myvt, _Myvt);
};
quaternion operator*(quaternion::_Myvt const& l,quaternion const &r);
quaternion operator*(quaternion const &l, quaternion::_Myvt const& r);
#endif // !_QUATERNION_
quaternion.cpp
#include "quaternion.h"
quaternion operator*(quaternion::_Myvt const& l, quaternion const &r) {
return quaternion(l*r.w, l*r.x, l*r.y, l*r.z);
}
quaternion operator*(quaternion const &r, quaternion::_Myvt const& l) {
return quaternion(l*r.w, l*r.x, l*r.y, l*r.z);
}
quaternion& quaternion::operator=(quaternion const& r) {
w = r.w;
x = r.x;
y = r.y;
z = r.z;
return *this;
}
quaternion& quaternion::operator=(quaternion && r) {
w = r.w;
x = r.x;
y = r.y;
z = r.z;
r.w = 0;
r.x = 0;
r.y = 0;
r.z = 0;
return *this;
}
quaternion::_Myvt quaternion::norm_square() {
return w*w + x*x + y*y + z*z;
}
quaternion::vec3 quaternion::to_vector3() {
return vec3(x, y, z);
}
quaternion& quaternion::from_vector3(_Myvt v1, _Myvt v2, _Myvt v3) {
w = 0;
x = v1;
y = v2;
z = v3;
return *this;
}
// Z - Y - X Euler angles
// https://www.cnblogs.com/21207-iHome/p/6894128.html
std::tuple<quaternion::_Myvt, quaternion::_Myvt, quaternion::_Myvt> quaternion::to_euler() {
_Myvt a, b, c;
const _Myvt _eps = 0.0009765625f;
const _Myvt _thres = 0.5f - _eps;
_Myvt _test = w*y - x*z;
if (_test < -_thres || _test > _thres) {// 奇异姿态,俯仰角为±90°
int s = _test < .0 ? -1 : 1;
c = -2 * s * atan2(x, w); // yaw
b = s * (3.1415926535 / 2.0); // pitch
a = 0; // roll
}
else {
a = atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z);
b = asin(-2 * (x*z - w*y));
c = atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z);
}
return std::make_tuple(a, b, c);
}
quaternion& quaternion::from_euler(_Myvt r, _Myvt p, _Myvt y) {
this->w = cos(r)*cos(p)*cos(y) + sin(r)*sin(p)*sin(y);
this->x = sin(r)*cos(p)*cos(y) - cos(r)*sin(p)*sin(y);
this->y = cos(r)*sin(p)*cos(y) + sin(r)*cos(p)*sin(y);
this->z = cos(r)*cos(p)*sin(y) - sin(r)*sin(p)*cos(y);
return *this;
}
quaternion quaternion::conjugate() {
return quaternion(w, -x, -y, -z);
}
quaternion quaternion::inv(void) {
auto a = 1.0 / (x*x + y*y + z*z + w*w);
return quaternion(a*w, a*-x, a*-y, a*-z);
}
auto quaternion::norm(void)->decltype(sqrt(w)) {
return sqrt(x*x + y*y + z*z + w*w);
}
quaternion quaternion::operator*(quaternion const &qr) {
quaternion q;
q.w = w*qr.w - x*qr.x - y*qr.y - z*qr.z;
q.x = w*qr.x + x*qr.w + y*qr.z - z*qr.y;
q.y = w*qr.y - x*qr.z + y*qr.w + z*qr.x;
q.z = w*qr.z + x*qr.y - y*qr.x + z*qr.w;
return q;
}
quaternion quaternion::operator+(quaternion const &r) {
return quaternion(w+r.w,x+r.x, y + r.y, z + r.z);
}
quaternion& quaternion::add(quaternion const& qr) {
x += qr.x;
y += qr.y;
z += qr.z;
w += qr.w;
return *this;
}
quaternion& quaternion::multi(quaternion const& qr) {
quaternion q;
q.w = w*qr.w - x*qr.x - y*qr.y - z*qr.z;
q.x = w*qr.x + x*qr.w + y*qr.z - z*qr.y;
q.y = w*qr.y - x*qr.z + y*qr.w + z*qr.x;
q.z = w*qr.z + x*qr.y - y*qr.x + z*qr.w;
this->x = q.x;
this->y = q.y;
this->z = q.z;
this->w = q.w;
return *this;
}
测试代码
#include <iostream>
using namespace std;
#include "quaternion.h"
int main(int argc, char**argv) {
quaternion q,v,invq;
const float pi = 3.1415926535f;
v.from_vector3(1, 1, 0); // 定义旋转点为 (1，1，0)
const float deg_z = pi / 4.f; // 定义本次旋转为：绕z轴逆时针转动 45°
q.from_euler(0, 0, deg_z*0.5f); // 将绕z轴转动22.5的欧拉角，转换为四元数
invq = q.inv(); // 求出q的逆
v = q*v*invq; // 先用q来旋转v，此时 w 轴偏离了0 ，在三维空间的投影变形，我们再用q^-1使其转回w = 0的位置
auto t = v.to_vector3();
cout << "旋转后坐标，x = " << get<0>(t) << ", y = " << get<1>(t) << ", z = " << get<2>(t) << endl;
return 0;
}
结果：
