C++ AABB函数代码示例

AABB CLevelEntity::CalculateBoundingBox(CLevelEntity* pThis)
{
	size_t iModel = pThis->GetModelID();
	CModel* pModel = CModelLibrary::GetModel(iModel);

	if (pModel)
		return pModel->m_aabbVisBoundingBox;

	tstring sAABB = pThis->GetParameterValue("BoundingBox");

	AABB aabbBounds = AABB(Vector(-0.5f, -0.5f, -0.5f), Vector(0.5f, 0.5f, 0.5f));

	if (CanUnserializeString_AABB(sAABB))
	{
		aabbBounds = UnserializeString_AABB(sAABB, pThis->GetName(), pThis->m_sClass, "BoundingBox");

		// Center the entity around this bounding box.
		Vector vecGlobalOrigin = aabbBounds.Center();
		aabbBounds.m_vecMins -= vecGlobalOrigin;
		aabbBounds.m_vecMaxs -= vecGlobalOrigin;
	}
	else
	{
		CSaveData* pSaveData = CBaseEntity::FindSaveDataByHandle(tstring("C"+pThis->m_sClass).c_str(), "BoundingBox");
		if (pSaveData && pSaveData->m_bDefault)
			memcpy(&aabbBounds, &pSaveData->m_oDefault, sizeof(aabbBounds));
	}

	if (pThis->m_hMaterialModel.IsValid() && pThis->m_hMaterialModel->m_ahTextures.size())
	{
		CTextureHandle hBaseTexture = pThis->m_hMaterialModel->m_ahTextures[0];

		if (hBaseTexture.IsValid())
		{
			aabbBounds.m_vecMaxs.y *= (float)hBaseTexture->m_iHeight/pThis->m_hMaterialModel->m_iTexelsPerMeter;
			aabbBounds.m_vecMins.y *= (float)hBaseTexture->m_iHeight/pThis->m_hMaterialModel->m_iTexelsPerMeter;

			aabbBounds.m_vecMaxs.z *= (float)hBaseTexture->m_iWidth/pThis->m_hMaterialModel->m_iTexelsPerMeter;
			aabbBounds.m_vecMins.z *= (float)hBaseTexture->m_iWidth/pThis->m_hMaterialModel->m_iTexelsPerMeter;
		}
	}

	aabbBounds.m_vecMins = aabbBounds.m_vecMins * pThis->GetScale();
	aabbBounds.m_vecMaxs = aabbBounds.m_vecMaxs * pThis->GetScale();

	return aabbBounds;
}

AABB buildBox (const Segment &s, const float &pitch, const float &height, const float &thickness) {
	const MatrixCoord3D &start = s.start;
	const MatrixCoord3D &end = s.end;

	Vec3f xyz (static_cast<float>(start[0])*pitch, static_cast<float>(start[2])*height, static_cast<float>(start[1])*pitch);
	Vec3f XYZ (static_cast<float>(end[0])*pitch, static_cast<float>(end[2])*height, static_cast<float>(end[1])*pitch);

	// must swap z and Z. because -XYZ will yield the smallest negative value for Z, that needs to be swapped so that the xyz and XYZ points get right for AABB creation.
	const float buffer = -xyz[2];
	xyz[2] = -XYZ[2];
	XYZ[2] = buffer;

	xyz -= Vec3f (thickness, thickness, thickness); // 0
	XYZ += Vec3f (thickness, thickness, thickness); // 1

	return AABB (xyz, XYZ);
}

	TEST(AABBTest, SplitZ)
	{
		auto split_z = AABB({10, 100, 1000}, {20, 200, 2000}).split(2, 1500);

		EXPECT_NEAR(10, split_z.left.minimum().x(), 1e-100);
		EXPECT_NEAR(100, split_z.left.minimum().y(), 1e-100);
		EXPECT_NEAR(1000, split_z.left.minimum().z(), 1e-100);
		EXPECT_NEAR(20, split_z.left.maximum().x(), 1e-100);
		EXPECT_NEAR(200, split_z.left.maximum().y(), 1e-100);
		EXPECT_NEAR(1500, split_z.left.maximum().z(), 1e-100);
		EXPECT_NEAR(10, split_z.right.minimum().x(), 1e-100);
		EXPECT_NEAR(100, split_z.right.minimum().y(), 1e-100);
		EXPECT_NEAR(1500, split_z.right.minimum().z(), 1e-100);
		EXPECT_NEAR(20, split_z.right.maximum().x(), 1e-100);
		EXPECT_NEAR(200, split_z.right.maximum().y(), 1e-100);
		EXPECT_NEAR(2000, split_z.right.maximum().z(), 1e-100);
	}

bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
  static const unsigned int CUTOFF = 100;

  DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
  bool coll_res = false;

  std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
  std::vector<CollisionObject*>::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
  unsigned int d1 = pos_end1 - pos_start1;

  if(d1 > CUTOFF)
  {
    std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
    std::vector<CollisionObject*>::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
    unsigned int d2 = pos_end2 - pos_start2;

    if(d2 > CUTOFF)
    {
      std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
      std::vector<CollisionObject*>::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
      unsigned int d3 = pos_end3 - pos_start3;

      if(d3 > CUTOFF)
      {
        if(d3 <= d2 && d3 <= d1)
          coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
        else
        {
          if(d2 <= d3 && d2 <= d1)
            coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
          else
            coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
        }
      }
      else
        coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
    }
    else
      coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
  }
  else
    coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);

  return coll_res;
}

/* Creates the sphere from a scene file. */
Sphere::Sphere(std::fstream& file, std::vector<Material*>* materials, std::vector<Light*>* lights) : Primitive(file, materials, lights)
{
    /* Read the sphere definition from the scene file. */
    SphereDefinition definition;
    file.read((char*)&definition, sizeof(SphereDefinition));

    /* Read the geometric center and radius of the sphere. */
    this->center = Vector(definition.center[0], definition.center[1], definition.center[2]);
    this->radius = definition.radius;

    /* Compute the sphere's radius squared. */
    this->radiusSquared = this->radius * this->radius;

    /* Compute the sphere's bounding box. */
    Vector radiusVector = Vector(this->radius, this->radius, this->radius);
    this->boundingBox = AABB(this->center - radiusVector, this->center + radiusVector);
}

    void Locator::Update(const FrameTime& fr, UpdateTypeEnum updateType)
    {               
        super::Update(fr,updateType);
		bool updatedBound = m_worldXformUpdated;

        Model* model = m_resource ? (Model*)m_resource->GetTarget() : NULL;                     
        if( model && model->IsReady())
        {
            if(m_modelTransforms.empty() || m_worldXformUpdated)
            {
                 const MatrixList& matrices = model->AbsoluteTransforms();
                 m_modelTransforms.resize(matrices.size());
                 for( unsigned int i = 0; i < m_modelTransforms.size(); ++i)
                 {
                     m_modelTransforms[i] = matrices[i] * m_world; // transform matrix array now holds complete world transform.
                 }
                 BuildRenderables(); 
				 updatedBound = true;
            }                               
        }

        m_boundsDirty = updatedBound;
        if(m_boundsDirty)        
        {                  
            if(!m_modelTransforms.empty())
            {                
               // assert(model && model->IsReady());
                m_localBounds = model->GetBounds();                                
                if(m_parent) m_parent->InvalidateBounds();                
            }
            else
            {
                m_localBounds = AABB(float3(-0.5f,-0.5f,-0.5f), float3(0.5f,0.5f,0.5f));                
            }      
            this->UpdateWorldAABB();            
        }

        if(RenderContext::Inst()->LightEnvDirty)
        {
            // update light env.
            for(auto  renderNode = m_renderables.begin(); renderNode != m_renderables.end(); renderNode++)
            {
                LightingState::Inst()->UpdateLightEnvironment(*renderNode);
            }
        }         
    }

AABB RasterizerDummy::mesh_get_aabb(RID p_mesh) const {

	Mesh *mesh = mesh_owner.get( p_mesh );
	ERR_FAIL_COND_V(!mesh,AABB());

	AABB aabb;

	for (int i=0;i<mesh->surfaces.size();i++) {

		if (i==0)
			aabb=mesh->surfaces[i]->aabb;
		else
			aabb.merge_with(mesh->surfaces[i]->aabb);
	}

	return aabb;
}

