Octree::Indices Octree::indexCellPath(const Locations& path) {
// First through the allocated cells
Indices cellPath = indexConcreteCellPath(path);
// Catch up from the last allocated cell on the path
auto currentIndex = cellPath.back();
for (int l = (Index) cellPath.size(); l < (Index) path.size(); l++) {
auto& location = path[l];
// Allocate the new index & connect it to the parent
auto newIndex = allocateCell(currentIndex, location);
// One more cell index on the path, moving on
currentIndex = newIndex;
cellPath.push_back(currentIndex);
// Except !!! if we actually couldn't allocate anymore
if (newIndex == INVALID_CELL) {
// no more cellID available, stop allocating
// THe last index added is INVALID_CELL so the caller will know we failed allocating everything
break;
}
}
return cellPath;
}
void merge(const Pointf3s& triangles) {
const size_t offs = points.size();
points.insert(points.end(), triangles.begin(), triangles.end());
indices.reserve(indices.size() + points.size() / 3);
for(int i = (int)offs; i < (int)points.size(); i += 3)
indices.emplace_back(i, i + 1, i + 2);
}
// 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);
}
}
void pcl::gpu::Octree::radiusSearch(const Queries& queries, const Indices& indices, float radius, int max_results, NeighborIndices& results) const
{
assert(queries.size() > 0 && indices.size() > 0);
results.create(static_cast<int> (indices.size()), max_results);
results.sizes.create(indices.size());
const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
static_cast<OctreeImpl*>(impl)->radiusSearch(q, indices, radius, results);
}
/** Constrain all the particles.
*/
void init_allParticlesConstrained()
{
indices.clear();
for(unsigned i = 0; i<numNodes; i++)
indices.push_back(i);
projection->f_indices.setValue(indices);
/// Init
sofa::simulation::getSimulation()->init(root.get());
}
void ParsePatterns(const Indices& pats, Builder& builder) const
{
Dbg("Patterns: %1% to parse", pats.Count());
for (Indices::Iterator it = pats.Items(); it; ++it)
{
const uint_t patIndex = *it;
Dbg("Parse pattern %1%", patIndex);
ParsePattern(patIndex, builder);
}
}
/** 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());
}
AssignStatement::AssignStatement(Expression *target, Indices const &indices, Expression *value)
: _indexCount(0)
{
_args.add(value);
_indexCount = dint(indices.size());
for(Indices::const_reverse_iterator i = indices.rbegin(); i != indices.rend(); ++i)
{
_args.add(*i);
}
_args.add(target);
}
void merge(const Contour3D& ctr) {
auto s3 = coord_t(points.size());
auto s = indices.size();
points.insert(points.end(), ctr.points.begin(), ctr.points.end());
indices.insert(indices.end(), ctr.indices.begin(), ctr.indices.end());
for(size_t n = s; n < indices.size(); n++) {
auto& idx = indices[n]; x(idx) += s3; y(idx) += s3; z(idx) += s3;
}
}
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);
}
}
void ParseOrnaments(const Indices& ornaments, Builder& builder) const
{
Dbg("Ornaments: %1% to parse", ornaments.Count());
const std::size_t ornamentsTable = fromLE(Source.OrnamentsOffset);
for (Indices::Iterator it = ornaments.Items(); it; ++it)
{
const uint_t ornIdx = *it;
Dbg("Parse ornament %1%", ornIdx);
const std::size_t ornOffset = ReadWord(ornamentsTable, ornIdx);
Ornament result;
ParseOrnament(ornOffset, result);
builder.SetOrnament(ornIdx, result);
}
}
void ParseSamples(const Indices& samples, Builder& builder) const
{
Dbg("Samples: %1% to parse", samples.Count());
const std::size_t samplesTable = fromLE(Source.SamplesOffset);
for (Indices::Iterator it = samples.Items(); it; ++it)
{
const uint_t samIdx = *it;
Dbg("Parse sample %1%", samIdx);
const std::size_t samOffset = ReadWord(samplesTable, samIdx);
Sample result;
ParseSample(samOffset, result);
builder.SetSample(samIdx, result);
}
}
virtual Indices filterParticles(const Particles&)
{
Indices indices;
const float time = parentDocument()->getAnimationTime();
int nb = birth.size();
float age = maxAge.getValue();
for(int i=0; i<nb; ++i)
{
if(time >= birth[i] + age)
indices.insert(i);
}
return indices;
}
std::vector<Indices> GenerateOddRank(const Indices& indices) const {
// Get all the three partitions of indices
auto partitions = indices.GetAllPartitions(3);
std::vector<Indices> result;
// Iterate over all partitions
for (auto& partition : partitions) {
if (partition.second.Size() != 0) {
// Generate the two-partitions of the rest with GenerateEvenRank
auto list = GenerateEvenRank(partition.second);
// Iterate over all the possible gamma indices
for (auto &l : list) {
auto current = partition.first;
current.Append(l);
result.push_back(current);
}
} else {
result.push_back(partition.first);
}
}
return result;
}
index_t
reshape( Indices const& a_sizes)
{
index_t const rankp1=a_sizes.size()+1;
my_strides.resize(rankp1);
return init_strides( a_sizes);
}
void RenderObject::setIndices( const Indices& indices )
{
m_indices.clear();
for ( int i = 0; i < indices.count(); ++i )
{
m_indices.append( indices.at( i ) );
}
if ( !m_indexBuffer.isCreated() )
{
m_indexBuffer.create();
}
m_vao.bind();;
m_indexBuffer.bind();
m_indexBuffer.allocate( m_indices.data(), m_indices.count() * sizeof( unsigned int ) );
m_vao.release();
}
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;
}
/**
Generates the
*/
std::vector<std::pair<Indices, Indices>> GenerateTwoPartition(const Indices& indices) const {
assert(indices.Size() > 1);
std::vector<std::pair<Indices, Indices>> result;
// Take the first letter
auto first = indices[0];
for (int i=1; i<indices.Size(); i++) {
auto current = indices[i];
Indices remaining = indices;
Indices gamma;
gamma.Insert(first);
gamma.Insert(current);
remaining.Remove(i);
remaining.Remove(0);
result.push_back({ gamma, remaining });
}
return result;
}
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";
}
void InitVertices( Vertices& vertices, Indices& indices )
{
assert( 0 == vertices.size() );
assert( 0 == indices.size() );
const float edgeSize = 100.0f;
const unsigned nVerticesPerEdge = 201;
for( unsigned i=0; i<nVerticesPerEdge; ++i )
{
for( unsigned j=0; j<nVerticesPerEdge; ++j )
{
vertices.push_back(
Vertex( 0.0f, edgeSize*i/(nVerticesPerEdge-1) - edgeSize/2, edgeSize*j/(nVerticesPerEdge-1) - edgeSize/2,
1.0f, 0.0f, 0.0f ) );
}
}
for( unsigned i=0; i<nVerticesPerEdge-1; ++i )
{
for( unsigned j=0; j<nVerticesPerEdge-1; ++j )
{
indices.push_back( (i)*nVerticesPerEdge + (j) );
indices.push_back( (i+1)*nVerticesPerEdge + (j) );
indices.push_back( (i+1)*nVerticesPerEdge + (j+1) );
indices.push_back( (i)*nVerticesPerEdge + (j) );
indices.push_back( (i+1)*nVerticesPerEdge + (j+1) );
indices.push_back( (i)*nVerticesPerEdge + (j+1) );
}
}
}
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;
}
transformation_t epnp(
const AbsoluteAdapterBase & adapter,
const Indices & indices )
{
//starting from 4 points, we have a unique solution
assert(indices.size() > 5);
modules::Epnp PnP;
PnP.set_maximum_number_of_correspondences(indices.size());
PnP.reset_correspondences();
for( size_t i = 0; i < indices.size(); i++ )
{
point_t p = adapter.getPoint(indices[i]);
bearingVector_t f = adapter.getBearingVector(indices[i]);
PnP.add_correspondence(p[0], p[1], p[2], f[0], f[1], f[2]);
}
double R_epnp[3][3], t_epnp[3];
PnP.compute_pose(R_epnp, t_epnp);
rotation_t rotation;
translation_t translation;
for(int r = 0; r < 3; r++)
{
for(int c = 0; c < 3; c++)
rotation(r,c) = R_epnp[r][c];
}
translation[0] = t_epnp[0];
translation[1] = t_epnp[1];
translation[2] = t_epnp[2];
//take inverse transformation
rotation.transposeInPlace();
translation = -rotation * translation;
transformation_t transformation;
transformation.col(3) = translation;
transformation.block<3,3>(0,0) = rotation;
return transformation;
}
int ParticleSystem::createSphere(int numParticles, float maxspray, Vertices &vtx, Indices &ind)
{
int i;
Vector3f pos;
Vector3f norm;
Vector2f texCoord;
Vector4f colour;
Vector3f wander;
vtx.resize(numParticles);
std::cout << " the vector's size is: " << vtx.size() << std::endl;
std::cout << " the vector's capacity is: " << vtx.capacity() << std::endl;
std::cout << " the vector's maximum size is: " << vtx.max_size() << std::endl;
ind.resize(0);
srand(time(0));
float trad = 0.4; // Defines the starting point of the particles
/* Create a set of points which will be the particles */
/* This is similar to drawing a torus: we will sample points on the surface of the torus */
float u, v, w, theta, phi, spray; // Work variables
for (int i = 0; i < numParticles; i++){
// Randomly select two numbers to define a point on the torus
u = ((double) rand() / (RAND_MAX));
v = ((double) rand() / (RAND_MAX));
// Use u and v to define the point on the torus
theta = u * 2.0f*M_PI;
phi = v * 2.0f*M_PI;
norm = Vector3f(cos(theta)*cos(phi), sin(theta)*cos(phi), sin(phi));
pos = Vector3f(norm.x*trad, norm.y*trad, norm.z*trad);
colour = Vector4f(((float)i) / ((float)numParticles), 0.0f, 1.0f - (((float)i) / ((float)numParticles)), 1.0f);
texCoord = Vector2f(0, 0); //not used for particels
// Now sample a point on a sphere to define a direction for points to wander around
u = ((double) rand() / (RAND_MAX));
v = ((double) rand() / (RAND_MAX));
w = ((double) rand() / (RAND_MAX));
theta = u * 2*M_PI;
phi = acos(2.0*v * -1.0);
spray = maxspray*pow((float) w, (float) (1.0/3.0)); // Cubic root
wander = Vector3f(spray*sin(theta)*sin(phi), spray*cos(theta)*sin(phi), spray*cos(phi));
norm = wander;
vtx[i] = Vertex(pos, colour, norm, texCoord);
}
return(0);
}
Plane::Plane( D3D::GraphicDevice &device,
const Material& material )
: device_(device),
vertexDeclaration_(device, DefaultVertexDeclaration),
vertexBuffer_(device),
indexBuffer_(device),
shader_(device, L"plane.vsh"),
material_(material)
{
Vertices vertices;
Indices indices;
InitVertices( vertices, indices );
nVertices_ = vertices.size();
nPrimitives_ = indices.size()/3;
vertexBuffer_.SetVertices( &vertices[0], vertices.size() );
indexBuffer_.SetIndices( &indices[0], indices.size() );
SetViewMatrix( UnityMatrix() );
SetProjectiveMatrix( UnityMatrix() );
}
void Tessellation( Node& n1, Node& n2, Node& n3, Vertices& vertices, Indices& indices,
unsigned curDepth, unsigned maxDepth )
{
if( curDepth<maxDepth )
{
Node *n12 = GetOrCreateChild(n1, n2, vertices);
Node *n23 = GetOrCreateChild(n2, n3, vertices);
Node *n31 = GetOrCreateChild(n3, n1, vertices);
Tessellation( n1, *n12, *n31, vertices, indices, curDepth+1, maxDepth );
Tessellation( n2, *n23, *n12, vertices, indices, curDepth+1, maxDepth );
Tessellation( n3, *n31, *n23, vertices, indices, curDepth+1, maxDepth );
Tessellation( *n12, *n23, *n31, vertices, indices, curDepth+1, maxDepth );
}
else
{
indices.push_back( n1.GetIndex() );
indices.push_back( n2.GetIndex() );
indices.push_back( n3.GetIndex() );
}
}
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;
}
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;
}
Tensor::Tensor Generate(const Indices& indices) const {
// Expect all the ranges to be in space
unsigned order = indices.Size();
for (auto& index : indices) {
if (index.GetRange() != Common::Range(1,3)) {
// throw error
}
}
// We cannot build anything with one index!
if (indices.Size() == 1) return Tensor::Tensor::Zero();
Tensor::Tensor result = Tensor::Tensor::Zero();
unsigned variableCounter = 0;
// Calculate
unsigned numEpsilon = (indices.Size() % 2 == 0) ? 0 : 1;
unsigned numGammas = (indices.Size() % 2 == 0) ? indices.Size()/2 : (indices.Size()-3)/2;
std::vector<Indices> possibleIndices;
if (numEpsilon == 1) {
possibleIndices = GenerateOddRank(indices);
} else {
possibleIndices = GenerateEvenRank(indices);
}
std::vector<Tensor::Tensor> tensors;
for (auto& newIndices : possibleIndices) {
// Create variable
Tensor::Scalar variable ("e", ++variableCounter);
tensors.push_back(variable * Tensor::Tensor::EpsilonGamma(numEpsilon, numGammas, newIndices));
}
return Tensor::Tensor::Add(tensors);
}
std::size_t map(const Indices& indices) const
{
std::size_t rank = shape_.size();
assert(rank == indices.size());
std::size_t address = 0;
for (std::size_t i = 0; i < rank; ++i)
{
address = address * shape_[i] + indices[i];
}
return address;
}
Sphere::Sphere( float radius, unsigned tesselationLevel, D3D::GraphicDevice device, float freq,
const Material& material )
: device_(device),
vertexDeclaration_(device, DefaultVertexDeclaration),
vertexBuffer_(device),
indexBuffer_(device),
shader_(device, L"sphere.vsh"),
radius_(radius),
tesselationLevel_(tesselationLevel),
freq_(freq),
material_(material)
{
Vertices vertices;
Indices indices;
InitVertices(tesselationLevel, radius*sqrtf(2), vertices, indices);
nVertices_ = vertices.size();
nPrimitives_ = indices.size()/3;
vertexBuffer_.SetVertices( &vertices[0], vertices.size() );
indexBuffer_.SetIndices( &indices[0], indices.size() );
SetPositionMatrix( UnityMatrix() );
SetViewMatrix( UnityMatrix() );
SetProjectiveMatrix( UnityMatrix() );
}
