C++ AABB类代码示例

void test(const shared_ptr< DistanceOctree< MeshType > > & distanceOctree, const MeshType & mesh, const AABB & domainAABB, Vector3<uint_t> numBlocks)
{
   Vector3<real_t> blockSize(domainAABB.xSize() / real_c(numBlocks[0]),
      domainAABB.ySize() / real_c(numBlocks[1]),
      domainAABB.zSize() / real_c(numBlocks[2]));

   real_t maxError = blockSize.min() / real_t(10);

   SetupBlockForest setupBlockforest;
   setupBlockforest.addRootBlockExclusionFunction(F(distanceOctree, maxError));
   setupBlockforest.addWorkloadMemorySUIDAssignmentFunction(blockforest::uniformWorkloadAndMemoryAssignment);


   setupBlockforest.init(domainAABB, numBlocks[0], numBlocks[1], numBlocks[2], false, false, false);
   WALBERLA_LOG_DEVEL(setupBlockforest.toString());


   std::vector< Vector3<real_t> > vertexPositions;
   vertexPositions.reserve(mesh.n_vertices());
   for (auto vIt = mesh.vertices_begin(); vIt != mesh.vertices_end(); ++vIt)
   {
      vertexPositions.push_back(toWalberla(mesh.point(*vIt)));
   }

   std::vector< const blockforest::SetupBlock* > setupBlocks;
   setupBlockforest.getBlocks(setupBlocks);

   // Check wether all vertices are located in allocated blocks
   std::vector< Vector3<real_t> > uncoveredVertices(vertexPositions);

   for (auto bIt = setupBlocks.begin(); bIt != setupBlocks.end(); ++bIt)
   {
      const AABB & aabb = (*bIt)->getAABB();

      uncoveredVertices.erase(std::remove_if(uncoveredVertices.begin(), uncoveredVertices.end(), PointInAABB(aabb)), uncoveredVertices.end());
   }

   WALBERLA_CHECK(uncoveredVertices.empty(), "Not all vertices of the mesh are located in allocated blocks!");

   //setupBlockforest.assignAllBlocksToRootProcess();
   //setupBlockforest.writeVTKOutput( "setupblockforest" );
}

/// [groupSyntax]
bool Triangle::Intersects(const AABB &aabb) const
{
/** The AABB-Triangle test implementation is based on the pseudo-code in
	Christer Ericson's Real-Time Collision Detection, pp. 169-172. */
	///@todo The Triangle-AABB intersection test can be greatly optimized by manually unrolling loops, trivial math and by avoiding
	/// unnecessary copying.
	float t1, t2, a1, a2;
	const float3 e[3] = { float3(1,0,0), float3(0,1,0), float3(0,0,1) };

	for(int i = 0; i < 3; ++i)
	{
		ProjectToAxis(e[i], t1, t2);
		aabb.ProjectToAxis(e[i], a1, a2);
		if (!RangesOverlap(t1, t2, a1, a2))
			return false;
	}

	float3 n = UnnormalizedNormalCCW();
	ProjectToAxis(n, t1, t2);
	aabb.ProjectToAxis(n, a1, a2);
	if (!RangesOverlap(t1, t2, a1, a2))
		return false;

	const float3 t[3] = { b-a, c-a, c-b };

	for(int i = 0; i < 3; ++i)
		for(int j = 0; j < 3; ++j)
		{
			float3 axis = Cross(e[i], t[j]);
			float len = axis.LengthSq();
			if (len <= 1e-4f)
				continue; // Ignore tests on degenerate axes.

			ProjectToAxis(axis, t1, t2);
			aabb.ProjectToAxis(axis, a1, a2);
			if (!RangesOverlap(t1, t2, a1, a2))
				return false;
		}

	// No separating axis exists, the AABB and triangle intersect.
	return true;
}

	void LightFieldCube::read(File *file, size_t head_address, AABB aabb) {
		file->readInt32BE(&type);
		file->readInt32BE(&value);

		point = aabb.center();

		if (type != LIBGENS_LIGHTFIELD_CUBE_NO_SPLIT) {
			left = new LightFieldCube();
			right = new LightFieldCube();

			file->goToAddress(head_address + LIBGENS_LIGHTFIELD_CUBE_SIZE * value);
			left->read(file, head_address, aabb.half(type, LIBGENS_MATH_SIDE_LEFT));

			file->goToAddress(head_address + LIBGENS_LIGHTFIELD_CUBE_SIZE * (value+1));
			right->read(file, head_address, aabb.half(type, LIBGENS_MATH_SIDE_RIGHT));
		}
		else {

		}
	}

