Example #1
0
void CLinearSolverGauss<T>::Linsolve(MatrixNxN<T> &A, VectorN<T> &b, VectorN<T> &x)
{
	int n = A.m_iN;
	int k=0;

	int p;
	for(;k<n-1;k++)
	{
		p=k;

		//check for largest |a_ij| as pivot
		for(int i=k+1;i<n;i++)
		{
			if(fabs(A(i,k)) > fabs(A(k,k)))
			{
				//swap rows if we find a bigger element
				A.SwapRows(k,i);
				T temp = b(k);
				b(k)=b(i);
				b(i)=temp;
			}
		}
		//check if there are only zero pivots
		while((A(p,k)==0.0) && (p<n))
		{
			p++;
		}

		if(p==n)
		{
			std::cout<<"No solution exists..."<<std::endl;
			return;
		}
		else
		{
			if(p !=k )
			{
				A.SwapRows(k,p);
				T temp = b(k);
				b(k)=b(p);
				b(p)=temp;
			}
		}
		
		for(int i=k+1;i<n;i++)
		{
			T p = A(i,k);
			T pivot = A(i,k)/A(k,k);

			//reduce the k+1-row
			for(int j=0;j<n;j++)
				A(i,j)=A(i,j) - pivot * A(k,j);

			//reduce the rhs
			b(i) = b(i) - pivot * b(k);
		}//end for i
	}//end for k

	if(A(n-1,n-1) == 0)
	{
		std::cout<<"No solution exists..."<<std::endl;
		return;
	}

	//backwards substitution
	for(int i=n-1;i>=0;i--)
	{
		T Sum = 0;
		x(i)=0;
		for(int j=i; j<=n-1;j++)
		{
			Sum += A(i,j) * x(j);
		}
		x(i)=(b(i)-Sum)/A(i,i);
	}

}
Example #2
0
void
RCM2Material :: giveEffectiveMaterialStiffnessMatrix(FloatMatrix &answer,
                                                     MatResponseForm form,
                                                     MatResponseMode rMode, GaussPoint *gp,
                                                     TimeStep *atTime)
//
// returns effective material stiffness matrix in full form
// for gp stress strain mode
//
{
    RCM2MaterialStatus *status = ( RCM2MaterialStatus * ) this->giveStatus(gp);
    StructuralMaterial *lMat = static_cast< StructuralMaterial * >( this->giveLinearElasticMaterial() );
    int numberOfActiveCracks = status->giveNumberOfTempActiveCracks();
    int i, j, indi, indj, ii, jj;
    double G, princStressDis, princStrainDis;
    FloatMatrix de, invDe, compliance, dcr, d, df, t, tt, tempCrackDirs;
    FloatArray principalStressVector, principalStrainVector;
    IntArray mask;

    if ( ( rMode == ElasticStiffness ) || ( numberOfActiveCracks == 0 ) ) {
        lMat->giveCharacteristicMatrix(answer, form, rMode, gp, atTime);
        return;
    }

    // this->updateActiveCrackMap(gp) must be done after restart.
    this->updateActiveCrackMap(gp);
    status->giveTempCrackDirs(tempCrackDirs);
    this->giveNormalElasticStiffnessMatrix(de, ReducedForm, rMode, gp, atTime,
                                           tempCrackDirs);
    invDe.beInverseOf(de);
    this->giveCrackedStiffnessMatrix(dcr, rMode, gp, atTime);
    this->giveStressStrainMask( mask, ReducedForm, gp->giveMaterialMode() );
    compliance.resize( mask.giveSize(), mask.giveSize() );

    // we will set
    // first we set compliances for normal streses in
    // local coordinate system defined by crackplane
    for ( i = 1; i <= 3; i++ ) {
        if ( ( indi = this->giveStressStrainComponentIndOf(FullForm, gp->giveMaterialMode(), i) ) ) {
            for ( j = 1; j <= 3; j++ ) {
                if ( ( indj = this->giveStressStrainComponentIndOf(FullForm, gp->giveMaterialMode(), j) ) ) {
                    compliance.at(indi, indj) += invDe.at(i, j);
                }
            }

            if ( status->isCrackActive(i) ) {
                if ( dcr.at(i, i) <= 1.e-8 ) {
                    compliance.at(indi, indi) *= rcm2_BIGNUMBER;
                } else {
                    compliance.at(indi, indi) += 1. / dcr.at(i, i);
                }
            }
        }
    }

    status->getPrincipalStressVector(principalStressVector);
    status->getPrincipalStrainVector(principalStrainVector);

    // now remain to set shears
    G = this->give(pscm_G, gp);
    for ( i = 4; i <= 6; i++ ) {
        if ( ( indi = this->giveStressStrainComponentIndOf(FullForm, gp->giveMaterialMode(), i) ) ) {
            if ( i == 4 ) {
                ii = 2;
                jj = 3;
            } else if ( i == 5 ) {
                ii = 1;
                jj = 3;
            } else {
                ii = 1;
                jj = 2;
            }

            princStressDis = principalStressVector.at(ii) -
                             principalStressVector.at(jj);
            princStrainDis = principalStrainVector.at(ii) -
                             principalStrainVector.at(jj);
            if ( fabs(princStrainDis) < rcm_SMALL_STRAIN ) {
                compliance.at(indi, indi) = 1. / G;
            } else if ( fabs(princStressDis) < 1.e-8 ) {
                compliance.at(indi, indi) = rcm2_BIGNUMBER;
            } else {
                compliance.at(indi, indi) = 2 * princStrainDis / princStressDis;
            }
        }
    }

    // now we invert compliance to get stiffness in reduced space
    d.beInverseOf(compliance);
    // delete compliance;
    //
    // now let d to grow to Full Format
    //
    this->giveStressStrainMask( mask, ReducedForm, gp->giveMaterialMode() );
    df.beSubMatrixOfSizeOf(d, mask, 6);
    //
    // final step - transform stiffnes to global c.s
    //

    this->giveStressVectorTranformationMtrx(t, tempCrackDirs, 1);
    tt.beTranspositionOf(t);
    df.rotatedWith(tt);


    if ( form == FullForm ) {
        answer = df;
    } else { // reduced form asked
        this->giveStressStrainMask( mask, FullForm, gp->giveMaterialMode() );
        answer.beSubMatrixOf(df, mask);
    }
}
void SciFiClusterMaker::run(TestBeamEvent *event, Clipboard *clipboard)
{
    double pcut=0.;
    double residCut=parameters->residualmaxx;
    //std::cout << " residCut is " << residCut;
    TestBeamTracks* tracks=(TestBeamTracks*)clipboard->get("Tracks");
    if(!tracks) return;
    int nTracks=0;
    int timeStampedTracks=0;
    int timeStampedSciFi=0;
    //std::vector<SciFiCluster*>* scifiClusters = new std::vector<SciFiCluster*>;
    double prevfibreTrackerClock=0;
    double prevtimeAfterShutterOpen=0;

    for(TestBeamTracks::iterator it=tracks->begin(); it<tracks->end(); it++) {
        nTracks++;
        TestBeamTrack* track= *it;
        TDCTrigger *itr = track->tdcTrigger();
        double currentfibreTrackerClock=0;

        if(!itr)continue;
        timeStampedTracks++;

        std::string chip="SciFi";

        //get intersection point of track with that sensor
        TestBeamTransform* mytransform = new TestBeamTransform(parameters->alignment[chip]->displacementX(),
                parameters->alignment[chip]->displacementY(),
                parameters->alignment[chip]->displacementZ(),
                parameters->alignment[chip]->rotationX(),
                parameters->alignment[chip]->rotationY(),
                parameters->alignment[chip]->rotationZ());

        PositionVector3D< Cartesian3D<double> > planePointLocalCoords(0,0,0);
        PositionVector3D< Cartesian3D<double> > planePointGlobalCoords = (mytransform->localToGlobalTransform())*planePointLocalCoords;
        PositionVector3D< Cartesian3D<double> > planePoint2LocalCoords(0,0,1);
        PositionVector3D< Cartesian3D<double> > planePoint2GlobalCoords = (mytransform->localToGlobalTransform())*planePoint2LocalCoords;

        float normal_x=planePoint2GlobalCoords.X()-planePointGlobalCoords.X();
        float normal_y=planePoint2GlobalCoords.Y()-planePointGlobalCoords.Y();
        float normal_z=planePoint2GlobalCoords.Z()-planePointGlobalCoords.Z();

        float length=((planePointGlobalCoords.X()-track->firstState()->X())*normal_x+
                      (planePointGlobalCoords.Y()-track->firstState()->Y())*normal_y+
                      (planePointGlobalCoords.Z()-track->firstState()->Z())*normal_z)/
                     (track->direction()->X()*normal_x+track->direction()->Y()*normal_y+track->direction()->Z()*normal_z);
        float x_inter=track->firstState()->X()+length*track->direction()->X();
        float y_inter=track->firstState()->Y()+length*track->direction()->Y();
        float z_inter=track->firstState()->Z()+length*track->direction()->Z();


        //change to local coordinates of that plane
        PositionVector3D< Cartesian3D<double> > intersect_global(x_inter,y_inter,z_inter);
        PositionVector3D< Cartesian3D<double> > intersect_local = (mytransform->globalToLocalTransform())*intersect_global;



        SciFiCluster* cluster;

        bool current_cluster=false;
        int prev_fibreTrackChannel=0;

        if(itr->nFibreTrackerHits()!=0) {
            timeStampedSciFi++;
        }

        for(int ih=0; ih<itr->nFibreTrackerHits(); ih++) {


            if((itr)->fibreTrackerADC(ih)<pcut) {
                continue;
            }

            if( abs((itr)->fibreTrackerChannel(ih)-89) < 3 ||
                    abs((itr)->fibreTrackerChannel(ih)-65) < 3 ||
                    abs((itr)->fibreTrackerChannel(ih)-67) < 3 )continue;


            channel->Fill((itr)->fibreTrackerChannel(ih));
            channel_ADC->Fill((itr)->fibreTrackerADC(ih));

            if(m_debug) {
                std::cout<<"HIT! chan: "<<(itr)->fibreTrackerChannel(ih)<<" = "<<(itr)->fibreTrackerADC(ih)<<std::endl;
            }
            if (m_debug) std::cout << "sci fi hit " << ih << " channel " << (itr)->fibreTrackerChannel(ih) << " clock " << (itr)->fibreTrackerClock(ih) << std::endl;
            currentfibreTrackerClock=(itr)->fibreTrackerClock(ih);
            if (m_debug) std::cout << " currentfibreTrackerClock updated to " << currentfibreTrackerClock << std::endl;

            scifisyncdelay->Fill((itr)->syncDelay());

            if(!current_cluster) {
                cluster = new SciFiCluster();
                cluster->addHit((itr)->fibreTrackerChannel(ih),(itr)->fibreTrackerADC(ih));
                if (m_debug)
                    std::cout<<"Making new cluster"<<std::endl;
                current_cluster=true;
                prev_fibreTrackChannel=(itr)->fibreTrackerChannel(ih);
                continue;
            }

            if(abs((itr)->fibreTrackerChannel(ih)-prev_fibreTrackChannel) < 3) {
                if(m_debug)
                    std::cout<<"Adding hit to cluster"<<std::endl;
                cluster->addHit((itr)->fibreTrackerChannel(ih),(itr)->fibreTrackerADC(ih));
                prev_fibreTrackChannel=(itr)->fibreTrackerChannel(ih);
            }
            else {
                if(m_debug)
                    std::cout<<"Saving previous cluster and making new one"<<std::endl;
                cluster_ADC->Fill(cluster->getADC());
                cluster_size->Fill(cluster->getClusterSize());
                setClusterCentre(cluster);
                if (m_debug)
                    std::cout << " cluster position " << (cluster)->getPosition() << " cluster_ADC " << cluster->getADC() << " cluster_size " << cluster->getClusterSize() << std::endl;
                if(fabs(intersect_local.X()-(cluster)->getPosition())<residCut) {
                    residualPlot->Fill(intersect_local.X()-(cluster)->getPosition());
                    scifiresidvschan->Fill((cluster)->getPosition()/(-0.25),intersect_local.X()-(cluster)->getPosition());
                    if (m_debug) std::cout << " Adding scifi alignment cluster to track " << std::endl;

                    if (((cluster)->getPosition()/(-0.25))<61.) {
                        track->addSciFiAlignmentClusterToTrack(cluster);
                    }
                }
                correlation_x->Fill(intersect_local.X()-(cluster)->getPosition());
                //scifiClusters->push_back(cluster);
                cluster = new SciFiCluster();
                cluster->addHit((itr)->fibreTrackerChannel(ih),(itr)->fibreTrackerADC(ih));
                prev_fibreTrackChannel=(itr)->fibreTrackerChannel(ih);
            }
        }

        if(current_cluster) {
            setClusterCentre(cluster);
            if(fabs(intersect_local.X()-(cluster)->getPosition())<residCut) {
                residualPlot->Fill(intersect_local.X()-(cluster)->getPosition());
                scifiresidvschan->Fill((cluster)->getPosition()/(-0.25),intersect_local.X()-(cluster)->getPosition());
                if (((cluster)->getPosition()/(-0.25))<61.) {
                    track->addSciFiAlignmentClusterToTrack(cluster);
                }
            }
            if (m_debug) std::cout<<"Saving current cluster"<<std::endl;
            if (m_debug) std::cout << " cluster position " << (cluster)->getPosition() << " cluster_ADC " << cluster->getADC() << " cluster_size " << cluster->getClusterSize() << std::endl;
            correlation_x->Fill(intersect_local.X()-(cluster)->getPosition());
//				scifiClusters->push_back(cluster);
            cluster_ADC->Fill(cluster->getADC());
            cluster_size->Fill(cluster->getClusterSize());
        }

        if (prevtimeAfterShutterOpen>0.&&currentfibreTrackerClock>0.) {
            if (m_debug) std::cout << " Filling scifitime resid: Previous shutter time " << prevtimeAfterShutterOpen << " current shutter time " << (itr)->timeAfterShutterOpen() << " previous fibre trackerclock " << prevfibreTrackerClock << " current fibre tracker clock " << currentfibreTrackerClock << std::endl;
            scifitimeresid->Fill(((itr)->timeAfterShutterOpen()-prevtimeAfterShutterOpen)-(currentfibreTrackerClock-prevfibreTrackerClock));
        }
        if (currentfibreTrackerClock==0) {
            prevtimeAfterShutterOpen=0.;
        } else {
            prevtimeAfterShutterOpen=(itr)->timeAfterShutterOpen();
            prevfibreTrackerClock=currentfibreTrackerClock;
        }
        //place track correlation here based on time stamp on track


    }

    timeStampedTracksPlot->Fill(timeStampedTracks);
    nTracksPlot->Fill(nTracks);

    nTracksPlot_perEvent->Fill(eventNum,nTracks);
    timeStampedTracksPlot_perEvent->Fill(eventNum,timeStampedTracks);

    timeStampedSciFiPlot_perEvent->Fill(eventNum,timeStampedSciFi);
    //*/





    int timeStampedSciFi2=0;

    for(unsigned int j=0; j<event->nTDCElements(); j++) {
        TestBeamEventElement *ele=event->getTDCElement(j);
        std::string chip = ele->detectorId();
        if(chip!=std::string("SciFi")) continue;
        TDCFrame* frame = (TDCFrame*)ele;

        TDCTriggers* triggers=frame->triggers();

        for(TDCTriggers::iterator itr=triggers->begin(); itr!=triggers->end(); itr++) {

            if((*itr)->nFibreTrackerHits()!=0) {
                timeStampedSciFi2++;
            }
        }
    }

    timeStampedSciFiPlot2_perEvent->Fill(eventNum,timeStampedSciFi2);





    /*

    	if(parameters->dut != "SciFi") return;
      if(event->nTDCElements() == 0) return;
      if(m_debug) std::cout << "In SciFiClusterMaker"<<std::endl;
    	double pcut=20.;
      std::vector<SciFiCluster*>* scifiClusters = new std::vector<SciFiCluster*>;
      scifiClusters->clear();
      for(unsigned int j=0;j<event->nTDCElements();j++){
        TestBeamEventElement *ele=event->getTDCElement(j);
        std::string chip = ele->detectorId();
        if(chip!=std::string("SciFi")) continue;
        TDCFrame* frame = (TDCFrame*)ele;
        if(m_debug) std::cout<<" Indexed TDC frame "<<frame->positionInSpill()
    			<<" at "<<std::cout.precision(15)<<std::cout.width(18)<<frame->timeStamp()
    			<<" with "<<frame->nTriggersInFrame()<<" triggers in "<<int(frame->timeShutterIsOpen()/1000)
    			<<"us ("<<frame->nTriggersUnmatchedInSpill()<<" mismatch in frame)"<<std::endl;
        //to reject those frames which number of triggers is not the same in the Telescope and in the TDC:
        //if(frame->nTriggersUnmatchedInSpill()) continue;
    		TDCTriggers* triggers=frame->triggers();

        for(TDCTriggers::iterator itr=triggers->begin();itr!=triggers->end();itr++){

          if(m_debug) std::cout<<"        trigger "<<(itr-triggers->begin())<<"/"<<frame->nTriggersInFrame()
    				<<" which occured "<<(*itr)->timeAfterShutterOpen()<<" ns after shutter-open, "
    				<<(*itr)->timeAfterShutterOpen()-frame->timeShutterIsOpen()<<" ns before shutter-closed"<<std::endl;

    			//			std::cout<<"Looking at trigger with "<<(*itr)->nFibreTrackerHits()<<" fibre tracker hits"<<std::endl;
    			SciFiCluster* cluster;

    			bool current_cluster=false;
    			int prev_fibreTrackChannel=0;
          for(int ih=0;ih<(*itr)->nFibreTrackerHits();ih++){

    				if((*itr)->fibreTrackerADC(ih)<pcut){
    					continue;
    				}

    				if( abs((*itr)->fibreTrackerChannel(ih)-89) < 3 ||
    					 abs((*itr)->fibreTrackerChannel(ih)-65) < 3 ||
    					 abs((*itr)->fibreTrackerChannel(ih)-67) < 3 )continue;


    				channel->Fill((*itr)->fibreTrackerChannel(ih));
    				channel_ADC->Fill((*itr)->fibreTrackerADC(ih));

    				if(m_debug){std::cout<<"HIT! chan: "<<(*itr)->fibreTrackerChannel(ih)<<" = "<<(*itr)->fibreTrackerADC(ih)<<std::endl;}

    				if(!current_cluster){
    					cluster = new SciFiCluster();
    					cluster->addHit((*itr)->fibreTrackerChannel(ih),(*itr)->fibreTrackerADC(ih));
    					if(m_debug) std::cout<<"Making new cluster"<<std::endl;
    					current_cluster=true;
    					prev_fibreTrackChannel=(*itr)->fibreTrackerChannel(ih);
    					continue;
    				}

    				if(abs((*itr)->fibreTrackerChannel(ih)-prev_fibreTrackChannel) < 3){
    					if(m_debug) std::cout<<"Adding hit to cluster"<<std::endl;
    					cluster->addHit((*itr)->fibreTrackerChannel(ih),(*itr)->fibreTrackerADC(ih));
    					prev_fibreTrackChannel=(*itr)->fibreTrackerChannel(ih);
    				}
    				else{
    					if(m_debug) std::cout<<"Saving previous cluster and making new one"<<std::endl;
    					cluster_ADC->Fill(cluster->getADC());
    					cluster_size->Fill(cluster->getClusterSize());
    					setClusterCentre(cluster);
    					scifiClusters->push_back(cluster);
    					cluster = new SciFiCluster();
    					cluster->addHit((*itr)->fibreTrackerChannel(ih),(*itr)->fibreTrackerADC(ih));
    					prev_fibreTrackChannel=(*itr)->fibreTrackerChannel(ih);
    				}
    			}

    			if(current_cluster){
    				setClusterCentre(cluster);
    				scifiClusters->push_back(cluster);
    				cluster_ADC->Fill(cluster->getADC());
    				cluster_size->Fill(cluster->getClusterSize());
    			}

    			//place track correlation here based on time stamp on track

    		}
    	}



    	TestBeamTracks* tracks=(TestBeamTracks*)clipboard->get("Tracks");
    	if(!tracks) return;
    	for(TestBeamTracks::iterator it=tracks->begin(); it<tracks->end(); it++){
    		TestBeamTrack* track= *it;

    		std::string chip="SciFi";

    		//get intersection point of track with that sensor
    		TestBeamTransform* mytransform = new TestBeamTransform(parameters->alignment[chip]->displacementX(),
    																													 parameters->alignment[chip]->displacementY(),
    																													 parameters->alignment[chip]->displacementZ(),
    																													 parameters->alignment[chip]->rotationX(),
    																													 parameters->alignment[chip]->rotationY(),
    																													 parameters->alignment[chip]->rotationZ());

    		PositionVector3D< Cartesian3D<double> > planePointLocalCoords(0,0,0);
    		PositionVector3D< Cartesian3D<double> > planePointGlobalCoords = (mytransform->localToGlobalTransform())*planePointLocalCoords;
    		PositionVector3D< Cartesian3D<double> > planePoint2LocalCoords(0,0,1);
    		PositionVector3D< Cartesian3D<double> > planePoint2GlobalCoords = (mytransform->localToGlobalTransform())*planePoint2LocalCoords;

    		float normal_x=planePoint2GlobalCoords.X()-planePointGlobalCoords.X();
    		float normal_y=planePoint2GlobalCoords.Y()-planePointGlobalCoords.Y();
    		float normal_z=planePoint2GlobalCoords.Z()-planePointGlobalCoords.Z();

    		float length=((planePointGlobalCoords.X()-track->firstState()->X())*normal_x+
    									(planePointGlobalCoords.Y()-track->firstState()->Y())*normal_y+
    									(planePointGlobalCoords.Z()-track->firstState()->Z())*normal_z)/
    		(track->direction()->X()*normal_x+track->direction()->Y()*normal_y+track->direction()->Z()*normal_z);
    		float x_inter=track->firstState()->X()+length*track->direction()->X();
    		float y_inter=track->firstState()->Y()+length*track->direction()->Y();
    		float z_inter=track->firstState()->Z()+length*track->direction()->Z();


    		//change to local coordinates of that plane
    		PositionVector3D< Cartesian3D<double> > intersect_global(x_inter,y_inter,z_inter);
    		PositionVector3D< Cartesian3D<double> > intersect_local = (mytransform->globalToLocalTransform())*intersect_global;


    		for(SciFiClusters::iterator cs=scifiClusters->begin(); cs!=scifiClusters->end(); cs++){
    			//place if on timestamp between cluster and track
    			correlation_x->Fill(intersect_local.X()-(*cs)->getPosition());
    		}

    	}

    //*/



    eventNum++;
    //  clipboard->put("SciFiClusters",(TestBeamObjects*)scifiClusters,CREATED);
}
// Separate into separate clouds and publish polygons
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > // use jsk_recognition_msgs::PointsArray
separate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot, std_msgs::Header header) {
  double x_pitch = 0.25, x_min = 1.0, x_max = 3.0; // 1.5~1.75 1.75~2.00 1.5~1.675
  double y_min = -0.75, y_max = 0.75;
  double z_min = -0.250, z_1 = 0.000, z_2 = 1.000, z_max = 1.750; // -0.3125, 2.0
  pcl::PointXYZ pt_1, pt_2, pt_3, pt_4, pt_5, pt_6; // deprecate with polygon

  // Divide large cloud
  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cloud_vector;
  // pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  // pcl::PointXYZ tmp_p;

  jsk_recognition_msgs::PolygonArray polygon_array;
  polygon_array.header = header;
  for (int i = 0; i < (int)( (x_max - x_min) / x_pitch ); i++) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    geometry_msgs::PolygonStamped polygon;
    visualization_msgs::Marker texts, line_strip; // TEXT_VIEW_FACING

    texts.header = header;
    texts.ns = "text"; // namespace + ID
    texts.action = visualization_msgs::Marker::ADD;
    texts.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

    texts.pose.orientation.x = 0.0;
    texts.pose.orientation.y = 0.0;
    texts.pose.orientation.z = 0.0;
    texts.pose.orientation.w = 1.0;
    texts.scale.x = 0.125;
    texts.scale.y = 0.125;
    texts.scale.z = 0.125;
    texts.color.r = 1.0f;
    texts.color.g = 0.0f;
    texts.color.b = 0.0f;
    texts.color.a = 1.0;

    geometry_msgs::Point32 tmp_p_up_0, tmp_p_up_1, tmp_p_up_2, tmp_p_down_0, tmp_p_down_1, tmp_p_down_2;
    pcl::PointXYZ tmp_p;
    double width_tmp, width_min_up = 2.000, width_min_down = 4.000;
    double width_min_bottom = 0.500, width_min_top = 0.200;
    for (pcl::PointCloud<pcl::PointXYZ>::const_iterator itr = cloud_xyz_rot->begin();
         itr != cloud_xyz_rot->end(); itr++) {
      if ( (x_min + i*x_pitch) < itr->x && itr->x < (x_min + (i+1)*x_pitch) ) {
        if (y_min < itr->y && itr->y < y_max) {
          if (z_min < itr->z && itr->z < z_max) {
            // compare tmp_p and itr, and calculate width and points
            if (itr != cloud_xyz_rot->begin()) { // skip at 1st time
              if ( (tmp_p.y < 0 && 0 <= itr->y) || (itr->y < 0 && 0 <= tmp_p.y) ) {
                if (itr->z < z_1) {
                  width_tmp = sqrt(pow(fabs(tmp_p.x - itr->x), 2)
                                   + pow(fabs(tmp_p.y - itr->y), 2)
                                   + pow(fabs(tmp_p.z - itr->z), 2));
                  if (width_min_bottom < width_tmp && width_tmp <= width_min_down) {
                    width_min_down = width_tmp; // create width_min array
                    tmp_p_down_0.x = tmp_p.x; tmp_p_down_0.y = tmp_p.y;
                    tmp_p_down_0.z = (tmp_p.z + itr->z) / 2;
                    tmp_p_down_1.x = itr->x; tmp_p_down_1.y = itr->y;
                    tmp_p_down_1.z = (tmp_p.z + itr->z) / 2;
                    tmp_p_down_2.x = tmp_p.x; // ignore adding sqrt
                    tmp_p_down_2.y = tmp_p.y + sqrt(pow(fabs(tmp_p.y - itr->y), 2)) / 2;
                    tmp_p_down_2.z = (tmp_p.z + itr->z) / 2;
                  }
                }
                if (z_2 < itr->z) {
                  width_tmp = sqrt(pow(fabs(tmp_p.x - itr->x), 2)
                                   + pow(fabs(tmp_p.y - itr->y), 2)
                                   + pow(fabs(tmp_p.z - itr->z), 2));
                  if (width_tmp <= width_min_down) {
                    width_min_up = width_tmp;
                    tmp_p_up_0.x = tmp_p.x; tmp_p_up_0.y = tmp_p.y;
                    tmp_p_up_0.z = (tmp_p.z + itr->z) / 2;
                    tmp_p_up_1.x = itr->x; tmp_p_up_1.y = itr->y;
                    tmp_p_up_1.z = (tmp_p.z + itr->z) / 2;
                    tmp_p_up_2.x = tmp_p.x; // ignore adding sqrt
                    tmp_p_up_2.y = tmp_p.y + sqrt(pow(fabs(tmp_p.y - itr->y), 2)) / 2;
                    tmp_p_up_2.z = (tmp_p.z + itr->z) / 2;
                  }
                }
              }
              tmp_p.x = itr->x; tmp_p.y = itr->y; tmp_p.z = itr->z;
              tmp_cloud->points.push_back(tmp_p);
            }
          }
        }
      }
      // From tmp_cloud, get 4 points to publish marker
      // Create polygon
    }

    cloud_vector.push_back(tmp_cloud);

    tmp_p_up_0.x = x_min + i*x_pitch - x_pitch/2;
    tmp_p_up_1.x = x_min + i*x_pitch - x_pitch/2;
    tmp_p_down_0.x = x_min + i*x_pitch - x_pitch/2;
    tmp_p_down_1.x = x_min + i*x_pitch - x_pitch/2;
    if (tmp_p_up_0.y < tmp_p_up_1.y) {
      polygon.polygon.points.push_back(tmp_p_up_0);
      polygon.polygon.points.push_back(tmp_p_up_1);
    }
    if (tmp_p_up_0.y >= tmp_p_up_1.y) {
      polygon.polygon.points.push_back(tmp_p_up_1);
      polygon.polygon.points.push_back(tmp_p_up_0);
    }
    if (tmp_p_down_0.y < tmp_p_down_1.y) {
      polygon.polygon.points.push_back(tmp_p_down_1);
      polygon.polygon.points.push_back(tmp_p_down_0);
    }
    if (tmp_p_down_0.y >= tmp_p_down_1.y) {
      polygon.polygon.points.push_back(tmp_p_down_0);
      polygon.polygon.points.push_back(tmp_p_down_1);
    }
    polygon.header = header;
    polygon_array.polygons.push_back(polygon);

    std::cerr << "count:" << i << ", " << "size:" << cloud_vector.at(i)->size() << std::endl;
    std::cerr << "width_min_up:" << width_min_up << std::endl;
    std::cerr << "width_min_down:" << width_min_down << std::endl;

    texts.id = 2*i;
    texts.pose.position.x = tmp_p_up_0.x;
    texts.pose.position.y = tmp_p_up_2.y;
    texts.pose.position.z = tmp_p_up_2.z;

    std::ostringstream strs;
    strs << width_min_up;
    std::string str = strs.str();
    texts.text = str;
    pub_marker.publish(texts);

    texts.id = 2*i + 1;
    texts.pose.position.x = tmp_p_down_0.x;
    texts.pose.position.y = tmp_p_down_2.y;
    texts.pose.position.z = tmp_p_down_2.z;

    strs.str("");
    strs.clear(std::stringstream::goodbit);
    strs << width_min_down;
    str = strs.str();
    texts.text = str;
    pub_marker.publish(texts);

  }
  pub_polygon_array.publish(polygon_array); // error
  return cloud_vector;
}
Example #5
0
/** \fn VideoOutWindow::ApplyDBScaleAndMove(void)
 *  \brief Apply scales and moves for "Overscan" and "Underscan" DB settings.
 *
 *  It doesn't make any sense to me to offset an image such that it is clipped.
 *  Therefore, we only apply offsets if there is an underscan or overscan which
 *  creates "room" to move the image around. That is, if we overscan, we can
 *  move the "viewport". If we underscan, we change where we place the image
 *  into the display window. If no over/underscanning is performed, you just
 *  get the full original image scaled into the full display area.
 */
