You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
611 lines
16 KiB
C++
611 lines
16 KiB
C++
/**
|
|
*
|
|
* Copyright (C) Jacobs University Bremen
|
|
*
|
|
* @author Vaibhav Kumar Mehta
|
|
* @author Corneliu Claudiu Prodescu
|
|
* @file normals.cc
|
|
*/
|
|
|
|
#include <vector>
|
|
#include <ANN/ANN.h>
|
|
#include "slam6d/io_types.h"
|
|
#include "slam6d/globals.icc"
|
|
#include "slam6d/scan.h"
|
|
#include "slam6d/fbr/panorama.h"
|
|
#include "slam6d/kd.h"
|
|
#include <scanserver/clientInterface.h>
|
|
#include "newmat/newmat.h"
|
|
#include "newmat/newmatap.h"
|
|
|
|
#include "normals/normals.h"
|
|
|
|
using namespace NEWMAT;
|
|
using namespace std;
|
|
|
|
//////////////////////////////////////////////////////
|
|
/////////////NORMALS USING AKNN METHOD ////////////////
|
|
///////////////////////////////////////////////////////
|
|
void calculateNormalsApxKNN(vector<Point> &normals,
|
|
vector<Point> &points,
|
|
int k,
|
|
const double _rPos[3],
|
|
double eps)
|
|
{
|
|
cout<<"Total number of points: "<<points.size()<<endl;
|
|
int nr_neighbors = k;
|
|
|
|
ColumnVector rPos(3);
|
|
for (int i = 0; i < 3; ++i)
|
|
rPos(i+1) = _rPos[i];
|
|
|
|
ANNpointArray pa = annAllocPts(points.size(), 3);
|
|
for (size_t i=0; i<points.size(); ++i)
|
|
{
|
|
pa[i][0] = points[i].x;
|
|
pa[i][1] = points[i].y;
|
|
pa[i][2] = points[i].z;
|
|
|
|
}
|
|
ANNkd_tree t(pa, points.size(), 3);
|
|
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
|
ANNdistArray d = new ANNdist[nr_neighbors];
|
|
|
|
for (size_t i=0; i<points.size(); ++i)
|
|
{
|
|
ANNpoint p = pa[i];
|
|
//ANN search for k nearest neighbors
|
|
//indexes of the neighbors along with the query point
|
|
//stored in the array n
|
|
t.annkSearch(p, nr_neighbors, nidx, d, eps);
|
|
Point mean(0.0,0.0,0.0);
|
|
Matrix X(nr_neighbors,3);
|
|
SymmetricMatrix A(3);
|
|
Matrix U(3,3);
|
|
DiagonalMatrix D(3);
|
|
//calculate mean for all the points
|
|
for (int j=0; j<nr_neighbors; ++j)
|
|
{
|
|
mean.x += points[nidx[j]].x;
|
|
mean.y += points[nidx[j]].y;
|
|
mean.z += points[nidx[j]].z;
|
|
}
|
|
mean.x /= nr_neighbors;
|
|
mean.y /= nr_neighbors;
|
|
mean.z /= nr_neighbors;
|
|
//calculate covariance = A for all the points
|
|
for (int i = 0; i < nr_neighbors; ++i) {
|
|
X(i+1, 1) = points[nidx[i]].x - mean.x;
|
|
X(i+1, 2) = points[nidx[i]].y - mean.y;
|
|
X(i+1, 3) = points[nidx[i]].z - mean.z;
|
|
}
|
|
|
|
A << 1.0/nr_neighbors * X.t() * X;
|
|
|
|
EigenValues(A, D, U);
|
|
|
|
//normal = eigenvector corresponding to lowest
|
|
//eigen value that is the 1st column of matrix U
|
|
ColumnVector n(3);
|
|
n(1) = U(1,1);
|
|
n(2) = U(2,1);
|
|
n(3) = U(3,1);
|
|
ColumnVector point_vector(3);
|
|
point_vector(1) = p[0] - rPos(1);
|
|
point_vector(2) = p[1] - rPos(2);
|
|
point_vector(3) = p[2] - rPos(3);
|
|
point_vector = point_vector / point_vector.NormFrobenius();
|
|
Real angle = (n.t() * point_vector).AsScalar();
|
|
if (angle < 0) {
|
|
n *= -1.0;
|
|
}
|
|
n = n / n.NormFrobenius();
|
|
normals.push_back(Point(n(1), n(2), n(3)));
|
|
}
|
|
|
|
delete[] nidx;
|
|
delete[] d;
|
|
annDeallocPts(pa);
|
|
}
|
|
////////////////////////////////////////////////////////////////
|
|
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
|
|
////////////////////////////////////////////////////////////////
|
|
void calculateNormalsAdaptiveApxKNN(vector<Point> &normals,
|
|
vector<Point> &points,
|
|
int kmin,
|
|
int kmax,
|
|
const double _rPos[3],
|
|
double eps)
|
|
{
|
|
ColumnVector rPos(3);
|
|
for (int i = 0; i < 3; ++i)
|
|
rPos(i+1) = _rPos[i];
|
|
|
|
cout<<"Total number of points: "<<points.size()<<endl;
|
|
int nr_neighbors;
|
|
ANNpointArray pa = annAllocPts(points.size(), 3);
|
|
for (size_t i=0; i<points.size(); ++i)
|
|
|
|
{
|
|
pa[i][0] = points[i].x;
|
|
pa[i][1] = points[i].y;
|
|
pa[i][2] = points[i].z;
|
|
|
|
}
|
|
ANNkd_tree t(pa, points.size(), 3);
|
|
|
|
Point mean(0.0,0.0,0.0);
|
|
double e1,e2,e3;
|
|
|
|
for (size_t i=0; i<points.size(); ++i)
|
|
{
|
|
Matrix U(3,3);
|
|
ANNpoint p = pa[i];
|
|
for(int kidx = kmin; kidx < kmax; kidx++)
|
|
{
|
|
nr_neighbors=kidx+1;
|
|
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
|
ANNdistArray d = new ANNdist[nr_neighbors];
|
|
//ANN search for k nearest neighbors
|
|
//indexes of the neighbors along with the query point
|
|
//stored in the array n
|
|
t.annkSearch(p, nr_neighbors, nidx, d, eps);
|
|
mean.x=0,mean.y=0,mean.z=0;
|
|
//calculate mean for all the points
|
|
for (int j=0; j<nr_neighbors; ++j)
|
|
{
|
|
mean.x += points[nidx[j]].x;
|
|
mean.y += points[nidx[j]].y;
|
|
mean.z += points[nidx[j]].z;
|
|
}
|
|
mean.x /= nr_neighbors;
|
|
mean.y /= nr_neighbors;
|
|
mean.z /= nr_neighbors;
|
|
|
|
Matrix X(nr_neighbors,3);
|
|
SymmetricMatrix A(3);
|
|
DiagonalMatrix D(3);
|
|
|
|
//calculate covariance = A for all the points
|
|
for (int j = 0; j < nr_neighbors; ++j) {
|
|
X(j+1, 1) = points[nidx[j]].x - mean.x;
|
|
X(j+1, 2) = points[nidx[j]].y - mean.y;
|
|
X(j+1, 3) = points[nidx[j]].z - mean.z;
|
|
}
|
|
|
|
A << 1.0/nr_neighbors * X.t() * X;
|
|
|
|
EigenValues(A, D, U);
|
|
|
|
e1 = D(1);
|
|
e2 = D(2);
|
|
e3 = D(3);
|
|
|
|
delete[] nidx;
|
|
delete[] d;
|
|
|
|
//We take the particular k if the second maximum eigen value
|
|
//is at least 25 percent of the maximum eigen value
|
|
if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25))
|
|
break;
|
|
}
|
|
|
|
//normal = eigenvector corresponding to lowest
|
|
//eigen value that is the 1rd column of matrix U
|
|
ColumnVector n(3);
|
|
n(1) = U(1,1);
|
|
n(2) = U(2,1);
|
|
n(3) = U(3,1);
|
|
ColumnVector point_vector(3);
|
|
point_vector(1) = p[0] - rPos(1);
|
|
point_vector(2) = p[1] - rPos(2);
|
|
point_vector(3) = p[2] - rPos(3);
|
|
point_vector = point_vector / point_vector.NormFrobenius();
|
|
Real angle = (n.t() * point_vector).AsScalar();
|
|
if (angle < 0) {
|
|
n *= -1.0;
|
|
}
|
|
n = n / n.NormFrobenius();
|
|
normals.push_back(Point(n(1), n(2), n(3)));
|
|
}
|
|
annDeallocPts(pa);
|
|
}
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////
|
|
/////////////NORMALS USING AKNN METHOD ////////////////
|
|
///////////////////////////////////////////////////////
|
|
void calculateNormalsKNN(vector<Point> &normals,
|
|
vector<Point> &points,
|
|
int k,
|
|
const double _rPos[3] )
|
|
{
|
|
cout<<"Total number of points: "<<points.size()<<endl;
|
|
int nr_neighbors = k;
|
|
|
|
ColumnVector rPos(3);
|
|
for (int i = 0; i < 3; ++i)
|
|
rPos(i+1) = _rPos[i];
|
|
|
|
double** pa = new double*[points.size()];
|
|
for (size_t i = 0; i < points.size(); ++i)
|
|
{
|
|
pa[i] = new double[3];
|
|
pa[i][0] = points[i].x;
|
|
pa[i][1] = points[i].y;
|
|
pa[i][2] = points[i].z;
|
|
}
|
|
|
|
KDtree t(pa, points.size());
|
|
|
|
for (size_t i=0; i<points.size(); ++i)
|
|
{
|
|
double p[3] = {pa[i][0], pa[i][1], pa[i][2]};
|
|
|
|
//ANN search for k nearest neighbors
|
|
//indexes of the neighbors along with the query point
|
|
//stored in the array n
|
|
|
|
vector<Point> temp = t.kNearestNeighbors(p,
|
|
nr_neighbors,
|
|
numeric_limits<double>::max());
|
|
nr_neighbors = temp.size();
|
|
|
|
Point mean(0.0,0.0,0.0);
|
|
Matrix X(nr_neighbors,3);
|
|
SymmetricMatrix A(3);
|
|
Matrix U(3,3);
|
|
DiagonalMatrix D(3);
|
|
|
|
//calculate mean for all the points
|
|
for (int j = 0; j < nr_neighbors; ++j)
|
|
{
|
|
mean.x += temp[j].x;
|
|
mean.y += temp[j].y;
|
|
mean.z += temp[j].z;
|
|
}
|
|
mean.x /= nr_neighbors;
|
|
mean.y /= nr_neighbors;
|
|
mean.z /= nr_neighbors;
|
|
|
|
//calculate covariance = A for all the points
|
|
for (int i = 0; i < nr_neighbors; ++i) {
|
|
X(i+1, 1) = temp[i].x - mean.x;
|
|
X(i+1, 2) = temp[i].y - mean.y;
|
|
X(i+1, 3) = temp[i].z - mean.z;
|
|
}
|
|
|
|
A << 1.0/nr_neighbors * X.t() * X;
|
|
|
|
EigenValues(A, D, U);
|
|
|
|
//normal = eigenvector corresponding to lowest
|
|
//eigen value that is the 1st column of matrix U
|
|
ColumnVector n(3);
|
|
n(1) = U(1,1);
|
|
n(2) = U(2,1);
|
|
n(3) = U(3,1);
|
|
ColumnVector point_vector(3);
|
|
point_vector(1) = p[0] - rPos(1);
|
|
point_vector(2) = p[1] - rPos(2);
|
|
point_vector(3) = p[2] - rPos(3);
|
|
point_vector = point_vector / point_vector.NormFrobenius();
|
|
Real angle = (n.t() * point_vector).AsScalar();
|
|
if (angle < 0) {
|
|
n *= -1.0;
|
|
}
|
|
n = n / n.NormFrobenius();
|
|
normals.push_back(Point(n(1), n(2), n(3)));
|
|
}
|
|
|
|
|
|
for (size_t i = 0; i < points.size(); ++i)
|
|
{
|
|
delete[] pa[i];
|
|
}
|
|
delete[] pa;
|
|
|
|
}
|
|
////////////////////////////////////////////////////////////////
|
|
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
|
|
////////////////////////////////////////////////////////////////
|
|
void calculateNormalsAdaptiveKNN(vector<Point> &normals,
|
|
vector<Point> &points,
|
|
int kmin,
|
|
int kmax,
|
|
const double _rPos[3])
|
|
{
|
|
ColumnVector rPos(3);
|
|
for (int i = 0; i < 3; ++i)
|
|
rPos(i+1) = _rPos[i];
|
|
|
|
cout<<"Total number of points: "<<points.size()<<endl;
|
|
int nr_neighbors;
|
|
|
|
double** pa = new double*[points.size()];
|
|
for (size_t i = 0; i < points.size(); ++i)
|
|
{
|
|
pa[i] = new double[3];
|
|
pa[i][0] = points[i].x;
|
|
pa[i][1] = points[i].y;
|
|
pa[i][2] = points[i].z;
|
|
}
|
|
|
|
KDtree t(pa, points.size());
|
|
|
|
Point mean(0.0,0.0,0.0);
|
|
double e1,e2,e3;
|
|
|
|
for (size_t i=0; i<points.size(); ++i)
|
|
{
|
|
Matrix U(3,3);
|
|
|
|
double p[3] = {pa[i][0], pa[i][1], pa[i][2]};
|
|
|
|
for(int kidx = kmin; kidx < kmax; kidx++)
|
|
{
|
|
nr_neighbors=kidx+1;
|
|
|
|
//ANN search for k nearest neighbors
|
|
//indexes of the neighbors along with the query point
|
|
//stored in the array n
|
|
|
|
vector<Point> temp = t.kNearestNeighbors(p,
|
|
nr_neighbors,
|
|
numeric_limits<double>::max());
|
|
nr_neighbors = temp.size();
|
|
|
|
mean.x=0,mean.y=0,mean.z=0;
|
|
//calculate mean for all the points
|
|
for (int j=0; j<nr_neighbors; ++j)
|
|
{
|
|
mean.x += temp[j].x;
|
|
mean.y += temp[j].y;
|
|
mean.z += temp[j].z;
|
|
}
|
|
mean.x /= nr_neighbors;
|
|
mean.y /= nr_neighbors;
|
|
mean.z /= nr_neighbors;
|
|
|
|
Matrix X(nr_neighbors,3);
|
|
SymmetricMatrix A(3);
|
|
DiagonalMatrix D(3);
|
|
|
|
//calculate covariance = A for all the points
|
|
for (int j = 0; j < nr_neighbors; ++j) {
|
|
X(j+1, 1) = temp[j].x - mean.x;
|
|
X(j+1, 2) = temp[j].y - mean.y;
|
|
X(j+1, 3) = temp[j].z - mean.z;
|
|
}
|
|
|
|
A << 1.0/nr_neighbors * X.t() * X;
|
|
|
|
EigenValues(A, D, U);
|
|
|
|
e1 = D(1);
|
|
e2 = D(2);
|
|
e3 = D(3);
|
|
|
|
//We take the particular k if the second maximum eigen value
|
|
//is at least 25 percent of the maximum eigen value
|
|
if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25))
|
|
break;
|
|
}
|
|
|
|
//normal = eigenvector corresponding to lowest
|
|
//eigen value that is the 1rd column of matrix U
|
|
ColumnVector n(3);
|
|
n(1) = U(1,1);
|
|
n(2) = U(2,1);
|
|
n(3) = U(3,1);
|
|
ColumnVector point_vector(3);
|
|
point_vector(1) = p[0] - rPos(1);
|
|
point_vector(2) = p[1] - rPos(2);
|
|
point_vector(3) = p[2] - rPos(3);
|
|
point_vector = point_vector / point_vector.NormFrobenius();
|
|
Real angle = (n.t() * point_vector).AsScalar();
|
|
if (angle < 0) {
|
|
n *= -1.0;
|
|
}
|
|
n = n / n.NormFrobenius();
|
|
normals.push_back(Point(n(1), n(2), n(3)));
|
|
}
|
|
|
|
for (size_t i = 0; i < points.size(); ++i)
|
|
{
|
|
delete[] pa[i];
|
|
}
|
|
delete[] pa;
|
|
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////
|
|
/////////////NORMALS USING IMAGE NEIGHBORS ////////////
|
|
///////////////////////////////////////////////////////
|
|
void calculateNormalsPANORAMA(vector<Point> &normals,
|
|
vector<Point> &points,
|
|
vector< vector< vector< cv::Vec3f > > > extendedMap,
|
|
const double _rPos[3])
|
|
{
|
|
ColumnVector rPos(3);
|
|
for (int i = 0; i < 3; ++i)
|
|
rPos(i+1) = _rPos[i];
|
|
|
|
cout<<"Total number of points: "<<points.size()<<endl;
|
|
points.clear();
|
|
int nr_neighbors = 0;
|
|
cout << "height of Image: "<<extendedMap.size()<<endl;
|
|
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
|
|
|
// as the nearest neighbors and then the same PCA method as done in AKNN
|
|
//temporary dynamic array for all the neighbors of a given point
|
|
vector<cv::Vec3f> neighbors;
|
|
for (size_t i=0; i< extendedMap.size(); i++)
|
|
{
|
|
for (size_t j=0; j<extendedMap[i].size(); j++)
|
|
{
|
|
if (extendedMap[i][j].size() == 0) continue;
|
|
neighbors.clear();
|
|
Point mean(0.0,0.0,0.0);
|
|
|
|
// Offset for neighbor computation
|
|
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
|
|
|
|
// Traversing all the cells in the extended map
|
|
for (int n = 0; n < 5; ++n) {
|
|
int x = i + offset[0][n];
|
|
int y = j + offset[1][n];
|
|
|
|
// Copy the neighboring buckets into the vector
|
|
if (x >= 0 && x < (int)extendedMap.size() &&
|
|
y >= 0 && y < (int)extendedMap[x].size()) {
|
|
for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) {
|
|
neighbors.push_back(extendedMap[x][y][k]);
|
|
}
|
|
}
|
|
}
|
|
|
|
nr_neighbors = neighbors.size();
|
|
cv::Vec3f p = extendedMap[i][j][0];
|
|
|
|
//if no neighbor point is found in the 4-neighboring pixels then normal is set to zero
|
|
if (nr_neighbors < 3)
|
|
{
|
|
points.push_back(Point(p[0], p[1], p[2]));
|
|
normals.push_back(Point(0.0,0.0,0.0));
|
|
continue;
|
|
}
|
|
|
|
//calculate mean for all the points
|
|
Matrix X(nr_neighbors,3);
|
|
SymmetricMatrix A(3);
|
|
Matrix U(3,3);
|
|
DiagonalMatrix D(3);
|
|
|
|
//calculate mean for all the points
|
|
for(int k = 0; k < nr_neighbors; k++)
|
|
{
|
|
cv::Vec3f pp = neighbors[k];
|
|
mean.x += pp[0];
|
|
mean.y += pp[1];
|
|
mean.z += pp[2];
|
|
}
|
|
|
|
mean.x /= nr_neighbors;
|
|
mean.y /= nr_neighbors;
|
|
mean.z /= nr_neighbors;
|
|
//calculate covariance = A for all the points
|
|
for (int n = 0; n < nr_neighbors; ++n) {
|
|
cv::Vec3f pp = neighbors[n];
|
|
X(n+1, 1) = pp[0] - mean.x;
|
|
X(n+1, 2) = pp[1] - mean.y;
|
|
X(n+1, 3) = pp[2] - mean.z;
|
|
}
|
|
|
|
A << 1.0/nr_neighbors * X.t() * X;
|
|
|
|
EigenValues(A, D, U);
|
|
//normal = eigenvector corresponding to lowest
|
|
//eigen value that is the 1st column of matrix U
|
|
ColumnVector n(3);
|
|
n(1) = U(1,1);
|
|
n(2) = U(2,1);
|
|
n(3) = U(3,1);
|
|
ColumnVector point_vector(3);
|
|
point_vector(1) = p[0] - rPos(1);
|
|
point_vector(2) = p[1] - rPos(2);
|
|
point_vector(3) = p[2] - rPos(3);
|
|
point_vector = point_vector / point_vector.NormFrobenius();
|
|
Real angle = (n.t() * point_vector).AsScalar();
|
|
if (angle < 0) {
|
|
n *= -1.0;
|
|
}
|
|
n = n / n.NormFrobenius();
|
|
|
|
for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) {
|
|
cv::Vec3f p = extendedMap[i][j][k];
|
|
points.push_back(Point(p[0], p[1], p[2]));
|
|
normals.push_back(Point(n(1), n(2), n(3)));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE //////////////////////////
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
/*
|
|
void calculateNormalsFAST(vector<Point> &normals,vector<Point> &points,cv::Mat &img,vector<vector<vector<cv::Vec3f>>> extendedMap)
|
|
{
|
|
cout<<"Total number of points: "<<points.size()<<endl;
|
|
points.clear();
|
|
int nr_points = 0;
|
|
//int nr_neighbors = 0,nr_neighbors_center = 0;
|
|
cout << "height of Image: "<<extendedMap.size()<<endl;
|
|
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
|
for (size_t i=0; i< extendedMap.size(); ++i)
|
|
{
|
|
for (size_t j=0; j<extendedMap[0].size(); j++)
|
|
{
|
|
double theta,phi,rho;
|
|
double x,y,z;
|
|
double dRdTheta,dRdPhi;
|
|
double n[3],m;
|
|
nr_points = extendedMap[i][j].size();
|
|
if (nr_points == 0 ) continue;
|
|
|
|
for (int k = 0; k< nr_points; k++)
|
|
{
|
|
cv::Vec3f p = extendedMap[i][j][k];
|
|
x = p[0];
|
|
y = p[1];
|
|
z = p[2];
|
|
rho = sqrt(x*x + y*y + z*z);
|
|
theta = atan(y/x);
|
|
phi = atan(z/x);
|
|
|
|
//Sobel Filter for the derivative
|
|
dRdTheta = dRdPhi = 0.0;
|
|
|
|
if (i == 0 || i == extendedMap.size()-1 || j == 0 || j == extendedMap[0].size()-1)
|
|
{
|
|
points.push_back(Point(x, y, z));
|
|
normals.push_back(Point(0.0,0.0,0.0));
|
|
continue;
|
|
}
|
|
dRdPhi += 10*img.at<uchar>(i-1,j);
|
|
dRdPhi += 3 *img.at<uchar>(i-1,j-1);
|
|
dRdPhi += 3 *img.at<uchar>(i-1,j+1);
|
|
dRdPhi -= 10*img.at<uchar>(i+1,j);
|
|
dRdPhi -= 3 *img.at<uchar>(i+1,j-1);
|
|
dRdPhi -= 3 *img.at<uchar>(i+1,j+1);
|
|
|
|
dRdTheta += 10*img.at<uchar>(i,j-1);
|
|
dRdTheta += 3 *img.at<uchar>(i-1,j-1);
|
|
dRdTheta += 3 *img.at<uchar>(i+1,j-1);
|
|
dRdTheta -= 10*img.at<uchar>(i,j+1);
|
|
dRdTheta -= 3 *img.at<uchar>(i-1,j+1);
|
|
dRdTheta -= 3 *img.at<uchar>(i+1,j+1);
|
|
|
|
n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) +
|
|
cos(theta) * cos(phi) * dRdPhi / rho;
|
|
|
|
n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) +
|
|
sin(theta) * cos(phi) * dRdPhi / rho;
|
|
|
|
n[2] = cos(phi) - sin(phi) * dRdPhi / rho;
|
|
|
|
//n[2] = -n[2];
|
|
|
|
m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
|
|
n[0] /= m; n[1] /= m; n[2] /= m;
|
|
|
|
points.push_back(Point(x, y, z));
|
|
normals.push_back(Point(n[0],n[1],n[2]));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
*/
|