bool Sphere::IntersectBox( aabb& a_Box )
{
	float dmin = 0;
	vector3 v1 = a_Box.GetPos(), v2 = a_Box.GetPos() + a_Box.GetSize();
	if (m_Centre.x < v1.x) 
	{
		dmin = dmin + (m_Centre.x - v1.x) * (m_Centre.x - v1.x);
	}
	else if (m_Centre.x > v2.x)
	{
		dmin = dmin + (m_Centre.x - v2.x) * (m_Centre.x - v2.x);
	}
	if (m_Centre.y < v1.y)
	{
		dmin = dmin + (m_Centre.y - v1.y) * (m_Centre.y - v1.y);
	}
	else if (m_Centre.y > v2.y)
	{
		dmin = dmin + (m_Centre.y - v2.y) * (m_Centre.y - v2.y);
	}
	if (m_Centre.z < v1.z)
	{
		dmin = dmin + (m_Centre.z - v1.z) * (m_Centre.z - v1.z);
	}
	else if (m_Centre.z > v2.z)
	{
		dmin = dmin + (m_Centre.z - v2.z) * (m_Centre.z - v2.z);
	}
	return (dmin <= m_SqRadius);
}

float KDTree::caculateCost(SplitNode *split,aabb &frontBox, aabb &backBox)
{
	float frontArea = 2 * (frontBox.w()*frontBox.d() + frontBox.w()*frontBox.h() + frontBox.d()*frontBox.h());
	float backArea = 2 * (backBox.w()*backBox.d() + backBox.w()*backBox.h() +backBox.d()*backBox.h());

	return frontArea * split->nlcount+ backArea * split->nrcount;
}

void camera::view_all(aabb const& box, vec3 const& up)
{
    float diagonal = length(box.size());
    float r = diagonal * 0.5f;

    vec3 eye = box.center() + vec3(0, 0, r + r / std::atan(fovy_));

    look_at(eye, box.center(), up);
}

/* get cost of sah(surface area heuristic) method for certain plane */
double get_sah_cost(double plane, aabb &voxel, int num_left, int num_right) 
{
	aabb voxel_left, voxel_right;
	voxel.split_aabb(dim, plane, voxel_left, voxel_right);
	double sa_left = voxel_left.get_surface_area();
	double sa_right = voxel_right.get_surface_area();
	double sa_union = voxel.get_surface_area();

	return get_cost(sa_left / sa_union, sa_right / sa_union, num_left, num_right);
}

bool PlanePrim::IntersectBox( aabb& a_Box )
{
	vector3 v[2];
	v[0] = a_Box.GetPos(), v[1] = a_Box.GetPos() + a_Box.GetSize();
	int side1, side2 = 0, i = 0;
	for ( side1 = 0, side2 = 0, i = 0; i < 8; i++ )
	{
		vector3 p( v[i & 1].x, v[(i >> 1) & 1].y, v[(i >> 2) & 1].z );
		if ((DOT( p, m_Plane.N ) + m_Plane.D) < 0) side1++; else side2++;
	}
	if ((side1 == 0) || (side2 == 0)) return false; else return true;
}

bool sphere::bounding_box(float t0, float t1, aabb& _aabb)
{
    vec3 _min = center - vec3(radius, radius, radius);
    vec3 _max = center + vec3(radius, radius, radius);

    _aabb.init(_min, _max);

    return true;
}

aap::aap(const aabb &total)
{
	VEC3F center = total.center();
	char xyz = 2;

	if (total.width() >= total.height() && total.width() >= total.depth()) {
		xyz = 0;
	} else
	if (total.height() >= total.width() && total.height() >= total.depth()) {
		xyz = 1;
	}

	this->xyz = xyz;
	this->p = center[xyz];
}

void Trigger::getLocalBounds( aabb& out ) const
{
	if ( triggerModel )
	{
		triggerModel->getBounds( out );
	}
	else
	{
		out.fromHalfSize( 4.f );
	}
}

