Blade Math Lib数学库

BPhy.cpp物理运算文件内容

#include "BMath2.h"
bool		BRay2Triangle(BVector3 pStart,BVector3 pEnd,BVector3 p1,BVector3 p2,BVector3 p3,B_FLOAT* len)
{
	BVector3 e1 = p2 - p1;
	BVector3 e2 = p3 - p1;

	BVector3 pvec;
	BVector3 pDir = pEnd - pStart;
	BVec3Cross(&pvec,&pDir,&e2);

	B_FLOAT det = BVec3Dot(&e1,&pvec);

	BVector3 tvec,qvec;

	if(det>B_DELAY)
	{
		tvec  = pStart - p1;
	}
	else
	{
		tvec = p1 - pStart;
		det = -det;
	}

	if(det<B_DELAY)
		return false;

	B_FLOAT tu,tv;

	tu = BVec3Dot(&tvec,&pvec);
	if(tu<B_DELAY || tu>det)
		return false;

	BVec3Cross(&qvec,&tvec,&e1);
	tv = BVec3Dot(&pDir,&qvec);
	if(tv<B_DELAY || (tu+tv)>det)
		return false;

    if(len!=NULL)
    {
        *len = BVec3Dot(&e2,&qvec);
        *len *= 1.000000f/det;
    }
	return true;
}

bool        BRay2Quad(BVector3 pStart,BVector3 pEnd,BVector3 p1,BVector3 p2,BVector3 p3,BVector3 p4,B_FLOAT* len)
{
/*
	if(BRay2Triangle(pStart,pEnd,p1,p2,p3,len))
		return true;

	if(BRay2Triangle(pStart,pEnd,p3,p2,p4,len))
		return true;
*/

//2013-03-27
    if(BRay2Triangle(pStart,pEnd,p1,p2,p4,len))
		return true;

    if(BRay2Triangle(pStart,pEnd,p4,p2,p3,len))
		return true;

	return false;
}

bool        BRay2Box(BVector3 pStart,BVector3 pEnd,BVector3 MinVector,BVector3 MaxVector,B_FLOAT* len,int* style)
{
	BVector3 p1,p2,p3,p4,p5,p6,p7,p8;

	p1 = BVector3(MinVector._x,MaxVector._y,MinVector._z);
	p2 = BVector3(MaxVector._x,MaxVector._y,MinVector._z);
	p3 = BVector3(MinVector._x,MaxVector._y,MaxVector._z);
	p4 = BVector3(MaxVector._x,MaxVector._y,MaxVector._z);

	p5 = BVector3(MinVector._x,MinVector._y,MinVector._z);
	p6 = BVector3(MaxVector._x,MinVector._y,MinVector._z);
	p7 = BVector3(MinVector._x,MinVector._y,MaxVector._z);
	p8 = BVector3(MaxVector._x,MinVector._y,MaxVector._z);


	//top
	if(BRay2Quad(pStart,pEnd,p1,p2,p3,p4,len))
	{
		if(style!=NULL)
			*style = 0;
		return true;
	}

	//bottom
	if(BRay2Quad(pStart,pEnd,p5,p6,p7,p8,len))
	{
		if(style!=NULL)
			*style = 1;
		return true;
	}

	//front
	if(BRay2Quad(pStart,pEnd,p3,p4,p7,p8,len))
	{
		if(style!=NULL)
			*style = 2;
		return true;
	}

	//back
	if(BRay2Quad(pStart,pEnd,p1,p2,p5,p6,len))
	{
		if(style!=NULL)
			*style = 3;
		return true;
	}


	//left
	if(BRay2Quad(pStart,pEnd,p1,p3,p5,p7,len))
	{
		if(style!=NULL)
			*style = 4;
		return true;
	}

	//right
	if(BRay2Quad(pStart,pEnd,p4,p2,p8,p6,len))
	{
		if(style!=NULL)
			*style = 5;
		return true;
	}

	return false;
}


