C++ Vec3d::dot方法代码示例

bool Triangle::angleCriterion( const double &minCosAngle, 
	const double &maxCosAngle )
{
	Vec3d ab = m_vertices[1].m_xyz - m_vertices[0].m_xyz;
	Vec3d bc = m_vertices[2].m_xyz - m_vertices[1].m_xyz;
	Vec3d ac = m_vertices[2].m_xyz - m_vertices[0].m_xyz;

	double lenAB = ab.dot(ab);
	double lenBC = bc.dot(bc);
	double lenAC = ac.dot(ac);

	double maxLen = lenAB, minLen = lenAB;
	double lenMaxE0 = lenBC, lenMaxE1 = lenAC;
	double lenMinE0 = lenBC, lenMinE1 = lenAC;
	Vec3d maxE0 = bc, maxE1 = ac, minE0 = bc, minE1 = ac;

	bool maxFlag = true, minFlag = true;
	if (maxCosAngle > COS180 && maxCosAngle <  COS60)
	{
		if (maxLen < lenBC)
		{
			maxLen = lenBC;
			lenMaxE0 = lenAB;
			lenMaxE1 = lenAC;
			maxE0 = ab;
			maxE1 = ac;
		}
		if (maxLen < lenAC)
		{
			lenMaxE0 = lenAB;
			lenMaxE1 = lenBC;
			maxE0 = -ab;
			maxE1 = bc;
		}
		maxFlag = maxE0.dot(maxE1) > sqrt(lenMaxE0) 
			* sqrt(lenMaxE1) * maxCosAngle;
	}
	if (minCosAngle < COS0 && minCosAngle > COS60)
	{
		if (minLen > lenBC)
		{
			minLen = lenBC;
			lenMinE0 = lenAB;
			lenMinE1 = lenAC;
			minE0 = ab;
			minE1 = ac;
		}
		if (minLen > lenAC)
		{
			lenMinE0 = lenAB;
			lenMinE1 = lenBC;
			minE0 = -ab;
			minE1 = bc;
		}
		minFlag = minE0.dot(minE1) < sqrt(lenMinE0) 
			* sqrt(lenMinE1) * minCosAngle;
	}
	
	return minFlag && maxFlag;
}

// this function will return B
static double calculateB( const Mat& img )
{
    double B = 0;
    for( int x = 0; x < img.rows; x++ )
    {
        for( int y = 0; y < img.cols; y++ )
        {
            Vec3d color = img.at<Vec3b>(x,y);
            if( y>0 ) // left
            {
                Vec3d diffColor = color - (Vec3d)img.at<Vec3b>(x,y-1);
                B += diffColor.dot(diffColor);
            }
            if( y>0 && x>0 ) // upleft
            {
                Vec3d diffColor = color - (Vec3d)img.at<Vec3b>(x-1,y-1);
                B += diffColor.dot(diffColor);
            }
            if( x>0 ) // up
            {
                Vec3d diffColor = color - (Vec3d)img.at<Vec3b>(x-1,y);
                B += diffColor.dot(diffColor);
            }
            if( x>0 && y<img.cols-1) // upright
            {
                Vec3d diffColor = color - (Vec3d)img.at<Vec3b>(x-1,y+1);
                B += diffColor.dot(diffColor);
            }
        }
    }
    B = 1.0f / (2 * B/(4*img.cols*img.rows - 3*img.cols - 3*img.rows + 2) );
    
    return B;
}

