c++实现 齐次坐标 4*4矩阵 四元数 坐标变换

齐次坐标

关于为什么用齐次坐标（4维）表示三维空间中的点和向量，可以参考什么是齐次坐标系?为什么要用齐次坐标系？，此处不再赘述
只需知道，齐次坐标$(x,y,z,0)$可以表示三维空间向量，$(x,y,z,w)&可以表示空间点&(x/w,y/w,z/w)&，w常取值为1。这些齐次坐标均可以使用下文提到的4*4矩阵进行变换

4*4变换矩阵

参考“为什么DirectX里表示三维坐标要建一个4*4的矩阵？”
矩阵是用于表示变换而不是坐标的。为什么变换要使用一个4×4的矩阵而不是3×3的矩阵呢？

平移

如何平移一个三维空间中的点呢，只需对于点坐标中的每个分量(x,y,z)对应轴上的平移距离相加即可。
例如，点p1(x1,y1,z1)在X轴Y轴以及Z轴上分别平移Δx,Δy,Δz到新的点p2(x2,y2,z2)，那么我们只需在坐标对应的分量上加上这些增量就可以确定点p2的坐标了。

$x2=x1+\Delta x$
$y2=y1+\Delta y$
$z2=z1+\Delta z$

旋转

需要从以下几个方面来描述一个旋转：

1. 旋转轴
2. 旋转方向
3. 旋转角度

在这里，我们假设点p需要绕Z轴顺时针旋转β度。

如图所示，我们的点P1(x1,y1,z1)以Z轴位轴顺时针旋转β度之后来到了点P2(x2,y2,z2)。那么根据三角函数公式（推导过程见上文链接）可以计算出P1点的具体坐标了
$x2=\cos \beta \cdot x1+\sin \beta \cdot y1$
$y2=\cos \beta \cdot y1-\sin \beta \cdot x1$
$z2=z1$
类似的，绕x轴逆时针旋转θ角：
$x2=x1$
$y2=\cos \theta \cdot y1-\sin \theta \dot{z1}$
$z2=\sin \theta \cdot y2+\cos \theta \dot{z1}$
绕y轴逆时针旋转θ角：
$x2=\cos \theta \cdot x1+\sin \theta \cdot z1$
$y2=y1$
$z2=-\sin \theta \cdot x1+\cos \theta \cdot z1$
绕z轴逆时针旋转θ角：
$x2=\cos \theta \cdot x1-\sin \theta \cdot y1$
$y2=\sin \theta \cdot x1+\cos \theta \cdot y1$
$z2=z1$

3×3矩阵和旋转

在现实中常常使用矩阵（由m × n个标量组成的长方形数组）来表示诸如平移、旋转以及缩放等线性变换。当两个变换矩阵A和B的积为P=AB时，则变换矩阵P相当于A和B所代表的变换。举一个例子，若A为旋转矩阵，B为平移矩阵，则矩阵P就能够实现旋转和平移变换。不过需要注意的是，矩阵乘法不符合交换律，因此AB和BA并不相等。
上文提到的绕z轴逆时针旋转θ角的变换，可以以矩阵形式描述为:
$\left[\begin{array}{c}x2\\ y2\\ z2\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x1\\ y1\\ z1\end{array}\right]$
相似的，绕x轴逆时针旋转θ角矩阵:
$\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$
绕y轴逆时针旋转θ角矩阵:
$\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$

4×4矩阵，引入平移

3×3矩阵解决了旋转变换的表示问题，但是对于下面所示的，旋转和平移的复合，却展现了局限性
$x2=a\cdot x1+b\cdot y1+c\cdot z1$
$x2=x1+\Delta x$
$y2=d\cdot x1+e\cdot y1+f\cdot z1$
$y2=y1+\Delta y$
$z2=g\cdot x1+h\cdot y1+i\cdot z1$
$z2=z1+\Delta z$
平移和旋转之间很重要的一个区别，那就是平移的表达式中带有常量Δx，而无论是旋转的表达式还是矩阵等式中都不存在这样一个常量能够与之对应。解决这个问题的方法，就是用4×4矩阵来实现。

4×4矩阵与齐次坐标

