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