这一节 ,应该是个一点点进行
QUAT q1={0,0,0,0},
先看下QUAT数据类型
//四元数
typedef struct QUAT_TYP
{
union
{
float M[4];
struct
{
float q0;
VECTOR3D qv;
};
struct
{
float w, x, y, z;
};
};
}QUAT, * QUAT_PTR;
先进行3D方向向量旋转一个角度来初始化一个四元数
旋转是四元数中非常重要的用途,
void ddraw_math::VECTOR3D_Theta_To_Quat(QUAT_PTR q, VECTOR3D_PTR v, float theta)
{
float theta_div_2 = 0.5 * theta;
float sin_theta = sinf( theta_div_2 );
q->x = sin_theta * v->x;
q->y = sin_theta * v->y;
q->z = sin_theta * v->z;
q->w = cosf( theta_div_2 );
}
两四元数相加,
void ddraw_math::QUAT_Add(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 ddraw_math::QUAT_Sub(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 ddraw_math::QUAT_Normalize(QUAT_PTR q, QUAT_PTR qn)
{
float qlength_inv = 1.0 / ( sqrtf( 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 ddraw_math::QUAT_Normalize(QUAT_PTR q)
{
float qlength_inv = 1.0 / ( sqrtf( q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z ) );
q->w = q->w * qlength_inv;
q->x = q->x * qlength_inv;
q->y = q->y * qlength_inv;
q->z = q->z * qlength_inv;
}
求四元数共轭
void ddraw_math::QUAT_Conjugate(QUAT_PTR q, QUAT_PTR qconj)
{
qconj->x = -q->x;
qconj->y = -q->y;
qconj->z = -q->z;
qconj->w = q->w;
}
四元数的逆为共轭/模的平方
void ddraw_math::QUAT_Inverse(QUAT_PTR q, QUAT_PTR qi)
{
float 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 ddraw_math::QUAT_Inverse(QUAT_PTR q)
{
float norm2_inv = 1.0 / ( q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z ) ;
q->w = q->w * norm2_inv;
q->x = -q->x * norm2_inv;
q->y = -q->y * norm2_inv;
q->z = -q->z * norm2_inv;
}
接下来进行四元数相乘。
void ddraw_math::QUAT_Mul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod)
{
qprod->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
qprod->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
qprod->y = q1->w * q2->y - q1->x * q2->z + q1->y * q2->w - q1->z * q2->x;
qprod->z = q1->w * q2->z + q1->x * q2->y - q1->y * q2->x + q1->z * q2->w;
}
通过3D向量求四元数
void ddraw_math::QUAT_INIT_VECTOR3D(QUAT_PTR q, VECTOR3D_PTR v)
{
q->w = 0;
q->x = v->x;
q->y = v->y;
q->z = v->z;
}
下一步求3个四元数相乘,由于(q*)*(v)*(q)或者(q)*(v)*(q*)是用于旋转向量或点V的三重乘积,所以很有用。
实现起来倒是简单,两两相乘
void ddraw_math::QUAT_Triple_Product(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, int (*Rotate_Polygon2D)(POLYGON2D_PTR, int))
{
QUAT qtmp;
QUAT_Mul( q1, q2, &qtmp );
QUAT_Mul( & qtmp, q3, qprod );
}
清零
void ddraw_math::QUAT_ZERO(QUAT_PTR q)
{
q->w = 0;
q->x = 0;
q->y = 0;
q->z = 0;
}
DEMO到这里进行完了,现在进行下四元数残余。
单位四元数的求逆,也就是共轭
void ddraw_math::QUAT_Unit_Inverse(QUAT_PTR q )
{
q->x = -q->x;
q->y = -q->y;
q->z = -q->z;
}
void ddraw_math::QUAT_Unit_Inverse(QUAT_PTR q, QUAT_PTR qi )
{
qi->w = q->w;
qi->x = -q->x;
qi->y = -q->y;
qi->z = -q->z;
}
4D向量旋转角度创建一个旋转四元数。
void ddraw_math::VECTOR4D_Theta_To_Quat(QUAT_PTR q, VECTOR4D_PTR v, float theta)
{
float theta_div_2 = 0.5 * theta;
float sin_theta = sinf( theta_div_2 );
q->x = sin_theta * v->x;
q->y = sin_theta * v->y;
q->z = sin_theta * v->z;
q->w = cosf( theta_div_2 );
}
根据绕X,Y,Z轴旋转的角度,创建一个ZYX顺序旋转对应的四元数
void ddraw_math::EulerZYX_To_QUAT(QUAT_PTR q, float theta_z, float theta_y, float theta_x)
{
float cos_z_2 = 0.5 * cosf( theta_z );
float cos_y_2 = 0.5 * cosf( theta_y );
float cos_x_2 = 0.5 * cosf( theta_x );
float sin_z_2 = 0.5 * sinf( theta_z );
float sin_y_2 = 0.5 * sinf( theta_y );
float sin_x_2 = 0.5 * sinf( theta_x );
q->w = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2;
q->x = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2;
q->y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2;
q->z = sin_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_x * sin_x_2;
}
下一步进行单位四元数转换为一个单位方向向量和一个绕该向量旋转的角度。
void ddraw_math::QUAT_To_VECTOR3D_Theta(QUAT_PTR q, VECTOR3D_PTR v, float *theta)
{
* theta = acosf( q->w );
float sinf_theta_inv = 1.0 / sinf( * theta );
v->x = q->x * sinf_theta_inv;
v->y = q->y * sinf_theta_inv;
v->z = q->z * sinf_theta_inv;
*theta *= 2;
}
对四元数的放缩。
void ddraw_math::QUAT_Scale(QUAT_PTR q, float scale, QUAT_PTR qs)
{
qs->x = scale * q->x;
qs->y = scale * q->y;
qs->z = scale * q->z;
qs->w = scale * q->w;
}
另一种形式
void ddraw_math::QUAT_Scale(QUAT_PTR q, float scale)
{
q->x = scale * q->x;
q->y = scale * q->y;
q->z = scale * q->z;
q->w = scale * q->w;
}
计算四元数的长度。
float ddraw_math::QUAT_Norm(QUAT_PTR q)
{
return ( sqrtf( q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w ) );
}
扩展到计算四元数长度的平方。
float ddraw_math::QUAT_Norm2(QUAT_PTR q)
{
return ( q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w );
}