Blade Math Lib数学库

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;
}