void CIntersectionAssistanceUnit::DebugUpdate() const
{
    if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled)
        {
            IEntity* pEntity = gEnv->pEntitySystem->GetEntity(m_subjectEntityId);
            if(pEntity)
                {
                    IPhysicalEntity *pPhysical = pEntity->GetPhysics();
                    if(pPhysical)
                        {
                            const float fFontSize = 1.2f;
                            float drawColor[4] = {1.0f, 1.0f, 1.0f, 1.0f};

                            string sMsg(string().Format(" Entity ID: [%d]", m_subjectEntityId));
                            sMsg += string().Format("\n Entity Name: [%s]", pEntity->GetName());

                            sMsg += string().Format("\n EmbedTimer: [%.3f]", m_embedTimer);
                            sMsg += string().Format("\n EmbedState: [%s]",(m_embedState == eES_None) ? "NONE" : (m_embedState == eES_Evaluating) ? "EVALUATING" : (m_embedState == eES_ReEvaluating) ? "REEVALUATING" : (m_embedState == eES_NotEmbedded) ? "NOT EMBEDDED" : (m_embedState == eES_Embedded) ? "EMBEDDED" : "UNKNOWN");

                            Vec3 vCurrTrans = m_entityStartingWPos - pEntity->GetWorldPos();
                            sMsg += string().Format("\n Translation: < %.3f, %.3f, %.3f >", vCurrTrans.x, vCurrTrans.y, vCurrTrans.z );
                            sMsg += string().Format("\n Trans magnitude: < %.3f >", vCurrTrans.GetLength() );
                            sMsg += string().Format("\n Trans per sec: < %.3f >", vCurrTrans.GetLength() / g_pGameCVars->pl_pickAndThrow.intersectionAssistTimePeriod );

                            sMsg += string().Format("\n Collision count: %u", m_collisionCount );

                            // RENDER
                            Vec3 vDrawPos = pEntity->GetWorldPos() + Vec3(0.0f,0.0f,0.6f);
                            gEnv->pRenderer->DrawLabelEx(vDrawPos, fFontSize, drawColor, true, true, sMsg.c_str());

                            // Box
                            pe_params_bbox bbox;
                            if(pPhysical->GetParams(&bbox))
                                {
                                    ColorB colDefault = ColorB( 127,127,127 );
                                    ColorB embedded = ColorB(255, 0, 0);
                                    ColorB notEmbedded = ColorB(0, 255, 0);

                                    gEnv->pRenderer->GetIRenderAuxGeom()->DrawAABB( AABB(bbox.BBox[0],bbox.BBox[1]), Matrix34(IDENTITY), false, (m_embedState == eES_Embedded) ? embedded : (m_embedState == eES_NotEmbedded) ? notEmbedded : colDefault, eBBD_Faceted);
                                }
                        }
                }

        }
}

void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
{
  setup();

  DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));

  std::vector<CollisionObject*>::iterator pos_start1 = objs_x.begin();
  std::vector<CollisionObject*>::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());

  while(pos_start1 < pos_end1)
  {
    if(*pos_start1 == obj)
    {
      objs_x.erase(pos_start1);
      break;
    }
    ++pos_start1;
  }

  std::vector<CollisionObject*>::iterator pos_start2 = objs_y.begin();
  std::vector<CollisionObject*>::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());

  while(pos_start2 < pos_end2)
  {
    if(*pos_start2 == obj)
    {
      objs_y.erase(pos_start2);
      break;
    }
    ++pos_start2;
  }

  std::vector<CollisionObject*>::iterator pos_start3 = objs_z.begin();
  std::vector<CollisionObject*>::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());

  while(pos_start3 < pos_end3)
  {
    if(*pos_start3 == obj)
    {
      objs_z.erase(pos_start3);
      break;
    }
    ++pos_start3;
  }
}