// Kok Lim Low's linearization
static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Vec3d& rpy, Vec3d& t)
{
  //Mat sub = Dst - Src;
  Mat A = Mat(Src.rows, 6, CV_64F);
  Mat b = Mat(Src.rows, 1, CV_64F);
  Mat rpy_t;

#if defined _OPENMP
#pragma omp parallel for
#endif
  for (int i=0; i<Src.rows; i++)
  {
    const Vec3d srcPt(Src.ptr<double>(i));
    const Vec3d dstPt(Dst.ptr<double>(i));
    const Vec3d normals(Dst.ptr<double>(i) + 3);
    const Vec3d sub = dstPt - srcPt;
    const Vec3d axis = srcPt.cross(normals);

    *b.ptr<double>(i) = sub.dot(normals);
    hconcat(axis.reshape<1, 3>(), normals.reshape<1, 3>(), A.row(i));
  }

  cv::solve(A, b, rpy_t, DECOMP_SVD);
  rpy_t.rowRange(0, 3).copyTo(rpy);
  rpy_t.rowRange(3, 6).copyTo(t);
}

bool Delaunay::inCircle( Vertex a, Vertex b, Vertex c, Vertex p )
{
	Vec3d cb = b.m_xyz - c.m_xyz;
	Vec3d ca = a.m_xyz - c.m_xyz;
	Vec3d pb = b.m_xyz - p.m_xyz;
	Vec3d pa = a.m_xyz - p.m_xyz;

	double cross_cbca = fabs(cb[0] * ca[1] - cb[1] * ca[0]);
	double cross_pbpa = fabs(pb[0] * pa[1] - pb[1] * pa[0]);
	double dot_cbca = cb.dot(ca);
	double dot_pbpa = pb.dot(pa);
	if (cross_cbca * dot_pbpa + cross_pbpa * dot_cbca < 0)
	{
		return true;
	}
	return false;
}

//--------------------------------------------------------------------------------------------------
// computeArtificialViscosityForces
void BasicSPH::computeArtificialViscosityForces()
{
    const double alpha = _bulkViscosity;	// Bulk viscosity
    const double beta = _shearViscosity;	// Shear viscosity

    _maxuij = 0.0;

    // Precompute coefficients
    const double speedOfSound = sqrt(_k);	// c
    const double h2 = _h*_h;

    // Compute artificial viscosity forces
    for (long p=0; p<_particles.size(); ++p)
    {
        SPHParticle &particle = _particles[p];

        // No need to compute current particle's contribution, since its gradient is null!

        // Get neighbors contributions
        for (long n=0; n<_neighbors[p].size(); ++n)
        {
            const Neighbor &neighbor = _neighbors[p][n];
            const SPHParticle &neighborParticle = _particles[neighbor.id];

            // Compute contribution (based on the paper of Monaghan (1992))
            // fv_i/rho_i = SUM_j(m_j * IIij * gradient(Wij))
            //         | (alpha * c * uij + beta * uij^2) / avgRho,	when vij.xij < 0
            // IIij = -|
            //         | 0,											otherwise
            // uij = h * (vij.xij) / (|xij|^2 + 0.01*h^2)
            // vij = vi - vj
            // xij = xi - xj
            // avgRho = 0.5 * (rho_i + rho_j)
            Vec3d vij = particle.vel - neighborParticle.vel;
            double vijxij = vij.dot(neighbor.xij);
            double dij = neighbor.dist;
            double uij = _h*vijxij / (dij*dij + 0.01*h2);
            if (uij < 0)
            {
                // Compute contribution
                double avgDensity = 0.5 * (particle.density + neighborParticle.density);
                double IIij = (alpha*uij*speedOfSound + beta*uij*uij) / avgDensity;
                Vec3d contribution = getKernelGradient(neighbor.dist, neighbor.xij);
                contribution *= IIij;
                contribution *= _mass;

                particle.accel += contribution;
            }

            // Update maxuij for adaptive time step calculations
            if (uij > _maxuij)
            {
                _maxuij = uij;
            }
        }
    }
}

