PhysicsSystem::PhysicsSystem()
{
b2Vec2 gravityVector(0, 10);
bool doSleep = 0;
physicsWorld = new b2World(gravityVector);
physicsWorld->SetAllowSleeping(doSleep);
updateTimer.reset();
}
void Segmentation::doSegmentation(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
//MainPlane groundMainPlane;
// Variables
isComputed = false ;
struct timeval tbegin,tend;
double texec=0.0;
// Start timer
gettimeofday(&tbegin,NULL);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZRGBA>);
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFilteredCopy(new pcl::PointCloud<pcl::PointXYZRGBA>);
_mainCloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
(void) passthroughFilter(-5.0, 5.0, -5.0, 5.0, -5.0, 5.0, cloud, cloudFiltered); // 3.0, 3.0, -0.2, 2.0, -4.0, 4.0
pcl::copyPointCloud(*cloudFiltered, *_mainCloud);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Hullground (new pcl::PointCloud<pcl::PointXYZRGBA>);
std::vector<pcl::PointIndices> vectorInliers(0);
std::vector <pcl::ModelCoefficients> vectorCoeff(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorCloudinliers(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorHull(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorCluster(0);
pcl::ModelCoefficients::Ptr coefficientsGround (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliersGround (new pcl::PointIndices);
// pcl::VoxelGrid< pcl::PointXYZRGBA > sor;
// sor.setInputCloud (_mainCloud);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*_mainCloud);
//(void) findGround(_mainCloud,_ground);
//isComputed = true;
if(findGround(_mainCloud,_ground))
{
Eigen::Vector3f gravityVector(_ground.getCoefficients()->values[0], _ground.getCoefficients()->values[1], _ground.getCoefficients()->values[2]);
//(void) findOtherPlanesRansac(_mainCloud,gravityVector,vectorCoeff , vectorCloudinliers,vectorHull);
//(void) regionGrowing(vectorCloudinliers, vectorCluster);
euclidianClustering(_mainCloud,vectorCloudinliers);
ransac(vectorCloudinliers,gravityVector,vectorCluster );
createPlane(vectorCluster);
// End timer
}
gettimeofday(&tend,NULL);
// Compute execution time
texec = ((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000;
std::cout << "time : " << texec << std::endl;
}
bool World::SetupPhysics(const Vector2& gravity, const Vector2& maxVertex, const Vector2& minVertex)
{
if (_physicsSetUp)
{
return false;
}
//setup physics
b2AABB worldAABB;
worldAABB.lowerBound.Set(minVertex.X, minVertex.Y);
worldAABB.upperBound.Set(maxVertex.X, maxVertex.Y);
b2Vec2 gravityVector(gravity.X, gravity.Y);
_physicsWorld = new b2World(gravityVector);
_physicsWorld->SetContactListener(this);
return _physicsSetUp = _physicsRunning = true;
}
bool Game::bulletSetup(){
AxisAlignedBox bounds(
Ogre::Vector3 (-10000, -10000, -10000),
Ogre::Vector3 (10000, 10000, 10000)
);
Vector3 gravityVector(0,-9.81 * 2,0);
mWorld = new OgreBulletDynamics::DynamicsWorld(mSceneMgr, bounds, gravityVector);
debugDrawer = new OgreBulletCollisions::DebugDrawer();
debugDrawer->setDrawWireframe(true); // we want to see the Bullet containers
mWorld->setDebugDrawer(debugDrawer);
mWorld->setShowDebugShapes(false);
SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode("debugDrawer", Ogre::Vector3::ZERO);
node->attachObject(static_cast <SimpleRenderable *> (debugDrawer));
return true;
}
bool World::SetupPhysics(Vector2 gravity, Vector2 maxVertex, Vector2 minVertex)
{
if (_physicsSetUp)
{
return false;
}
//setup physics
b2AABB worldAABB;
worldAABB.lowerBound.Set(minVertex.X, minVertex.Y);
worldAABB.upperBound.Set(maxVertex.X, maxVertex.Y);
b2Vec2 gravityVector(gravity.X, gravity.Y);
bool doSleep = true;
_physicsWorld = new b2World(worldAABB, gravityVector, doSleep);
_physicsWorld->SetContactListener(this);
_physicsDebugDraw = new /*Physics*/DebugDraw();
_physicsWorld->SetDebugDraw(_physicsDebugDraw);
return _physicsSetUp = true;
}
void PhysicsSystem::setGravity(Vector2 value)
{
b2Vec2 gravityVector(value.x, value.y);
physicsWorld->SetGravity(gravityVector);
}