_Use_decl_annotations_
void BIH::Query(const AABB& test, uint32_t* numTriangles)
{
    *numTriangles = 0;
    if (!test.Intersects(_bounds))
    {
        return;
    }

    Query(test, _root, numTriangles);
}

int BroadPhaseBasic::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {

	int rc=0;

	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {

		const AABB aabb=E->get().aabb;
		if (aabb.intersects_segment(p_from,p_to)) {

			p_results[rc]=E->get().owner;
			p_result_indices[rc]=E->get().subindex;
			rc++;
			if (rc>=p_max_results)
				break;
		}
	}

	return rc;

}

	AABB MOFMesh::getBoudingBox()
	{
		AABB aabb;
		Vector3D min;
		Vector3D max;

		if(this->m_numMeshes>0)
			aabb = this->getSubmesh(0)->getBoudingBox();

		for(int i=0; i<this->m_numMeshes; ++i)
		{
			MOFSubmesh *smesh;
			smesh = this->getSubmesh(i);

			AABB sAABB = smesh->getBoudingBox();
			aabb.load(sAABB);
		}

		return aabb;
	}

 bool cull(const AABB& aabb) const
 {
   if (aabb.isNull())
     return false;
   for(unsigned i=0; i<planes().size(); ++i)
   {
     if ( plane(i).isOutside(aabb) )
       return true;
   }
   return false;
 }

AABB AABB::GetTransformedAABB(const Matrix4x4f &mat) const
{
	AABB transformedAABB;

	Vec3f newCenter(mat * m_center);

	transformedAABB.m_lowerBound = newCenter;
	transformedAABB.m_upperBound = newCenter;

	// Loop through all corners, transform, and compare
	for(int x = -1; x <= 1; x += 2)
		for(int y = -1; y <= 1; y += 2)
			for(int z = -1; z <= 1; z += 2)
			{
				Vec3f corner(x * m_halfDims.x + m_center.x, y * m_halfDims.y + m_center.y, z * m_halfDims.z + m_center.z);

				// Transform the corner
				corner = mat * corner;

				// Compare bounds
				if(corner.x > transformedAABB.m_upperBound.x)
					transformedAABB.m_upperBound.x = corner.x;
				if(corner.y > transformedAABB.m_upperBound.y)
					transformedAABB.m_upperBound.y = corner.y;
				if(corner.z > transformedAABB.m_upperBound.z)
					transformedAABB.m_upperBound.z = corner.z;

				if(corner.x < transformedAABB.m_lowerBound.x)
					transformedAABB.m_lowerBound.x = corner.x;
				if(corner.y < transformedAABB.m_lowerBound.y)
					transformedAABB.m_lowerBound.y = corner.y;
				if(corner.z < transformedAABB.m_lowerBound.z)
					transformedAABB.m_lowerBound.z = corner.z;
			}

	// Move from local into world space
	transformedAABB.CalculateHalfDims();
	transformedAABB.CalculateCenter();

	return transformedAABB;
}

/// NOTE: Expects @param nodes to be a link list along the left child
AABB::AABB(AABB *nodes) : left(0), right(0), move(0) {
  if (!nodes) return;

  // Compute bounds
  unsigned count = 0;
  for (AABB *it = nodes; it; it = it->left) {
    if (it->right) THROW("Unexpected right-hand AABB node");
    add(*it);
    count++;
  }

  // Degenerate cases
  if (count < 3) {
    if (count == 2) right = nodes->left;
    left = nodes->prepend(0);
    return;
  }

  // Decide split
  unsigned axis = getDimensions().findLargest();
  real cut = getMax()[axis] + getMin()[axis];

  // Partition nodes
  AABB *lessThan = 0;
  AABB *greaterThan = 0;
  unsigned lessCount = 0;
  unsigned greaterCount = 0;

  for (AABB *it = nodes; it;) {
    AABB *next = it->left;
    bool less = it->getMax()[axis] + it->getMin()[axis] < cut;

    if (less) {lessThan = it->prepend(lessThan); lessCount++;}
    else {greaterThan = it->prepend(greaterThan); greaterCount++;}

    it = next;
  }

  // Check for bad partition
  if (!lessThan) lessThan = greaterThan->split(greaterCount / 2);
  if (!greaterThan) greaterThan = lessThan->split(lessCount / 2);

  // Recur
  left = new AABB(lessThan);
  right = new AABB(greaterThan);
}

