立锥体
#include <iostream> #include <math.h> using namespace std; typedef struct { float x,y,z; }VECTOR3; typedef struct { VECTOR3 v[8]; }BoundingBox; unsigned char collision_BoundingBox(BoundingBox *b1,BoundingBox *b2); unsigned char isInFrustum(VECTOR3 v0,VECTOR3 v1,VECTOR3 v2,VECTOR3 v3,VECTOR3 v4,VECTOR3 vTest); VECTOR3 unitFunc(VECTOR3 V); VECTOR3 vec(float x,float y , float z); VECTOR3 flatEquation(VECTOR3 v1,VECTOR3 v2,VECTOR3 v3,VECTOR3 p1,VECTOR3 p2); float distance ( VECTOR3 v1,VECTOR3 v2); /* v1 v2 v3 v4 为长方体 v1------v2 | | | c1 | | | v4------v3 v5 为立方锥的 顶点 */ unsigned char isInFrustum(VECTOR3 v0,VECTOR3 v1,VECTOR3 v2,VECTOR3 v3,VECTOR3 v4,VECTOR3 vTest)// { VECTOR3 c; c.x = (v1.x + v3.x ) /2 ; c.y = (v1.y + v3.y ) /2 ; c.z = (v1.z + v3.z ) /2 ; VECTOR3 V_ca;//中心轴 c->v0 V_ca.x = v0.x - c.x ; V_ca.y = v0.x - c.y ; V_ca.z = v0.x - c.z ; VECTOR3 V_ct;// c －>vTect; V_ct.x = vTest.x - c.x ; V_ct.y = vTest.x - c.y ; V_ct.z = vTest.x - c.z ; VECTOR3 V_ca_unit; //中心轴 c->v0 向量化 V_ca_unit = unitFunc (V_ca ); float temp; temp = V_ct.x * V_ca_unit.x + V_ct.y * V_ca_unit.y + V_ct.z * V_ca_unit.z ; VECTOR3 V_ca_p; //vTest 在中心轴的向量 V_ca_p.x = temp * V_ca_unit.x; V_ca_p.y = temp * V_ca_unit.y; V_ca_p.z = temp * V_ca_unit.z; VECTOR3 cap;//投影点的具体位置 cap = vec ( V_ca_p.x + c.x ,V_ca_p.y + c.y ,V_ca_p.z + c.z ) ; VECTOR3 Node = flatEquation ( v0,v2,v3,cap,vTest);//交点 float d1 = distance ( cap,Node ); float d2 = distance ( cap,vTest); if ( d1 <d2 ) { return 0;//不在 }else if d1 == d2 ) { return 0 ; //刚好在边上 } return 0; } //直线与平面求交点函数 VECTOR3 flatEquation(VECTOR3 v1,VECTOR3 v2,VECTOR3 v3,VECTOR3 p1,VECTOR3 p2) { VECTOR3 V_12,V_13; V_12 = vec ( v2.x - v1.x ,v2.y - v1.y ,v2.z - v1.z ); V_13 = vec ( v3.x - v1.x ,v3.y - v1.y ,v3.z - v1.z ); VECTOR3 nf; nf = vec ( V_12.y * V_13.z - V_12.z * V_13.y , V_12.z * V_13.x - V_12.x * V_13.z ,V_12.z * V_13.y - V_12.y * V_13.x ); //法向量 //float X,Y,Z; float t; //nf.x * X + nf.y * Y + nf.z * Z == v1.x + v1.y + v1.z ;//平面方程 //nf.x * ( p1.x + t * ( p2.x - p1.x )) + nf.y * ( p1.y + t * ( p2.y - p1.y )) + nf.z * ( p1.z + t * ( p2.z - p1.z )) == v1.x + v1.y + v1.z; //nf.x * p1.x + nf.y * p1.y + nf.z * p1.z + t ( (p2.x - p1.x ) + (p2.y - p1.y )+ (p2.z - p1.z )) = v1.x + v1.y + v1.z; float d = v1.x + v1.y + v1.z - nf.x * p1.x - nf.y * p1.y - nf.z * p1.z; float a = (p2.x - p1.x ) + (p2.y - p1.y )+ (p2.z - p1.z ); t = d / a; VECTOR3 result = vec ( p1.x + t * ( p2.x - p1.x ),p1.x + t * ( p2.x - p1.x ),p1.x + t * ( p2.x - p1.x )); return result; } //求单位 VECTOR3 unitFunc(VECTOR3 V)//据每个球的不同方向，求单位向量方向 { float s; s = V.x * V.x + V.y * V.y + V.z * V.z ; float d; d = sqrt (s ); VECTOR3 result; result.x = V.x / d ; result.y = V.y / d ; result.z = V.z / d ; return result; } VECTOR3 vec(float x,float y , float z) { VECTOR3 v; v.x = x; v.y = y; v.z = z; return v; } float distance ( VECTOR3 v1,VECTOR3 v2) { return sqrt (( v1.x - v2.x ) * ( v1.x - v2.x ) + ( v1.y - v2.y ) * ( v1.y - v2.y ) + ( v1.z - v2.z ) * ( v1.z - v2.z )); } int main() { VECTOR3 v0,v1,v2,v3,v4,vTest; v0 = vec( 4,4,5); v1 = vec( 2,6,0); v2 = vec( 6,6,0); v3 = vec( 6,2,0); v4 = vec( 2,2,0); vTest = vec (3,3,4); isInFrustum( v0, v1, v2, v3, v4,vTest); }