四元数旋转向量

转自：http://www.oschina.net/code/snippet_876234_20178

001	//公式都是网上搜罗的，下面这些经过简单的测试，确认可用。

002

003	//ps: x,y,z,w 分别是四元素的四个值。稍微修改下就可以用。

004	// 由旋转矩阵创建四元数

005	inline CQuaternion(const _Matrix4& m)

006

{

007	float tr, s, q[4];

008	int i, j, k;

009

010	int nxt[3] = {1, 2, 0 };

011

// 计算矩阵轨迹

012	tr = m._11 + m._22 + m._33;

013

014	// 检查矩阵轨迹是正还是负

015	if(tr>0.0f)

016

{

017	s = sqrt(tr + 1.0f);

018	this->w = s / 2.0f;

019	s = 0.5f / s;

020	this->x = (m._23 - m._32) * s;

021	this->y = (m._31 - m._13) * s;

022	this->z = (m._12 - m._21) * s;

023

}

024

else

025

{

026

// 轨迹是负

027	// 寻找m11 m22 m33中的最大分量

028

i = 0;

029	if(m.m[1][1]>m.m[0][0]) i = 1;

030	if(m.m[2][2]>m.m[i][i]) i = 2;

031	j = nxt[i];

032	k = nxt[j];

033

034	s = sqrt((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1.0f);

035	q[i] = s * 0.5f;

036	if( s!= 0.0f) s = 0.5f / s;

037	q[3] = (m.m[j][k] - m.m[k][j]) * s;

038	q[j] = (m.m[i][j] - m.m[j][i]) * s;

039	q[k] = (m.m[i][k] - m.m[k][i]) * s;

040	this->x = q[0];

041	this->y = q[1];

042	this->z = q[2];

043	this->w = q[3];

044

}

045

};

046

047	// 由欧拉角创建四元数

048	inline CQuaternion(const _Vector3& angle)

049

{

050	float cx = cos(angle.x/2);

051	float sx = sin(angle.x/2);

052	float cy = cos(angle.y/2);

053	float sy = sin(angle.y/2);

054	float cz = cos(angle.z/2);

055	float sz = sin(angle.z/2);

056

057	this->w = cx*cy*cz + sx*sy*sz;

058	this->x = sx*cy*cz - cx*sy*sz;

059	this->y = cx*sy*cz + sx*cy*sz;

060	this->z = cx*cy*sz - sx*sy*cz;

061

};

062

063	// 给定角度和轴创建四元数

064	inline CQuaternion(_Vector3 anxi, const float& angle)

065

{

066	CVector3 t;

067	t.x = anxi.x;

068	t.y = anxi.y;

069	t.z = anxi.z;

070	t.Normalize();

071	float cosa = cos(angle);

072	float sina = sin(angle);

073	this->w = cosa;

074	this->x = sina * t.x;

075	this->y = sina * t.y;

076	this->z = sina * t.z;

077

};

078

079	// 由旋转四元数推导出矩阵

080	inline CMatrix4 GetMatrixLH()

081

{

082	CMatrix4 ret;

083	float xx = x*x;

084	float yy = y*y;

085	float zz = z*z;

086	float xy = x*y;

087	float wz = w*z;

088	float wy = w*y;

089	float xz = x*z;

090	float yz = y*z;

091	float wx = w*x;

092

093	ret._11 = 1.0f-2*(yy+zz);

094	ret._12 = 2*(xy-wz);

095	ret._13 = 2*(wy+xz);

096	ret._14 = 0.0f;

097

098	ret._21 = 2*(xy+wz);

099	ret._22 = 1.0f-2*(xx+zz);

100	ret._23 = 2*(yz-wx);

101	ret._24 = 0.0f;

102

103	ret._31 = 2*(xy-wy);

104	ret._32 = 2*(yz+wx);

105	ret._33 = 1.0f-2*(xx+yy);

106	ret._34 = 0.0f;

107

108	ret._41 = 0.0f;

109	ret._42 = 0.0f;

110	ret._43 = 0.0f;

111	ret._44 = 1.0f;

112

113	return ret;

114

};

115	inline CMatrix4 GetMatrixRH()

116

{

117	CMatrix4 ret;

118	float xx = x*x;

119	float yy = y*y;

120	float zz = z*z;

121	float xy = x*y;

122	float wz = -w*z;

123	float wy = -w*y;

124	float xz = x*z;

125	float yz = y*z;

126	float wx = -w*x;

127

128	ret._11 = 1.0f-2*(yy+zz);

129	ret._12 = 2*(xy-wz);

130	ret._13 = 2*(wy+xz);

131	ret._14 = 0.0f;

132

133	ret._21 = 2*(xy+wz);

134	ret._22 = 1.0f-2*(xx+zz);

135	ret._23 = 2*(yz-wx);

136	ret._24 = 0.0f;

137

138	ret._31 = 2*(xy-wy);

139	ret._32 = 2*(yz+wx);

140	ret._33 = 1.0f-2*(xx+yy);

141	ret._34 = 0.0f;

142

143	ret._41 = 0.0f;

144	ret._42 = 0.0f;

145	ret._43 = 0.0f;

146	ret._44 = 1.0f;

147

148	return ret;

149

};

150

151	// 由四元数返回欧拉角（主要是这个dx api里没有提供）

152	inline CVector3 GetEulerAngle()

153

{

154	CVector3 ret;

155

156	float test = y*z + x*w;

157	if (test > 0.4999f)

158

{

159	ret.z = 2.0f * atan2(y, w);

160	ret.y = PIOver2;

161	ret.x = 0.0f;

162	return ret;

163

}

164	if (test < -0.4999f)

165

{

166	ret.z = 2.0f * atan2(y, w);

167	ret.y = -PIOver2;

168	ret.x = 0.0f;

169	return ret;

170

}

171	float sqx = x * x;

172	float sqy = y * y;

173	float sqz = z * z;

174	ret.z = atan2(2.0f * z * w - 2.0f * y * x, 1.0f - 2.0f * sqz - 2.0f * sqx);

175	ret.y = asin(2.0f * test);

176	ret.x = atan2(2.0f * y * w - 2.0f * z * x, 1.0f - 2.0f * sqy - 2.0f * sqx);

177

178	return ret;

179

};

举报