void
insert_constraints_using_spatial_sort(SDG& sdg)
{
typedef typename Points_container::const_iterator Points_iterator;
typedef std::vector<Points_iterator> Indices;
typedef std::vector<typename SDG::Vertex_handle> Vertices;
Sort_traits_2<K, Points_iterator> sort_traits;
Indices indices;
indices.reserve(points.size());
for(Points_iterator it = points.begin(); it != points.end(); ++it) {
indices.push_back(it);
}
std::random_shuffle(indices.begin(), indices.end());
CGAL::spatial_sort(indices.begin(), indices.end(),
sort_traits);
std::cerr << "Inserting " << points.size() << " points...";
CGAL::Timer timer;
timer.start();
Vertices vertices;
vertices.resize(points.size());
typename SDG::Vertex_handle hint;
for(typename Indices::const_iterator
pt_it_it = indices.begin(), end = indices.end();
pt_it_it != end; ++pt_it_it) {
typename SDG::Vertex_handle vh = sdg.insert(**pt_it_it, hint);
hint = vh;
vertices[*pt_it_it - points.begin()] = vh;
}
timer.stop();
std::cerr << " done (" << timer.time() << "s)\n";
std::cerr << "Inserting " << constraints.size() << " constraints...";
timer.reset();
timer.start();
for(typename Constraints_container::const_iterator
cit = constraints.begin(), end = constraints.end();
cit != end; ++cit) {
const typename SDG::Vertex_handle& v1 = vertices[cit->first];
const typename SDG::Vertex_handle& v2 = vertices[cit->second];
if(v1 != v2)
sdg.insert(v1, v2);
}
timer.stop();
std::cerr << " done (" << timer.time() << "s)\n";
}
// Copy and validate indices.
Tiles::Tiles(Indices const& rIndices, bool remoteFlag) {
ConstIterator i_index;
for (i_index = rIndices.begin(); i_index != rIndices.end(); i_index++) {
IndexType const index = *i_index;
Tile const tile = Tile(index, remoteFlag); // validation happens here
Add(tile);
}
}
/** Constrain one particle, and not the last one.
Detects bugs like not setting the projection matrix entries beyond the last constrained particle
*/
void init_oneConstrainedParticle()
{
indices.clear();
indices.push_back(1);
std::sort(indices.begin(),indices.end()); // checking vectors in linear time requires sorted indices
projection->f_indices.setValue(indices);
/// Init
sofa::simulation::getSimulation()->init(root.get());
}
int main()
{
Points points = gen_points(200);
Hash2::Point cnt(1.,1.);
double rd = 0.45;
Indices dist = dist_nearest(points, cnt, rd);
Indices hash = hash_nearest(points, cnt, rd);
std::sort(dist.begin(), dist.end());
std::sort(hash.begin(), hash.end());
if (dist.size() != hash.size()) {
IMP_THROW("lists are different sizes", ValueException);
}
if (!std::equal(dist.begin(), dist.end(), hash.begin())) {
IMP_THROW("lists have differing elements", ValueException);
}
return 0;
}
int main(int argc, char *argv[]) {
IMP::setup_from_argv(argc, argv, "Test geometric hash.");
Points points = gen_points(200);
Hash2::Point cnt(1., 1.);
double rd = 0.45;
Indices dist = dist_nearest(points, cnt, rd);
Indices hash = hash_nearest(points, cnt, rd);
std::sort(dist.begin(), dist.end());
std::sort(hash.begin(), hash.end());
if (dist.size() != hash.size()) {
IMP_THROW("lists are different sizes", IMP::ValueException);
}
if (!std::equal(dist.begin(), dist.end(), hash.begin())) {
IMP_THROW("lists have differing elements", IMP::ValueException);
}
return 0;
}
void
FilterIndices::compute (PointCloud3D& output) const
{
Indices indices;
output.clear ();
compute (indices);
for (Indices::const_iterator it = indices.begin (); it != indices.end (); ++it) {
output += this->operator[] (*it);
}
}
bool
ok_indices( Indices const& a_indices)
/**@brief
* Is a_index... a valid argument to
* offset_at_indices?
*/
{
unsigned n=a_indices.size();
bool result= n<=rank();
auto index_iter=a_indices.begin();
for( unsigned i=0; i<n && result; ++i,++index_iter)
{
index_t index_i=*index_iter;
result = index_i<size(i);
}
return result;
}
index_t
offset_at_indices
( Indices const& a_indices
)const
/**@brief
* The offset of element in an array
* corresponding to indices, a_index...
*/
{
index_t const offset
= std::inner_product
( a_indices.begin()
, a_indices.end()
, my_strides.begin()+my_dir
, index_t(0)
);
return offset;
}
bool test_projectVelocity()
{
VecDeriv vprev(numNodes);
typename MechanicalObject::WriteVecDeriv v = dofs->writeVelocities();
for (unsigned i=0; i<numNodes; i++){
vprev[i] = v[i] = CPos(i,0,0);
}
// cerr<<"test_projectVelocity, v before = " << v << endl;
projection->projectVelocity(core::MechanicalParams::defaultInstance(), *dofs->write(core::VecDerivId::velocity()) );
// cerr<<"test_projectVelocity, v after = " << v << endl;
bool succeed=true;
typename Indices::const_iterator it = indices.begin(); // must be sorted
for(unsigned i=0; i<numNodes; i++ )
{
if ((it!=indices.end()) && ( i==*it )) // constrained particle
{
CPos crossprod = v[i].cross(direction); // should be parallel
Real scal = crossprod.norm(); // null if v is ok
// cerr<<"scal = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Velocity of constrained particle " << i << " is wrong: " << v[i] ;
}
it++;
}
else // unconstrained particle: check that it has not changed
{
CPos dv = v[i]-vprev[i];
Real scal = dv*dv;
// cerr<<"scal gap = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Velocity of unconstrained particle " << i << " is wrong: " << v[i] ;
}
}
}
return succeed;
}
bool test_projectPosition()
{
VecCoord xprev(numNodes);
typename MechanicalObject::WriteVecCoord x = dofs->writePositions();
for (unsigned i=0; i<numNodes; i++){
xprev[i] = x[i] = CPos(i,0,0);
}
// cerr<<"test_projectPosition, x before = " << x << endl;
projection->projectPosition(core::MechanicalParams::defaultInstance(), *dofs->write(core::VecCoordId::position()) );
// cerr<<"test_projectPosition, x after = " << x << endl;
bool succeed=true;
typename Indices::const_iterator it = indices.begin(); // must be sorted
for(unsigned i=0; i<numNodes; i++ )
{
if ((it!=indices.end()) && ( i==*it )) // constrained particle
{
CPos crossprod = (x[i]-origin).cross(direction); // should be parallel
Real scal = crossprod*crossprod; // null if x is on the line
// cerr<<"scal = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Position of constrained particle " << i << " is wrong: " << x[i] ;
}
it++;
}
else // unconstrained particle: check that it has not changed
{
CPos dx = x[i]-xprev[i];
Real scal = dx*dx;
// cerr<<"scal gap = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Position of unconstrained particle " << i << " is wrong: " << x[i] ;
}
}
}
return succeed;
}
