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) { //Test y axis //Test for earliest time of impact: if( (aMax.y < bMin.y) && (relativeVelocity.y < 0) ) { t0.y = (aMax.y - bMin.y) / relativeVelocity.y; //Potential time of impact, normalized } else if( (bMax.y < aMin.y) && (relativeVelocity.y > 0) ) { t0.y = (aMin.y - bMax.y) / relativeVelocity.y; //Potential time of impact, normalized } //Test for last time of impact: if( (bMax.y > aMin.y) && (relativeVelocity.y < 0) ) { t1.y = (aMin.y - bMax.y) / relativeVelocity.y; //Potential time of impact, normalized } else if( (aMax.y > bMin.y) && (relativeVelocity.y > 0) ) { t1.y = (aMax.y - bMin.y) / relativeVelocity.y; } } else { //Test z axis //Test for earliest time of impact: if( (aMax.z < bMin.z) && (relativeVelocity.z < 0) ) { t0.z = (aMax.z - bMin.z) / relativeVelocity.z; //Potential time of impact, normalized } else if( (bMax.z < aMin.z) && (relativeVelocity.z > 0) ) { t0.z = (aMin.z - bMax.z) / relativeVelocity.z; //Potential time of impact, normalized } //Test for last time of impact: if( (bMax.z > aMin.z) && (relativeVelocity.z < 0) ) { t1.z = (aMin.z - bMax.z) / relativeVelocity.z; //Potential time of impact, normalized } else if( (aMax.z > bMin.z) && (relativeVelocity.z > 0) ) { t1.z = (aMax.z - bMin.z) / relativeVelocity.z; } } } float tMin, tMax; //Store our earliest and latest impact times (normalized) //Potential time of first impact tMin = max(t0.x, max(t0.y, t0.z)); //Potential time of last impact tMax = min(t1.x, min(t1.y, t1.z)); //An impact has only occurred if the time of first impact is less than or equal to the time of last impact if(tMin <= tMax) { 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 = tMin; core._collisions->currentCollision.boxA_movementVector = boxA->center() - boxA->centerPointPrevious; core._collisions->currentCollision.boxB_movementVector = boxB->center() - boxB->centerPointPrevious; core._collisions->addToList(); timeOfImpact = tMin; } } if(collisionOccurred) { return true; } else { return false; } }
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; if(min_dist <= 2 && min_dist >= 1.0) { marker2 = drawCUBE(vec2, j, 1) ; marker_array.markers.push_back(marker2); //filtered_points for creating a mesh of points filtered_points->InsertNextPoint(points.poses[j].position.x, points.poses[j].position.y, points.poses[j].position.z); if(min_dist > 1 && min_dist < 1.3) { std::cout<<"minimum distance: "<<min_dist<<std::endl; std::cout<<"points position x: "<<points.poses[j].position.x<<" points position y: "<<points.poses[j].position.y<<" points position z: "<<points.poses[j].position.z<<std::endl; } } } //***************making a mesh (Optional)******************* // vtkSmartPointer< vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New(); // polydata->SetPoints(filtered_points); // // vtkSmartPointer<vtkXMLPolyDataWriter> pointsWriter = vtkSmartPointer<vtkXMLPolyDataWriter>::New(); // pointsWriter->SetFileName("points.vtp"); // pointsWriter->SetInput(polydata); // pointsWriter->Write(); // // //convert vtp to ply mesh // std::string inputFileName = "points.vtp"; // std::string outputFileName = "samples.ply"; // // vtkSmartPointer<vtkXMLPolyDataReader> reader = vtkSmartPointer<vtkXMLPolyDataReader>::New(); // reader->SetFileName(inputFileName.c_str()); // reader->Update(); // // vtkSmartPointer<vtkPLYWriter> writer = vtkSmartPointer<vtkPLYWriter>::New(); // writer->SetFileName(outputFileName.c_str()); // writer->SetInputConnection(reader->GetOutputPort()); // writer->Update(); // ROS_INFO("DONE :) samples mesh is generated"); //*********************************************** //my stupid way***** // create empty tree with resolution 0.1 // double octMapRes = 1.0; // octomap::OcTree* octTree = new octomap::OcTree(octMapRes); // octomap::Pointcloud octPointCloud; // double distance = 0; // double minDist= 1.0 ; // double maxDist= 3.8 ; // //Filtering the sample views data if needed // for (int j=0; j<points.poses.size();j++) // { // for(int i = 0;i<cloud->points.size();i++) // { // distance = dist(cloud->points[i],points.poses[j]); // if(distance>=minDist && distance<=maxDist) // { // ROS_INFO("samples filtering using distance"); // octomap::point3d sample((float) points.poses[j].position.x,(float) points.poses[j].position.y,(float) points.poses[j].position.z); // std::cout<<std::endl<<"position X: "<<points.poses[j].position.x<<" position y: "<<points.poses[j].position.y<<" position z: "<<points.poses[j].position.z<<std::endl; // octPointCloud.push_back(sample); // break; // } // } // } // octomap::point3d origin(0.0,0.0,0.0); // octTree->insertPointCloud(octPointCloud,origin); // octTree->updateInnerOccupancy(); // //octTree->writeBinary("static_occ.bt"); visCubePub.publish(marker_array); visualization_msgs::Marker marker; while(ros::ok()) { //mesh points publish sensor_msgs::PointCloud2 cloud1; pcl::toROSMsg(*cloud, cloud1); cloud1.header.frame_id = "base_point_cloud"; cloud1.header.stamp = ros::Time::now(); model_pub.publish(cloud1); //visualize the points points.header.frame_id= "base_point_cloud"; points.header.stamp = ros::Time::now(); point_pub.publish(points); // octomap_msgs::Octomap octomap ; // octomap.binary = 1 ; // octomap.id = 1 ; // octomap.resolution =1.0; // octomap.header.frame_id = "base_point_cloud"; // octomap.header.stamp = ros::Time::now(); // bool res = octomap_msgs::fullMapToMsg(*octTree, octomap); // if(res) // { // OctmapPub.publish(octomap); // } // else // { // ROS_WARN("OCT Map serialization failed!"); // } ros::spinOnce(); } return 0; }