三维矢量添加了第四个分量，这样之前的三维矢量(x,y,z)就变成了四维的(x,y,z,w)，这样由4个分量组成的矢量便被称为齐次坐标。需要说明的是，齐次坐标(x,y,z,w)等价于三维坐标(x/w,y/w,z/w)，因此只要w分量的值是1，那么这个齐次坐标就可以被当作三维坐标来使用，而且所表示的坐标就是以x，y，z这3个值为坐标值的点。
因此，为了和4×4矩阵相乘，我们的P1点坐标就变成了(x1,y1,z1,1)。而矩阵等式也变成了下面这个样子：
$\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right]=\left[\begin{array}{cccc}a& b& c& d\\ e& f& g& h\\ i& j& k& l\\ m& n& o& p\end{array}\right]\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]$
其中，4×4矩阵的左上角3×3部分等同于上文的3×3矩阵，d、h、l代表x、y、z方向上的平移，m、n、o一般为0，p为1。这个4*4矩阵代表旋转和平移的复合（先旋转后平移）。
一个4×4的平移矩阵如下所示:
$\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& \Delta x\\ 0& 1& 0& \Delta y\\ 0& 0& 1& \Delta z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]$

c++实现

//point3D.h
class Point3D
{
public:
	double x, y, z;
	Point3D();
	Point3D(double x, double y, double z)
	{
		this->x = x;
		this->y = y;
		this->z = z;
	}
};

#pragma once
//matrix.h
#include "Point3D.h"
class Matrix
{
public:
	Matrix();
	Matrix(int m[16]);
	~Matrix();
	Point3D Mutiply(Point3D ptVec);
private:
	double p[4][4];
};

//matrix.cpp
#include "Matrix.h"
Matrix::Matrix()
{}

Matrix::Matrix(int m[16])
{
	int y, x, i = 0;
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			p[y][x] = m[i];
		}
	}
}

Matrix::~Matrix()
{}

Point3D Matrix::Mutiply(Point3D ptVec)
{
	Point3D ptRes;
	ptRes.x = p[0][0] * ptVec.x + p[0][1] * ptVec.y + p[0][2] * ptVec.z + p[0][3];
	ptRes.y = p[1][0] * ptVec.x + p[1][1] + ptVec.y + p[1][2] + ptVec.z + p[1][3];
	ptRes.z = p[2][0] * ptVec.x + p[2][1] * ptVec.y + p[2][2] * ptVec.z + p[2][3];
	return ptRes;
}

四元数

四元数的定义

四元数(quaternion)可以看作中学时学的复数的扩充，它有三个虚部。形式如下：
$\vec{q}=w+xi+yj+zk$
可以写成：
$\vec{q}=\vec{s}+\vec{v}$
以矩阵形式记录其四个分量：
$q={\left[\begin{array}{cccc}w& x& y& z\end{array}\right]}^T$
这四个分量满足：
$|q{|}^2=w^2+x^2+y^2+z^2=1$

四元数的基本运算

参考四元数和旋转-知乎，默认讨论右手系。

乘法运算

设$\overrightarrow{q_1}=s_1+\overrightarrow{v_1}$，$\overrightarrow{q_2}=s_2+\overrightarrow{v_2}$则:
$$\begin{gathered} \overrightarrow{q_1}\overrightarrow{q_2}=\left[\begin{array}{c}w1\\ x1\\ y1\\ z1\end{array}\right]\ast \left[\begin{array}{c}w2\\ x2\\ y2\\ z2\end{array}\right] \\ =\left[\begin{array}{c}w1w2-x1x2-y1y2-z1z2\\ x1w2+w1x2+y1z2-z1y2\\ y1w2+w1y2+z1x2-x1z2\\ z1w2+w1z2+x1y2-y1x2\end{array}\right] \\ =s_1{s}_2-\overrightarrow{v_1}\overrightarrow{v_2}+s_1\overrightarrow{v_2}+s_2\overrightarrow{v_1}+\overrightarrow{v_1}\times \overrightarrow{v_2} \end{gathered}$$
注意，此处讨论的是四元数乘法，不同于向量点乘或叉乘。

共轭四元数

一个四元数$\vec{q}=s+\vec{v}$的共轭(用$\overline{\stackrel{⃗}{q}}$表示)为$\overline{\stackrel{⃗}{q}}=s-\vec{v}$
一个四元数和它的共轭的积等于该四元数与自身的点乘，也等于该四元数长度的平方。即：
$\vec{q}\overline{\stackrel{⃗}{q}}=\overline{\stackrel{⃗}{q}}\vec{q}=\vec{q}\cdot \vec{q}=||\vec{q}|{|}^2=q^2$

