四元数旋转向量

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

//寻找m11m22m33中的最大分量

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(_Vector3anxi,

const

float

&angle)

065

{

066

CVector3t;

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

CMatrix4GetMatrixLH()

081

{

082

CMatrix4ret;

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

CMatrix4GetMatrixRH()

116

{

117

CMatrix4ret;

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

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

152

inline

CVector3GetEulerAngle()

153

{

154

CVector3ret;

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

};