TriMesh::TriMesh(Stream *stream, InstanceManager *manager) 
	: Shape(stream, manager), m_tangents(NULL) {
	m_name = stream->readString();
	m_aabb = AABB(stream);

	uint32_t flags = stream->readUInt();
	m_vertexCount = stream->readSize();
	m_triangleCount = stream->readSize();

	m_positions = new Point[m_vertexCount];
	stream->readFloatArray(reinterpret_cast<Float *>(m_positions), 
		m_vertexCount * sizeof(Point)/sizeof(Float));

	m_faceNormals = flags & EFaceNormals;

	if (flags & EHasNormals) {
		m_normals = new Normal[m_vertexCount];
		stream->readFloatArray(reinterpret_cast<Float *>(m_normals), 
			m_vertexCount * sizeof(Normal)/sizeof(Float));
	} else {
		m_normals = NULL;
	}

	if (flags & EHasTexcoords) {
		m_texcoords = new Point2[m_vertexCount];
		stream->readFloatArray(reinterpret_cast<Float *>(m_texcoords), 
			m_vertexCount * sizeof(Point2)/sizeof(Float));
	} else {
		m_texcoords = NULL;
	}

	if (flags & EHasColors) {
		m_colors = new Spectrum[m_vertexCount];
		stream->readFloatArray(reinterpret_cast<Float *>(m_colors), 
			m_vertexCount * sizeof(Spectrum)/sizeof(Float));
	} else {
		m_colors = NULL;
	}

	m_triangles = new Triangle[m_triangleCount];
	stream->readUIntArray(reinterpret_cast<uint32_t *>(m_triangles), 
		m_triangleCount * sizeof(Triangle)/sizeof(uint32_t));
	m_flipNormals = false;
	configure();
}

/*!
*  \brief      Loads the environment configuration from a json file.
*  \author     Sascha Kaden
*  \param[in]  file path
*  \param[out] result of the loading
*  \date       2017-10-18
*/
bool EnvironmentConfigurator::loadConfig(const std::string &filePath) {
    nlohmann::json json = loadJson(filePath);
    if (json.empty())
        return false;

    m_obstaclePaths.clear();
    auto numObstacles = json["NumObstacles"].get<size_t>();
    for (size_t i = 0; i < numObstacles; ++i)
        m_obstaclePaths.push_back(json["ObstaclePath" + std::to_string(i)].get<std::string>());
    m_workspaceDim = json["WorkspaceDim"].get<unsigned int>();
    Vector3 bottomLeft = stringToVector<3>(json["MinWorkspaceBound"].get<std::string>());
    Vector3 topRight = stringToVector<3>(json["MaxWorkspaceBound"].get<std::string>());
    m_workspceBounding = AABB(bottomLeft, topRight);
    m_factoryType = static_cast<FactoryType>(json["FactoryType"].get<int>());
    m_robotType = static_cast<RobotType>(json["RobotType"].get<int>());

    return true;
}

VillagerC::VillagerC()
{
	Texture* tex = textureLoader::getTexture("friendly_npcs");
	AnimatedSprite sprite = AnimatedSprite(&tex->texture, 0, 0, tex->cellWidth, tex->cellHeight, 0 * tex->uSize, 5 * tex->vSize, 1 * tex->uSize, 1 * tex->vSize);
	*this = VillagerC((VillagerC&)sprite);
	type = 1;
	name = "villagerC";
	isAnimated = false;

	//Setup Collider
	int xOffset = 18;
	int yOffset = 15;
	int width = 28;
	int height = 45;
	float uSize = 1;
	float vSize = 1;
	colliderXOffset = xOffset;
	colliderYOffset = yOffset;
	setCollider(&AABB(x + xOffset, y + yOffset, width, height));
	maxSpeed = 50;
	isColliderDrawn = false;
	
	// Walking Animation
	int numFrames = 1;
	int timeToNextFrame = 300;
	std::vector<AnimationFrame> frames;
	frames.assign(numFrames, AnimationFrame());

	frames[0] = AnimationFrame(0, 5, uSize, vSize);
	//frames[1] = AnimationFrame(1, 0, uSize, vSize);
	Animation animation_walking = Animation("Walking", frames, numFrames);
	animations[animation_walking.name] = AnimationData(animation_walking, timeToNextFrame, true);

	// Idle Animation
	numFrames = 1;
	frames.clear();
	frames.assign(numFrames, AnimationFrame());

	frames[0] = AnimationFrame(0, 5, uSize, vSize);
	Animation animation_idle = Animation("Idle", frames, numFrames);
	animations[animation_idle.name] = AnimationData(animation_idle, timeToNextFrame, true);

	//setAnimation("Walking");
}

