📄 xmath.h
字号:
{
r.x=v.x*m(0,0)+v.y*m(1,0)+m(2,0);
r.y=v.x*m(0,1)+v.y*m(1,1)+m(2,1);
}
// 3D向量和4D齐次矩阵相乘
template<class T>
void vec_mul_mat(const CVector<T,3> &v, const CMatrix<T,4,3> &m, CVector<T,3> &r)
{
// 假设m的最后一列为[0,0,0,1]T，并使用4X3矩阵表示
r.x=v.x*m(0,0)+v.y*m(1,0)+v.z*m(2,0)+m(3,0);
r.y=v.x*m(0,1)+v.y*m(1,1)+v.z*m(2,1)+m(3,1);
r.z=v.x*m(0,2)+v.y*m(1,2)+v.z*m(2,2)+m(3,2);
}
template<class T>
void vec_mul_mat(const CVector<T,3> &v, const CMatrix<T,4,4> &m, CVector<T,3> &r)
{
r.x=v.x*m(0,0)+v.y*m(1,0)+v.z*m(2,0)+m(3,0);
r.y=v.x*m(0,1)+v.y*m(1,1)+v.z*m(2,1)+m(3,1);
r.z=v.x*m(0,2)+v.y*m(1,2)+v.z*m(2,2)+m(3,2);
}
///////////////////////////////////////////////////////////////////////////////////
//
// 四元数
//
template<class T>
class CQuaternion
{
//
// 数据
//
public:
union
{
T M[4];
struct
{
T qa;
CVector<T,3> qv;
};
struct
{
T w,x,y,z;
};
};
//
// 构造/析构函数
//
public:
CQuaternion () {}
~CQuaternion () {}
// COPY构造函数
CQuaternion (const CQuaternion<T> &q)
{
*this=q;
}
CQuaternion& operator = (const CQuaternion<T> &q)
{
qa=q.qa;
qv=q.qv;
return *this;
}
// 根据旋转向量和旋转角构建四元数
CQuaternion (const CVector<T,3> &v, T theta)
{
FromVectorAngle(theta,v);
}
// 与3D向量间的转换
CQuaternion (const CVector<T,3> &v):
qa(0), qv(v)
{}
inline CQuaternion& operator = (const CVector<T,3> &v)
{
qa = 0;
qv = v;
return *this;
}
//
// 外部接口
//
public:
//
// 四元数基本运算
//
inline CQuaternion& operator += (const CQuaternion &q)
{
return Add(q);
}
inline CQuaternion& operator -= (const CQuaternion &q)
{
return Sub(q);
}
// 四元数加法
inline CQuaternion& Add (const CQuaternion &q)
{
w += q.w;
x += q.x;
y += q.y;
z += q.z;
return *this;
}
inline CQuaternion& Add (const CQuaternion &q1, const CQuaternion &q2)
{
w = q1.w + q2.w;
x = q1.x + q2.x;
y = q1.y + q2.y;
z = q1.z + q2.z;
return *this;
}
// 四元数减法
inline CQuaternion& Sub (const CQuaternion &q)
{
w -= q.w;
x -= q.x;
y -= q.y;
z -= q.z;
return *this;
}
inline CQuaternion& Sub (const CQuaternion &q1, const CQuaternion &q2)
{
w = q1.w - q2.w;
x = q1.x - q2.x;
y = q1.y - q2.y;
z = q1.z - q2.z;
return *this;
}
// 标量乘法
inline CQuaternion& Scale (T val)
{
w *= val;
x *= val;
y *= val;
z *= val;
return *this;
}
// 点积
inline T Dot (const CQuaternion &q)
{
return (w*q.w + x*q.x + y*q.y + z*q.z);
}
// 四元数乘法，this=q1*q2
CQuaternion& Mul (const CQuaternion &q1, const CQuaternion &q2)
{
//
// 公式：[ w1*w2 - (v1 dot v2), w1*v2 + w2*v1 + (v1 cross v2) ]
//
// 直接计算，16次乘法，12次加法
/*
w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x;
z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w;
*/
// 通过提取公共因子的改进算法，9次乘法，27次加法
T a0 = (q1.z - q1.y) * (q2.y - q2.z);
T a1 = (q1.w + q1.x) * (q2.w + q2.x);
T a2 = (q1.w - q1.x) * (q2.y + q2.z);
T a3 = (q1.y + q1.z) * (q2.w - q2.x);
T a4 = (q1.z - q1.x) * (q2.x - q2.y);
T a5 = (q1.z + q1.x) * (q2.x + q2.y);
T a6 = (q1.w + q1.y) * (q2.w - q2.z);
T a7 = (q1.w - q1.y) * (q2.w + q2.z);
T a8 = a5 + a6 + a7;
T a9 = T(0.5 * (a4 + a8));
w = a0 + a9 - a5;
x = a1 + a9 - a8;
y = a2 + a9 - a7;
z = a3 + a9 - a6;
return *this;
}
// 求q1到q2的角度变换，结果保存在this中
CQuaternion& Difference (const CQuaternion &q1, const CQuaternion &q2)
{
CQuaternion tmp=q1;
tmp.Inverse();
Mul(tmp,q2);
return *this;
}
// 四元数的值
inline T Magnitude () const
{
return (sqrt(Magnitude2()));
}
// 值的平方
inline T Magnitude2 () const
{
return (w*w + x*x + y*y + z*z);
}
// 将this设为它的共轭
inline CQuaternion& Conjugate ()
{
x = -x;
y = -y;
z = -z;
return *this;
}
// 将this设为它的逆
CQuaternion& Inverse ()
{
T inv = Magnitude();
inv = 1/inv;
Conjugate().Scale(inv);
return *this;
}
// 单位四元数的逆
inline CQuaternion& InverseUnit()
{
Conjugate();
}
// log(this)
CQuaternion& Log (const CQuaternion &q)
{
T a = acos(q.w);
w = 0;
x = y = z = a;
return *this;
}
// e^this
CQuaternion& Exp (const CQuaternion &q)
{
T a = q.x;
w = cos(a);
x = y = z = sin(a);
return *this;
}
// this^(t)
inline CQuaternion& Power (T t)
{
return (Log(*this).Scale(t).Exp(*this));
}
// 根据转轴和转角构造四元数
// 参数normalized指出传入的向量是否单位向量
void FromVectorAngle (const CVector<T,3> &v, T angle, bool normalized = true)
{
angle /= 2;
T sina;
if(!normalized)
{
// 如果不是单位向量，先要将其规整为单位向量
T len = v.Length();
len = 1/len;
sina = sin(angle) * len;
}
else
{
sina = sin(angle);
}
w = cos(angle);
x = v.x * sina;
y = v.y * sina;
z = v.z * sina;
}
// 从四元数中提取转轴和转角
void ToVectorAngle (CVector<T,3> &v, T &angle) const
{
angle = acos(w);
T inv_sina = sin(angle);
if(inv_sina == 0)
{
v.x = v.y = v.z = 0;
}
else
{
inv_sina = 1/inv_sina;
v.x = x * inv_sina;
v.y = y * inv_sina;
v.z = z * inv_sina;
}
angle *= 2;
}
};
typedef CQuaternion<int> QUATi;
typedef CQuaternion<float> QUATf;
typedef CQuaternion<double> QUATd;
//
// 全局运算符重载
//
template<class T>
inline CQuaternion<T> operator + (const CQuaternion<T> &q1, const CQuaternion<T> &q2)
{
CQuaternion q;
return q.Add(q1,q2);
}
template<class T>
inline CQuaternion<T> operator - (const CQuaternion<T> &q1, const CQuaternion<T> &q2)
{
CQuaternion q;
return q.Sub(q1,q2);
}
template<class T>
inline CQuaternion<T> operator * (const CQuaternion<T> &q1, const CQuaternion<T> &q2)
{
CQuaternion q;
return q.Mul(q1,q2);
}
template<class T>
inline CQuaternion<T> operator / (const CQuaternion<T> &q1, const CQuaternion<T> &q2)
{
CQuaternion q;
return q.Difference(q1,q2);
}
///////////////////////////////////////////////////////////////////////////////
//
// 角度变换函数
//
//
// 欧拉角
//
template<class T>
struct CEuler
{
union
{
T M[3];
struct
{
T x, y, z;
};
struct
{
T pitch, yaw, roll;
};
};
};
typedef CEuler<int> EULERi;
typedef CEuler<float> EULERf;
typedef CEuler<double> EULERd;
// 涉及欧拉角的转换中，均默认使用roll-pitch-yaw（Z-Y-X）的旋转顺序
//
// 欧拉角和旋转矩阵之间的转换
//
template<class T>
void EulerToMatrix3D (const CEuler<T> &euler, CMatrix<T, 3, 3> &mat)
{
T sinr = sin(euler.roll);
T sinp = sin(euler.pitch);
T siny = sin(euler.yaw);
T cosr = cos(euler.roll);
T cosp = cos(euler.pitch);
T cosy = cos(euler.yaw);
T cosy_cosr = cosy * cosr;
T cosy_sinr = cosy * sinr;
T siny_cosr = siny * cosr;
T siny_sinr = siny * sinr;
mat(0,0) = cosy_cosr + siny_sinr * sinp;
mat(0,1) = sinr * cosp;
mat(0,2) = cosy_sinr * sinp - siny_cosr;
mat(1,0) = siny_cosr * sinp - cosy_sinr;
mat(1,1) = cosr * cosp;
mat(1,2) = siny_sinr + cosy_cosr * sinp;
mat(2,0) = siny * cosp;
mat(2,1) = - sinp;
mat(2,2) = cosy * cosp;
}
template<class T>
void Matrix3DToEuler (const CMatrix<T, 3, 3> &mat, CEuler<T> &euler)
{
// 由于浮点数存在波动，需要作一些处理
T M21 = mat(2,1);
if (M21>=1.0)
euler.pitch = PI_DIV_2;
else if (M21<=-1.0)
euler.pitch = -PI_DIV_2;
else
euler.pitch = asin(-mat(2,1));
if (M21>0.99999)
{
// gimbal lock
euler.roll = 0;
euler.yaw = asin(-mat(0,2));
}
else
{
euler.roll = atan2(mat(0,1), mat(1,1));
euler.yaw = atan2(mat(2,0), mat(2,2));
}
}
template<class T>
void EulerToMatrix4D (const CEuler<T> &euler, CMatrix<T, 4, 4> &mat)
{
T sinr = sin(euler.roll);
T sinp = sin(euler.pitch);
T siny = sin(euler.yaw);
T cosr = cos(euler.roll);
T cosp = cos(euler.pitch);
T cosy = cos(euler.yaw);
T cosy_cosr = cosy * cosr;
T cosy_sinr = cosy * sinr;
T siny_cosr = siny * cosr;
T siny_sinr = siny * sinr;
mat(0,0) = cosy_cosr + siny_sinr * sinp;
mat(0,1) = sinr * cosp;
mat(0,2) = cosy_sinr * sinp - siny_cosr;
mat(1,0) = siny_cosr * sinp - cosy_sinr;
mat(1,1) = cosr * cosp;
mat(1,2) = siny_sinr + cosy_cosr * sinp;
mat(2,0) = siny * cosp;
mat(2,1) = - sinp;
mat(2,2) = cosy * cosp;
mat(3,3) = 1;
mat(3,0) = mat(3,1) = mat(3,2) = mat(0,3) = mat(1,3) = mat(2,3) = 0;
}
template<class T>
void Matrix4DToEuler (const CMatrix<T, 4, 4> &mat, CEuler<T> &euler)
{
// 处理浮点数可能出现的波动
T M21 = mat(2,1);
if (M21>=1.0)
euler.pitch = PI_DIV_2;
else if (M21<=-1.0)
euler.pitch = -PI_DIV_2;
else
euler.pitch = asin(-mat(2,1));
if (M21>0.99999)
{
// gimbal lock
euler.roll = 0;
euler.yaw = asin(-mat(0,2));
}
else
{
euler.roll = atan2(mat(0,1), mat(1,1));
euler.yaw = atan2(mat(2,0), mat(2,2));
}
}
//
// 四元数和旋转矩阵之间的转换
//
template<class T>
void QuatToMatrix3D (const CQuaternion<T> &quat, CMatrix<T, 3, 3> &mat)
{
// 使用临时变量来减少乘法
T xx = quat.x * quat.x;
T yy = quat.y * quat.y;
T zz = quat.z * quat.z;
T ww = quat.w * quat.w;
T xy = quat.x * quat.y;
T xz = quat.x * quat.z;
T xw = quat.x * quat.w;
T yz = quat.y * quat.z;
T yw = quat.y * quat.w;
T zw = quat.z * quat.w;
mat(0,0) = 1 - 2 * (yy+zz);
mat(1,1) = 1 - 2 * (xx+zz);
mat(2,2) = 1 - 2 * (xx+yy);
mat(0,1) = 2 * (xy+zw);
mat(0,2) = 2 * (xz-yw);
mat(1,0) = 2 * (xy-zw);
mat(1,2) = 2 * (yz+xw);
mat(2,0) = 2 * (yw+xz);
mat(2,1) = 2 * (yz-xw);
}
template<class T>
void Matrix3DToQuat (const CMatrix<T, 3, 3> &mat, CQuaternion<T> &quat)
{
// 确定最大的分量
T max_w = mat(0,0) + mat(1,1) + mat(2,2);
T max_x = mat(0,0) - mat(1,1) - mat(2,2);
T max_y = mat(1,1) - mat(2,2) - mat(0,0);
T max_z = mat(2,2) - mat(0,0) - mat(1,1);
int id=0;
T max=max_w;
if(max_x>max)
{
max=max_x;
id=1;
}
if(max_y>max)
{
max=max_y;
id=2;
}
if(max_z>max)
{
max=max_z;
id=3;
}
// 计算四元数的值
max = T(sqrt(max+1)*0.5);
T mult = T(0.25/max);
switch(id)
{
case 0:
quat.w = max;
quat.x = (mat(1,2) - mat(2,1)) * mult;
quat.y = (mat(2,0) - mat(0,2)) * mult;
quat.z = (mat(0,1) - mat(1,0)) * mult;
break;
case 1:
quat.x = max;
quat.w = (mat(1,2) - mat(2,1)) * mult;
quat.y = (mat(0,1) + mat(1,0)) * mult;
quat.z = (mat(2,0) + mat(0,2)) * mult;
break;
case 2:
quat.y = max;
quat.w = (mat(2,0) - mat(0,2)) * mult;
quat.x = (mat(0,1) + mat(1,0)) * mult;
quat.z = (mat(1,2) + mat(2,1)) * mult;
break;
case 3:
quat.z = max;
quat.w = (mat(0,1) - mat(1,0)) * mult;
quat.x = (mat(2,0) + mat(0,2)) * mult;
quat.y = (mat(1,2) + mat(2,1)) * mult;
break;
}
}
template<class T>
void QuatToMatrix4D (const CQuaternion<T> &quat, CMatrix<T, 4, 4> &mat)
{
T xx = quat.x * quat.x;
T yy = quat.y * quat.y;
T zz = quat.z * quat.z;
T ww = quat.w * quat.w;
T xy = quat.x * quat.y;
T xz = quat.x * quat.z;
T xw = quat.x * quat.w;
T yz = quat.y * quat.z;
T yw = quat.y * quat.w;
T zw = quat.z * quat.w;
mat(0,0) = 1 - 2 * (yy+zz);
mat(1,1) = 1 - 2 * (xx+zz);
mat(2,2) = 1 - 2 * (xx+yy);
mat(0,1) = 2 * (xy+zw);
mat(0,2) = 2 * (xz-yw);
mat(1,0) = 2 * (xy-zw);
mat(1,2) = 2 * (yz+xw);
mat(2,0) = 2 * (yw+xz);
mat(2,1) = 2 * (yz-xw);
mat(3,3) = 1;
mat(3,0) = mat(3,1) = mat(3,2) = mat(0,3) = mat(1,3) = mat(2,3) = 0;
}
template<class T>
void Matrix4DToQuat (const CMatrix<T, 4, 4> &mat, CQuaternion<T> &quat)
{
// 确定最大的分量
T max_w = mat(0,0) + mat(1,1) + mat(2,2);
T max_x = mat(0,0) - mat(1,1) - mat(2,2);
T max_y = mat(1,1) - mat(2,2) - mat(0,0);
T max_z = mat(2,2) - mat(0,0) - mat(1,1);
int id=0;
T max=max_w;
if(max_x>max)
{
max=max_x;
id=1;
}
if(max_y>max)
{
max=max_y;
id=2;
}
if(max_z>max)
{
max=max_z;
id=3;
}
// 计算四元数的值
max = T(sqrt(max+1)*0.5);
T mult = T(0.25/max);
switch(id)
{
case 0:
quat.w = max;
quat.x = (mat(1,2) - mat(2,1)) * mult;
quat.y = (mat(2,0) - mat(0,2)) * mult;
quat.z = (mat(0,1) - mat(1,0)) * mult;
break;
case 1:
quat.x = max;
quat.w = (mat(1,2) - mat(2,1)) * mult;
quat.y = (mat(0,1) + mat(1,0)) * mult;
quat.z = (mat(2,0) + mat(0,2)) * mult;
break;
case 2:
quat.y = max;
quat.w = (mat(2,0) - mat(0,2)) * mult;
quat.x = (mat(0,1) + mat(1,0)) * mult;
quat.z = (mat(1,2) + mat(2,1)) * mult;
break;
case 3:
quat.z = max;
quat.w = (mat(0,1) - mat(1,0)) * mult;
quat.x = (mat(2,0) + mat(0,2)) * mult;
quat.y = (mat(1,2) + mat(2,1)) * mult;
break;
}
}
//
// 欧拉角和四元数之间的转换
//
template<class T>
void EulerToQuat (const CEuler<T> &euler, CQuaternion<T> &quat)
{
T r = T(euler.roll * 0.5);
T p = T(euler.pitch * 0.5);
T y = T(euler.yaw * 0.5);
float cosr = cos(r);
float cosp = cos(p);
float cosy = cos(y);
float sinr = sin(r);
float sinp = sin(p);
float siny = sin(y);
quat.w = cosy*cosp*cosr + siny*sinp*sinr;
quat.x = cosy*sinp*cosr + siny*cosp*sinr;
quat.y = siny*cosp*cosr - cosy*sinp*sinr;
quat.z = cosy*cosp*cosr - siny*sinp*cosr;
}
template<class T>
void QuatToEuler (const CQuaternion<T> &quat, CEuler<T> &euler)
{
T tmp = -2 * (quat.y*quat.z - quat.w*quat.x);
if(abs(tmp)>0.99999f)
{
// gimbal lock
euler.pitch = PI_DIV_2*tmp;
euler.yaw = atan2(T(-quat.x*quat.z + quat.w*quat.y), T(-quat.y*quat.y - quat.z*quat.z));
euler.roll = 0;
}
else
{
T xx = quat.x * quat.x;
euler.pitch = asin(tmp);
euler.yaw = atan2(T(quat.x*quat.z + quat.w*quat.y), T(0.5 - xx - quat.y*quat.y));
euler.roll = atan2(T(quat.x*quat.y + quat.w*quat.z), T(0.5 - xx - quat.z*quat.z));
}
}
//////////////////////////////////////////////////////////////////////////////////////
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -