C++ Vec3d函数代码示例

Vec3d StelMovementMgr::mountFrameToJ2000(const Vec3d& v) const
{
	switch (mountMode)
	{
		case MountAltAzimuthal:
			return core->altAzToJ2000(v, StelCore::RefractionOff);
		case MountEquinoxEquatorial:
			return core->equinoxEquToJ2000(v);
		case MountGalactic:
			return core->galacticToJ2000(v);
	}
	Q_ASSERT(0);
	return Vec3d(0);
}

SoundSource::SoundSource(double nearClip, double farClip, AttenuationLaw law, DopplerType dopplerType,
        double farBias, int delaySize
        )
	:	DistAtten<double>(nearClip, farClip, law, farBias),
      mSound(delaySize), mUseAtten(true), mDopplerType(dopplerType), mUsePerSampleProcessing(false)
{

	// initialize the position history to be VERY FAR AWAY so that we don't deafen ourselves...
	for(int i=0; i<mPosHistory.size(); ++i){
		mPosHistory(Vec3d(1000, 0, 0));
	}

	presenceFilter.set(2700);
}

Vec3d CameraController::switchCoordinateSystem_vector( const Vec3d& vec, const Matrixd& from, const Matrixd& to )
{
	Matrixd mat;
	mat.invert(to);

	Vec3d begin = Vec3d(0, 0, 0);
	Vec3d end = vec;

	Vec3d begin2 = begin * from * mat;
	Vec3d end2 = end * from * mat;

	Vec3d v = end2 - begin2;
	return v;
}