void VideoOutWindow::ApplyDBScaleAndMove(void)
{
    if (db_scale_vert > 0)
    {
        // Veritcal overscan. Move the Y start point in original image.
        float tmp = 1.0f - 2.0f * db_scale_vert;
        video_rect.moveTop((int) round(video_rect.height() * db_scale_vert));
        video_rect.setHeight((int) round(video_rect.height() * tmp));

        // If there is an offset, apply it now that we have a room.
        int yoff = db_move.y();
        if (yoff > 0)
        {
            // To move the image down, move the start point up.
            // Don't offset the image more than we have overscanned.
            yoff = min(video_rect.top(), yoff);
            video_rect.moveTop(video_rect.top() - yoff);
        }
        else if (yoff < 0)
        {
            // To move the image up, move the start point down.
            // Don't offset the image more than we have overscanned.
            if (abs(yoff) > video_rect.top())
                yoff = 0 - video_rect.top();
            video_rect.moveTop(video_rect.top() - yoff);
        }
    }
    else if (db_scale_vert < 0)
    {
        // Vertical underscan. Move the starting Y point in the display window.
        // Use the abolute value of scan factor.
        float vscanf = fabs(db_scale_vert);
        float tmp = 1.0f - 2.0f * vscanf;

        display_video_rect.moveTop(
            (int) round(display_visible_rect.height() * vscanf) +
            display_visible_rect.top());

        display_video_rect.setHeight(
            (int) round(display_visible_rect.height() * tmp));

        // Now offset the image within the extra blank space created by
        // underscanning. To move the image down, increase the Y offset
        // inside the display window.
        int yoff = db_move.y();
        if (yoff > 0)
        {
            // Don't offset more than we have underscanned.
            yoff = min(display_video_rect.top(), yoff);
            display_video_rect.moveTop(display_video_rect.top() + yoff);
        }
        else if (yoff < 0)
        {
            // Don't offset more than we have underscanned.
            if (abs(yoff) > display_video_rect.top())
                yoff = 0 - display_video_rect.top();
            display_video_rect.moveTop(display_video_rect.top() + yoff);
        }
    }

    // Horizontal.. comments, same as vertical...
    if (db_scale_horiz > 0)
    {
        float tmp = 1.0f - 2.0f * db_scale_horiz;
        video_rect.moveLeft(
            (int) round(video_disp_dim.width() * db_scale_horiz));
        video_rect.setWidth((int) round(video_disp_dim.width() * tmp));

        int xoff = db_move.x();
        if (xoff > 0)
        {
            xoff = min(video_rect.left(), xoff);
            video_rect.moveLeft(video_rect.left() - xoff);
        }
        else if (xoff < 0)
        {
            if (abs(xoff) > video_rect.left())
                xoff = 0 - video_rect.left();
            video_rect.moveLeft(video_rect.left() - xoff);
        }
    }
    else if (db_scale_horiz < 0)
    {
        float hscanf = fabs(db_scale_horiz);
        float tmp = 1.0f - 2.0f * hscanf;

        display_video_rect.moveLeft(
            (int) round(display_visible_rect.width() * hscanf) +
            display_visible_rect.left());

        display_video_rect.setWidth(
            (int) round(display_visible_rect.width() * tmp));

        int xoff = db_move.x();
        if (xoff > 0)
        {
            xoff = min(display_video_rect.left(), xoff);
            display_video_rect.moveLeft(display_video_rect.left() + xoff);
        }
        else if (xoff < 0)
        {
            if (abs(xoff) > display_video_rect.left())
                xoff = 0 - display_video_rect.left();
            display_video_rect.moveLeft(display_video_rect.left() + xoff);
        }
    }

}
Example #6
0
/*************************************************************************
* This function performs an edge-based FM refinement
**************************************************************************/
void RedoMyLink(ctrl_t *ctrl, graph_t *graph, idx_t *home, idx_t me,
                idx_t you, real_t *flows, real_t *sr_cost, real_t *sr_lbavg)
{
    idx_t h, i, r;
    idx_t nvtxs, nedges, ncon;
    idx_t  pass, lastseed, totalv;
    idx_t *xadj, *adjncy, *adjwgt, *where, *vsize;
    idx_t *costwhere, *lbwhere, *selectwhere;
    idx_t *ed, *id, *bndptr, *bndind, *perm;
    real_t *nvwgt, mycost;
    real_t lbavg, *lbvec;
    real_t best_lbavg, other_lbavg = -1.0, bestcost, othercost = -1.0;
    real_t *npwgts, *pwgts, *tpwgts;
    real_t ipc_factor, redist_factor, ftmp;
    idx_t mype;
    gkMPI_Comm_rank(MPI_COMM_WORLD, &mype);


    WCOREPUSH;

    nvtxs  = graph->nvtxs;
    nedges = graph->nedges;
    ncon   = graph->ncon;
    xadj   = graph->xadj;
    nvwgt  = graph->nvwgt;
    vsize  = graph->vsize;
    adjncy = graph->adjncy;
    adjwgt = graph->adjwgt;
    where  = graph->where;

    ipc_factor    = ctrl->ipc_factor;
    redist_factor = ctrl->redist_factor;

    /* set up data structures */
    id     = graph->sendind = iwspacemalloc(ctrl, nvtxs);
    ed     = graph->recvind = iwspacemalloc(ctrl, nvtxs);
    bndptr = graph->sendptr = iwspacemalloc(ctrl, nvtxs);
    bndind = graph->recvptr = iwspacemalloc(ctrl, nvtxs);

    costwhere = iwspacemalloc(ctrl, nvtxs);
    lbwhere   = iwspacemalloc(ctrl, nvtxs);
    perm      = iwspacemalloc(ctrl, nvtxs);

    lbvec  = rwspacemalloc(ctrl, ncon);
    pwgts  = rset(2*ncon, 0.0, rwspacemalloc(ctrl, 2*ncon));
    npwgts = rwspacemalloc(ctrl, 2*ncon);
    tpwgts = rwspacemalloc(ctrl, 2*ncon);

    graph->gnpwgts = npwgts;

    RandomPermute(nvtxs, perm, 1);
    icopy(nvtxs, where, costwhere);
    icopy(nvtxs, where, lbwhere);

    /* compute target pwgts */
    for (h=0; h<ncon; h++) {
        tpwgts[h]      = -1.0*flows[h];
        tpwgts[ncon+h] = flows[h];
    }

    for (i=0; i<nvtxs; i++) {
        if (where[i] == me) {
            for (h=0; h<ncon; h++) {
                tpwgts[h] += nvwgt[i*ncon+h];
                pwgts[h]  += nvwgt[i*ncon+h];
            }
        }
        else {
            ASSERT(where[i] == you);
            for (h=0; h<ncon; h++) {
                tpwgts[ncon+h] += nvwgt[i*ncon+h];
                pwgts[ncon+h]  += nvwgt[i*ncon+h];
            }
        }
    }

    /* we don't want any weights to be less than zero */
    for (h=0; h<ncon; h++) {
        if (tpwgts[h] < 0.0) {
            tpwgts[ncon+h] += tpwgts[h];
            tpwgts[h] = 0.0;
        }

        if (tpwgts[ncon+h] < 0.0) {
            tpwgts[h] += tpwgts[ncon+h];
            tpwgts[ncon+h] = 0.0;
        }
    }


    /* now compute new bisection */
    bestcost = (real_t)isum(nedges, adjwgt, 1)*ipc_factor +
               (real_t)isum(nvtxs, vsize, 1)*redist_factor;
    best_lbavg = 10.0;

    lastseed = 0;
    for (pass=N_MOC_REDO_PASSES; pass>0; pass--) {
        iset(nvtxs, 1, where);

        /* find seed vertices */
        r = perm[lastseed] % nvtxs;
        lastseed = (lastseed+1) % nvtxs;
        where[r] = 0;

        Mc_Serial_Compute2WayPartitionParams(ctrl, graph);
        Mc_Serial_Init2WayBalance(ctrl, graph, tpwgts);
        Mc_Serial_FM_2WayRefine(ctrl, graph, tpwgts, 4);
        Mc_Serial_Balance2Way(ctrl, graph, tpwgts, 1.02);
        Mc_Serial_FM_2WayRefine(ctrl, graph, tpwgts, 4);

        for (i=0; i<nvtxs; i++)
            where[i] = (where[i] == 0) ? me : you;

        for (i=0; i<ncon; i++) {
            ftmp = (pwgts[i]+pwgts[ncon+i])/2.0;
            if (ftmp != 0.0)
                lbvec[i] = fabs(npwgts[i]-tpwgts[i])/ftmp;
            else
                lbvec[i] = 0.0;
        }
        lbavg = ravg(ncon, lbvec);

        totalv = 0;
        for (i=0; i<nvtxs; i++)
            if (where[i] != home[i])
                totalv += vsize[i];

        mycost = (real_t)(graph->mincut)*ipc_factor + (real_t)totalv*redist_factor;

        if (bestcost >= mycost) {
            bestcost = mycost;
            other_lbavg = lbavg;
            icopy(nvtxs, where, costwhere);
        }

        if (best_lbavg >= lbavg) {
            best_lbavg = lbavg;
            othercost = mycost;
            icopy(nvtxs, where, lbwhere);
        }
    }

    if (other_lbavg <= .05) {
        selectwhere = costwhere;
        *sr_cost = bestcost;
        *sr_lbavg = other_lbavg;
    }
    else {
        selectwhere = lbwhere;
        *sr_cost = othercost;
        *sr_lbavg = best_lbavg;
    }

    icopy(nvtxs, selectwhere, where);

    WCOREPOP;
}
Example #7
0
static int ff_filter_frame_framed(AVFilterLink *link, AVFrame *frame)
{
    int (*filter_frame)(AVFilterLink *, AVFrame *);
    AVFilterContext *dstctx = link->dst;
    AVFilterPad *dst = link->dstpad;
    AVFrame *out = NULL;
    int ret;
    AVFilterCommand *cmd= link->dst->command_queue;
    int64_t pts;

    if (link->closed) {
        av_frame_free(&frame);
        return AVERROR_EOF;
    }

    if (!(filter_frame = dst->filter_frame))
        filter_frame = default_filter_frame;

    /* copy the frame if needed */
    if (dst->needs_writable && !av_frame_is_writable(frame)) {
        av_log(link->dst, AV_LOG_DEBUG, "Copying data in avfilter.\n");

        /* Maybe use ff_copy_buffer_ref instead? */
        switch (link->type) {
        case AVMEDIA_TYPE_VIDEO:
            out = ff_get_video_buffer(link, link->w, link->h);
            break;
        case AVMEDIA_TYPE_AUDIO:
            out = ff_get_audio_buffer(link, frame->nb_samples);
            break;
        default:
            ret = AVERROR(EINVAL);
            goto fail;
        }
        if (!out) {
            ret = AVERROR(ENOMEM);
            goto fail;
        }

        ret = av_frame_copy_props(out, frame);
        if (ret < 0)
            goto fail;

        switch (link->type) {
        case AVMEDIA_TYPE_VIDEO:
            av_image_copy(out->data, out->linesize, (const uint8_t **)frame->data, frame->linesize,
                          frame->format, frame->width, frame->height);
            break;
        case AVMEDIA_TYPE_AUDIO:
            av_samples_copy(out->extended_data, frame->extended_data,
                            0, 0, frame->nb_samples,
                            av_get_channel_layout_nb_channels(frame->channel_layout),
                            frame->format);
            break;
        default:
            ret = AVERROR(EINVAL);
            goto fail;
        }

        av_frame_free(&frame);
    } else
        out = frame;

    while(cmd && cmd->time <= out->pts * av_q2d(link->time_base)){
        av_log(link->dst, AV_LOG_DEBUG,
               "Processing command time:%f command:%s arg:%s\n",
               cmd->time, cmd->command, cmd->arg);
        avfilter_process_command(link->dst, cmd->command, cmd->arg, 0, 0, cmd->flags);
        ff_command_queue_pop(link->dst);
        cmd= link->dst->command_queue;
    }

    pts = out->pts;
    if (dstctx->enable_str) {
        int64_t pos = av_frame_get_pkt_pos(out);
        dstctx->var_values[VAR_N] = link->frame_count;
        dstctx->var_values[VAR_T] = pts == AV_NOPTS_VALUE ? NAN : pts * av_q2d(link->time_base);
        dstctx->var_values[VAR_POS] = pos == -1 ? NAN : pos;

        dstctx->is_disabled = fabs(av_expr_eval(dstctx->enable, dstctx->var_values, NULL)) < 0.5;
        if (dstctx->is_disabled &&
            (dstctx->filter->flags & AVFILTER_FLAG_SUPPORT_TIMELINE_GENERIC))
            filter_frame = default_filter_frame;
    }
    ret = filter_frame(link, out);
    link->frame_count++;
    link->frame_requested = 0;
    ff_update_link_current_pts(link, pts);
    return ret;

fail:
    av_frame_free(&out);
    av_frame_free(&frame);
    return ret;
}
Example #8
0
void rf3_pg(double *po, double *pr, int npeaks, double lfm, double l_ult, double uc_mult)
{
  // assumes po has storage for the 5 by npeaks array
  // pr are the peaks, is length npeaks
    double a[16384], range, mean, adj_range, lfmargin, adj_zero_mean_range;
    int tot_num, index, j, cNr;

    lfmargin = l_ult - fabs(lfm);
    tot_num = npeaks;
    //    printf ("in rf3_pg, tot_num = %d\n", tot_num);

    j = -1;
    cNr = 1;
    for (index=0; index<tot_num; index++) 
      {
        a[++j]=*pr++;
        while ( (j >= 2) && (fabs(a[j-1]-a[j-2]) <= fabs(a[j]-a[j-1])) ) 
	  {
            range=fabs(a[j-1]-a[j-2]);
            switch(j)
	      {
	      case 0: { break; }
	      case 1: { break; }
	      case 2: {
		mean=(a[0]+a[1])/2;
		adj_range = range * lfmargin / ( l_ult - fabs(mean) );
		adj_zero_mean_range = range * l_ult / ( l_ult - fabs(mean) );
		a[0]=a[1];
		a[1]=a[2];
		j=1;
		if (range > 0) {
		  *po++=range;
		  *po++=mean;
		  *po++=adj_range;
		  *po++=uc_mult;
		  *po++=adj_zero_mean_range;
		}
		break;
	      }
	      default: {
		mean=(a[j-1]+a[j-2])/2;
		adj_range = range * lfmargin / ( l_ult - fabs(mean) );
		adj_zero_mean_range = range * l_ult / ( l_ult - fabs(mean) );
		a[j-2]=a[j];
		j=j-2;
		if (range > 0) {
		  *po++=range;
		  *po++=mean;
		  *po++=adj_range;
		  *po++=1.00;
		  *po++=adj_zero_mean_range;
		  cNr++;
		}
		break;
	      }
	      }
	  }
      }
    for (index=0; index<j; index++) 
      {
	range=fabs(a[index]-a[index+1]);
	mean=(a[index]+a[index+1])/2;
	adj_range = range * lfmargin / ( l_ult - fabs(mean) );
	adj_zero_mean_range = range * l_ult / ( l_ult - fabs(mean) );
	if (range > 0)
	  {
	    *po++=range;
	    *po++=mean;
	    *po++=adj_range;
	    *po++=uc_mult;
	    *po++=adj_zero_mean_range;
	  }
      }
    //    printf ("exiting rf3_pg\n");
}
Example #9
0
/*
-------------------------
Remote_MaintainHeight
-------------------------
*/
void Remote_MaintainHeight( void )
{	
	float	dif;

	// Update our angles regardless
	NPC_UpdateAngles( qtrue, qtrue );

	if ( NPC->client->ps.velocity[2] )
	{
		NPC->client->ps.velocity[2] *= VELOCITY_DECAY;

		if ( fabs( NPC->client->ps.velocity[2] ) < 2 )
		{
			NPC->client->ps.velocity[2] = 0;
		}
	}
	// If we have an enemy, we should try to hover at or a little below enemy eye level
	if ( NPC->enemy )
	{
		if (TIMER_Done( NPC, "heightChange"))
		{
			TIMER_Set( NPC,"heightChange",Q_irand( 1000, 3000 ));

			// Find the height difference
			dif = (NPC->enemy->r.currentOrigin[2] +  Q_irand( 0, NPC->enemy->r.maxs[2]+8 )) - NPC->r.currentOrigin[2]; 

			// cap to prevent dramatic height shifts
			if ( fabs( dif ) > 2 )
			{
				if ( fabs( dif ) > 24 )
				{
					dif = ( dif < 0 ? -24 : 24 );
				}
				dif *= 10;
				NPC->client->ps.velocity[2] = (NPC->client->ps.velocity[2]+dif)/2;
				G_Sound( NPC, CHAN_AUTO, G_SoundIndex("sound/chars/remote/misc/hiss.wav"));
			}
		}
	}
	else
	{
		gentity_t *goal = NULL;

		if ( NPCInfo->goalEntity )	// Is there a goal?
		{
			goal = NPCInfo->goalEntity;
		}
		else
		{
			goal = NPCInfo->lastGoalEntity;
		}
		if ( goal )
		{
			dif = goal->r.currentOrigin[2] - NPC->r.currentOrigin[2];

			if ( fabs( dif ) > 24 )
			{
				dif = ( dif < 0 ? -24 : 24 );
				NPC->client->ps.velocity[2] = (NPC->client->ps.velocity[2]+dif)/2;
			}
		}
	}

	// Apply friction
	if ( NPC->client->ps.velocity[0] )
	{
		NPC->client->ps.velocity[0] *= VELOCITY_DECAY;

		if ( fabs( NPC->client->ps.velocity[0] ) < 1 )
		{
			NPC->client->ps.velocity[0] = 0;
		}
	}

	if ( NPC->client->ps.velocity[1] )
	{
		NPC->client->ps.velocity[1] *= VELOCITY_DECAY;

		if ( fabs( NPC->client->ps.velocity[1] ) < 1 )
		{
			NPC->client->ps.velocity[1] = 0;
		}
	}
}
void video_refresh_timer(void *userdata) {

  VideoState *is = (VideoState *)userdata;
  VideoPicture *vp;
  double actual_delay, delay, sync_threshold, ref_clock, diff;

  if(is->video_st) {
    if(is->pictq_size == 0) {
      schedule_refresh(is, 1);
    } else {
      vp = &is->pictq[is->pictq_rindex];

      is->video_current_pts = vp->pts;
      is->video_current_pts_time = av_gettime();

      delay = vp->pts - is->frame_last_pts; /* the pts from last time */
      if(delay <= 0 || delay >= 1.0) {
	/* if incorrect delay, use previous one */
	delay = is->frame_last_delay;
      }
      /* save for next time */
      is->frame_last_delay = delay;
      is->frame_last_pts = vp->pts;

      /* update delay to sync to audio if not master source */
      if(is->av_sync_type != AV_SYNC_VIDEO_MASTER) {
	ref_clock = get_master_clock(is);
	diff = vp->pts - ref_clock;

	/* Skip or repeat the frame. Take delay into account
	   FFPlay still doesn't "know if this is the best guess." */
	sync_threshold = (delay > AV_SYNC_THRESHOLD) ? delay : AV_SYNC_THRESHOLD;
	if(fabs(diff) < AV_NOSYNC_THRESHOLD) {
	  if(diff <= -sync_threshold) {
	    delay = 0;
	  } else if(diff >= sync_threshold) {
	    delay = 2 * delay;
	  }
	}
      }

      is->frame_timer += delay;
      /* computer the REAL delay */
      actual_delay = is->frame_timer - (av_gettime() / 1000000.0);
      if(actual_delay < 0.010) {
	/* Really it should skip the picture instead */
	actual_delay = 0.010;
      }
      schedule_refresh(is, (int)(actual_delay * 1000 + 0.5));

      /* show the picture! */
      video_display(is);

      /* update queue for next picture! */
      if(++is->pictq_rindex == VIDEO_PICTURE_QUEUE_SIZE) {
	is->pictq_rindex = 0;
      }
      pthread_mutex_lock(is->pictq_mutex);
      is->pictq_size--;
      pthread_cond_signal(is->pictq_cond);
      pthread_mutex_unlock(is->pictq_mutex);
    }
  } else {
    schedule_refresh(is, 100);
  }
}
Example #11
0
/** @brief Tester program for EGlpNum_t structure and functions
 * @return zero on success, non-zero otherwise 
 * @par Description:
 * Perform various tests on EGlpNum_t and their functions 
 * */
int main (int argc,
					char **argv)
{
	/* local variables */
	EGlpNum_t ntmp[4];
	double dtmp[5];
	char *strnum1,
	 *strnum2,
	 *strnum3;
	int rval=0;
	#ifdef HAVE_LIBGMP
	int n_char = 0;
	mpq_t qnum;
	#endif
	EGlpNumStart();
	/* set signal and limits */
	EGsigSet(rval,CLEANUP);
	EGsetLimits(3600.0,4294967295UL);
	#ifdef HAVE_LIBGMP
	EGlpNumSetPrecision (128);
	mpq_init (qnum);
	#endif
	EGlpNumInitVar (ntmp[0]);
	EGlpNumInitVar (ntmp[1]);
	EGlpNumInitVar (ntmp[2]);
	EGlpNumInitVar (ntmp[3]);
	/* the input should have at least two parameters, namelly the number where we
	 * will work on */
	if (argc < 3)
	{
		fprintf (stderr,
						 "usage: %s num1 num2\n\tWhere num1 and num2 are numbers in"
						 " the number format (either a/b or regular doubles)\n", argv[0]);
		exit (1);
	}

	/* we ask for two numbers and perform some basic operations and compare
	 * aganist double arithmetic */
	#ifdef HAVE_LIBGMP
	n_char = mpq_EGlpNumReadStrXc (qnum, argv[1]);
	#endif
	EGlpNumReadStr (ntmp[0], argv[1]);
	EGlpNumReadStr (ntmp[1], argv[2]);
	dtmp[0] = EGlpNumToLf (ntmp[0]);
	dtmp[1] = EGlpNumToLf (ntmp[1]);

	/* convert numbers */
	strnum1 = EGlpNumGetStr (ntmp[0]);
	strnum2 = EGlpNumGetStr (ntmp[1]);
	fprintf (stderr, "You Input %s (%lg) and %s (%lg)\n", strnum1, dtmp[0],
					 strnum2, dtmp[1]);
	#ifdef HAVE_LIBGMP
	strnum3 = mpq_EGlpNumGetStr (qnum);
	fprintf (stderr, "Your first number represented as exact rational is %s, "
					 "readed with %d chars\n", strnum3, n_char);
	free (strnum3);
	mpq_EGlpNumSet (qnum, dtmp[0]);
	strnum3 = mpq_EGlpNumGetStr (qnum);
	fprintf (stderr, "Your first number represented as continuous fraction "
					 "is %s, readed with %d chars\n", strnum3, n_char);
	free (strnum3);
	#endif

	/* internal constants */
	strnum3 = EGlpNumGetStr (oneLpNum);
	fprintf (stderr, "1.0 = %s\n", strnum3);
	EGfree (strnum3);
	strnum3 = EGlpNumGetStr (zeroLpNum);
	fprintf (stderr, "0.0 = %s\n", strnum3);
	EGfree (strnum3);
	strnum3 = EGlpNumGetStr (epsLpNum);
	fprintf (stderr, "eps = %s\n", strnum3);
	EGfree (strnum3);
	strnum3 = EGlpNumGetStr (MaxLpNum);
	fprintf (stderr, "Max = %s\n", strnum3);
	EGfree (strnum3);
	strnum3 = EGlpNumGetStr (MinLpNum);
	fprintf (stderr, "Min = %s\n", strnum3);
	EGfree (strnum3);

	/* copying functions */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = %s (%lg)\n", strnum3, strnum1, dtmp[0]);
	EGfree (strnum3);
	EGlpNumCopyDiff (ntmp[2], ntmp[0], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = %s - %s (%lg)\n", strnum3, strnum1, strnum2,
					 dtmp[0] - dtmp[1]);
	EGfree (strnum3);
	EGlpNumCopyDiffRatio (ntmp[2], ntmp[0], ntmp[1], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = (%s  - %s)/%s (%lg)\n", strnum3, strnum1, strnum2,
					 strnum1, (dtmp[0] - dtmp[1]) / dtmp[0]);
	EGfree (strnum3);
	EGlpNumCopySum (ntmp[2], ntmp[0], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = %s + %s (%lg)\n", strnum3, strnum1, strnum2,
					 dtmp[0] + dtmp[1]);
	EGfree (strnum3);
	EGlpNumCopySqrOver (ntmp[2], ntmp[1], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = %s^2/%s (%lg)\n", strnum3, strnum2, strnum1,
					 dtmp[1] * dtmp[1] / dtmp[0]);
	EGfree (strnum3);
	EGlpNumCopyAbs (ntmp[2], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = |%s| (%lg)\n", strnum3, strnum1, fabs (dtmp[0]));
	EGfree (strnum3);
	EGlpNumCopyNeg (ntmp[2], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = -1*%s (%lg)\n", strnum3, strnum1, -dtmp[0]);
	EGfree (strnum3);
	EGlpNumCopyFrac (ntmp[2], ntmp[1], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s = %s/%s (%lg)\n", strnum3, strnum2, strnum1,
					 dtmp[1] / dtmp[0]);
	EGfree (strnum3);

	/* add */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumAddTo (ntmp[2], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s + %s = %s (%lg)\n", strnum1, strnum2, strnum3,
					 dtmp[0] + dtmp[1]);
	EGfree (strnum3);
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumAddUiTo (ntmp[2], 0xffU);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s + %u = %s (%lg)\n", strnum1, 0xffU, strnum3,
					 dtmp[0] + 0xffU);
	EGfree (strnum3);

	/* substract */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumSubTo (ntmp[2], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s - %s = %s (%lg)\n", strnum1, strnum2, strnum3,
					 dtmp[0] - dtmp[1]);
	EGfree (strnum3);
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumSubUiTo (ntmp[2], 0xffU);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s - %u = %s (%lg)\n", strnum1, 0xffU, strnum3,
					 dtmp[0] - 0xffU);
	EGfree (strnum3);

	/* multiply */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumMultTo (ntmp[2], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s * %s = %s (%lg)\n", strnum1, strnum2, strnum3,
					 dtmp[0] * dtmp[1]);
	EGfree (strnum3);

	/* multiply unsigned */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumMultUiTo (ntmp[2], 13);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s * 13 = %s (%lg)\n", strnum1, strnum3, dtmp[0] * 13);
	EGfree (strnum3);

	/* inverse */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumInv (ntmp[2]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "1/%s = %s (%lg)\n", strnum1, strnum3, 1.0 / dtmp[0]);
	EGfree (strnum3);

	/* floor and ceil */
	EGlpNumFloor (ntmp[2], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "floor(%s) = %s (%lg)\n", strnum1, strnum3, floor (dtmp[0]));
	EGfree (strnum3);
	EGlpNumCeil (ntmp[2], ntmp[0]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "ceil(%s) = %s (%lg)\n", strnum1, strnum3, ceil (dtmp[0]));
	EGfree (strnum3);

	/* negative and inner products */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumSign (ntmp[2]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "-(%s) = %s (%lg)\n", strnum1, strnum3, -dtmp[0]);
	EGfree (strnum3);
	EGlpNumOne (ntmp[2]);
	EGlpNumAddInnProdTo (ntmp[2], ntmp[0], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "1.0 + %s*%s = %s (%lg)\n", strnum1, strnum2, strnum3,
					 1.0 + dtmp[1] * dtmp[0]);
	EGfree (strnum3);
	EGlpNumOne (ntmp[2]);
	EGlpNumSubInnProdTo (ntmp[2], ntmp[0], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "1.0 - %s*%s = %s (%lg)\n", strnum1, strnum2, strnum3,
					 1.0 - dtmp[1] * dtmp[0]);
	EGfree (strnum3);

	/* divide */
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumDivTo (ntmp[2], ntmp[1]);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s / %s = %s (%lg)\n", strnum1, strnum2, strnum3,
					 dtmp[0] / dtmp[1]);
	EGfree (strnum3);
	EGlpNumCopy (ntmp[2], ntmp[0]);
	EGlpNumDivUiTo (ntmp[2], 0xffU);
	strnum3 = EGlpNumGetStr (ntmp[2]);
	fprintf (stderr, "%s / %u = %s (%lg)\n", strnum1, 0xffU, strnum3,
					 dtmp[0] / 0xffU);
	EGfree (strnum3);
	EGfree (strnum1);
	EGfree (strnum2);