// second item in the function
//the function is for each pixel (item1+item2)
//store each node's (not including s and t) neighbors' information is 4 different matrix
static void calcWeight( const Mat& img, Mat& leftWeight, Mat& upleftWeight, Mat& upWeight, Mat& uprightWeight, double B, double gamma )
{
    double gammaWithSqrt= gamma / sqrt(2.0f);
    //use four mat to store the left, upleft, up and up right weight for each node(pixel) in the image
    leftWeight.create( img.rows, img.cols, CV_64FC1 );
    upleftWeight.create( img.rows, img.cols, CV_64FC1 );
    upWeight.create( img.rows, img.cols, CV_64FC1 );
    uprightWeight.create( img.rows, img.cols, CV_64FC1 );
    for( int x = 0; x < img.rows; x++ )
    {
        for( int y = 0; y < img.cols; y++ )
        {
            Vec3d color = img.at<Vec3b>(x,y);
            if( y-1>=0 ) // left
            {
                Vec3d diff = color - (Vec3d)img.at<Vec3b>(x,y-1);
                leftWeight.at<double>(x,y) = gamma * exp(-B*diff.dot(diff));
            }
            else
                leftWeight.at<double>(x,y) = 0;
            if( x-1>=0 && y-1>=0 ) // upleft
            {
                Vec3d diff = color - (Vec3d)img.at<Vec3b>(x-1,y-1);
                upleftWeight.at<double>(x,y) = gammaWithSqrt * exp(-B*diff.dot(diff));
            }
            else
                upleftWeight.at<double>(x,y) = 0;
            if( x-1>=0 ) // up
            {
                Vec3d diff = color - (Vec3d)img.at<Vec3b>(x-1,y);
                upWeight.at<double>(x,y) = gamma * exp(-B*diff.dot(diff));
            }
            else
                upWeight.at<double>(x,y) = 0;
            if( y+1<img.cols && x-1>=0 ) // upright
            {
                Vec3d diff = color - (Vec3d)img.at<Vec3b>(x-1,y+1);
                uprightWeight.at<double>(x,y) = gammaWithSqrt * exp(-B*diff.dot(diff));
            }
            else
                uprightWeight.at<double>(x,y) = 0;
        }
    }
}

//--------------------------------------------------------------------------------------------------
// buildNeighbors
void BasicSPH::buildNeighbors()
{
    // Reserve space and initialize neighbors' data
    _neighbors.clear();
    _neighbors.resize(_particles.size());

    // Init spatial grid
    Vec3d borders(_h*2.0, _h*2.0, _h*2.0);
    Vec3d gridMin = _volumeMin;
    gridMin -= borders;
    Vec3d gridMax = _volumeMax;
    gridMax += borders;
    SpatialGrid<long> grid(_h, gridMin, gridMax);

    // Insert particles into grid
    for (long p=0; p<_particles.size(); ++p)
    {
        grid.insert(p, _particles[p].pos);
    }

    // Use grid to retrieve neighbor particles
    double h2 = _h*_h;
    std::vector<long*> nearbyParticles;
    for (long p=0; p<_particles.size(); ++p)
    {
        const SPHParticle &particle = _particles[p];

        // Get nearby particles
        grid.getElements(particle.pos, _h, nearbyParticles);

        // Find particles that are within smoothing radius
        _neighbors[p].reserve(50);
        for (long i=0; i<nearbyParticles.size(); ++i)
        {
            long nID = *nearbyParticles[i];
            const SPHParticle &neighborParticle = _particles[nID];

            // Skip current particle
            if (nID==p)
                continue;

            Vec3d xij = particle.pos - neighborParticle.pos;

            // Check if distance is lower than smoothing radius
            double dist2 = xij.dot(xij);
            if (dist2 < h2)
            {
                // Yup! Add the particle to the neighbors list along with
                // some precomputed informations
                _neighbors[p].push_back(Neighbor(nID, xij, sqrt(dist2)));
            }
        }
    }
}

