C++ AABB::center方法代码示例

 Sphere computeBoundingSphere() const
 {
   AABB aabb;
   const int count = T_GL_Size == 4 ? 3 : T_GL_Size;
   for(size_t i=0; i<size(); ++i)
   {
     vec3 v;
     const T_Scalar* pv = reinterpret_cast<const T_Scalar*>(&at(i));
     for( int j=0; j<count; ++j )
       v.ptr()[j] = (Real)pv[j];
     aabb += v;
   }
   Real radius = 0;
   vec3 center = aabb.center();
   for(size_t i=0; i<size(); ++i)
   {
     vec3 v;
     const T_Scalar* pv = reinterpret_cast<const T_Scalar*>(&at(i));
     for( int j=0; j<count; ++j )
       v.ptr()[j] = (Real)pv[j];
     Real r = (v-center).lengthSquared();
     if (r > radius)
       radius = r;
   }
   return Sphere( center, sqrt(radius) );
 }

	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 {

		}
	}

    void keepPlanarCycles(float epsilon)
    {
      std::vector< std::vector< ref<Atom> > > kept_cycles;
      for(unsigned icycle=0; icycle<molecule()->cycles().size(); ++icycle)
      {
        AABB aabb;
        for(unsigned iatom=0; iatom<molecule()->cycle(icycle).size(); ++iatom)
          aabb += (vec3)molecule()->cycle(icycle)[iatom]->coordinates();
        fvec3 center = (fvec3)aabb.center();

        fvec3 normal;
        for(unsigned iatom=0; iatom<molecule()->cycle(icycle).size(); ++iatom)
        {
          int iatom2 = (iatom+1) % molecule()->cycle(icycle).size();
          Atom* atom1 = molecule()->cycle(icycle)[iatom].get();
          Atom* atom2 = molecule()->cycle(icycle)[iatom2].get();
          fvec3 v1 = (atom1->coordinates()-center).normalize();
          fvec3 v2 = (atom2->coordinates()-center).normalize();
          normal += cross(v1, v2);
        }
        normal.normalize();

        int ok = true;
        for(unsigned iatom=0; iatom<molecule()->cycle(icycle).size(); ++iatom)
        {
          fvec3 v1   = molecule()->cycle(icycle)[iatom]->coordinates() - center;
          float dist = dot(normal, v1);
          if (fabs(dist)>epsilon)
          {
            ok = false;
            break;
          }
        }
        if (ok && molecule()->cycle(icycle).size())
          kept_cycles.push_back(molecule()->cycle(icycle));
      }
      molecule()->cycles() = kept_cycles;
    }

float KRReverbZone::getContainment(const Vector3 &pos)
{
    AABB bounds = getBounds();
    if(bounds.contains(pos)) {
        Vector3 size = bounds.size();
        Vector3 diff = pos - bounds.center();
        diff = diff * 2.0f;
        diff = Vector3::Create(diff.x / size.x, diff.y / size.y, diff.z / size.z);
        float d = diff.magnitude();
        
        if(m_gradient_distance <= 0.0f) {
            // Avoid division by zero
            d = d > 1.0f ? 0.0f : 1.0f;
        } else {
            d = (1.0f - d) / m_gradient_distance;
            d = KRCLAMP(d, 0.0f, 1.0f);
        }
        return d;
        
    } else {
        return 0.0f;
    }
}

