Blade Math Lib数学库

BVector.cpp向量运算文件内容

#include "BMath2.h"

//-==========================

BVector2::BVector2()
{
	_x = 0.0f;
	_y = 0.0f;
}

BVector2::BVector2(B_FLOAT x,B_FLOAT y)
{
	_x = x;
	_y = y;
}

BVector2::BVector2(B_FLOAT *v)
{
	_x = v[0];
	_y = v[1];
}


BVector2 BVector2::operator =( BVector2 v )
{
	_x = v._x;
	_y = v._y;

	return *this;
}

BVector2 BVector2::operator =( B_FLOAT* v )
{
	_x = v[0];
	_y = v[1];

	return *this;
}


BVector2 BVector2::operator +=( BVector2 v )
{
	_x = _x+v._x;
	_y = _y+v._y;

	return *this;
}

BVector2 BVector2::operator -=( BVector2 v )
{
	_x = _x-v._x;
	_y = _y-v._y;

	return *this;
}

BVector2 BVector2::operator *=( B_FLOAT v)
{
	_x = _x*v;
	_y = _y*v;

	return *this;
}

BVector2 BVector2::operator /=( B_FLOAT v)
{
	_x = _x/v;
	_y = _y/v;

	return *this;
}

BVector2  BVector2::operator +(BVector2 V)
{
	return BVector2(_x+V._x,_y+V._y);
}

BVector2  BVector2::operator -(BVector2 V)
{
	return BVector2(_x-V._x,_y-V._y);
}

BVector2  BVector2::operator *(B_FLOAT V)
{
	return BVector2(_x*V,_y*V);
}

BVector2  BVector2::operator /(B_FLOAT V)
{
	return BVector2(_x/V,_y/V);
}

BVector2 BVector2::operator +()
{
	return *this;
}

BVector2 BVector2::operator -()
{
	return BVector2(-_x,-_y);
}

bool BVector2::operator ==( BVector2 v)
{
	if(abs(_x- v._x)<0.000001f &&
	   abs(_y- v._y)<0.000001f )
		return true;

	return false;
}

bool BVector2::operator !=( BVector2 v )
{
	if(abs(_x- v._x)>=0.000001f ||
	   abs(_y- v._y)>=0.000001f)
	   return true;

	return false;
}

//vector3
BVector3::BVector3()
{
	_x = 0.0f;
	_y = 0.0f;
	_z = 0.0f;
}

BVector3::BVector3(B_FLOAT x,B_FLOAT y,B_FLOAT z)
{
	_x = x;
	_y = y;
	_z = z;
}

BVector3::BVector3(B_FLOAT *v)
{
	_x = v[0];
	_y = v[1];
	_z = v[2];
}


BVector3 BVector3::operator =( BVector3 v )
{
	_x = v._x;
	_y = v._y;
	_z = v._z;

	return *this;
}

BVector3 BVector3::operator =( B_FLOAT* v )
{
	_x = v[0];
	_y = v[1];
	_z = v[2];

	return *this;
}


BVector3 BVector3::operator +=( BVector3 v )
{
	_x = _x+v._x;
	_y = _y+v._y;
	_z = _z+v._z;

	return *this;
}

BVector3 BVector3::operator -=( BVector3 v )
{
	_x = _x-v._x;
	_y = _y-v._y;
	_z = _z-v._z;

	return *this;
}

BVector3 BVector3::operator *=( B_FLOAT v)
{
	_x = _x*v;
	_y = _y*v;
	_z = _z*v;

	return *this;
}

BVector3 BVector3::operator /=( B_FLOAT v)
{
	_x = _x/v;
	_y = _y/v;
	_z = _z/v;

	return *this;
}

BVector3  BVector3::operator +(BVector3 V)
{
	return BVector3(_x+V._x,_y+V._y,_z+V._z);
}

BVector3  BVector3::operator -(BVector3 V)
{
	return BVector3(_x-V._x,_y-V._y,_z-V._z);
}

BVector3  BVector3::operator *(B_FLOAT V)
{
	return BVector3(_x*V,_y*V,_z*V);
}

BVector3  BVector3::operator /(B_FLOAT V)
{
	return BVector3(_x/V,_y/V,_z/V);
}

BVector3 BVector3::operator +()
{
	return *this;
}

BVector3 BVector3::operator -()
{
	return BVector3(-_x,-_y,-_z);
}

bool BVector3::operator ==( BVector3 v)
{
	if(abs(_x- v._x)<0.000001f &&
	   abs(_y- v._y)<0.000001f &&
	   abs(_z- v._z)<0.000001f)
		return true;

	return false;
}

