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

// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to )
{
    const value_type epsilon = 0.0000001;

    value_type length1  = from.length();
    value_type length2  = to.length();

    // dot product vec1*vec2
    value_type cosangle = from*to/(length1*length2);

    if ( fabs(cosangle - 1) < epsilon )
    {
        OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;

        // cosangle is close to 1, so the vectors are close to being coincident
        // Need to generate an angle of zero with any vector we like
        // We'll choose (1,0,0)
        makeRotate( 0.0, 0.0, 0.0, 1.0 );
    }
    else
    if ( fabs(cosangle + 1.0) < epsilon )
    {
        // vectors are close to being opposite, so will need to find a
        // vector orthongonal to from to rotate about.
        Vec3d tmp;
        if (fabs(from.x())<fabs(from.y()))
            if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
            else tmp.set(0.0,0.0,1.0);
        else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
        else tmp.set(0.0,0.0,1.0);

        Vec3d fromd(from.x(),from.y(),from.z());

        // find orthogonal axis.
        Vec3d axis(fromd^tmp);
        axis.normalize();

        _v[0] = axis[0]; // sin of half angle of PI is 1.0.
        _v[1] = axis[1]; // sin of half angle of PI is 1.0.
        _v[2] = axis[2]; // sin of half angle of PI is 1.0.
        _v[3] = 0; // cos of half angle of PI is zero.

    }
    else
    {
        // This is the usual situation - take a cross-product of vec1 and vec2
        // and that is the axis around which to rotate.
        Vec3d axis(from^to);
        value_type angle = acos( cosangle );
        makeRotate( angle, axis );
    }
}

void AngleMeasure::calculateEndsOneLine(const Vec3d start, const Vec3d end, Vec3d &perp1Start, Vec3d &perp1End, Vec3d &perp2Start, Vec3d &perp2End, double &angle)
{
	Vec3d v0 = end - start;
	Vec3d v1 = Vec3d(0,0,0) - start;
	Vec3d p = v0 ^ v1;
	p *= 0.08;  // end width
	perp1Start.set(start[0]-p[0],start[1]-p[1],start[2]-p[2]);
	perp1End.set(start[0]+p[0],start[1]+p[1],start[2]+p[2]);

	v1 = Vec3d(0,0,0) - end;
	p = v0 ^ v1;
	p *= 0.08;  // end width
	perp2Start.set(end[0]-p[0],end[1]-p[1],end[2]-p[2]);
	perp2End.set(end[0]+p[0],end[1]+p[1],end[2]+p[2]);

	angle = start.angle(end);
}

//  sampleGridArroundAtoms
//  	takes from a 3D grid values at grid points which are in area between $atom_Rmin[i] to $atom_Rmaxs[i] distance from any i-th atom at position $atom_posp[i]
//		if $canStore == true it will save values to $sampled_val and postions to $sampled_pos else it only returns number of gridpoints fullfilling the conditions
int sampleGridArroundAtoms( 
	int natoms, Vec3d * atom_pos_, double * atom_Rmin, double * atom_Rmax, bool * atom_mask, 
	double * sampled_val, Vec3d * sampled_pos_, bool canStore, bool pbc, bool show_where
){
	Vec3d * atom_pos    = (Vec3d*) atom_pos_;
	Vec3d * sampled_pos = (Vec3d*) sampled_pos_;
	int nx  = GRID::n.x;
	int ny  = GRID::n.y;
	int nz  = GRID::n.z;
	int nxy = ny * nx; // used in macro i3D( ia, ib, ic )
	Vec3d rProbe;  rProbe.set( 0.0, 0.0, 0.0 ); // we may shift here
	int points_found = 0;
	int nimg = 1; if (pbc) nimg = 27; // PBC ?
	for ( int ia=0; ia<nx; ia++ ){ 
		//printf( " ia %i \n", ia );
		rProbe.add( GRID::dCell.a );  
		for ( int ib=0; ib<ny; ib++ ){ 
			rProbe.add( GRID::dCell.b );
			for ( int ic=0; ic<nz; ic++ ){
				rProbe.add( GRID::dCell.c );
				bool withinRmin = false; 
				bool withinRmax = false; 
				for ( int iimg=0; iimg<nimg; iimg++ ){
					Vec3d cell_shift;
					cell_shift.set_lincomb( images[iimg][0], images[iimg][1], images[iimg][2], GRID::cell.a, GRID::cell.b, GRID::cell.c );
					for ( int iatom=0; iatom<natoms; iatom++ ){
						Vec3d dr;
						dr.set_sub( rProbe, atom_pos[iatom] );
						dr.sub( cell_shift ); 
						double r2   = dr.norm2();
						double rmin = atom_Rmin[iatom];
						double rmax = atom_Rmax[iatom];
						if( r2<(rmax*rmax) ){ 
							if( atom_mask[iatom] ){ withinRmax = true; }  
							if( r2<(rmin*rmin)   ){ withinRmin = true; break; }
						}
					}
				}
				if( withinRmax && (!withinRmin) ){
					if( canStore ){
						sampled_val[points_found] = GRID::grid[ i3D( ia, ib, ic ) ];
						if( show_where ) GRID::grid[ i3D( ia, ib, ic ) ] = +100.0d;
						sampled_pos[points_found].set( rProbe );
					}
					points_found++;
				}
			} 
			rProbe.add_mul( GRID::dCell.c, -nz );
		} 
		rProbe.add_mul( GRID::dCell.b, -ny );
	}
	return points_found;
}