int	CrashQuad2Quad(BVector2 v1,BVector2 v2,BVector2 m1,BVector2 m2)
{
	B_FLOAT lx = abs(v2._x-v1._x)/2 + abs(m2._x - m1._x)/2;//两中心点间理论距离
	B_FLOAT ly = abs(v2._y-v1._y)/2 + abs(m2._y - m1._y)/2;

	B_FLOAT cx = abs((v2._x+v1._x)/2 - (m2._x+m1._x)/2);
	B_FLOAT cy = abs((v2._y+v1._y)/2 - (m2._y+m1._y)/2);

	if((lx-cx)>B_DELAY && (ly-cy)>B_DELAY)
	{
		return CRASH_IN;

	}

	if(( (lx-cx)>B_DELAY && ly==cy) || (lx==cx && (ly-cy)>B_DELAY))
		return CRASH_TOUCH;

	return CRASH_OUT;
}


int CrashBox2Box(BVector3 Min1,BVector3 Max1,BVector3 Min2,BVector3 Max2)
{
    //two center point real length
    BVector3 len = (Min1+Max1)*0.5f - (Min2+Max2)*0.5f;
    len._x = fabs(len._x);
    len._y = fabs(len._y);
    len._z = fabs(len._z);

    int isCrash = 0;
    int isTouch = 0;

    //two center poing crash length
    B_FLOAT cx = (Max1._x - Min1._x)/2 + (Max2._x - Min2._x)/2 ;
    B_FLOAT cy = (Max1._y - Min1._y)/2 + (Max2._y - Min2._y)/2 ;
    B_FLOAT cz = (Max1._z - Min1._z)/2 + (Max2._z - Min2._z)/2 ;

    if(len._x < cx)
        isCrash++;

    if(len._y < cy)
        isCrash++;

    if(len._z < cz)
        isCrash++;

    if(len._x==cx)
        isTouch++;

    if(len._y==cy)
        isTouch++;

    if(len._z==cz)
        isTouch++;

    if(isCrash>2)
    {
        return CRASH_IN;
    }
    else if( isCrash==2 && isTouch==1)
    {
        return CRASH_TOUCH;
    }

    return CRASH_OUT;

}


bool	CrashTriangle(BVector3 p1,BVector3 p2,BVector3 p3,BVector3 b1,BVector3 b2,BVector3 b3)//三角形碰撞
{
	B_FLOAT len=0;

	if(BRay2Triangle((p1+p2)*0.5f,p3,b1,b2,b3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(p3,(p1+p2)*0.5f,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle((p1+p3)*0.5f,p2,b1,b2,b3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(p2,(p1+p3)*0.5f,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle((p3+p2)*0.5f,p1,b1,b2,b3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(p1,(p3+p2)*0.5f,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	//-----------------------------------------------------------


	if(BRay2Triangle(p1,p2,b1,b2,b3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(p2,p1,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle(p2,p3,b1,b2,b3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(p3,p2,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle(p1,p3,b1,b2,b3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(p3,p1,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}
//----------------------
	if(BRay2Triangle(b1,b2,p1,p2,p3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(b2,b1,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle(b2,b3,p1,p2,p3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(b3,b2,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle(b1,b3,p1,p2,p3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(b3,b1,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	//------------------------------------

	if(BRay2Triangle((b1+b2)*0.5f,b3,p1,p2,p3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(b3,(b1+b2)*0.5f,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle((b1+b3)*0.5f,b2,p1,p2,p3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(b2,(b1+b3)*0.5f,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	if(BRay2Triangle((b3+b2)*0.5f,b1,p1,p2,p3,&len) == true)
	{
		if(len > B_DELAY)
		{
			if(BRay2Triangle(b1,(b3+b2)*0.5f,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
					return true;
			}
		}
	}

	return false;
}

BVector3 GetRayPos(BVector3 pStart,BVector3 pEnd,B_FLOAT v)
{
/*
	BVector3 tt;

	tt = pStart+(pEnd-pStart)*v;
	return tt;
*/
//2013-01-16
	BVector3 p = pEnd-pStart;
	BVector3 pNormal;
	BVec3Normalize(&pNormal,&p);

	if(pNormal._y == v)
	{
		return pStart;
    }

    float n = (v-pStart._y)/pNormal._y;

	return  (pStart+pNormal*n);
}
滚动到顶部