四元数-glest四元数代码学习

头文件定义：

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