bool IsAABBInsideSphere( const aabb& bb, const vec3_c& sphereCenter, float sphereRadius )
{
	float radSq = Square( sphereRadius );
	for ( u32 i = 0; i < 8; i++ )
	{
		vec3_c p = bb.getPoint( i );
		if ( p.distSQ( sphereCenter ) > radSq )
			return false;
	}
	return true;
}

const collision line_aabb(const line &a, const aabb &b)
{
	collision r;
	r.result = false;
	
	for (int i = 0; i < 4; ++i)
	{
		r.result = r.result || line_line(a, b.edge(i)).result;
	}

	return r;
}

void KDTree::GetBoxVertex(aabb box, Point3 *Pos)
{
	Point3 start;
	Point3 end;
	for(int i = 0; i< 3; i++)
	{
		start.cell[i] = box.GetPos().cell[i];
		end.cell[i] = box.GetPos().cell[i] + box.GetSize().cell[i];
	}

	Pos[0].set(start);
	Pos[6].set(end);

	Pos[1].set(Point3(end.x,start.y,start.z));
	Pos[2].set(Point3(end.x,start.y,end.z));
	Pos[3].set(Point3(start.x,start.y,end.z));

	Pos[4].set(Point3(start.x,end.y,start.z));
	Pos[5].set(Point3(end.x,end.y,start.z));
	Pos[7].set(Point3(start.x,end.y,end.z));
}

const collision aabb_aabb(const aabb &a, const aabb &b)
{
	collision r;
	debug("AABB AABB : only result, no normal data", false);

	r.result = 
	!(a.max().x < a.min().x ||
	b.max().x < a.min().x ||
	a.max().y < a.min().y ||
	b.max().y < a.min().y);

	return r;
}

void KDTree::Optimize(KDTreeNode* R,Side s,aabb AABB)
{
	if(!R) return;
	while(R->Type != LEFT)
	{
		if(s-2*R->m_Axis>=0&&s-2*R->m_Axis<=1)
		{
			if(s%2==0)
				R = R->m_rchild;
			else 
				R = R->m_lchild;

		}

		else if(R->m_Split >=AABB.GetPos().cell[R->m_Axis]+AABB.GetSize().cell[R->m_Axis])
			R = R->m_lchild;

		else if(R->m_Split <= AABB.GetPos().cell[R->m_Axis])
			R = R->m_rchild;

		else
			break;
	}
}

/* build kd-tree */
kdtree::kdtree(aabb &voxel, vector<primitive*> &pris, int depth, kdtree* parent)
{
	parent_kdtree = parent;
	left_kdtree = right_kdtree = NULL;
	bbox = voxel;

	/* end criteria; make this leaf */
	if (depth > maximum_depth || pris.size() < 1)
	{
		primitives = pris;
	}

	/* general case: determine whether and how to devide current voxel */
	else
	{
		double cost_asleaf = cost_i * pris.size();	
		cost_min = 1E100;							

		for (dim = 0; dim < 3; dim++)
		{
			get_best_plane(pris, voxel);
		}

		/* internal node; divide further */
		if (cost_min < cost_asleaf)
		{
			vector<primitive*> left_pris, right_pris;
			classify_primitives(pris, best_plane, left_pris, right_pris);

			aabb left_voxel, right_voxel;
			voxel.split_aabb(best_dim, best_plane, left_voxel, right_voxel);

			left_kdtree = new kdtree(left_voxel, left_pris, depth + 1, this);
			right_kdtree = new kdtree(right_voxel, right_pris, depth + 1, this);
		}

		/* leaf; division end */
		else							
		{
			primitives = pris;
		}
	}
}