#ifdef HAVE_LIBGMP
	/* test transformation to rationals */
	{
		mpq_t n1, n2;
		mpf_t f1, f2;
		mpq_init (n1);
		mpq_init (n2);
		mpf_init (f1);
		mpf_init (f2);
		mpf_EGlpNumSet(f1,EGlpNumToLf(ntmp[0]));
		mpf_EGlpNumSet(f2,EGlpNumToLf(ntmp[1]));
		mpq_EGlpNumSet_mpf (n1, f1);
		mpq_EGlpNumSet_mpf (n2, f2);
		strnum1 = mpq_EGlpNumGetStr (n1);
		strnum2 = mpq_EGlpNumGetStr (n2);
		fprintf (stderr, "Your input in rational was:\n\t(%10.7lf) %s\n\t(%10.7lf)"
				" %s\n", EGlpNumToLf (ntmp[0]), strnum1, EGlpNumToLf (ntmp[1]),
				strnum2);
		EGfree (strnum1);
		EGfree (strnum2);

		/* test natural exponentiation */
		mpf_EGlpNumEpow (f1, -38.81624211135693732736499880);
		mpf_set_ui (f2, (unsigned long)1);
		mpf_div_2exp (f2, f2, (unsigned long)56);
		strnum1 = mpf_EGlpNumGetStr (f1);
		strnum2 = mpf_EGlpNumGetStr (f2);
		fprintf (stderr, "2^-56 = %s ~ %s\n", strnum1, strnum2);
		EGfree (strnum1);
		EGfree (strnum2);
		mpq_clear (n1);
		mpq_clear (n2);
		mpf_clear (f1);
		mpf_clear (f2);
	}
#endif

	/* ending */
	CLEANUP:
	EGlpNumClearVar (ntmp[0]);
	EGlpNumClearVar (ntmp[1]);
	EGlpNumClearVar (ntmp[2]);
	EGlpNumClearVar (ntmp[3]);
	#ifdef HAVE_LIBGMP
	mpq_clear (qnum);
	#endif
	EGlpNumClear();
	return 0;
}
Example #12
0
/*!
 * Write one PET frame from IMG data struct into Analyze 7.5 database file.
 *  This function can be called repeatedly to write all frames one at a time
 *  to conserve memory. This function does not write SIF.
 *
 * @param dbname name of file where IMG contents will be written.
 *  If file does not exist, it is created.
 *  Make sure to delete existing file, unless you want to add data
 * @param frame_to_write PET frame number (1..frameNr) which will be written:
 *  If set to 0, frame data will be written to an existing or new PET file as
 *  a new frame, never overwriting existing data.
 *  If >0, then frame data is written as specified frame number, overwriting
 *  any data existing with the same frame number
 * @param img pointer to the IMG data struct
 * @param frame_index IMG frame index (0..dimt-1) which will be written
 * @param fmin minimum pixel value in all frames that will be written;
 *  used only when writing the first frame
 * @param fmax maximum pixel value in all frames that will be written;
 *  used only when writing the first frame
 * @return errstatus, which is STATUS_OK (0) when call was successful,
 * and >0 in case of an error.
 */
int imgWriteAnalyzeFrame(
  const char *dbname, int frame_to_write, IMG *img, int frame_index,
  float fmin, float fmax
) {
  IMG test_img;
  int ret=0, pxlNr, zi, xi, yi, little;
  FILE *fp;
  short int *sdata=NULL, *sptr;
  char datfile[FILENAME_MAX], hdrfile[FILENAME_MAX], siffile[FILENAME_MAX];
  ANALYZE_DSR dsr;
  float scale_factor=1.0;


  if(IMG_TEST) printf("\nimgWriteAnalyzeFrame(%s, %d, *img, %d, %g, %g)\n",
    dbname, frame_to_write, frame_index, fmin, fmax);

  /*
   *  Check the input 
   */
  if(dbname==NULL) return STATUS_FAULT;
  if(img==NULL) return STATUS_FAULT;
  if(img->status!=IMG_STATUS_OCCUPIED) return STATUS_FAULT;
  if(frame_to_write<0) return STATUS_FAULT;
  if(frame_index<0 || frame_index>=img->dimt) return STATUS_FAULT;
  if(img->_fileFormat!=IMG_ANA_L && img->_fileFormat!=IMG_ANA)
    return STATUS_FAULT;

  /*
   *  If database does not exist, then create it with new header,
   *  and if it does exist, then read and check header information.
   *  Create or edit header to contain correct frame nr.
   *  Determine the global scaling factor.   
   */
  imgInit(&test_img);
  if(anaDatabaseExists(dbname, hdrfile, datfile, siffile)==0) { // not existing

    /* Create database filenames */
    sprintf(hdrfile, "%s.hdr", dbname);
    sprintf(datfile, "%s.img", dbname);
    sprintf(siffile, "%s.sif", dbname);

    /* Set main header */
    imgSetAnalyzeHeader(img, dbname, &dsr, fmin, fmax);
    if(frame_to_write==0) frame_to_write=1;
    dsr.dime.dim[4]=frame_to_write;
    scale_factor=dsr.dime.funused1;
    if(fabs(scale_factor)>1.0E-20) scale_factor=1.0/scale_factor;

    /* Write Analyze header */
    ret=anaWriteHeader(hdrfile, &dsr);
    if(ret && IMG_TEST) printf("anaWriteHeader() := %d\n", ret);
    if(ret) return STATUS_CANTWRITEHEADERFILE;

    /* Remove datafile if necessary */
    if(access(datfile, 0) != -1) remove(datfile);

  } else { /* database does exist */
  
    /* Read header information for checking */
    ret=imgReadAnalyzeHeader(dbname, &test_img); if(ret!=0) return ret;
    /* Check that file format is the same */
    if(img->_fileFormat!=test_img._fileFormat || img->type!=test_img.type)
      return STATUS_WRONGFILETYPE;
    /* Check that matrix sizes are the same */
    if(img->dimz!=test_img.dimz || img->dimx!=test_img.dimx ||
       img->dimy!=test_img.dimy)
      return STATUS_VARMATSIZE;
    imgEmpty(&test_img);

    /* Read the header, set new frame number, and write it back */
    /* Get also the scale factor */
    if((ret=anaReadHeader(hdrfile, &dsr))!=0) return STATUS_NOMAINHEADER;
    scale_factor=1.0/dsr.dime.funused1;
    if(frame_to_write==0) frame_to_write=dsr.dime.dim[4]+1;
    if(dsr.dime.dim[4]<frame_to_write) {
      if(dsr.dime.dim[4]+1<frame_to_write) return STATUS_MISSINGMATRIX;
      dsr.dime.dim[4]=frame_to_write;
    }
    if((ret=anaWriteHeader(hdrfile, &dsr))!=0) return STATUS_NOWRITEPERM;
  }
  if(IMG_TEST>2) {
    printf("frame_to_write := %d\n", frame_to_write);
    printf("hdrfile := %s\n", hdrfile);
    printf("datfile := %s\n", datfile);
    printf("siffile := %s\n", siffile);
  }

  /* Allocate memory for matrix short int data (one plane) */
  pxlNr=img->dimx*img->dimy;
  sdata=(short int*)malloc(pxlNr*sizeof(short int));
  if(sdata==NULL) return STATUS_NOMEMORY;

  /* Open datafile, not removing possible old contents */
  if(frame_to_write==1) fp=fopen(datfile, "wb"); else fp=fopen(datfile, "r+b");
  if(fp==NULL) {free(sdata); return STATUS_CANTWRITEIMGFILE;}
  /* Move file pointer to the place of current frame */
  if(fseek(fp, (frame_to_write-1)*pxlNr*img->dimz*sizeof(short int),
           SEEK_SET)!=0) {
    free(sdata); fclose(fp); return STATUS_MISSINGMATRIX;}
  little=little_endian();
  /* Copy, scale and write data plane-by-plane */
  if(anaFlipping()==0) {
    for(zi=0; zi<img->dimz; zi++) {
      sptr=sdata;
     /*printf("plane := %d\n  scale_factor := %g\n", zi+1, scale_factor);*/
      for(yi=img->dimy-1; yi>=0; yi--) for(xi=img->dimx-1; xi>=0; xi--) {
        *sptr=temp_roundf(scale_factor*img->m[zi][yi][xi][frame_index]); sptr++;
      }
      /* Change byte order if necessary */
      sptr=sdata; if(little!=dsr.little) swabip(sptr, pxlNr*sizeof(short int));
      /* Write image data */
      sptr=sdata;
      if(fwrite(sptr, sizeof(short int), pxlNr, fp) != pxlNr) {
        free(sdata); fclose(fp); return STATUS_CANTWRITEIMGFILE;
      }
    }
  } else {
    for(zi=img->dimz-1; zi>=0; zi--) {
      sptr=sdata;
      for(yi=img->dimy-1; yi>=0; yi--) for(xi=img->dimx-1; xi>=0; xi--) {
        *sptr=temp_roundf(scale_factor*img->m[zi][yi][xi][frame_index]); sptr++;
      }
      /* Change byte order if necessary */
      sptr=sdata; if(little!=dsr.little) swabip(sptr, pxlNr*sizeof(short int));
      /* Write image data */
      sptr=sdata;
      if(fwrite(sptr, sizeof(short int), pxlNr, fp) != pxlNr) {
        free(sdata); fclose(fp); return STATUS_CANTWRITEIMGFILE;
      }
    }
  }
  free(sdata);
  fclose(fp);

  return STATUS_OK;
}
Example #13
0
/*!
 * Copy header information in IMG struct into Analyze 7.5 header struct.
 *  Min, max, and scale factor are set here and they apply to all frames.
 *
 * @param img pointer to IMG struct
 * @param dbname Analyze 7.5 database name
 * @param dsr pointer to Analyze header struct to be filled
 * @param fmin minimum pixel value in all frames that will be written
 * @param fmax maximum pixel value in all frames that will be written
 * @return errstatus, which is STATUS_OK (0) when call was successful,
 * and >0 in case of an error.
 */