// FUNCTION ======	draw
void draw(){
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); 

	Vec3d oldPos; oldPos.set( body.pos );

	//printVec( oldPos ); printf(" // oldPos \n");

/*
	printf("invIbody\n");
	printMat(rbody.invIbody);
	printf("invI\n");
	printMat(rbody.invI);
	printf("rotMat\n");
	printMat(rbody.rotMat);
	printQuat(rbody.qrot  ); printf("qrot\n");
	printVec(rbody.L    );   printf("L\n");
	printVec(rbody.omega);   printf("omega\n");
	printVec(rbody.pos  );   printf("pos\n");
*/

	physics();  // ALL PHYSICS COMPUTATION DONE HERE 

	
	camera ();

	//drawLine( oldPos, body.pos );
	glEnable (GL_LIGHTING);
	glShadeModel(GL_FLAT);
	rbody.render();

	rbody2.render();
	glColor3f( 0, 0, 0);  
	constrain1->render();

/*
	printf("invI\n");
	printMat(rbody.invI);
	printf("rotMat\n");
	printMat(rbody.rotMat);
	printQuat(rbody.qrot  ); printf("qrot\n");
	printVec(rbody.L    );   printf("L\n");
	printVec(rbody.omega);   printf("omega\n");
	printVec(rbody.pos  );   printf("pos\n");
	quit(); 	
*/

	drawAxis( 5 );


	glFinish();
	SDL_GL_SwapBuffers();
}

 void setupBlockWorld( const Vec3d& pos0_, const Vec3d& scaling_, const Mat3d& rot ){
     pos0   .set( pos0_    );
     scaling.set( scaling_ );
     buildRotations( rot );
     /*
     printf( " pos      (%3.3f,%3.3f,%3.3f) \n", pos0.x, pos0.y, pos0.z );
     //printf( " scaling_ (%3.3f,%3.3f,%3.3f) \n", scaling_.x, scaling_.y, scaling_.z );
     printf( " scaling  (%3.3f,%3.3f,%3.3f) \n", scaling.x,  scaling.y, scaling.z );
     printf( " a        (%3.3f,%3.3f,%3.3f) \n", rotations[0].a.x, rotations[0].a.y, rotations[0].a.z );
     printf( " b        (%3.3f,%3.3f,%3.3f) \n", rotations[0].b.x, rotations[0].b.y, rotations[0].b.z );
     printf( " c        (%3.3f,%3.3f,%3.3f) \n", rotations[0].c.x, rotations[0].c.y, rotations[0].c.z );
     */
 }

void setup(){
	int ifree,igl,nvert,ndiv;
	

	ray0.set( 1.0, 1.0, 1.0 );
	hRay.set( -1.0, -1.0, -1.0 );  hRay.normalize();

	a.set( 1.0, 0.0, 0.0 );
	b.set( 0.0, 1.0, 0.0 );
	c.set( 0.0, 0.0, 1.0 );


	bool inside =  false;
	double dist = rayTriangle(  ray0, hRay,   a,b,c,  inside, phit );
	if(inside){
		printf( " triagnle hit: TRUE   in dist: %f ", inside, dist );
	}else{
		printf( " triagnle hit: FALSE  in dist: %f ", inside, dist );
	}

	ifree = glObjects->getFree();
	igl = glGenLists(1);
	glObjects->data[ ifree ] = igl;
	glNewList( igl , GL_COMPILE );
		glColor3f( 0.9,0.9,0.9 );
		drawPointCross( a, 0.1 );
		drawPointCross( b, 0.1 );
		drawPointCross( c, 0.1 );
		drawTriangle( a, b, c );
		glColor3f( 0.1,0.1,0.1 );
		drawVecInPos  ( hRay*10, ray0 );
		drawPointCross( phit, 0.2 );
	glEndList();



}

