从零实现3D图像引擎:(9)四元数函数库
2011-02-18 10:29
351 查看
1. 数学分析
1) 四元数是什么东西?
这个东西算盘、矩阵、复数是一类东西,即数学工具,数学家们创造了这个东西来解决一些数学问题。其实四元数是一种超复数,他不是只有一个虚数的复数,而是有三个虚数的复数。我们先回顾一下复数吧。
2) 虚数的来源
实数集中没有-1的平方根,因为没有哪个实数的平方等于-1,所以数学家们就创造它——虚数i,并且定义了i * i = -1。
所以我们可以计算sqrt(-4)了,sqrt(-4) = 2*i
3) 复数
定义:一个实数与一个虚数的和。 z = a + bi。
a叫实部,b叫虚部。
其中(a,b)为复平面上的点。这也是为什么3D图形运算能和四元数挂上关系了。
运算法则:
z1 = a + b*i
z2 = c + d*i
与标量相乘 k*z1 = k*a + K*b*i
复数相加 z1 + z2 = a + c + (b + d) * i
复数相乘 z1 * z2 = (a + b * i) * (c + d * i) = a * c + a * d * i + c * b * i - b * d = a*c - b*d + (a*d+b*c) * i
复数相除 z1 / z2 = (a + b*i) / (c + d*i)
z1的共轭z1* = a - b*i,作用是z×z*为实数
所以复数相除 z1 / z2 = (a + b*i)*(c - d*i) / (c2+d2) = ... = (a*c + b*d)/(a2+b2) + (b*c-a*d)*i)/(a2+b2)
z1的倒数 1/z = 1 / (a+b*i),同样可以转化为:a/(a2+b2) + b*i/(a2+b2)
z1的范数 |z| = sqrt(a2+b2) = sqrt(z×z*)
4) 超复数
q = q0 + q1*i + q2*j + q3*k
上面就是四元数的表示,其中q0为实部,而虚部<q1,q2,q3>正好组成了3D复平面中的向量。
简写就是q = q0 + qv,其中qv是向量<q1, q2 ,q3>。
超复数的运算法则与复数相同,这里就不再重复了,但要特别说明四元数的倒数q-1。
q×q-1 = 1
两边同时乘以q*:
q×q-1×q* = q*
因为:q×q* = |q|2
所以q-1=q* / |q|2
可以注意到,如果q是一个单位四元数的话,那么q的倒数就等于q的共轭。
这个特性非常重要,因为在四元数旋转中要使用。
5) 四元数旋转
对于一个3D向量<x, y, z>,我们把他表现为四元数形式,则是:vq= <0, x, y, z>。
以及给定一个表示旋转轴和旋转角度的单位四元数q,那么向量vq绕指定轴旋转指定角度的结果四元数vq'满足如下:
右手坐标系:
顺时针旋转:vq' = q*×vq×q
逆时针旋转:vq' = q×vq×q*
左手坐标系:
顺时针旋转:vq' = q×vq×q*
逆时针旋转:vq' = q*×vq×q
其中,对于那个用于存储旋转轴和旋转角度的单位四元数q,他与旋转轴v = <x0, y0, z0>和角度theta关系如下:
q = Cos(theta/2) + Sin(theta/2) × v
2. 代码实现
1) 四元数结构体定义
view plaincopy to clipboardprint?
typedef struct QUAT_TYPE // 四元数
{
union
{
double M[4];
struct
{
double w, x, y, z;
};
struct
{
double q0;
VECTOR3D qv;
};
};
} QUAT, *QUAT_PTR;
typedef struct QUAT_TYPE // 四元数
{
union
{
double M[4];
struct
{
double w, x, y, z;
};
struct
{
double q0;
VECTOR3D qv;
};
};
} QUAT, *QUAT_PTR;
2) 四元数常用操作函数实现
view plaincopy to clipboardprint?
void _CPPYIN_Math::QuatCreate(QUAT_PTR q, VECTOR3D_PTR v, double theta) // 创建用于旋转的四元数q, v必须为单位向量
{
double theta_div_2 = (0.5)*theta;
double sin_theta = sin(theta_div_2);
q->x = sin_theta * v->x;
q->y = sin_theta * v->y;
q->z = sin_theta * v->z;
q->w = cos( theta_div_2 );
}
void _CPPYIN_Math::QuatGetVectorAndTheta(QUAT_PTR q, VECTOR3D_PTR v, double *theta)
{
*theta = acos(q->w);
double sin_theta_inv = 1.0/sin(*theta);
v->x = q->x * sin_theta_inv;
v->y = q->y * sin_theta_inv;
v->z = q->z * sin_theta_inv;
*theta *= 2;
}
void _CPPYIN_Math::QuatAdd(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum)
{
qsum->x = q1->x + q2->x;
qsum->y = q1->y + q2->y;
qsum->z = q1->z + q2->z;
qsum->w = q1->w + q2->w;
}
void _CPPYIN_Math::QuatSub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff)
{
qdiff->x = q1->x - q2->x;
qdiff->y = q1->y - q2->y;
qdiff->z = q1->z - q2->z;
qdiff->w = q1->w - q2->w;
}
void _CPPYIN_Math::QuatConjugate(QUAT_PTR q, QUAT_PTR qconj) // 求共轭
{
qconj->x = -q->x;
qconj->y = -q->y;
qconj->z = -q->z;
qconj->w = q->w;
}
void _CPPYIN_Math::QuatScale(QUAT_PTR q, double scale, QUAT_PTR qs)
{
qs->x = scale * q->x;
qs->y = scale * q->y;
qs->z = scale * q->z;
qs->w = scale * q->w;
}
double _CPPYIN_Math::QuatNorm(QUAT_PTR q)
{
return sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
}
double _CPPYIN_Math::QuatNorm2(QUAT_PTR q)
{
return q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z;
}
void _CPPYIN_Math::QuatNormalize(QUAT_PTR q, QUAT_PTR qn)
{
double qlength_inv = 1.0/(sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z));
qn->w = q->w * qlength_inv;
qn->x = q->x * qlength_inv;
qn->y = q->y * qlength_inv;
qn->z = q->z * qlength_inv;
}
void _CPPYIN_Math::QuatUnitInverse(QUAT_PTR q, QUAT_PTR qi) // 单位四元数的逆,等于求共轭
{
qi->w = q->w;
qi->x = -q->x;
qi->y = -q->y;
qi->z = -q->z;
}
void _CPPYIN_Math::QuatInverse(QUAT_PTR q, QUAT_PTR qi) // 非单位四元数的逆
{
double norm2_inv = 1.0 / (q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
qi->w = q->w * norm2_inv;
qi->x = -q->x * norm2_inv;
qi->y = -q->y * norm2_inv;
qi->z = -q->z * norm2_inv;
}
void _CPPYIN_Math::QuatMul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod)
{
double prd_0 = (q1->z - q1->y) * (q2->y - q2->z);
double prd_1 = (q1->w + q1->x) * (q2->w + q2->x);
double prd_2 = (q1->w - q1->x) * (q2->y + q2->z);
double prd_3 = (q1->y + q1->z) * (q2->w - q2->x);
double prd_4 = (q1->z - q1->x) * (q2->x - q2->y);
double prd_5 = (q1->z + q1->x) * (q2->x + q2->y);
double prd_6 = (q1->w + q1->y) * (q2->w - q2->z);
double prd_7 = (q1->w - q1->y) * (q2->w + q2->z);
double prd_8 = prd_5 + prd_6 + prd_7;
double prd_9 = 0.5 * (prd_4 + prd_8);
qprod->w = prd_0 + prd_9 - prd_5;
qprod->x = prd_1 + prd_9 - prd_8;
qprod->y = prd_2 + prd_9 - prd_7;
qprod->z = prd_3 + prd_9 - prd_6;
}
void _CPPYIN_Math::QuatMul3(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod) // 三个四元数相乘,用于做旋转变换
{
QUAT qtmp;
QuatMul(q1, q2, &qtmp);
QuatMul(&qtmp, q3, qprod);
}
本文来自CSDN博客,转载请标明出处:http://blog.csdn.net/cppyin/archive/2011/02/10/6177742.aspx
1) 四元数是什么东西?
这个东西算盘、矩阵、复数是一类东西,即数学工具,数学家们创造了这个东西来解决一些数学问题。其实四元数是一种超复数,他不是只有一个虚数的复数,而是有三个虚数的复数。我们先回顾一下复数吧。
2) 虚数的来源
实数集中没有-1的平方根,因为没有哪个实数的平方等于-1,所以数学家们就创造它——虚数i,并且定义了i * i = -1。
所以我们可以计算sqrt(-4)了,sqrt(-4) = 2*i
3) 复数
定义:一个实数与一个虚数的和。 z = a + bi。
a叫实部,b叫虚部。
其中(a,b)为复平面上的点。这也是为什么3D图形运算能和四元数挂上关系了。
运算法则:
z1 = a + b*i
z2 = c + d*i
与标量相乘 k*z1 = k*a + K*b*i
复数相加 z1 + z2 = a + c + (b + d) * i
复数相乘 z1 * z2 = (a + b * i) * (c + d * i) = a * c + a * d * i + c * b * i - b * d = a*c - b*d + (a*d+b*c) * i
复数相除 z1 / z2 = (a + b*i) / (c + d*i)
z1的共轭z1* = a - b*i,作用是z×z*为实数
所以复数相除 z1 / z2 = (a + b*i)*(c - d*i) / (c2+d2) = ... = (a*c + b*d)/(a2+b2) + (b*c-a*d)*i)/(a2+b2)
z1的倒数 1/z = 1 / (a+b*i),同样可以转化为:a/(a2+b2) + b*i/(a2+b2)
z1的范数 |z| = sqrt(a2+b2) = sqrt(z×z*)
4) 超复数
q = q0 + q1*i + q2*j + q3*k
上面就是四元数的表示,其中q0为实部,而虚部<q1,q2,q3>正好组成了3D复平面中的向量。
简写就是q = q0 + qv,其中qv是向量<q1, q2 ,q3>。
超复数的运算法则与复数相同,这里就不再重复了,但要特别说明四元数的倒数q-1。
q×q-1 = 1
两边同时乘以q*:
q×q-1×q* = q*
因为:q×q* = |q|2
所以q-1=q* / |q|2
可以注意到,如果q是一个单位四元数的话,那么q的倒数就等于q的共轭。
这个特性非常重要,因为在四元数旋转中要使用。
5) 四元数旋转
对于一个3D向量<x, y, z>,我们把他表现为四元数形式,则是:vq= <0, x, y, z>。
以及给定一个表示旋转轴和旋转角度的单位四元数q,那么向量vq绕指定轴旋转指定角度的结果四元数vq'满足如下:
右手坐标系:
顺时针旋转:vq' = q*×vq×q
逆时针旋转:vq' = q×vq×q*
左手坐标系:
顺时针旋转:vq' = q×vq×q*
逆时针旋转:vq' = q*×vq×q
其中,对于那个用于存储旋转轴和旋转角度的单位四元数q,他与旋转轴v = <x0, y0, z0>和角度theta关系如下:
q = Cos(theta/2) + Sin(theta/2) × v
2. 代码实现
1) 四元数结构体定义
view plaincopy to clipboardprint?
typedef struct QUAT_TYPE // 四元数
{
union
{
double M[4];
struct
{
double w, x, y, z;
};
struct
{
double q0;
VECTOR3D qv;
};
};
} QUAT, *QUAT_PTR;
typedef struct QUAT_TYPE // 四元数
{
union
{
double M[4];
struct
{
double w, x, y, z;
};
struct
{
double q0;
VECTOR3D qv;
};
};
} QUAT, *QUAT_PTR;
2) 四元数常用操作函数实现
view plaincopy to clipboardprint?
void _CPPYIN_Math::QuatCreate(QUAT_PTR q, VECTOR3D_PTR v, double theta) // 创建用于旋转的四元数q, v必须为单位向量
{
double theta_div_2 = (0.5)*theta;
double sin_theta = sin(theta_div_2);
q->x = sin_theta * v->x;
q->y = sin_theta * v->y;
q->z = sin_theta * v->z;
q->w = cos( theta_div_2 );
}
void _CPPYIN_Math::QuatGetVectorAndTheta(QUAT_PTR q, VECTOR3D_PTR v, double *theta)
{
*theta = acos(q->w);
double sin_theta_inv = 1.0/sin(*theta);
v->x = q->x * sin_theta_inv;
v->y = q->y * sin_theta_inv;
v->z = q->z * sin_theta_inv;
*theta *= 2;
}
void _CPPYIN_Math::QuatAdd(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum)
{
qsum->x = q1->x + q2->x;
qsum->y = q1->y + q2->y;
qsum->z = q1->z + q2->z;
qsum->w = q1->w + q2->w;
}
void _CPPYIN_Math::QuatSub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff)
{
qdiff->x = q1->x - q2->x;
qdiff->y = q1->y - q2->y;
qdiff->z = q1->z - q2->z;
qdiff->w = q1->w - q2->w;
}
void _CPPYIN_Math::QuatConjugate(QUAT_PTR q, QUAT_PTR qconj) // 求共轭
{
qconj->x = -q->x;
qconj->y = -q->y;
qconj->z = -q->z;
qconj->w = q->w;
}
void _CPPYIN_Math::QuatScale(QUAT_PTR q, double scale, QUAT_PTR qs)
{
qs->x = scale * q->x;
qs->y = scale * q->y;
qs->z = scale * q->z;
qs->w = scale * q->w;
}
double _CPPYIN_Math::QuatNorm(QUAT_PTR q)
{
return sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
}
double _CPPYIN_Math::QuatNorm2(QUAT_PTR q)
{
return q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z;
}
void _CPPYIN_Math::QuatNormalize(QUAT_PTR q, QUAT_PTR qn)
{
double qlength_inv = 1.0/(sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z));
qn->w = q->w * qlength_inv;
qn->x = q->x * qlength_inv;
qn->y = q->y * qlength_inv;
qn->z = q->z * qlength_inv;
}
void _CPPYIN_Math::QuatUnitInverse(QUAT_PTR q, QUAT_PTR qi) // 单位四元数的逆,等于求共轭
{
qi->w = q->w;
qi->x = -q->x;
qi->y = -q->y;
qi->z = -q->z;
}
void _CPPYIN_Math::QuatInverse(QUAT_PTR q, QUAT_PTR qi) // 非单位四元数的逆
{
double norm2_inv = 1.0 / (q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
qi->w = q->w * norm2_inv;
qi->x = -q->x * norm2_inv;
qi->y = -q->y * norm2_inv;
qi->z = -q->z * norm2_inv;
}
void _CPPYIN_Math::QuatMul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod)
{
double prd_0 = (q1->z - q1->y) * (q2->y - q2->z);
double prd_1 = (q1->w + q1->x) * (q2->w + q2->x);
double prd_2 = (q1->w - q1->x) * (q2->y + q2->z);
double prd_3 = (q1->y + q1->z) * (q2->w - q2->x);
double prd_4 = (q1->z - q1->x) * (q2->x - q2->y);
double prd_5 = (q1->z + q1->x) * (q2->x + q2->y);
double prd_6 = (q1->w + q1->y) * (q2->w - q2->z);
double prd_7 = (q1->w - q1->y) * (q2->w + q2->z);
double prd_8 = prd_5 + prd_6 + prd_7;
double prd_9 = 0.5 * (prd_4 + prd_8);
qprod->w = prd_0 + prd_9 - prd_5;
qprod->x = prd_1 + prd_9 - prd_8;
qprod->y = prd_2 + prd_9 - prd_7;
qprod->z = prd_3 + prd_9 - prd_6;
}
void _CPPYIN_Math::QuatMul3(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod) // 三个四元数相乘,用于做旋转变换
{
QUAT qtmp;
QuatMul(q1, q2, &qtmp);
QuatMul(&qtmp, q3, qprod);
}
本文来自CSDN博客,转载请标明出处:http://blog.csdn.net/cppyin/archive/2011/02/10/6177742.aspx
相关文章推荐
- (转)从零实现3D图像引擎:(9)四元数函数库
- 从零实现3D图像引擎:(9)四元数函数库
- (转)从零实现3D图像引擎:(5)3D坐标系函数库
- 【转载】从零实现3D图像引擎:(2)画2D直线不简单
- 从零实现3D图像引擎:(3)超级重要的2D矩形裁剪
- 从零实现3D图像引擎:(6)向量函数库
- (转)从零实现3D图像引擎:(6)向量函数库
- (转)从零实现3D图像引擎:(10)Hello3DWorld
- 从零实现3D图像引擎:(7)矩阵函数库
- 从零实现3D图像引擎:(14)背面消隐的三大陷阱
- (转)从零实现3D图像引擎:(11)苍井空做客讲解3D变换矩阵的推导
- (转)从零实现3D图像引擎:(2)画2D直线不简单
- (转)从零实现3D图像引擎:(7)矩阵函数库
- (转)从零实现3D图像引擎:(3)超级重要的2D矩形裁剪
- (转)从零实现3D图像引擎:(8)参数化直线与3D平面函数库
- 从零实现3D图像引擎:(1)环境配置与项目框架
- 从零实现3D图像引擎:(4)三角函数库
- 从零实现3D图像引擎:(8)参数化直线与3D平面函数库
- 从零实现3D图像引擎:(13)把宽高比、透视投影矩阵、屏幕变换矩阵说透
- (转)从零实现3D图像引擎:(12)构建支持欧拉和UVN的相机系统