void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
{
#ifndef __SPU__
int i;
btVector3 vtx;
btScalar newDot;
for (i=0;i<numVectors;i++)
{
supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
}
for (int j=0;j<numVectors;j++)
{
const btVector3& vec = vectors[j];
for( int k = 0; k < getNumVertices(); k += 128 )
{
btVector3 temp[128];
int inner_count = MIN(getNumVertices() - k, 128);
for( i = 0; i < inner_count; i++ )
getVertex(i,temp[i]);
i = (int) vec.maxDot( temp, inner_count, newDot);
if (newDot > supportVerticesOut[j][3])
{
supportVerticesOut[j] = temp[i];
supportVerticesOut[j][3] = newDot;
}
}
}
#endif //__SPU__
}
void KVFModel::applyVFLogSpiral()
{
TimeMeasurment t;
/* FIXME: hack - the code doesn't work when field is parallel*/
if(!pinnedVertexes.size())
return;
for (unsigned int i = 0; i < getNumVertices(); i++)
{
counts[i] = 0;
newPoints[i] = Point2(0,0);
}
for (unsigned int i = 0; i < faces->size(); i++)
{
for (unsigned int j = 0; j < 3; j++)
{
int e1 = (*faces)[i][j];
int e2 = (*faces)[i][(j+1)%3];
std::complex<double> v1(vf[e1].x,vf[e1].y);
std::complex<double> v2(vf[e2].x,vf[e2].y);
std::complex<double> p1(vertices[e1].x, vertices[e1].y);
std::complex<double> p2(vertices[e2].x, vertices[e2].y);
std::complex<double> z = (v1-v2)/(p1-p2);
std::complex<double> p0 = (p2*v1-p1*v2)/(v1-v2);
double c = z.real();
double alpha = z.imag();
Point2 p(p0.real(),p0.imag());
Point2 l1(vertices[e1][0], vertices[e1][1]);
Point2 l2(vertices[e2][0], vertices[e2][1]);
LogSpiral spiral;
spiral.p0 = p;
spiral.c = c;
spiral.alpha = alpha;
Point2 result1 = spiral.evaluate(l1,1);
Point2 result2 = spiral.evaluate(l2,1);
counts[e1]++;
counts[e2]++;
newPoints[e1] += result1;
newPoints[e2] += result2;
}
}
lastVFApplyTime = t.measure_msec();
printf("KVF: Log spiral time: %f msec\n", lastVFApplyTime);
for (unsigned int i = 0; i < getNumVertices(); i++)
vertices[i] = newPoints[i] / counts[i];
}
void Writer::open(const std::string &filename)
{
MLSGPU_ASSERT(!isOpen(), state_error);
handle = handleFactory();
handle->open(filename);
std::string header = makeHeader();
handle->resize(header.size() + getNumVertices() * vertexSize + getNumTriangles() * triangleSize);
handle->write(header.data(), header.size(), 0);
vertexStart = header.size();
triangleStart = vertexStart + getNumVertices() * vertexSize;
}
bool MeshModel::saveOFF(std::ofstream& ofile) const
{
ofile << "OFF\n";
ofile << getNumVertices() << ' ' << getNumFaces() << " 0\n"; // don't bother counting edges
for (unsigned int i = 0; i < getNumVertices(); i++)
ofile << vertices[i][0] << ' ' << vertices[i][1] << " 0\n";
for (unsigned int i = 0; i < getNumFaces(); i++)
ofile << "3 " << (*faces)[i][0] << ' ' << (*faces)[i][1] << ' ' << (*faces)[i][2] << std::endl;
return true;
}
void Cell::findBdElement(int i, std::vector<MVertex*>& vertices) const
{
vertices.clear();
switch (_dim) {
case 1:
vertices.push_back(_v[i]);
return;
case 2:
switch (getNumVertices()) {
case 3:
for(int j = 0; j < 2; j++)
vertices.push_back(_v[MTriangle::edges_tri(i, j)]);
return;
case 4:
for(int j = 0; j < 2; j++)
vertices.push_back(_v[MQuadrangle::edges_quad(i, j)]);
return;
default: return;
}
case 3:
switch (getNumVertices()) {
case 4:
for(int j = 0; j < 3; j++)
vertices.push_back(_v[MTetrahedron::faces_tetra(i, j)]);
return;
case 5:
if(i < 4)
for(int j = 0; j < 3; j++)
vertices.push_back(_v[MPyramid::faces_pyramid(i, j)]);
else
for(int j = 0; j < 4; j++)
vertices.push_back(_v[MPyramid::faces_pyramid(i, j)]);
return;
case 6:
if(i < 2)
for(int j = 0; j < 3; j++)
vertices.push_back(_v[MPrism::faces_prism(i, j)]);
else
for(int j = 0; j < 4; j++)
vertices.push_back(_v[MPrism::faces_prism(i, j)]);
return;
case 8:
for(int j = 0; j < 4; j++)
vertices.push_back(_v[MHexahedron::faces_hexa(i, j)]);
return;
default: return;
}
default: return;
}
}
template<typename PointT> void
pcl::registration::LUM<PointT>::setCorrespondences (Vertex source_vertex, Vertex target_vertex, pcl::CorrespondencesPtr corrs)
{
if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex)
{
PCL_ERROR("[pcl::registration::LUM::setCorrespondences] You are attempting to set a set of correspondences between non-existing or identical graph vertices.\n");
return;
}
Edge e;
bool present;
boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
if (!present)
boost::tuples::tie (e, present) = add_edge (source_vertex, target_vertex, *slam_graph_);
(*slam_graph_)[e].corrs_ = corrs;
}
void Writer::writeVertices(size_type first, size_type count, const float *data)
{
MLSGPU_ASSERT(isOpen(), state_error);
MLSGPU_ASSERT(first + count <= getNumVertices() && first <= std::numeric_limits<size_type>::max() - count, std::out_of_range);
Statistics::Timer timer(writeVerticesTime);
handle->write(data, count * vertexSize, vertexStart + first * vertexSize);
}
//------------------------------------------------------------------------------
void NiceGraph::getPageRank(map<int,float> &pageRank, int iterations, float damping)
{
int total = getNumVertices();
// set initial page ranks
for (map<int,Vertex*>::iterator vIter = vertexList.begin(); vIter != vertexList.end(); vIter++)
{
pageRank[vIter->first] = 1.0/(float)total;
}
// run arbitrary number of iterations
for (int i = 0; i < iterations; i++)
{
for (map<int,Vertex*>::iterator vIter = vertexList.begin(); vIter != vertexList.end(); vIter++)
{
float votes = 0;
int id = vIter->first;
vector<int> inList = getInNeighborList(id);
for (vector<int>::iterator nIter = inList.begin(); nIter != inList.end(); nIter++)
{
votes+=pageRank[(*nIter)]/(float)getOutDegree(*nIter);
}
pageRank[id] = (1.0-damping) + damping * votes;
}
}
}
int *Mesh::constructSortedVertexIndices(Vec3 sortAxis)
{
sortAxisProjector = sortAxis;
sortAxisMesh = this;
int *result = new int[getNumVertices()];
for (int i = 0; i < getNumVertices(); i++)
{
result[i] = i;
}
qsort(result, getNumVertices(), sizeof(int), projectedComparator);
return result;
}
btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
int i;
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
for (i=0;i<getNumVertices();i++)
{
getVertex(i,vtx);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
void TriangleMeshData::getDataProperty(int property, void **_pvData)
{
if (property == NUM_VERTICES)
{
((int *)*_pvData)[0] = getNumVertices();
}
else if (property == NUM_INDICES)
{
((int *)*_pvData)[0] = getNumIndices();
}
else if (property == COORDINATES)
{
*_pvData = getVertices();
}
else if (property == INDICES)
{
*_pvData = getIndices();
}
else if (property == VALUES)
{
*_pvData = getValues();
}
else
{
Data3D::getDataProperty(property, _pvData);
}
}
void ofxMesh::scale(float x, float y, float z) {
for (int i=0; i<getNumVertices(); i++) {
getVertices()[i].x *= x;
getVertices()[i].y *= y;
getVertices()[i].z *= z;
}
}
Vector Circle::getVertex(int vertexNumber) const
{
float theta = (2 * PI / getNumVertices()) * vertexNumber;
std::auto_ptr<Vector> vertex(new Vector(radius * std::cos(theta) + centre->GetX(), radius * std::sin(theta) + centre->GetY()));
return *vertex;
}
//------------------------------------------------------------------------------
void NiceGraph::sphereLayout(float radius)
{
int num = getNumVertices();
float x,y,z, theta = 0, phi = 0;
float d_theta = 3.14159265 / sqrt (num);
float d_phi = 2.0 * 3.14159265 / sqrt (num);
int theta_total = ceil( sqrt (num));
int phi_total = ceil( sqrt (num));
map<int,Vertex*>::iterator iter = vertexList.begin();
for (int theta_steps = 0; theta_steps < theta_total; theta_steps++)
{
for (int phi_steps = 0; phi_steps < phi_total; phi_steps++)
{
if (iter != vertexList.end())
{
int n = iter->first;
phi += d_phi;
x = radius * sin (theta) * cos (phi);
y = radius * sin (theta) * sin (phi);
z = radius * cos (theta);
setXYZPos(n,x,y,z);
iter++;
}
}
theta += d_theta;
}
}
void MeshModel::moveMesh(Vector2 newCenter)
{
Vector2 move = newCenter - center;
for (unsigned int i = 0; i < getNumVertices(); i++)
vertices[i] += move;
center += move;
}
void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
{
int i;
btVector3 vtx;
btScalar newDot;
for (i=0;i<numVectors;i++)
{
supportVerticesOut[i][3] = btScalar(-1e30);
}
for (int j=0;j<numVectors;j++)
{
const btVector3& vec = vectors[j];
for (i=0;i<getNumVertices();i++)
{
getVertex(i,vtx);
newDot = vec.dot(vtx);
if (newDot > supportVerticesOut[j][3])
{
//WARNING: don't swap next lines, the w component would get overwritten!
supportVerticesOut[j] = vtx;
supportVerticesOut[j][3] = newDot;
}
}
}
}
//------------------------------------------------------------------------------
void NiceGraph::spiralLayout(float radius)
{
float x, y, angle;
int wraps = 2;
float interval = wraps * 2.0 * 3.14159265 / getNumVertices();
float radIncrement = radius / (1.1 * getNumVertices());
float zIncrement = 2.0 * radius / getNumVertices();
for (map<int,Vertex*>::iterator iter = vertexList.begin(); iter != vertexList.end(); iter++)
{
int index = iter->first;
angle = interval * index;
x = index * radIncrement * cos (angle);
y = index * radIncrement * sin (angle);
setXYZPos(index,x,y,-1* radius + index * zIncrement);
}
}
Vertex* SGWalk::getVertex(size_t idx) const
{
assert(idx < getNumVertices());
if(idx == 0)
return m_pStartVertex;
else
return m_edges[idx - 1]->getEnd();
}
bool MFace::computeCorrespondence(const MFace &other, int &rotation, bool &swap) const
{
rotation = 0;
swap = false;
if (*this == other) {
for (int i = 0; i < getNumVertices(); i++) {
if (_v[0] == other.getVertex(i)) {
rotation = i;
break;
}
}
if (_v[1] == other.getVertex((rotation + 1) % getNumVertices())) swap = false;
else swap = true;
return true;
}
return false;
}
template<typename PointT> inline pcl::CorrespondencesPtr
pcl::registration::LUM<PointT>::getCorrespondences (Vertex source_vertex, Vertex target_vertex)
{
if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ())
{
PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences between non-existing graph vertices.\n");
return (pcl::CorrespondencesPtr ());
}
Edge e;
bool present;
boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
if (!present)
{
PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences from a non-existing graph edge.\n");
return (pcl::CorrespondencesPtr ());
}
return ((*slam_graph_)[e].corrs_);
}
void BaseMesh::move(float x, float y, float z)
{
const unsigned nv = getNumVertices();
for(unsigned i = 0; i < nv; i++) {
_vertices[i].x += x;
_vertices[i].y += y;
_vertices[i].y += z;
}
}
const BoundingBox BaseMesh::calculateBBox() const
{
BoundingBox box;
const unsigned nv = getNumVertices();
for(unsigned i = 0; i < nv; i++) {
box.updateMin(_vertices[i]);
box.updateMax(_vertices[i]);
}
return box;
}
void KVFModel::renderVF_common(Vector2* VF) const
{
#define VF_SCALE 1
glLineWidth(1.5);
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
double totalNorm = 0;
for (unsigned int i = 0; i < getNumVertices(); i++)
totalNorm += VF[i].normSquared();
totalNorm = sqrt(totalNorm);
glBegin(GL_LINES);
for (unsigned int i = 0; i < getNumVertices(); i++) {
glVertex2f(vertices[i][0], vertices[i][1]);
glVertex2f(vertices[i][0]+VF[i][0]/totalNorm*VF_SCALE, vertices[i][1]+VF[i][1]/totalNorm*VF_SCALE);
}
glEnd();
}
template<typename PointT> inline void
pcl::registration::LUM<PointT>::setPointCloud (Vertex vertex, PointCloudPtr cloud)
{
if (vertex >= getNumVertices ())
{
PCL_ERROR("[pcl::registration::LUM::setPointCloud] You are attempting to set a point cloud to a non-existing graph vertex.\n");
return;
}
(*slam_graph_)[vertex].cloud_ = cloud;
}
template<typename PointT> inline Eigen::Vector6f
pcl::registration::LUM<PointT>::getPose (Vertex vertex)
{
if (vertex >= getNumVertices ())
{
PCL_ERROR("[pcl::registration::LUM::getPose] You are attempting to get a pose estimate from a non-existing graph vertex.\n");
return (Eigen::Vector6f::Zero ());
}
return ((*slam_graph_)[vertex].pose_);
}
Vector2s RigidBody::computeWorldSpaceEdge( int i ) const
{
assert( i >= 0 );
assert( i < getNumEdges() );
int i0 = i;
int i1 = (i+1)%getNumVertices();
return m_R*(m_vertices.segment<2>(2*i1)-m_vertices.segment<2>(2*i0));
}
void Writer::writeVertices(
Timeplot::Worker &tworker,
size_type first, size_type count,
const boost::shared_ptr<AsyncWriterItem> &data,
AsyncWriter &async)
{
MLSGPU_ASSERT(isOpen(), state_error);
MLSGPU_ASSERT(first + count <= getNumVertices() && first <= std::numeric_limits<size_type>::max() - count, std::out_of_range);
async.push(tworker, data, handle, count * vertexSize, vertexStart + first * vertexSize);
}
template<typename PointT> inline typename pcl::registration::LUM<PointT>::PointCloudPtr
pcl::registration::LUM<PointT>::getPointCloud (Vertex vertex)
{
if (vertex >= getNumVertices ())
{
PCL_ERROR("[pcl::registration::LUM::getPointCloud] You are attempting to get a point cloud from a non-existing graph vertex.\n");
return (PointCloudPtr ());
}
return ((*slam_graph_)[vertex].cloud_);
}
//------------------------------------------------------------------------------
void NiceGraph::getClosenessCentrality(map<int,float> &cCentrality)
{
// this approach based on Floyd-Warshall shortest paths algorithm (iterative!)
// almost the same as avg shortest paths, but only calculates against reachable vertices
// therefore always returns a normal value even if the graph is partially disconnected
unsigned int size = vertexList.size();
unsigned long int distance[size][size];
// initialize all values
for (unsigned int q = 0; q < size; q++)
{
for (unsigned int r = 0; r < size; r++)
{
if (testFromToConnected(q,r))
distance[q][r] = getWeight(q,r);
else if (q == r)
distance[q][r] = 0; // zero distance to self
else
distance[q][r] = INT_MAX; // set distance to infinity if not connected directly
}
}
// now iterate over and over to update shortest paths...
for (unsigned int k = 0; k < size; k++){
for (unsigned int i = 0; i < size; i++){
for (unsigned int j = 0; j < size; j++){
unsigned int sum = distance[i][k] + distance[k][j];
if (sum < distance[i][j]){
distance[i][j] = sum; }
}
}
}
// now compute averages for each guy distance matrix
for (unsigned int m = 0; m < size; m++) {
long double sum = 0;
int reachable = 0;
for (unsigned int n = 0; n < size; n++) {
if (distance[m][n] < size) // ie. if the vertex is reachable
{
sum += distance[m][n]; reachable++;}
}
if (!(reachable == 1)) // ie can only get to itself
sum = sum / (reachable-1);
else sum = 0;
if (sum >=getNumVertices()) // should never happen here
sum = -1;
cCentrality[m] = sum;
}
}
void BaseMesh::putIntoObjectSpace()
{
BoundingBox b = BaseMesh::calculateBBox();
setBBox(b);
const Vector3F c = b.center();
const unsigned nv = getNumVertices();
for(unsigned i = 0; i < nv; i++)
_vertices[i] -= c;
}
