void setupConeVBO(const Vec3f pts[5], const Vec3f &color, std::vector<VBOPosNormalColor> &faces) {
Vec3f normal = computeNormal(pts[0],pts[1],pts[2]);
faces.push_back(VBOPosNormalColor(pts[0],normal,Vec3f(1,1,1)));
faces.push_back(VBOPosNormalColor(pts[1],normal,color));
faces.push_back(VBOPosNormalColor(pts[2],normal,color));
normal = computeNormal(pts[0],pts[2],pts[3]);
faces.push_back(VBOPosNormalColor(pts[0],normal,Vec3f(1,1,1)));
faces.push_back(VBOPosNormalColor(pts[2],normal,color));
faces.push_back(VBOPosNormalColor(pts[3],normal,color));
normal = computeNormal(pts[0],pts[3],pts[4]);
faces.push_back(VBOPosNormalColor(pts[0],normal,Vec3f(1,1,1)));
faces.push_back(VBOPosNormalColor(pts[3],normal,color));
faces.push_back(VBOPosNormalColor(pts[4],normal,color));
normal = computeNormal(pts[0],pts[4],pts[1]);
faces.push_back(VBOPosNormalColor(pts[0],normal,Vec3f(1,1,1)));
faces.push_back(VBOPosNormalColor(pts[4],normal,color));
faces.push_back(VBOPosNormalColor(pts[1],normal,color));
normal = computeNormal(pts[1],pts[3],pts[2]);
faces.push_back(VBOPosNormalColor(pts[1],normal,color));
faces.push_back(VBOPosNormalColor(pts[3],normal,color));
faces.push_back(VBOPosNormalColor(pts[2],normal,color));
faces.push_back(VBOPosNormalColor(pts[1],normal,color));
faces.push_back(VBOPosNormalColor(pts[4],normal,color));
faces.push_back(VBOPosNormalColor(pts[3],normal,color));
}
bool Cylinder::intersects(const Vector3D& v0,const Vector3D& v1,Vector3D* intersection,Vector3D* normal) const
{
if(!intersection) return false;
// mindist between line directed by tDir and line(v0 v1).
Vector3D u = v1 - v0; u.normalize();
if(tDirection == u || tDirection == -u) // colinear
{
float dist = dotProduct(tDirection,v0 - getTransformedPosition());
Vector3D ext = v0 - (tDirection*dist + getTransformedPosition());
float r = ext.getNorm(); ext = ext / r;
if(r == radius) //intersection
{
*intersection = getTransformedPosition() + ext * radius;
if(normal)
*normal = computeNormal(*intersection);
return true;
}
else if(r < radius)
{
*intersection = getTransformedPosition() + tDirection * length*0.5f + ext * r;
if(normal)
*normal = computeNormal(*intersection);
return true;
}
return false;
}
else
{
Vector3D pp = getTransformedPosition() - v0, uv = u;
uv.crossProduct(tDirection);
float dist = std::abs(dotProduct(pp,uv))/uv.getNorm();
float d = dotProduct(tDirection,v0 - getTransformedPosition());
Vector3D ext = v0 - (tDirection*d + getTransformedPosition());
float r = ext.getNorm();
float ah = std::cos(std::asin(dist/r))*r;
Vector3D h = v0 + u*ah;
if(contains(h)) // intersection
{
float offset = 3.1415926535897932384626433832795f*0.5f*dist/radius;
*intersection = h - offset * u;
if(normal)
*normal = computeNormal(*intersection);
return true;
}
return false;
}
}
void Surface_DifferentialProperties_Plugin::computeNormalFromDialog()
{
QList<QListWidgetItem*> currentItems = m_computeNormalDialog->list_maps->selectedItems();
if(!currentItems.empty())
{
const QString& mapName = currentItems[0]->text();
QString positionName = m_computeNormalDialog->combo_positionAttribute->currentText();
QString normalName;
if(m_computeNormalDialog->normalAttributeName->text().isEmpty())
normalName = m_computeNormalDialog->combo_normalAttribute->currentText();
else
normalName = m_computeNormalDialog->normalAttributeName->text();
bool autoUpdate = (currentItems[0]->checkState() == Qt::Checked);
computeNormal(mapName, positionName, normalName, autoUpdate);
// create VBO if asked
if (m_computeNormalDialog->enableVBO->isChecked())
{
MapHandlerGen* mhg = getSCHNApps()->getMap(mapName);
if (mhg != NULL)
mhg->createVBO(normalName);
}
}
}
bool Face::plane_intersect(const Ray &r, Hit &h, bool intersect_backfacing) const {
// insert the explicit equation for the ray into the implicit equation of the plane
// equation for a plane
// ax + by + cz = d;
// normal . p + direction = 0
// plug in ray
// origin + direction * t = p(t)
// origin . normal + t * direction . normal = d;
// t = d - origin.normal / direction.normal;
glm::vec3 normal = computeNormal();
float d = glm::dot(normal,(*this)[0]->get());
float numer = d - glm::dot(r.getOrigin(),normal);
float denom = glm::dot(r.getDirection(),normal);
if (denom == 0) return 0; // parallel to plane
if (!intersect_backfacing && glm::dot(normal,r.getDirection()) >= 0)
return 0; // hit the backside
float t = numer / denom;
if (t > EPSILON && t < h.getT()) {
h.set(t,this->getMaterial(),normal);
assert (h.getT() >= EPSILON);
return 1;
}
return 0;
}
Vector3d getNormal() {
if (HaveNormal_) {
return normal_;
} else {
return computeNormal();
}
}
//---------------------------------------------------------------------
void ProgressiveMesh::PMTriangle::replaceVertex(
ProgressiveMesh::PMFaceVertex *vold, ProgressiveMesh::PMFaceVertex *vnew)
{
assert(vold && vnew);
assert(vold==vertex[0] || vold==vertex[1] || vold==vertex[2]);
assert(vnew!=vertex[0] && vnew!=vertex[1] && vnew!=vertex[2]);
if(vold==vertex[0]){
vertex[0]=vnew;
}
else if(vold==vertex[1]){
vertex[1]=vnew;
}
else {
assert(vold==vertex[2]);
vertex[2]=vnew;
}
int i;
vold->commonVertex->face.erase(this);
vnew->commonVertex->face.insert(this);
for(i=0;i<3;i++) {
vold->commonVertex->removeIfNonNeighbor(vertex[i]->commonVertex);
vertex[i]->commonVertex->removeIfNonNeighbor(vold->commonVertex);
}
for(i=0;i<3;i++) {
assert(vertex[i]->commonVertex->face.find(this) != vertex[i]->commonVertex->face.end());
for(int j=0;j<3;j++) if(i!=j) {
#if OGRE_DEBUG_MODE
ofdebug << "Adding vertex " << (unsigned int)vertex[j]->commonVertex->index << " to the neighbor list "
"of vertex " << (unsigned int)vertex[i]->commonVertex->index << std::endl;
#endif
vertex[i]->commonVertex->neighbor.insert(vertex[j]->commonVertex);
}
}
computeNormal();
}
//===================================================================
void AnnulusMesh::makeMesh()
{
// calculates the vertices of the mesh
PolygonMesh polOu(mRou, mSeg, this);
PolygonMesh polIn(mRin, mSeg, this);
float angStep = qDegreesToRadians(360. / mSeg);
float ang = 0.0;
int index = 0;
for (int count = 0; count < polOu.verticesCount() / 3; count++) {
addVertex(polOu.vertices().at(index));
addVertex(polOu.vertices().at(index+1));
polIn.vertex(index).setU(0.5 + mRin/mRou * 0.5 * qCos(ang));
polIn.vertex(index).setV(0.5 + mRin/mRou * 0.5 * qSin(ang));
addVertex(polIn.vertices().at(index));
addVertex(polOu.vertices().at(index+1));
polIn.vertex(index+1).setU(0.5 + mRin/mRou * 0.5 * qCos(ang + angStep));
polIn.vertex(index+1).setV(0.5 + mRin/mRou * 0.5 * qSin(ang + angStep));
addVertex(polIn.vertices().at(index+1));
addVertex(polIn.vertices().at(index));
index +=3;
ang += angStep;
}
computeNormal();
}
bool Face::plane_intersect(const Ray &r, Hit &h, bool intersect_backfacing, bool* backfacing_hit) {
// insert the explicit equation for the ray into the implicit equation of the plane
// equation for a plane
// ax + by + cz = d;
// normal . p + direction = 0
// plug in ray
// origin + direction * t = p(t)
// origin . normal + t * direction . normal = d;
// t = d - origin.normal / direction.normal;
Vec3f normal = computeNormal();
double d = normal.Dot3((*this)[0]->get());
double numer = d - r.getOrigin().Dot3(normal);
double denom = r.getDirection().Dot3(normal);
if (denom == 0) return 0; // parallel to plane
if (!intersect_backfacing && normal.Dot3(r.getDirection()) >= 0)
return 0; // hit the backside
double t = numer / denom;
if (t > EPSILON && t < h.getT()) {
h.set(t,this->getMaterial(),normal,this);
assert (h.getT() >= EPSILON);
//hit the backside but that's okay in this case
if (normal.Dot3(r.getDirection()) >= 0){
*backfacing_hit = true;
}
return 1;
}
return 0;
}
TerrainModel::TerrainModel(string heightMap)
{
vector<VertexData> vertices;
vector<unsigned int> indices;
VertexData v;
SDL_Surface* image = utl::loadRawImage(heightMap);
int vertexCount = image->h;
m_heights.resize(vertexCount, vector<float>(vertexCount, 0));
m_gridSquareSize = DEFAULT_SIZE / ((float)vertexCount - 1);
m_sideVertexCount = vertexCount;
int maxGray, minGray;
getMinMax(image, &maxGray, &minGray);
for (int z = 0; z < vertexCount; z++)
{
for (int x = 0; x < vertexCount; x++)
{
VertexData v;
v.m_position.x = (float)x / ((float)vertexCount - 1) * DEFAULT_SIZE;
float height = getHeight(x, z, image, maxGray, minGray);
m_heights[z][x] = height;
v.m_position.y = height;
v.m_position.z = (float)z / ((float)vertexCount - 1) * DEFAULT_SIZE;
// http://stackoverflow.com/questions/13983189/opengl-how-to-calculate-normals-in-a-terrain-height-grid
v.m_normal = computeNormal(x, z, image, maxGray, minGray);
v.m_UV.x = (float)x / ((float)vertexCount - 1);
v.m_UV.y = (float)z / ((float)vertexCount - 1);
vertices.push_back(v);
}
}
for (int gz = 0; gz < vertexCount - 1; gz++)
{
for (int gx = 0; gx < vertexCount - 1; gx++)
{
int topLeft = (gz * vertexCount) + gx;
int topRight = topLeft + 1;
int bottomLeft = ((gz + 1) * vertexCount) + gx;
int bottomRight = bottomLeft + 1;
indices.push_back(topLeft);
indices.push_back(bottomLeft);
indices.push_back(topRight);
indices.push_back(topRight);
indices.push_back(bottomLeft);
indices.push_back(bottomRight);
}
}
Mesh m(vertices, indices);
m_meshes.push_back(m);
}
//this grabs the normal vector at point x z
QVector3D terrain::getNormal(int x, int z)
{
if(!updateNormal)
{
computeNormal();
}
return normal[z][x];
}
// given a point, compute the tangent to the
// point at the orthogonal projection intersection point
vrpn_HapticPlane TexturePlane::computeTangentPlaneAt(vrpn_HapticPosition pnt) const
{
// compute normal at pnt[0],pnt[2]
vrpn_HapticVector pNormal = computeNormal(pnt[0],pnt[2]);
// assume pnt is in the tangent plane
vrpn_HapticPosition proj = projectPointOrthogonalToStaticPlane(pnt);
vrpn_HapticPlane tangPlane = vrpn_HapticPlane(pNormal, proj);
return tangPlane;
}
void Shape::updateOutline()
{
std::size_t count = m_vertices.getVertexCount() - 2;
m_outlineVertices.resize((count + 1) * 2);
for (std::size_t i = 0; i < count; ++i)
{
std::size_t index = i + 1;
// Get the two segments shared by the current point
Vector2f p0 = (i == 0) ? m_vertices[count].position : m_vertices[index - 1].position;
Vector2f p1 = m_vertices[index].position;
Vector2f p2 = m_vertices[index + 1].position;
// Compute their normal
Vector2f n1 = computeNormal(p0, p1);
Vector2f n2 = computeNormal(p1, p2);
// Make sure that the normals point towards the outside of the shape
// (this depends on the order in which the points were defined)
if (dotProduct(n1, m_vertices[0].position - p1) > 0)
n1 = -n1;
if (dotProduct(n2, m_vertices[0].position - p1) > 0)
n2 = -n2;
// Combine them to get the extrusion direction
float factor = 1.f + (n1.x * n2.x + n1.y * n2.y);
Vector2f normal = (n1 + n2) / factor;
// Update the outline points
m_outlineVertices[i * 2 + 0].position = p1;
m_outlineVertices[i * 2 + 1].position = p1 + normal * m_outlineThickness;
}
// Duplicate the first point at the end, to close the outline
m_outlineVertices[count * 2 + 0].position = m_outlineVertices[0].position;
m_outlineVertices[count * 2 + 1].position = m_outlineVertices[1].position;
// Update outline colors
updateOutlineColors();
// Update the shape's bounds
m_bounds = m_outlineVertices.getBounds();
}
void Polygon::finish( AppearanceManager& appearanceManager, bool doTesselate )
{
TVec3d normal = computeNormal();
if ( doTesselate ) tesselate( appearanceManager, normal ); else mergeRings( appearanceManager );
// Create the normal per point field
_normals.resize( _vertices.size() );
for ( unsigned int i = 0; i < _vertices.size(); i++ )
_normals[i] = TVec3f( (float)normal.x, (float)normal.y, (float)normal.z );
}
void
_SoNurbsPickV4SurfaceMap::point( float *v )
//
////////////////////////////////////////////////////////////////////////
{
//
// Store the incoming point and calculate the normal at that point
//
P.p[0] = v[0];
P.p[1] = v[1];
P.p[2] = v[2];
P.p[3] = v[3];
computeFirstPartials ();
computeNormal ();
if (!cacheReady) {
// The cache of mesh vertices is still filling, so a triangle
// isn't ready to be intersected yet. Just fill this point
// into the cache and update the cache vertices.
SP[cacheIndices[curCacheIndex]].p[0] = P.p[0]/P.p[3];
SP[cacheIndices[curCacheIndex]].p[1] = P.p[1]/P.p[3];
SP[cacheIndices[curCacheIndex]].p[2] = P.p[2]/P.p[3];
SP[cacheIndices[curCacheIndex]].norm[0] = P.norm[0];
SP[cacheIndices[curCacheIndex]].norm[1] = P.norm[1];
SP[cacheIndices[curCacheIndex]].norm[2] = P.norm[2];
TP[cacheIndices[curCacheIndex]][0] = tmpTexPt[0];
TP[cacheIndices[curCacheIndex]][1] = tmpTexPt[1];
if (curCacheIndex == 1) {
cacheReady = 1;
}
curCacheIndex = 1 - curCacheIndex;
return;
}
// The cache is full now, so triangles are now forming with each
// vertex. Take the two cached vertices and the current vertex
// to form a triangle and intersect it.
SP[curPrimIndex].p[0] = P.p[0]/P.p[3];
SP[curPrimIndex].p[1] = P.p[1]/P.p[3];
SP[curPrimIndex].p[2] = P.p[2]/P.p[3];
SP[curPrimIndex].norm[0] = P.norm[0];
SP[curPrimIndex].norm[1] = P.norm[1];
SP[curPrimIndex].norm[2] = P.norm[2];
TP[curPrimIndex][0] = tmpTexPt[0];
TP[curPrimIndex][1] = tmpTexPt[1];
intersectTriangle();
// Update the cache indices
int tmp = curPrimIndex;
curPrimIndex = cacheIndices[curCacheIndex];
cacheIndices[curCacheIndex] = tmp;
curCacheIndex = 1 - curCacheIndex;
}
void MarchingCubes::drawIfBoundary(const glm::vec3 &p1, const glm::vec3 &p2, const glm::vec3 &p3) {
if ((p1.x <= 0.1*dx && p2.x <= 0.1*dx && p3.x <= 0.1*dx) ||
(p1.y <= 0.1*dy && p2.y <= 0.1*dy && p3.y <= 0.1*dy) ||
(p1.z <= 0.1*dz && p2.z <= 0.1*dz && p3.z <= 0.1*dz) ||
(p1.x >= dx*(nx-1.1) && p2.x >= dx*(nx-1.1) && p3.x >= dx*(nx-1.1)) ||
(p1.y >= dy*(ny-1.1) && p2.y >= dy*(ny-1.1) && p3.y >= dy*(ny-1.1)) ||
(p1.z >= dz*(nz-1.1) && p2.z >= dz*(nz-1.1) && p3.z >= dz*(nz-1.1))) {
glm::vec3 normal = computeNormal(p1,p2,p3);
drawTriangleWithNormals(normal,p1,normal,p2,normal,p3);
}
}
void MarchingCubes::drawIfBoundary(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3) {
if ((p1.x() <= 0.1*dx && p2.x() <= 0.1*dx && p3.x() <= 0.1*dx) ||
(p1.y() <= 0.1*dy && p2.y() <= 0.1*dy && p3.y() <= 0.1*dy) ||
(p1.z() <= 0.1*dz && p2.z() <= 0.1*dz && p3.z() <= 0.1*dz) ||
(p1.x() >= dx*(nx-1.1) && p2.x() >= dx*(nx-1.1) && p3.x() >= dx*(nx-1.1)) ||
(p1.y() >= dy*(ny-1.1) && p2.y() >= dy*(ny-1.1) && p3.y() >= dy*(ny-1.1)) ||
(p1.z() >= dz*(nz-1.1) && p2.z() >= dz*(nz-1.1) && p3.z() >= dz*(nz-1.1))) {
Vec3f normal = computeNormal(p1,p2,p3);
drawTriangleWithNormals(normal,p1,normal,p2,normal,p3);
}
}
TRIANGLE::TRIANGLE(VECTOR3 v1, VECTOR3 v2, VECTOR3 v3, VECTOR3 norm, COLOR col)
{
vertex[0] = v1;
vertex[1] = v2;
vertex[2] = v3;
if(norm.isNull())
computeNormal();
else
normal = norm;
color = col;
}
void Mesh::DrawWireFrame(const VBO& vbos, int line_width)
{
computeNormal();
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glLineWidth(line_width);
unsigned int size = m_vertices_number;
unsigned int element_num = m_triangle_list.size();
// position and color
std::vector<glm::vec3> colors; colors.clear();
glBindBuffer(GL_ARRAY_BUFFER, vbos.m_vbo);
for (unsigned int i = 0; i < size; ++i)
{
m_positions[i] = glm::vec3(m_current_positions[3*i+0], m_current_positions[3*i+1], m_current_positions[3*i+2]);
colors.push_back(glm::vec3(0,0,0));
}
glBufferData(GL_ARRAY_BUFFER, 3 * size * sizeof(float), &m_positions[0], GL_DYNAMIC_DRAW);
// color
glBindBuffer(GL_ARRAY_BUFFER, vbos.m_cbo);
glBufferData(GL_ARRAY_BUFFER, 3 * size * sizeof(float), &colors[0], GL_STATIC_DRAW);
// indices
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vbos.m_ibo);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, element_num * sizeof(unsigned int), &m_triangle_list[0], GL_STATIC_DRAW);
glEnableVertexAttribArray(0);
glEnableVertexAttribArray(1);
glBindBuffer(GL_ARRAY_BUFFER, vbos.m_vbo);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);
glBindBuffer(GL_ARRAY_BUFFER, vbos.m_cbo);
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, 0);
glm::mat4 identity = glm::mat4(); // identity matrix
glUniformMatrix4fv(vbos.m_uniform_transformation, 1, false, &identity[0][0]);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vbos.m_ibo);
glDrawElements(GL_TRIANGLES, element_num, GL_UNSIGNED_INT, 0);
glDisableVertexAttribArray(0);
glDisableVertexAttribArray(1);
glBindBuffer(GL_ARRAY_BUFFER, 0);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}
void Shape::updateOutline()
{
unsigned int count = m_vertices.getVertexCount() - 2;
m_outlineVertices.resize((count + 1) * 2);
for (unsigned int i = 0; i < count; ++i)
{
unsigned int index = i + 1;
// Get the two segments shared by the current point
Vector2f p0 = (i == 0) ? m_vertices[count].position : m_vertices[index - 1].position;
Vector2f p1 = m_vertices[index].position;
Vector2f p2 = m_vertices[index + 1].position;
// Compute their normal
Vector2f n1 = computeNormal(p0, p1);
Vector2f n2 = computeNormal(p1, p2);
// Combine them to get the extrusion direction
float factor = 1.f + (n1.x * n2.x + n1.y * n2.y);
Vector2f normal = -(n1 + n2) / factor;
// Update the outline points
m_outlineVertices[i * 2 + 0].position = p1;
m_outlineVertices[i * 2 + 1].position = p1 + normal * m_outlineThickness;
}
// Duplicate the first point at the end, to close the outline
m_outlineVertices[count * 2 + 0].position = m_outlineVertices[0].position;
m_outlineVertices[count * 2 + 1].position = m_outlineVertices[1].position;
// Update outline colors
updateOutlineColors();
// Update the shape's bounds
m_bounds = m_outlineVertices.getBounds();
}
cv::Mat3b OrganizedSurfaceNormalNode::getSurfNorm(const KinectCloud& pcd)
{
cv::Mat3b vis(pcd.height, pcd.width, cv::Vec3b(0, 0, 0));
normals_->resize(pcd.size());
for(size_t y = 0; y < pcd.height; ++y) {
for(size_t x = 0; x < pcd.width; ++x) {
int idx = y * pcd.width + x;
cv::Point2i img_pt(x, y);
if(pcd.at(idx).x != pcd.at(idx).x) { // this will only occur if it's 'nan'
continue;
}
computeNormal(pcd, pcd.at(idx), img_pt, &normals_->at(idx));
normalToColor(normals_->at(idx), &vis(y, x));
}
}
normals_otl_.push(normals_);
return vis;
}
Triangle::Triangle(Vertex* v0, Vertex* v1, Vertex* v2, int id, Triangle* store):
id(id)
{
v[0]=v0;
v[1]=v1;
v[2]=v2;
computeNormal();
for(int i = 0; i < 3; i++){
if(v[0]->position[i]<v[1]->position[i]){
min[i] = v[0]->position[i];
max[i] = v[1]->position[i];
}else{
min[i] = v[1]->position[i];
max[i] = v[0]->position[i];
}
if(min[i] > v[2]->position[i]) min[i] = v[2]->position[i];
if(max[i] < v[2]->position[i]) max[i] = v[2]->position[i];
}
if(store){
for(int i = 0; i < 3; i++){
neighbours[i] = NULL;
for(list<int>::iterator iter = v[i]->faces.begin(); iter != v[i]->faces.end(); iter++ ){
Triangle &t = store[*iter];
int v_next_id = v[(i+1)%3]->id;
//assert(t.id != id); // don't insert your self
for(int j = 0; j < 3; j++){
if(v_next_id == t.v[j]->id){
//assert(neighbours[i] == NULL);
neighbours[i] = &store[t.id];
//assert(t.neighbours[j] == NULL); // slot is empty
t.neighbours[j] = &store[id];
}
}
}
v[i]->faceNormals.push_back(faceNormal);
v[i]->faces.push_back(id);
}
}
}
void OrganizedSurfaceNormalNode::_compute()
{
KinectCloud::ConstPtr pcd = pcd_otl_->pull();
ROS_ASSERT(normals_->empty());
normals_->resize(pcd->size());
cv::Mat1b mask = mask_otl_->pull();
for(size_t y = 0; y < pcd->height; ++y) {
for(size_t x = 0; x < pcd->width; ++x) {
if(mask(y, x) != 255)
continue;
int idx = y * pcd->width + x;
cv::Point2i img_pt(x, y);
computeNormal(*pcd, pcd->at(idx), img_pt, &normals_->at(idx));
}
}
normals_otl_.push(normals_);
}
/**
* estimateNormals
*/
void ZAdaptiveNormals::estimateNormals(const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, const std::vector<int> &normals_indices, std::vector<Eigen::Vector3f> &normals)
{
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
std::vector< int > indices;
#pragma omp parallel for private(eigen_vectors,indices)
for (unsigned i=0; i<normals_indices.size(); i++) {
indices.clear();
int idx = normals_indices[i];
int u = idx%width;
int v = idx/width;
const Eigen::Vector3f &pt = cloud.data[idx];
Eigen::Vector3f &n = normals[i];
if(!std::isnan(pt[0]) && !std::isnan(pt[1]) && !std::isnan(pt[2])) {
if(param.adaptive) {
int dist = (int) (pt[2]*2); // *2 => every 0.5 meter another kernel radius
getIndices(cloud, u,v, param.kernel_radius[dist], indices);
}
else
getIndices(cloud, u,v, param.kernel, indices);
}
if (indices.size()<4) {
n[0] = NaN;
continue;
}
/* curvature= */ computeNormal(cloud, indices, eigen_vectors);
n[0] = eigen_vectors (0,0);
n[1] = eigen_vectors (1,0);
n[2] = eigen_vectors (2,0);
if (n.dot(pt) > 0) {
n *= -1;
//n.getNormalVector4fMap()[3] = 0;
//n.getNormalVector4fMap()[3] = -1 * n.getNormalVector4fMap().dot(pt.getVector4fMap());
}
}
}
/*
IMPORTANT: This function assumes that the plane equation is
y = 0 for simplicity of computation
the position is expected to be in a coordinate system such that
the plane has this equation.
Therefore, to simulate other planes, the position in world coordinates
must be transformed correctly
*/
vrpn_HapticPosition TexturePlane::computeSCPfromGradient(vrpn_HapticPosition currentPos) const
{
double pos_x, pos_y, pos_z, pos_r; // position in local coordinates
double scp_h; // scp in local coordinates
// first, translate position into texture coordinates
pos_x = (currentPos[0]);
pos_y = currentPos[1];
pos_z = (currentPos[2]);
pos_r = sqrt(pos_x*pos_x + pos_z*pos_z);
// the first time we contact
if (!inContact){
return currentPos;
}
vrpn_HapticPosition newSCP;
// get height of surface at contact point
scp_h = computeHeight(pos_r);
// return current position if we are not touching the surface
if (scp_h < pos_y)
newSCP = currentPos;
else {
vrpn_HapticVector normal = computeNormal(pos_x, pos_z);
normal.normalize();
double delta_y = scp_h - pos_y;
// compute scp in untranslated coordinates
newSCP[0] = pos_x + normal[0]*(delta_y/normal[1]);
newSCP[1] = scp_h;
newSCP[2] = pos_z + normal[2]*(delta_y/normal[1]);
}
return newSCP;
}
void OrganizedSurfaceNormalNode::computeNormal(const KinectCloud& pcd,
const pcl::PointXYZRGB& pt,
const cv::Point2i& img_pt,
pcl::Normal* normal)
{
indices_.clear();
inliers_.clear();
ImageRegionIterator it(cv::Size(pcd.width, pcd.height), radius_);
for(it.setCenter(img_pt); !it.done(); ++it) {
int idx = it.index();
// if(idx % 2 != 0)
// continue;
if(isnan(pcd[idx].z))
continue;
indices_.push_back(it.index());
}
computeNormal(pcd, pt, indices_, normal);
}
//---------------------------------------------------------------------
void ProgressiveMesh::PMTriangle::setDetails(size_t newindex,
ProgressiveMesh::PMFaceVertex *v0, ProgressiveMesh::PMFaceVertex *v1,
ProgressiveMesh::PMFaceVertex *v2)
{
assert(v0!=v1 && v1!=v2 && v2!=v0);
index = newindex;
vertex[0]=v0;
vertex[1]=v1;
vertex[2]=v2;
computeNormal();
// Add tri to vertices
// Also tell vertices they are neighbours
for(int i=0;i<3;i++) {
vertex[i]->commonVertex->face.insert(this);
for(int j=0;j<3;j++) if(i!=j) {
vertex[i]->commonVertex->neighbor.insert(vertex[j]->commonVertex);
}
}
}
bool NormalMap::load(float scale)
{
int actual_components = 0;
int requested_components = 1;
components = requested_components;
image = stbi_load(filename, &width, &height, &actual_components, requested_components);
if (image) {
assert(width > 0);
assert(height > 0);
assert(actual_components > 0);
if (actual_components != requested_components) {
if (verbose) {
printf("warning: %d component normal map treated as gray scale height map\n",
actual_components);
}
}
normal_image = new GLubyte[width*height*3];
assert(normal_image);
GLubyte* p = normal_image;
for (int y=0; y<height; y++) {
for (int x=0; x<width; x++) {
float3 normal = computeNormal(x,y, scale);
PackedNormal packed_normal(normal);
p[0] = packed_normal.n[0];
p[1] = packed_normal.n[1];
p[2] = packed_normal.n[2];
p += 3;
}
}
assert(p == normal_image+(width*height*3));
// success
return true;
} else {
printf("%s: failed to load image %s\n", program_name, filename);
return false;
}
}
void Surface_DifferentialProperties_Plugin::attributeModified(unsigned int orbit, QString nameAttr)
{
if(orbit == VERTEX)
{
MapHandlerGen* map = static_cast<MapHandlerGen*>(QObject::sender());
if(computeNormalLastParameters.contains(map->getName()))
{
ComputeNormalParameters& params = computeNormalLastParameters[map->getName()];
if(params.autoUpdate && params.positionName == nameAttr)
computeNormal(map->getName(), params.positionName, params.normalName, true);
}
if(computeCurvatureLastParameters.contains(map->getName()))
{
ComputeCurvatureParameters& params = computeCurvatureLastParameters[map->getName()];
if(params.autoUpdate && (params.positionName == nameAttr || params.normalName == nameAttr))
computeCurvature(
map->getName(),
params.positionName, params.normalName,
params.KmaxName, params.kmaxName, params.KminName, params.kminName, params.KnormalName,
true
);
}
}
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
pcl::fromROSMsg (*cloud, cloud_xyzrgb_);
t0 = my_clock();
if( limitPoint( cloud_xyzrgb_, cloud_xyzrgb, distance_th ) > 10 ){
std::cout << "compute normals and voxelize...." << std::endl;
#ifdef CCHLAC_TEST
getVoxelGrid( grid, cloud_xyzrgb, cloud_downsampled, voxel_size );
#else
//****************************************
//* compute normals
computeNormal( cloud_xyzrgb, cloud_normal );
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_normal, cloud_downsampled, voxel_size );
#endif
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
#ifdef CCHLAC_TEST
search_obj.setC3HLAC( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_downsampled, voxel_size, box_size );
#else
search_obj.setVOSCH( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_normal, cloud_downsampled, voxel_size, box_size );
#endif
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
#ifdef CCHLAC_TEST
search_obj.search();
#else
search_obj.searchWithoutRotation();
#endif
search_obj.removeOverlap();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
#ifdef CCHLAC_TEST
std::cout << "voxelize :"<< t1 - t0 << " sec" << std::endl;
#else
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
#endif
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
#if 0
//* show the limited space
marker_pub_.publish( setMarker( -1, (x_max+x_min)/2, (y_max+y_min)/2, (z_max+z_min)/2, x_max-x_min, y_max-y_min, z_max-z_min, 0.1, 1.0, 0.0, 0.0 ) );
#endif
for( int m=0; m<model_num; m++ ){
for( int q=0; q<rank_num; q++ ){
//if( search_obj.maxDot( m, q ) < detect_th ) break;
std::cout << search_obj.maxX( m, q ) << " " << search_obj.maxY( m, q ) << " " << search_obj.maxZ( m, q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( m, q ) << std::endl;
//if( (search_obj.maxX( m, q )!=0)||(search_obj.maxY( m, q )!=0)||(search_obj.maxZ( m, q )!=0) ){
//* publish markers for detected regions
if( search_obj.maxDot( m, q ) < detect_th )
marker_array_msg_.markers.push_back( setMarker( m, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) );
else{
marker_array_msg_.markers.push_back( setMarker( m, search_obj.maxX( m, q ) * region_size + sliding_box_size_x/2 + x_min, search_obj.maxY( m, q ) * region_size + sliding_box_size_y/2 + y_min, search_obj.maxZ( m, q ) * region_size + sliding_box_size_z/2 + z_min, sliding_box_size_x, sliding_box_size_y, sliding_box_size_z, 0.5, marker_color_r[ m ], marker_color_g[ m ], marker_color_b[ m ] ) );
}
}
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
glm::vec3 rayTrace(Ray &ray, const float& t, RayTracerState& state) {
glm::vec3 normal = computeNormal(ray, t);
return effect->rayTrace(ray, t, normal, state);
}