void TestAppMesh::eventHandling ( const SDL_Event& event  ){
    //printf( "NonInert_seats::eventHandling() \n" );
    switch( event.type ){
        case SDL_KEYDOWN :
            switch( event.key.keysym.sym ){
                case SDLK_p:  first_person = !first_person; break;
                case SDLK_o:  perspective  = !perspective; break;
                //case SDLK_r:  world.fireProjectile( warrior1 ); break;
            }
            break;
        case SDL_MOUSEBUTTONDOWN:
            switch( event.button.button ){
                case SDL_BUTTON_LEFT:
                    Vec3d ray0;
                    ray0.set_lincomb( 1, mouse_begin_x, mouse_begin_y, camPos, camMat.a, camMat.b );
                    //glColor3f(1,1,1); Draw3D::drawPointCross( ray0, 0.2 );
                    ipicked= mesh.pickVertex( ray0, camMat.c );
                    mouse0.set(mouse_begin_x, mouse_begin_y);
                    dragging=true;
                    break;
            }
            break;
        case SDL_MOUSEBUTTONUP:
            switch( event.button.button ){
                case SDL_BUTTON_LEFT:
                    if(dragging){
                        Vec3d d;
                        d.set_lincomb( 1, mouse_begin_x, mouse_begin_y, camPos, camMat.a, camMat.b );
                        d.sub(mesh.points[ipicked]);
                        double c = d.dot(camMat.c);  d.add_mul(camMat.c, -c);
                        mesh.points[ipicked].add(d);

                        glDeleteLists(mesh.rendered_shape, 1);
                        mesh.rendered_shape = glGenLists(1);
                        glNewList( mesh.rendered_shape , GL_COMPILE );
                            glEnable( GL_LIGHTING );
                            glColor3f( 0.8f, 0.8f, 0.8f );
                            Draw3D::drawMesh( mesh );
                            mesh.tris2normals(true);
                            glColor3f(0.0f,0.0f,0.9f);
                            for(int i=0; i<mesh.points.size(); i++){
                                Draw3D::drawVecInPos( mesh.normals[i], mesh.points[i] );
                            }
                        glEndList();
                    }
                    dragging=false;
                    break;
            }
    };
    AppSDL2OGL::eventHandling( event );
}

void VRAdjacencyGraph::compCurvatures(int range) {
    vertex_curvatures.clear();
    auto sgeo = geo.lock();
    if (!sgeo) return;

    auto pos = sgeo->getMesh()->geo->getPositions();
    auto norms = sgeo->getMesh()->geo->getNormals();
    int N = pos->size();

    /*auto curvMax = [&](int i, int range) {
		Vec3d n = norms->getValue<Vec3f>(i);
		Vec3d vi = pos->getValue<Pnt3f>(i);
		float K = 0;
		float Kmax = 0;
		auto Ne = getNeighbors(i,range);
		if (Ne.size() == 0) return K;

		for (int j : Ne) {
			if (j >= N) continue;
			Vec3d d = pos->getValue<Pnt3f>(j) - vi;
			float k = 2*n.dot(d)/d.squareLength();
			if (abs(k) > Kmax) {
                K = k;
                Kmax = abs(k);
			}
		}

		return K;
    };*/

    auto curvAvg = [&](int i, int range) {
		Vec3d n = Vec3d(norms->getValue<Vec3f>(i));
		Pnt3f vi = pos->getValue<Pnt3f>(i);
		float K = 0;
		auto Ne = getNeighbors(i,range);
		if (Ne.size() == 0) return K;

		for (int j : Ne) {
			if (j >= N) continue;
			Vec3d d = Vec3d(pos->getValue<Pnt3f>(j) - vi);
			K += 2*n.dot(d)/d.length();
		}

		K /= Ne.size();
		return K;
    };

    vertex_curvatures.resize(N);
    for (int i = 0; i < N; i++) vertex_curvatures[i] = curvAvg(i,range);
}