void Mesh::add_surface(PrimitiveType p_primitive,const Array& p_arrays,const Array& p_blend_shapes,bool p_alphasort) {


	ERR_FAIL_COND(p_arrays.size()!=ARRAY_MAX);

	Surface s;

	VisualServer::get_singleton()->mesh_add_surface(mesh,(VisualServer::PrimitiveType)p_primitive, p_arrays,p_blend_shapes,p_alphasort);
	surfaces.push_back(s);



	/* make aABB? */ {

		DVector<Vector3> vertices=p_arrays[ARRAY_VERTEX];
		int len=vertices.size();
		ERR_FAIL_COND(len==0);
		DVector<Vector3>::Read r=vertices.read();
		const Vector3 *vtx=r.ptr();

		// check AABB
		AABB aabb;
		for (int i=0;i<len;i++) {

			if (i==0)
				aabb.pos=vtx[i];
			else
				aabb.expand_to(vtx[i]);
		}

		surfaces[surfaces.size()-1].aabb=aabb;
		surfaces[surfaces.size()-1].alphasort=p_alphasort;

		_recompute_aabb();

	}

	triangle_mesh=Ref<TriangleMesh>();
	_change_notify();

}

/**
* @return The axis-aligned bounding box (AABB) of the body in world-space coordinates
*/
AABB CollisionBody::getAABB() const {

    AABB bodyAABB;

    if (mProxyCollisionShapes == NULL) return bodyAABB;

    mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform());

    // For each proxy shape of the body
    for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) {

        // Compute the world-space AABB of the collision shape
        AABB aabb;
        shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform());

        // Merge the proxy shape AABB with the current body AABB
        bodyAABB.mergeWithAABB(aabb);
    }

    return bodyAABB;
}

bool World::overlapsCollidableTiles(int centerX, int centerY, int nodeWidth, int nodeHeight)
{
	vector<WorldLayer*>::iterator it = layers->begin();
	while (it != layers->end())
	{
		WorldLayer *layer = (*it);
		if (layer->hasCollidableTiles())
		{
			AABB aabb;
			aabb.setCenterX((float)centerX);
			aabb.setCenterY((float)centerY);
			aabb.setWidth((float)nodeWidth);
			aabb.setHeight((float)nodeHeight);
			bool overlaps = layer->overlapsCollidableTile(aabb);
			if (overlaps)
				return true;
		}
		it++;
	}
	return false;
}

bool CScene::CollisionCheck( CGameObject *pObj1, CGameObject *pObj2 )
{
	// 충돌되면 true 반환
	AABB playerBox = pObj1->m_bcMeshBoundingCube;
	AABB objBox = pObj2->m_bcMeshBoundingCube;

	// AABB를 해당 오브젝트에 맞게 변환		-> 물체를 생성할 때 위치를 잡고나면 update하지 않아도 되야 하는데 자꾸 초기화됨
	playerBox.Update( &( pObj1->m_mtxWorld ) );
	objBox.Update( &( pObj2->m_mtxWorld ) );

	// AABB 출돌 검사
	if (playerBox.m_vMax.x < objBox.m_vMin.x) return true;
	if (playerBox.m_vMax.y < objBox.m_vMin.y) return true;
	if (playerBox.m_vMax.z < objBox.m_vMin.z) return true;
	if (playerBox.m_vMin.x > objBox.m_vMax.x) return true;
	if (playerBox.m_vMin.y > objBox.m_vMax.y) return true;
	if (playerBox.m_vMin.z > objBox.m_vMax.z) return true;

	// 아무런 체크도 되지 않으면 충돌하지 않은 것
	return false;
}

pair<double, SplitSide> SAH(SplitPlane plane, AABB box, int left, int right, int planar) {
	pair<AABB, AABB> boxes = box.split(plane);
	double area = surfaceArea(box);
	double ratioLeft = surfaceArea(boxes.first) / area;
	double ratioRight = surfaceArea(boxes.second) / area;

	double costLeft = cost(ratioLeft, ratioRight, left + planar, right);
	double costRight = cost(ratioLeft, ratioRight, left, planar + right);

	if (costLeft < costRight) return make_pair(costLeft, LEFT);
	else return make_pair(costRight, RIGHT);
}

/// Determines whether a point is outside of a AABB
bool AABB::outside(const AABB& a, const Point3d& point, double tol)
{
  const unsigned THREE_D = 3;

  assert(a.get_relative_pose() == point.pose);

  for (unsigned i=0; i< THREE_D; i++)
    if (point[i] < a.minp[i] - tol || point[i] > a.maxp[i] + tol)
      return true;

  return false;
}