int imgSetAnalyzeHeader(
  IMG *img, const char *dbname, ANALYZE_DSR *dsr, float fmin, float fmax
) {
  struct tm *st;
  char *cptr;
  float g;

  if(IMG_TEST) printf("\nimgSetAnalyzeHeader(*img, *dsr)\n");
  
  /* Check the input */
  if(img==NULL) return STATUS_FAULT;
  if(img->status!=IMG_STATUS_INITIALIZED && img->status!=IMG_STATUS_OCCUPIED)
    return STATUS_FAULT;
  imgSetStatus(img, STATUS_FAULT);
  if(dsr==NULL) return STATUS_FAULT;

  /* Byte order */
  if(img->_fileFormat==IMG_ANA_L) dsr->little=1; else dsr->little=0;
  /* Header key */
  memset(&dsr->hk, 0, sizeof(ANALYZE_HEADER_KEY));
  memset(&dsr->dime, 0, sizeof(ANALYZE_HEADER_IMGDIM));
  memset(&dsr->hist, 0, sizeof(ANALYZE_HEADER_HISTORY));
  dsr->hk.sizeof_hdr=348;
  strcpy(dsr->hk.data_type, "");
  cptr=strrchr(dbname, '/'); if(cptr==NULL) cptr=strrchr(dbname, '\\');
  if(cptr!=NULL) cptr++; if(cptr==NULL) cptr=(char*)dbname;
  strncpy(dsr->hk.db_name, cptr, 17);
  dsr->hk.extents=16384;
  dsr->hk.regular='r';
  /* Image dimension */
  dsr->dime.dim[0]=4;
  dsr->dime.dim[1]=img->dimx;
  dsr->dime.dim[2]=img->dimy;
  dsr->dime.dim[3]=img->dimz;
  dsr->dime.dim[4]=img->dimt;
  dsr->dime.datatype=ANALYZE_DT_SIGNED_SHORT;
  dsr->dime.bitpix=16;
  dsr->dime.pixdim[0]=0.0;
  dsr->dime.pixdim[1]=img->sizex;
  dsr->dime.pixdim[2]=img->sizey;
  dsr->dime.pixdim[3]=img->sizez;
  dsr->dime.pixdim[4]=0.0;
  dsr->dime.funused1=0.0; /* Scale factor is set later */
  /* dsr.dime.funused2=img->zoom; */ /* Reconstruction zoom */
  dsr->dime.funused3=img->isotopeHalflife;
  /* Data history */
  if(img->decayCorrection==IMG_DC_CORRECTED)
    strcpy(dsr->hist.descrip, "Decay corrected.");
  else if(img->decayCorrection==IMG_DC_NONCORRECTED)
    strcpy(dsr->hist.descrip, "No decay correction.");
  else
    strcpy(dsr->hist.descrip, "");
  strncpy(dsr->hist.scannum, img->studyNr, 10);
  st=localtime(&img->scanStart);
  if(st!=NULL) {
    strftime(dsr->hist.exp_date, 10, "%Y%m%d", st);
    strftime(dsr->hist.exp_time, 10, "%H:%M:%S", st);
  } else {
    strcpy(dsr->hist.exp_date, "19000101");
    strcpy(dsr->hist.exp_time, "00:00:00");
  }
  /* Determine and set scale factor and cal_min & cal_max */
  if(fmin<fmax) {
    dsr->dime.cal_min=fmin; dsr->dime.cal_max=fmax;
  } else { /* not given in function call, try to find those here */
    if(img->status==IMG_STATUS_OCCUPIED &&
       imgMinMax(img, &dsr->dime.cal_min, &dsr->dime.cal_max)==0) {}
    else return STATUS_FAULT;
  }
  if(fabs(dsr->dime.cal_min) > fabs(dsr->dime.cal_max)) g=fabs(dsr->dime.cal_min);
  else g = fabs(dsr->dime.cal_max);
  /* if(fabs(dsr->dime.cal_min)>fabs(dsr->dime.cal_max))
     g=fabs(dsr->dime.cal_min); */
  /* else g=fabs(dsr->dime.cal_max); */
  if(g<1E-20) g=1.0; else g=32767./g; dsr->dime.funused1=1.0/g;
  /* Set header glmin & glmax */
  dsr->dime.glmin=temp_roundf(fmin*g); dsr->dime.glmax=temp_roundf(fmax*g);
  /* printf("glmin=%d\n", dsr->dime.glmin); */
  /* printf("glmax=%d\n", dsr->dime.glmax); */

  imgSetStatus(img, STATUS_OK);
  return STATUS_OK;
}
Example #14
0
/*!
 * Write Analyze 7.5 image.
 *   Analyze database name must be given with path. Path must exist.
 *   Image and header files with .img and .hdr extensions are created.
 *   Existing files are overwritten.
 *   anaFlipping() determines whether image is flipped in z-direction;
 *   image is always flipped in x,y-directions.
 *   Byte order is determined based on _fileFormat field.
 * 
 * @param dbname analyze database name with path, without extension
 * @param img pointer to IMG data
 * @return 0 if ok, 1 invalid input, 2 invalid image status (image not occupied), 
 * 3 failed to resolve extreme values (min and max),
 * 12 failed to allocate temp memory, 14 failed to open file for writing,
 * 15 failed to write data, 21 failed to write header, and
 * sets IMG->statmsg in case of error
*/
int imgWriteAnalyze(const char *dbname, IMG *img) {
  FILE *fp;
  int ret, fi, pi, xi, yi, little;
  float g;
  ANALYZE_DSR dsr;
  char datfile[FILENAME_MAX], hdrfile[FILENAME_MAX], siffile[FILENAME_MAX];
  const char *cptr;
  int pxlNr=0;
  struct tm *st;
  short int *sdata, *sptr, smin, smax;
  SIF sif;


  if(IMG_TEST) printf("imgWriteAnalyze(%s, *img)\n", dbname);

  /* Check the arguments */
  imgSetStatus(img, STATUS_OK);
  if(img==NULL || img->status!=IMG_STATUS_OCCUPIED) {
    imgSetStatus(img, STATUS_FAULT); return(2);}
  if(dbname==NULL || !dbname[0]) {imgSetStatus(img, STATUS_FAULT); return(1);}
  
  /* Make the image and header filenames */  
  strcpy(datfile, dbname); strcat(datfile, ".img");
  strcpy(hdrfile, dbname); strcat(hdrfile, ".hdr");
  strcpy(siffile, dbname); strcat(siffile, ".sif");


  /*
   *  Fill Analyze header
   */
  if(img->_fileFormat==IMG_ANA_L) dsr.little=1; else dsr.little=0;
  /* Header key */
  memset(&dsr.hk, 0, sizeof(ANALYZE_HEADER_KEY));
  memset(&dsr.dime, 0, sizeof(ANALYZE_HEADER_IMGDIM));
  memset(&dsr.hist, 0, sizeof(ANALYZE_HEADER_HISTORY));
  dsr.hk.sizeof_hdr=348;
  strcpy(dsr.hk.data_type, "");
  cptr=strrchr(dbname, '/'); if(cptr==NULL) cptr=strrchr(dbname, '\\');
  if(cptr!=NULL) cptr++; if(cptr==NULL) cptr=dbname;
  strncpy(dsr.hk.db_name, cptr, 17);
  dsr.hk.extents=16384;
  dsr.hk.regular='r';
  /* Image dimension */
  dsr.dime.dim[0]=4;
  dsr.dime.dim[1]=img->dimx;
  dsr.dime.dim[2]=img->dimy;
  dsr.dime.dim[3]=img->dimz;
  dsr.dime.dim[4]=img->dimt;
  dsr.dime.datatype=ANALYZE_DT_SIGNED_SHORT;
  dsr.dime.bitpix=16;
  dsr.dime.pixdim[0]=0.0;
  dsr.dime.pixdim[1]=img->sizex;
  dsr.dime.pixdim[2]=img->sizey;
  dsr.dime.pixdim[3]=img->sizez;
  dsr.dime.pixdim[4]=0.0;
  dsr.dime.funused1=0.0; /* Scale factor is set later */
  /* dsr.dime.funused2=img->zoom; */ /* Reconstruction zoom */
  dsr.dime.funused3=img->isotopeHalflife;
  /* Data history */
  if(img->decayCorrection==IMG_DC_CORRECTED)
    strcpy(dsr.hist.descrip, "Decay corrected.");
  else if(img->decayCorrection==IMG_DC_NONCORRECTED) 
    strcpy(dsr.hist.descrip, "No decay correction.");
  else
    strcpy(dsr.hist.descrip, "");
  strncpy(dsr.hist.scannum, img->studyNr, 10);
  st=localtime(&img->scanStart);
  if(st!=NULL) {
    strftime(dsr.hist.exp_date, 10, "%Y-%m-%d", st);
    strftime(dsr.hist.exp_time, 10, "%H:%M:%S", st);
  } else {
    strncpy(dsr.hist.exp_date, "1900-01-01", 10);
    strncpy(dsr.hist.exp_time, "00:00:00", 10);
  }

  /*
   *  Scale data to short int range
   *  Determine and set scale factor and cal_min & cal_max
   */
  if(IMG_TEST) printf("scaling data to short ints\n");
  ret=imgMinMax(img, &dsr.dime.cal_min, &dsr.dime.cal_max);
  if(ret) {imgSetStatus(img, STATUS_FAULT); return(3);}
  if(fabs(dsr.dime.cal_min)>fabs(dsr.dime.cal_max)) g=fabs(dsr.dime.cal_min);
  else g=fabs(dsr.dime.cal_max);
  if(g<1E-20) g=1.0; else g=32767./g; dsr.dime.funused1=1.0/g;
  if(IMG_TEST) printf("min=%g max=%g scale_factor=%g\n",
    dsr.dime.cal_min, dsr.dime.cal_max, dsr.dime.funused1);

  /* Allocate memory for short int array */
  pxlNr=(img->dimx)*(img->dimy)*(img->dimz);
  sdata=malloc(pxlNr*sizeof(short int));
  if(sdata==NULL) {
    imgSetStatus(img, STATUS_NOMEMORY);
    return 12;
  }

  /* Open image data file for write */
  if((fp=fopen(datfile, "wb")) == NULL) {
    imgSetStatus(img, STATUS_CANTWRITEIMGFILE);
    free(sdata);
    return 14;
  }

  /* Copy and write image matrix data to short int array */
  /* Data is written one frame at a time */
  smin=smax=temp_roundf(g*img->m[0][0][0][0]);
  for(fi=0; fi<img->dimt; fi++) {
    sptr=sdata;
    if(anaFlipping()==0) {
      for(pi=0; pi<img->dimz; pi++)
        for(yi=img->dimy-1; yi>=0; yi--)
          for(xi=img->dimx-1; xi>=0; xi--) {
            *sptr=temp_roundf(g*img->m[pi][yi][xi][fi]);
            if(*sptr>smax) smax=*sptr; else if(*sptr<smin) smin=*sptr;
            sptr++;
          }
    } else {
      for(pi=img->dimz-1; pi>=0; pi--)
        for(yi=img->dimy-1; yi>=0; yi--)
          for(xi=img->dimx-1; xi>=0; xi--) {
            *sptr=temp_roundf(g*img->m[pi][yi][xi][fi]);
            if(*sptr>smax) smax=*sptr; else if(*sptr<smin) smin=*sptr;
            sptr++;
          }
    }
    /* Change byte order if necessary */
    little=little_endian();
    if(little!=dsr.little)
      swabip(sdata, pxlNr*sizeof(short int));
    /* Write image data */
    if(fwrite(sdata, 2, pxlNr, fp) != pxlNr) {
      imgSetStatus(img, STATUS_CANTWRITEIMGFILE);
      free(sdata); fclose(fp);
      return 15;
    }
  }
  /* Done writing */
  fclose(fp);
  free(sdata);

  if(IMG_TEST) printf("smin=%d smax=%d\n", smin, smax);

  /* Set header glmin & glmax */
  dsr.dime.glmin=smin; dsr.dime.glmax=smax;
  
  /* Write Analyze header */
  ret=anaWriteHeader(hdrfile, &dsr);
  if(ret) {
    imgSetStatus(img, STATUS_CANTWRITEHEADERFILE);
    return 21;
  }
  imgSetStatus(img, STATUS_OK);

  /* Otherwise ready, but check if SIF should/can be written */
  sifInit(&sif);
  /* Try to read existing SIF */
  ret=sifRead(siffile, &sif);
  if(ret==0) { // SIF could be read
    if(sif.frameNr==img->dimt) {
      /* If size matches, then update the contents, but keep counts, in case
         previous SIF comes with actual count info from scanner */
      ret=img2sif(img, &sif, 1, 1, 0);
    } else {
      /* otherwise create SIF contents */
      ret=img2sif(img, &sif, 1, 1, 1);
    }
  } else {
    /* otherwise create SIF contents */
    ret=img2sif(img, &sif, 1, 1, 1);
  }
  if(ret!=0) {
    /* SIF data could not be made: do not give error, just do not write it */
    if(IMG_TEST>0) printf("SIF contents could not be filled.\n");
    return 0;
  }
  /* Write SIF */
  ret=sifWrite(&sif, siffile);  
  if(ret!=0) {
    /* SIF could not be written: do not give error, just do not write it */
    if(IMG_TEST>0)
      fprintf(stderr, "Error: SIF could not be written (%d).\n", ret);
  }

  imgSetStatus(img, STATUS_OK);
  return 0;
}
Example #15
0
int gsl_sf_bessel_jl_steed_array(const int lmax, const double x, double * jl_x)
{
  /* CHECK_POINTER(jl_x) */

  if(lmax < 0 || x < 0.0) {
    int j;
    for(j=0; j<=lmax; j++) jl_x[j] = 0.0;
    GSL_ERROR ("error", GSL_EDOM);
  }
  else if(x == 0.0) {
    int j;
    for(j=1; j<=lmax; j++) jl_x[j] = 0.0;
    jl_x[0] = 1.0;
    return GSL_SUCCESS;
  }
  else if(x < 2.0*GSL_ROOT4_DBL_EPSILON) {
    /* first two terms of Taylor series */
    double inv_fact = 1.0;  /* 1/(1 3 5 ... (2l+1)) */
    double x_l      = 1.0;  /* x^l */
    int l;
    for(l=0; l<=lmax; l++) {
      jl_x[l]  = x_l * inv_fact;
      jl_x[l] *= 1.0 - 0.5*x*x/(2.0*l+3.0);
      inv_fact /= 2.0*l+3.0;
      x_l      *= x;
    }
    return GSL_SUCCESS;
  }
  else {
    /* Steed/Barnett algorithm [Comp. Phys. Comm. 21, 297 (1981)] */
    double x_inv = 1.0/x;
    double W = 2.0*x_inv;
    double F = 1.0;
    double FP = (lmax+1.0) * x_inv;
    double B = 2.0*FP + x_inv;
    double end = B + 20000.0*W;
    double D = 1.0/B;
    double del = -D;
    
    FP += del;
    
    /* continued fraction */
    do {
      B += W;
      D = 1.0/(B-D);
      del *= (B*D - 1.);
      FP += del;
      if(D < 0.0) F = -F;
      if(B > end) {
	GSL_ERROR ("error", GSL_EMAXITER);
      }
    }
    while(fabs(del) >= fabs(FP) * GSL_DBL_EPSILON);
    
    FP *= F;
    
    if(lmax > 0) {
      /* downward recursion */
      double XP2 = FP;
      double PL = lmax * x_inv;
      int L  = lmax;
      int LP;
      jl_x[lmax] = F;
      for(LP = 1; LP<=lmax; LP++) {
	jl_x[L-1] = PL * jl_x[L] + XP2;
	FP = PL*jl_x[L-1] - jl_x[L];
	XP2 = FP;
	PL -= x_inv;
	--L;
      }
      F = jl_x[0];
    }
    
    /* normalization */
    W = x_inv / sqrt(FP*FP + F*F);
    jl_x[0] = W*F;
    if(lmax > 0) {
      int L;
      for(L=1; L<=lmax; L++) {
	jl_x[L] *= W;
      }
    }

    return GSL_SUCCESS;
  }
}
Example #16
0
 /**
  * Compare the ballast fraction setting with the specified value.
  *
  * @return true if the current setting is the same, false if the
  * value is different or if there is no value
  */
 bool CompareBallastFraction(fixed value) const {
   return ballast_fraction_available &&
     fabs(ballast_fraction - value) <= fixed(0.01);
 }