bool PhysicsCollision::CollisionDetection(vector<AABB*> &boxes, Octree* octree, bool test_z_axis)
{
	/******************************************************
	*	Function Name:		CollisionDetection()
	*	Programmer:			Josh Archer
	*
	*	Determines if a collision has taken place between
	*	two AABB's, and returns the appropriate boolean
	*	value.
	*
	*	Uses the Separating Axis Theorem for detection.
	*
	******************************************************/

	vector<AABBPair> bps;
	bool collisionOccurred = false;

	_octree->potentialBoxBoxCollision(bps, boxes, octree);
	for(unsigned int i = 0; i < bps.size(); i++) 
	{
		_octree->add(boxes[i]);
		AABBPair bp = bps[i];
		
		AABB* shapeOne = bp.aabb1;
		AABB* shapeTwo = bp.aabb2;

		//Test shape projections along each axes for overlap. Function can exit as soon as a disjoint (non intersecting) projection is found
		if(shapeOne->maxPoint.x < shapeTwo->minPoint.x || shapeTwo->maxPoint.x < shapeOne->minPoint.x)
		{
			//The shapes' projections along the x-axis are disjoint, so the shapes are not colliding
		}
		else
		{
			if(shapeOne->maxPoint.y < shapeTwo->minPoint.y || shapeTwo->maxPoint.y < shapeOne->minPoint.y)
			{
				//The shapes' projection along the y-axis are disjoint, so the shapes are not colliding
			}
			else
			{
				if(test_z_axis)
				{
					//Collision detection along z axis is desired, so test the third axis for intersection:
					if(shapeOne->maxPoint.z < shapeTwo->minPoint.z || shapeTwo->maxPoint.z < shapeOne->minPoint.z)
					{
						//The shapes' projection along the z-axis are disjoint, so the shapes are not colliding
					}
					else
					{
						//The shapes' projection along all three axes are intersecting, so the shapes ARE colliding
						collisionOccurred	=	true;

						core._collisions->currentCollision.boxA_ID					=	shapeOne->ID;
						core._collisions->currentCollision.boxB_ID					=	shapeTwo->ID;
						core._collisions->currentCollision.impactPoint.x			=	-FLT_MAX;
						core._collisions->currentCollision.impactPoint.y			=	-FLT_MAX;
						core._collisions->currentCollision.impactPoint.z			=	-FLT_MAX;
						core._collisions->currentCollision.boxA_movementVector		=	shapeOne->center() - shapeOne->centerPointPrevious;
						core._collisions->currentCollision.boxB_movementVector		=	shapeTwo->center() - shapeTwo->centerPointPrevious;
						core._collisions->currentCollision.timeOfImpact				=	-FLT_MAX;

						core._collisions->addToList();
					}
				}
				else
				{
					//Collision detection along z axis is NOT desired, and the shapes' projections along both the x and y axes are intersecting, so the shapes ARE colliding
					collisionOccurred	=	true;

					core._collisions->currentCollision.boxA_ID					=	shapeOne->ID;
					core._collisions->currentCollision.boxB_ID					=	shapeTwo->ID;
					core._collisions->currentCollision.impactPoint.x			=	-FLT_MAX;
					core._collisions->currentCollision.impactPoint.y			=	-FLT_MAX;
					core._collisions->currentCollision.impactPoint.z			=	-FLT_MAX;
					core._collisions->currentCollision.boxA_movementVector		=	shapeOne->center() - shapeOne->centerPointPrevious;
					core._collisions->currentCollision.boxB_movementVector		=	shapeTwo->center() - shapeTwo->centerPointPrevious;
					core._collisions->currentCollision.timeOfImpact				=	-FLT_MAX;

					core._collisions->addToList();
				}
			}
		}
	}//End for loop
	if(collisionOccurred)
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool PhysicsCollision::sweptCCD(vector<AABB*> boxes, Octree* octree, float &timeOfImpact)
{
	/******************************************************
	*	Function Name:		sweptCCD()
	*	Programmer:			Josh Archer
	*
	*	Determines if there was a collision at any point
	*	between the previous position and current position
	*	of two AABB's.
	*	
	*	Also assigns a normalized time of impact value
	*	to pass-by-reference parameter "timeOfImpact"
	*
	******************************************************/

	vector<AABBPair> bps;
	bool collisionOccurred = false;

	_octree->potentialBoxBoxCollision(bps, boxes, octree);
	for(unsigned int i = 0; i < bps.size(); i++) 
	{
		_octree->add(boxes[i]);
		AABBPair bp = bps[i];
		
		AABB* boxA = bp.aabb1;
		AABB* boxB = bp.aabb2;

		//Check if box A and box B were already overlapping in their previous positions:
		D3DXVECTOR3 AB_separation = boxA->centerPointPrevious - boxB->centerPointPrevious;

		if(		(fabs(AB_separation.x) <= fabs(boxA->extent().x + boxB->extent().x))
			&&	(fabs(AB_separation.y) <= fabs(boxA->extent().y + boxB->extent().y))
			&&	(fabs(AB_separation.z) <= fabs(boxA->extent().z + boxB->extent().z))		)
		{
			//The boxes were already overlapping at their previous positions
			
			collisionOccurred = true;
			core._collisions->currentCollision.boxA_ID					=	boxA->ID;
			core._collisions->currentCollision.boxB_ID					=	boxB->ID;
			core._collisions->currentCollision.impactPoint.x			=	-FLT_MAX;
			core._collisions->currentCollision.impactPoint.y			=	-FLT_MAX;
			core._collisions->currentCollision.impactPoint.z			=	-FLT_MAX;
			core._collisions->currentCollision.timeOfImpact				=	0.0f;
			core._collisions->currentCollision.boxA_movementVector		=	boxA->center() - boxA->centerPointPrevious;
			core._collisions->currentCollision.boxB_movementVector		=	boxB->center() - boxB->centerPointPrevious;
			
			core._collisions->addToList();

			timeOfImpact = 0.0f;
		}

		//We know they weren't overlapping at thier previous positions, so let's set up for detecting potential times of impact 
	
	
		D3DXVECTOR3 displacementA = boxA->center() - boxA->centerPointPrevious;		//Displacement of box A between previous and current positions
		D3DXVECTOR3 displacementB = boxB->center() - boxB->centerPointPrevious;		//Displacement of box B between previous and current positions

		D3DXVECTOR3 relativeVelocity = displacementB - displacementA;		//Relative velocity between box A and box B
	
		D3DXVECTOR3 aMin = boxA->centerPointPrevious - boxA->extent();		//Previous min point of box A
		D3DXVECTOR3 aMax = boxA->centerPointPrevious + boxA->extent();		//Previous max point of box A

		D3DXVECTOR3 bMin = boxB->centerPointPrevious - boxB->extent();		//Previous min point of box B
		D3DXVECTOR3 bMax = boxB->centerPointPrevious + boxB->extent();		//Previous max point of box B
	
		//First time of overlap along all axes
		D3DXVECTOR3 t0;
		t0.x, t0.y, t0.z = 0, 0, 0;
		//Last time of overlap along all axes
		D3DXVECTOR3 t1;
		t1.x, t1.y, t1.z = 1, 1, 1;

		for(int i = 0; i < 3; i++)
		{
			if(i = 0)
			{
				//Test x axis

				//Test for earliest time of impact:
				if(		(aMax.x < bMin.x)	&&	(relativeVelocity.x < 0)	)
				{
					t0.x = (aMax.x - bMin.x) / relativeVelocity.x;		//Potential time of impact, normalized
				}
				else if(		(bMax.x < aMin.x)	&&	(relativeVelocity.x > 0)	)
				{
					t0.x = (aMin.x - bMax.x) / relativeVelocity.x;		//Potential time of impact, normalized
				}

				//Test for last time of impact:
				if(		(bMax.x > aMin.x)	&&	(relativeVelocity.x < 0)	)
				{
					t1.x = (aMin.x - bMax.x) / relativeVelocity.x;		//Potential time of impact, normalized
				}
				else if(		(aMax.x > bMin.x)	&&	(relativeVelocity.x > 0)		)
				{
					t1.x = (aMax.x - bMin.x) / relativeVelocity.x;
				}
			}
			else if(i = 1)
			{
//.........这里部分代码省略.........

 BoundingSphere::BoundingSphere(const AABB& aabb)
 {
    pos_radius = vec4(aabb.center(), length(aabb.offset * vec3(0.5f)));
 }

int main(int argc, char **argv)
{

    ros::init(argc, argv, "discretization");
    ros::NodeHandle n;
    ros::Publisher model_pub = n.advertise<sensor_msgs::PointCloud2>("point_cloud", 1);
    ros::Publisher point_pub = n.advertise<geometry_msgs::PoseArray>("voxel", 1);
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
    ros::Publisher OctmapPub = n.advertise<octomap_msgs::Octomap>("LaserOctmap", 1);
    ros::Publisher visCubePub = n.advertise<visualization_msgs::MarkerArray>("filtered_poses", 1);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    std::string path = ros::package::getPath("component_test");
    pcl::io::loadPCDFile<pcl::PointXYZ> (path+"/src/pcd/scaled_desktop.pcd", *cloud);
    //    pcl::io::loadPCDFile<pcl::PointXYZ> (path+"/src/pcd/plane_desktop.pcd", *cloud);
    //      pcl::io::loadPCDFile<pcl::PointXYZ> (path+"/src/pcd/bun000_Structured.pcd", *cloud);

    geometry_msgs::PoseArray points;
    geometry_msgs::Pose pose;

    int x_space=16;//put half the length here (32)
    int y_space=11;//put half the length here (18)
    int z_space=37;
    double res=1;
    for (int z=(-1*z_space) ; z < 2; z+=res)//the length of the aircraft
    {

        for (int y=-1*(y_space-4) ; y< y_space; y+=res)//the hight of the aircraft
        {

            for (int x=-1*x_space ; x< x_space; x+=res)//the width of the aircraft
            {
                pose.position.z=z;
                pose.position.y=y;
                pose.position.x=x;
                pose.orientation.x=0;pose.orientation.y=0;pose.orientation.z=0;pose.orientation.w=1;
                points.poses.push_back(pose);

            }
        }
    }

    //filtering
    //testing the mesh example
    std::vector<Vec3f> p1;
    std::vector<Triangle> t1;
    std::string str = path+"/src/mesh/desktop_scaleddown.obj";
    loadOBJFile(str.c_str(), p1, t1);

    BVHModel<OBBRSS>* m1 = new BVHModel<OBBRSS>();
    boost::shared_ptr<CollisionGeometry> m1_ptr(m1);

    m1->beginModel();
    m1->addSubModel(p1, t1);
    m1->endModel();

    Transform3f tf1;
    tf1.setIdentity();
    tf1.setTranslation(Vec3f(0,0,0));
    CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(m1), tf1);
    Transform3f tf0;

    visualization_msgs::MarkerArray marker_array ;
    visualization_msgs::Marker marker2 ;

    //filtered_points for creating a mesh of points
    vtkSmartPointer<vtkPoints> filtered_points = vtkSmartPointer< vtkPoints >::New();
    for (int j=0; j<points.poses.size();j++)
    {
        boost::shared_ptr<Box> Sample(new Box(1));
        tf0.setIdentity();
        tf0.setTranslation(Vec3f(points.poses[j].position.x , points.poses[j].position.y  ,points.poses[j].position.z ));
        CollisionObject co0(Sample, tf0);
        static const int num_max_contacts = std::numeric_limits<int>::max();
        static const bool enable_contact = true ;
        fcl::CollisionResult result;
        fcl::CollisionRequest request(num_max_contacts, enable_contact);
        fcl::collide(&co0, obj, request, result);
        AABB a    = co0.getAABB() ;
        Vec3f vec2 =  a.center() ;

        //        if (result.isCollision() == true )
        //        {
        //////            marker = drawCUBE(vec2,i,2) ;
        //////            marker_array.markers.push_back(marker);
        //////            collisionFlag.data = true ;
        //////            collisionFlagPub.publish(collisionFlag) ;
        //////            collisionDetected = true;
        //        }
        //        else
        //        {
        //            marker2 = drawCUBE(vec2, j, 1) ;
        //            marker_array.markers.push_back(marker2);
        //        }

        DistanceRequest request2;
        DistanceResult localResult;
        distance(&co0, obj, request2, localResult);
        FCL_REAL min_dist = localResult.min_distance;
//        std::cout<<"minimum distance: "<<min_dist<<std::endl;
//.........这里部分代码省略.........