void main( int argc, char ** argv )
{
   Environment env(argc, argv);
   WALBERLA_UNUSED(env);
   walberla::mpi::MPIManager::instance()->useWorldComm();

   //init domain partitioning
   auto forest = blockforest::createBlockForest( AABB(0,0,0,20,20,20), // simulation domain
                                                 Vector3<uint_t>(2,2,2), // blocks in each direction
                                                 Vector3<bool>(false, false, false) // periodicity
                                                 );
   domain::BlockForestDomain domain(forest);

   //init data structures
   data::ParticleStorage ps(100);

   //initialize particle
   auto uid = createSphere(ps, domain);
   WALBERLA_LOG_DEVEL_ON_ROOT("uid: " << uid);

   //init kernels
   mpi::ReduceProperty    RP;
   mpi::SyncNextNeighbors SNN;

   //sync
   SNN(ps, domain);

   auto pIt = ps.find(uid);
   if (pIt != ps.end())
   {
      pIt->getForceRef() += Vec3(real_c(walberla::mpi::MPIManager::instance()->rank()));
   }

   RP.operator()<ForceTorqueNotification>(ps);

   if (walberla::mpi::MPIManager::instance()->rank() == 0)
   {
      WALBERLA_CHECK_FLOAT_EQUAL( pIt->getForce(), Vec3(real_t(28)) );
   } else
   {
      WALBERLA_CHECK_FLOAT_EQUAL( pIt->getForce(), Vec3(real_t(walberla::mpi::MPIManager::instance()->rank())) );
   }
}

AABB AnimatedTransform::getTranslationBounds() const {
	if (m_tracks.size() == 0) {
		Point p = m_transform(Point(0.0f));
		return AABB(p, p);
	}

	AABB aabb;

	for (size_t i=0; i<m_tracks.size(); ++i) {
		const AbstractAnimationTrack *absTrack = m_tracks[i];
		switch (absTrack->getType()) {
			case AbstractAnimationTrack::ETranslationX:
			case AbstractAnimationTrack::ETranslationY:
			case AbstractAnimationTrack::ETranslationZ: {
					int idx  = absTrack->getType() - AbstractAnimationTrack::ETranslationX;
					const FloatTrack *track =
						static_cast<const FloatTrack *>(absTrack);
					for (size_t j=0; j<track->getSize(); ++j) {
						Float value = track->getValue(j);
						aabb.max[idx] = std::max(aabb.max[idx], value);
						aabb.min[idx] = std::min(aabb.min[idx], value);
					}
				}
				break;

			case AbstractAnimationTrack::ETranslationXYZ: {
					const VectorTrack *track =
						static_cast<const VectorTrack *>(absTrack);
					for (size_t j=0; j<track->getSize(); ++j)
						aabb.expandBy(Point(track->getValue(j)));
				}
				break;
			default:
				break;
		}
	}
	for (int i=0; i<3; ++i) {
		if (aabb.min[i] > aabb.max[i])
			aabb.min[i] = aabb.max[i] = 0.0f;
	}

	return aabb;
}

const AABB& LightNode::getSelectedComponentsBounds() const {
	// Create a new axis aligned bounding box
	m_aabb_component = AABB();

	if (_light.isProjected()) {
		// Include the according vertices in the AABB
		m_aabb_component.includePoint(_lightTargetInstance.getVertex());
		m_aabb_component.includePoint(_lightRightInstance.getVertex());
		m_aabb_component.includePoint(_lightUpInstance.getVertex());
		m_aabb_component.includePoint(_lightStartInstance.getVertex());
		m_aabb_component.includePoint(_lightEndInstance.getVertex());
	}
	else {
		// Just include the light center, this is the only vertex that may be out of the light volume
		m_aabb_component.includePoint(_lightCenterInstance.getVertex());
	}

	return m_aabb_component;
}