Example #17
0
/////////////////////////////////////////////////////////
/// Cooler & Filter Wheel monitoring
/////////////////////////////////////////////////////////
void ATIKCCD::TimerHit()
{
    double currentTemperature = TemperatureN[0].value;

    int flags, level, minlvl, maxlvl, setpoint;

    pthread_mutex_lock(&accessMutex);
    int rc = ArtemisCoolingInfo(hCam, &flags, &level, &minlvl, &maxlvl, &setpoint);
    pthread_mutex_unlock(&accessMutex);

    if (rc != ARTEMIS_OK)
    {
        LOGF_ERROR("Cooling Info inquiry failed (%d)", rc);
        genTimerID = SetTimer(TEMP_TIMER_MS);
        return;
    }

    LOGF_DEBUG("Cooling: flags (%d) level (%d), minlvl (%d), maxlvl (%d), setpoint (%d)", flags, level, minlvl, maxlvl, setpoint);

    int temperature = 0;
    pthread_mutex_lock(&accessMutex);
    rc = ArtemisTemperatureSensorInfo(hCam, 1, &temperature);
    pthread_mutex_unlock(&accessMutex);
    TemperatureN[0].value = temperature / 100.0;

    switch (TemperatureNP.s)
    {
        case IPS_IDLE:
        case IPS_OK:
            if (fabs(currentTemperature - TemperatureN[0].value)  > TEMP_THRESHOLD / 10.0)
            {
                IDSetNumber(&TemperatureNP, nullptr);
            }
            break;

        case IPS_ALERT:
            break;

        case IPS_BUSY:
            // If we're within threshold, let's make it BUSY ---> OK
            if (fabs(TemperatureRequest - TemperatureN[0].value)  <= TEMP_THRESHOLD)
            {
                TemperatureNP.s = IPS_OK;
            }
            IDSetNumber(&TemperatureNP, nullptr);
            break;
    }

    if (HasCooler())
    {
        bool coolerChanged = false;
        double coolerPower = static_cast<double>(level) / maxlvl * 100.0;
        if (fabs(CoolerN[0].value - coolerPower) > 0.01)
        {
            CoolerN[0].value = coolerPower;
            coolerChanged = true;
        }

        // b5 0 = normal control 1=warming up
        // b6 0 = cooling off 1 = cooling on
        if (!(flags & 0x20) && // Normal Control?
                (flags & 0x40))   // Cooling On?
        {
            if (CoolerNP.s != IPS_BUSY)
                coolerChanged = true;
            CoolerNP.s = IPS_BUSY;
        }
        // Otherwise cooler is either warming up or not active
        else
        {
            if (CoolerNP.s != IPS_IDLE)
                coolerChanged = true;
            CoolerNP.s = IPS_IDLE;
        }

        if (coolerChanged)
            IDSetNumber(&CoolerNP, nullptr);
    }

    // If filter wheel is in motion
    if (FilterSlotNP.s == IPS_BUSY)
    {
        int numFilters, moving, currentPos, targetPos;
        pthread_mutex_lock(&accessMutex);
        int rc = ArtemisFilterWheelInfo(hCam, &numFilters, &moving, &currentPos, &targetPos);
        pthread_mutex_unlock(&accessMutex);

        if (rc != ARTEMIS_OK)
        {
            LOGF_ERROR("Querying internal filter wheel failed (%d).", rc);
        }
        else
        {
            if (moving == 0 && currentPos == targetPos)
            {
                SelectFilterDone(currentPos + 1);
            }
        }

    }

    genTimerID = SetTimer(TEMP_TIMER_MS);
}
Example #18
0
 /**
  * Compare the ballast overload setting with the specified value.
  *
  * @return true if the current setting is the same, false if the
  * value is different or if there is no value
  */
 bool CompareBallastOverload(fixed value) const {
   return ballast_overload_available &&
     fabs(ballast_overload - value) <= fixed(0.01);
 }
Example #19
0
void dd::GibbsSampling::learn(const int & n_epoch, const int & n_sample_per_epoch, 
                              const double & stepsize, const double & decay, 
                              const double reg_param, const double reg1_param,
                              const bool is_quiet){
			      //const std::string meta_file, const bool is_quiet){

  Timer t_total;

  double current_stepsize = stepsize;

  Timer t;
  int nvar = this->factorgraphs[0].n_var;
  int nnode = n_numa_nodes + 1;
  int nweight = this->factorgraphs[0].n_weight;

//  int num_sources_per_var[nvar];
//  for (int i = 0; i < nvar; i++) {
//    num_sources_per_var[i] = 1;
//  }
//  std::string full_feature_names[nvar];
//  int feature_values[nvar];
//  std::map<std::string, int*> feature_var_map;
//  std::map<std::string,int*>::iterator it;
//  int num_feature_names = 0;
//
//  std::cout<<"Opening file "<<meta_file<<std::endl;
//  std::ifstream myfile;
//  std::string data;
//  myfile.open (meta_file);
//  if (myfile.good()) {
//    while ( std::getline(myfile,data)) {
//      istringstream iss(data);
//      std::string temp;
//      std::getline(iss, temp, '\t');
//      int feature_id = std::stoi(temp, nullptr);
//      std::string table_name;
//      std::getline(iss, table_name, '\t');
//      std::string feature_data;
//      std::getline(iss, feature_data);
//      feature_data = feature_data.substr(1, feature_data.length() - 2);
//      temp = feature_data.substr(14 + feature_data.find("source_count\":"), feature_data.find(",feature_name") - feature_data.find("source_count\":") - 14 );
//      num_sources_per_var[feature_id] = std::stoi(temp, nullptr);
//      temp = feature_data.substr(13 + feature_data.find("feature_name:"), feature_data.find(",feature_value") - feature_data.find("feature_name:") - 13 );
//      full_feature_names[feature_id] = table_name + "." + temp;
//      temp = feature_data.substr(14 + feature_data.find("feature_value:"), feature_data.find("}") - feature_data.find("feature_value:") - 14 );
//      feature_values[feature_id] = (int)std::stod(temp, nullptr);
//      it = feature_var_map.find(full_feature_names[feature_id]);
//      if (it != feature_var_map.end()) {
//        int* arr = it;
//        arr[feature_values[feature_id]] = feature_id;
//      } else {
//        feature_var_map[full_feature_names[feature_id]] = new int[10]; // NOTE: Hardcoded, change later.
//      }
//      feature_var_map[full_feature_names[feature_id]] = ;
//      std::cout << feature_id << " -- " << table_name << " - " << full_feature_names[feature_id] << " ;; " << feature_values[feature_id] << " ,, " << num_sources_per_var[feature_id] << std::endl;
//    }
//  }
//  myfile.close();
  
  
  // single node samplers
  std::vector<SingleNodeSampler> single_node_samplers;
  for(int i=0;i<=n_numa_nodes;i++){
    single_node_samplers.push_back(SingleNodeSampler(&this->factorgraphs[i], 
      n_thread_per_numa, i, false, 0, learn_non_evidence));
  }

  std::unique_ptr<double[]> ori_weights(new double[nweight]);
  memcpy(ori_weights.get(), this->factorgraphs[0].infrs->weight_values, sizeof(double)*nweight);

  // learning epochs
  for(int i_epoch=0;i_epoch<n_epoch;i_epoch++){

    if (!is_quiet) {
      std::cout << std::setprecision(2) << "LEARNING EPOCH " << i_epoch * nnode <<  "~" 
        << ((i_epoch+1) * nnode) << "...." << std::flush;
    }

    t.restart();
    
    // set stepsize
    for(int i=0;i<nnode;i++){
      single_node_samplers[i].p_fg->stepsize = current_stepsize;
    }

    // performs stochastic gradient descent with sampling
    for(int i=0;i<nnode;i++){
      single_node_samplers[i].sample_sgd();
    }

    // wait the samplers to finish
    for(int i=0;i<nnode;i++){
      single_node_samplers[i].wait_sgd();
    }

    FactorGraph & cfg = this->factorgraphs[0];

    // sum the weights and store in the first factor graph
    // the average weights will be calculated and assigned to all factor graphs 
    for(int i=1;i<=n_numa_nodes;i++){
      FactorGraph & cfg_other = this->factorgraphs[i];
      for(int j=0;j<nweight;j++){
        cfg.infrs->weight_values[j] += cfg_other.infrs->weight_values[j];
      }
    }

    // calculate average weights and regularize weights
    for(int j=0;j<nweight;j++){
      cfg.infrs->weight_values[j] /= nnode;
      if(cfg.infrs->weights_isfixed[j] == false){
        cfg.infrs->weight_values[j] *= (1.0/(1.0+reg_param*current_stepsize));
	double l1delta = reg1_param * current_stepsize;
	if (cfg.infrs->weight_values[j] > l1delta) {		
	  cfg.infrs->weight_values[j] -= l1delta;
	} else if (cfg.infrs->weight_values[j] < - l1delta) {
	  cfg.infrs->weight_values[j] += l1delta;
	} else {
	  cfg.infrs->weight_values[j] = 0;
	}
      }
    }

    // set weights for other factor graph to be the same as the first factor graph
    for(int i=1;i<=n_numa_nodes;i++){
      FactorGraph &cfg_other = this->factorgraphs[i];
      for(int j=0;j<nweight;j++){
        if(cfg.infrs->weights_isfixed[j] == false){
          cfg_other.infrs->weight_values[j] = cfg.infrs->weight_values[j];
        }
      }
    }    

    // calculate the norms of the difference of weights from the current epoch
    // and last epoch
    double lmax = -1000000;
    double l2=0.0;
    for(int i=0;i<nweight;i++){
      double diff = fabs(ori_weights[i] - cfg.infrs->weight_values[i]);
      ori_weights[i] = cfg.infrs->weight_values[i];
      l2 += diff*diff;
      if(lmax < diff){
        lmax = diff;
      }
    }
    lmax = lmax/current_stepsize;
    
    double elapsed = t.elapsed();
    if (!is_quiet) {
      std::cout << "" << elapsed << " sec.";
      std::cout << ","  << (nvar*nnode)/elapsed << " vars/sec." << ",stepsize=" << current_stepsize << ",lmax=" << lmax << ",l2=" << sqrt(l2)/current_stepsize << std::endl;
    }

    current_stepsize = current_stepsize * decay;

  }

  double elapsed = t_total.elapsed();
  std::cout << "TOTAL LEARNING TIME: " << elapsed << " sec." << std::endl;
}
Example #20
0
 /**
  * Compare the wing loading setting with the specified value.
  *
  * @return true if the current setting is the same, false if the
  * value is different or if there is no value
  */
 bool CompareWingLoading(fixed value) const {
   return wing_loading_available &&
     fabs(wing_loading - value) <= fixed(0.5);
 }
Example #21
0
//----------------------------------------------------------------------
void MapLighter::CalcMapShadow (const Vector3 &sun)
{
    uint  height = MapUtil::getSingleton().getMapHeight ();
    uint  width = MapUtil::getSingleton().getMapWidth ();
    Real *heightMap = MapUtil::getSingleton().getHeightData ();

    std::cout << "Shadow Map Calc : ";
	std::cout << "Dir: " << sun.x << "," << sun.y << "," << sun.z << std::endl;

	/* Allocate space for the shadow map */
    const uint size = width * height;
    uchar *ShadowMap = new uchar[size];
    memset (ShadowMap, 0, size * sizeof (uchar));
    // Assign the color texture
    mShadowMap.loadDynamicImage  (ShadowMap, 
        static_cast <size_t> (width), 
        static_cast <size_t> (height), 1, 
        PF_L8, true);

    /* Make sure the light source is normalised */
    const Vector3 LightDirection = sun.normalisedCopy();

//    LightDirection.x /= PagingLandScapeOptions::getSingleton().scale.x;
//    LightDirection.y /= PagingLandScapeOptions::getSingleton().scale.y;
//    LightDirection.z /= PagingLandScapeOptions::getSingleton().scale.z;


    if (LightDirection.y == 0.0f ) 
    {
        /* light vector horizontal*/
        /* all in shadow */
        return;
    }

    /* For each heightmap location */ 
    uint index_pixel = 0;
    for (uint j = 0; j < height; j++) 
     { 
        DEBUG_PROGRESS_OUTPUT(".")
        for (uint i = 0; i < width; i++) 
        {
            /* If already in shadow no need to compute again ?
             what if we store precise height at which it is not shadowed ?*/
            if (ShadowMap[index_pixel] == 0)
            {
                /* a line passing starting from heightmap point 
                    and following light direction*/
                Real terrain_height = heightMap[ index_pixel ];
                    
                if (fabs(LightDirection.x) < fabs(LightDirection.z))
                { 
                    int index_z = 1;
                    float index_x = LightDirection.x / LightDirection.z;
                    float index_y = LightDirection.y / LightDirection.z;
                    if (LightDirection.z < 0) 
                    {
                        index_x = -index_x; 
                        index_y = -index_y;
                        index_z = -index_z; 
                    }
                    int z = j + index_z;
                    float x = i + index_x;
                    terrain_height += index_y;
                    uint zmap = ((uint)z);
                    uint xmap = ((uint)x);
                    while  (//terrain_height >= 0.0f &&
                            xmap < width &&
                            zmap < height)
                    {        
                        if ((zmap < height) && 
                             heightMap[xmap + zmap*width] <= terrain_height)
                             ShadowMap[xmap + zmap*width] = 255; 
                    
                        x += index_x;
                        z += index_z;
                        terrain_height += index_y;

                        zmap = ((uint)z);
                        xmap = ((uint)x);
                     }
                }
                else 
                {
                    int index_x = 1;
                    float index_z = LightDirection.z / LightDirection.x;
                    float index_y = LightDirection.y / LightDirection.x;
                    if (LightDirection.x < 0) 
                    {
                        index_x = -index_x;  
                        index_y = -index_y;
                        index_z = -index_z;
                    }

                    float z = j + index_z;
                    int x = i + index_x;
                    terrain_height += index_y;
                    uint zmap = ((uint)z);
                    uint xmap = ((uint)x);
                    while (//terrain_height >= 0.0f &&
                            xmap < width &&
                            zmap < height)
                    {
                        if ((zmap < height) && 
                            heightMap[xmap + zmap*width] <= terrain_height)
                            ShadowMap[xmap + zmap*width] = 255;   
                        x += index_x; 
                        z += index_z;
                        terrain_height += index_y;
                        zmap = ((uint)z);
                        xmap = ((uint)x);
                    }
                 }
            }
            index_pixel++;
        }
    }

    std::cout << "\n";
}
Example #22
0
 /**
  * Compare the bugs setting with the specified value.
  *
  * @return true if the current setting is the same, false if the
  * value is different or if there is no value
  */
 bool CompareBugs(fixed value) const {
   return bugs_available && fabs(bugs - value) <= fixed(0.01);
 }
Example #23
0
void lndr_step(Lander *lander, float dT)
{
    lander->x += lander->dX * dT;
    lander->y += lander->dY * dT;
    lander->dY += lander->dYY * dT;

    // Handle rotations
    if((glob_game.keysDown & GMK_LEFT) && lander->rotation < PI_2)
    {
        float dif = lander->dR * dT;
        if(lander->rotation + dif < PI_2)
            lander->rotation += dif;
        else
            lander->rotation = PI_2;
    }
    if((glob_game.keysDown & GMK_RIGHT) && lander->rotation > -PI_2)
    {
        float dif = lander->dR * dT;
        if(lander->rotation - dif > -PI_2)
            lander->rotation -= dif;
        else
            lander->rotation = -PI_2;
    }

    // Make zero sticky to make sure a vertical landing is possible
    if(fabs(lander->rotation) < lander->dR * dT - 0.01)
        lander->rotation = 0;

    // Deal with thrust
    double thrust = lndr_thrust_for_state(lander);
    lander->dX += -sin(lander->rotation) * thrust;
    lander->dY -= -cos(lander->rotation) * thrust;

    // Handle jet animations
    if(lander->jetState == JS_ON || lander->jetState == JS_INCREASING)
        lander->jetFrames += dT * 1000;
    else if(lander->jetState == JS_DECREASING)
        lander->jetFrames -= dT * 1000;

    if((glob_game.keysDown & GMK_UP) && lander->prevJetState != JS_INCREASING && lander->prevJetState != JS_ON)
        lander->jetState = JS_INCREASING;
    else if(lander->jetFrames > FLAME_GROW_RATE && lander->jetState == JS_INCREASING)
    {
        lander->jetFrames = 0;
        lander->jetState = JS_ON;
    }
    else if(!(glob_game.keysDown & GMK_UP) && lander->prevJetState == JS_ON)
    {
        lander->jetFrames = FLAME_GROW_RATE;
        lander->jetState = JS_DECREASING;
    }
    else if(!(glob_game.keysDown & GMK_UP) && lander->prevJetState == JS_INCREASING)
        lander->jetState = JS_DECREASING;
    else if(lander->jetState == JS_DECREASING && lander->jetFrames <= 0)
    {
        lander->jetState = JS_OFF;
        lander->jetFrames = 0;
    }

    // Save the current jet state
    lander->prevJetState = lander->jetState;

    // Generate new matrices
    lndr_gen_mv_matrix(lander);
    lndr_gen_jet_mv_matrix(lander);
}
Example #24
0
 /**
  * Compare the QNH setting with the specified value.
  *
  * @return true if the current setting is the same, false if the
  * value is different or if there is no value
  */
 bool CompareQNH(AtmosphericPressure value) const {
   return qnh_available &&
     fabs(qnh.GetHectoPascal() - value.GetHectoPascal()) <= fixed(0.5);
 }
Example #25
0
void
RCM2Material ::  giveRealPrincipalStressVector3d(FloatArray &answer, GaussPoint *gp,
                                                 FloatArray &principalStrain,
                                                 FloatMatrix &tempCrackDirs,
                                                 TimeStep *atTime)