bool BVector3::operator !=( BVector3 v )
{
	if(abs(_x- v._x)>=0.000001f ||
	   abs(_y- v._y)>=0.000001f ||
	   abs(_z- v._z)>=0.000001f)
	   return true;

	return false;
}

// BVector4
BVector4::BVector4()
{
	_x = 0.0f;
	_y = 0.0f;
	_z = 0.0f;
	_w = 0.0f;
}

BVector4::BVector4(B_FLOAT x,B_FLOAT y,B_FLOAT z,B_FLOAT w)
{
	_x = x;
	_y = y;
	_z = z;
	_w = w;
}

BVector4::BVector4(B_FLOAT *v)
{
	_x = v[0];
	_y = v[1];
	_z = v[2];
	_w = v[3];
}

BVector4 BVector4::operator =( BVector4 v )
{
	_x = v._x;
	_y = v._y;
	_z = v._z;
	_w = v._w;

	return *this;
}

BVector4 BVector4::operator =( B_FLOAT* v )
{
	_x = v[0];
	_y = v[1];
	_z = v[2];
	_w = v[3];

	return *this;
}


BVector4 BVector4::operator +=( BVector4 v )
{
	BAdd4(_v,_v,v._v);

	return *this;
}

BVector4 BVector4::operator -=( BVector4 v )
{
	BSub4(_v,_v,v._v);
	return *this;
}

BVector4 BVector4::operator *=( B_FLOAT v)
{
	BMul4(_v,_v,v);
	return *this;
}

BVector4 BVector4::operator /=( B_FLOAT v)
{
	BDiv4(_v,_v,v);

	return *this;
}

BVector4  BVector4::operator +(BVector4 V)
{
	return BVector4(_x+V._x,_y+V._y,_z+V._z,_w+V._w);
}

BVector4  BVector4::operator -(BVector4 V)
{
	return BVector4(_x-V._x,_y-V._y,_z-V._z,_w-V._w);
}

BVector4  BVector4::operator *(B_FLOAT V)
{
	return BVector4(_x*V,_y*V,_z*V,_w*V);
}

BVector4  BVector4::operator /(B_FLOAT V)
{
	return BVector4(_x/V,_y/V,_z/V,_w/V);
}

BVector4 BVector4::operator +()
{
	return *this;
}

BVector4 BVector4::operator -()
{
	return BVector4(-_x,-_y,-_z,_w);
}

bool BVector4::operator ==( BVector4 v)
{
	if(abs(_x- v._x)<0.000001f &&
	   abs(_y- v._y)<0.000001f &&
	   abs(_z- v._z)<0.000001f &&
	   abs(_w- v._w)<0.000001f)
		return true;

	return false;
}

bool BVector4::operator !=( BVector4 v )
{
	if(abs(_x- v._x)>=0.000001f ||
	   abs(_y- v._y)>=0.000001f ||
	   abs(_z- v._z)>=0.000001f ||
	   abs(_w- v._w)>=0.000001f)
	   return true;

	return false;
}

BVector2* BVec2Lerp(BVector2* dest,BVector2* src1,BVector2* src2,B_FLOAT s)
{
	dest->_x = src1->_x + ( src2->_x - src1->_x ) * s ;
	dest->_y = src1->_y + ( src2->_y - src1->_y ) * s ;

	return dest;
}

B_FLOAT		BVec2LengthSq(BVector2* src)
{
	return (src->_x*src->_x) + (src->_y * src->_y);
}

B_FLOAT       BVec2DistanceSq(BVector2* src,BVector2* det)
{
		return (src->_x-det->_x)*(src->_x-det->_x)+(src->_y-det->_y)*(src->_y-det->_y);
}

BVector2*	BVec2Maximize(BVector2* dest,BVector2* src1,BVector2* src2)
{
	if( src1->_x > src2->_x )
		dest->_x = src1->_x;
	else
		dest->_x = src2->_x;

	if( src1->_y > src2->_y )
		dest->_y = src1->_y;
	else
		dest->_y = src2->_y;

	return dest;
}

BVector2*   BVec2Minimize(BVector2* dest,BVector2* src1,BVector2* src2)
{
	if( src1->_x < src2->_x )
		dest->_x = src1->_x;
	else
		dest->_x = src2->_x;

	if( src1->_y < src2->_y )
		dest->_y = src1->_y;
	else
		dest->_y = src2->_y;

	return dest;
}