四元数的逆

一个非零四元数$\vec{q}$的逆为${\vec{q}}^{-1}=\frac{\overline{\stackrel{⃗}{q}}}{{\vec{q}}^2}$。显然$\vec{q}{\vec{q}}^{-1}=1$

四元数表示旋转

三维空间中的旋转可以被认为是一个函数$\phi$,从${\mathbb{R}}^3$到自身的映射。函数$\phi$表示的一个旋转，旋转过程中保持向量长度(lengths)、向量夹角(angles)和handedness不变。
(handedness和左右手坐标系有关，例如左手坐标系中向量旋转后，仍要符合左手坐标系规则)
由下面公式给出的函数，可以表示旋转：
${\phi }_q(\vec{P})=\vec{q}\vec{P}{\vec{q}}^{-1}$
这里$\vec{q}$是一个非零四元数，函数的参数$\vec{P}$可以看作三维空间中的点(即一个实部或标量部分为0的四元数)。

由旋转到四元数

可以找到一个单位四元数$\vec{q}$，对应于绕旋转轴$\vec{A}$旋转$\theta$角度的旋转变换：
$$\begin{gathered} \vec{q}=s+\vec{v} \\ =s+t\vec{A} \\ =\cos \frac{\theta }{2}+\vec{A}\sin \frac{\theta }{2} \end{gathered}$$
其中，$\vec{A}$是单位向量
此外，对于任意非零$a$(如$a$=-1),$a\vec{q}$和$\vec{q}$表示同一个旋转，证明见上文链接。
两个四元数$\overrightarrow{q1}$和$\overrightarrow{q2}$的乘积也表示一个旋转。例如，$\overrightarrow{q1}\overrightarrow{q2}$ 表示施加旋转 $\overrightarrow{q2}$，再施加旋转$\overrightarrow{q1}$ 。因为:
$\overrightarrow{q1}(\overrightarrow{q2}\vec{P}{\overrightarrow{q2}}^{-1})=(\overrightarrow{q1}\overrightarrow{q2})\vec{P}(\overrightarrow{q1}\overrightarrow{q2}{)}^{-1}$
总之，通过一个四元数 $\vec{q}$ 对一个三维点 $\vec{P}$(把 $\vec{P}$ 当成一个实部为0的四元数)施加旋转变换，只需要做如下计算:
$\overrightarrow{P^{\prime }}=\vec{q}\vec{P}{\vec{q}}^{-1}$

四元数到4*4矩阵

对于单位四元数$\vec{q}$，即满足$w^2+x^2+y^2+z^2=1$，其3×3旋转矩阵为:
$\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy-2wz& 2xz+2wy\\ 2xy+2wz& 1-2x^2-2z^2& 2yz-2wx\\ 2xz-2wy& 2yz+2wx& 1-2x^2-2y^2\end{array}\right]$
加上上文提到的，与三维空间中的平移相复合(先旋转 后平移)，则4×4变换矩阵为：
$\left[\begin{array}{cccc}1-2y^2-2z^2& 2xy-2wz& 2xz+2wy& \Delta x\\ 2xy+2wz& 1-2x^2-2z^2& 2yz-2wx& \Delta y\\ 2xz-2wy& 2yz+2wx& 1-2x^2-2y^2& \Delta z\\ 0& 0& 0& 1\end{array}\right]$

齐次坐标与4*4矩阵的c++实现

构造了齐次坐标类Coordinate，4*4变换矩阵类Matrix。其中，求逆矩阵的方法为伴随矩阵求逆矩阵。矩阵求逆的验算可以使用这个在线平台云算网人算不如云算。下文代码已经测试通过。

//Main.cpp
#include "Matrix.h"

int main()
{
	double v1[16] = { 2,3,5,7,11,13,17,19,23,29,31,37,41,43,47,53 };
	Matrix m1, m2, m3;
	m1.SetValue(v1);
	if (m1.GetDeterminant() == 0)	//计算矩阵行列式，从而验证矩阵可逆
		return 1;
	m2 = m1.GetInverse();		//m2为逆矩阵
	m3 = m1.GetMultiply(m2);	//m3为单位阵
	return 0;
}