//
// returns real principal stress vector in 3d stress space of receiver according to
// previous level of stress and current
// strain increment, the only way, how to correctly update gp records
// updates principal strain and stress of the receiver's status.
//
{
    int i, iter, ind;
    double maxErr;
    FloatArray crackStrainVector, reducedTotalStrainVector;
    FloatArray strainIncrement, crackStrainIterativeIncrement;
    FloatArray prevPrincipalStrain;
    FloatArray dSigma;
    FloatArray elastStrain, sigmaEl, sigmaCr(3);
    FloatArray fullDSigma;
    IntArray activatedCracks, crackMapping;
    FloatMatrix dcr, de, decr, fullDecr, crackDirs;
    RCM2MaterialStatus *status = ( RCM2MaterialStatus * ) this->giveStatus(gp);

    /*
     * if (status -> giveStressVector() == NULL) status->letStressVectorBe(new FloatArray(this->giveSizeOfReducedStressStrainVector(gp->giveMaterialMode())));
     * if (status -> giveStrainVector() == NULL) status->letStrainVectorBe(new FloatArray(this->giveSizeOfReducedStressStrainVector(gp->giveMaterialMode())));
     * // if (status -> givePlasticStrainVector() == NULL) status->letPlasticStrainVectorBe(new FloatArray(6));
     * if (status -> giveStressIncrementVector() == NULL) status->letStressIncrementVectorBe(new FloatArray(this->giveSizeOfReducedStressStrainVector(gp->giveMaterialMode())));
     * if (status -> giveStrainIncrementVector() == NULL) status->letStrainIncrementVectorBe(new FloatArray(this->giveSizeOfReducedStressStrainVector(gp->giveMaterialMode())));
     * // if (status -> givePlasticStrainIncrementVector() == NULL) status->letPlasticStrainIncrementVectorBe(new FloatArray(6));
     */

    /*
     * // totalStressVector = gp -> giveStressVector()->GiveCopy();
     * reducedTotalStrainVector = status -> giveStrainVector();
     * reducedTotalStrainVector.add(fullStrainIncrement);
     * crossSection->giveFullCharacteristicVector(totalStrainVector, gp, reducedTotalStrainVector);
     * //delete reducedTotalStrainVector;
     * // plasticStrainVector = status -> givePlasticStrainVector()->GiveCopy();
     *
     *
     * // already cracked - next directions are determined
     * // according to principal strain directions
     * status->giveTempCrackDirs(tempCrackDirs);
     * this->computePrincipalValDir (principalStrain, tempCrackDirs,
     *              totalStrainVector,
     *              principal_strain);
     * status->letTempCrackDirsBe (tempCrackDirs);
     */
    status->giveCrackStrainVector(crackStrainVector); // local one
    status->giveCrackDirs(crackDirs);
    if ( principalStrain.containsOnlyZeroes() ) {
        // keep old principal values
        status->letTempCrackDirsBe(crackDirs);
    } else {
        this->sortPrincDirAndValCloseTo(& principalStrain,
                                        & tempCrackDirs, & crackDirs);
        status->letTempCrackDirsBe(tempCrackDirs);
    }

    // compute de in local system
    // for iso materials no transformation if stiffness required
    //
    // local strain increment
    status->givePrevPrincStrainVector(prevPrincipalStrain);
    strainIncrement.beDifferenceOf(principalStrain, prevPrincipalStrain);
    status->letPrincipalStrainVectorBe(principalStrain);

    this->giveNormalElasticStiffnessMatrix(de, FullForm, TangentStiffness,
                                           gp, atTime, tempCrackDirs);
    //
    // construct mapping matrix of active cracks
    // this mapping will dynamically change as
    // some crack can unlo or reload
    //
    this->updateActiveCrackMap(gp);
    status->giveCrackMap(crackMapping);
    // start iteration until stress computed from elastic increment
    // is equal to stress computed from cracking strain increment
    // we do this computation in reduced stress strain space
    dSigma.resize(0);
    for ( iter = 1; iter <= 20; iter++ ) {
        //
        // first check if already cracked
        //
        if ( status->giveNumberOfTempActiveCracks() ) {
            // active crack exist
            this->giveCrackedStiffnessMatrix(dcr, TangentStiffness, gp, atTime);
            fullDecr = de;
            fullDecr.add(dcr);
            decr.beSubMatrixOf(fullDecr, crackMapping);

            if ( dSigma.giveSize() == 0 ) {
                fullDSigma.beProductOf(de, strainIncrement);
                dSigma.beSubArrayOf(fullDSigma, crackMapping);
            }

            decr.solveForRhs(dSigma, crackStrainIterativeIncrement);
            for ( i = 1; i <= 3; i++ ) {
                if ( ( ind = crackMapping.at(i) ) ) {
                    crackStrainVector.at(i) += crackStrainIterativeIncrement.at(ind);
                }
            }

            // check for crack closing, updates also cracking map
            this->checkIfClosedCracks(gp, crackStrainVector, crackMapping);

            // elastic strain component
            elastStrain.beDifferenceOf(principalStrain, crackStrainVector);
            sigmaEl.beProductOf(de, elastStrain);

            // Stress in cracks
            for ( i = 1; i <= 3; i++ ) {
                if ( crackMapping.at(i) ) {
                    sigmaCr.at(i) = giveNormalCrackingStress(gp, crackStrainVector.at(i), i);
                }
            }

            // update status
            status->letCrackStrainVectorBe(crackStrainVector);
        } else {
            //
            // no active crack exist - elastic behaviour
            //
            elastStrain.beDifferenceOf(principalStrain, crackStrainVector);
            sigmaEl.beProductOf(de, elastStrain);
            sigmaCr.zero();
        }

        // check for new cracks
        // and update crack map if necessary
        // when we update map, we need to add new crack at end
        // because sigmaCr is build
        this->checkForNewActiveCracks(activatedCracks, gp, crackStrainVector,
                                      sigmaEl, sigmaCr, principalStrain);
        if ( activatedCracks.giveSize() ) {
            // update crack map also
            this->updateActiveCrackMap(gp, & activatedCracks);
            status->giveCrackMap(crackMapping); // update crackMap
        }

        //

        // compute unbalanced stress
        // dSigma = sigmaEl - sigmaCr for active cracks
        fullDSigma = sigmaEl;
        fullDSigma.subtract(sigmaCr);
        dSigma.beSubArrayOf(fullDSigma, crackMapping);
        // find max error in dSigma
        // if max err < allovedErr -> stop iteration
        // allowed Err is computed relative to Ft;

        // check only for active cracks
        maxErr = 0.;
        for ( i = 1; i <= dSigma.giveSize(); i++ ) {
            if ( fabs( dSigma.at(i) ) > maxErr ) {
                maxErr = fabs( dSigma.at(i) );
            }
        }

        if ( maxErr < rcm_STRESSRELERROR * this->give(pscm_Ft, gp) ) {
            status->letPrincipalStressVectorBe(sigmaEl);
            answer = sigmaEl;
            return;
        }
    } // loop

    // convergence not reached
    _error("GiveRealStressVector3d - convergence not reached");
}
Example #26
0
 /**
  * Compare the MacCready setting with the specified value.
  *
  * @return true if the current setting is the same, false if the
  * value is different or if there is no value
  */
 bool CompareMacCready(fixed value) const {
   return mac_cready_available && fabs(mac_cready - value) <= fixed(0.05);
 }
Example #27
0
/**
  * \param evt Event that occured
  * \param *wparam Any kind of value that needs to be passed on to the function.
  *               (For example if a structure needs to be passed.)
  * \param lparam Numeric value that needed to be passed for event processing.
  * \param *returnvalue Return value, if any, will be stored here.
  *                     If NULL, no return value shall be stored.
  * \return Boolean value specifying if the event was processed at all. True
  *         means an event was caught by the widget, or by a child.
  *
  * This function does default processing of event, so that each widget
  * does not have to reinvent the wheel. If the widget does not want to
  * process an event, it simply can call upon this function.
  *
  * There is very little practical use for this function apart from internal use
  * when making a widget. You may be more interested in classical CastEvent().
  *
  * For external use you should only use CastEvent().
  *
  * For more information on how the event system works, see the CastEvent() as
  * well. You'll also read of parameters there, as well as return value.
  *
  * \sa CastEvent(glictEvents evt, void* wparam, long lparam, void* returnvalue)
  * \todo Clicking must be handled via verifying if an element has been rendered on
  *       certain pixel, not the way it's done right now. Currently we cannot do
  *       custom-shaped widgets, only rectangle widgets! (This todo is not strictly
  *       related to this function.)
  *
  */
bool glictContainer::DefaultCastEvent(glictEvents evt, void* wparam, long lparam, void* returnvalue) {
	//printf("Default event of type %s passing through %s (%s) with %d children\n", EvtTypeDescriptor(evt), objtype, parent ? parent->objtype : "NULL", this->objects.size());
	switch (evt) {
		case GLICT_KEYPRESS:



			// as default behaviour is that widget doesnt know what to do with
			// a key, it'll pass exec to the top focused item, whatever it
			// might be
			if (glictGlobals.topFocused)
				if (this != glictGlobals.topFocused)
					return glictGlobals.topFocused->CastEvent(evt, wparam, lparam, returnvalue);
			// if it cant find top focused item, or that it is actually the
			// focused item, report that it doesnt know how to proc the event
			return false;

		case GLICT_KEYDOWN:

			return false;
		case GLICT_KEYUP:

			return false;

		case GLICT_MOUSEUP:
		case GLICT_MOUSEDOWN:
		case GLICT_MOUSECLICK:
		case GLICT_MOUSEMOVE:
			{
			std::vector<glictContainer*>::reverse_iterator it;

            // first let's handle dragging.
            // if user dragged over some child object, then we would never reach the code below
            // and that's why we must have dragging handled in separate section
            if (evt == GLICT_MOUSEMOVE) {
			    if (draggedchild) {
                    draggedchild->SetPos(
                        ((glictPos*)wparam)->x - draggedchild->dragrelmouse.x,
                        ((glictPos*)wparam)->y - draggedchild->dragrelmouse.y
                    );
			    }
			} else
            if (evt == GLICT_MOUSEUP) {
                if (draggedchild) {
                    draggedchild->SetPos(
                        ((glictPos*)wparam)->x - draggedchild->dragrelmouse.x,
                        ((glictPos*)wparam)->y - draggedchild->dragrelmouse.y
                    );
                    StopDraggingChild(*(glictPos*)wparam);
                }
            }

            // now let's give our children a chance to handle events
            // in case they handle events, we "handled" the events too, so
            // let's bail out
			if  (((glictPos*)wparam)->x > this->clipleft &&
                 ((glictPos*)wparam)->x < this->clipright &&
                 ((glictPos*)wparam)->y > this->cliptop &&
                 ((glictPos*)wparam)->y < this->clipbottom) { // we are inside the visible area of the widget?

                    // first let's try passing our children the event
                    // if any of them parses the event, let's say to our parent that we parsed the event, too!
					if (objects.size())
						for (it=objects.rbegin(); it != objects.rend(); it++)
							if (it != objects.rend() && *it )  // <=== FIXME do we need this? i don't think so
								if ((*it)->CastEvent(evt, wparam, lparam, returnvalue))
									return true;

			}


			// now, since no child has parsed the event, we can take over!


			if (evt == GLICT_MOUSEDOWN) {
				glictGlobals.lastMousePos.x = ((glictPos*)wparam)->x ; // remembers x and y when pressing the mouse down
				glictGlobals.lastMousePos.y = ((glictPos*)wparam)->y ;
				if (focusable) {
					if (((glictPos*)wparam)->x > this->clipleft &&
					 ((glictPos*)wparam)->x < this->clipright &&
					 ((glictPos*)wparam)->y > this->cliptop &&
					 ((glictPos*)wparam)->y < this->clipbottom ) {


                        if (draggable) {
                            if (!parent)
                                return false;

                            glictPos p;
                            p.x = ((glictPos*)wparam)->x-this->x ;
                            p.y = ((glictPos*)wparam)->y-this->y ;

                            parent->StartDraggingChild(this, p);
                        }




						if (this->OnMouseDown) { // FIXME BUG: even if the object isn't focusable we should be calling OnMouseDown
                            glictPos relpos;
                            relpos.x = ((glictPos*)wparam)->x - this->left - this->containeroffsetx + this->virtualpos.x;
                            relpos.y = ((glictPos*)wparam)->y - this->top - this->containeroffsety + this->virtualpos.y;
                            this->OnMouseDown(&relpos, this);
                        }

						this->Focus(NULL);
						return true;
					 }
				}
			} else if (evt == GLICT_MOUSEUP) {

				if (fabs (((glictPos*)wparam)->x - glictGlobals.lastMousePos.x) < 3 && // upon release verifies the location of mouse, and if nearby then it's a click - cast a click event
					fabs (((glictPos*)wparam)->y - glictGlobals.lastMousePos.y) < 3 ) { // if up to 2 pixels diff
					//printf("Considering it a click\n");
					//if (!parent) {
						//printf("Casting click event.\n");

						return this->CastEvent(GLICT_MOUSECLICK, wparam, lparam, returnvalue);
					//} else {
						//Not toplevel! Has a parent. Thus ignoring a click, letting toplevel parse properly.
					//}

				} else {
					//Considering it dragging. Ignoring it!
				}

                if (((glictPos*)wparam)->x > this->clipleft &&
                 ((glictPos*)wparam)->x < this->clipright &&
                 ((glictPos*)wparam)->y > this->cliptop &&
                 ((glictPos*)wparam)->y < this->clipbottom ) {

                    if (this->OnMouseUp) {
                        glictPos relpos;
                        relpos.x = ((glictPos*)wparam)->x - this->left - this->containeroffsetx + this->virtualpos.x;
                        relpos.y = ((glictPos*)wparam)->y - this->top - this->containeroffsety + this->virtualpos.y;
                        this->OnMouseUp(&relpos, this);
                        return true;
                    }
                }



			} else if (evt == GLICT_MOUSECLICK) { // not mousedown , not mouseup? mouseclick!


				if (((glictPos*)wparam)->x > this->clipleft &&
					((glictPos*)wparam)->x < this->clipright &&
					((glictPos*)wparam)->y > this->cliptop &&
					((glictPos*)wparam)->y < this->clipbottom ) {
					if (this->OnClick) {
						//printf("Click on %s.\n", objtype);
						glictPos relpos;
						relpos.x = ((glictPos*)wparam)->x - this->left - this->containeroffsetx + this->virtualpos.x;
						relpos.y = ((glictPos*)wparam)->y - this->top - this->containeroffsety + this->virtualpos.y;
						this->OnClick(&relpos, this);
						return true;
					}
					// if it happened within our boundaries, let it be as if we proc'ed it!
					// of course, only if we're not a container

					if (strcmp(objtype, "Container")) {// FIXME this is ugly lowperformance check, we should make it a bool or sth
                        //printf("Announcing click in %s\n", objtype);
					    return true;
					}
				}
				// it didnt? then lets ignore it

				return false;

			}
			return false; // came here? defaultcastevent caught nothing
			}


		default:
			printf("Unhandled event\n");
			return false; // unprocessed, unknown event
	}

	// should never come here, but just in case:
	return false;
}
Example #28
0
int
gsl_sf_bessel_jl_e(const int l, const double x, gsl_sf_result * result)
{
  if(l < 0 || x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = ( l > 0 ? 0.0 : 1.0 );
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(l == 0) {
    return gsl_sf_bessel_j0_e(x, result);
  }
  else if(l == 1) {
    return gsl_sf_bessel_j1_e(x, result);
  }
  else if(l == 2) {
    return gsl_sf_bessel_j2_e(x, result);
  }
  else if(x*x < 10.0*(l+0.5)/M_E) {
    gsl_sf_result b;
    int status = gsl_sf_bessel_IJ_taylor_e(l+0.5, x, -1, 50, GSL_DBL_EPSILON, &b);
    double pre   = sqrt((0.5*M_PI)/x);
    result->val  = pre * b.val;
    result->err  = pre * b.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return status;
  }
  else if(GSL_ROOT3_DBL_EPSILON * x > (l*l + l + 1.0)) {
    gsl_sf_result b;
    int status = gsl_sf_bessel_Jnu_asympx_e(l + 0.5, x, &b);
    double pre = sqrt((0.5*M_PI)/x);
    result->val = pre * b.val;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val) + pre * b.err;
    return status;
  }
  else if(l > 1.0/GSL_ROOT6_DBL_EPSILON) {
    gsl_sf_result b;
    int status = gsl_sf_bessel_Jnu_asymp_Olver_e(l + 0.5, x, &b);
    double pre = sqrt((0.5*M_PI)/x);
    result->val = pre * b.val;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val) + pre * b.err;
    return status;
  }
  else {
    double sgn;
    double ratio;
    int stat_CF1 = gsl_sf_bessel_J_CF1(l+0.5, x, &ratio, &sgn);
    double jellp1 = GSL_SQRT_DBL_EPSILON * ratio;
    double jell   = GSL_SQRT_DBL_EPSILON;
    double jellm1;
    int ell;
    for(ell = l; ell > 0; ell--) {
      jellm1 = -jellp1 + (2*ell + 1)/x * jell;
      jellp1 = jell;
      jell   = jellm1;
    }

    if(fabs(jell) > fabs(jellp1)) {
      gsl_sf_result j0_result;
      int stat_j0  = gsl_sf_bessel_j0_e(x, &j0_result);
      double pre   = GSL_SQRT_DBL_EPSILON / jell;
      result->val  = j0_result.val * pre;
      result->err  = j0_result.err * fabs(pre);
      result->err += 2.0 * GSL_DBL_EPSILON * (0.5*l + 1.0) * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_j0, stat_CF1);
    }
    else {
      gsl_sf_result j1_result;
      int stat_j1  = gsl_sf_bessel_j1_e(x, &j1_result);
      double pre   = GSL_SQRT_DBL_EPSILON / jellp1;
      result->val  = j1_result.val * pre;
      result->err  = j1_result.err * fabs(pre);
      result->err += 2.0 * GSL_DBL_EPSILON * (0.5*l + 1.0) * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_j1, stat_CF1);
    }
  }
}
/* Function: CPlan9ELConfig()
 * Incept:   EPN, Tue Jun 19 09:50:52 2007
 * 
 * Purpose:  Turn EL local ends in a CM Plan 9 HMM on based on 
 *           the local end probs in the CM. 
 *           
 * Args:     cm     - the CM, must have valid CP9 HMM
 *                    
 * Return:   (void)
 *           HMM probabilities are modified.
 */