void calcDepthRange(mat4 const& pr, mat4 const& mv,
                    aabb const& bbox, float& minval, float& maxval)
{
    vec3 center = vec4( mv * vec4(bbox.center(), 1.0f) ).xyz();
    vec3 min    = vec4( mv * vec4(bbox.min, 1.0f) ).xyz();
    vec3 max    = vec4( mv * vec4(bbox.max, 1.0f) ).xyz();

    float radius = length(max - min) * 0.5f;

    // Depth buffer of ibrPlanes
    vec3 scal(center);
    scal = normalize(scal) * radius;
    min = center - scal;
    max = center + scal;

    vec4 min4 = pr * vec4(min, 1.f);
    vec4 max4 = pr * vec4(max, 1.f);

    min = min4.xyz() / min4.w;
    max = max4.xyz() / max4.w;

    minval = (min.z+1.f) * 0.5f;
    maxval = (max.z+1.f) * 0.5f;
}

void draw_aabb(const mat4 &m, const aabb &a, const vec4 &color)
{
	for (int i = 0; i < 4; ++i)
		draw_line(m, a.edge(i), color);
}

VkBool32 frustum::isVisible(const aabb& aabbWorld) const
{
	return isVisible(aabbWorld.getObb());
}

bool KDTree::findOptimalSplitPositon(TriangleList *list,aabb &box,int depth, real &splitPosition,aabb &frontBox, aabb &backBox)
{
	bool foundOptimalSplit = false;

	//make a list of the split position candidates
	int axis = depth % Dimension;    
	real pos1 = box.GetPos().cell[axis];//the rangle of the axis direction within the box
	real pos2 = box.GetPos().cell[axis] + box.GetSize().cell[axis];

	bool *pright = new bool[list->GetCount()];
	TriangleNode *tempList = list->GetHead();
	SplitList* SList = new SplitList();
	int aidx = 0;
	while(tempList)
	{
		real pos;
		pright[aidx++]= true;
		for(int i=0;i<3;i++)
		{
			pos = tempList->GetVertex(i).cell[axis];
			if((pos >= pos1)&&(pos <= pos2)) SList->InsertSplitPos(pos);
		}
		tempList = tempList->GetNext();
	}

	//determine nlcount / nrcont for each split position
	aabb tempFrontBox = box;
	aabb tempBackBox = box;
	float tempFrontBoxp1 = tempFrontBox.GetPos().cell[axis];
	float tempBackBoxp2 = tempBackBox.GetPos().cell[axis] + tempBackBox.GetSize().cell[axis];
	SplitNode* splist = SList->GetHead();
	while(splist!=NULL)
	{
		tempBackBox.GetPos().cell[axis] = splist->splitpos;
		tempBackBox.GetSize().cell[axis] = pos2 - splist->splitpos;
		tempFrontBox.GetSize().cell[axis] = splist->splitpos - pos1;

		float tempFrontBoxp2 = tempFrontBox.GetPos().cell[axis] + tempFrontBox.GetSize().cell[axis];
		float tempBackBoxp1 = tempBackBox.GetPos().cell[axis];
		tempList = list->GetHead();
		int i = 0;
		while(tempList)
		{
			if(pright[i])
			{
				int position = partitionTriangle(tempList,depth,splist->splitpos);
				switch(position)
				{
				case kdBefore:
					splist->nlcount++;
					pright[i]=false;
					break;
				case kdAfter:
					splist->nrcount++;
					break;
				case kdIntersection:
					splist->nlcount++;
					splist->nrcount++;
					break;
				}
			}else
			{
				splist->nlcount++;
			}
			tempList = tempList->GetNext();
		}
		splist = splist->next;
		i++;
	}
	delete pright;

	float bestPos = 0.0;
	float lowCost = caculateSingleVoxelCost(list->GetCount(),box);
	float bestCost = lowCost;
	splist = SList->GetHead();
	while(splist)
	{
		tempBackBox.GetPos().cell[axis] = splist->splitpos;
		tempBackBox.GetSize().cell[axis] = pos2 - splist->splitpos;
		tempFrontBox.GetSize().cell[axis] = splist->splitpos - pos1;

		real splitcost = cTraversal * lowCost + caculateCost(splist,tempFrontBox,tempBackBox);
		if(splitcost < lowCost)
		{
			foundOptimalSplit = true;
			bestCost = splitcost;
			bestPos = splist->splitpos;
			frontBox = tempFrontBox;
			backBox = tempBackBox;
			
		}
		splist = splist->next;
	}
	splitPosition = bestPos;
	return foundOptimalSplit;
}

