BQuater.cpp四元数运算文件内容
#include"BMath2.h"
BQuaternion::BQuaternion()
{
_x = 0;
_y = 0;
_z = 0;
_w = 0;
}
BQuaternion::BQuaternion(B_FLOAT* val)
{
_x = val[0];
_y = val[1];
_z = val[2];
_w = val[3];
}
BQuaternion::BQuaternion(B_FLOAT x,B_FLOAT y,B_FLOAT z,B_FLOAT w)
{
_x = x;
_y = y;
_z = z;
_w = w;
}
BQuaternion BQuaternion::operator +=(BQuaternion q)
{
_x = _x + q._x;
_y = _y + q._y;
_z = _z + q._z;
_w = _w + q._w;
return *this;
}
BQuaternion BQuaternion::operator -=(BQuaternion q)
{
_x = _x + q._x;
_y = _y + q._y;
_z = _z + q._z;
_w = _w + q._w;
return *this;
}
BQuaternion BQuaternion::operator *=(BQuaternion q)
{
/*B_FLOAT w = BSub(BSub(BSub(BMul(_w,q._w),BMul(_x,q._x)),BMul(_y,q._y)),BMul(_z,q._z));
B_FLOAT x = BSub(BAdd(BAdd(BMul(_w,q._x),BMul(_x,q._w)),BMul(_y,q._z)),BMul(_z,q._y));
B_FLOAT y = BSub(BAdd(BAdd(BMul(_w,q._y),BMul(_y,q._w)),BMul(_z,q._x)),BMul(_x,q._z));
B_FLOAT z = BSub(BAdd(BAdd(BMul(_w,q._z),BMul(_z,q._w)),BMul(_x,q._y)),BMul(_y,q._x));*/
B_FLOAT w = _w*q._w - _x*q._x - _y*q._y - _z*q._z;
B_FLOAT x = _w*q._x + _x*q._w + _y*q._z - _z*q._y;
B_FLOAT y = _w*q._y + _y*q._w + _z*q._x - _x*q._z;
B_FLOAT z = _w*q._z + _z*q._w + _x*q._y - _y*q._x;
_x = x;
_y = y;
_z = z;
_w = w;
return *this;
}
BQuaternion BQuaternion::operator *=(B_FLOAT v)
{
_x = _x * v;
_y = _y * v;
_z = _z * v;
_w = _w * v;
return *this;
}
BQuaternion BQuaternion::operator /=(B_FLOAT v)
{
_x = _x / v;
_y = _y / v;
_z = _z / v;
_w = _w / v;
return *this;
}
BQuaternion BQuaternion::operator +()
{
return *this;
}
BQuaternion BQuaternion::operator -()
{
return BQuaternion(-_x,-_y,-_z,-_w);
}
BQuaternion BQuaternion::operator +(BQuaternion q)
{
return BQuaternion(_x + q._x,_y + q._y ,_z + q._z,_w + q._w);
}
BQuaternion BQuaternion::operator -(BQuaternion q)
{
return BQuaternion(_x - q._x,_y - q._y ,_z - q._z,_w - q._w);
}
BQuaternion BQuaternion::operator *(BQuaternion q)
{
B_FLOAT w = _w*q._w - _x*q._x - _y*q._y - _z*q._z;
B_FLOAT x = _w*q._x + _x*q._w + _y*q._z - _z*q._y;
B_FLOAT y = _w*q._y + _y*q._w + _z*q._x - _x*q._z;
B_FLOAT z = _w*q._z + _z*q._w + _x*q._y - _y*q._x;
return BQuaternion(x,y,z,w);
}
BQuaternion BQuaternion::operator *(B_FLOAT v)
{
return BQuaternion(_x * v,_y * v ,_z * v,_w * v);
}
BQuaternion BQuaternion::operator /(B_FLOAT v)
{
return BQuaternion(_x / v,_y / v ,_z / v,_w / v);
}
bool BQuaternion::operator == ( BQuaternion q)
{
if(_x == q._x)
if( _y == q._y)
if(_z == q._z)
if(_w == q._w)
return true;
return false;
}
bool BQuaternion::operator != ( BQuaternion q)
{
if(_x == q._x)
if( _y == q._y)
if(_z == q._z)
if(_w == q._w)
return false;
return true;
}
B_FLOAT BQuatLengthSq(BQuaternion* src)
{
B_FLOAT v = src->_x*src->_x + src->_y*src->_y + src->_z*src->_z + src->_w*src->_w;
return v;
}
BQuaternion* BQuatNormalize(BQuaternion* dest,BQuaternion* src)
{
B_FLOAT s = sqrt(BQuatLengthSq(src));
*dest = *src /s;
return dest;
}
BQuaternion* BQuatSlerp(BQuaternion* dest,BQuaternion* src1, BQuaternion* src2, B_FLOAT s,bool line)
{
if(line)
{
dest->_x = (1.0f-s)*src1->_x + s*src2->_x;
dest->_y = (1.0f-s)*src1->_y + s*src2->_y;
dest->_z = (1.0f-s)*src1->_z + s*src2->_z;
dest->_w = (1.0f-s)*src1->_w + s*src2->_w;
return dest;
}
B_FLOAT tol[4];
B_FLOAT omega, cosom, sinom, scale0, scale1;
cosom = (src1->_x * src2->_x) + (src1->_y * src2->_y) + (src1->_z + src2->_z) * (src1->_w * src2->_w);
if(cosom<0.0)
{
cosom = -cosom;
tol[0] = -src2->_x;
tol[1] = -src2->_y;
tol[2] = -src2->_z;
tol[3] = -src2->_w;
}
else
{
tol[0] = -src2->_x;
tol[1] = -src2->_y;
tol[2] = -src2->_z;
tol[3] = -src2->_w;
}
if((1.0-cosom)>0.01f)
{
omega = acos(cosom);
sinom = sin(omega);
scale0 = sin((1.0f - s) * omega) / sinom;
scale1 = sin(s * omega) / sinom;
}
else
{
scale0 = 1.0f - s;
scale1 = s;
}
dest->_x = scale0 * src1->_x + scale1 * tol[0];
dest->_y = scale0 * src1->_y + scale1 * tol[1];
dest->_z = scale0 * src1->_z + scale1 * tol[2];
dest->_w = scale0 * src1->_w + scale1 * tol[3];
return dest;
}
BQuaternion* BQuatRotationAxis(BQuaternion* dest,BVector3* pv,B_FLOAT angle)
{
BVector3 c;
BVec3Normalize(&c,pv);
BVector3 t = c * sin(angle*0.5f);
dest->_x = t._x;
dest->_y = t._y;
dest->_z = t._z;
dest->_w = cos(angle*0.5f);
return dest;
}
void MatToQuat(BQuaternion* dest,BMatrix* src)
{
B_FLOAT s;
B_FLOAT tq[4];
int i, j;
// Use tq to store the largest trace
tq[0] = 1.0f + src->bm[0]+src->bm[5]+src->bm[10];
tq[1] = 1.0f + src->bm[0]-src->bm[5]-src->bm[10];
tq[2] = 1.0f - src->bm[0]+src->bm[5]-src->bm[10];
tq[3] = 1.0f - src->bm[0]-src->bm[5]+src->bm[10];
// 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 */
dest->_w = tq[0];
dest->_x = src->bm[6]-src->bm[9];
dest->_y = src->bm[8]-src->bm[2];
dest->_z = src->bm[1]-src->bm[4];
}
else if (j==1)
{
dest->_w = src->bm[6]-src->bm[9];
dest->_x = tq[1];
dest->_y = src->bm[1]+src->bm[4];
dest->_z = src->bm[8]+src->bm[2];
}
else if (j==2)
{
dest->_w = src->bm[8]-src->bm[2];
dest->_x = src->bm[1]+src->bm[4];
dest->_y = tq[2];
dest->_z = src->bm[6]+src->bm[9];
}
else /* if (j==3) */
{
dest->_w = src->bm[1]-src->bm[4];
dest->_x = src->bm[8]+src->bm[2];
dest->_y = src->bm[6]+src->bm[9];
dest->_z = tq[3];
}
s = BSqrt(0.25f / tq[j]);
*dest *=s;
}
void QuatToMat(BMatrix* dest,BQuaternion* src)
{
B_FLOAT wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
//计算相关的系数
x2 = src->_x + src->_x;
y2 = src->_y + src->_y;
z2 = src->_z + src->_z;
xx = src->_x * x2;
xy = src->_x * y2;
xz = src->_x * z2;
yy = src->_y * y2;
yz = src->_y * z2;
zz = src->_z * z2;
wx = src->_w * x2;
wy = src->_w * y2;
wz = src->_w * z2;
//将其填入矩阵位置
dest->bm[0] = 1.000000f - (yy +zz);
dest->bm[4] = xy - wz;
dest->bm[8] = xz + wy;
dest->bm[12] = 0.0f;
dest->bm[1] = xy + wz;
dest->bm[5] = 1.000000f-(xx+zz);
dest->bm[9] = yz - wx;
dest->bm[13] = 0.0f;
dest->bm[2] = xz - wy;
dest->bm[6] = yz + wx;
dest->bm[10] = 1.000000f - (xx + yy);
dest->bm[14] = 0.0f;
dest->bm[3] = 0.0f;
dest->bm[7] = 0.0f;
dest->bm[11] = 0.0f;
dest->bm[15] = 1.000000f;
}