void
CPlan9ELConfig(CM_t *cm)
{
  /*printf("IN CPlan9ELConfig\n");*/
  /* Contract checks */
  if(cm->cp9 == NULL)
    cm_Fail("ERROR in CPlan9ELConfig, cm->cp9 is NULL.\n");
  if(cm->cp9map == NULL)
    cm_Fail("ERROR in CPlan9ELConfig, cm->cp9map is NULL.\n");
  if(!(cm->flags & CMH_CP9))
     cm_Fail("ERROR in CPlan9ELConfig, CMH_CP9 flag is down.");
  if(cm->cp9->flags & CPLAN9_EL)
     cm_Fail("ERROR in CPlan9ELConfig, CP9_EL flag is already up.");
  
  int v;
  int k;                     /* counter over HMM nodes */
  int nd;
  int seen_exit;
  float to_el_prob;
  float norm_factor;
  int   nexits;

  /* If the CM has local ends on, check to make sure all non-zero 
   * local end probabilities in the CM are identical (within reasonable 
   * precision), use that probability to set all HMM transitions to 
   * EL states.
   */
  if(cm->flags & CMH_LOCAL_END) { 
    seen_exit  = FALSE;
    to_el_prob = 0.;
    for(v = 0; v < cm->M; v++) {
      nd = cm->ndidx[v];
      if (((cm->ndtype[nd] == MATP_nd || cm->ndtype[nd] == MATL_nd ||
	  cm->ndtype[nd] == MATR_nd || cm->ndtype[nd] == BEGL_nd ||
	  cm->ndtype[nd] == BEGR_nd) && 
	 cm->ndtype[nd+1] != END_nd) && cm->nodemap[nd] == v) {
      /* this should have a non-zero local end probability */
      if(fabs(cm->end[v] - 0.) < 0.00001) /* non-zero */
	cm_Fail("In CPlan9ELConfig(), CM state %d should have non-zero local end prob, but it doesn't.\n", v);
      if(!seen_exit) {
	to_el_prob = cm->end[v];
	seen_exit  = TRUE;
      }
      else if(fabs(to_el_prob - cm->end[v]) > 0.00001)
	cm_Fail("In CPlan9ELConfig(), not all CM states EL probs are identical.\n");
      }
    }
    if(! seen_exit && cm->nodes != 3) cm_Fail("In CPlan9ELConfig(), CM_LOCAL_END flag up, cm->nodes != 3, but all CM local end probs are zero."); 
  }
  else {
    /* CM_LOCAL_END flag is down, local ends are off in the CM 
     * We figure out what the local end prob would be given cm->pend
     * and set the HMM local end probs based on that. 
     * First, count internal nodes MATP, MATL, MATR, BEGL, BEGR that aren't
     * adjacent to END nodes.
     */
    nexits = 0;
    for (nd = 1; nd < cm->nodes; nd++) {
      if ((cm->ndtype[nd] == MATP_nd || cm->ndtype[nd] == MATL_nd ||
	   cm->ndtype[nd] == MATR_nd || cm->ndtype[nd] == BEGL_nd ||
	   cm->ndtype[nd] == BEGR_nd) && 
	  cm->ndtype[nd+1] != END_nd)
	nexits++;
    } 
    to_el_prob = cm->pend / (float) nexits;
  }

  /* transitions from HMM node 0 to EL is impossible */
  cm->cp9->t[0][CTMEL] = 0.;
  for(k = 1; k <= cm->cp9->M; k++) 
    {
      if(cm->cp9->has_el[k])
	{
	  cm->cp9->t[k][CTMEL] = to_el_prob;
	  norm_factor = 1. - (cm->cp9->t[k][CTMEL] / (1. - cm->cp9->end[k]));
	  cm->cp9->t[k][CTMM] *= norm_factor;
	  cm->cp9->t[k][CTMI] *= norm_factor;
	  cm->cp9->t[k][CTMD] *= norm_factor;
	  /* cm->cp9->end[k] untouched */
	}
    }
  cm->cp9->flags &= ~CPLAN9_HASBITS;	/* clear the log-odds ready flag */

  CP9Logoddsify(cm->cp9);

  cm->cp9->flags |= CPLAN9_EL;          /* EL end locals now on */
  /*debug_print_cp9_params(cm->cp9);*/
  return;
}
Example #30
0
void plotZmm(const TString  outputDir,   // output directory
             const Double_t lumi         // integrated luminosity (/fb)
) {
  gBenchmark->Start("plotZmm");
  gStyle->SetTitleOffset(1.100,"Y");

  //--------------------------------------------------------------------------------------------------------------
  // Settings 
  //==============================================================================================================   
  
  //
  // input ntuple file names
  //
  enum { eData, eZmm, eEWK };  // data type enum
  vector<TString> fnamev;
  vector<Int_t>   typev;
  
  fnamev.push_back("/data/blue/ksung/EWKAna/8TeV/Selection/Zmumu/ntuples/data_select.root"); typev.push_back(eData);
  fnamev.push_back("/data/blue/ksung/EWKAna/8TeV/Selection/Zmumu/ntuples/zmm_select.root");  typev.push_back(eZmm);
  fnamev.push_back("/data/blue/ksung/EWKAna/8TeV/Selection/Zmumu/ntuples/ewk_select.root");  typev.push_back(eEWK);
  fnamev.push_back("/data/blue/ksung/EWKAna/8TeV/Selection/Zmumu/ntuples/top_select.root");  typev.push_back(eEWK);

  //
  // Fit options
  //
  const Int_t    NBINS     = 60;
  const Double_t MASS_LOW  = 60;
  const Double_t MASS_HIGH = 120;  
  const Double_t PT_CUT    = 25;
  const Double_t ETA_CUT   = 2.1;
  
  // plot output file format
  const TString format("png");
 
   
  //--------------------------------------------------------------------------------------------------------------
  // Main analysis code 
  //==============================================================================================================  

  // event category enumeration
  enum { eMuMu2HLT=1, eMuMu1HLT, eMuMuNoSel, eMuSta, eMuTrk };
  
  // Create output directory
  gSystem->mkdir(outputDir,kTRUE);
  CPlot::sOutDir = outputDir;  
  
  // histograms for full selection (MuMu2HLT + MuMu1HLT)
  TH1D *hData = new TH1D("hData","",NBINS,MASS_LOW,MASS_HIGH); hData->Sumw2();
  TH1D *hZmm  = new TH1D("hZmm", "",NBINS,MASS_LOW,MASS_HIGH); hZmm->Sumw2();
  TH1D *hEWK  = new TH1D("hEWK", "",NBINS,MASS_LOW,MASS_HIGH); hEWK->Sumw2();
  TH1D *hMC   = new TH1D("hMC",  "",NBINS,MASS_LOW,MASS_HIGH); hMC->Sumw2();
    
  //
  // Declare variables to read in ntuple
  //
  UInt_t  runNum, lumiSec, evtNum;
  UInt_t  matchGen;
  UInt_t  category;
  UInt_t  npv, npu;
  Float_t genVPt, genVPhi, genVy, genVMass;
  Float_t scale1fb;
  Float_t met, metPhi, sumEt, u1, u2;
  Int_t   q1, q2;
  LorentzVector *dilep=0, *lep1=0, *lep2=0;
  Float_t pfCombIso1, pfCombIso2;

  TFile *infile=0;
  TTree *intree=0;

  for(UInt_t ifile=0; ifile<fnamev.size(); ifile++) {
  
    // Read input file and get the TTrees
    cout << "Processing " << fnamev[ifile] << "..." << endl;
    infile = new TFile(fnamev[ifile]);	    assert(infile);
    intree = (TTree*)infile->Get("Events"); assert(intree);

    intree->SetBranchAddress("runNum",     &runNum);      // event run number
    intree->SetBranchAddress("lumiSec",    &lumiSec);     // event lumi section
    intree->SetBranchAddress("evtNum",     &evtNum);      // event number
    intree->SetBranchAddress("matchGen",   &matchGen);    // event has both leptons matched to MC Z->ll
    intree->SetBranchAddress("category",   &category);    // dilepton category
    intree->SetBranchAddress("npv",        &npv);	  // number of primary vertices
    intree->SetBranchAddress("npu",        &npu);	  // number of in-time PU events (MC)
    intree->SetBranchAddress("genVPt",     &genVPt);      // GEN Z boson pT (signal MC)
    intree->SetBranchAddress("genVPhi",    &genVPhi);     // GEN Z boson phi (signal MC)
    intree->SetBranchAddress("genVy",      &genVy);       // GEN Z boson rapidity (signal MC)
    intree->SetBranchAddress("genVMass",   &genVMass);    // GEN Z boson mass (signal MC)
    intree->SetBranchAddress("scale1fb",   &scale1fb);    // event weight per 1/fb (MC)
    intree->SetBranchAddress("met",        &met);	  // MET
    intree->SetBranchAddress("metPhi",     &metPhi);      // phi(MET)
    intree->SetBranchAddress("sumEt",      &sumEt);       // Sum ET
    intree->SetBranchAddress("u1",         &u1);	  // parallel component of recoil
    intree->SetBranchAddress("u2",         &u2);	  // perpendicular component of recoil
    intree->SetBranchAddress("q1",         &q1);	  // charge of tag lepton
    intree->SetBranchAddress("q2",         &q2);	  // charge of probe lepton
    intree->SetBranchAddress("dilep",      &dilep);       // dilepton 4-vector
    intree->SetBranchAddress("lep1",       &lep1);        // tag lepton 4-vector
    intree->SetBranchAddress("lep2",       &lep2);        // probe lepton 4-vector
    intree->SetBranchAddress("pfCombIso1", &pfCombIso1);  // combined PF isolation of tag lepton
    intree->SetBranchAddress("pfCombIso2", &pfCombIso2);  // combined PF isolation of probe lepton
  
    //
    // loop over events
    //
    for(UInt_t ientry=0; ientry<intree->GetEntries(); ientry++) {
      intree->GetEntry(ientry);
   
      if(dilep->M()        < MASS_LOW)  continue;
      if(dilep->M()        > MASS_HIGH) continue;
      if(lep1->Pt()        < PT_CUT)    continue;
      if(lep2->Pt()        < PT_CUT)    continue;
      if(fabs(lep1->Eta()) > ETA_CUT)   continue;      
      if(fabs(lep2->Eta()) > ETA_CUT)   continue;
      
      Float_t mass = dilep->M();
      
      Double_t weight=1;
      if(typev[ifile]!=eData) {
	weight *= scale1fb*lumi;
      }
      
      // fill Z events passing selection (MuMu2HLT + MuMu1HLT)
      if((category==eMuMu2HLT) || (category==eMuMu1HLT)) {
        if(typev[ifile]==eData) { 
	  hData->Fill(mass); 
	
	} else {
	  LorentzVector slep1 = (*lep1);
	  slep1 *= gRandom->Gaus(slep1.E(), getResCorr(lep1->Eta()))/slep1.E();
	  
	  LorentzVector slep2 = (*lep2);
	  slep2 *= gRandom->Gaus(slep2.E(), getResCorr(lep2->Eta()))/slep2.E();
	  
	  mass = (slep1+slep2).M();
	
	  hMC->Fill(mass,weight);
	  if(typev[ifile]==eZmm) { hZmm->Fill(mass,weight); }
	  if(typev[ifile]==eEWK) { hEWK->Fill(mass,weight); }
	}
      }
    }
    
    delete infile;
    infile=0, intree=0;
  } 

  TH1D *hZmumuDiff = makeDiffHist(hData,hMC,"hZmumuDiff");
  hZmumuDiff->SetMarkerStyle(kFullCircle); 
  hZmumuDiff->SetMarkerSize(0.9);
  
  
  //--------------------------------------------------------------------------------------------------------------
  // Make plots 
  //==============================================================================================================  

  char ylabel[100];     // string buffer for y-axis label
  
  // label for lumi
  char lumitext[100];
  if(lumi<0.1) sprintf(lumitext,"%.1f pb^{-1}  at  #sqrt{s} = 8 TeV",lumi*1000.);
  else         sprintf(lumitext,"%.2f fb^{-1}  at  #sqrt{s} = 8 TeV",lumi);  
  
  // plot colors
  Int_t linecolorZ   = kOrange-3;
  Int_t fillcolorZ   = kOrange-2;
  Int_t linecolorEWK = kOrange+10;
  Int_t fillcolorEWK = kOrange+7;
  Int_t ratioColor   = kGray+2;

  TCanvas *c = MakeCanvas("c","c",800,800);
  c->Divide(1,2,0,0);
  c->cd(1)->SetPad(0,0.3,1.0,1.0);
  c->cd(1)->SetTopMargin(0.1);
  c->cd(1)->SetBottomMargin(0.01);
  c->cd(1)->SetLeftMargin(0.15);  
  c->cd(1)->SetRightMargin(0.07);  
  c->cd(1)->SetTickx(1);
  c->cd(1)->SetTicky(1);  
  c->cd(2)->SetPad(0,0,1.0,0.3);
  c->cd(2)->SetTopMargin(0.05);
  c->cd(2)->SetBottomMargin(0.45);
  c->cd(2)->SetLeftMargin(0.15);
  c->cd(2)->SetRightMargin(0.07);
  c->cd(2)->SetTickx(1);
  c->cd(2)->SetTicky(1); 
  TGaxis::SetMaxDigits(3);
  
  //
  // MuMu2HLT + MuMu1HLT categories
  //   
  sprintf(ylabel,"Events / %.1f GeV/c^{2}",hData->GetBinWidth(1));
  CPlot plotZmumu("zmm","","",ylabel);
  plotZmumu.AddHist1D(hData,"data","E");
  plotZmumu.AddToStack(hZmm,"Z#rightarrow#mu#mu",fillcolorZ,linecolorZ);
  plotZmumu.AddTextBox("CMS Preliminary",0.63,0.92,0.95,0.99,0);
  plotZmumu.AddTextBox(lumitext,0.55,0.80,0.90,0.86,0);
  plotZmumu.SetYRange(0.01,1.2*(hData->GetMaximum() + sqrt(hData->GetMaximum())));
  plotZmumu.TransLegend(-0.35,-0.15);
  plotZmumu.Draw(c,kFALSE,format,1);

  CPlot plotZmumuDiff("zmm","","M(#mu^{+}#mu^{-}) [GeV/c^{2}]","#chi");
  plotZmumuDiff.AddHist1D(hZmumuDiff,"EX0",ratioColor);
  plotZmumuDiff.SetYRange(-8,8);
  plotZmumuDiff.AddLine(MASS_LOW, 0,MASS_HIGH, 0,kBlack,1);
  plotZmumuDiff.AddLine(MASS_LOW, 5,MASS_HIGH, 5,kBlack,3);
  plotZmumuDiff.AddLine(MASS_LOW,-5,MASS_HIGH,-5,kBlack,3);
  plotZmumuDiff.Draw(c,kTRUE,format,2);
  
  CPlot plotZmumu2("zmmlog","","",ylabel);
  plotZmumu2.AddHist1D(hData,"data","E");
  plotZmumu2.AddToStack(hEWK,"EWK",fillcolorEWK,linecolorEWK);
  plotZmumu2.AddToStack(hZmm,"Z#rightarrow#mu#mu",fillcolorZ,linecolorZ);
  plotZmumu2.AddTextBox("CMS Preliminary",0.63,0.92,0.95,0.99,0);plotZmumu2.SetName("zmmlog");
  plotZmumu2.AddTextBox(lumitext,0.55,0.80,0.90,0.86,0);
  plotZmumu2.SetLogy();
  plotZmumu2.SetYRange(1e-4*(hData->GetMaximum()),10*(hData->GetMaximum()));
  plotZmumu2.TransLegend(-0.35,-0.15);
  plotZmumu2.Draw(c,kTRUE,format,1);

    
  //--------------------------------------------------------------------------------------------------------------
  // Output
  //==============================================================================================================
   
  cout << "*" << endl;
  cout << "* SUMMARY" << endl;
  cout << "*--------------------------------------------------" << endl;  
  cout << endl;
  
  cout << endl;
  cout << "  <> Output saved in " << outputDir << "/" << endl;    
  cout << endl;     

  gBenchmark->Show("plotZmm");
}