BVector2*   BVec2Normalize(BVector2* dest,BVector2* src)
{
	B_FLOAT s = BSqrt( src->_x*src->_x + src->_y*src->_y);

	*dest = *src / s;

	return dest;
}

B_FLOAT		BVec2Cross(BVector2* src1,BVector2* src2)
{
	return src1->_x * src2->_y - src1->_y * src2->_x;
}

B_FLOAT		BVec2Dot(BVector2* src1,BVector2* src2)
{
	return src1->_x*src2->_x + src1->_y*src2->_y;
}

B_FLOAT     BVec2ComputAngle(BVector2* v1,BVector2* v2,BVector2* v3)
{
    float val = (v2->_x-v1->_x) * (v2->_x-v3->_x) + (v2->_y-v1->_y) * (v2->_y-v3->_y) ;

    float t1 = sqrt( (v2->_x-v1->_x) * (v2->_x-v1->_x) + (v2->_y-v1->_y) * (v2->_y-v1->_y) );
    float t2 = sqrt( (v2->_x-v3->_x) * (v2->_x-v3->_x) + (v2->_y-v3->_y) * (v2->_y-v3->_y) );

    return acos(val/(t1*t2));
}


BVector3*	BVec3Lerp(BVector3* dest,BVector3* src1,BVector3* src2,B_FLOAT s)
{
	dest->_x =  src1->_x +  ( src2->_x - src1->_x ) *s ;
	dest->_y =  src1->_y +  ( src2->_y - src1->_y ) *s ;
	dest->_z =  src1->_z +  ( src2->_z - src1->_z ) *s ;

	return dest;
}


B_FLOAT		BVec3LengthSq(BVector3* src)
{
	return src->_x*src->_x+src->_y * src->_y + src->_z*src->_z;
}

B_FLOAT       BVec3DistanceSq(BVector3* src,BVector3* det)
{
	return (src->_x-det->_x)*(src->_x-det->_x)+(src->_y-det->_y)*(src->_y-det->_y)+(src->_z-det->_z)*(src->_z-det->_z);

}

BVector3*	BVec3Maximize(BVector3* dest,BVector3* src1,BVector3* src2)
{
	if( src1->_x > src2->_x )
		dest->_x = src1->_x;
	else
		dest->_x = src2->_x;

	if( src1->_y > src2->_y )
		dest->_y = src1->_y;
	else
		dest->_y = src2->_y;

	if( src1->_z > src2->_z )
		dest->_z = src1->_z;
	else
		dest->_z = src2->_z;

	return dest;
}

BVector3*   BVec3Minimize(BVector3* dest,BVector3* src1,BVector3* src2)
{
	if( src1->_x < src2->_x )
		dest->_x = src1->_x;
	else
		dest->_x = src2->_x;

	if( src1->_y < src2->_y )
		dest->_y = src1->_y;
	else
		dest->_y = src2->_y;

	if( src1->_z < src2->_z )
		dest->_z = src1->_z;
	else
		dest->_z = src2->_z;

	return dest;
}

BVector3*   BVec3Normalize(BVector3* dest,BVector3* src)
{
	B_FLOAT s = sqrt(BVec3LengthSq(src));

	*dest = *src / s;

	return dest;
}

BVector3*		BVec3Cross(BVector3* dest,BVector3* src1,BVector3* src2)
{
	dest->_x = src1->_y * src2->_z - src1->_z * src2->_y;
	dest->_y = src1->_z * src2->_x - src1->_x * src2->_z;
	dest->_z = src1->_x * src2->_y - src1->_y * src2->_x;

	return dest;
}

B_FLOAT		BVec3Dot(BVector3* src1,BVector3* src2)
{
	return src1->_x*src2->_x + src1->_y*src2->_y + src1->_z*src2->_z;
}


B_FLOAT     BVec3ComputAngle(BVector3* v1,BVector3* v2,BVector3* v3)
{
    float val = (v2->_x-v1->_x) * (v2->_x-v3->_x) + (v2->_y-v1->_y) * (v2->_y-v3->_y) + (v2->_z-v1->_z) * (v2->_z-v3->_z);

    float t1 = sqrt( (v2->_x-v1->_x) * (v2->_x-v1->_x) + (v2->_y-v1->_y) * (v2->_y-v1->_y) + (v2->_z-v1->_z) * (v2->_z-v1->_z));
    float t2 = sqrt( (v2->_x-v3->_x) * (v2->_x-v3->_x) + (v2->_y-v3->_y) * (v2->_y-v3->_y) + (v2->_z-v3->_z) * (v2->_z-v3->_z));

    return acos(val/(t1*t2));
}