//--------------------------------------------------------------------------------------------------
/// Transform the min and max coordinate with the given transformation matrix
//--------------------------------------------------------------------------------------------------
void BoundingBox::transform(const Mat4d& matrix)
{
	// Check if box is invalid, and don't transform if so
	if (!isValid()) return;

	BoundingBox newBox;
    newBox.reset();

    Vec3d node;
    
    node.set(m_min.x(), m_min.y(), m_min.z());
    node.transformPoint(matrix);
    newBox.add(node);

    node.set(m_max.x(), m_min.y(), m_min.z());
    node.transformPoint(matrix);
    newBox.add(node);

    node.set(m_max.x(), m_max.y(), m_min.z());
    node.transformPoint(matrix);
    newBox.add(node);

    node.set(m_min.x(), m_max.y(), m_min.z());
    node.transformPoint(matrix);
    newBox.add(node);

    node.set(m_min.x(), m_min.y(), m_max.z());
    node.transformPoint(matrix);
    newBox.add(node);

    node.set(m_max.x(), m_min.y(), m_max.z());
    node.transformPoint(matrix);
    newBox.add(node);

    node.set(m_max.x(), m_max.y(), m_max.z());
    node.transformPoint(matrix);
    newBox.add(node);

    node.set(m_min.x(), m_max.y(), m_max.z());
    node.transformPoint(matrix);
    newBox.add(node);

	*this = newBox;
}

    void drawForces( double scale, bool DEBUG ){
	    if( ( world.truss.points != NULL )&&( world.truss.forces != NULL ) ){
            glBegin( GL_LINES );
            for( int i=0; i<world.truss.npoints; i++ ){
                Vec3d& p  =  world.truss.points[i];
                Vec3d& f  =  world.truss.forces[i];
                Vec3d  pf; pf.set(p); pf.add_mul( f, scale );
                glVertex3f( (float)p.x, (float)p.y, (float)p.z );
                glVertex3f( (float)pf.x, (float)pf.y, (float)pf.z );
                if( DEBUG ){
                    //printf( " %i  %i %i   (%3.3f,%3.3f,%3.3f)  (%3.3f,%3.3f,%3.3f)\n",  i,   bond.i, bond.j,  pi.x, pi.y, pi.z,  f.x, f.y, f.z  );
                    printf( " %i  (%3.3f,%3.3f,%3.3f) \n",  i, f.x, f.y, f.z  );
                }
            }
            glEnd();
	    }
	}

bool CEvent_UpdatePositionParametricExpression::Update(CEnvironment* p_env)
{	 
	float x,y,z;
	m_expression_variables_x.mv_variable[0].m_value= p_env->get_elapsed_time();
	x = m_expression_list_x.evaluate(m_expr_x_index);
	m_expression_variables_y.mv_variable[0].m_value= p_env->get_elapsed_time();
	y = m_expression_list_y.evaluate(m_expr_y_index);
	m_expression_variables_z.mv_variable[0].m_value= p_env->get_elapsed_time();
	z = m_expression_list_z.evaluate(m_expr_z_index);

	Vec3d vec;
	vec.set(x,y,z);
	CPositionAttitudeTransform *p_PAT = p_env->get_PAT(m_index_PAT);
	if(p_PAT!= NULL) p_PAT->mp_PAT->setPosition(vec);
	
	return true;
}

 void drawBlock( Uint16 shape, Uint8 orientation, Uint8 ix, Uint8 iy, Uint8 iz ){
     Mat3d rotmat = rotations[ orientation & 0b00011111 ];
     rotmat.a.mul( scaling.x );
     rotmat.b.mul( scaling.y );
     rotmat.c.mul( scaling.z );
     if (orientation & 0b10000000) rotmat.a.mul( -1.0d );
     if (orientation & 0b01000000) rotmat.b.mul( -1.0d );
     if (orientation & 0b00100000) rotmat.c.mul( -1.0d );
     Vec3d pos; pos.set( ix*scaling.x+pos0.x, iy*scaling.y+pos0.y, iz*scaling.z+pos0.z );
     float glMat[16];
     glPushMatrix ( );
     /*
     printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos.x, pos.y, pos.z );
     printf( " a   (%3.3f,%3.3f,%3.3f) \n", rotmat.a.x, rotmat.a.x, rotmat.a.x );
     printf( " b   (%3.3f,%3.3f,%3.3f) \n", rotmat.b.x, rotmat.b.x, rotmat.b.x );
     printf( " c   (%3.3f,%3.3f,%3.3f) \n", rotmat.c.x, rotmat.c.x, rotmat.c.x );
     */
     Draw3D::toGLMat( pos, rotmat, glMat );
     glMultMatrixf( glMat );
     glCallList  ( shape );
     glPopMatrix ( );
 }

 void getPlaneWaveDescriptor( double * center_, int np, double * points, int nk,  double * ks, double * coefs ){
     Vec3d center; center.set( center_[0], center_[1], center_[2] );
     return getPlaneWaveDescriptor( center, np, (Vec3d*)points, nk,  (Vec3d*)ks, coefs );
 }