#pragma once
//Coordinate.h
//齐次坐标类
//w=0，则表示向量(x,y,z)
//w!=0,则表示点(x/w,y/w,z/w)。w一般取1

#include <math.h>

class Coordinate
{
public:
	double x;
	double y;
	double z;
	double w;

	Coordinate();
	Coordinate(double x, double y, double z, double w);
	~Coordinate();
	void Normalize();	//将点的w设为1 对向量不作变换
	double GetDotProduct(Coordinate c);	//向量点乘
	Coordinate GetCrossProduct(Coordinate c);	//向量叉乘

private:
};

//Coordinate.cpp
#include "Coordinate.h"
Coordinate::Coordinate()
{
	this->x = 0;
	this->y = 0;
	this->z = 0;
	this->w = 0;
}

Coordinate::Coordinate(double x, double y, double z, double w)
{
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

void Coordinate::Normalize()
{
	if (this->w == 0)
		return;
	this->x /= this->w;
	this->y /= this->w;
	this->z /= this->w;
	this->w = 1;
	return;
}

Coordinate::~Coordinate()
{}

double Coordinate::GetDotProduct(Coordinate c)
{
	double result;
	result = this->x*c.x + this->y*c.y + this->z*c.z;
	return result;
}

Coordinate Coordinate::GetCrossProduct(Coordinate c)
{
	Coordinate result;
	result.x = this->y*c.z - this->z*c.y;
	result.y = this->z*c.x - this->x*c.z;
	result.z = this->x*c.y - this->y*c.x;
	result.w = 0;
	return result;
}

#pragma once
//matrix.h
//矩阵运算类，以4×4矩阵形式表示空间变换
//旋转、平移运算的复合，先旋转、后平移
//对4维齐次坐标进行运算

//#include "Point3D.h"
#include "Coordinate.h"

class Matrix
{
public:
	Matrix();
	Matrix(double m[16]);	//矩阵初始化(先行后列)
	Matrix(Coordinate c0, Coordinate c1, Coordinate c2, Coordinate c3);	//四个列向量构造一个矩阵
	void SetValue(double m[16]);	//设置矩阵值(先行后列)
	void SetValue(Coordinate c0, Coordinate c1, Coordinate c2, Coordinate c3);//四个列向量为矩阵赋值
	~Matrix();
	Coordinate GetMultiply(Coordinate c);	//this矩阵×齐次坐标
	Matrix GetMultiply(Matrix m);	//this矩阵×矩阵
	Matrix GetInverse();	//求逆矩阵
	double GetDeterminant();	//求this矩阵行列式
	Matrix GetTranspose();		//求this矩阵的转置矩阵
private:
	double m_dItem[4][4];
	double GetMinor(int y, int x);	//求this矩阵在y,x处的余子式
	double GetCofactor(int y, int x);	//求this矩阵在y,x处的代数余子式
};

//matrix.cpp
#include "Matrix.h"
Matrix::Matrix()
{
	int y, x, i = 0;
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			m_dItem[y][x] = 0;
		}
	}
}

Matrix::Matrix(double m[16])
{
	int y, x, i = 0;
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			m_dItem[y][x] = m[i];
		}
	}
}

Matrix::Matrix(Coordinate c0, Coordinate c1, Coordinate c2, Coordinate c3)
{
	m_dItem[0][0] = c0.x;
	m_dItem[0][1] = c1.x;
	m_dItem[0][2] = c2.x;
	m_dItem[0][3] = c3.x;
	m_dItem[1][0] = c0.y;
	m_dItem[1][1] = c1.y;
	m_dItem[1][2] = c2.y;
	m_dItem[1][3] = c3.y;
	m_dItem[2][0] = c0.z;
	m_dItem[2][1] = c1.z;
	m_dItem[2][2] = c2.z;
	m_dItem[2][3] = c3.z;
	m_dItem[3][0] = c0.w;
	m_dItem[3][1] = c1.w;
	m_dItem[3][2] = c2.w;
	m_dItem[3][3] = c3.w;
	return;
}

Matrix::~Matrix()
{}

void Matrix::SetValue(double m[16])
{
	int y, x, i = 0;
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			m_dItem[y][x] = m[i];
			i++;
		}
	}
	return;
}

