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;
}