BVector4*	BVec4Lerp(BVector4* dest,BVector4* src1,BVector4* src2,B_FLOAT s)
{
	dest->_x = src1->_x + (src2->_x - src1->_x)  *s ;
	dest->_y = src1->_y + (src2->_y - src1->_y)  *s ;
	dest->_z = src1->_z + (src2->_z - src1->_z)  *s ;
	dest->_w = src1->_w + (src2->_w - src1->_w)  *s ;

	return dest;
}

B_FLOAT		BVec4LengthSq(BVector4* src)
{
	return src->_x*src->_x+src->_y * src->_y + src->_z*src->_z + src->_w *src->_w;
}

B_FLOAT       BVec4DistanceSq(BVector4* src,BVector4* det)
{
	return (src->_x-det->_x)*(src->_x-det->_x)+(src->_y-det->_y)*(src->_y-det->_y)+(src->_z-det->_z)*(src->_z-det->_z)+(src->_w-det->_w)*(src->_w-det->_w);

}

BVector4*	BVec4Maximize(BVector4* dest,BVector4* src1,BVector4* src2)
{
	if( src1->_x > src2->_x )
		dest->_x = src1->_x;
	else
		dest->_x = src2->_x;

	if( src1->_y > src2->_y )
		dest->_y = src1->_y;
	else
		dest->_y = src2->_y;

	if( src1->_z > src2->_z )
		dest->_z = src1->_z;
	else
		dest->_z = src2->_z;

	if( src1->_w > src2->_w )
		dest->_w = src1->_w;
	else
		dest->_w = src2->_w;

	return dest;
}

BVector4*   BVec4Minimize(BVector4* dest,BVector4* src1,BVector4* src2)
{
	if( src1->_x < src2->_x )
		dest->_x = src1->_x;
	else
		dest->_x = src2->_x;

	if( src1->_y < src2->_y )
		dest->_y = src1->_y;
	else
		dest->_y = src2->_y;

	if( src1->_z < src2->_z )
		dest->_z = src1->_z;
	else
		dest->_z = src2->_z;

	if( src1->_w < src2->_w )
		dest->_w = src1->_w;
	else
		dest->_w = src2->_w;

	return dest;
}

BVector4*   BVec4Normalize(BVector4* dest,BVector4* src)
{
	B_FLOAT s = sqrt(BVec4LengthSq(src));

	*dest = *src / s;

	return dest;
}

BVector4* 	BVec4Cross(BVector4* dest,BVector4* src1,BVector4* src2,BVector4* src3)
{
	B_FLOAT a1 = (src2->_z*src3->_w)-(src2->_w*src3->_z);
	B_FLOAT a2 = (src2->_y*src3->_w)-(src2->_w*src3->_y);
	B_FLOAT a3 = (src2->_y*src3->_z)-(src2->_z*src3->_y);
	B_FLOAT a4 = (src2->_x*src3->_z)-(src2->_z*src3->_x);
	B_FLOAT a5 = (src2->_x*src3->_w)-(src2->_w*src3->_x);
	B_FLOAT a6 = (src2->_x*src3->_y)-(src2->_y*src3->_x);

	dest->_x =  (src1->_y*a1) - (src1->_z*a2)  + (src1->_w*a3);
	dest->_y = -((src1->_x*a1) - (src1->_z*a5)  + (src1->_w*a4));
	dest->_z =  (src1->_x*a2) - (src1->_y*a5)  + (src1->_w*a6);
	dest->_w = -((src1->_x*a3) - (src1->_y*a4)  + (src1->_z*a6));

	return dest;
}

B_FLOAT		BVec4Dot(BVector4* src1,BVector4* src2)
{
	return src1->_x*src2->_x + src1->_y*src2->_y + src1->_z*src2->_z + src1->_w*src2->_w;
}


BVector4*   BVec2Transform(BVector4* dest,BVector2* src,BMatrix*  m)
{
	BVector4 T(src->_x,src->_y,0,1.0f);

	BVec4Transform(dest,&T,m);

	return dest;
}

BVector4*   BVec3Transform(BVector4* dest,BVector3* src,BMatrix*  m)
{
	BVector4 T(src->_x,src->_y,src->_z,1.0f);

	BVec4Transform(dest,&T,m);

	return dest;
}