float KDTree::caculateSingleVoxelCost(int numofTriangles,aabb &box)
{
	float area = 2 * (box.w()*box.d() + box.w()*box.h() + box.d()*box.h());
	return area * numofTriangles;
}

void ireon::kd_tree::KdTree<T>::subdivide( KdTreeNode<T>* node, const aabb& box, int depth, int a_Prims )
{
	// recycle used split list nodes
	//add sPool_ at end sList_
	if (sList_)
	{
		SplitList* list = sList_;
		while (list->next) 
			list = list->next;
		list->next = sPool_;
		sPool_ = sList_, sList_ = 0;
	}

	// determine split axis
	Vector3 s = box.getSize();
	if ((s.x >= s.y) && (s.x >= s.z)) 
		node->setAxis( 0 );
	else if ((s.y >= s.x) && (s.y >= s.z))
		node->setAxis( 1 );
	
	int axis = node->getAxis();
	
	// make a list of the split position candidates
	ObjectList<T>* l = node->getList();
	real p1, p2;
	real pos1 = box.getPos().val[axis];
	real pos2 = box.getPos().val[axis] + box.getSize().val[axis];
	bool* pright = new bool[a_Prims];
	float* eleft = new float[a_Prims], *eright = new float[a_Prims];
	T** parray = new T*[a_Prims];
	
	real etleft, etright;
	int aidx = 0;
	while (l)
	{
		T* p = parray[aidx] = l->getPrimitive();
		pright[aidx] = true;

		etleft =  eleft[aidx];
		etright = eright[aidx];
		p->calculateRange( etleft, etright, axis );
		eleft[aidx] = (float)etleft;
		eright[aidx] = (float)etright;
		aidx++;
		for ( int i = 0; i < 3; i++ )
		{
			p1 = (float)p->vertice( i )->cell[axis];
			if ((p1 >= pos1) && (p1 <= pos2)) 
				insertSplitPos( p1 );
		}
		
		l = l->getNext();
	}
	// determine n1count / n2count for each split position
	aabb b1, b2, b3 = box, b4 = box;
	SplitList* splist = sList_;
	float b3p1 = b3.getPos().val[axis];
	float b4p2 = b4.getPos().val[axis] + b4.getSize().val[axis];
	Vector3 foo;
	while (splist)
	{
		foo = b4.getPos();
		foo.val[axis] = splist->splitpos;
		b4.setPos(foo);
		foo = b4.getSize();
		foo.val[axis] = pos2 - splist->splitpos;
		b4.setSize(foo);
		foo = b3.getSize();
		foo.val[axis] = splist->splitpos - pos1;
		b3.setSize(foo);
		
		float b3p2 = b3.getPos().val[axis] + b3.getSize().val[axis];
		float b4p1 = b4.getPos().val[axis];
		for ( int i = 0; i < a_Prims; i++ ) if (pright[i])
		{
			T* p = parray[i];
			if ((eleft[i] <= b3p2) && (eright[i] >= b3p1))
				if (p->intersectBox( b3 )) splist->n1count++;
			if ((eleft[i] <= b4p2) && (eright[i] >= b4p1))
				if (p->intersectBox( b4 )) splist->n2count++; else pright[i] = false;
		}
		else splist->n1count++;
		splist = splist->next;
	}
	delete[] pright;
	// calculate surface area for current node
	real SAV = 0.5f / (box.w() * box.d() + box.w() * box.h() + box.d() * box.h());
	// calculate cost for not splitting
	real Cleaf = a_Prims * 1.0f;
	// determine optimal split plane position
	splist = sList_;
	real lowcost = 10000;
	real bestpos = 0;
	while (splist)
	{
		// calculate child node extends
		foo = b4.getPos();
		foo.val[axis] = splist->splitpos;
		b4.setPos(foo);
		foo = b4.getSize();
//.........这里部分代码省略.........