头文件定义:
#ifndef _SHARED_GRAPHICS_QUATERNION_H_
#define _SHARED_GRAPHICS_QUATERNION_H_
#include <string>
#include "vec.h"
#include "matrix.h"
using namespace std;
namespace Shared{ namespace Graphics{
// =====================================================
// class AxisAngle
// =====================================================
class AxisAngle{
public:
Vec3f axis;
float angle;
AxisAngle(){};
AxisAngle(const Vec3f &axis, float angle);
};
// =====================================================
// class EulerAngles
// =====================================================
class EulerAngles{
public:
float x, y, z;
EulerAngles(){};
EulerAngles(float x, float y, float z);
};
// =====================================================
// class Quaternion
// =====================================================
class Quaternion{
private:
float w;
Vec3f v;
public:
Quaternion();
Quaternion(float w, const Vec3f &v);
Quaternion(const EulerAngles &eulerAngles);
Quaternion(const AxisAngle &axisAngle);
//initializers
void setMultIdentity();
void setAddIdentity();
void setAxisAngle(const AxisAngle &axisAngle);
void setEuler(const EulerAngles &eulerAngles);
//unary operators
float length();
Quaternion conjugate();
void normalize();
//binary operators
Quaternion operator + (const Quaternion &q) const;
Quaternion operator * (const Quaternion &q) const;
void operator += (const Quaternion &q);
void operator *= (const Quaternion &q);
//ternary operators
Quaternion lerp(float t, const Quaternion &q) const;
//conversions
Matrix3f toMatrix3() const;
Matrix4f toMatrix4() const;
AxisAngle toAxisAngle() const;
//local axis
Vec3f getLocalXAxis() const;
Vec3f getLocalYAxis() const;
Vec3f getLocalZAxis() const;
};
}}//end namespace
#endif
1. 从笛卡尔坐标表示法初始化四元数实现
绕x轴旋转角度:
void Quaternion::setAxisAngle(const AxisAngle &axisAngle){
w= cosf(axisAngle.angle/2.0f);
v.x= axisAngle.axis.x * sinf(axisAngle.angle/2.0f);
v.y= axisAngle.axis.y * sinf(axisAngle.angle/2.0f);
v.z= axisAngle.axis.z * sinf(axisAngle.angle/2.0f);
}
对应公式:
其中是绕旋转轴旋转的角度,
为旋转轴在x,y,z方向的分量(由此确定了旋转轴)。
2.从欧拉角初始化四元数实现
void Quaternion::setEuler(const EulerAngles &eulerAngles){
Quaternion qx, qy, qz, qr;
qx.w= cosf(eulerAngles.x/2.0f);
qx.v= Vec3f(sinf(eulerAngles.x/2.0f), 0.0f, 0.0f);
qy.w= cosf(eulerAngles.y/2.0f);
qy.v= Vec3f(0.0f, sinf(eulerAngles.y/2.0f), 0.0f);
qz.w= cosf(eulerAngles.z/2.0f);
qz.v= Vec3f(0.0f, 0.0f, sinf(eulerAngles.z/2.0f));
qr= qx*qy*qz;
w= qr.w;
v= qr.v;
}
对应公式:
- Roll –
: rotation about the X-axis
- Pitch –
: rotation about the Y-axis
- Yaw –
: rotation about the Z-axis
上面的qx*qy*qy = q. ? 未经推倒过,应该是的吧。。
3. 四元数到三维和四维矩阵
Matrix4f Quaternion::toMatrix4() const{ Matrix4f rm; //row1 rm[0]= 1.0f - 2*v.y*v.y - 2*v.z*v.z; rm[4]= 2*v.x*v.y - 2*w*v.z; rm[8]= 2*v.x*v.z + 2*w*v.y; rm[12]= 0.0f; //row2 rm[1]= 2*v.x*v.y + 2*w*v.z; rm[5]= 1.0f - 2*v.x*v.x - 2*v.z*v.z; rm[9]= 2*v.y*v.z - 2*w*v.x; rm[13]= 0.0f; //row3 rm[2]= 2*v.x*v.z - 2*w*v.y; rm[6]= 2*v.y*v.z + 2*w*v.x; rm[10]= 1.0f - 2*v.x*v.x - 2*v.y*v.y; rm[14]= 0.0f; //row4 rm[3]= 0.0f; rm[7]= 0.0f; rm[11]= 0.0f; rm[15]= 1.0f; return rm; }
Matrix3f Quaternion::toMatrix3() const{ Matrix3f rm; //row1 rm[0]= 1.0f - 2*v.y*v.y - 2*v.z*v.z; rm[3]= 2*v.x*v.y - 2*w*v.z; rm[6]= 2*v.x*v.z + 2*w*v.y; //row2 rm[1]= 2*v.x*v.y + 2*w*v.z; rm[4]= 1.0f - 2*v.x*v.x - 2*v.z*v.z; rm[7]= 2*v.y*v.z - 2*w*v.x; //row3 rm[2]= 2*v.x*v.z - 2*w*v.y; rm[5]= 2*v.y*v.z + 2*w*v.x; rm[8]= 1.0f - 2*v.x*v.x - 2*v.y*v.y; return rm; }
对应公式:
x' 旋转至x''
4. 取模
float Quaternion::length(){ return sqrt(w*w+v.x*v.x+v.y*v.y+v.z*v.z); }
对应公式:
5.共轭
Quaternion Quaternion::conjugate(){ return Quaternion(w, -v); }
对应公式:
To define it, let q = a + bi + cj + dk be a quaternion. The conjugate of q is the quaternion a − bi − cj − dk.
6.标准化
void Quaternion::normalize(){ float il= 1.f/length(); w*= il; v= v*il; }
7.加法
Quaternion Quaternion::operator + (const Quaternion &q) const{ return Quaternion(w +q.w, v+q.v); }
对应公式:
或者:
q1 + q2 = (w1+w2) + (x1+x2) i + (y1+y2) j + (z1+z2) k
8. 乘法
Quaternion Quaternion::operator * (const Quaternion &q) const{ return Quaternion( w*q.w - v.x*q.v.x - v.y*q.v.y - v.z*q.v.z, Vec3f( w*q.v.x + v.x*q.w + v.y*q.v.z - v.z*q.v.y, w*q.v.y + v.y*q.w + v.z*q.v.x - v.x*q.v.z, w*q.v.z + v.z*q.w + v.x*q.v.y - v.y*q.v.x)); }
对应公式:
where "·" is the dot product and "×" is the cross product.
或者:
q1 * q2 =
(w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2) i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2) j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2) k
9.线性差值
Quaternion Quaternion::lerp(float t, const Quaternion &q) const{ return Quaternion( w * (1.0f-t) + q.w * t, v * (1.0f-t) + q.v * t); }
10.四元数到笛卡尔坐标
AxisAngle Quaternion::toAxisAngle() const{ float scale= 1.0f/(v.x*v.x + v.y*v.y + v.z*v.z); return AxisAngle(v*scale, 2*acosf(w)); }