void RIMLS::Fit()
{
	double f = 0;

	Vec3d grad_f(0, 0, 0);
	do 
	{
		int i = 0;
		do 
		{
			double sumW, sumF;
			sumW = sumF = 0;
			Vec3d sumGW, sumGF, sumN;
			sumGW = sumGF = sumN = Vec3d(0.0, 0.0, 0.0);

			for (DataPoint& p : m_neighbors)
			{
				Vec3d px = m_x - p.pos();
				double fx = px.dot(p.normal());

				double alpha = 1.0;
				if (i > 0)
				{
					alpha = exp(-pow((fx - f) / m_sigmaR, 2)) *
						exp(-pow((p.normal() - grad_f).norm() / m_sigmaN, 2));
				}

				double phi = exp(-pow(px.norm() / m_sigmaT, 2));
				double w = alpha*phi;
				Vec3d grad_w = -2.0*alpha*phi*px / pow(m_sigmaT, 2);

				sumW += w;
				sumGW += grad_w;
				sumF += w*fx;
				sumGF += grad_w*fx;
				sumN += w*p.normal();
			}
			f = sumF / sumW;
			grad_f = (sumGF - f*sumGW + sumN) / sumW;
		} while (++i<m_iter && !convergence());

		m_x -= f*grad_f;

	} while ((f*grad_f).norm() > m_threshold);
}