BVector4*   BVec4Transform(BVector4* dest,BVector4* src,BMatrix*  m)
{
	dest->_x =  src->_x*m->bm[0]+src->_y*m->bm[4]+src->_z*m->bm[8]+src->_w*m->bm[12];
	dest->_y =  src->_x*m->bm[1]+src->_y*m->bm[5]+src->_z*m->bm[9]+src->_w*m->bm[13];
	dest->_z =  src->_x*m->bm[2]+src->_y*m->bm[6]+src->_z*m->bm[10]+src->_w*m->bm[14];
	dest->_w =  src->_x*m->bm[3]+src->_y*m->bm[7]+src->_z*m->bm[11]+src->_w*m->bm[15];

	return dest;
}

BVector2*	BVec2BaryCentric(BVector2* dest,BVector2* point1,BVector2* point2,BVector2* point3,B_FLOAT f,B_FLOAT g)
{
	*dest = *point1 + (*point2- *point1)*f + (*point3 - *point1)*g;
	return dest;
}

BVector3*	BVec3BaryCentric(BVector3* dest,BVector3* point1,BVector3* point2,BVector3* point3,B_FLOAT f,B_FLOAT g)
{
	*dest = *point1 + (*point2- *point1)*f + (*point3 - *point1)*g;
	return dest;
}

BVector2*   BVec2CatmullRom(BVector2* dest,BVector2* src1,BVector2* src2,BVector2* src3,BVector2* src4,B_FLOAT s)
{
	BVector2 t1 = (*src3 - *src1)/2;
	BVector2 t2 = (*src4 - *src2)/2;

	B_FLOAT s2 = BPower(s,2);
	B_FLOAT s3 = BPower(s,3);

	*dest = *src2 * (2*s3-3*s2+1) + *src3 * (-2*s3+3*s2) + t1*(s3-2*s2+s) + t2*(s3-s2);

	return dest;
}

BVector3*   BVec3CatmullRom(BVector3* dest,BVector3* src1,BVector3* src2,BVector3* src3,BVector3* src4,B_FLOAT s)
{
	BVector3 t1 = (*src3 - *src1)/2;
	BVector3 t2 = (*src4 - *src2)/2;

	B_FLOAT s2 = BPower(s,2);
	B_FLOAT s3 = BPower(s,3);

	*dest = *src2 * (2*s3-3*s2+1) + *src3 * (-2*s3+3*s2) + t1*(s3-2*s2+s) + t2*(s3-s2);
	return dest;
}

BVector4*   BVec4CatmullRom(BVector4* dest,BVector4* src1,BVector4* src2,BVector4* src3,BVector4* src4,B_FLOAT s)
{
	BVector4 t1 = (*src3 - *src1)/2;
	BVector4 t2 = (*src4 - *src2)/2;

	B_FLOAT s2 = BPower(s,2);
	B_FLOAT s3 = BPower(s,3);

	*dest = *src2 * (2*s3-3*s2+1) + *src3 * (-2*s3+3*s2) + t1*(s3-2*s2+s) + t2*(s3-s2);

	return dest;
}

BVector2*   BVec2Hermite(BVector2* dest,BVector2* src1,BVector2* tan1,BVector2* src2,BVector2* tan2,B_FLOAT s)
{
	BVector2 A = *src1 * 2 - *src2 * 2 + *tan2 + *tan1;
	BVector2 B = *src2 * 3 - *src1 * 3 - *tan1*2 - *tan2;

	*dest = A*BPower(s,3) + B*BPower(s,2) + *tan1*s + *src1;

	return dest;
}

BVector3*   BVec3Hermite(BVector3* dest,BVector3* src1,BVector3* tan1,BVector3* src2,BVector3* tan2,B_FLOAT s)
{
	BVector3 A = *src1 * 2 - *src2 * 2 + *tan2 + *tan1;
	BVector3 B = *src2 * 3 - *src1 * 3 - *tan1*2 - *tan2;

	*dest = A*BPower(s,3) + B*BPower(s,2) + *tan1*s + *src1;

	return dest;
}


BVector4*   BVec4Hermite(BVector4* dest,BVector4* src1,BVector4* tan1,BVector4* src2,BVector4* tan2,B_FLOAT s)
{
	BVector4 A = *src1 * 2 - *src2 * 2 + *tan2 + *tan1;
	BVector4 B = *src2 * 3 - *src1 * 3 - *tan1*2 - *tan2;

	*dest = A*BPower(s,3) + B*BPower(s,2) + *tan1*s + *src1;

	return dest;
}
滚动到顶部