void MassPointSystem::buildDoubleTetrahedron(const Vec3d &center, const Vec3d &vel, const Vec3d &angVel)
{
    massPointVector.push_back(MassPoint(center + Vec3d(0.0, 0.0, 4.082), vel + angVel.cross(Vec3d(0.0, 0.0, 4.082))));
    massPointVector.push_back(MassPoint(center + Vec3d(-1.443, -2.5, 0.0), vel + angVel.cross(Vec3d(-1.443, -2.5, 0.0))));
    massPointVector.push_back(MassPoint(center + Vec3d(-1.443, 2.5, 0.0), vel + angVel.cross(Vec3d(-1.443, 2.5, 0.0))));
    massPointVector.push_back(MassPoint(center + Vec3d(2.886, 0.0, 0.0), vel + angVel.cross(Vec3d(2.886, 0.0, 0.0))));
    massPointVector.push_back(MassPoint(center + Vec3d(0.0, 0.0, -4.082), vel + angVel.cross(Vec3d(0.0, 0.0, -4.082))));

    jointVector.push_back(new SpringDamperJoint(0, 1, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(0, 2, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(0, 3, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(1, 2, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(1, 3, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(1, 4, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(2, 3, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(2, 4, &massPointVector));
    jointVector.push_back(new SpringDamperJoint(3, 4, &massPointVector));
    for (unsigned int i = 0; i < massPointVector.size(); ++i)
    {
        jointVector.push_back(new GroundContactJoint(i));
        jointVector.push_back(new GravityJoint(i));
    }
}

Vec3d Camera::collisionPointOnPlane(
const Mat4x4f &proj, const Mat4x4f &modelview, const Vec2i &resolution,
const Vec2i &screen_point, Vec3f planeNormal) const
{
	Vec3f start = screenSpacePointTo3DRay(
		proj, modelview, resolution, screen_point, 0.0);
	Vec3f end = screenSpacePointTo3DRay(
		proj, modelview, resolution, screen_point, 1.0);

	Vec3f line = start - end;
	float len = (-start).dot(planeNormal / line.dot(planeNormal));
	Vec3f i = start + (line * len);
	return Vec3d(i.x, i.y, i.z);
}

void MeshPointSet::BeforeStep(double deltaTime)
{
	MaterialPointSet::BeforeStep(deltaTime);
			
	for(int i=0; i < m_count; i++) m_stiffnessForces[i] = Vec3d(0);
	if (m_s == 0) return;

	for(int ix = 1; ix < m_Nx - 1; ++ix)
		for(int iy = 1; iy < m_Ny - 1; ++iy)
		{
			int id = ix + m_Nx * iy;
					
			Vec3d toNeighbours = Vec3d(0);
			for(int dir = 0; dir < 8; dir += 1)
			{
				int nxNeighbour = ix + g_vecDir[dir].x;
				int nyNeighbour = iy + g_vecDir[dir].y;				
				int neighbourID = nxNeighbour + nyNeighbour*m_Nx;
				if(nxNeighbour >= 0 && nxNeighbour < m_Nx && nyNeighbour >= 0 && nyNeighbour < m_Ny)
				{					
					Vec3d diff = m_points[neighbourID].m_pos - m_points[id].m_pos;
					toNeighbours+=diff;
				}						
			}
			toNeighbours *= (1.0/8.0);
			Vec3d force = m_s * toNeighbours;

			m_stiffnessForces[id] += force;
			for(int dir = 0; dir < 8; dir += 1)
			{
				int nxNeighbour = ix + g_vecDir[dir].x;
				int nyNeighbour = iy + g_vecDir[dir].y;				
				int neighbourID = nxNeighbour + nyNeighbour*m_Nx;
				m_stiffnessForces[neighbourID] -= force*(1.0/8.0);
			}
		}
}

   // automatically generates edges:
   DLL_HEADER Ng_Result Ng_STL_MakeEdges (Ng_STL_Geometry * geom,
                                          Ng_Mesh* mesh,
                                          Ng_Meshing_Parameters * mp)
   {
      STLGeometry* stlgeometry = (STLGeometry*)geom;
      Mesh* me = (Mesh*)mesh;

      // Philippose - 27/07/2009
      // Do not locally re-define "mparam" here... "mparam" is a global 
      // object 
      //MeshingParameters mparam;
      mp->Transfer_Parameters();

      me -> SetGlobalH (mparam.maxh);
      me -> SetLocalH (stlgeometry->GetBoundingBox().PMin() - Vec3d(10, 10, 10),
                       stlgeometry->GetBoundingBox().PMax() + Vec3d(10, 10, 10),
                       0.3);

      me -> LoadLocalMeshSize (mp->meshsize_filename);
      /*
      if (mp->meshsize_filename)
      {
      ifstream infile (mp->meshsize_filename);
      if (!infile.good()) return NG_FILE_NOT_FOUND;
      me -> LoadLocalMeshSize (infile);
      }
      */

      STLMeshing (*stlgeometry, *me);

      stlgeometry->edgesfound = 1;
      stlgeometry->surfacemeshed = 0;
      stlgeometry->surfaceoptimized = 0;
      stlgeometry->volumemeshed = 0;

      return NG_OK;
   }

void StelMovementMgr::panView(const double deltaAz, const double deltaAlt)
{
	// The function is called in update loops, so make a quick check for exit.
	if ((deltaAz==0.) && (deltaAlt==0.))
		return;

	double azVision, altVision;
	StelUtils::rectToSphe(&azVision,&altVision,j2000ToMountFrame(viewDirectionJ2000));
	// Az is counted from South, eastward.

	// qDebug() << "Azimuth:" << azVision * 180./M_PI << "Altitude:" << altVision * 180./M_PI << "Up.X=" << upVectorMountFrame.v[0] << "Up.Y=" << upVectorMountFrame.v[1] << "Up.Z=" << upVectorMountFrame.v[2];

	// if we are just looking into the pole, azimuth can hopefully be recovered from the customized up vector!
	// When programmatically centering on a pole, we should have set a better up vector for |alt|>0.9*M_PI/2.
	if (fabs(altVision)> 0.95* M_PI/2.)
	{
		if (upVectorMountFrame.v[2] < 0.9)
		{
			// qDebug() << "Recovering azimuth...";
			azVision=atan2(-upVectorMountFrame.v[1], -upVectorMountFrame.v[0]);
			if (altVision < 0.)
				azVision+=M_PI;
		}
//		else
//		{
//			// qDebug() << "UpVector:" << upVectorMountFrame.v[0] << "/" << upVectorMountFrame.v[1] << "/" << upVectorMountFrame.v[2] << "Cannot recover azimuth. Hope it's OK";
//		}
	}

	// if we are moving in the Azimuthal angle (left/right)
	if (deltaAz)
		azVision-=deltaAz;
	if (deltaAlt)
	{
		if (altVision+deltaAlt <= M_PI_2 && altVision+deltaAlt >= -M_PI_2) altVision+=deltaAlt;
		if (altVision+deltaAlt > M_PI_2) altVision = M_PI_2 - 0.000001;		// Prevent bug
		if (altVision+deltaAlt < -M_PI_2) altVision = -M_PI_2 + 0.000001;	// Prevent bug
	}

	// recalc all the position variables
	if (deltaAz || deltaAlt)
	{
		setFlagTracking(false);
		Vec3d tmp;
		StelUtils::spheToRect(azVision, altVision, tmp);
		setViewDirectionJ2000(mountFrameToJ2000(tmp));
		setViewUpVector(Vec3d(0., 0., 1.)); // We ensured above that view vector is never parallel to this simple up vector.
	}
}

  int Ng_STLCalcLocalH  (ClientData clientData,    
			 Tcl_Interp * interp,
			 int argc, tcl_const char *argv[])
  {
    for (int i = 0; i < geometryregister.Size(); i++)
      geometryregister[i] -> SetParameters (interp);


    Ng_SetMeshingParameters (clientData, interp, argc, argv);

    STLGeometry * stlgeometry = dynamic_cast<STLGeometry*> (ng_geometry);
    if (mesh.Ptr() && stlgeometry)
      {
	mesh -> SetLocalH (stlgeometry->GetBoundingBox().PMin() - Vec3d(10, 10, 10),
			   stlgeometry->GetBoundingBox().PMax() + Vec3d(10, 10, 10),
			   mparam.grading);
	stlgeometry -> RestrictLocalH(*mesh, mparam.maxh);

	if (stlparam.resthsurfmeshcurvenable)
	  mesh -> CalcLocalHFromSurfaceCurvature (stlparam.resthsurfmeshcurvfac);
      }

    return TCL_OK;
  }

// read a correspondence in history
bool
MyGlWindow::read_correspondence ( int frameid, int lineid, EdgePlane *ep )
{
	char filename[256];
	sprintf(filename, "correspondences/%d.dat", frameid);
	
	FILE *f = fopen( filename, "r" );
	
	if ( !f )
		return false;
	
	double s0,s1,s2,a0,a1,a2,b0,b1,b2;
	int camera_id, line_status,line_id;
	
	while ( fscanf(f, "%d%d%d%lf%lf%lf%lf%lf%lf%lf%lf%lf", &line_id, &line_status, &camera_id,
		&s0, &s1, &s2, &a0, &a1, &a2, &b0, &b1, &b2 ) == 12 ) {
		
		if ( line_id == lineid ) {
			ep->_a = Vec3d(a0,a1,a2);
			ep->_b = Vec3d(b0,b1,b2);
			ep->_s = Vec3d(s0,s1,s2);
			ep->_cameraId = camera_id;		
			
			ep->_normal = norm(cross(norm(ep->_a),norm(ep->_b)));
			ep->_uid = 0;
			ep->_length = ep->length();
			
			fclose(f);
			
			return true;
		}
	}
	
	fclose (f);
	return false;
}

  // automatically generates edges:
DLL_HEADER Ng_Result Ng_STL_MakeEdges (Ng_STL_Geometry * geom,
		                                 Ng_Mesh* mesh,
		                                 Ng_Meshing_Parameters * mp)
{
  STLGeometry* stlgeometry = (STLGeometry*)geom;
  Mesh* me = (Mesh*)mesh;
  
  MeshingParameters mparam;

  mparam.maxh = mp->maxh;
  mparam.meshsizefilename = mp->meshsize_filename;

  me -> SetGlobalH (mparam.maxh);
  me -> SetLocalH (stlgeometry->GetBoundingBox().PMin() - Vec3d(10, 10, 10),
		   stlgeometry->GetBoundingBox().PMax() + Vec3d(10, 10, 10),
		   0.3);

  me -> LoadLocalMeshSize (mp->meshsize_filename);
  /*
  if (mp->meshsize_filename)
    {
      ifstream infile (mp->meshsize_filename);
      if (!infile.good()) return NG_FILE_NOT_FOUND;
      me -> LoadLocalMeshSize (infile);
    }
  */

  STLMeshing (*stlgeometry, *me);
  
  stlgeometry->edgesfound = 1;
  stlgeometry->surfacemeshed = 0;
  stlgeometry->surfaceoptimized = 0;
  stlgeometry->volumemeshed = 0;
  
  return NG_OK;
}

void GLGizmoRotate::render_grabber(const BoundingBoxf3& box) const
{
    double grabber_radius = (double)m_radius * (1.0 + (double)GrabberOffset);
    m_grabbers[0].center = Vec3d(::cos(m_angle) * grabber_radius, ::sin(m_angle) * grabber_radius, 0.0);
    m_grabbers[0].angles(2) = m_angle;

    glsafe(::glColor3fv((m_hover_id != -1) ? m_drag_color : m_highlight_color));

    ::glBegin(GL_LINES);
    ::glVertex3f(0.0f, 0.0f, 0.0f);
    ::glVertex3dv(m_grabbers[0].center.data());
    glsafe(::glEnd());

    ::memcpy((void*)m_grabbers[0].color, (const void*)m_highlight_color, 3 * sizeof(float));
    render_grabbers(box);
}

// return the average camera position over the last n frames
//
Vec3d MyGlWindow::get_average_position( int n )
{

	Vec3d t = Vec3d(0,0,0);

	int counter = 0;
	for (int i=pose_history.size()-1;i>=MAX(0,pose_history.size()-n);i--) {
		t = t + pose_history[i].getTranslation();
		counter++;
	}

	if ( counter > 0 ) 
		t = t / counter;

	return t;
}

void TestStelSphericalGeometry::testGreatCircleIntersection()
{
	Vec3d v0,v1,v2,v3;
	double deg5 = 5.*M_PI/180.;
	StelUtils::spheToRect(-deg5, -deg5, v3);
	StelUtils::spheToRect(+deg5, -deg5, v2);
	StelUtils::spheToRect(+deg5, +deg5, v1);
	StelUtils::spheToRect(-deg5, +deg5, v0);

	bool ok;
	Vec3d v(0);
	QBENCHMARK {
		v = greatCircleIntersection(v3, v1, v0, v2, ok);
	}
	QVERIFY(v.angle(Vec3d(1.,0.,0.))<0.00001);
}

        inline Vec3d barycentric_coordinates(const Vec3d& P, const Vec3d& A, const Vec3d& B, const Vec3d& C){
            Vec3d v0 = B - A, v1 = C - A, v2 = P - A;

            double d00 = dot(v0, v0);
            double d01 = dot(v0, v1);
            double d02 = dot(v0, v2);
            double d11 = dot(v1, v1);
            double d12 = dot(v1, v2);

            double w = (d00 * d12 - d01 * d02) / (d00 * d11 - d01 * d01);
            double v = (d00 >= d01)? (d02 - d01 * w) / d00: (d12 - d11 * w) / d01;
            //double v = (d00 >= d01)? d02 / d00 - (d01 / d00) * w: d12 / d01 - (d11 / d01) * w;
            double u = 1.0 - v - w;

            return Vec3d(u, v, w);
        }