void Matrix::SetValue(Coordinate c0, Coordinate c1, Coordinate c2, Coordinate c3)
{
	m_dItem[0][0] = c0.x;
	m_dItem[0][1] = c1.x;
	m_dItem[0][2] = c2.x;
	m_dItem[0][3] = c3.x;
	m_dItem[1][0] = c0.y;
	m_dItem[1][1] = c1.y;
	m_dItem[1][2] = c2.y;
	m_dItem[1][3] = c3.y;
	m_dItem[2][0] = c0.z;
	m_dItem[2][1] = c1.z;
	m_dItem[2][2] = c2.z;
	m_dItem[2][3] = c3.z;
	m_dItem[3][0] = c0.w;
	m_dItem[3][1] = c1.w;
	m_dItem[3][2] = c2.w;
	m_dItem[3][3] = c3.w;
	return;
}

Coordinate Matrix::GetMultiply(Coordinate c)
{
	Coordinate result;
	result.x = m_dItem[0][0] * c.x + m_dItem[0][1] * c.y + m_dItem[0][2] * c.z + m_dItem[0][3] * c.w;
	result.y = m_dItem[1][0] * c.x + m_dItem[1][1] * c.y + m_dItem[1][2] * c.z + m_dItem[1][3] * c.w;
	result.z = m_dItem[2][0] * c.x + m_dItem[2][1] * c.y + m_dItem[2][2] * c.z + m_dItem[2][3] * c.w;
	result.w = m_dItem[3][0] * c.x + m_dItem[3][1] * c.y + m_dItem[3][2] * c.z + m_dItem[3][3] * c.w;
	return result;
}

Matrix Matrix::GetMultiply(Matrix m)
{
	int y, x;
	Matrix result;
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			result.m_dItem[y][x] = this->m_dItem[y][0] * m.m_dItem[0][x] + this->m_dItem[y][1] * m.m_dItem[1][x]
				+ this->m_dItem[y][2] * m.m_dItem[2][x] + this->m_dItem[y][3] * m.m_dItem[3][x];
		}
	}
	return result;
}

Matrix Matrix::GetInverse()
{
	int y, x;
	double det;	//矩阵行列式
	Matrix result;

	//求矩阵行列式
	det = this->GetDeterminant();
	if (det == 0)
		return result;
	//求每个位置的代数余子式
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			result.m_dItem[y][x] = this->GetCofactor(y, x);
		}
	}
	//求伴随矩阵
	result = result.GetTranspose();
	//最终结果
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			result.m_dItem[y][x] /= det;
		}
	}

	return result;
}

double Matrix::GetDeterminant()
{
	double result;
	result = m_dItem[0][0] * GetCofactor(0, 0) + m_dItem[0][1] * GetCofactor(0, 1) + m_dItem[0][2] * GetCofactor(0, 2) + m_dItem[0][3] * GetCofactor(0, 3);
	return result;
}

Matrix Matrix::GetTranspose()
{
	int y, x;
	Matrix result;
	for (y = 0; y < 4; y++)
	{
		for (x = 0; x < 4; x++)
		{
			result.m_dItem[y][x] = m_dItem[x][y];
		}
	}
	return result;
}

double Matrix::GetMinor(int y, int x)
{
	int i, n, col[3], row[3];	//col[3]记录行列式对应哪几列，row[3]记录行列式对应的哪几行
	double result;
	for (i = 0, n = 0; i < 4; i++)
	{
		if (i == x)
			continue;
		col[n++] = i;
	}
	for (i = 0, n = 0; i < 4; i++)
	{
		if (i == y)
			continue;
		row[n++] = i;
	}
	result = m_dItem[row[0]][col[0]] * (m_dItem[row[1]][col[1]] * m_dItem[row[2]][col[2]] - m_dItem[row[1]][col[2]] * m_dItem[row[2]][col[1]])
		- m_dItem[row[0]][col[1]] * (m_dItem[row[1]][col[0]] * m_dItem[row[2]][col[2]] - m_dItem[row[1]][col[2]] * m_dItem[row[2]][col[0]])
		+ m_dItem[row[0]][col[2]] * (m_dItem[row[1]][col[0]] * m_dItem[row[2]][col[1]] - m_dItem[row[1]][col[1]] * m_dItem[row[2]][col[0]]);
	return result;
}

double Matrix::GetCofactor(int y, int x)
{
	double result;
	result = pow(-1, y + x)*GetMinor(y, x);
	return result;
}