void Yiq::getYIQ(Vec3d& hhh) const
{
    hhh.set(yiq[Y],yiq[I],yiq[Q]);
}

void Gun::set_direction( Vec3d dir ){
	Vec3d up; up.set( 0.0d, 0.0d, 1.0d );
	lrot.a.set( dir );
	lrot.c.set_cross( lrot.a, up     );
	lrot.b.set_cross( lrot.c, lrot.a );
}

void Atmosphere::computeColor
	(double JD, Vec3d sunPos, Vec3d moonPos, float moonPhase, StelCore* core, float eclipseFac,
	 float latitude, float altitude, float temperature, float relativeHumidity)
{
	// We lazily initialize vertex buffer at the first draw, 
	// so we can only call this after that.
	// We also need a renderer reference (again lazily from draw()) to 
	// construct index buffers as they might change at every call to computeColor().
	if(NULL == renderer) {return;}

	const StelProjectorP prj = core->getProjection(StelCore::FrameAltAz, StelCore::RefractionOff);
	if (viewport != prj->getViewport())
	{
		// The viewport changed: update the number of points in the grid
		updateGrid(prj);
	}

	eclipseFactor = eclipseFac;
	if(eclipseFac < 0.0001f)
		eclipseFactor = 0.0001f;

	// No need to calculate if not visible
	if (!fader.getInterstate())
	{
		averageLuminance = 0.001f + lightPollutionLuminance;
		return;
	}

	// Calculate the atmosphere RGB for each point of the grid
	if (myisnan(sunPos.length()))
		sunPos.set(0.,0.,-1.*AU);
	if (myisnan(moonPos.length()))
		moonPos.set(0.,0.,-1.*AU);

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

	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.setParamsv(sun_pos, 5.f);

	skyb.setLocation(latitude * M_PI/180., altitude, temperature, relativeHumidity);
	skyb.setSunMoon(moon_pos[2], sun_pos[2]);

	// Calculate the date from the julian day.
	int year, month, day;
	StelUtils::getDateFromJulianDay(JD, &year, &month, &day);
	skyb.setDate(year, month, moonPhase);

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

	Vec3d point(1., 0., 0.);
	skylightStruct2 b2;
	float lumi;

	vertexGrid->unlock();
	// Compute the sky color for every point above the ground
	for (int i=0; i<(1+skyResolutionX)*(1+skyResolutionY); ++i)
	{
		const Vec2f position = vertexGrid->getVertex(i).position;
		prj->unProject(position[0], position[1], point);

		Q_ASSERT(fabs(point.lengthSquared()-1.0) < 1e-10);

		if (point[2]<=0)
		{
			point[2] = -point[2];
			// The sky below the ground is the symmetric of the one above :
			// it looks nice and gives proper values for brightness estimation
		}

		// Use the Skybright.cpp 's models for brightness which gives better results.
		lumi = skyb.getLuminance(moon_pos[0]*point[0]+moon_pos[1]*point[1]+moon_pos[2]*point[2], 
								 sun_pos[0]*point[0]+sun_pos[1]*point[1]+sun_pos[2]*point[2],
		                         point[2]);
		lumi *= eclipseFactor;
		// Add star background luminance
		lumi += 0.0001;
		// Multiply by the input scale of the ToneConverter (is not done automatically by the xyYtoRGB method called later)
		//lumi*=eye->getInputScale();

		// Add the light pollution luminance AFTER the scaling to avoid scaling it because it is the cause
		// of the scaling itself
		lumi += lightPollutionLuminance;

		// Store for later statistics
		sum_lum+=lumi;

		Q_ASSERT_X(NULL != vertexGrid, Q_FUNC_INFO, 
		           "Vertex buffer not initialized when setting colors");

//.........这里部分代码省略.........