/** @name Test_Cases
For each of these cases, we check if the accurate forces are computed
*/
DiagonalStiffness_test():Inherited::ForceField_test()
{
this->errorFactorPotentialEnergy = 3; // increading tolerance for potential energy test due to non-linearities
//Position
x.resize(3);
DataTypes::set( x[0], 0,0,0);
DataTypes::set( x[1], -1,0,0);
DataTypes::set( x[2], 7,0,0);
//Velocity
v.resize(3);
DataTypes::set( v[0], 0,0,0);
DataTypes::set( v[1], 0,0,0);
DataTypes::set( v[2], 0,0,0);
//Force
f.resize(3);
DataTypes::set( f[0], 0,0,0 );
DataTypes::set( f[1], 100,0,0 );
DataTypes::set( f[2], -49,0,0 );
// Set force parameters
VecDeriv& stiffnesses = *Inherited::force->diagonal.beginWriteOnly();
stiffnesses.resize(3);
stiffnesses[0] = 10;
stiffnesses[1] = 100;
stiffnesses[2] = 7;
Inherited::force->diagonal.endEdit();
}
void ClosestPointRegistrationForceFieldCam<DataTypes, DepthTypes>::initTarget()
{
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr target (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB> target;
pcl::io::loadPCDFile ("/home/apetit/soft/catkin_ws/test_pcd12.pcd", target);
VecCoord targetpos;
targetpos.resize(target.size());
Vector3 pos;
Vector3 col;
for (unsigned int i=0; i<targetpos.size(); i++)
{
pos[0] = target[i].x;
pos[1] = target[i].y;
pos[2] = target[i].z;
targetpos[i]=pos;
}
//targetPositions.setValue(targetpos);
// build k-d tree
//const VecCoord& p = targetPositions.getValue();
const VecCoord& p = targetpos;
targetPositions.setValue(p);
targetKdTree.build(p);
// detect border
if(targetBorder.size()!=p.size()) { targetBorder.resize(p.size()); detectBorder(targetBorder,targetTriangles.getValue()); }
}
TrianglePressureForceField_test(): Inherited::ForceField_test(std::string(SOFABOUNDARYCONDITION_TEST_SCENES_DIR) + "/" + "TrianglePressureForceField.scn")
{
// potential energy is not implemented and won't be tested
this->flags &= ~Inherited::TEST_POTENTIAL_ENERGY;
// Set vectors, using DataTypes::set to cope with tests in dimension 2
//Position
x.resize(3);
DataTypes::set( x[0], 0,0,0);
DataTypes::set( x[1], 1,0,0);
DataTypes::set( x[2], 1,1,0);
//Velocity
v.resize(3);
DataTypes::set( v[0], 0,0,0);
DataTypes::set( v[1], 0,0,0);
DataTypes::set( v[2], 0,0,0);
//Force
f.resize(3);
Vec3 f0(0,0,0.1);
DataTypes::set( f[0], f0[0], f0[1], f0[2]);
DataTypes::set( f[1], f0[0], f0[1], f0[2]);
DataTypes::set( f[2], f0[0], f0[1], f0[2]);
// Set the properties of the force field
Inherited::force->normal.setValue(Deriv(0,0,1));
Inherited::force->dmin.setValue(-0.01);
Inherited::force->dmax.setValue(0.01);
Inherited::force->pressure=Coord(0,0,0.6);
}
void ClosestPointRegistrationForceFieldCam<DataTypes, DepthTypes>::extractTargetPCD()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr target;
target = PCDFromRGBD(depth,foreground);
pcl::PointCloud<pcl::PointXYZRGB> target0;
pcl::io::loadPCDFile ("/home/apetit/soft/catkin_ws/test_pcd12.pcd", target0);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr target;
VecCoord targetpos;
targetpos.resize(target->size());
Vector3 pos;
Vector3 col;
for (unsigned int i=0; i<target->size(); i++)
{
pos[0] = target->points[i].x;
pos[1] = target->points[i].y;
pos[2] = target->points[i].z;
targetpos[i]=pos;
//std::cout << " x " << pos[0] << " y " << pos[1] << " z " << pos[2] << std::endl;
}
/*for (unsigned int i=target->size(); i<targetpos.size(); i++)
{
pos[0] = target0[i].x;
pos[1] = target0[i].y;
pos[2] = target0[i].z;
targetpos[i]=pos;
//std::cout << " x " << pos[0] << " y " << pos[1] << " z " << pos[2] << std::endl;
}*/
//targetPositions.setValue(targetpos);
// build k-d tree
//const VecCoord& p = targetPositions.getValue();
const VecCoord& p = targetpos;
targetPositions.setValue(p);
targetKdTree.build(p);
// detect border
if(targetBorder.size()!=p.size()) { targetBorder.resize(p.size()); detectBorder(targetBorder,targetTriangles.getValue()); }
}
