Mesh::Mesh(VertexList const& vertices)
: m_id(afth::UUID::v4())
{
size_t size = vertices.size() * 3;
float* arr = new float[size];
VertexList::const_iterator it = vertices.begin(), end = vertices.end();
for (size_t i = 0; it != end; ++it, i += 3)
{
std::memcpy(arr + i, (*it).coordinates().arr().data(), 3 * sizeof(float));
}
//glGenVertexArrays(1, &m_vertexArray);
//glGenBuffers(1, &m_vertexBuffer);
//glBindBuffer(GL_ARRAY_BUFFER, m_vertexBuffer);
//glBufferData(GL_ARRAY_BUFFER, size * sizeof(float), arr, GL_STATIC_DRAW);
//glBindVertexArray(m_vertexArray);
//GLint positionIndex = glGetAttribLocation(glProgramUniform1, "position");
//glEnableVertexAttribArray(0);
//glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);
//glBindBuffer(GL_ARRAY_BUFFER, 0);
//glBindVertexArray(0);
delete [] arr;
}
void IncrementalDelaunayTriangulator::insertSites(const VertexList& vertices)
{
for (VertexList::const_iterator x=vertices.begin();
x != vertices.end(); ++x) {
insertSite(*x);
}
}
void Reprojector::projectCloud( IplImage *image )
{
cvZero( image );
if( !this->projVertices.size() )
return;
VertexList tempList;
tempList.resize( this->projVertices.size() );
transform3DTo2D( this->projVertices.size(), &( this->projVertices[0] ), &( tempList[0] ), this->cxProjector, this->cyProjector, this->focalLengthProjector, false );
unsigned short val = 0;
unsigned short *tempPtr = NULL;
for( VertexList::const_iterator it = tempList.begin(); it != tempList.end(); ++it )
{
unsigned int x = it->X;
unsigned int y = it->Y;
unsigned short depth = -( it->Z );
if( x >= image->width || y >= image->height )
continue;
val = depth;
tempPtr = (unsigned short*)( image->imageData ) + x + y * image->width;
if( !( *tempPtr ) || *tempPtr > val ) //z-test
*tempPtr = val;
}
}
void print_out (VertexList V)
{
// print out
for (VertexList::iterator i = V.begin(); i != V.end(); ++i)
{
cout << i->first << " spe:" << i->second.spe << " pi:" << i->second.pi << endl;
}
}
void VertexListToBufP(std::vector<float> &dst,const VertexList &list){
dst.clear();
dst.reserve(list.size()*3);
for(VertexList::const_iterator i = list.begin();i!=list.end();i++){
dst.push_back((*i).pos.x);
dst.push_back((*i).pos.y);
dst.push_back((*i).pos.z);
}
}
// copyVertexListToPointList a vector for Vec3 into a vector of Point's.
void copyVertexListToPointList(const VertexList& in,PointList& out)
{
out.reserve(in.size());
for(VertexList::const_iterator itr=in.begin();
itr!=in.end();
++itr)
{
out.push_back(Point(0,*itr));
}
}
const SrSphere3D createBoundingSphere(const SrPoint3D* point, int numPoint)
{
ASSERT(numPoint>0);
SrPoint3D supportPoint[4];
VertexList vertexList;
SrPoint3D* buffer = new SrPoint3D[numPoint];
int i;
for( i=0 ; i<numPoint ; i++ )
buffer[i] = point[i];
std::random_shuffle(buffer,buffer + numPoint);
std::copy(buffer,buffer + numPoint, std::back_inserter(vertexList));
delete []buffer;
SrSphere3D sphere;
smallestEnclosingSphere(supportPoint,0,vertexList,vertexList.end(),sphere);
return sphere;
}
void ProgressiveTriangleGeometry::FindEdgeToCollapse(VertexList& /*org_vertex_list*/,
TriangleList& /*org_triangle_list*/, VertexList& vertex_list, TriangleList& triangle_list, Edge& edge) {
if (triangle_list.empty())
return;
float current_error = 0.0f;
float current_max_error = 0.0f;
edge.v1_ = 0;
edge.v2_ = 0;
edge.triangle_list_.clear();
// Calculate mean error.
VertexList::iterator v_iter;
for (v_iter = vertex_list.begin();
v_iter != vertex_list.end();
++v_iter) {
if (v_iter == vertex_list.begin()) {
current_max_error = (*v_iter)->error_;
} else if((*v_iter)->error_ > current_max_error) {
current_max_error = (*v_iter)->error_;
}
current_error += (*v_iter)->error_;
}
current_error /= (float)vertex_list.size();
float min_error = 0.0f;
float min_error1 = 0.0f; // Temporary error value storage for _edge->v1_.
float min_error2 = 0.0f; // Temporary error value storage for _edge->v2_.
bool first = true;
// Test vertex collaps on all triangles.
TriangleList::iterator tri_iter;
for (tri_iter = triangle_list.begin();
tri_iter != triangle_list.end();
++tri_iter) {
Triangle* triangle = *tri_iter;
vec3 diff1;
vec3 diff2;
Vertex mid;
// Test V1 and V2.
mid.x() = (triangle->v1_->x() + triangle->v2_->x()) * 0.5f;
mid.y() = (triangle->v1_->y() + triangle->v2_->y()) * 0.5f;
mid.z() = (triangle->v1_->z() + triangle->v2_->z()) * 0.5f;
// Calculate the distance between the new, merged position,
// and the original vertex position.
diff1.Set(mid.x() - triangle->v1_->twin_->x(),
mid.y() - triangle->v1_->twin_->y(),
mid.z() - triangle->v1_->twin_->z());
diff2.Set(mid.x() - triangle->v2_->twin_->x(),
mid.y() - triangle->v2_->twin_->y(),
mid.z() - triangle->v2_->twin_->z());
float error1 = diff1.GetLength() + triangle->v1_->error_;
float error2 = diff2.GetLength() + triangle->v2_->error_;
float error = (error1 + error2 + current_error) / 3.0f;
if (first == true || error < min_error) {
edge.v1_ = triangle->v1_;
edge.v2_ = triangle->v2_;
min_error1 = error1;
min_error2 = error2;
min_error = error;
first = false;
}
// Test V2 and V3.
mid.x() = (triangle->v2_->x() + triangle->v3_->x()) * 0.5f;
mid.y() = (triangle->v2_->y() + triangle->v3_->y()) * 0.5f;
mid.z() = (triangle->v2_->z() + triangle->v3_->z()) * 0.5f;
// Calculate the distance between the new, merged position,
// and the original vertex position.
diff1.Set(mid.x() - triangle->v2_->twin_->x(),
mid.y() - triangle->v2_->twin_->y(),
mid.z() - triangle->v2_->twin_->z());
diff2.Set(mid.x() - triangle->v3_->twin_->x(),
mid.y() - triangle->v3_->twin_->y(),
mid.z() - triangle->v3_->twin_->z());
error1 = diff1.GetLength() + triangle->v1_->error_;
error2 = diff2.GetLength() + triangle->v2_->error_;
error = (error1 + error2 + current_error) / 3.0f;
if (error < min_error) {
edge.v1_ = triangle->v1_;
edge.v2_ = triangle->v2_;
min_error = error;
min_error1 = error1;
min_error2 = error2;
}
// Test V3 and V1.
mid.x() = (triangle->v3_->x() + triangle->v1_->x()) * 0.5f;
mid.y() = (triangle->v3_->y() + triangle->v1_->y()) * 0.5f;
mid.z() = (triangle->v3_->z() + triangle->v1_->z()) * 0.5f;
// Calculate the distance between the new, merged position,
// and the original vertex position.
diff1.Set(mid.x() - triangle->v3_->twin_->x(),
mid.y() - triangle->v3_->twin_->y(),
mid.z() - triangle->v3_->twin_->z());
diff2.Set(mid.x() - triangle->v1_->twin_->x(),
mid.y() - triangle->v1_->twin_->y(),
mid.z() - triangle->v1_->twin_->z());
error1 = diff1.GetLength() + triangle->v1_->error_;
error2 = diff2.GetLength() + triangle->v2_->error_;
error = (error1 + error2 + current_error) / 3.0f;
if (error < min_error) {
edge.v1_ = triangle->v1_;
edge.v2_ = triangle->v2_;
min_error = error;
min_error1 = error1;
min_error2 = error2;
}
if (min_error == 0.0f && edge.v1_ != 0 && edge.v2_ != 0)
break;
}
edge.v1_->error_ = min_error1;
edge.v2_->error_ = min_error2;
// Now add all triangles to _edge that share the two vertices
// _edge->v1_ and _edge->v2_.
for (tri_iter = triangle_list.begin();
tri_iter != triangle_list.end();
++tri_iter) {
Triangle* triangle = *tri_iter;
if (triangle->HaveVertex(edge.v1_) &&
triangle->HaveVertex(edge.v2_)) {
edge.triangle_list_.push_back(triangle);
}
}
}
void dump_graph_to_png_file(const AdjacencyList &adjacency_list,
const VertexList &vertex_list,
const char *png_filename)
{
auto bbox = get_aabb(vertex_list);
double width = bbox.maxx - bbox.minx;
double height = bbox.maxy - bbox.miny;
auto surface = std::shared_ptr<cairo_surface_t>(
cairo_image_surface_create(CAIRO_FORMAT_RGB24, width, height),
cairo_surface_destroy);
check_cairo_surface_status(surface.get());
auto cr = std::shared_ptr<cairo_t>(
cairo_create(surface.get()),
cairo_destroy);
check_cairo_status(cr.get());
cairo_set_line_width(cr.get(), 1.0);
for (const auto &edge: adjacency_list) {
auto u = vertex_list.find(std::get<0>(edge));
if (u == vertex_list.end()) {
std::cout << "[WARNING] reference to non-existing vertex "
<< std::get<0>(edge) << std::endl;
continue;
}
for (const auto &way: std::get<1>(edge)) {
auto v = vertex_list.find(way.destination);
if (v == vertex_list.end()) {
std::cout << "[WARNING] reference to non-existing vertex "
<< way.destination << std::endl;
continue;
}
switch (way.type) {
case 0:
cairo_set_source_rgb(cr.get(), 1.0, 0.0, 0.0);
break;
case 1:
cairo_set_source_rgb(cr.get(), 0.0, 1.0, 0.0);
break;
case 2:
cairo_set_source_rgb(cr.get(), 0.0, 0.0, 1.0);
break;
default:
std::cout << "[WARNING] Wrong type of arc" << std::endl;
}
cairo_move_to(cr.get(),
u->second.x - bbox.minx,
height - (u->second.y - bbox.miny));
cairo_line_to(cr.get(),
v->second.x - bbox.minx,
height - (v->second.y - bbox.miny));
cairo_stroke(cr.get());
}
}
{
auto status = cairo_surface_write_to_png(surface.get(), png_filename);
cairo_status_to_exception(status);
}
}
void SFUtils::DoTopologicalSort( SLSF::Subsystem subsystem ) {
int vertexIndex = 0;
VertexIndexBlockMap vertexIndexBlockMap;
BlockVertexIndexMap blockVertexIndexMap;
BlockVector blockVector = subsystem.Block_kind_children();
for( BlockVector::iterator blvItr = blockVector.begin(); blvItr != blockVector.end(); ++blvItr, ++vertexIndex ) {
SLSF::Block block = *blvItr;
vertexIndexBlockMap[ vertexIndex ] = block;
blockVertexIndexMap[ block ] = vertexIndex;
std::string blockType = block.BlockType();
if ( blockType == "UnitDelay" ) { // check on other delay blocks as well ...
++vertexIndex; // UnitDelay is vertexed twice - one for outputs (timestep n-1), and one for inputs: vertexIndex is as destination, vertexIndex + 1 is as source
}
}
Graph graph( vertexIndex );
LineSet lineSet = subsystem.Line_kind_children();
for( LineSet::iterator lnsItr = lineSet.begin(); lnsItr != lineSet.end() ; ++lnsItr ) {
SLSF::Line line = *lnsItr;
SLSF::Port sourcePort = line.srcLine_end();
SLSF::Port destinationPort = line.dstLine_end();
SLSF::Block sourceBlock = sourcePort.Block_parent();
SLSF::Block destinationBlock = destinationPort.Block_parent();
if ( sourceBlock == subsystem || destinationBlock == subsystem ) continue;
int sourceBlockVertexIndex = blockVertexIndexMap[ sourceBlock ];
if ( static_cast< std::string >( sourceBlock.BlockType() ) == "UnitDelay" ) {
++sourceBlockVertexIndex;
}
int destinationBlockVertexIndex = blockVertexIndexMap[ destinationBlock ];
boost::add_edge( sourceBlockVertexIndex, destinationBlockVertexIndex, graph );
}
LoopDetector loopDetector( graph );
if ( loopDetector.check() ) {
// TODO: add support for loops involving integrator and other stateful blocks
// Determine what Blocks caused the loop
typedef std::map< Vertex, int > VertexStrongComponentIndexMap;
VertexStrongComponentIndexMap vertexStrongComponentIndexMap;
boost::associative_property_map< VertexStrongComponentIndexMap > apmVertexStrongComponentIndexMap( vertexStrongComponentIndexMap );
strong_components( graph, apmVertexStrongComponentIndexMap );
typedef std::vector< Vertex > VertexVector;
typedef std::map< int, VertexVector > StrongComponentIndexVertexGroupMap;
StrongComponentIndexVertexGroupMap strongComponentIndexVertexGroupMap;
for( VertexStrongComponentIndexMap::iterator vsmItr = vertexStrongComponentIndexMap.begin(); vsmItr != vertexStrongComponentIndexMap.end(); ++vsmItr ) {
strongComponentIndexVertexGroupMap[ vsmItr->second ].push_back( vsmItr->first );
}
std::string error( "Dataflow Graph '" + static_cast< std::string >( subsystem.Name() ) + "' has unhandled loops: " );
for( StrongComponentIndexVertexGroupMap::iterator svmItr = strongComponentIndexVertexGroupMap.begin(); svmItr != strongComponentIndexVertexGroupMap.end(); ++svmItr ) {
VertexVector vertexVector = svmItr->second;
if ( vertexVector.size() <= 1 ) continue;
error.append( "\n" );
for( VertexVector::iterator vtvItr = vertexVector.begin(); vtvItr != vertexVector.end(); ++vtvItr ) {
error.append( blockVector[ *vtvItr ].getPath("/") );
error.append( ", " );
}
error.erase( error.size() - 2 );
}
throw udm_exception(error);
}
typedef std::set< Vertex > VertexSet;
typedef std::map< int, VertexSet > PriorityVertexSetMap;
PriorityVertexSetMap priorityVertexSetMap;
for( BlockVector::iterator blvItr = blockVector.begin() ; blvItr != blockVector.end() ; ++blvItr ) {
SLSF::Block block = *blvItr;
int priority = block.Priority();
if ( priority == 0 ) continue;
Vertex vertex = blockVertexIndexMap[ block ];
priorityVertexSetMap[ priority ].insert( vertex );
}
if ( priorityVertexSetMap.size() > 1 ) {
PriorityVertexSetMap::iterator lstPvmItr = priorityVertexSetMap.end();
--lstPvmItr;
for( PriorityVertexSetMap::iterator pvmItr = priorityVertexSetMap.begin() ; pvmItr != lstPvmItr ; ) {
PriorityVertexSetMap::iterator nxtPvmItr = pvmItr;
++nxtPvmItr;
VertexSet &higherPriorityVertexSet = pvmItr->second;
VertexSet &lowerPriorityVertexSet = nxtPvmItr->second;
for( VertexSet::iterator hvsItr = higherPriorityVertexSet.begin() ; hvsItr != higherPriorityVertexSet.end() ; ++hvsItr ) {
for( VertexSet::iterator lvsItr = lowerPriorityVertexSet.begin() ; lvsItr != lowerPriorityVertexSet.end() ; ++lvsItr ) {
boost::add_edge( *hvsItr, *lvsItr, graph );
LoopDetector loopDetector( graph );
if ( loopDetector.check( *hvsItr ) ) {
SLSF::Block higherPriorityBlock = vertexIndexBlockMap[ *hvsItr ];
SLSF::Block lowerPriorityBlock = vertexIndexBlockMap[ *lvsItr ];
std::cerr << "WARNING: Cannot implement priority difference between block \"" << higherPriorityBlock.getPath( "/" ) << "\" (Priority = " << *hvsItr << ") and " << std::endl;
std::cerr << " block \"" << lowerPriorityBlock.getPath( "/" ) << "\" (Priority = " << *lvsItr << "): contradicts topology of subsystem or other implemented block priority order." << std::endl;
boost::remove_edge( *hvsItr, *lvsItr, graph );
}
}
}
pvmItr = nxtPvmItr;
}
}
VertexList vertexList;
boost::topological_sort( graph, std::back_inserter( vertexList ) );
/* PUT ALL "DataStoreMemory" BLOCKS AT END OF "C" SO THEY HAVE HIGHEST PRIORITY */
VertexList::reverse_iterator vtlRit = vertexList.rbegin();
while( vtlRit != vertexList.rend() ) {
int index = *vtlRit;
SLSF::Block block = vertexIndexBlockMap[ index ];
(void)++vtlRit;
if ( block != Udm::null && static_cast< std::string >( block.BlockType() ) == "DataStoreMemory" ) {
VertexList::reverse_iterator vtlRit2 = vtlRit;
vertexList.splice( vertexList.end(), vertexList, vtlRit2.base() );
}
}
int priority = 0;
for( VertexList::reverse_iterator vtlRit = vertexList.rbegin() ; vtlRit != vertexList.rend() ; ++vtlRit ) {
int index = *vtlRit;
SLSF::Block block = vertexIndexBlockMap[ index ];
if ( block == Udm::null ) { // unit delay as source is not registered - we will invoke it initially, and invoke it as destination in the priority order
// const std::string& bt = blk.BlockType();
// assert(bt.compare("UnitDelay") == 0);
/* Unit Delay Block as destination */
continue;
}
block.Priority() = priority++;
}
}
