点的旋转(4):四元数的乘法

点的旋转(4):四元数的乘法

  • 前言
  • 矢量积
  • 旋转
  • C++代码

前言

同样的,跟前面一眼四元数的乘法也代表了旋转,四元数的积有多种定义,这里我们只讲被用在旋转操作的上积 —— 矢量积

矢量积

对于两个四元数q,p:
q = w 1 + x 1 i + y 1 j + z 1 k q = w_1 + x_1i+y_1j+z_1k q=w1+x1i+y1j+z1k
p = w 2 + x 2 i + y 2 j + z 2 k p = w_2 + x_2i+y_2j+z_2k p=w2+x2i+y2j+z2k
我们来看q右乘p,按照分配律
q p = ( w 1 ∗ w 2 − x 1 ∗ x 2 − y 1 ∗ y 2 − z 1 ∗ z 2 ) + qp = (w_1*w_2-x_1*x_2-y_1*y_2-z_1*z_2) + qp=(w1w2x1x2y1y2z1z2)+
( x 1 ∗ w 2 + w 1 ∗ x 2 + z 1 ∗ y 2 − z 1 ∗ y 2 ) i + (x_1*w_2+w_1*x_2+z_1*y_2-z_1*y_2) i+ (x1w2+w1x2+z1y2z1y2)i+ ( y 1 ∗ w 2 + w 1 ∗ y 2 + z 1 ∗ x 2 − x 1 ∗ z 2 ) j + (y_1*w_2+w_1*y_2+z_1*x_2-x_1*z_2) j+ (y1w2+w1y2+z1x2x1z2)j+ ( z 1 ∗ w 2 + w 1 ∗ z 2 + x 1 ∗ y 2 − x 2 ∗ y 1 ) k + (z_1*w_2+w_1*z_2+x_1*y_2-x_2*y_1) k+ (z1w2+w1z2+x1y2x2y1)k+
按照同样算法算pq结果是等于-qp的
也就是
p q = − q p pq = -qp 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
p = ( 0 , v ) p = (0,v) p=(0,v)
t = q p t = qp t=qp
经过一次旋转后 w t w_t wt 不为0了,这在三维空间看起来已经变形了
为什么呢,因为一次旋转涉及i,j,k三个轴的变化,他们是联动的,
当你从i轴向j轴转动时,k轴向ij向量积的方向转动

你可能会问一个轴怎么转动向他所指的方向转动?
事实上这个轴是投影在三维空间的一个半径为无穷大的圆...

在这里有一个3Blue1Brown的交互性视频,如果你无法理解我的概念可以自己去试着操作一下超球(Hypersphere)
https://eater.net/quaternions/video/quatmult

看,这是旋转之前的投影
点的旋转(4):四元数的乘法_第1张图片
现在我们绕k轴旋转
q 1 = 0.8 + 0.0 i + 0.0 j + 0.6 k q_1 = 0.8 + 0.0i + 0.0j + 0.6k q1=0.8+0.0i+0.0j+0.6k
q 2 = 1 q_2 = 1 q2=1

q 1 p ∗ 1 q_1p*1 q1p1
q 1 q_1 q1模长1,确保四维超球不会变形
我们看
点的旋转(4):四元数的乘法_第2张图片
可以看到确实旋转了,我们来看看k轴的情况
点的旋转(4):四元数的乘法_第3张图片
点的旋转(4):四元数的乘法_第4张图片
球体变形了,他膨胀了!

为什么?我们明明限制了 q 1 q_1 q1的模长1
根据运算结果,w不为0时偏离了我们的三维空间,导致超球在三维的投影变形了

如何去修复他?
我们左乘一个 q 2 q_2 q2,使得其按左手法则,向相反的方向旋转k轴,将超球拉回w=0处
当然,为了不变形超球, q 2 q_2 q2模长 = 1,
我们做稍稍调整, q 2 = ( w , − q 1 . x , − q 1 . y , − q 1 . z ) q_2 = (w,-q_1.x,-q_1.y,-q_1.z) q2=(w,q1.x,q1.y,q1.z)
并用 q 2 q_2 q2左乘p,这样便可以抵消w的偏移
点的旋转(4):四元数的乘法_第5张图片

你可以同时拿出你的右手和左手,让食指从i轴的方向旋至j轴的方向,一个对应q1,另一个对应q2
两根拇指的方向相反,但是食指的转动的方向相同

也就是说:为了调整偏离w = 0的操作我们旋过了两倍 q 1 q_1 q1的操作。
或者说,要旋转 ( α , β , γ ) (α,β,γ) (α,β,γ)欧拉角,我们应该使q1等于其一半
q 1 = e u l e r ( α 2 , β 2 , γ 2 ) q_1 = euler(\frac{α}{2},\frac{β}{2},\frac{γ}{2}) q1=euler(2α2β2γ)
之后 q 1 p q 2 q_1pq_2 q1pq2 即可

至此,四元数的三维空间旋转应用已经结束,下面给出C++代码

C++代码

quaternion.h


// 四元数
// Ayww
// 2018年12月30日15:10:11

#ifndef _QUATERNION_
#define _QUATERNION_
#define _USE_MATH_DEFINES
#include 
#include 
#include 

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 
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;
}

结果:
x = 0 , y = 2 , z = 0 x = 0 , y =\sqrt{2},z = 0 x=0,y=2 z=0
点的旋转(4):四元数的乘法_第6张图片

你可能感兴趣的:(算法)