四元数和旋转矩阵
2016-03-23 10:53
597 查看
四元数和旋转矩阵
Quaternion(四元数)Quaternion 的定义
四元数一般定义如下：
q=w+xi+yj+zk
其中 w,x,y,z是实数。同时，有:
i*i=-1
j*j=-1
k*k=-1
四元数也可以表示为：
q=[w,v]
其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的。
通俗的讲，一个四元数（Quaternion）描述了一个旋转轴和一个旋转角度。这个旋转轴和这个角度可以通过 Quaternion::ToAngleAxis转换得到。当然也可以随意指定一个角度一个旋转轴来构造一个Quaternion。这个角度是相对于单位四元数而言的，也可以说是相对于物体的初始方向而言的。
当用一个四元数乘以一个向量时，实际上就是让该向量围绕着这个四元数所描述的旋转轴，转动这个四元数所描述的角度而得到的向量。
四元组的优点
有多种方式可表示旋转，如 axis/angle、欧拉角(Euler angles)、矩阵(matrix)、四元组等。 相对于其它方法，四元组有其本身的优点：
四元数不会有欧拉角存在的 gimbal lock 问题
四元数由4个数组成，旋转矩阵需要9个数
两个四元数之间更容易插值
四元数、矩阵在多次运算后会积攒误差，需要分别对其做规范化(normalize)和正交化(orthogonalize)，对四元数规范化更容易
与旋转矩阵类似，两个四元组相乘可表示两次旋转
Quaternion 的基本运算
Normalizing a quaternion
// normalising a quaternion works similar to a vector. This method will not do anything
// if the quaternion is close enough to being unit-length. define TOLERANCE as something
// small like 0.00001f to get accurate results
void Quaternion::normalise()
{
// Don't normalize if we don't have to
float mag2 = w * w + x * x + y * y + z * z;
if ( mag2!=0.f && (fabs(mag2 - 1.0f) > TOLERANCE)) {
float mag = sqrt(mag2);
w /= mag;
x /= mag;
y /= mag;
z /= mag;
}
}
The complex conjugate of a quaternion
// We need to get the inverse of a quaternion to properly apply a quaternion-rotation to a vector
// The conjugate of a quaternion is the same as the inverse, as long as the quaternion is unit-length
Quaternion Quaternion::getConjugate()
{
return Quaternion(-x, -y, -z, w);
}
Multiplying quaternions
// Multiplying q1 with q2 applies the rotation q2 to q1
Quaternion Quaternion::operator* (const Quaternion &rq) const
{
// the constructor takes its arguments as (x, y, z, w)
return Quaternion(w * rq.x + x * rq.w + y * rq.z - z * rq.y,
w * rq.y + y * rq.w + z * rq.x - x * rq.z,
w * rq.z + z * rq.w + x * rq.y - y * rq.x,
w * rq.w - x * rq.x - y * rq.y - z * rq.z);
}
Rotating vectors
// Multiplying a quaternion q with a vector v applies the q-rotation to v
Vector3 Quaternion::operator* (const Vector3 &vec) const
{
Vector3 vn(vec);
vn.normalise();
Quaternion vecQuat, resQuat;
vecQuat.x = vn.x;
vecQuat.y = vn.y;
vecQuat.z = vn.z;
vecQuat.w = 0.0f;
resQuat = vecQuat * getConjugate();
resQuat = *this * resQuat;
return (Vector3(resQuat.x, resQuat.y, resQuat.z));
}
How to convert to/from quaternions1
Quaternion from axis-angle
// Convert from Axis Angle
void Quaternion::FromAxis(const Vector3 &v, float angle)
{
float sinAngle;
angle *= 0.5f;
Vector3 vn(v);
vn.normalise();
sinAngle = sin(angle);
x = (vn.x * sinAngle);
y = (vn.y * sinAngle);
z = (vn.z * sinAngle);
w = cos(angle);
}
Quaternion from Euler angles
// Convert from Euler Angles
void Quaternion::FromEuler(float pitch, float yaw, float roll)
{
// Basically we create 3 Quaternions, one for pitch, one for yaw, one for roll
// and multiply those together.
// the calculation below does the same, just shorter
float p = pitch * PIOVER180 / 2.0;
float y = yaw * PIOVER180 / 2.0;
float r = roll * PIOVER180 / 2.0;
float sinp = sin(p);
float siny = sin(y);
float sinr = sin(r);
float cosp = cos(p);
float cosy = cos(y);
float cosr = cos(r);
this->x = sinr * cosp * cosy - cosr * sinp * siny;
this->y = cosr * sinp * cosy + sinr * cosp * siny;
this->z = cosr * cosp * siny - sinr * sinp * cosy;
this->w = cosr * cosp * cosy + sinr * sinp * siny;
normalise();
}
Quaternion to Matrix
// Convert to Matrix
Matrix4 Quaternion::getMatrix() const
{
float x2 = x * x;
float y2 = y * y;
float z2 = z * z;
float xy = x * y;
float xz = x * z;
float yz = y * z;
float wx = w * x;
float wy = w * y;
float wz = w * z;
// This calculation would be a lot more complicated for non-unit length quaternions
// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by
// OpenGL
return Matrix4( 1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy), 0.0f,
2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx), 0.0f,
2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f)
}
Quaternion to axis-angle
// Convert to Axis/Angles
void Quaternion::getAxisAngle(Vector3 *axis, float *angle)
{
float scale = sqrt(x * x + y * y + z * z);
axis->x = x / scale;
axis->y = y / scale;
axis->z = z / scale;
*angle = acos(w) * 2.0f;
}
Quaternion 插值
线性插值
最简单的插值算法就是线性插值，公式如：
q(t)=(1-t)q1 + t q2
但这个结果是需要规格化的，否则q(t)的单位长度会发生变化，所以
q(t)=(1-t)q1+t q2 / || (1-t)q1+t q2 ||
球形线性插值
尽管线性插值很有效，但不能以恒定的速率描述q1到q2之间的曲线，这也是其弊端，我们需要找到一种插值方法使得q1->q(t)之间的夹角θ是线性的，即θ(t)=(1-t)θ1+t*θ2，这样我们得到了球形线性插值函数q(t)，如下：
q(t)=q1 * sinθ(1-t)/sinθ + q2 * sinθt/sineθ
如果使用D3D，可以直接使用 D3DXQuaternionSlerp 函数就可以完成这个插值过程。
用 Quaternion 实现 Camera 旋转
总体来讲，Camera 的操作可分为如下几类：
沿直线移动
围绕某轴自转
围绕某轴公转
下面是一个使用了 Quaternion 的 Camera 类：
class Camera {
private:
Quaternion m_orientation;
public:
void rotate (const Quaternion& q);
void rotate(const Vector3& axis, const Radian& angle);
void roll (const GLfloat angle);
void yaw (const GLfloat angle);
void pitch (const GLfloat angle);
};
void Camera::rotate(const Quaternion& q)
{
// Note the order of the mult, i.e. q comes after
m_Orientation = q * m_Orientation;
}
void Camera::rotate(const Vector3& axis, const Radian& angle)
{
Quaternion q;
q.FromAngleAxis(angle,axis);
rotate(q);
}
void Camera::roll (const GLfloat angle) //in radian
{
Vector3 zAxis = m_Orientation * Vector3::UNIT_Z;
rotate(zAxis, angleInRadian);
}
void Camera::yaw (const GLfloat angle) //in degree
{
Vector3 yAxis;
{
// Rotate around local Y axis
yAxis = m_Orientation * Vector3::UNIT_Y;
}
rotate(yAxis, angleInRadian);
}
void Camera::pitch (const GLfloat angle) //in radian
{
Vector3 xAxis = m_Orientation * Vector3::UNIT_X;
rotate(xAxis, angleInRadian);
}
void Camera::gluLookAt() {
GLfloat m[4][4];
identf(&m[0][0]);
m_Orientation.createMatrix (&m[0][0]);
glMultMatrixf(&m[0][0]);
glTranslatef(-m_eyex, -m_eyey, -m_eyez);
}
用 Quaternion 实现 trackball
用鼠标拖动物体在三维空间里旋转，一般设计一个 trackball，其内部实现也常用四元数。
class TrackBall
{
public:
TrackBall();
void push(const QPointF& p);
void move(const QPointF& p);
void release(const QPointF& p);
QQuaternion rotation() const;
private:
QQuaternion m_rotation;
QVector3D m_axis;
float m_angularVelocity;
QPointF m_lastPos;
};
void TrackBall::move(const QPointF& p)
{
if (!m_pressed)
return;
QVector3D lastPos3D = QVector3D(m_lastPos.x(), m_lastPos.y(), 0.0f);
float sqrZ = 1 - QVector3D::dotProduct(lastPos3D, lastPos3D);
if (sqrZ > 0)
lastPos3D.setZ(sqrt(sqrZ));
else
lastPos3D.normalize();
QVector3D currentPos3D = QVector3D(p.x(), p.y(), 0.0f);
sqrZ = 1 - QVector3D::dotProduct(currentPos3D, currentPos3D);
if (sqrZ > 0)
currentPos3D.setZ(sqrt(sqrZ));
else
currentPos3D.normalize();
m_axis = QVector3D::crossProduct(lastPos3D, currentPos3D);
float angle = 180 / PI * asin(sqrt(QVector3D::dotProduct(m_axis, m_axis)));
m_axis.normalize();
m_rotation = QQuaternion::fromAxisAndAngle(m_axis, angle) * m_rotation;
m_lastPos = p;
}
---------------------------------------------------------------------------------------------------------
每一个单位四元数都可以对应到一个旋转矩阵
单位四元数q=（s，V）的共轭为q*=（s，-V）
单位四元数的模为||q||=1；
四元数q=(s,V)的逆q^(-1)=q*/(||q||)=q*
一个向量r，沿着向量n旋转a角度之后的向量是哪个（假设为v），这个用四元数可以轻松搞定
构造两个四元数q=（cos(a/2),sin(a/2)*n）,p=(0,r)
p`=q * p * q^(-1) 这个可以保证求出来的p`也是（0，r`）形式的，求出的r`就是r旋转后的向量
另外其实对p做q * p * q^(-1)操作就是相当于对p乘了一个旋转矩阵，这里先假设 q=（cos(a/2),sin(a/2)*n）=（s，（x, y, z））
两个四元数相乘也表示一个旋转
Q1 * Q2 表示先以Q2旋转，再以Q1旋转
则这个矩阵为
同理一个旋转矩阵也可以转换为一个四元数，即给你一个旋转矩阵可以求出（s，x，y，z）这个四元数，
方法是：
给定任意单位轴q(q1,q2,q3)(向量),求向量p(x,y,z)(或点p)饶q旋转theta角度的变换后的新向量p'(或点p')：
1.用四元数工具：
-------------------------------------------------------------------------
结论：构造四元数变换p'= q*p*q-1，（p,q是由向量p,q扩展成的四元数）。那么，p'转换至对应的向量(或点)就是变换后的新向量p'(或点p')。
其中，p',q,p,q-1均为四元数。q由向量q扩展,为q=(cos(theta/2),sin(theta/2)*q)，p由向量p扩展,为p=(0,x,y,z),q-1为q的逆，因为q为单位四元数，所以q-1=q*=(cos(theta/2),-sin(theta/2)*q)。
http://www.linuxgraphics.cn/opengl/opengl_quaternion.html
http://blog.csdn.net/qq960885333/article/details/8191448
http://blog.csdn.net/jiexuan357/article/details/7727634
转载：
http://blog.csdn.net/wangjiannuaa/article/details/8952196
代码片段：
// 由旋转矩阵创建四元数
inline CQuaternion(const _Matrix4& m)
{
float tr, s, q[4];
int i, j, k;
int nxt[3] = {1, 2, 0 };
// 计算矩阵轨迹
tr = m._11 + m._22 + m._33;
// 检查矩阵轨迹是正还是负
if(tr>0.0f)
{
s = sqrt(tr + 1.0f);
this->w = s / 2.0f;
s = 0.5f / s;
this->x = (m._23 - m._32) * s;
this->y = (m._31 - m._13) * s;
this->z = (m._12 - m._21) * s;
}
else
{
// 轨迹是负
// 寻找m11 m22 m33中的最大分量
i = 0;
if(m.m[1][1]>m.m[0][0]) i = 1;
if(m.m[2][2]>m.m[i][i]) i = 2;
j = nxt[i];
k = nxt[j];
s = sqrt((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1.0f);
q[i] = s * 0.5f;
if( s!= 0.0f) s = 0.5f / s;
q[3] = (m.m[j][k] - m.m[k][j]) * s;
q[j] = (m.m[i][j] - m.m[j][i]) * s;
q[k] = (m.m[i][k] - m.m[k][i]) * s;
this->x = q[0];
this->y = q[1];
this->z = q[2];
this->w = q[3];
}
};
// 由欧拉角创建四元数
inline CQuaternion(const _Vector3& angle)
{
float cx = cos(angle.x/2);
float sx = sin(angle.x/2);
float cy = cos(angle.y/2);
float sy = sin(angle.y/2);
float cz = cos(angle.z/2);
float sz = sin(angle.z/2);
this->w = cx*cy*cz + sx*sy*sz;
this->x = sx*cy*cz - cx*sy*sz;
this->y = cx*sy*cz + sx*cy*sz;
this->z = cx*cy*sz - sx*sy*cz;
};
// 给定角度和轴创建四元数
inline CQuaternion(_Vector3 anxi, const float& angle)
{
CVector3 t;
t.x = anxi.x;
t.y = anxi.y;
t.z = anxi.z;
t.Normalize();
float cosa = cos(angle);
float sina = sin(angle);
this->w = cosa;
this->x = sina * t.x;
this->y = sina * t.y;
this->z = sina * t.z;
};
// 由旋转四元数推导出矩阵
inline CMatrix4 GetMatrixLH()
{
CMatrix4 ret;
float xx = x*x;
float yy = y*y;
float zz = z*z;
float xy = x*y;
float wz = w*z;
float wy = w*y;
float xz = x*z;
float yz = y*z;
float wx = w*x;
ret._11 = 1.0f-2*(yy+zz);
ret._12 = 2*(xy-wz);
ret._13 = 2*(wy+xz);
ret._14 = 0.0f;
ret._21 = 2*(xy+wz);
ret._22 = 1.0f-2*(xx+zz);
ret._23 = 2*(yz-wx);
ret._24 = 0.0f;
ret._31 = 2*(xy-wy);
ret._32 = 2*(yz+wx);
ret._33 = 1.0f-2*(xx+yy);
ret._34 = 0.0f;
ret._41 = 0.0f;
ret._42 = 0.0f;
ret._43 = 0.0f;
ret._44 = 1.0f;
return ret;
};
inline CMatrix4 GetMatrixRH()
{
CMatrix4 ret;
float xx = x*x;
float yy = y*y;
float zz = z*z;
float xy = x*y;
float wz = -w*z;
float wy = -w*y;
float xz = x*z;
float yz = y*z;
float wx = -w*x;
ret._11 = 1.0f-2*(yy+zz);
ret._12 = 2*(xy-wz);
ret._13 = 2*(wy+xz);
ret._14 = 0.0f;
ret._21 = 2*(xy+wz);
ret._22 = 1.0f-2*(xx+zz);
ret._23 = 2*(yz-wx);
ret._24 = 0.0f;
ret._31 = 2*(xy-wy);
ret._32 = 2*(yz+wx);
ret._33 = 1.0f-2*(xx+yy);
ret._34 = 0.0f;
ret._41 = 0.0f;
ret._42 = 0.0f;
ret._43 = 0.0f;
ret._44 = 1.0f;
return ret;
};
// 由四元数返回欧拉角
inline CVector3 GetEulerAngle()
{
CVector3 ret;
float test = y*z + x*w;
if (test > 0.4999f)
{
ret.z = 2.0f * atan2(y, w);
ret.y = PIOver2;
ret.x = 0.0f;
return ret;
}
if (test < -0.4999f)
{
ret.z = 2.0f * atan2(y, w);
ret.y = -PIOver2;
ret.x = 0.0f;
return ret;
}
float sqx = x * x;
float sqy = y * y;
float sqz = z * z;
ret.z = atan2(2.0f * z * w - 2.0f * y * x, 1.0f - 2.0f * sqz - 2.0f * sqx);
ret.y = asin(2.0f * test);
ret.x = atan2(2.0f * y * w - 2.0f * z * x, 1.0f - 2.0f * sqy - 2.0f * sqx);
return ret;
};
求旋转矩阵转为四元数的程序。 代码来至与ID4号引擎。
Quat Mat3ToQuat( float** mat )
{
Quat q;
float trace;
float s;
float t;
int i;
int j;
int k;
static int next[ 3 ] = { 1, 2, 0 };
trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
if ( trace > 0.0f )
{
t = trace + 1.0f;
s = 0.5f / sqrtf(t);
q[3] = s * t;
q[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
q[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
q[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;
}
else
{
i = 0;
if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
i = 1;
}
if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
i = 2;
}
j = next[ i ];
k = next[ j ];
t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
s = 0.5f / sqrtf(t);
q[i] = s * t;
q[3] = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
q[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
q[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
}
return q;
}
之前在网上找旋转矩阵和四元数相互转换的代码，找了几个都不大对劲，正反算算不过来，最后还是从osg源码里贴出来的这个，应该没什么问题。
这里给一个链接，Matrix and Quaternion FAQ
http://www.flipcode.com/documents/matrfaq.html
以下是源文件：
#include<iostream>
#include<cmath>
using namespace std;
typedef double ValType;
struct Quat;
struct Matrix;
struct Quat {
ValType _v[4];//x, y, z, w
/// Length of the quaternion = sqrt( vec . vec )
ValType length() const
{
return sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3]);
}
/// Length of the quaternion = vec . vec
ValType length2() const
{
return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
}
};
struct Matrix {
ValType _mat[3][3];
};
#define QX q._v[0]
#define QY q._v[1]
#define QZ q._v[2]
#define QW q._v[3]
void Quat2Matrix(const Quat& q, Matrix& m)
{
double length2 = q.length2();
if (fabs(length2) <= std::numeric_limits<double>::min())
{
m._mat[0][0] = 0.0; m._mat[1][0] = 0.0; m._mat[2][0] = 0.0;
m._mat[0][1] = 0.0; m._mat[1][1] = 0.0; m._mat[2][1] = 0.0;
m._mat[0][2] = 0.0; m._mat[1][2] = 0.0; m._mat[2][2] = 0.0;
}
else
{
double rlength2;
// normalize quat if required.
// We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
// That is a square of a square root, so it is possible to avoid that
if (length2 != 1.0)
{
rlength2 = 2.0/length2;
}
else
{
rlength2 = 2.0;
}
// Source: Gamasutra, Rotating Objects Using Quaternions
//
//http://www.gamasutra.com/features/19980703/quaternions_01.htm
double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
// calculate coefficients
x2 = rlength2*QX;
y2 = rlength2*QY;
z2 = rlength2*QZ;
xx = QX * x2;
xy = QX * y2;
xz = QX * z2;
yy = QY * y2;
yz = QY * z2;
zz = QZ * z2;
wx = QW * x2;
wy = QW * y2;
wz = QW * z2;
// Note. Gamasutra gets the matrix assignments inverted, resulting
// in left-handed rotations, which is contrary to OpenGL and OSG's
// methodology. The matrix assignment has been altered in the next
// few lines of code to do the right thing.
// Don Burns - Oct 13, 2001
m._mat[0][0] = 1.0 - (yy + zz);
m._mat[1][0] = xy - wz;
m._mat[2][0] = xz + wy;
m._mat[0][1] = xy + wz;
m._mat[1][1] = 1.0 - (xx + zz);
m._mat[2][1] = yz - wx;
m._mat[0][2] = xz - wy;
m._mat[1][2] = yz + wx;
m._mat[2][2] = 1.0 - (xx + yy);
}
}
void Matrix2Quat(const Matrix& m, Quat& q)
{
ValType s;
ValType tq[4];
int i, j;
// Use tq to store the largest trace
tq[0] = 1 + m._mat[0][0]+m._mat[1][1]+m._mat[2][2];
tq[1] = 1 + m._mat[0][0]-m._mat[1][1]-m._mat[2][2];
tq[2] = 1 - m._mat[0][0]+m._mat[1][1]-m._mat[2][2];
tq[3] = 1 - m._mat[0][0]-m._mat[1][1]+m._mat[2][2];
// Find the maximum (could also use stacked if's later)
j = 0;
for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;
// check the diagonal
if (j==0)
{
/* perform instant calculation */
QW = tq[0];
QX = m._mat[1][2]-m._mat[2][1];
QY = m._mat[2][0]-m._mat[0][2];
QZ = m._mat[0][1]-m._mat[1][0];
}
else if (j==1)
{
QW = m._mat[1][2]-m._mat[2][1];
QX = tq[1];
QY = m._mat[0][1]+m._mat[1][0];
QZ = m._mat[2][0]+m._mat[0][2];
}
else if (j==2)
{
QW = m._mat[2][0]-m._mat[0][2];
QX = m._mat[0][1]+m._mat[1][0];
QY = tq[2];
QZ = m._mat[1][2]+m._mat[2][1];
}
else /* if (j==3) */
{
QW = m._mat[0][1]-m._mat[1][0];
QX = m._mat[2][0]+m._mat[0][2];
QY = m._mat[1][2]+m._mat[2][1];
QZ = tq[3];
}
s = sqrt(0.25/tq[j]);
QW *= s;
QX *= s;
QY *= s;
QZ *= s;
}
void printMatrix(const Matrix& r, string name)
{
cout<<"RotMat "<<name<<" = "<<endl;
cout<<"\t"<<r._mat[0][0]<<" "<<r._mat[0][1]<<" "<<r._mat[0][2]<<endl;
cout<<"\t"<<r._mat[1][0]<<" "<<r._mat[1][1]<<" "<<r._mat[1][2]<<endl;
cout<<"\t"<<r._mat[2][0]<<" "<<r._mat[2][1]<<" "<<r._mat[2][2]<<endl;
cout<<endl;
}
void printQuat(const Quat& q, string name)
{
cout<<"Quat "<<name<<" = "<<endl;
cout<<"\t"<<q._v[0]<<" "<<q._v[1]<<" "<<q._v[2]<<" "<<q._v[3]<<endl;
cout<<endl;
}
int main()
{
ValType phi, omiga, kappa;
phi = 1.32148229302237 ; omiga = 0.626224465189316 ; kappa = -1.4092143985971;
ValType a1,a2,a3,b1,b2,b3,c1,c2,c3;
a1 = cos(phi)*cos(kappa) - sin(phi)*sin(omiga)*sin(kappa);
a2 = -cos(phi)*sin(kappa) - sin(phi)*sin(omiga)*cos(kappa);
a3 = -sin(phi)*cos(omiga);
b1 = cos(omiga)*sin(kappa);
b2 = cos(omiga)*cos(kappa);
b3 = -sin(omiga);
c1 = sin(phi)*cos(kappa) + cos(phi)*sin(omiga)*sin(kappa);
c2 = -sin(phi)*sin(kappa) + cos(phi)*sin(omiga)*cos(kappa);
c3 = cos(phi)*cos(omiga);
Matrix r;
r._mat[0][0] = a1;
r._mat[0][1] = a2;
r._mat[0][2] = a3;
r._mat[1][0] = b1;
r._mat[1][1] = b2;
r._mat[1][2] = b3;
r._mat[2][0] = c1;
r._mat[2][1] = c2;
r._mat[2][2] = c3;
printMatrix(r, "r");
//////////////////////////////////////////////////////////
Quat q;
Matrix2Quat(r, q);
printQuat(q, "q");
Matrix _r;
Quat2Matrix(q, _r);
printMatrix(_r, "_r");
system("pause");
return 0;
}
内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理 
相关文章推荐
- makefile.in
- json日期格式转换
- log4j 如何配置不同的classeslog写入不同的文件中
- Tableview最后一行无法显示或者显示不全的问题
- 简明Python教程学习笔记_7_输入输出和文件操作
- [Mysql] find_in_set()使用注意
- [2016/03/23] 2016年蓝桥杯JAVA A组 参赛总结和感想
- 设计删除顺序表中【x，y】中的所有元素算法
- struts.xml中Action的method与路径的几种匹配模式。
- Android 如何判断 ViewStub 是否inflate
- iOS底层学习-KVC使用实践以及实现原理
- WebView加载html5页面
- 盒子模型
- iOS开发--泛型
- SpringMVC结合ajaxfileupload.js实现文件无刷新上传
- WiFI Display（WFD）
- Hadoop contrib介绍
- 结对编程心得体会
- Spring Security（17）——基于方法的权限控制
- mysql访问被拒绝解决 ---谷营中西软件科技园