void Delaunay::computeDelaunay(Mesh &mesh)
{
	int size = (int)mesh.getVerticesSize();
	if (size == 0)
	{
		return;
	}
	mesh.computeVerticesNormals();
	m_preSize = mesh.m_curIndex;

	TriangleSet triSet;
	// 依次遍历每个点，寻找最近邻，进行三角化
	for (; mesh.m_curIndex < size; mesh.m_curIndex++)
	{
		Vertex v = mesh.getVertex(mesh.m_curIndex);
		if (v.m_isInner)
		{
			mesh.pushTriBeginIndex((int)triSet.size());
			continue;
		}

		Vec3d normal = v.m_normal;
		int id = 2;
		// 判断法向量哪个不为0，z->y->x
		if (normal[2] != 0)		// z
		{
			id = 2;
		}
		else if (normal[1] != 0)// y
		{
			id = 1;
		}
		else if (normal[0] != 0)// x
		{
			id = 0;
		}
		else	// 法向量为(0, 0, 0)，
		{
			mesh.pushTriBeginIndex((int)triSet.size());
			continue;
		}

		double minDistance = -1;
		int cnt = v.m_neighbors[0];					// 最近邻数目
		double dists[k];
		for (int j = 1; j < cnt + 1; j++)
		{
			Vec3d dv = mesh.getVertex(v.m_neighbors[j]).m_xyz - v.m_xyz;
			dists[j] = dv.ddot(dv);
		}
		minDistance = dists[1];
		VertexVector vVector, tmpvVector;
		// 将最近邻点投射到该点的切平面上
		for (int j = 1; j < cnt + 1; j++)
		{
			Vertex tmpv = mesh.getVertex(v.m_neighbors[j]);
			if (dists[j] < u * minDistance ||		// 去除非常接近的点
				(tmpv.m_index < v.m_index && tmpv.m_index >= m_preSize) ||	// 去除已遍历过的点
				tmpv.m_isInner)						// 去除内点
			{
				continue;
			}
			
			Vec3d vv = tmpv.m_xyz - v.m_xyz;
			double dist2 = dists[j] * 0.75f;	// sqrt
			double alpha = vv.dot(normal);
			alpha = alpha * alpha;
			if (alpha > dist2)		// 去除与法向量夹角小于30度或大于150度的点
			{
				continue;
			}
			Vec3d proj = tmpv.m_xyz - alpha * normal;		// 投射到切平面
			tmpvVector.push_back(Vertex(proj, v.m_neighbors[j]));
		}
		if (tmpvVector.size() < 3)	// 少于3个不能构成三角形
		{
			mesh.pushTriBeginIndex((int)triSet.size());
			continue;
		}

		// 将切平面转换为x-y平面进行三角形计算
		vVector.push_back(Vertex(Vec3d(0, 0, 0), mesh.m_curIndex));	// 原点
		Vec3d vx = tmpvVector[0].m_xyz - v.m_xyz;		// x轴
		vx = normalize(vx);
		for (int j = 0; j < tmpvVector.size(); j++)
		{
			Vec3d vv = tmpvVector[j].m_xyz - v.m_xyz;
			double x = vv.dot(vx);
			double y = vx.cross(vv)[id] / normal[id];
			Vec3d proj(x, y, 0);
			vVector.push_back(Vertex(proj, tmpvVector[j].m_index));
		}

		TriangleVector tVector;
		computeDelaunay(vVector, tVector);
// 		cout << vVector.size() << " " << tVector.size() << endl; 
// 		drawTrianglesOnPlane(tVector);
		for (int j = 0; j < tVector.size(); j++)
		{
			Triangle t = tVector[j];
//.........这里部分代码省略.........

double Vertex::distance2( const Vertex &v )
{
	Vec3d d = m_xyz - v.m_xyz;
	return d.dot(d);
}

void Atmosphere::compute_color(double JD, Vec3d sunPos, Vec3d moonPos, float moon_phase,
                               ToneReproductor * eye, Projector* prj,
                               float latitude, float altitude, float temperature, float relative_humidity)
{

	float min_mw_lum = 0.13;

	// no need to calculate if not visible
	if (!fader.getInterstate()) {
		atm_intensity = 0;
		world_adaptation_luminance = 3.75f + lightPollutionLuminance;
		milkyway_adaptation_luminance = min_mw_lum;  // brighter than without atm, since no drawing addition of atm brightness
		return;
	} else {
		atm_intensity = fader.getInterstate();
	}


	//Vec3d obj;
	skylight_struct2 b2;

	// these are for radii
	double sun_angular_size = atan(696000./AU/sunPos.length());
	double moon_angular_size = atan(1738./AU/moonPos.length());

	double touch_angle = sun_angular_size + moon_angular_size;
	double dark_angle = moon_angular_size - sun_angular_size;

	sunPos.normalize();
	moonPos.normalize();

	// determine luminance falloff during solar eclipses
	double separation_angle = acos( sunPos.dot( moonPos ));  // angle between them

	//	printf("touch at %f\tnow at %f (%f)\n", touch_angle, separation_angle, separation_angle/touch_angle);

	// bright stars should be visible at total eclipse
	// TODO: correct for atmospheric diffusion
	// TODO: use better coverage function (non-linear)
	// because of above issues, this algorithm darkens more quickly than reality
	if ( separation_angle < touch_angle) {
		float min;
		if (dark_angle < 0) {
			// annular eclipse
			float asun = sun_angular_size*sun_angular_size;
			min = (asun - moon_angular_size*moon_angular_size)/asun;  // minimum proportion of sun uncovered
			dark_angle *= -1;
		}
		//		else min = 0.004;  // so bright stars show up at total eclipse
		else min = 0.001;  // so bright stars show up at total eclipse

		if (separation_angle < dark_angle) atm_intensity = min;
		else atm_intensity *= min + (1.-min)*(separation_angle-dark_angle)/(touch_angle-dark_angle);

		//		printf("atm int %f (min %f)\n", atm_intensity, min);
	}

	float sun_pos[3];
	sun_pos[0] = sunPos[0];
	sun_pos[1] = sunPos[1];
	sun_pos[2] = sunPos[2];

	float moon_pos[3];
	moon_pos[0] = moonPos[0];
	moon_pos[1] = moonPos[1];
	moon_pos[2] = moonPos[2];

	sky.set_paramsv(sun_pos, 5.f);

	skyb.set_loc(latitude * M_PI/180., altitude, temperature, relative_humidity);
	skyb.set_sun_moon(moon_pos[2], sun_pos[2]);

	// Calculate the date from the julian day.
	ln_date date;
	NShadeDateTime::JulianToDate(JD, &date);

	skyb.set_date(date.years, date.months, moon_phase);

	float stepX = (float)prj->getViewportWidth() / sky_resolution;
	float stepY = (float)prj->getViewportHeight() / sky_resolution;
	float viewport_left = (float)prj->getViewportPosX();
	float viewport_bottom = (float)prj->getViewportPosY();

	Vec3d point(1., 0., 0.);

	// Variables used to compute the average sky luminance
	double sum_lum = 0.;
	unsigned int nb_lum = 0;

	// Compute the sky color for every point above the ground
	for (int x=0; x<=sky_resolution; ++x) {
		for (int y=0; y<=sky_resolution; ++y) {
			prj->unproject_local((double)viewport_left+x*stepX, (double)viewport_bottom+y*stepY,point);
			point.normalize();

			if (point[2]<=0) {
				point[2] = -point[2];
				// The sky below the ground is the symetric of the one above :
				// it looks nice and gives proper values for brightness estimation
			}
//.........这里部分代码省略.........

void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
{

    // This routine takes any vector as argument but normalized 
    // vectors are necessary, if only for computing the dot product.
    // Too bad the API is that generic, it leads to performance loss.
    // Even in the case the 2 vectors are not normalized but same length,
    // the sqrt could be shared, but we have no way to know beforehand
    // at this point, while the caller may know.
    // So, we have to test... in the hope of saving at least a sqrt
    Vec3d sourceVector = from;
    Vec3d targetVector = to;
    
    value_type fromLen2 = length2(from);
    value_type fromLen;
    // normalize only when necessary, epsilon test
    if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) {
      fromLen = sqrt(fromLen2);
      sourceVector = Vec3d(sourceVector[0]/fromLen,
			   sourceVector[1]/fromLen,
			   sourceVector[2]/fromLen);
    } else fromLen = 1.0;
    
    value_type toLen2 = length2(to);
    // normalize only when necessary, epsilon test
    if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
        value_type toLen;
        // re-use fromLen for case of mapping 2 vectors of the same length
        if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) {
            toLen = fromLen;
        } 
        else toLen = sqrt(toLen2);
	targetVector = Vec3d(targetVector[0]/toLen,
			   targetVector[1]/toLen,
			   targetVector[2]/toLen);

    }

    
    // Now let's get into the real stuff
    // Use "dot product plus one" as test as it can be re-used later on
    double dotProdPlus1 = 1.0 + sourceVector.dot( targetVector);
    
    // Check for degenerate case of full u-turn. Use epsilon for detection
    if (dotProdPlus1 < 1e-7) {
    
        // Get an orthogonal vector of the given vector
        // in a plane with maximum vector coordinates.
        // Then use it as quaternion axis with pi angle
        // Trick is to realize one value at least is >0.6 for a normalized vector.
        if (fabs(sourceVector[0]) < 0.6) {
            const double norm = sqrt(1.0 - sourceVector[0] * sourceVector[0]);
            _v[0] = 0.0; 
            _v[1] = sourceVector[2] / norm;
            _v[2] = -sourceVector[1] / norm;
            _v[3] = 0.0;
        } else if (fabs(sourceVector[1]) < 0.6) {
            const double norm = sqrt(1.0 - sourceVector[1] * sourceVector[1]);
            _v[0] = -sourceVector[2] / norm;
            _v[1] = 0.0;
            _v[2] = sourceVector[0] / norm;
            _v[3] = 0.0;
        } else {
            const double norm = sqrt(1.0 - sourceVector[2] * sourceVector[2]);
            _v[0] = sourceVector[1] / norm;
            _v[1] = -sourceVector[0] / norm;
            _v[2] = 0.0;
            _v[3] = 0.0;
        }
    }
    
    else {
        // Find the shortest angle quaternion that transforms normalized vectors
        // into one other. Formula is still valid when vectors are colinear
        const double s = sqrt(0.5 * dotProdPlus1);
         Vec3d tmp = sourceVector.cross( targetVector);
	tmp= Vec3d(tmp[0]/ (2.0*s), tmp[1]/ (2.0*s), tmp[2]/ (2.0*s));
        _v[0] = tmp[0];
        _v[1] = tmp[1];
        _v[2] = tmp[2];
        _v[3] = s;
    }
}