nullalgo(double timeout_sec): rpc_server(timeout_sec) {

    rpc_server::add<bool(std::string, datum) >("update_random", pfi::lang::bind(&Impl::update_random, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<datum(std::string, std::string) >("query_random", pfi::lang::bind(&Impl::query_random, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<bool(std::string, std::string, datum) >("update_cht", pfi::lang::bind(&Impl::update_cht, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2, pfi::lang::_3));
    rpc_server::add<datum(std::string, std::string, std::string) >("query_cht", pfi::lang::bind(&Impl::query_cht, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2, pfi::lang::_3));
    rpc_server::add<datum(std::string, std::string, std::string) >("query_cht_nolock", pfi::lang::bind(&Impl::query_cht_nolock, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2, pfi::lang::_3));
    rpc_server::add<bool(std::string, std::string) >("save", pfi::lang::bind(&Impl::save, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<bool(std::string, std::string) >("load", pfi::lang::bind(&Impl::load, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<std::map<std::string, std::map<std::string, std::string > >(std::string) >("get_status", pfi::lang::bind(&Impl::get_status, static_cast<Impl*>(this), pfi::lang::_1));
  }
Beispiel #2
0
avro::GenericDatum CommonEnum::toAvro() const
{
    avro::GenericDatum datum(getSchema());
    avro::GenericEnum &enum_ = datum.value<avro::GenericEnum>();
    enum_.set(symbol_);
    return datum;
}
Beispiel #3
0
void GetGridBox(const CViewPoint *view_point, CBox &ext){
	gp_Pnt sp[4];
	double zval = 0.5;
	wxSize size = wxGetApp().m_current_viewport->GetViewportSize();
	sp[0] = gp_Pnt(0, 0, zval);
	sp[1] = gp_Pnt(size.GetWidth(), 0, zval);
	sp[2] = gp_Pnt(size.GetWidth(), size.GetHeight(), zval);
	sp[3] = gp_Pnt(0, size.GetHeight(), zval);
	gp_Vec vx, vy;
	view_point->GetTwoAxes(vx, vy, false, 0);
	gp_Pnt datum(0, 0, 0);
	gp_Trsf orimat = wxGetApp().GetDrawMatrix(false);
	datum.Transform(orimat);
	orimat = make_matrix(datum, vx, vy);
	gp_Pln plane(datum, gp_Vec(0, 0, 1).Transformed(orimat));
	{
		for(int i =0; i<4; i++){
			gp_Pnt p1 = view_point->glUnproject(sp[i]);
			sp[i].SetZ(0);
			gp_Pnt p2 = view_point->glUnproject(sp[i]);
			gp_Lin line = make_line(p1, p2);
			gp_Pnt pnt;
			if(intersect(line, plane, pnt))
			{
				ext.Insert(pnt.X(), pnt.Y(), pnt.Z());
			}
		}
	}
}
  recommender(double timeout_sec): rpc_server(timeout_sec) {

    rpc_server::add<std::string(std::string) >("get_config", pfi::lang::bind(&Impl::get_config, static_cast<Impl*>(this), pfi::lang::_1));
    rpc_server::add<bool(std::string, std::string) >("clear_row", pfi::lang::bind(&Impl::clear_row, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<bool(std::string, std::string, datum) >("update_row", pfi::lang::bind(&Impl::update_row, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2, pfi::lang::_3));
    rpc_server::add<bool(std::string) >("clear", pfi::lang::bind(&Impl::clear, static_cast<Impl*>(this), pfi::lang::_1));
    rpc_server::add<datum(std::string, std::string) >("complete_row_from_id", pfi::lang::bind(&Impl::complete_row_from_id, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<datum(std::string, datum) >("complete_row_from_datum", pfi::lang::bind(&Impl::complete_row_from_datum, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<similar_result(std::string, std::string, uint32_t) >("similar_row_from_id", pfi::lang::bind(&Impl::similar_row_from_id, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2, pfi::lang::_3));
    rpc_server::add<similar_result(std::string, datum, uint32_t) >("similar_row_from_datum", pfi::lang::bind(&Impl::similar_row_from_datum, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2, pfi::lang::_3));
    rpc_server::add<datum(std::string, std::string) >("decode_row", pfi::lang::bind(&Impl::decode_row, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<std::vector<std::string >(std::string) >("get_all_rows", pfi::lang::bind(&Impl::get_all_rows, static_cast<Impl*>(this), pfi::lang::_1));
    rpc_server::add<float(std::string, datum, datum) >("calc_similarity", pfi::lang::bind(&Impl::calc_similarity, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2, pfi::lang::_3));
    rpc_server::add<float(std::string, datum) >("calc_l2norm", pfi::lang::bind(&Impl::calc_l2norm, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<bool(std::string, std::string) >("save", pfi::lang::bind(&Impl::save, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<bool(std::string, std::string) >("load", pfi::lang::bind(&Impl::load, static_cast<Impl*>(this), pfi::lang::_1, pfi::lang::_2));
    rpc_server::add<std::map<std::string, std::map<std::string, std::string > >(std::string) >("get_status", pfi::lang::bind(&Impl::get_status, static_cast<Impl*>(this), pfi::lang::_1));
  }
Beispiel #5
0
string  xml_formatter::
output_symbol_details(symbol_entry const * symb,
    size_t & detail_index, size_t const lo, size_t const hi)
{
	if (!has_sample_counts(symb->sample.counts, lo, hi))
		return "";

	sample_container::samples_iterator it = profile->begin(symb);
	sample_container::samples_iterator end = profile->end(symb);

	ostringstream str;
	for (; it != end; ++it) {
		counts_t c;

		for (size_t p = lo; p <= hi; ++p)  {
			size_t count = it->second.counts[p];

			if (count == 0) continue;

			str << open_element(DETAIL_DATA, true);
			str << init_attr(TABLE_ID, detail_index++);

			// first output the vma field
			field_datum datum(*symb, it->second, 0, c, 
					  extra_found_images, 0.0);
			output_attribute(str, datum, ff_vma, VMA);
			if (flags & ff_linenr_info) {
				string sym_file;
				size_t sym_line;
				string samp_file;
				size_t samp_line;
				string sym_info = get_linenr_info(symb->sample.file_loc, true);
				string samp_info = get_linenr_info(it->second.file_loc, true);

				if (extract_linenr_info(samp_info, samp_file, samp_line)) {
					if (extract_linenr_info(sym_info, sym_file, sym_line)) {
						// only output source_file if it is different than the symbol's 
						// source file.  this can happen with inlined functions in
						// #included header files
						if (sym_file != samp_file)
							str << init_attr(SOURCE_FILE, samp_file);
					}
					str << init_attr(SOURCE_LINE, samp_line);
				}
			}
			str << close_element(NONE, true);

			// output buffered sample data
			output_sample_data(str, it->second, p);

			str << close_element(DETAIL_DATA);
		}
	}
	return str.str();
}
Beispiel #6
0
avro::GenericDatum CommonRecord::toAvro() const
{
    avro::GenericDatum datum(getSchema());
    avro::GenericRecord &record = datum.value<avro::GenericRecord>();

    size_t fieldsCount = schema_->names();
    for (size_t fieldIndex = 0; fieldIndex < fieldsCount; ++fieldIndex) {
        const std::string & fieldName = schema_->nameAt(fieldIndex);
        auto it = getFields().find(fieldName);
        if (it != getFields().end()) {
            const avro::NodePtr &innode = schema_->leafAt(fieldIndex);

            avro::GenericDatum fieldDatum = it->second->toAvro();
            if (innode->type() == avro::AVRO_UNION) {

                avro::Type avro_type = fieldDatum.type();

                avro::GenericDatum indatum(innode);
                size_t branches = indatum.unionBranch();
                size_t currentFieldBranch = 0;

                bool is_compound = avro::isCompound(avro_type);

                while (currentFieldBranch < branches) {
                    avro::Type intype = innode->leafAt(currentFieldBranch)->type();
                    indatum.selectBranch(currentFieldBranch);

                    bool found = false;
                    if (is_compound) {
                        ISchemaDependent * dependent = dynamic_cast<ISchemaDependent *>(it->second.get());
                        if (innode->leafAt(currentFieldBranch)->hasName() && dependent->getSchema()->hasName() &&
                                dependent->getSchema()->name().fullname() == innode->leafAt(currentFieldBranch)->name().fullname()) {
                            found = SetAvroValueHelper::setValue(avro_type, indatum, fieldDatum);
                        } else if (innode->leafAt(currentFieldBranch)->type() == avro::Type::AVRO_ARRAY
                                && dependent->getSchema()->type() == avro::Type::AVRO_ARRAY) {
                            found = SetAvroValueHelper::setValue(avro::Type::AVRO_ARRAY, indatum, fieldDatum);
                        }
                    } else if (intype == avro_type) {
                        found = SetAvroValueHelper::setValue(avro_type, indatum, fieldDatum);
                    }
                    if (found) {
                        break;
                    }
                    ++currentFieldBranch;
                }
                record.setField(it->first, indatum);
            } else {
                record.setField(it->first, fieldDatum);
            }
        }
    }
    return datum;
}
Beispiel #7
0
void main()
{
	//Datum
	printf("\n\nPretvorba datuma:");
	char jmbg[14] = "1101995144567";
	char *d;
	printf("\nJMBG: %s", jmbg);
	d = datum(jmbg);
	printf("\nDatum rodjenja je: %s", d);
	//Kraj Datum

	getchar();
}
  virtual void OnRead()
    {
      assert(Read(0));
      JointPos& datum = Content();
//      printf("eye %g %g %g\n", datum(1), datum(2), datum(3));
      head_state_manager.SetEyes(datum(1), datum(2), datum(3));
//      head_state_manager.SetNeck(datum(6), datum(7), datum(4), datum(5));
      head_state_manager.SetNeck(datum(4), datum(5), datum(7), datum(6));
      head_state_manager.SetJoints(datum);
      wake_up_call.Post();
    }
Beispiel #9
0
//static
bool AvroUtils::convert_binary_to_json(const std::vector<uint8_t>& binary,
                                       const avro::ValidSchema& schema,
                                       const avro::EncoderPtr& jsonEncoder,
                                       std::string& json_str)
{
    avro::GenericDatum datum(schema);
    avro::DecoderPtr d = avro::binaryDecoder(); // cheap to create
    std::auto_ptr<avro::InputStream> in = avro::memoryInputStream(binary.data(), binary.size());
    d->init(*in);
    avro::decode(*d, datum);

    AvroUtils::convert_to_json_by_encoder(datum, jsonEncoder, json_str);
    return true;
}
Beispiel #10
0
void xml_formatter::
output_the_symbol_data(ostream & out, symbol_entry const * symb, op_bfd * & abfd)
{
	string const name = symbol_names.name(symb->name);
	assert(name.size() > 0);

	string const image = get_image_name(symb->image_name,
		image_name_storage::int_filename, extra_found_images);
	string const qname = image + ":" + name;
	map<string, size_t>::iterator sd_it = symbol_data_table.find(qname);

	if (sd_it != symbol_data_table.end()) {
		// first time we've seen this symbol
		out << open_element(SYMBOL_DATA, true);
		out << init_attr(TABLE_ID, sd_it->second);

		field_datum datum(*symb, symb->sample, 0, counts,
				  extra_found_images);

		output_attribute(out, datum, ff_symb_name, NAME);

		if (flags & ff_linenr_info) {
			output_attribute(out, datum, ff_linenr_info, SOURCE_FILE);
			output_attribute(out, datum, ff_linenr_info, SOURCE_LINE);
		}

		if (name.size() > 0 && name[0] != '?') {
			output_attribute(out, datum, ff_vma, STARTING_ADDR);

			if (need_details) {
				get_bfd_object(symb, abfd);
				if (abfd && abfd->symbol_has_contents(symb->sym_index))
					xml_support->output_symbol_bytes(bytes_out, symb, sd_it->second, *abfd);
			}
		}
		out << close_element();

		// seen so remove (otherwise get several "no symbols")
		symbol_data_table.erase(qname);
	}
}
		/** Import */
		X509CertList(const std::string& certstr)
		{
			unsigned int certcount = 3;
			certs.resize(certcount);
			Datum datum(certstr);

			int ret = gnutls_x509_crt_list_import(raw(), &certcount, datum.get(), GNUTLS_X509_FMT_PEM, GNUTLS_X509_CRT_LIST_IMPORT_FAIL_IF_EXCEED);
			if (ret == GNUTLS_E_SHORT_MEMORY_BUFFER)
			{
				// the buffer wasn't big enough to hold all certs but gnutls changed certcount to the number of available certs,
				// try again with a bigger buffer
				certs.resize(certcount);
				ret = gnutls_x509_crt_list_import(raw(), &certcount, datum.get(), GNUTLS_X509_FMT_PEM, GNUTLS_X509_CRT_LIST_IMPORT_FAIL_IF_EXCEED);
			}

			ThrowOnError(ret, "Unable to load certificates");

			// Resize the vector to the actual number of certs because we rely on its size being correct
			// when deallocating the certs
			certs.resize(certcount);
		}
Beispiel #12
0
// Convert the json encoded string to an avro object, then encode to binary.
//static
bool AvroUtils::convert_json_to_binary_by_schema_str( const std::string& json_str,
                                                      const std::string& type_schema_str,
                                                      std::vector<uint8_t>& bytes)
{
    try
    {
        // create the schema
        avro::ValidSchema type_schema = AvroUtils::get_or_compile_schema( type_schema_str );
        // return convert_json_to_object_by_schema(json_str, type_schema, avro_object);

        // create a generic datum given the schema
        avro::GenericDatum datum( type_schema );

        // set up decoder
        std::istringstream iss (json_str, std::istringstream::in);
        std::auto_ptr<avro::InputStream> in = avro::istreamInputStream( iss );
        avro::DecoderPtr d = avro::jsonDecoder( type_schema );

        // decode into the avro object
        d->init( *in );
        avro::decode( *d, datum );

        // Note that the chunk size will be adjusted in convert_to_byte_stream().
        AvroMemoryOutputStream out(AvroMemoryOutputStream::DEFAULT_CHUNK_SIZE);
        avro::EncoderPtr e = avro::binaryEncoder();
        e->init( out );
        avro::encode( *e, datum);
        e->flush(); // reclaim unused space

        // Copy the binary data to the output vector
        out.getBuffer( bytes );

        return true;
    }
    catch (const std::exception &excep )
    {
        return false;
    }
} // end convert_json_to_binary_by_schema_str
std::string AlbersConEqArea::wkt()
{
	OGRSpatialReference sr;
	int epsg = DATUM2EPSG[datum()];
	char **wkt = 0;
	std::string output = "";
	OGRErr err;

	sr.SetProjCS("Albers Conic Equal Area");
	if (epsg != -1) {
		sr.importFromEPSG(epsg);
		sr.SetACEA(param(2), param(3), param(5), param(4), param(6), param(7));
		err = sr.exportToPrettyWkt(wkt);
	} else {
		return output;
	}

	if (err == OGRERR_NONE) {
		output = *wkt;
		OGRFree(wkt);
	}

	return output;
}
Beispiel #14
0
void formatter::
do_output(ostream & out, symbol_entry const & symb, sample_entry const & sample,
          counts_t & c, diff_array_t const & diffs, bool hide_immutable)
{
	size_t padding = 0;

	// first output the vma field
	field_datum datum(symb, sample, 0, c, extra_found_images);
	if (flags & ff_vma)
		padding = output_field(out, datum, ff_vma, padding, false);

	// repeated fields for each profile class
	for (size_t pclass = 0 ; pclass < nr_classes; ++pclass) {
		field_datum datum(symb, sample, pclass, c,
				  extra_found_images, diffs[pclass]);

		if (flags & ff_nr_samples)
			padding = output_field(out, datum,
			       ff_nr_samples, padding, false);

		if (flags & ff_nr_samples_cumulated)
			padding = output_field(out, datum, 
			       ff_nr_samples_cumulated, padding, false);

		if (flags & ff_percent)
			padding = output_field(out, datum,
			       ff_percent, padding, false);

		if (flags & ff_percent_cumulated)
			padding = output_field(out, datum,
			       ff_percent_cumulated, padding, false);

		if (flags & ff_diff)
			padding = output_field(out, datum,
				ff_diff, padding, false);

		if (flags & ff_percent_details)
			padding = output_field(out, datum,
			       ff_percent_details, padding, false);

		if (flags & ff_percent_cumulated_details)
			padding = output_field(out, datum,
			       ff_percent_cumulated_details, padding, false);
	}

	// now the remaining field
	if (flags & ff_linenr_info)
		padding = output_field(out, datum, ff_linenr_info,
		       padding, false);

	if (flags & ff_image_name)
		padding = output_field(out, datum, ff_image_name,
		       padding, hide_immutable);

	if (flags & ff_app_name)
		padding = output_field(out, datum, ff_app_name,
		       padding, hide_immutable);

	if (flags & ff_symb_name)
		padding = output_field(out, datum, ff_symb_name,
		       padding, hide_immutable);

	out << "\n";
}
Beispiel #15
0
void CSite::writeSite(const int& iisitsf)
{
   //---------------------------------------------------------------------------------------------//
   bool isita  = bAtomInSite,            isiti  = bSaltInSite,             isitmd = bMDInSite,            isitpot = bPotentialInSite,
        isitx  = bAtomCoordInSite,       isitq  = bCrgInSite,              isitf  = bFieldInSite,         isitp   = bGridPotentialInSite,
        isitr  = bReactPotentialInSite,  isitc  = bCoulombPotentialInSite, isitap = bAtomPotentialInSite, isitdeb = bDebyeFractionInSite,
        isitsf = bSurfCrgInSite,         isittf = bTotalForceInSite,       isitrf = bReactForceInSite,    isitt   = bTotalPotentialInSite,
        isitcf = bCoulombForceInSite,    iself  = bPDB2FRCInSite;

   //----- ifrm is true if ANY of the flags for special output have been set
   bool ifrm = isita || isitq || isitp || isitf || isitr || isitt || isitc || isitx || isiti || isitrf || isitcf || isitap ||
               isittf || isitdeb;
   bool isitmp1 = ifrm; ifrm = ifrm || isitsf;
   bool ofrm = true;

   if (isitmd || isitpot) {iFrcFormatOut = -1; ofrm = false;}

   if (!ifrm && (0 == iFrcFormatOut || 3 == iFrcFormatOut))
   {
      isitx = true; isitq = true; isitf = true; isitp = true;
   }

   switch (iFrcFormatOut)
   {
      case 1: isitx = true; isitq = true; isitr = true; isitc = true; break;
      case 2: isitx = true; isitq = true; isitr = true; break;
      case 3: ofrm = false; break;
   }

   string vrow(80,' '),datum(65,' '); int j = 0, k = 0;

   if (isita)
   {
      vrow.replace(j,15,"ATOM DESCRIPTOR"); datum.replace(k,5,"ATOM ");
      j += 20; k += 5;
   }

   if (isitx)
   {
      vrow.replace(j+4,24,"ATOM COORDINATES (X,Y,Z)"); datum.replace(k,12,"COORDINATES ");
      j += 30; k += 12;
   }

   if (isitq)
   {
      vrow.replace(j+3,6,"CHARGE"); datum.replace(k,7,"CHARGE ");
      j += 10; k += 7;
   }

   if (isitp)
   {
      vrow.replace(j+2,8,"GRID PT."); datum.replace(k,11,"POTENTIALS ");
      j += 10; k += 11;
   }

   if (isiti)
   {
      vrow.replace(j+1,8,"SALT CON"); datum.replace(k,5,"SALT ");
      j += 10; k += 5;
   }

   if (80 <= j)
   {
      isitr  = false; isitc  = false; isitap = false; isitdeb = false; isitf = false;
      isitsf = false; isittf = false; isitrf = false; isitcf  = false; isitt = false;
   }

   if (isitr)
   {
      vrow.replace(j,10," REAC. PT."); datum.replace(k,9,"REACTION ");
      j += 10; k += 9;
   }

   if (80 <= j)
   {
                      isitc  = false; isitap = false; isitdeb = false; isitf = false;
      isitsf = false; isittf = false; isitrf = false; isitcf  = false; isitt = false;
   }

   if (isitc)
   {
      vrow.replace(j,10," COUL. POT"); datum.replace(k,10,"COULOMBIC ");
      j += 10; k += 10;
   }

   if (80 <= j)
   {
                                      isitap = false; isitdeb = false; isitf = false;
      isitsf = false; isittf = false; isitrf = false; isitcf  = false; isitt = false;
   }

   if (isitap)
   {
      vrow.replace(j+2,8,"ATOM PT."); datum.replace(k,11,"ATOMIC PT. ");
      j += 10; k += 11;
   }

   if (80 <= j)
   {
                                                      isitdeb = false; isitf = false;
      isitsf = false; isittf = false; isitrf = false; isitcf  = false; isitt = false;
   }

   if (isitdeb)
   {
      vrow.replace(j+3,11,"DEBFRACTION"); datum.replace(k,12,"DEBFRACTION ");
      j += 14; k += 12;
   }

   if (60 <= j)
   {
                                                                       isitf = false;
      isitsf = false; isittf = false; isitrf = false; isitcf  = false; isitt = false;
   }

   if (isitf)
   {
      vrow.replace(j+4,25,"GRID FIELDS: (Ex, Ey, Ez)"); datum.replace(k,7,"FIELDS ");
      j += 30;
   }

   if (60 <= j)
   {
      isitsf = false; isittf = false; isitrf = false; isitcf  = false; isitt  = false;
   }

   if (isitrf)
   {
      vrow.replace(j+4,25,"REAC. FORCE: (Rx, Ry, Rz)"); datum.replace(k,7,"RFORCE ");
      j += 30;
   }

   if (60 <= j)
   {
      isitsf = false; isittf = false; isitcf  = false; isitt  = false;
   }

   if (isitcf)
   {
      vrow.replace(j+4,25,"COUL. FORCE: (Cx, Cy, Cz)"); datum.replace(k,7,"CFORCE ");
      j += 30;
   }

   if (60 <= j)
   {
      isitsf = false; isittf = false; isitt  = false;
   }

   if (isittf)
   {
      vrow.replace(j+4,25,"TOTAL FORCE: (Tx, Ty, Tz)"); datum.replace(k,7,"TFORCE ");
      j += 30;
   }

   if (70 <= j)
   {
      isitt  = false;
   }

   if (isitt)
   {
      vrow.replace(j+4,6," TOTAL"); datum.replace(k,6,"TOTAL ");
      j += 10;
   }

   if (50 <= j) isitsf = false;

   if (isitsf)
   {
      vrow.replace(j+4,65,"sCharge,    x          y       z       urf.E°n,surf. E[kT/(qA)]");
      datum.replace(k,35,"SCh, x, y, z, surf En, surf. E");
      j += 50;
   }

   //---------------------------------------------------------------------------------------------//
   /*
    * if site potentials required and unformatted read/write, skip during formatted frc file read/write can write unformatted frc.pdb
    */
   bool ifrm2 = false, iqass = true;
   ifstream ifFileStream;
   vector<bool> residsf(iResidueNum,false);

   if (!(isitmd || isitpot)) cout << "\nwriting potentials at given sites...\n";

   if (iself)
   {
      cout << "using the current pdb file\n";
      ifrm2 = true; iqass = false;
   }
   else
   {
      if (!isitpot)
      {
         ifFileStream.open(strFrciFile.c_str()); // just inquire whether the file exists or not
         if (!ifFileStream.is_open())
         {
            CUnknownInFrcFile warning(strFrciFile);
            ifFileStream.close();
            return;
         }
         else
         {
            cout << "coordinates, etc for potential output read from file " << strFrciFile << endl;
            ifrm2 = checkFileFormat(strFrciFile);
         }

         ifFileStream.close();
      }
   }

   //----- if unformatted may not contain all the info needed for all options, i.e atom info
   if (!ifrm2 && isita)
   {
      CNoAtomInfo warning(strFrciFile);
      isita = false; iqass = false;
   }

   if (!ifrm2) iqass = false;

   if (!iself && !isitpot)
   {
      if (ifrm2) ifFileStream.open(strFrciFile.c_str());
      else       ifFileStream.open(strFrciFile.c_str(),ios::binary);

      if (!ifFileStream.is_open()) CUnknownInFrcFile warning(strFrciFile);
   }

   if (isitsf) //----- isitsf assumes ifrm2=.true.
   {
      ifstream ifFileStream15;
      string strLine,strHead;
      int iresnum;

      ifFileStream15.open(strFrciFile.c_str()); // just inquire whether the file exists or not
      if (!ifFileStream15.is_open())
      {
         CUnknownInFrcFile warning(strFrciFile);
      }
      else
      {
         cout << "coordinates, etc for potential output read from file " << strFrciFile << endl;
         while (!ifFileStream15.eof()) // loop D302
         {
            getline(ifFileStream15,strLine);
            strHead = strLine.substr(0,6);
            if (0 != strHead.compare("      ")) strHead = toUpperCase(strHead);
            if (0 != strHead.compare("ATOM  ") && 0 != strHead.compare("HETATM")) continue;
            iresnum = atoi(strLine.substr(23,4).c_str());
            residsf[iresnum-1] = true;
         }
      }

      ifFileStream15.close();
   }

   //---------------------------------------------------------------------------------------------//
   ofstream ofFileStream;

   if (ofrm) ofFileStream.open(strFrcFile.c_str());
   if(!(ofrm || isitmd || isitpot)) ofFileStream.open(strFrcFile.c_str(),ios::binary);

   if (!isitmd && !isitpot) cout << "potentials written to file " << strFrcFile << endl << endl;

   if (ofrm)
   {
      ofFileStream << "DELPHI SITE POTENTIAL FILE\n";
      ofFileStream << "grid size,percent fill:   " << iGrid << "    " << fPercentageFill << endl;
      ofFileStream << "outer diel. and first one assigned :   " << fExDielec << "    " << vctfMediaEps[1]*fEPKT << endl;
      ofFileStream << "ionic strength (M):   " << fIonStrength << endl;
      ofFileStream << "ion excl., probe radii:   " << fIonRadius << "    " << rgfProbeRadius[0] << "    " << rgfProbeRadius[1] << endl;
      ofFileStream << "linear, nolinear iterations:   " << iLinIterateNum << "    " << iNonIterateNum << endl;
      ofFileStream << "boundary condition:   " << iBndyType << endl;
      ofFileStream << "Data Output:   " << datum << endl;
      ofFileStream << "title: " << rgcFileMap << endl;
      ofFileStream << "\n\n";
      ofFileStream << vrow << endl;
   }

   if (!ofrm && (!(isitmd || isitpot)))
   {
      ofFileStream << fixed << setprecision(4);

      string strLine;
      strLine = "DELPHI FRC FILE"; ofFileStream << strLine << endl;
      strLine = "FORMAT NUMBER=1"; ofFileStream << strLine << endl;
      strLine = "DATA="; strLine.append(datum); ofFileStream << strLine << endl;
      ofFileStream << setw(5) << right << iGrid << setw(10) << right << fPercentageFill << setw(10) << right << fExDielec
                   << setw(10) << right << fIonStrength << endl;
      for (int i = 1; i <= iMediaNum; i++)
         ofFileStream << "dielectric in medium nr. " << i << ": " << vctfMediaEps[i]*fEPKT << endl;
      ofFileStream << setw(10) << right << fIonRadius << setw(10) << right << rgfProbeRadius[0] << setw(10) << right << rgfProbeRadius[1]
                   << setw(5) << right << iLinIterateNum << setw(5) << right << iNonIterateNum << setw(5) << right << iBndyType << endl;

      ofFileStream.unsetf(ios_base::floatfield);
   }

   if (!iself && (isitrf || isitmd || isittf))
   {
      CCalcReactForceError warning;
      isitrf = false; isittf = false; isitmd = false;
   }

   vector< SGrid<real> > rfield;

   if (isitrf || isitmd || isittf)
   {
      if (1 == iMediaNum && fZero > abs(vctfMediaEps[1]*fEPKT-1.0))
         rfield = rforceeps1();
      else
         rfield = rforce();
   }

   //---------------------------------------------------------------------------------------------//
   integer nnatom,inum,ncrgs;
   SGrid<real> cxyz = {0.0,0.0,0.0},xo,xn,fu,fl,xo2,fxyz,vtemp,xu2,xu,rxyz;
   real chrgv,radu,goff,vphi,aphi,etot,phiv,temp,phii,debyefraction,phirt,phict,phir,phias,tcrgs,dist,phirtt,crgs,phiat,phic,phiac,eps,phiact,phit;
   real sdist,ff,fn;
   string atm,res,rnum,chn,crdstr,atnum,atdes(16,' '),strLine,strHead;
   bool isitmp;
   int iFound,iresnum,idist,ncrg,jtmp;
   vector<bool> atmsf(iAtomNum*iisitsf,false);
   vector<real> sold,scomp;
   char otemp[10];
   string oline(80,' ');

   nnatom = 0; chrgv = 0.0; goff = ((real)iGrid+1.0)/2.0; etot = 0.0; phirt = 0.0; phict =0.0;

   if (isitpot)
   {
      CSitePhiError warning;
   }
   else
   {
      do // beginning of the big loop on natom
      {
         if(iself)
         {
            if (iAtomNum == nnatom) break;
            xo    = prgfgAtomCoordA[nnatom];
            chrgv = vctapAtomPdb[nnatom].getCharge();
            radu  = vctapAtomPdb[nnatom].getRadius()*fScale;
            atm   = vctapAtomPdb[nnatom].getAtInf().substr(0,4);
            res   = vctapAtomPdb[nnatom].getAtInf().substr(6,3);
            rnum  = vctapAtomPdb[nnatom].getAtInf().substr(11,4);
            chn   = vctapAtomPdb[nnatom].getAtInf().substr(10,1);
         }
         else
         {
            //if (!ifFileStream.is_open()) break;

            if(ifrm2) // formatted reading
            {
               getline(ifFileStream,strLine);

               if (ifFileStream.eof()) break;

               strHead = strLine.substr(0,6); strHead = toUpperCase(strHead);
               if (0 != strHead.compare("ATOM  ") && 0 != strHead.compare("HETATM")) continue;
               crdstr = strLine.substr(30,24);
               atnum = strLine.substr(6,5);
               xo.nX = atof(crdstr.substr(0,8).c_str()); xo.nY = atof(crdstr.substr(8,8).c_str()); xo.nZ = atof(crdstr.substr(16,8).c_str());
               inum = atoi(atnum.c_str());
            }
            else // unformatted (binary) reading
            {
               ifFileStream.read( reinterpret_cast<char*>(&xo.nX),sizeof(real) );
               ifFileStream.read( reinterpret_cast<char*>(&xo.nY),sizeof(real) );
               ifFileStream.read( reinterpret_cast<char*>(&xo.nZ),sizeof(real) );
               ifFileStream.read( reinterpret_cast<char*>(&radu), sizeof(real) );
               ifFileStream.read( reinterpret_cast<char*>(&chrgv),sizeof(real) );
            }
         } //----- end of atom reading

         nnatom++;

         isitmp = (isitq && iqass) || isitap || isitp;

         if((isita || isitmp) && !iself)
         {
            atm  = strLine.substr(11,5);
            res  = strLine.substr(17,3);
            rnum = strLine.substr(22,4);
            chn  = strLine.substr(21,1);

            if (0 !=  atm.compare("     ")) {atm  = removeSpace(atm);  atm  = toUpperCase(atm);}
            if (0 !=  res.compare("   "))   {res  = removeSpace(res);  res  = toUpperCase(res);}
            if (0 != rnum.compare("    "))  {rnum = removeSpace(rnum); rnum = toUpperCase(rnum);}
            if (0 != chn.compare(" "))      {chn  = removeSpace(chn);  chn  = toUpperCase(chn);}
         }

         xn = (xo-fgBoxCenter)*fScale+goff; // scale atoms to grid space

         if (isita)
         {
            atdes.assign(16,' ');
            atdes.replace( 0,atm.size(),atm);
            atdes.replace( 5,res.size(),res);
            atdes.replace( 9,chn.size(),chn);
            atdes.replace(11,rnum.size(),rnum);
         }

         /*
          * assign charge to atom, searching for decreasingly specific specification
          * note if no charge record found, is assumed to be 0.0
          */
         if(!iself && ifrm2 && isitmp)
         {
            chrgv = 0.0;
            iFound = FindRecord(atm,res,rnum,chn,CHARGEFILE,chrgv);
            if(isitap) iFound = FindRecord(atm,res,rnum,chn,SIZEFILE,radu);
            radu = radu*fScale;
         }

         if (isitsf)
         {
            iresnum = atoi(rnum.c_str());
            atmsf[nnatom-1] = false;
            if (residsf[iresnum-1]) atmsf[nnatom-1] = true;
         }

         if(isitap && fZero < abs(chrgv))
         {
            real rads = min(radu,fPotentialUpperBond*fScale);
            SGrid<real> xt;

            xt = xn; xt.nX += rads;
            vphi = interpl(iGrid,phimap,xt);
            aphi = vphi;

            xt = xn; xt.nX -= rads;
            vphi = interpl(iGrid,phimap,xt);
            aphi += vphi;

            xt = xn; xt.nY += rads;
            vphi = interpl(iGrid,phimap,xt);
            aphi += vphi;

            xt = xn; xt.nY -= rads;
            vphi = interpl(iGrid,phimap,xt);
            aphi += vphi;

            xt = xn; xt.nZ += rads;
            vphi = interpl(iGrid,phimap,xt);
            aphi += vphi;

            xt = xn; xt.nZ -= rads;
            vphi = interpl(iGrid,phimap,xt);
            aphi += vphi;

            aphi = aphi/6.0;
         }

         if (isitp || isiti || (isitap && fZero > abs(chrgv)))
         {
            vphi = interpl(iGrid,phimap,xn);
            if (isitap && fZero > abs(chrgv)) aphi = vphi;
            if (isitp) { etot += chrgv*vphi; phiv = vphi; }

            if (isiti)
            {
               CNoIDebMap warning;

               /*
                * we have changed the iconc action so that the phimap has NOT been converted to salt concentrations. therefore
                */
               if (0 != iNonIterateNum)
               {
                  temp = vphi*fTaylorCoeff5+fTaylorCoeff4; temp = vphi*temp+fTaylorCoeff3;
                  temp = vphi*temp+fTaylorCoeff2;          temp = vphi*temp+fTaylorCoeff1;
                  phii = vphi*temp;
               }
               else
                  phii = -fIonStrength*2.0*vphi;
            }
         } //----- end if isitp or isiti, salt and or potentials

         if (isitdeb) // it calculates the fraction of closest grid points that are in solution
         {
            cout << "Calculating Debye Fraction\n";
            debyefraction = boolinterpl(iGrid,prgbDielecMap,xn);
         }

         if (isitf)
         {
            xn.nX += 1.0;               fu.nX = interpl(iGrid,phimap,xn);
            xn.nX -= 2.0;               fl.nX = interpl(iGrid,phimap,xn);
            xn.nX += 1.0; xn.nY += 1.0; fu.nY = interpl(iGrid,phimap,xn);
            xn.nY -= 2.0;               fl.nY = interpl(iGrid,phimap,xn);
            xn.nY += 1.0; xn.nZ += 1.0; fu.nZ = interpl(iGrid,phimap,xn);
            xn.nZ -= 2.0;               fl.nZ = interpl(iGrid,phimap,xn);
            xn.nZ += 1.0;
            fxyz = (fl-fu)*0.5*fScale; // the electric field is opposite the potential gradient so I change the sign
         }

         /*
          * check if this point is within the box.
          */
         if (isitt)
         {
            SExtrema<real> bedge;
            bedge.nMin = fgBoxCenter-0.5*(iGrid-1)/fScale; bedge.nMax = fgBoxCenter+0.5*(iGrid-1)/fScale;

            int it = 0;
            if ( optORLT<real>(xo,bedge.nMin) || optORGT<real>(xo,bedge.nMax) ) it = 1;

            if (0 == it)
            {
               xo2 = (xo-fgBoxCenter)*fScale+goff;

               /*
                * first find reaction field from surface elements inside of the box..
                */
               phir=0.0; phias=0.0; ncrgs=0; tcrgs=0.0; sold.assign(30,0.0);

               for (integer i = 0; i < iDielecBndySum; i++)
               {
                  vtemp = xo - prgfgSurfCrgA[i]; dist = sqrt(optDot(vtemp,vtemp));
                  //----- first find reaction field from surface elements inside of the box
                  ncrgs++; tcrgs += prgfSurfCrgE[i]; phirtt = prgfSurfCrgE[i]/dist;
                  //----- medeps either epsin contain the 561.0 factor....
                  phirtt = phirtt*fEPKT; phir += phirtt;
                  xu2.nX = (real)prgigBndyGrid[i].nX; xu2.nY = (real)prgigBndyGrid[i].nY; xu2.nZ = (real)prgigBndyGrid[i].nZ;
                  crgs = prgfSurfCrgE[i];
                  //----- took place of repsin because eps is no more included in schrg , surface charge
                  phiat = tops(xu2,xo2,crgs,1.0,1); phiat = phiat*2.0; phias += phiat;
                  idist = (int)dist; sold[idist] += phiat - phirtt;
               }

               temp = 0.0;
               cout << "Writing sold(1:30) and temp \n";
               for (integer i = 0; i < 30; i++)
               {
                  temp += sold[i];
                  cout << sold[i] << " " << temp;
               }
               cout << endl;

               /*
                * next find the colombic potential for that site from charges within the box
                */
               phic = 0.0; phiac = 0.0; ncrg = 0;

               for (integer i = 0; i < iCrgGridNum; i++)
               {
                  it = 0;
                  if (optORLT<real>(prgfgCrgPoseA[i],bedge.nMin) || optORGT<real>(prgfgCrgPoseA[i],bedge.nMax)) it = 1;

                  if (0 == it)
                  {
                     ncrg++;
                     vtemp = xo - prgfgCrgPoseA[i]; dist = sqrt(optDot(vtemp,vtemp));

                     if (5.0 > dist)
                     {
                        if (fZero < dist) {temp = prggvAtomicCrg[i].nValue/dist; phic += temp/prgfAtomEps[i];}
                        //----- find analytic potential from this real charge..=phiac
                        xu  = prgfgCrgPoseA[i]; crgs = prggvAtomicCrg[i].nValue;
                        xu2 = (xu-fgBoxCenter)*fScale+goff;
                        eps = prgfAtomEps[i]*fEPKT;
                        phiact = tops(xu2,xo2,crgs,eps,1);
                        phiac += phiact;
                     }
                  }
               }

               /*
                * medeps, either epsin contain the 561.0 factor....
                */
               phiac = phiac*2.0;

               /*
                * find the grid potentials..
                */
               phiv = interpl(iGrid,phimap,xn);

               string strFileName7 = "extra.dat";
               ofstream ofFileSteam7;
               ofFileSteam7.open(strFileName7.c_str());
               ofFileSteam7 << phic << " " << phir << " " << phiv << " " << phias << " " << phiac << " "<< ncrg << " "
                            << ncrgs << " " << tcrgs << endl;
               ofFileSteam7.close();

               phit = phic + phir + phiv - phias - phiac;
            }
            else
               phit = 0.0;

            /*
             * phit contains the total corrected potential
             */
         }

         if (isitr)
         {
            scomp.assign(30,0.0); sold.assign(30,0.0); phir = 0.0;

            for (integer i = 0; i < iDielecBndySum; i++)
            {
               vtemp = xo - prgfgSurfCrgA[i]; dist = sqrt(optDot(vtemp,vtemp));
               idist = (int)dist;
               if (30 > idist) sold[idist] += fEPKT*prgfSurfCrgE[i]/dist;
               phir += prgfSurfCrgE[i]/dist;
            }

            /*
             * medeps either epsin contains the 561.0 factor....
             */
            phir = phir*fEPKT;

            for (integer i = 0; i < 30; i++)
            {
               if (0 == i) scomp[i] = sold[i];
               if (0 != i) scomp[i] = scomp[i-1]+sold[i];
            }

            phirt += phir*chrgv;
         }

         /*
          * medeps either epsin contains the 561.0 factor....
          */
         if (isitrf || isitmd || isittf) rxyz = rfield[nnatom-1]*fEPKT;

         if(isitcf || isitmd || isittf)
         {
            cxyz.nX = 0.0; cxyz.nY = 0.0; cxyz.nZ = 0.0;

            if (fZero < abs(chrgv))
            {
               for (integer i = 0; i < iCrgGridNum; i++)
               {
                  vtemp = xo - prgfgCrgPoseA[i]; dist = optDot(vtemp,vtemp);
                  if (fZero < dist)
                  {
                     sdist = sqrt(dist)*dist;
                     temp  = prggvAtomicCrg[i].nValue/(prgfAtomEps[i]*sdist);
                     cxyz  = cxyz + vtemp*temp;
                  }
               }

               /*
                * atmeps and medeps and epsin contain the 561.0 factor....
                */
               cxyz = cxyz*chrgv;
            }
         }

         if (isitc)
         {
            phic = 0.0;

            for (integer i = 0; i < iCrgGridNum; i++)
            {
               vtemp = xo - prgfgCrgPoseA[i]; dist = optDot(vtemp,vtemp);
               if (fZero < dist)
               {
                  sdist = sqrt(dist);
                  temp  = prggvAtomicCrg[i].nValue/sdist;
                  phic += temp/prgfAtomEps[i];
               }
            }

            /*
             * atmeps and medeps and epsin contain the 561.0 factor....
             */
            phict += phic*chrgv;
         }

         //---------------------------------------------------------------------------------------//
         /*
          * write out calculated/assigned charges
          *
          * need otemp cos can not write into a substring apparently
          * otemp needs to be at least 15 long to avoid an error!!
          */
         oline.assign(80,' '); // reset oline
         j = 0;

         if (isita)
         {
            oline.replace(j,16,atdes.substr(0,16));
            j += 20;
         }

         if (isitx)
         {
            sprintf(otemp,"%10.4f",xo.nX); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",xo.nY); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",xo.nZ); oline.replace(j,10,otemp); j += 10;
         }

         if (isitq)
         {
            sprintf(otemp,"%10.4f",chrgv); oline.replace(j,10,otemp); j += 10;
         }

         if (isitp)
         {
            sprintf(otemp,"%10.4f",phiv);  oline.replace(j,10,otemp); j += 10;

#ifdef MCCE
            mcce_phiv.push_back(phiv);
#endif
         }

         if (isiti)
         {
            sprintf(otemp,"%10.4f",phii);  oline.replace(j,10,otemp); j += 10;
         }

         if (isitr)
         {
            sprintf(otemp,"%10.4f",phir);  oline.replace(j,10,otemp); j += 10;
         }

         if (isitc)
         {
            sprintf(otemp,"%10.4f",phic);  oline.replace(j,10,otemp); j += 10;
         }

         if (isitap)
         {
            sprintf(otemp,"%10.4f",aphi);  oline.replace(j,10,otemp); j += 10;
         }

         if (isitdeb)
         {
            sprintf(otemp,"%10.4f",debyefraction); oline.replace(j,10,otemp); j += 10;
         }

         if (isitf)
         {
            sprintf(otemp,"%10.4f",fxyz.nX); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",fxyz.nY); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",fxyz.nZ); oline.replace(j,10,otemp); j += 10;
         }

         if (isitrf)
         {
            sprintf(otemp,"%10.4f",rxyz.nX); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",rxyz.nY); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",rxyz.nZ); oline.replace(j,10,otemp); j += 10;
         }

         if (isitcf)
         {
            sprintf(otemp,"%10.4f",cxyz.nX); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",cxyz.nY); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",cxyz.nZ); oline.replace(j,10,otemp); j += 10;
         }

         if (isittf)
         {
            vtemp = rxyz + cxyz;
            sprintf(otemp,"%10.4f",vtemp.nX); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",vtemp.nY); oline.replace(j,10,otemp); j += 10;
            sprintf(otemp,"%10.4f",vtemp.nZ); oline.replace(j,10,otemp); j += 10;
         }

         if (isitmd)
         {
            vtemp = rxyz + cxyz;
            cout << "atom: " << nnatom << " rx= " << rxyz.nX << " cx= " << cxyz.nX << " tx= " << vtemp.nX << endl;
            cout << "atom: " << nnatom << " ry= " << rxyz.nY << " cy= " << cxyz.nY << " ty= " << vtemp.nY << endl;
            cout << "atom: " << nnatom << " rz= " << rxyz.nZ << " cz= " << cxyz.nZ << " tz= " << vtemp.nZ << endl;
         }

         if (isitt)
         {
            sprintf(otemp,"%10.4f",phit); oline.replace(j,10,otemp); j += 10;
         }

         if (ofrm && isitmp1) ofFileStream << oline << endl;

         if (!ofrm && isitmp1)
         {
            if (isita)  ofFileStream << atdes << endl;
            if (isitx)  ofFileStream << xo    << endl;
            if (isitq)  ofFileStream << chrgv << endl;
            if (isitp)  ofFileStream << phiv  << endl;
            if (isiti)  ofFileStream << phii  << endl;
            if (isitr)  ofFileStream << phir  << endl;
            if (isitc)  ofFileStream << phic  << endl;
            if (isitap) ofFileStream << aphi  << endl;
            if (isitf)  ofFileStream << fxyz  << endl;
            if (isitrf) ofFileStream << rxyz  << endl;
            if (isitcf) ofFileStream << cxyz  << endl;
            if (isittf) { vtemp = rxyz + cxyz; ofFileStream << vtemp << endl;}
         }

      } while(true); // end of the big loop on natom
   }

   if (isitsf)
   {
      for (integer jj = 0; jj < iBndyGridNum; jj++)
      {
         integer i = prgiAtSurf[jj];

         if (atmsf[i-1] && (0 < prgiAtNdx[jj]))
         {
            /*
             * if the bgp belongs to the interesting site
             * attention: using always radprb(1), in some case might be inappropriate
             */
            xo = prgfgSurfCrgA[jj]+rgfProbeRadius[0]*prgfgSurfCrgE[jj];
            xn = (xo-fgBoxCenter)*fScale+goff;

            xn.nX += 1.0;               fu.nX = interpl(iGrid,phimap,xn);
            xn.nX -= 2.0;               fu.nX = interpl(iGrid,phimap,xn);
            xn.nX += 1.0; xn.nY += 1.0; fu.nY = interpl(iGrid,phimap,xn);
            xn.nY -= 2.0;               fu.nY = interpl(iGrid,phimap,xn);
            xn.nY += 1.0; xn.nZ += 1.0; fu.nZ = interpl(iGrid,phimap,xn);
            xn.nZ -= 2.0;               fu.nZ = interpl(iGrid,phimap,xn);
            xn.nZ += 1.0;

            fxyz = fl - fu;
            fn   = 0.5*fScale*(optDot(fxyz,prgfgSurfCrgE[jj]));
            ff   = 0.5*fScale*(optDot(fxyz,fxyz));

            if (ofrm)
            {
               jtmp = j;

               sprintf(otemp,"%10.4f",prgfSurfCrgE[jj]); oline.replace(jtmp,10,otemp); jtmp += 10;
               sprintf(otemp,"%10.4f",xo.nX);            oline.replace(jtmp,10,otemp); jtmp += 10;
               sprintf(otemp,"%10.4f",xo.nY);            oline.replace(jtmp,10,otemp); jtmp += 10;
               sprintf(otemp,"%10.4f",xo.nZ);            oline.replace(jtmp,10,otemp); jtmp += 10;
               sprintf(otemp,"%10.4f",fn);               oline.replace(jtmp,10,otemp); jtmp += 10;
               sprintf(otemp,"%10.4f",ff);               oline.replace(jtmp,10,otemp); jtmp += 10;

               ofFileStream << oline << endl;
            }

            if (!ofrm)  ofFileStream << prgfSurfCrgE[jj] << " " << fn << endl;
         }
      }
   }

   if(!iself) ifFileStream.close();

#ifdef VERBOSE
   cout << "\n number of atom coordinates read : " << nnatom << endl << endl;
#endif

   etot = etot/2.0;

   if (ofrm)
   {
      if (0 == iFrcFormatOut)
      {
         ofFileStream << "total energy = " << etot << " kt\n";
         if (isitr)  ofFileStream << "corrected reaction field energy= " << phirt/2.0 << " kt\n";
         if (isitap) ofFileStream << "Atomic potential for charged atoms is averaged over a spherical surface of less than "
                                  << fPotentialUpperBond << " A\n";
      }

      if (1 == iFrcFormatOut)
      {
         ofFileStream << "corrected reaction field energy= " << phirt/2.0 << " kt\n";
         ofFileStream << "total coulombic energy     = " << phict/2.0 << " kt\n";
         if (isitap) ofFileStream << "Atomic potential for charged atoms is averaged over a spherical surface of less than "
                                  << fPotentialUpperBond << " A\n";
      }

      if (2 == iFrcFormatOut)
      {
         ofFileStream << "corrected reaction field energy= " << phirt/2.0 << " kt\n";
         if (isitap) ofFileStream << "Atomic potential for charged atoms is averaged over a spherical surface of less than "
                                  << fPotentialUpperBond << " A\n";
      }
   }

   /*
    * end of formatted frc read/write and unformatted frc write
    * end of unformatted frc.pdb read and frc write
    */
   if (ofFileStream.is_open()) ofFileStream.close();

#ifdef VERBOSE
   cout << "frc stuff now done at "; pTimer->showTime(); cout << endl;
#endif
}
Beispiel #16
0
INT4 InitPcsi (void)

/***********************************************************************
*
*_Title	InitPcsi - Initialize pcsi parameters 
*
*_DESC	Initialize pcsi parameters 
*
*_HIST	Mar 9 2003 Janet Barrett, Created by modifying the initpcp
*                  subroutine used by the pc2d program.
*       Jul 9 2003 JB - Fixed a problem with handling Level 1 labels
*                  in a Level 2 image.
*       Jan 11 2010 JB - The both.level element is not being set by
*                  calls to lev1_init or lev2_init. This was fixed by
*                  using the spi_level value in place of it.
*                  
*_END
************************************************************************/

{

  BOOLEAN levels,frame;
  INT4 level,spi_level;
  INT4 lzin,llog,lnote;
  INT4 length,icount,ret;
  INT4 naxes,order,core_size[3],suf_size[3];
  INT4 issi,isli;
  INT4 iesi,ieli;
  INT4 iflag,jflag;
  INT4 lpos;
  INT4 icard;
  INT4 ccont[3];
  INT4 scont[3];
  INT4 dflag;
  INT4 icountx,icounty;
  INT4 levgroups;
  INT4 found;
  INT4 i,j;
  int (*p)();

  FLOAT4 dzxm,dzym;
  FLOAT4 z01,z02,z03,z04;
  FLOAT4 xsinci,xlinci;
  FLOAT4 dzdip;
  FLOAT4 arg;
  FLOAT4 si0;
  FLOAT4 se0;
  FLOAT4 bclipin;
  FLOAT4 db1,db2;
  FLOAT4 cardaz;
  FLOAT4 delaza,delazb;
  FLOAT4 xcenter,ycenter;
  FLOAT4 clat,clon;

  FLOAT8 azi;
  FLOAT8 lat,lon;
  FLOAT8 inc,emi,pha;
  FLOAT8 scaz,sunaz;
  FLOAT8 ssclat,ssclon;
  FLOAT8 ssunlat,ssunlon;
  FLOAT8 radii[3];
  FLOAT8 slantrange;

  CHAR axis_name[3][MAXLITLEN+1];
  CHAR item_type[MAXLITLEN+1];
  CHAR prmname[30];
  CHAR tmpstr[256];
  CHAR planet[11];
  CHAR sfrom[136];
  CHAR scan_sc[NUMOFSCAN][LEV_SPACECRAFT_NAME_SIZE] = 
       {"MARS_GLOBAL_SURVEYOR",
        "MARS_GLOBAL_SURVEYOR",
	"MARS_ODYSSEY"};
  CHAR scan_inst[NUMOFSCAN][LEV_INSTRUMENT_NAME_SIZE] =
       {"MOC_NA_A",
        "MOC_WA_A",
	"THEMIS_IR"};
  CHAR frame_sc[NUMOFFRAME][LEV_SPACECRAFT_NAME_SIZE] =
       {"GALILEO_1",
        "VIKING_ORBITER_1",
	"VIKING_ORBITER_1",
	"VIKING_ORBITER_2",
	"VIKING_ORBITER_2",
	"MARS_ODYSSEY",
	"VOYAGER_1",
	"VOYAGER_1",
	"VOYAGER_2",
	"VOYAGER_2"};
/*	"CASSINI",
	"LUNAR_ORBITER_4"};*/
  CHAR frame_inst[NUMOFFRAME][LEV_INSTRUMENT_NAME_SIZE] =
       {"SSI",
        "VISUAL_IMAGING_SUBSYSTEM_CAMERA_A",
	"VISUAL_IMAGING_SUBSYSTEM_CAMERA_B",
        "VISUAL_IMAGING_SUBSYSTEM_CAMERA_A",
	"VISUAL_IMAGING_SUBSYSTEM_CAMERA_B",
	"THEMIS_VIS",
	"WA",
	"NA",
	"WA",
	"NA"};
/*	"VIMS",
	"24_INCH_FOCAL_LENGTH_CAMERA"};*/
  FLOAT8 frame_flinpix[NUMOFFRAME] =
       {98455.811,
        40323.83,
	40328.08,
	40341.85,
	40298.585,
	22222.222,
        16989.138,
	127248.26,
	17209.598,
	127528.17};

  LevData data,tmpdata;
  Lev both;
  FLOAT8 sampres,lineres,azres,sres;

  INT4 PcsiFirstGuess (void);
  CHAR obj_name[5]="QUBE";
  CHAR grp_name[1] = "";
  CHAR grp2_name[21]="IMAGE_MAP_PROJECTION";
  INT4 mulpht;
  INT4 whcnt,hg1cnt,hg2cnt,hhcnt,b0cnt,thetacnt,bhcnt,chcnt,kcnt,lcnt;

/******************************************************************************
* Get input and output file parameters from TAE
******************************************************************************/
  strcpy(prmname,"TO");
  u_get_file_parm("TO",1,(CHAR *)topofile,sizeof(topofile),&length,&icount,&ret);
  if (ret) goto user_parameter_error;
  if (icount <= 0 || strlen(topofile) == 0) goto to_file_error;
  (void) u_strtrim(topofile,topofile);
  if (strlen(topofile) == 0) goto to_file_error;
  if (u_file_exist(topofile)) goto to_file_exist;
  if (strncmp(&(topofile[strlen(topofile)-1]),"/",1) == 0) goto to_file_error;
  (void) u_ver_flspec(topofile,"cub",topofile,sizeof(topofile),&ret);
  if (ret) goto to_file_invalid;

  strcpy(prmname,"ZOUT");
  u_get_file_parm("ZOUT",1,(CHAR *)zoutfile,sizeof(zoutfile),&length,&icount,&ret);
  if (ret) goto user_parameter_error;
  if (icount <= 0 || strlen(zoutfile) == 0) {
    dozout = FALSE;
  } else {
    (void) u_strtrim(zoutfile,zoutfile);
    (void) u_ver_flspec(zoutfile,"cub",zoutfile,sizeof(zoutfile),&ret);
    if (ret) goto flspec_error;
    if (u_file_exist(zoutfile)) {
      if (remove(zoutfile)) goto zout_remove_error;
    }
    dozout = TRUE;
  }

  (void) xctae_files(zin,sizeof(zin),&lzin,&usezin,&usedatum,logfile,
	      sizeof(logfile),&llog,note,sizeof(note),&lnote);

/******************************************************************************
* Get subcube specifiers
******************************************************************************/
  strcpy(prmname,"SFROM");
  u_get_str_parm("SFROM",1,(CHAR *)sfrom,sizeof(sfrom),&length,&icount,&ret);
  if (ret) goto user_parameter_error;
  if (icount <= 0) (void) strcpy(sfrom," ");

/******************************************************************************
* Open the FROM file and get information
******************************************************************************/
  strcpy(prmname,"FROM");
  u_get_file_parm("FROM",1,(CHAR *)from,sizeof(from),&length,&icount,&ret);
  if (ret) goto user_parameter_error;
  q_open(&fid,0,from,READ_ONLY,0,"",0,0,1,0,&ret);
  if (ret) goto cube_open_error;
  q_check_std(fid,&ret);
  if (ret) goto invalid_cube;
  q_get_sys_keys(fid,1,&naxes,&order,core_size,suf_size,
	         (char *)axis_name,sizeof(axis_name[0]),&image_bytes,
		 item_type,sizeof(item_type),image_scale,&ret);
  if (ret) goto bad_sys_keys;

/******************************************************************************
* Parse the SFROM subcube specification and apply to the FROM file and
* then read system keywords again to reflect the virtual cube
******************************************************************************/
  (void) u_parse_isub(fid,sfrom,1,core_size,suf_size,ccont,scont,&ret);
  if (ret) goto parse_sfrom_error;
  (void) q_set_virtual(fid,&ret);
  if (ret) goto sfrom_error;
  q_get_sys_keys(fid,1,&naxes,&order,core_size,suf_size,
                 (char *)axis_name,sizeof(axis_name[0]),&image_bytes,
		 item_type,sizeof(item_type),image_scale,&ret);
  if (ret) goto bad_sys_keys;
  ins = core_size[0];
  inl = core_size[1];

/******************************************************************************
* Set the variables describing the FROM subcube
******************************************************************************/
  u_get_isub_desc(fid,"SAMPLE",&issi,&iesi,&xsinci,&dflag,&ret);
  if (ret) goto bad_subsamp;
  if (dflag != 1) goto bad_subsamp;
  u_get_isub_desc(fid,"LINE",&isli,&ieli,&xlinci,&dflag,&ret);
  if (ret) goto bad_subline;
  if (dflag != 1) goto bad_subline;
  if (xsinci != xlinci) goto inc_error; /* Need to be equal or
                               map scale will not be accurate */

/******************************************************************************
* Set center of virtual subcube
******************************************************************************/
  xcenter = 0.5*(ins+1);
  ycenter = 0.5*(inl+1);

/******************************************************************************
* Get photoclinometry parameters from TAE
******************************************************************************/
  strcpy(prmname,"MAXMEM");
  u_get_int_parm("MAXMEM",1,&C_MEM3->nmax,&icount,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"DNATM");
  u_get_real_parm("DNATM",1,1,6,&C_DNORM->dnatm,&icount,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"DNDATUM");
  u_get_real_parm("DNDATUM",1,1,6,&dndatum,&icount,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"DATUMTYP");
  u_get_int_parm("DATUMTYP",1,&C_DATUM2->datumtyp,&icount,&ret);
  if (ret) goto user_parameter_error;

  nucenter = FALSE; /* Reset to True if X,Y inputs given */

  strcpy(prmname,"X");
  u_get_dbl_parm("X",1,1,6,&x,&icountx,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"Y");
  u_get_dbl_parm("Y",1,1,6,&y,&icounty,&ret);
  if (ret) goto user_parameter_error;
  if ((icountx >= 1) && (icounty >= 1)) nucenter = TRUE;

  strcpy(prmname,"DISTORTD");
  u_get_str_parm("DISTORTD",1,(CHAR *)tmpstr,sizeof(tmpstr),&length,&icount,&ret);
  if (ret) goto user_parameter_error;

  distortd = TRUE;
  if (strncmp(tmpstr,"NO",1) == 0) 
    distortd = FALSE;

  strcpy(prmname,"METHOD");
  u_get_str_parm("METHOD",1,(CHAR *)method,sizeof(method),&length,&icount,&ret);
  if (ret) goto user_parameter_error;
  if (strncmp(method,"SOR",3) == 0) {
    sordir = 0;
  } else if (strncmp(method,"DIR",3) == 0) {
    sordir = 1;
  } else {
    sordir = 2;
  }

  strcpy(prmname,"PHOFUNC");
  u_get_str_parm("PHOFUNC",1,(CHAR *)C_PPARS4->phofunc,sizeof(C_PPARS4->phofunc),
                 &length,&icount,&ret);
  if (ret) goto user_parameter_error;

  C_PPARS1->piopt = 0;
  if (strncmp(C_PPARS4->phofunc,"HAPLEG",6) == 0)
    C_PPARS1->piopt = 1;
  if (strncmp(C_PPARS4->phofunc,"HAPL_S",6) == 0)
    C_PPARS1->piopt = 1;

  C_PPARS3->hapke = FALSE;
  if (strncmp(C_PPARS4->phofunc,"H",1) == 0) 
    C_PPARS3->hapke = TRUE;

  strcpy(prmname,"WH");
  u_get_real_parm("WH",1,1,6,&C_PPARS5->pwh,&whcnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"HG1");
  u_get_real_parm("HG1",1,1,6,&C_PPARS5->phg1,&hg1cnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"HG2");
  u_get_real_parm("HG2",1,1,6,&C_PPARS5->phg2,&hg2cnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"HH");
  u_get_real_parm("HH",1,1,6,&C_PPARS5->phh,&hhcnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"B0");
  u_get_real_parm("B0",1,1,6,&C_PPARS5->pb0,&b0cnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"THETA");
  u_get_real_parm("THETA",1,1,6,&C_PPARS5->ptheta,&thetacnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"BH");
  u_get_real_parm("BH",1,1,6,&C_PPARS5->pbh,&bhcnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"CH");
  u_get_real_parm("CH",1,1,6,&C_PPARS5->pch,&chcnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"K");
  u_get_real_parm("K",1,1,6,&C_PPARS2->pex,&kcnt,&ret);
  if (ret) goto user_parameter_error;

  strcpy(prmname,"L");
  u_get_real_parm("L",1,1,6,&C_PPARS2->pl2,&lcnt,&ret);
  if (ret) goto user_parameter_error;

  if (strncmp(C_PPARS4->phofunc,"HAPHEN",6) == 0) {
    if (whcnt < 1 || thetacnt < 1 || b0cnt < 1 || hhcnt < 1 ||
        hg1cnt < 1 || hg2cnt < 1) goto haphen_error;
  } else if (strncmp(C_PPARS4->phofunc,"HAPLEG",6) == 0) {
    if (whcnt < 1 || thetacnt < 1 || b0cnt < 1 || hhcnt < 1 ||
        bhcnt < 1 || chcnt < 1) goto hapleg_error;
  } else if (strncmp(C_PPARS4->phofunc,"HAPH_S",6) == 0) {
    if (whcnt < 1 || hg1cnt < 1 || hg2cnt < 1) goto haph_s_error;
  } else if (strncmp(C_PPARS4->phofunc,"HAPL_S",6) == 0) {
    if (whcnt < 1 || bhcnt < 1 || chcnt < 1) goto hapl_s_error;
  } else if (strncmp(C_PPARS4->phofunc,"LAMBER",6) == 0) {
    C_PPARS2->pex = 1.0;
    C_PPARS2->pl2 = 0.0;
  } else if (strncmp(C_PPARS4->phofunc,"LOMSEL",6) == 0) {
    C_PPARS2->pex = 1.0;
    C_PPARS2->pl2 = 1.0;
  } else if (strncmp(C_PPARS4->phofunc,"MIN",3) == 0) {
    C_PPARS2->pl2 = 0.0;
    if (kcnt < 1) goto min_error;
  } else if (strncmp(C_PPARS4->phofunc,"LUNLAM",6) == 0) {
    C_PPARS2->pex = 1.0;
    if (lcnt < 1) goto lunlam_error;
  }

/******************************************************************************
* Evaluate default (center) location in image if needed. This default is in
* ALL cases now the center of the virtual subcube, transferred to full cube.
******************************************************************************/
  if (!nucenter) {
    x = (FLOAT8) (issi) + (xcenter-1.0) * xsinci;
    y = (FLOAT8) (isli) + (ycenter-1.0) * xlinci;
  }

/******************************************************************************
* Check for the 3 levels Level 1 keyword groups (ISIS_INSTRUMENT, 
* ISIS_GEOMETRY, ISIS_TARGET)
******************************************************************************/
  levgroups = 0;
  (void) p_check_key(fid,"QUBE","ISIS_INSTRUMENT","",&icount,&ret);
  if (!ret) levgroups = levgroups + 1;
  (void) p_check_key(fid,"QUBE","ISIS_GEOMETRY","",&icount,&ret);
  if (!ret) levgroups = levgroups + 1;
  (void) p_check_key(fid,"QUBE","ISIS_TARGET","",&icount,&ret);
  if (!ret) levgroups = levgroups + 1;

/******************************************************************************
* All 3 levels Level 1 keyword groups are missing. This is either a 
* pre-levels Level 1 file or a Level 2 file that has had the groups erased
* from the labels (or, under the old system, never got them). If the file
* contains either ISIS_MOSAIC or IMAGE_MAP_PROJECTION keyword groups, then
* it is not a Level 1 image and cannot be used. Otherwise, try using old
* SPICE routines.
******************************************************************************/
  if (levgroups == 0) {
    (void) p_check_key(fid,"QUBE","ISIS_MOSAIC","",&icount,&ret);
    if (!ret) {
    /* Level 2 mosaic without required groups */
      goto mosaic_group_error;
    } else {
      (void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION","",&icount,&ret);
      if (!ret) {
        (void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION",
	  "MAP_PROJECTION_TYPE",&icount,&ret);
        if (!ret) {
        /* Level 2 image without required groups */
	  goto level2_group_error;
        }
      }
    }

    /* Pre-levels Level 1 image */
    iflag = 0;
    jflag = 1;
    if (!distortd) jflag = 0;
    spi_phosun(fid,obj_name,sizeof(obj_name),grp_name,sizeof(grp_name),
      grp2_name,sizeof(grp2_name),jflag,0,&iflag,
      &y,&x,&lat,&lon,&emi,&inc,&pha,&ssclat,&ssclon,
      &scaz,&ssunlat,&ssunlon,&sunaz,&aznor,&res,&ret);
    if (ret) goto spi_phosun_error;

/******************************************************************************
* If we got to here without an error, this is a valid pre-levels Level 1
* file. 
******************************************************************************/
    levels = FALSE;
    level = 1;

/******************************************************************************
* All pre-levels instruments are framing cameras.
******************************************************************************/
    frame = TRUE;

/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the framing camera case:  Z axis is viewing direction, so
* emission angle is used to calculate datum derivatives rather than emission
* vector. Also calculate the scaling variable, cosemi, to be used for 
* line-of-sight to vertical on output and vertical to line-of-sight on
* input.
******************************************************************************/
    res = res * xsinci; /* xsinci=xlinci guaranteed */
    sres = res;
    C_AEPAR1->aspect = 1.0;
    C_PSNPAR->phase[0] = pha;
    clinc = pha;
    azinc = sunaz;
    clemi = 0.0;
    azemi = 0.0;
    dip = emi;
    az = scaz + 180.0;
    cosemi = cos(emi*L_DEG2RAD);

/******************************************************************************
* Obtain parameters needed for spherical datum
******************************************************************************/
    if (C_DATUM2->datumtyp == 2) {
      spi_lbplan(fid,"QUBE","","IMAGE_MAP_PROJECTION",planet,
	sizeof(planet),radii,&lpos,&ret);
      if (ret) goto spi_lbplan_error;
      if (radii[1] == 0.0) radii[1] = radii[0];
      if (radii[2] == 0.0) radii[2] = radii[0];
      radius = sqrt((radii[0]*radii[0]+radii[1]*radii[1]+
		 radii[2]*radii[2])/3.0);
      C_DATUM1->r0 = radius/res;  /* Planet radius in pixels */

/******************************************************************************
* Obtain planet center in full frame and convert to subframe. xs0 and yl0 are
* with respect to the pixels of the subcube; xs0f and yl0f are computed by
* geomset at each grid resolution and refer to coordinates equal to indices
* in the DEM.
******************************************************************************/
      spi_findlin(fid,(FLOAT4) ssclat,(FLOAT4) ssclon,
        &C_DATUM1->xs0,&C_DATUM1->yl0,&ret);
      if (ret) goto spi_findlin_error;
      C_DATUM1->xs0=(C_DATUM1->xs0 - (FLOAT4) issi)/xsinci
	+ 1.0;
      C_DATUM1->yl0=(C_DATUM1->yl0 - (FLOAT4) isli)/xlinci
	+ 1.0;

/******************************************************************************
* Must calculate z00s for sphere, putting middle of image at height 0
******************************************************************************/
      C_DATUM1->z00s = -sqrt(C_DATUM1->r0*C_DATUM1->r0 - 
	pow((xcenter-C_DATUM1->xs0)*C_AEPAR1->aspect,2.0) -
	pow((ycenter-C_DATUM1->yl0),2.0));
    }

/******************************************************************************
* All 3 levels Level 1 keyword groups were found. This is a levels file,
* either Level 1 or Level 2 with the Level 1 groups included.
******************************************************************************/
  } else if (levgroups == 3) {
    levels = TRUE;
    spi_level = lev_find_level(fid);
    if (spi_level == 2) {
      if (lev2_init_from_labels(fid,&both.map)) goto lev_init_error;
      if (lev1_init(fid,&both.spi,0)) goto lev_init_error;
    } else {
      if (lev1_init(fid,&both.spi,0)) goto lev_init_error;
    }
/*      if (lev_init(fid,&both)) goto lev_init_error;*/

/******************************************************************************
* If input file is level 2 (map projected), then get the center latitude
* and center longitude.
******************************************************************************/
    if (spi_level > 1 && strcmp(both.map.proj.name,"ORTHOGRAPHIC") == 0) {
      (void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LATITUDE",
        &icount,&ret);
      if (ret) goto clat_error;
      icount = 1;
      p_get_real_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LATITUDE",
        2,&icount,&clat,&ret);
      if (ret) goto clat_error;
      (void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LONGITUDE",
        &icount,&ret);
      if (ret) goto clon_error;
      icount = 1;
      p_get_real_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LONGITUDE",
        2,&icount,&clon,&ret);
      if (ret) goto clon_error;
    }

/******************************************************************************
* This is a levels Level 1 image. 
******************************************************************************/
    if (spi_level == 1) {
      level = 1;
      data.samp = x;
      data.line = y;
      if (lev1_linesamp_to_latlon(&both.spi,&data)) goto ls_to_latlon;

/******************************************************************************
* Return emission, incidence, phase as emi, inc, pha respectively. These
* angles are evaluated at location x,y relative to the full image cube.
******************************************************************************/
      lev1_calc_emission_angle(&data,&emi);
      lev1_calc_incidence_angle(&data,&inc);
      lev1_calc_phase_angle(&data,&pha);

/******************************************************************************
* Check spacecraft/instrument to determine if it's a scanner or a
* framing camera .
******************************************************************************/
      found = -1;
      for (icount=0; icount<NUMOFSCAN; icount++) {
        if (strcmp(both.spi.inst.spacecraft,scan_sc[icount]) == 0
	    && strcmp(both.spi.inst.name,scan_inst[icount]) == 0) {
          frame = FALSE;
	  found = icount;
        }
      }
      if (found == -1) {
	for (icount=0; icount<NUMOFFRAME; icount++) {
	  if (strcmp(both.spi.inst.spacecraft,frame_sc[icount]) == 0
	      && strcmp(both.spi.inst.name,frame_inst[icount]) == 0) {
            frame = TRUE;
	    found = icount;
          }
	}
      }
      /* Invalid instrument */
      if (found == -1) goto unknown_inst;
	
/******************************************************************************
* This is a scanner. Spherical datum model does not make sense.
******************************************************************************/
      if (frame == FALSE) {

/******************************************************************************
* Return sample resolution, line resolution, spacecraft azimuth, sun azimuth 
* as sampres, lineres, scaz, sunaz respectively. All of these values are
* evaluated at location x,y relative to the full image cube.
******************************************************************************/
        if (lev1_calc_resolution(&both.spi,&data,&sampres,&lineres)) 
          goto res_error;
        if (sampres > lineres) azres = sampres;
        else azres = lineres;
        lev1_calc_spacecraft_azimuth(&both.spi,&data,azres,&scaz);
        lev1_calc_sun_azimuth(&both.spi,&data,azres,&sunaz);

/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the scanner case:  Z axis is local vertical, so
* emission angle is used to calculate emission vector rather than datum
* derivatives. Also calculate the scaling variable, cosemi, to be used 
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input (cosemi=1.0 for scanners).
******************************************************************************/
        res = lineres*xlinci;
	sres = sampres*xsinci;
	C_AEPAR1->aspect = sampres/lineres;
	C_PSNPAR->phase[0] = pha;
	clinc = inc;
	azinc = sunaz;
	clemi = emi;
	azemi = scaz;
	dip = 0.0;
	az = 0.0;
	C_DATUM2->datumtyp = 1;
	cosemi = 1.0;

/******************************************************************************
* This is a framing camera. 
******************************************************************************/
      } else {
        lev1_calc_slant_distance(&data,&slantrange);

/******************************************************************************
* Return resolution, spacecraft azimuth, sun azimuth as res[index],
* scaz, sunaz respectively. All of these values are evaluated at
* location x,y relative to the full image cube.
******************************************************************************/
        res = (slantrange/frame_flinpix[found])*xlinci;
        sres = res;
        azres = res;
        lev1_calc_spacecraft_azimuth(&both.spi,&data,azres,&scaz);
        lev1_calc_sun_azimuth(&both.spi,&data,azres,&sunaz);

/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the framing camera case:  Z axis is viewing direction, so
* emission angle is used to calculate datum derivatives rather than 
* emission vector. Also calculate the scaling variable, cosemi, to be used 
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input.
******************************************************************************/
        C_AEPAR1->aspect = 1.0;
	C_PSNPAR->phase[0] = pha;
	clinc = pha;
	azinc = sunaz;
	clemi = 0.0;
	azemi = 0.0;
	dip = emi;
	az = scaz + 180.0;
        cosemi = cos(emi*L_DEG2RAD);

/******************************************************************************
* Obtain parameters needed for spherical datum
******************************************************************************/
	if (C_DATUM2->datumtyp == 2) {
          radii[0] = both.spi.target.radii[0];
	  radii[1] = both.spi.target.radii[1];
	  radii[2] = both.spi.target.radii[2];
          if (radii[1] == 0.0) radii[1] = radii[0];
          if (radii[2] == 0.0) radii[2] = radii[0];
          radius = sqrt((radii[0]*radii[0]+radii[1]*radii[1]+
		   radii[2]*radii[2])/3.0);
          C_DATUM1->r0 = radius/res;  /* Planet radius in pixels */
	  lev1_calc_subspace(&both.spi,&data,&tmpdata);
	  ssclat = tmpdata.lat;
	  ssclon = tmpdata.lon;
	  data.lat = ssclat;
	  data.lon = ssclon;
	  if (lev1_latlon_to_linesamp(&both.spi,&data)) goto latlon_to_ls;
          C_DATUM1->xs0 = data.samp;
          C_DATUM1->yl0 = data.line;

/******************************************************************************
* We have planet center in terms of full image cube, so convert to 
* subcube.
******************************************************************************/
          C_DATUM1->xs0=(C_DATUM1->xs0 - (FLOAT4) issi)/xsinci
	    + 1.0;
          C_DATUM1->yl0=(C_DATUM1->yl0 - (FLOAT4) isli)/xlinci
	    + 1.0;

/******************************************************************************
* Must calculate z00s for sphere, putting middle of image at height 0
******************************************************************************/
          C_DATUM1->z00s = -sqrt(C_DATUM1->r0*C_DATUM1->r0 - 
	    pow((xcenter-C_DATUM1->xs0)*C_AEPAR1->aspect,2.0) -
	    pow((ycenter-C_DATUM1->yl0),2.0));
        }
      }

/******************************************************************************
* This is a levels Level 2 image. Having two Level 2 files is permitted
* in software. The two image subcubes are the same size and they have
* the same projection parameters. It doesn't matter if the scalar datum
* variables are set twice from labels for both images; the results are
* guaranteed to be the same.
******************************************************************************/
    } else {
      level = 2;

/******************************************************************************
* Next, check the projection. Orthographic projection allows a set of
* images to be treated like one image. The coverage could be highly
* localized (setting projection center to subspacecraft point minimizes
* parallax) or hemispheric in scale. Spherical datum type is always used
* in this projection, with center at center of projection. Coordinate
* system has Z axis out of planet at center of projection. This point is
* known by lat and lon. First find its Level 1 coordinates so we can find
* ema, inc, phase here. NOTE that azimuths are returned but they are with
* respect to Level 1 space so they don't get used.
******************************************************************************/
      if (strcmp(both.map.proj.name,"ORTHOGRAPHIC") == 0) {
	data.lat = (FLOAT8) clat;
	data.lon = (FLOAT8) clon;
	if (lev1_latlon_to_linesamp(&both.spi,&data)) 
	  goto latlon_to_ls;
        lev1_calc_emission_angle(&data,&emi);
        lev1_calc_incidence_angle(&data,&inc);
        lev1_calc_phase_angle(&data,&pha);

/******************************************************************************
* Now find the Level 2 coordinates of the center so we can find the
* azimuths. This routine gives line and sample of a given lat,lon in
* terms of the full image. Will later calculate position relative to
* subcube.
******************************************************************************/
        if (lev2_latlon_to_linesamp(&both.map,&data))
	  goto latlon_to_ls;
        C_DATUM1->xs0 = data.samp;
        C_DATUM1->yl0 = data.line;

/******************************************************************************
* Find azimuth to sun and spacecraft in Level 2.
******************************************************************************/
        /* Convert mapscale to units of resolution */
        res = both.map.proj.kmperpix * xlinci;
	sres = res;
        azres = res;
	tmpdata = data;
        lev2_calc_spacecraft_azimuth(&both.spi,&data,
	  &both.map,&tmpdata,azres,&scaz);
	tmpdata = data;
        lev2_calc_sun_azimuth(&both.spi,&data,
	  &both.map,&tmpdata,azres,&sunaz);

/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the map projected case:  Z axis is the local vertical,
* so emission angle is used to calculate emission vector rather than datum
* derivatives. Also calculate the scaling variable, cosemi, to be used 
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input (cosemi=1.0 for map-projected images).
******************************************************************************/
        C_AEPAR1->aspect = 1.0;
	C_PSNPAR->phase[0] = pha;
	clinc = inc;
	azinc = sunaz;
	clemi = emi;
	azemi = scaz;
	C_DATUM2->datumtyp = 2;
	cosemi = 1.0;

/******************************************************************************
* Obtain parameters needed for spherical datum
******************************************************************************/
        radii[0] = both.spi.target.radii[0];
	radii[1] = both.spi.target.radii[1];
	radii[2] = both.spi.target.radii[2];
        if (radii[1] == 0.0) radii[1] = radii[0];
        if (radii[2] == 0.0) radii[2] = radii[0];
        radius = sqrt((radii[0]*radii[0]+radii[1]*radii[1]+
	  radii[2]*radii[2])/3.0);
        C_DATUM1->r0 = radius/res;  /* Planet radius in pixels */

/******************************************************************************
* Call above gave planet center in full image cube, so convert to subcube
******************************************************************************/
        C_DATUM1->xs0=(C_DATUM1->xs0 - (FLOAT4) issi)/xsinci
	  + 1.0;
        C_DATUM1->yl0=(C_DATUM1->yl0 - (FLOAT4) isli)/xlinci
	  + 1.0;

/******************************************************************************
* Must calculate z00s for sphere, putting middle of image at height 0
******************************************************************************/
        C_DATUM1->z00s = -sqrt(C_DATUM1->r0*C_DATUM1->r0 - 
	  pow((xcenter-C_DATUM1->xs0)*C_AEPAR1->aspect,2.0) -
	  pow((ycenter-C_DATUM1->yl0),2.0));

/******************************************************************************
* Finally, calculate dip and azimuth of dip for sphere at reference
* point. Do this by calling datum to get the elevation differences across
* the pixel near the reference point (converted from full image cube to 
* subcube coordinates.
******************************************************************************/
        (void) geomset(1.0); /* Setup needed before datum */
	i = (INT4) ((x - (FLOAT4) issi)/
	  xsinci + .5) + 1;
	j = (INT4) ((y - (FLOAT4) isli)/
	  xlinci + .5) + 1;
        (void) datum(i,j,4,&z01,&z02,&z03,&z04);
	dzxm = 0.5*(-z01+z02+z03-z04);
	dzym = 0.5*(-z01-z02+z03+z04);
        dip = atan(sqrt((dzxm/C_AEPAR1->aspect)*(dzxm/C_AEPAR1->aspect)+
	  dzym*dzym))*L_RAD2DEG;
	az = atan2(dzym*C_AEPAR1->aspect*C_AEPAR1->aspect,dzxm)*L_RAD2DEG;

/******************************************************************************
* Non-orthographic projections can be used to rectify scanner images.
* Intent is to represent a small region only, so datum type is planar and
* in fact has zero dip, similar to handling of Level 1 scanner images. No
* error checking is done to see if the projection has severe skew or other
* distortions that would make the calculation nonsensical.
*
* User input or default reference point location is in Level 2 space, so
* convert to Level 1 space using lat,lon coordinates.
******************************************************************************/
      } else {
        data.samp = x;
        data.line = y;
        if (lev2_linesamp_to_latlon(&both.map,&data)) goto ls_to_latlon;
        if (lev2_latlon_to_linesamp(&both.map,&data)) goto latlon_to_ls;
	tmpdata = data;
	if (lev1_latlon_to_linesamp(&both.spi,&data)) 
	  goto latlon_to_ls;
        else if (lev1_linesamp_to_latlon(&both.spi,&data)) 
	  goto ls_to_latlon;

/******************************************************************************
* Return emission, incidence, phase as emi, inc, pha respectively. These
* angles are evaluated at a location relative to the whole image.
******************************************************************************/
        lev1_calc_emission_angle(&data,&emi);
        lev1_calc_incidence_angle(&data,&inc);
        lev1_calc_phase_angle(&data,&pha);

/******************************************************************************
* Get ground resolutions of map at the reference point.
******************************************************************************/
        if (lev2_calc_map_resolution(&both.spi,&data,
	    &both.map,&tmpdata,&sampres,&lineres)) 
	  goto res_error;

/******************************************************************************
* Now, find azimuth to sun and spacecraft in Level 2. 
******************************************************************************/
        res = lineres*xlinci;
	sres = sampres*xsinci;
        azres = res;
        lev2_calc_spacecraft_azimuth(&both.spi,&data,
	  &both.map,&tmpdata,azres,&scaz);
        lev2_calc_sun_azimuth(&both.spi,&data,
	  &both.map,&tmpdata,azres,&sunaz);

/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the map projected case:  Z axis is the local vertical,
* so emission angle is used to calculate emission vector rather than datum
* derivatives. Also calculate the scaling variable, cosemi, to be used 
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input (cosemi=1.0 for map-projected images).
******************************************************************************/
        C_AEPAR1->aspect = sampres/lineres;
	C_PSNPAR->phase[0] = pha;
	clinc = inc;
	azinc = sunaz;
	clemi = emi;
	azemi = scaz;
	dip = 0.0;
	az = 0.0;
	C_DATUM2->datumtyp = 1;
	cosemi = 1.0;
      }
    }

/******************************************************************************
* Only some of the needed levels Level 1 keyword groups found. Image is
* invalid.
******************************************************************************/
  } else {
    goto labels_not_complete;
  }

/******************************************************************************
* Now proceed to calculate stored values with label information. Still
* need to calculate derivatives of the plane datum.
******************************************************************************/
  dzdip = tan(dip*L_DEG2RAD)/sqrt(pow(cos(az*L_DEG2RAD)*C_AEPAR1->aspect,2.0)
    +pow(sin(az*L_DEG2RAD),2.0));
  C_DATUM1->dzx0 = -cos(az*L_DEG2RAD)*dzdip*C_AEPAR1->aspect*C_AEPAR1->aspect;
  C_DATUM1->dzy0 = -sin(az*L_DEG2RAD)*dzdip;
  C_DATUM1->z00 = -(C_DATUM1->dzx0*xcenter+C_DATUM1->dzy0*ycenter);
  C_DATUM1->dz10 = -(C_DATUM1->dzx0+C_DATUM1->dzy0);
  C_DATUM1->dz20 = -(C_DATUM1->dzx0-C_DATUM1->dzy0);
  (void) geomset(1.0);
  scale = 1000.0*res;

  C_PGEOM->ci[0] = cos(clinc*L_DEG2RAD)*SQ2;
  si0 = sin(clinc*L_DEG2RAD)/
    sqrt(pow(cos(azinc*L_DEG2RAD)*C_AEPAR1->aspect,2.0)
    +pow(sin(azinc*L_DEG2RAD),2.0));
  C_PGEOM->si1[0] = si0*cos((45.0-azinc)*L_DEG2RAD);
  C_PGEOM->si2[0] = si0*sin((45.0-azinc)*L_DEG2RAD);
  C_PGEOM->ce[0] = cos(clemi*L_DEG2RAD)*SQ2;
  se0 = sin(clemi*L_DEG2RAD)/
    sqrt(pow(cos(azemi*L_DEG2RAD)*C_AEPAR1->aspect,2.0)
    +pow(sin(azemi*L_DEG2RAD),2.0));
  C_PGEOM->se1[0] = se0*cos((45.0-azemi)*L_DEG2RAD);
  C_PGEOM->se2[0] = se0*sin((45.0-azemi)*L_DEG2RAD);

/******************************************************************************
* Calculations above are purely GEOMETRIC; those below are PHOTOMETRIC,
* i.e., depending on surface properties and description thereof.
******************************************************************************/
  if (C_PPARS3->hapke) {
    if (strncmp(C_PPARS4->phofunc,"HAPH_S",6) == 0) {
      C_PPARS2->falpha[0] = (1.0-C_PPARS5->phg2)*
	(1.0-C_PPARS5->phg1*C_PPARS5->phg1)/
        (pow(1.0+C_PPARS5->phg1*C_PPARS5->phg1+2.0*
	C_PPARS5->phg1*cos(C_PSNPAR->phase[0]*L_DEG2RAD),1.5))+
	C_PPARS5->phg2*(1.0-C_PPARS5->phg1*C_PPARS5->phg1)/
	(pow(1.0+C_PPARS5->phg1*C_PPARS5->phg1-2.0*C_PPARS5->phg1*
        cos(C_PSNPAR->phase[0]*L_DEG2RAD),1.5))-1.0;
    } else if (strncmp(C_PPARS4->phofunc,"HAPL_S",6) == 0) {
      C_PPARS2->falpha[0] = 1.0+C_PPARS5->pbh*cos(C_PSNPAR->phase[0]*
	L_DEG2RAD)+C_PPARS5->pch*(1.5*cos(C_PSNPAR->phase[0]*L_DEG2RAD)+
	0.5);
    }
    C_PPARS2->twogam = 2.0*sqrt(1.0-C_PPARS5->pwh);
    if (strncmp(C_PPARS4->phofunc,"HAPLEG",6) == 0 ||
        strncmp(C_PPARS4->phofunc,"HAPHEN",6) == 0) {
      mulpht = 62;
      if (strncmp(C_PPARS4->phofunc,"HAPHEN",6) == 0) 
	mulpht = mulpht + 23;
      if (C_PPARS5->phh != 0.0) mulpht = mulpht + 3;
      if (C_PPARS5->ptheta != 0.0) mulpht = mulpht + 127;
      C_PPARS1->mulps = (16+mulpht)*2;
      C_PPARS1->mulpsp = (42+mulpht*4)*2;
    } else {
      C_PPARS1->mulps = 15;
      C_PPARS1->mulpsp = 15;
    }
  } else {
    C_PPARS2->pex1 = C_PPARS2->pex-1.0;
    C_PPARS2->pl2 = C_PPARS2->pl2+C_PPARS2->pl2;
    C_PPARS2->pl1 = 1.0-.5*C_PPARS2->pl2;
    if (C_PPARS2->pl2 == 0.0) {
      if (C_PPARS2->pex == 1.0) {
        C_PPARS1->mulps = 0;
	C_PPARS1->mulpsp = 0;
      } else {
        C_PPARS1->mulps = 15;
        C_PPARS1->mulpsp = 4;
      }
    } else if (C_PPARS2->pl2 == 2.0) {
      C_PPARS1->mulps = 3;
      C_PPARS1->mulpsp = 4;
    } else {
      if (C_PPARS2->pex == 1.0) {
        C_PPARS1->mulps = 5;
	C_PPARS1->mulpsp = 7;
      } else {
        C_PPARS1->mulps = 21;
	C_PPARS1->mulpsp = 14;
      }
    }
  }

/******************************************************************************
* End of material-dependent code. Find photometric function
* normalization, etc. (AEPARs)
******************************************************************************/
  bclipin = 1.0e30;
  C_AEPAR1->bmax = 1.0e30;
  i = (INT4) (xcenter+.5);
  j = (INT4) (ycenter+.5);
  (void) pbder(i,j,0.0,0.0,&C_AEPAR1->bnorm,&db1,&db2,1);
  (void) findbmax();
  if (C_AEPAR1->bmax < C_AEPAR1->bnorm*bclipin) C_AEPAR1->bclip = C_AEPAR1->bmax;
  else C_AEPAR1->bclip = C_AEPAR1->bnorm*bclipin;
  C_AEPAR2->flop = FALSE;
  C_AEPAR2->logimg = FALSE;

/******************************************************************************
* Find azimuth of characteristic strips. This is calculated in terms of
* true Cartesian azimuth, whereas the other azimuths are those on the
* pixel grid, which may not have a unit aspect ratio. We will output the
* azimuth of characteristics transformed to a grid azimuth. Starting point
* for the search is the (numerator) sun azimuth, transformed to Cartesian.
******************************************************************************/
  azi = atan2(sin(azinc*L_DEG2RAD)/C_AEPAR1->aspect,cos(azinc*L_DEG2RAD))*
    L_RAD2DEG;
  if (azi >= 0) icount = (INT4) (azi/90.0+.5);
  else icount = (INT4) (azi/90.0-.5);
  icard = icount-4*(icount/4);
  cardaz = 90.0*((FLOAT4) icard);
  delaza = cardaz-azi-45.0;
  delazb = cardaz-azi+45.0;
  p = ddberr;
  C_AEPAR1->delaz = zbrent(p,delaza,delazb,1.0e-6);
  if (ins > inl) arg = ins;
  else arg = inl;
  if (fabs(sin(C_AEPAR1->delaz*L_DEG2RAD)*cos(C_AEPAR1->delaz*L_DEG2RAD))*
      arg <= 0.5) C_AEPAR1->delaz = 0.0;
  while (icard < 0) {
    icard = icard+4;
    cardaz = cardaz+360.;
  }
  C_AEPAR1->charaz = cardaz-C_AEPAR1->delaz;
  if (C_AEPAR1->dbrat*(0.5-(icard%2)) < 0.0) C_AEPAR1->charaz = 
                                             C_AEPAR1->charaz+90.;
  C_AEPAR1->charaz = C_AEPAR1->charaz-360.0*((INT4) (C_AEPAR1->charaz/360.0));
  if (C_AEPAR1->charaz > 180.) C_AEPAR1->charaz = C_AEPAR1->charaz-360.;
  if (C_AEPAR1->charaz >= 0.) arg = 180.;
  else arg = -180.;
  if (fabs(C_AEPAR1->charaz) > 90.) C_AEPAR1->charaz = C_AEPAR1->charaz-arg;

/******************************************************************************
* We use Cartesian charaz but print grid-relative charazgrid for the
* user. No longer need to use ioct in main program to control geometric
* transformation of images on input. Instead, set ioct1=1 and use it in
* calls to pblinein1, etc. By setting the octant of the characteristics
* ioct here and using it to control the SOR sweep direction, we get
* optimal results for any charaz. ioct is passed via common.
******************************************************************************/
  charazgrid = atan2(sin(C_AEPAR1->charaz*L_DEG2RAD)*C_AEPAR1->aspect,
               cos(C_AEPAR1->charaz*L_DEG2RAD))*L_RAD2DEG;
  if ((-90.0 <= C_AEPAR1->charaz) && (C_AEPAR1->charaz < -45.0)) 
    C_AEPAR3->ioct = -2;
  else if ((-45.0 <= C_AEPAR1->charaz) && (C_AEPAR1->charaz < 0.0)) 
    C_AEPAR3->ioct = -1;
  else if ((45.0 < C_AEPAR1->charaz) && (C_AEPAR1->charaz <= 90.0))
    C_AEPAR3->ioct = 2;
  else
    C_AEPAR3->ioct = 1;

  sprintf(C_MSG->pcpmsg,"X= %f",x);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Y= %f",y);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"RADIUS= %f",radius);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Dip of datum= %f",dip);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Azimuth of dip= %f",az);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Azimuth of characteristics= %f",charazgrid);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Resolution (km/pix) = %f",sres);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Aspect = %f",1.0/C_AEPAR1->aspect);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Emission angle = %f",emi);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Incidence angle = %f",inc);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Phase angle = %f",pha);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Spacecraft azimuth = %f",scaz);
  u_write_msg(3,C_MSG->pcpmsg);
  sprintf(C_MSG->pcpmsg,"Sun azimuth = %f",sunaz);
  u_write_msg(3,C_MSG->pcpmsg);

/******************************************************************************
* Finish getting TAE parameters
******************************************************************************/
  (void) pcsi_ipars(&nsmooth);

  (void) write2log(1,ins,inl,note,lnote,from,clinc,
                   azinc,clemi,azemi,sres,
		   dip,az,charazgrid,radius,dndatum,
		   1.0/C_AEPAR1->aspect);

  if (PcsiFirstGuess()) goto first_guess_error;

  (void) write2log(2,ins,inl,note,lnote,from,clinc,
                   azinc,clemi,azemi,sres,
		   dip,az,charazgrid,radius,dndatum,
		   1.0/C_AEPAR1->aspect);

  return(0);

/*****************************************************
  Error section
*****************************************************/
  user_parameter_error:
    sprintf(C_MSG->pcpmsg,"Error retrieving TAE parameter %s",prmname);
    u_error("INITPCSI-TAEERR",C_MSG->pcpmsg,-1,1);
    return(-1);

  spi_phosun_error:
    sprintf(C_MSG->pcpmsg,
      "Unable to get spacecraft and sun position information for %s",from);
    u_error("INITPCSI-SPIPHOSUN",C_MSG->pcpmsg,-2,1);
    return(-2);

  spi_findlin_error:
    sprintf(C_MSG->pcpmsg,"Unable to convert lat,lon to line,samp for %s",
            from);
    u_error("INITPCSI-SPIFINDLIN",C_MSG->pcpmsg,-3,1);
    return(-3);

  spi_lbplan_error:
    sprintf(C_MSG->pcpmsg,"Unable to read planet radii and lpos from %s",
            from);
    u_error("INITPCSI-SPILBP",C_MSG->pcpmsg,-4,1);
    return(-4);

  first_guess_error:
    sprintf(C_MSG->pcpmsg,"Unable to generate an initial solution for topography");
    u_error("INITPCSI-SOLNERR",C_MSG->pcpmsg,-5,1);
    return(-5);
  
  lev_init_error:
    sprintf(C_MSG->pcpmsg,"Unable to initialize input file %s",from);
    u_error("INITPCSI-LEVINIT",C_MSG->pcpmsg,-6,1);
    return(-6);

  ls_to_latlon:
    sprintf(C_MSG->pcpmsg,"Unable to convert line,sample to lat,lon for %s",
            from);
    u_error("INITPCSI-LSTOLATLON",C_MSG->pcpmsg,-7,1);
    return(-7);

  latlon_to_ls:
    sprintf(C_MSG->pcpmsg,"Unable to convert lat,lon to line,sample for %s",
            from);
    u_error("INITPCSI-LATLONTOLS",C_MSG->pcpmsg,-8,1);
    return(-8);

  res_error:
    sprintf(C_MSG->pcpmsg,
    "Unable to determine line,sample resolution for %s",from);
    u_error("INITPCSI-LSRES",C_MSG->pcpmsg,-9,1);
    return(-9);

  bad_subsamp:
    sprintf(C_MSG->pcpmsg,"Error getting sub-sample information for %s",
            from);
    u_error("INITPCSI-SUBSAMP",C_MSG->pcpmsg,-10,1);
    return(-10);

  bad_subline:
    sprintf(C_MSG->pcpmsg,"Error getting sub-line information for %s",
            from);
    u_error("INITPCSI-SUBLINE",C_MSG->pcpmsg,-11,1);
    return(-11);

  inc_error:
    sprintf(C_MSG->pcpmsg,"Line and sample increment of file %s are not equal",
            from);
    u_error("INITPCSI-LINCSINC",C_MSG->pcpmsg,-12,1);
    return(-12);

  mosaic_group_error:
    sprintf(C_MSG->pcpmsg,
    "Mosaicked images must have all of groups: ISIS_INSTRUMENT, ISIS_GEOMETRY, ISIS_TARGET");
    u_error("INITPCSI-MOSGRP",C_MSG->pcpmsg,-13,1);
    return(-13);

  level2_group_error:
    sprintf(C_MSG->pcpmsg,
    "Level 2 images must have all of groups: ISIS_INSTRUMENT, ISIS_GEOMETRY, ISIS_TARGET");
    u_error("INITPCSI-LEV2GRP",C_MSG->pcpmsg,-14,1);
    return(-14);

  unknown_inst:
    sprintf(C_MSG->pcpmsg,
    "Unknown spacecraft/instrument found in %s",from);
    u_error("INITPCSI-BILEV1",C_MSG->pcpmsg,-15,1);
    return(-15);

  labels_not_complete:
    sprintf(C_MSG->pcpmsg,
    "Necessary Level 1 keyword groups not found in %s",from);
    u_error("INITPCSI-INCLAB",C_MSG->pcpmsg,-16,1);
    return(-16);

  haphen_error:
    sprintf(C_MSG->pcpmsg,
    "The following values must be provided for function HAPHEN: wh,theta,b0,hh,hg1,hg2");
    u_error("INITPCSI-HAPHEN",C_MSG->pcpmsg,-17,1);
    return(-17);

  hapleg_error:
    sprintf(C_MSG->pcpmsg,
    "The following values must be provided for function HAPLEG: wh,theta,b0,hh,bh,ch");
    u_error("INITPCSI-HAPLEG",C_MSG->pcpmsg,-18,1);
    return(-18);

  haph_s_error:
    sprintf(C_MSG->pcpmsg,
    "The following values must be provided for function HAPH_S: wh,hg1,hg2");
    u_error("INITPCSI-HAPH_S",C_MSG->pcpmsg,-19,1);
    return(-19);

  hapl_s_error:
    sprintf(C_MSG->pcpmsg,
    "The following values must be provided for function HAPL_S: wh,bh,ch");
    u_error("INITPCSI-HAPL_S",C_MSG->pcpmsg,-20,1);
    return(-20);

  min_error:
    sprintf(C_MSG->pcpmsg,
    "The following values must be provided for function MIN: k");
    u_error("INITPCSI-MIN",C_MSG->pcpmsg,-21,1);
    return(-21);

  lunlam_error:
    sprintf(C_MSG->pcpmsg,
    "The following values must be provided for function LUNLAM: l");
    u_error("INITPCSI-LUNLAM",C_MSG->pcpmsg,-22,1);
    return(-22);

  zout_remove_error:
    sprintf(C_MSG->pcpmsg,
    "Unable to delete pre-existing ZOUT file");
    u_error("INITPCSI-ZOUTREM",C_MSG->pcpmsg,-23,1);
    return(-23);

  flspec_error:
    sprintf(C_MSG->pcpmsg,
    "The ZOUT filename is invalid");
    u_error("INITPCSI-FLSPEC",C_MSG->pcpmsg,-24,1);
    return(-24);

  to_file_error:
    sprintf(C_MSG->pcpmsg,
    "TO file was not specified by user");
    u_error("INITPCSI-TOERR",C_MSG->pcpmsg,-25,1);
    return(-25);

  to_file_exist:
    sprintf(C_MSG->pcpmsg,
    "TO file already exists");
    u_error("INITPCSI-TOEXIST",C_MSG->pcpmsg,-26,1);
    return(-26);

  to_file_invalid:
    sprintf(C_MSG->pcpmsg,
    "TO file name invalid");
    u_error("INITPCSI-TOINV",C_MSG->pcpmsg,-27,1);
    return(-27);

  cube_open_error:
    sprintf(C_MSG->pcpmsg,"Error opening input cube file %s",from);
    u_error("INITPCSI-OPENERR",C_MSG->pcpmsg,-28,1);
    return(-28);

  invalid_cube:
    sprintf(C_MSG->pcpmsg,"%s is not a standard cube file",from);
    u_error("INITPCSI-INVCUBE",C_MSG->pcpmsg,-29,1);
    return(-29); 

  bad_sys_keys:
    sprintf(C_MSG->pcpmsg,"Error obtaining system keywords from %s",from);
    u_error("INITPCSI-SYSKEYS",C_MSG->pcpmsg,-30,1);
    return(-30);

  parse_sfrom_error:
    sprintf(C_MSG->pcpmsg,"Unable to parse SFROM parameter %s for %s",sfrom,from);
    u_error("INITPCSI-PRSSFROM",C_MSG->pcpmsg,-31,1);
    return(-31); 

  sfrom_error:
    sprintf(C_MSG->pcpmsg,"Unable to apply SFROM %s to input file %s",sfrom,from);
    u_error("INITPCSI-SFROMERR",C_MSG->pcpmsg,-32,1);
    return(-32); 

  clat_error:
    sprintf(C_MSG->pcpmsg,"Unable to get clat from labels of %s",from);
    u_error("INITPCSI-CLAT",C_MSG->pcpmsg,-33,1);
    return(-33); 

  clon_error:
    sprintf(C_MSG->pcpmsg,"Unable to get clon from labels of %s",from);
    u_error("INITPCSI-CLON",C_MSG->pcpmsg,-34,1);
    return(-34); 
}
Beispiel #17
0
static void RenderGrid(const CViewPoint *view_point, int plane)
{
	switch(wxGetApp().grid_mode){
	case 1:
		{
			const HeeksColor& bg = wxGetApp().background_color[0];
			HeeksColor cc = bg.best_black_or_white();
			gp_Vec v_bg((double)bg.red, (double)bg.green, (double)bg.blue);
			gp_Vec v_cc((double)cc.red, (double)cc.green, (double)cc.blue);
			gp_Vec v_contrast = v_cc - v_bg;
			gp_Vec unit_contrast = v_contrast.Normalized();
			double l1, l2, l3;
			if(v_cc * gp_Vec(1,1,1)>0){
				l1 = 200;
				l2 = 130;
				l3 = 80;
			}
			else{
				l1 = 100;
				l2 = 30;
				l3 = 10;
			}
			if(l1>v_contrast.Magnitude())l1 = v_contrast.Magnitude();
			if(l2>v_contrast.Magnitude())l2 = v_contrast.Magnitude();
			if(l3>v_contrast.Magnitude())l3 = v_contrast.Magnitude();
			gp_Vec uf = (view_point->forwards_vector()).Normalized();
			gp_Vec vx, vy;
			view_point->GetTwoAxes(vx, vy, false, plane);
			gp_Pnt datum(0, 0, 0);
			gp_Trsf orimat = wxGetApp().GetDrawMatrix(false);
			datum = datum.Transformed(orimat);
			orimat = make_matrix(datum, vx, vy);
			gp_Vec v_up = gp_Vec(0,0, 1).Transformed(orimat);
			double fufz = fabs(uf * v_up);
			if(fufz<0.7){
				double there = (fufz - 0.3) / 0.4;
				l1 *= there;
				l2 *= there;
			}
			gp_Vec v_gc1 = v_bg + unit_contrast * l1;
			gp_Vec v_gc2 = v_bg + unit_contrast * l2;
			gp_Vec v_gc3 = v_bg + unit_contrast * l3;
			glColor3ub((unsigned char)(v_gc3.X()), (unsigned char)(v_gc3.Y()), (unsigned char)(v_gc3.Z()));
			RenderGrid(view_point, 200, false, true, NULL, NULL, 0, plane);
			glColor3ub((unsigned char)(v_gc2.X()), (unsigned char)(v_gc2.Y()), (unsigned char)(v_gc2.Z()));
			RenderGrid(view_point, 20, true, false, NULL, NULL, 0, plane);
			glColor3ub((unsigned char)(v_gc1.X()), (unsigned char)(v_gc1.Y()), (unsigned char)(v_gc1.Z()));
			RenderGrid(view_point, 20, false, false, NULL, NULL, 0, plane);
		}
		break;

	case 2:
	case 3:
		{
			const HeeksColor& bg = wxGetApp().background_color[0];
			HeeksColor cc = bg.best_black_or_white();
			bool light_color = cc.red + cc.green + cc.blue > 384;
			wxGetApp().EnableBlend();
			RenderGrid(view_point, 200, false, true, &bg, &cc, light_color ? 40:10, plane);
			RenderGrid(view_point, 20, true, false, &bg, &cc, light_color ? 80:20, plane);
			RenderGrid(view_point, 20, false, false, &bg, &cc, light_color ? 120:30, plane);
			wxGetApp().DisableBlend();
		}
		break;
	}
}
Beispiel #18
0
static void RenderGrid(const CViewPoint *view_point, double max_number_across, bool in_between_spaces, bool miss_main_lines, const HeeksColor *bg, const HeeksColor *cc, unsigned char brightness, int plane_mode){
	gp_Pnt sp[4];
	double zval = 0.5;
	wxSize size = wxGetApp().m_current_viewport->GetViewportSize();
	sp[0] = gp_Pnt(0, 0, zval);
	sp[1] = gp_Pnt(size.GetWidth(), 0, zval);
	sp[2] = gp_Pnt(size.GetWidth(), size.GetHeight(), zval);
	sp[3] = gp_Pnt(0, size.GetHeight(), zval);
	gp_Vec vx, vy;
	int plane_mode2 = view_point->GetTwoAxes(vx, vy, false, plane_mode);
	gp_Pnt datum(0, 0, 0);
	gp_Trsf orimat = wxGetApp().GetDrawMatrix(false);
	datum.Transform(orimat);
	orimat = make_matrix(datum, vx, vy);
	gp_Vec unit_forward = view_point->forwards_vector().Normalized();
	double plane_dp = fabs(gp_Vec(0, 0, 1).Transformed(orimat) * unit_forward);
	if(plane_dp < 0.3)return;
	gp_Pln plane(gp_Pnt(0, 0, 0).Transformed(orimat), gp_Vec(0, 0, 1).Transformed(orimat));
	{
		for(int i =0; i<4; i++){
			gp_Pnt p1 = view_point->glUnproject(sp[i]);
			sp[i].SetZ(0);
			gp_Pnt p2 = view_point->glUnproject(sp[i]);
			if(p1.Distance(p2) < 0.00000000001)return;
			gp_Lin line = make_line(p1, p2);
			gp_Pnt pnt;
			if(intersect(line, plane, pnt))
			{
				sp[i].SetX((gp_Vec(pnt.XYZ()) * vx) - (gp_Vec(datum.XYZ()) * vx));
				sp[i].SetY((gp_Vec(pnt.XYZ()) * vy) - (gp_Vec(datum.XYZ()) * vy));
				sp[i].SetZ(0);
			}
		}
	}
	CBox b;
	{
		for(int i = 0; i<4; i++){
			b.Insert(sp[i].X(), sp[i].Y(), sp[i].Z());
		}
	}
	double width = b.Width();
	double height = b.Height();
	double biggest_dimension;
	if(height > width)biggest_dimension = height;
	else biggest_dimension = width;
	double widest_spacing = biggest_dimension/max_number_across;
	bool dimmer = false;
	double dimness_ratio = 1.0;
	double spacing;

	if(wxGetApp().draw_to_grid){
		spacing = wxGetApp().digitizing_grid;
		if(!miss_main_lines)spacing *= 10;
		if(spacing<0.0000000001)return;
		if(biggest_dimension / spacing > max_number_across * 1.5)return;
		if(biggest_dimension / spacing > max_number_across){
			dimmer = true;
			dimness_ratio = (max_number_across * 1.5 - biggest_dimension / spacing)/ (max_number_across * 0.5);
		}

	}
	else{
		double l = log10(widest_spacing / wxGetApp().m_view_units);

		double intl = (int)l;
		if(l>0)intl++;

		spacing = pow(10.0, intl) * wxGetApp().m_view_units;
	}

	if(wxGetApp().grid_mode == 3){
		dimmer = true;
		dimness_ratio *= plane_dp;
		dimness_ratio *= plane_dp;
	}
	double ext2d[4];
	ext2d[0] = b.m_x[0];
	ext2d[1] = b.m_x[1];
	ext2d[2] = b.m_x[3];
	ext2d[3] = b.m_x[4];
	{
		for(int i = 0; i<4; i++){
			double intval = (int)(ext2d[i]/spacing);
			if(i <2 ){
				if(ext2d[i]<0)intval--;
			}
			else{
				if(ext2d[i]>0)intval++;
			}

			ext2d[i] = intval * spacing;
		}
	}
	if(cc){
		HeeksColor col = *cc;
		if(wxGetApp().grid_mode == 3){
			switch(plane_mode2){
			case 0:
				col.green = (unsigned char)(0.6 * (double)(bg->green));
				break;
			case 1:
				col.red = (unsigned char)(0.9 * (double)(bg->red));
				break;
			case 2:
				col.blue = bg->blue;
				break;
			}
		}

		if(dimmer){
			double d_brightness = (double)brightness;
			d_brightness *= dimness_ratio;
			unsigned char uc_brightness = (unsigned char)d_brightness;

			glColor4ub(col.red, col.green, col.blue, uc_brightness);
		}
		else{
			glColor4ub(col.red, col.green, col.blue, brightness);
		}
	}
	glBegin(GL_LINES);
	double extra = 0;
	if(in_between_spaces)extra = spacing * 0.5;
	for(double x = ext2d[0] - extra; x<ext2d[2] + extra; x += spacing){
		if(miss_main_lines){
			double xr = x/spacing/5;
			if( fabs(  xr - (double)(int)(xr+ (xr>0 ? 0.5:-0.5)) ) < 0.1)continue;
		}
		gp_Pnt temp(datum.XYZ() + (vx.XYZ() * x) + (vy.XYZ() * ext2d[1]));
		glVertex3d(temp.X(), temp.Y(), temp.Z());
		temp = (datum.XYZ() + (vx.XYZ() * x) + (vy.XYZ() * ext2d[3]));
		glVertex3d(temp.X(), temp.Y(), temp.Z());
	}
	for(double y = ext2d[1] - extra; y<ext2d[3] + extra; y += spacing){
		if(miss_main_lines){
			double yr = y/spacing/5;
			if( fabs(  yr - (double)(int)(yr+(yr>0 ? 0.5:-0.5)) ) < 0.1)continue;
		}
		gp_Pnt temp = (datum.XYZ() + (vx.XYZ() * ext2d[0]) + (vy.XYZ() * y));
		glVertex3d(temp.X(), temp.Y(), temp.Z());
		temp = (datum.XYZ() + (vx.XYZ() * ext2d[2]) + (vy.XYZ() * y));
		glVertex3d(temp.X(), temp.Y(), temp.Z());
	}
	glEnd();
}
Beispiel #19
0
void CogGaze::Apply(JointPos& joints)
{
  HeadState state;
  JointPos& datum = joints;
  state.SetEyes(datum(1), datum(2), datum(3));
  state.SetNeck(datum(4), datum(5), datum(7), datum(6));

  // gyro reported orientation applies to origin point p_origin
  // right between the eyes, fixed wrt head.
  
  double tilt_ticks_per_right_angle = 24371;
  double left_pan_ticks_per_right_angle = 42791;
  double right_pan_ticks_per_right_angle = 44238;
  double neck_lean_ticks_per_right_angle = 25657;
  double neck_pan_ticks_per_right_angle = 49549;
  double neck_tilt_ticks_per_right_angle = 36335;
  double neck_roll_ticks_per_right_angle = 38025;
  
  double rads_per_right_angle = M_PI/2;
  double tilt_ticks_per_rad = tilt_ticks_per_right_angle/rads_per_right_angle;
  double left_pan_ticks_per_rad = left_pan_ticks_per_right_angle/rads_per_right_angle;
  double right_pan_ticks_per_rad = right_pan_ticks_per_right_angle/rads_per_right_angle;
  double neck_lean_ticks_per_rad = neck_lean_ticks_per_right_angle/rads_per_right_angle;
  double neck_pan_ticks_per_rad = neck_pan_ticks_per_right_angle/rads_per_right_angle;
  double neck_roll_ticks_per_rad = neck_roll_ticks_per_right_angle/rads_per_right_angle;
  double neck_tilt_ticks_per_rad = neck_tilt_ticks_per_right_angle/rads_per_right_angle;
  
  double deg = 180.0/M_PI;
  
  double lean_ref = -10000;
  double tilt_ref = -14000;
  
#if 0
  printf("gyro %g %g %g || eye %g %g %g || neck %g %g %g %g || ",
	 state.yaw/M_PI*180, state.pitch/M_PI*180, state.roll/M_PI*180,
	 state.tilt, state.left_pan, state.right_pan,
	 state.neck_lean, state.neck_pan, state.neck_tilt,
	 state.neck_roll);
  //#else
  printf("gyro %.1f %.1f %.1f || eye %.1f %.1f %.1f || neck %.1f %.1f %.1f %.1f || ",
	 state.yaw/M_PI*180, state.pitch/M_PI*180, state.roll/M_PI*180,
	 state.tilt*deg/tilt_ticks_per_rad, 
	 state.left_pan*deg/left_pan_ticks_per_rad, 
	 state.right_pan*deg/right_pan_ticks_per_rad,
	 (state.neck_lean-lean_ref)*deg/neck_lean_ticks_per_rad, 
	 state.neck_pan*deg/neck_pan_ticks_per_rad, 
	 (state.neck_tilt-tilt_ref)*deg/neck_tilt_ticks_per_rad,
	 state.neck_roll*deg/neck_roll_ticks_per_rad);
#endif	  
  
  
  Matrix m(4,4);
  // using homogeneous matrices; don't actually need to
  // since we just care about rotations in this case
  
  // A rotation about the y axis, for panning (left)
  double left_pan_angle = state.left_pan / left_pan_ticks_per_rad;
  double c = cos(left_pan_angle);
  double s = sin(left_pan_angle);
  m[0][0] = c;  m[0][1] = 0;  m[0][2] = s;  m[0][3] = 0; 
  m[1][0] = 0;  m[1][1] = 1;  m[1][2] = 0;  m[1][3] = 0;
  m[2][0] =-s;  m[2][1] = 0;  m[2][2] = c;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_left_pan = m;
  
  // A rotation about the y axis, for panning (right)
  double right_pan_angle = state.right_pan/right_pan_ticks_per_rad;
  c = cos(right_pan_angle);
  s = sin(right_pan_angle);
  m[0][0] = c;  m[0][1] = 0;  m[0][2] = s;  m[0][3] = 0;
  m[1][0] = 0;  m[1][1] = 1;  m[1][2] = 0;  m[1][3] = 0;
  m[2][0] =-s;  m[2][1] = 0;  m[2][2] = c;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_right_pan = m;
  
  // A rotation about the x axis, for tilting
  double tilt_angle = state.tilt/tilt_ticks_per_rad;
  c = cos(tilt_angle);
  s = sin(tilt_angle);
  m[0][0] = 1;  m[0][1] = 0;  m[0][2] = 0;  m[0][3] = 0;
  m[1][0] = 0;  m[1][1] = c;  m[1][2] =-s;  m[1][3] = 0;
  m[2][0] = 0;  m[2][1] = s;  m[2][2] = c;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_tilt = m;
  
  // A rotation about the z axis, for neck_roll
  double roll_angle = -state.neck_roll/neck_roll_ticks_per_rad;
  c = cos(roll_angle);
  s = sin(roll_angle);
  m[0][0] = c;  m[0][1] =-s;  m[0][2] = 0;  m[0][3] = 0;
  m[1][0] = s;  m[1][1] = c;  m[1][2] = 0;  m[1][3] = 0;
  m[2][0] = 0;  m[2][1] = 0;  m[2][2] = 1;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_neck_roll = m;
  
  // A rotation about the x axis, for neck_tilt
  // This axis is the only one whose "resting" position is very 
  // far from neutral -- need to check with pasa where calibration
  // puts the zero point, assuming for now that it is orthogonal
  // to gravity vector
  double neck_tilt_angle_offset = M_PI/2;
  double neck_tilt_angle = -(state.neck_tilt-tilt_ref)/neck_tilt_ticks_per_rad;
  neck_tilt_angle += neck_tilt_angle_offset;
  c = cos(neck_tilt_angle);
  s = sin(neck_tilt_angle);
  m[0][0] = 1;  m[0][1] = 0;  m[0][2] = 0;  m[0][3] = 0;
  m[1][0] = 0;  m[1][1] = c;  m[1][2] =-s;  m[1][3] = 0;
  m[2][0] = 0;  m[2][1] = s;  m[2][2] = c;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_neck_tilt = m;
  
  
  /*
   // A rotation about the y axis, for neck_pan
   double neck_pan_angle = -state.neck_pan/neck_pan_ticks_per_rad;
   c = cos(neck_pan_angle);
   s = sin(neck_pan_angle);
   m[0][0] = c;  m[0][1] = 0;  m[0][2] = s;  m[0][3] = 0;
   m[1][0] = 0;  m[1][1] = 1;  m[1][2] = 0;  m[1][3] = 0;
   m[2][0] =-s;  m[2][1] = 0;  m[2][2] = c;  m[2][3] = 0;
   m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
   Matrix m_neck_pan = m;
   */
  
  // A rotation about the z axis, for neck_pan (actually a roll)
  double neck_pan_angle = -state.neck_pan/neck_pan_ticks_per_rad;
  c = cos(neck_pan_angle);
  s = sin(neck_pan_angle);
  m[0][0] = c;  m[0][1] =-s;  m[0][2] = 0;  m[0][3] = 0;
  m[1][0] = s;  m[1][1] = c;  m[1][2] = 0;  m[1][3] = 0;
  m[2][0] = 0;  m[2][1] = 0;  m[2][2] = 1;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_neck_pan = m;
  
  // A rotation about the x axis, for neck_lean
  // neck_lean zero point -10000
  double neck_lean_angle = (state.neck_lean-lean_ref)/neck_lean_ticks_per_rad;
  c = cos(neck_lean_angle);
  s = sin(neck_lean_angle);
  m[0][0] = 1;  m[0][1] = 0;  m[0][2] = 0;  m[0][3] = 0;
  m[1][0] = 0;  m[1][1] = c;  m[1][2] =-s;  m[1][3] = 0;
  m[2][0] = 0;  m[2][1] = s;  m[2][2] = c;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_neck_lean = m;
  
  /*
  // Rotation corresponding to yaw, pitch, and roll
  double y = state.yaw, p = state.pitch, r = state.roll;
  double cosp = cos(p), sinp = sin(p);
  double cosy = cos(y), siny = sin(y);
  double cosr = cos(r), sinr = sin(r);
  m[0][0] = cosp*cosy;  m[0][1] = sinr*sinp*cosy-cosr*siny; 
  m[0][2] = cosr*sinp*cosy+sinr*siny;  m[0][3] = 0;
  m[1][0] = cosp*siny;  m[1][1] = sinr*sinp*siny+cosr*cosy;  
  m[1][2] = cosr*sinp*siny-sinr*cosy;  m[1][3] = 0;
  m[2][0] = -sinp;  m[2][1] = cosp*sinr;  
  m[2][2] = cosp*cosr;  m[2][3] = 0;
  m[3][0] = 0;  m[3][1] = 0;  m[3][2] = 0;  m[3][3] = 1;
  Matrix m_euler = m;
  
  Matrix m_left = m_euler * m_tilt * m_left_pan;
  Matrix m_right = m_euler * m_tilt * m_right_pan;
   */
  
  // Consider a vector pointing out from camera
  Matrix m_common = m_neck_lean * m_neck_pan * m_neck_tilt * m_neck_roll * m_tilt;
  Matrix m_left_all = m_common * m_left_pan;
  Matrix m_right_all = m_common * m_right_pan;
  
  CogGaze& dir = *this;
  for (int i=0; i<3; i++)
    {
      dir.x_left[i] = m_left_all[i][0];
      dir.y_left[i] = m_left_all[i][1];
      dir.z_left[i] = m_left_all[i][2];
      dir.x_right[i] = m_right_all[i][0];
      dir.y_right[i] = m_right_all[i][1];
      dir.z_right[i] = m_right_all[i][2];
    }
  

  Digest(m_left_all,dir.theta_left,dir.phi_left,dir.roll_left);
  Digest(m_right_all,dir.theta_right,dir.phi_right,dir.roll_right);
	  
  printf("LEFT %+8.2f %+8.2f %+8.2f      RIGHT %+8.2f %+8.2f %+8.2f\n", 
	 dir.theta_left*180.0/M_PI, 
	 dir.phi_left*180.0/M_PI,
	 dir.roll_left*180.0/M_PI,
	 dir.theta_right*180.0/M_PI, 
	 dir.phi_right*180.0/M_PI,
	 dir.roll_right*180.0/M_PI);
}
void NewXZRotExperimentHCI::update(const std::vector<MinVR::EventRef> &events){

	//later to be pass into closestTouchPair function
	glm::dvec3 pos1;
	glm::dvec3 pos2;
	double minDistance = DBL_MAX; 
	MinVR::TimeStamp timestamp;

	//boolean flags
	bool xzTrans = false;
	glm::dmat4 xzTransMat = dmat4(0.0);
	bool yTrans = false;
	glm::dmat4 yTransMat = dmat4(0.0);
	bool yRotScale = false;

	numTouchForHand1 = 0;
	numTouchForHand2 = 0;

	 //only update the map and other variables first
	for(int p = 0; p < events.size(); p++) {

		
		timestamp = events[p]->getTimestamp();
		std::string name = events[p]->getName();
		int id = events[p]->getId();


		if (boost::algorithm::starts_with(name, "TUIO_Cursor_up")) {
			 //delete the cursor down associated with this up event
			std::map<int, TouchDataRef>::iterator it = registeredTouchData.find(id); 

			if (it != registeredTouchData.end()) { // if id is found
				registeredTouchData.erase(it);	   //erase value associate with that it
				//std::cout << "UP" <<std::endl;
			} else {
				std::cout<<"ERROR: Received touch up for a cursor not in the registered touch data!"<<std::endl;
				std::cout << events[p]->toString() << std::endl;
				for (auto iter = registeredTouchData.begin(); iter != registeredTouchData.end(); iter++) {
					std::cout << "\t" << iter->second->toString() << std::endl;
				}
			}

		} else if (boost::algorithm::starts_with(name, "TUIO_Cursor_down")) {
			// always add a new one on DOWN
			glm::dvec3 roomCoord = convertScreenToRoomCoordinates(events[p]->get2DData());
			TouchDataRef datum(new TouchData(events[p], roomCoord));
			registeredTouchData.insert(std::pair<int, TouchDataRef>(id, datum));
			//std::cout << "DOWN " << glm::to_string(events[p]->get2DData()) <<std::endl;


		} else if (boost::algorithm::starts_with(name, "TUIO_CursorMove")) {
			 //update the map with the move event
			// if the corresponding id was down, make it a move event
			std::map<int, TouchDataRef>::iterator it = registeredTouchData.find(id); 
			//std::cout << "Move " << events[p]->getId() <<std::endl;

			if (it != registeredTouchData.end()) { // if id is found
				glm::dvec2 screenCoord (events[p]->get4DData());
				glm::dvec3 roomCoord = convertScreenToRoomCoordinates(glm::dvec2(events[p]->get4DData()));

				// update map
				it->second->setCurrentEvent(events[p]);
				it->second->setCurrRoomPos(roomCoord);
			}

		}
		// end of TUIO events

		// Update hand positions
		if (name == "Hand_Tracker1") {
			//std::cout << "Inside hand tracking 1 event (xz) " << std::endl;
			//only enter one time to init prevHandPos1
			if (prevHandPos1.y == -1.0) {
				glm::dvec3 currHandPos1 (events[p]->getCoordinateFrameData()[3]);
				prevHandPos1 = currHandPos1;
				initRoomPos = true;
			} else {
				//std::cout << "updating hand 1 curr and prev position  " << std::endl;
				prevHandPos1 = currHandPos1;
				currHandPos1 = glm::dvec3(events[p]->getCoordinateFrameData()[3]);
			} 
		} 
		

		//std::cout<<"currHandPos1: "<<glm::to_string(currHandPos1)<<std::endl;
		//std::cout<<"prevHandPos1: "<<glm::to_string(prevHandPos1)<<std::endl;

		if(name == "Hand_Tracker2") {
			//std::cout << "Inside hand tracking 2 event (xz) " << std::endl;
			if (prevHandPos2.y == -1.0) {
				glm::dvec3 currHandPos2 (events[p]->getCoordinateFrameData()[3]);
				prevHandPos2 = currHandPos2;
			} else {
				//std::cout << "updating hand 2 curr and prev position  " << std::endl;
				prevHandPos2 = currHandPos2;
				currHandPos2 = glm::dvec3(events[p]->getCoordinateFrameData()[3]);
			} 
		}
	} // end of data-updating for loop
	
	// give feedback object the touch data so
	 //it can draw touch positions
	feedback->registeredTouchData = registeredTouchData;

	// At this point, the touch data should be updated, and hand positions
	std::map<int, TouchDataRef>::iterator iter;
	for (iter = registeredTouchData.begin(); iter != registeredTouchData.end(); iter++) {

		glm::dvec3 currRoomPos (iter->second->getCurrRoomPos());
		bool belongsToHand1 = (glm::length(currHandPos1 - currRoomPos) <  glm::length(currHandPos2 - currRoomPos));
		
		if (belongsToHand1) {
			numTouchForHand1++;
			if(iter->second->getBelongTo() == -1){
				iter->second->setBelongTo(1);
			}
		} 
		else { // belongs to hand 2
			numTouchForHand2++;
			if(iter->second->getBelongTo() == -1){
				iter->second->setBelongTo(2);
			}
		}
	} // end touch enumeration
	 //from TUIO move
	

	
	//std::cout << "Touch size: " << registeredTouchData.size() << std::endl;
	 //find closest pair of TouchPoints
	if (registeredTouchData.size() > 1) {
		closestTouchPair(registeredTouchData , pos1, pos2, minDistance);

		//std::cout << "Min distance: " << minDistance << std::endl;
		
	}

	
	

	if (minDistance < 0.06 && currHandPos1 != prevHandPos1) {
		
		xzRotFlag = true;
		//std::cout << "Inside XZRot Mode" << std::endl;

	}

	if (xzRotFlag && liftedFingers) { // might have to be xzRotFlag and not any other flag 

		feedback->displayText = "rotating";

		if(initRoomPos){
			// pos1 and pos2 are where you put your fingers down. We're finding the centroid
			 //to calculate the box boundary for xzRotMode
			initRoomTouchCentre = 0.5*(pos1 + pos2); 
			initRoomPos = false;

		}

		// try to change around center of origin
		glm::dvec3 centOfRot (glm::dvec3((glm::column(cFrameMgr->getVirtualToRoomSpaceFrame(), 3))));
		//calculate the current handToTouch vector
		
		if(registeredTouchData.size() > 1 && !centerRotMode) {
			roomTouchCentre = 0.5*(pos1 + pos2);
		}
		
		//std::cout << "Touch Center: " << glm::to_string(roomTouchCentre) << std::endl; 

		//Should not be equal in XZRotMode but just in case.
		 //for choosing the hand that rotates
		/*std::cout<<"currHandPos1: "<<glm::to_string(currHandPos1)<<std::endl;*/


		if (numTouchForHand1 >= numTouchForHand2) { 
			//std::cout << "Using right hand: " << std::endl; 
			if (roomTouchCentre - currHandPos1 != glm::dvec3(0.0)) {//zero guard
				currHandToTouch = roomTouchCentre - currHandPos1;
			}

			if (roomTouchCentre - prevHandPos1 != glm::dvec3(0.0)) {//zero guard
				prevHandToTouch = roomTouchCentre - prevHandPos1;
			}

		} else {
			//std::cout << "Using Left hand: " << std::endl;

			if (roomTouchCentre - currHandPos2 != glm::dvec3(0.0)) { //zero guard
				currHandToTouch = roomTouchCentre - currHandPos2;
			}

			if (roomTouchCentre - prevHandPos2 != glm::dvec3(0.0)) { //zero guard
				prevHandToTouch = roomTouchCentre - prevHandPos2;
			}
		}


		//set up the 2 vertices for a square boundry for the gesture
		glm::dvec3 upRight = glm::dvec3(initRoomTouchCentre.x+0.2, 0.0, initRoomTouchCentre.z+0.2);
		glm::dvec3 lowLeft = glm::dvec3(initRoomTouchCentre.x-0.2, 0.0, initRoomTouchCentre.z-0.2);


		 //this if-else block for setting xzRotFlag,
		 //also for grabbing xzCentOfRot
		if(registeredTouchData.size() == 0) { //if no touch on screen then automatically exit the xzrot mode
			xzRotFlag = false;
			initRoomPos = true;
			liftedFingers = true;
			feedback->displayText = "";
			feedback->centOfRot.x = DBL_MAX;
			//std::cout<<"no touchyyy so I quit"<<std::endl;
		}
		else { //if there are touch(s) then check if the touch is in bound of the rectangle

			bool setxzRotFlag = true;
			std::map<int, TouchDataRef>::iterator iter;
			int countFingers = 0;
			for (iter = registeredTouchData.begin(); iter != registeredTouchData.end(); iter++) {
				

				// not exactly sure why roomPos. > upRight.z , I think it should be <. but that doesn't work
				if(!(iter->second->getCurrRoomPos().x > upRight.x || iter->second->getCurrRoomPos().z > upRight.z ||iter->second->getCurrRoomPos().x < lowLeft.x ||iter->second->getCurrRoomPos().z < lowLeft.z)){ //you are in the box

					//std::cout << "fingers in bound so STILL IN XZRot Mode" << std::endl;
					setxzRotFlag = false; 
					countFingers += 1;

				} //else{ // touch point not in box, assume as center of rotation
				//	centOfRot = iter->second->getCurrRoomPos();
				//	centerRotMode = true;
				//	//std::cout << "Cent of Rot set" << std::endl;
				//	feedback->centOfRot = centOfRot;
				//}

				// only tries to change the xzRotFlag at the end of the data in the map
				if(iter == std::prev(registeredTouchData.end(),1) && setxzRotFlag) {
					xzRotFlag = false;
					initRoomPos = true;
					feedback->displayText = ""; 
					feedback->centOfRot.x = DBL_MAX;
					//std::cout << "all fingers went out of bound so Out of XZRot Mode" << std::endl;

					// found bug where person just drags their fingers across the table, and it reinitiates xzRotMode
					liftedFingers = false;
				}
			} // end for loop over registeredTouchData

			if(countFingers == registeredTouchData.size()){//all fingers in bound 
				centerRotMode = false;
				feedback->centOfRot.x = DBL_MAX;
			}

		} // end if/else block

		
		//std::cout<<"currHandToTouch: "<<glm::to_string(currHandToTouch)<<std::endl;
		//std::cout<<"prevHandToTouch: "<<glm::to_string(prevHandToTouch)<<std::endl;
		//std::cout<<"dot product of them: "<< glm::to_string(glm::dot(glm::normalize(currHandToTouch), glm::normalize(prevHandToTouch))) << std::endl;
		
			
			//std::cout<<"what we clamping: "<<glm::clamp(glm::dot(currHandToTouch, prevHandToTouch),-1.0,1.0)<<std::endl;

			double alpha = glm::acos(glm::clamp(glm::dot(glm::normalize(currHandToTouch), glm::normalize(prevHandToTouch)),-1.0,1.0)); // from 0 to pi
			//std::cout<<"alpha: " << alpha << std::endl;

			// get cross prod
			
			glm::dvec3 cross = glm::normalize(glm::cross(currHandToTouch, prevHandToTouch)); 
			//std::cout<<"cross: "<<glm::to_string(cross)<<std::endl;
			glm::dvec3 projCross = glm::normalize(glm::dvec3(cross.x, 0.0, cross.z)); // projection
			//std::cout<<"projcross: "<<glm::to_string(projCross)<<std::endl;

			// project cross prod onto the screen, get a length
			
			double lengthOfProjection = glm::dot(cross, projCross); 
			//std::cout<<"lengthOfProjection: "<<glm::to_string(lengthOfProjection)<<std::endl;
			// projected cross prod 
			//glm::dvec3 projectedCrossProd = lengthOfProjection * normProjCross; 

			//std::cout<<"lengthOfProjection: "<<lengthOfProjection<<std::endl;
			//std::cout<<"alpha in degree before times lengthofprojection: "<<glm::degrees(alpha)<<std::endl;

			alpha = alpha * lengthOfProjection;

			//std::cout<<"alpha in degree after: "<<glm::degrees(alpha)<<std::endl;
			//std::cout<<"normProjCross: "<<glm::to_string(normProjCross)<<std::endl;

			glm::dmat4 XZRotMat = glm::rotate(glm::dmat4(1.0), glm::degrees(alpha) /* * 2.0 */, glm::normalize(projCross));

			// have translations when we have a touch point not in the bounding box

			// translate to origin
			glm::dmat4 transMat(glm::translate(glm::dmat4(1.0), -1.0*centOfRot));
			 //translate back
			glm::dmat4 transBack(glm::translate(glm::dmat4(1.0), centOfRot));	

			 //put it into the matrix stack	
			//std::cout<<"XZRotMat: "<<glm::to_string(XZRotMat) <<std::endl;
			
			cFrameMgr->setRoomToVirtualSpaceFrame(cFrameMgr->getRoomToVirtualSpaceFrame() * transBack * XZRotMat * transMat);
		

	} // end xzRot Gesture

	
	

	



	if(registeredTouchData.size() == 0) {
		feedback->displayText = "";
		liftedFingers = true;
	}

	 //this is bret's commented out line
	//updateHandPos(events);
}
Beispiel #21
0
main(int argc, char *argv[]) {
	char     *file;                         /* data file name       */
	int       ok, help, chann, nchan;       /* cmd line options     */
	int       type;                         /* cmd line options     */
	int       showmap;                      /* more cmd line opts   */
	int       probe;                        /* daffie probe         */
	int       i, j, k;                      /* loop counters        */
	int       nitem;                        /* total data, items per channel */
	int       xs, ys, prevxs, prevys;       /* screen size          */
	int       ix, iy;                       /* screen coords        */
	int       x0;                           /* panned data origin   */
	int       pan;                          /* pan value            */
	int       key;                          /* mouse key on press   */
	int       imgxs, imgys, *img;           /* image data           */
	double    range, dscale;                /* data range/scale     */
	float     xscale, yscale, off;          /* data offset & scaling*/
	float     v;                            /* a datum              */
	int       select[MAXCHANN], nselect;    /* select channels      */
	char      title[300] = {'\0'};          /* title bar            */
	char     *cmapfile = NULL;              /* color map file name  */
	COLORMAP  cmap[MAX_CMAP];               /* color map from above */
	int       ncmap;                        /* # entriesin above    */
	int       cindx[256];                   /* allocated (X) color cells */
	int       crgb[256];                    /* corresponding rgb val */
	int       chindx[256];                  /* color index by channel*/
	int       plotx0, ploty0;               /* base of plot area     */

	Pgm = argv[0];  if (streq(Pgm, "app_main")) Pgm = "wplot";
	++argv; --argc;

	chann = 0;
	nchan = 0;
	help  = 0;
	ok    = 1;
	type  = 0;
	showmap = 0;
	Sample  = 1;
	probe   = 0;
	while (argc && *argv[0] == '-') {
		char *opt;

		opt = argv[0]+1;
		++argv; --argc;

		if (substr(opt, "help")) ++help;
		else if (substr(opt, "scatter"))     type |= T_SCATTER;
		else if (substr(opt, "line"))        type |= T_LINE;
		else if (substr(opt, "colormapped")) type |= T_COLORMAP;
		else if (substr(opt, "stacked"))     type |= T_STACKED;
		else if (substr(opt, "showmap"))     showmap = 1;
		else if (substr(opt, "probe"))       probe   = 1;
		else if (substr(opt, "ascii"))       Binary  = 0;
		else if (substr(opt, "binary"))      Binary  = 1;
		else if (substr(opt, "bin64"))       Binary  = BinDouble = 1;
		else if (substr(opt, "autopan"))     Autopan = 1;
		else if (substr(opt, "channel")) {
			char *s;
			int           x;

			if (!argc || !isdigit(*argv[0])) {
				fprintf(stderr,
					"%s: missing argument to -%s\n",
					Pgm, opt);
				ok = 0;
				continue;
			}

			s = argv[0];
			++argv; --argc;

			nchan = chann = 0;
			while (*s && isdigit(*s))
			   nchan = 10*nchan + *s++ - '0';
			if (*s) ++s;
			while (*s && isdigit(*s))
			  chann = 10*chann + *s++ - '0';

			if (!nchan) {
				fprintf(stderr,
					"%s: invalid argument to -%s\n",
					Pgm, opt);
				ok = 0;
			}
		}

		else if (substr(opt, "dimensions")) {
			int   i = 0;
			int   x;
			char *s;

			if (!argc || !isdigit(*argv[0])) {
				fprintf(stderr,
					"%s: missing argument to -%s\n",
					Pgm, opt);
				ok = 0;
				continue;
			}
			s = argv[0];

			i = 0;
			--s;
			do {
				x = 0; ++s;
				while (isdigit(*s)) x = 10*x + *s++ -'0';
				Dim[i++] = x;
			} while (*s == '.' || *s == 'x');

			++argv; --argc;
		}

		else if (substr(opt, "pan")) {
			if (!argc || !isdigit(*argv[0])) {
				fprintf(stderr,
					"%s: missing argument to -%s\n",
					Pgm, opt);
				ok = 0;
				continue;
			}

			Pan = 0.01 * atoi(argv[0]);
			++argv; --argc;
		}

		else if (substr(opt, "sample")) {
			if (!argc || !isdigit(*argv[0])) {
				fprintf(stderr,
					"%s: missing argument to -%s\n",
					Pgm, opt);
				ok = 0;
				continue;
			}

			Sample = atoi(argv[0]);
			++argv; --argc;
		}

		else if (substr(opt, "title")) {
			if (!argc || !isdigit(*argv[0])) {
				fprintf(stderr,
					"%s: missing argument to -%s\n",
					Pgm, opt);
				ok = 0;
				continue;
			}

			strcpy(title, argv[0]);
			++argv; --argc;
		}
		else if (substr(opt, "range")) {
			if (argc < 2) {
				fprintf(stderr,
					"%s: missing argument to -%s\n",
					Pgm, opt);
				ok = 0;
				continue;
			}
			ok &= sscanf(argv[0], " %f", &RangeMin);
			ok &= sscanf(argv[1], " %f", &RangeMax);
			if (!ok) {
				fprintf(stderr,
					"%s: invalid arguments to -%s\n",
					Pgm, opt);
			}
			Autorange = Rerange = 0;
			argv += 2;
			argc -= 2;
		}
		else if (substr(opt, "map")) {
			if (!argc) {
				fprintf(stderr,
					"%s: missing argument to -%s\n",
					Pgm, opt);
				ok = 0;
			}
			else {
				cmapfile = argv[0];
				++argv; --argc;
			}
		}
		else if (substr(opt, "double")) Double = 1;
		else if (streq(opt, "db"))      Double = 1;
		else {
			 fprintf(stderr, "%s: ignoring argument -%s\n", Pgm, opt);
			 ok = 0;
		}
	}

	if (!ok || help || !argc) {
		usage(Pgm, help);
		exit(!help);
	}

	/* See if -dim was given if not -chan */
	if (!nchan && Dim[1]) nchan = Dim[1];
	if (nchan) Nchan = nchan;
	else       Nchan = 1;

	file = argv[0];
	++argv; --argc;

	/* Allocate storage for the data */
	Data = (float *)malloc(MAXDATA * sizeof(float));
	if (!Data) {
		fprintf(stderr, "%s: not enough memory for %d datum\n", Pgm, MAXDATA);
		exit(1);
	}

	/*
	 * Read in data file
	 */
	if (substr("daffie://", file) || substr("//", file)) probe = 1;

	if (!probe) readfile(file);
	else        setprobe(file);

	if (Autorange) {
		RangeMin = DataMin;
		RangeMax = DataMax;
	}
	range = RangeMax - RangeMin;
	dscale = 1.0/range;

	printf("%s: read %d items (min = %f, max = %f)\n",
		Pgm, Ndata, DataMin, DataMax);

	if (Nchan > 1) printf("%s: display channel %d of %d\n", Pgm, chann, Nchan);

	PlotType = type;

	if (!title[0]) sprintf(title, "%s %s", Pgm, file);
	xw_title(title);

	/* Initialize display */
	xw_size(1000, 512);
	xw_init();
	for (i = 0; i < 8; ++i) xw_setcol(i, (i&1)?255:0, (i&2)?255:0, (i&4)?255:0);

	/* Initializing RGB byte ordering table */
	make_rgb_ordering();

	xw_color(7);

#ifdef XWINDOWS
	/* Create back buffer */
	if (Double) {
		int xs, ys;

		xw_qsize(&xs, &ys);
		BackBuf  = xw_unused();
		FrontBuf = xw_select(BackBuf);
		xw_pixmap(1);
		xw_size(xs, ys);
		xw_init();
		for (i = 0; i < 8; ++i)
		    xw_setcol(i, (i&1)?255:0, (i&2)?255:0, (i&4)?255:0);

		xw_select(FrontBuf);
	}
#endif

	/* Create color map */
	ncmap = make_colormap(cmapfile, cmap, MAX_CMAP);

	for (i = 0; i < 128; ++i) {
		int k, ci, rgb;

		j  = ncmap-1 - ((float)i)/127.0 * (ncmap-1);
		ci = xw_alloccol(cmap[j].r, cmap[j].g, cmap[j].b);
	     /****
		printf("Allocated color cell %d for index %d (%d %d %d)\n",
			k, i<<1, cmap[j].r, cmap[j].g, cmap[j].b);
	     ****/
#ifdef XWINDOWS
		if (Double) {
			xw_select(BackBuf);
			xw_setcol(ci, cmap[j].r, cmap[j].g, cmap[j].b);
			xw_select(FrontBuf);
		}
#endif
		k = i<<1;

		cindx[k]   = ci;
		cindx[k+1] = ci;

	     /*****
		rgb = (cmap[j].r<<16) | (cmap[j].g<<8) | cmap[j].b;
	      *****/
		rgb = make_rgb(cmap[j].r, cmap[j].g, cmap[j].b);

		crgb[k]   = rgb;
		crgb[k+1] = rgb;
	}

	if (Nchan) for (i = 0; i < Nchan; ++i) {
		j = ((float) i)/(Nchan-1) * 255.0;
	     /****
		printf("Setting channel %d to cmap entry %d, color cell %d\n", i, j, cindx[j]);
	     ****/
		chindx[i] = cindx[j];
	}

	if (Nchan && !chann) for (i = 0; i < Nchan; ++i) select[i] = 1;
	else for (i = 0; i < Nchan; ++i) select[i] = (i == chann-1);

	if (showmap) {
		int xs, ys, box0, box1;
		float bs;

		xw_qsize(&xs, &ys);
		bs = (float)xs / (float)Nchan;

		for (i = 0; i < Nchan; ++i) {
			printf("Channel %d - color index %d\n", i, chindx[i]);
			xw_color(chindx[i]);
			box0 = i*bs;
			box1 = (i+1)*bs - 1;
			xw_move(box0,  0);
			xw_flbox(box1, ys-1);
		}
		printf("Click in window to continue...\n");
		xw_xyin(&ix, &iy);
	}

	/* Initialize user interface */
	startui();

	x0 = 0;

	img = NULL;
	prevxs = prevys = 0;
	while (!Done) {
		char  text[100];
		int   border = 4;
		int   th, tw, ta, td;   /* text metrics */

		updateui();

		/* Verify/allocate an in-core image buffer for the display */
		xw_qsize(&xs, &ys);
		if (!img || imgxs != xs || imgys != ys) {
			if (img) free(img);
			img = (int *)malloc(xs*ys*sizeof(int));
			if (!img) {
				fprintf(stderr,
					"Not enough memory for %dx%d image buffer\n",
					xs, ys);
				exit(4);
			}
			imgxs = xs;
			imgys = ys;
			memset(img, 0, xs*ys*sizeof(int));
		}

		/* Redraw if window size changed */
		if (xs != prevxs || ys != prevys) {
			prevxs = xs;
			prevys = ys;
			Redraw = 1;
		}

		/*
		 * Set effective number of items to plot given number of
		 * channels and sampling size
		 */
		nitem  = Ndata/(Nchan*Sample);

		/* Do any desired panning */
		pan  = Pan * xs;  /* pan amount */

		if (PanLeft)       x0 = max(x0-pan, 0);
		else if (PanRight) x0 = min(x0+pan, max(0,nitem-xs));
		if (!KeepTrucking) PanLeft = PanRight = 0;
		else {
			unsigned long long now, elapsed;

			now = event_clock();
			elapsed = now-Lastdraw;
			if (elapsed < MINREDRAW)
			   event_sleep(NORMREDRAW-elapsed);
		}

		if (!Redraw && !KeepTrucking) {
			event_sleep(20);
			continue;
		}
		Redraw = 0;

#ifdef XWINDOWS
		/* Use double buffer on X Windows displays */
		if (Double) xw_select(BackBuf);
#endif

		/* Compute data range if necessary */
		if (Autorange && Rerange) {
			if (DataMin < RangeMin) {
				float delta = RangeMin - DataMin;

				while (RangeInc < delta) {
					if (9.0*RangeInc >= delta)
					  while (RangeInc < delta) RangeInc += RangeInc;
					else RangeInc *= 10.0;
				}
				RangeMin -= RangeInc;
				printf("Set Range min to %f\n", RangeMin);
			}
			if (DataMax > RangeMax) {
				float delta = DataMax - RangeMax;

				while (RangeInc < delta) {
					if (9.0*RangeInc >= delta)
					  while (RangeInc < delta) RangeInc += RangeInc;
					else RangeInc *= 10.0;
				}

				RangeMax += RangeInc;
				printf("Set Range max to %f\n", RangeMax);
			}
			Rerange = 0;
		}
		range  = RangeMax-RangeMin;
		if (range > EPSILON) dscale = 1.0/range;
		else                 dscale = 1.0;
		off    = -RangeMin;

		yscale = ys*dscale;

		/*
		 * Redraw the display
		 */
		xw_erase();
		xw_color(7);
		xw_move(0, 0);

		plotx0 = 0;
		ploty0 = 1;

		nselect = 0;
		for (k = 0; k < Nchan; ++k) if (select[k]) ++nselect;
		if (!nselect) {
			printf("nothing selected!!!");
			break;
		}

		if (PlotType & T_STACKED) yscale /= Nchan;

		if (PlotType & T_COLORMAP) {
			if (!img) for (i = 0; i < min(xs, nitem); ++i) {
				float yband;
				int    iy;

				 yband = (float)(ys - ploty0) / (float)Nchan;

				ix = plotx0 + i;
				xw_move(ix, ploty0);

				for (k = 0; k < Nchan; ++k) {
					int ci;

				     /******
					v  = Data[Nchan*(x0 + i*Sample) + k];
				      *****/
					v = datum(Data, x0+i, k, Sample);

					ci = (v+off)*dscale * 255.0 + 0.5;
					xw_color(cindx[ci]);

					iy = (k+1) * yband;

					xw_draw(ix, iy);
				}
			}
			else {
			  for (i = 0; i < min(xs, nitem/Sample); ++i) {
				float yband;
				int   iy0, iy1;
				int  *pix;

				yband = (float)(ys - ploty0) / (float)Nchan;

				ix  = plotx0 + i;
				iy0 = ploty0;

				pix = img + imgxs * (imgys-1) + ix;

				for (k = 0; k < Nchan; ++k) {
					int ci, rgbcol;

				     /*******
					v  = Data[Nchan*(x0 + i*Sample) + k];
				      *******/
					v = datum(Data, x0+i, k, Sample);

					ci = (v+off)*dscale * 255.0 + 0.5;
					rgbcol = crgb[ci];
					iy1 = (k+1) * yband;

					while (iy0++ <= iy1) {
						*pix = rgbcol;
						pix -= xs;
					}
					--iy0;
				}
			  }
			  /* Fill remainder of image with black */
			  for (ix = i; ix < imgxs; ++ix)
			    for (iy = ploty0; iy < imgys; ++iy)
				img[imgxs*iy + ix] = 0;

			  /* Send above created image to the display */
			  xw_showimage(img, i, imgys, 0, imgys-1, 0);
			}
		}
		else for (k = 0; k < Nchan; ++k) {
			if (!select[k]) continue;

			if (nselect > 1) {
			     /***
				printf("plotting chan %d color %d\n", k, chindx[k]);
			      ***/
				xw_color(chindx[k]);
			}
			else xw_color(7);

			if (PlotType & T_STACKED) ploty0 = k*(float)ys/(float)Nchan;

			for (i = 0; i < min(xs, nitem/Sample); i += Sample) {
				v  = Data[Nchan*(x0 + i*Sample) + k];
				ix = plotx0 + i;
				iy = ploty0 + yscale*(v+off);

				if (PlotType & T_SCATTER || !i) xw_move(ix, iy);

				xw_draw(ix, iy);
			}
		}

		if (!(PlotType & (T_STACKED|T_COLORMAP))) {
			iy = yscale*off;
			xw_color(1);
			xw_move(plotx0,  1+iy);
			xw_draw(xs,      1+iy);
		}
		xw_color(7);
		sprintf(text, "%d", Domain_X0/Nchan + x0);
		xw_qtext(text, &tw, &th, &ta, &td);

		xw_move(border, border);
		xw_text(text);
		xw_move(border, ys - border - th);
		xw_text(text);

		sprintf(text, "%d", min(nitem, x0+xs*Sample-1));
		xw_qtext(text, &tw, &th, &ta, &td);

		xw_move(xs-tw-border, border);
		xw_text(text);
		xw_move(xs-tw-border, ys - border - th);
		xw_text(text);

		xw_flush();

#ifdef XWINDOWS
		if (Double) {
			int xs, ys;

			xw_qsize(&xs, &ys);
			xw_select(FrontBuf);
			xw_copyfrom(BackBuf, 0, 0, xs, ys, 0, 0);
			xw_flush();
		}
#endif
		Lastdraw = event_clock();


	}
#ifdef XWINDOWS
	xw_erase();
	xw_flush();
	sleep(1);
	xw_fini();
#endif
}
Beispiel #22
0
void threads::localization::update()
{
  
  bool new_datum_available = false;
  ///// BEGIN LOCKED SECTION
  {
    // pop Datum from queue
    std::lock_guard<std::mutex> lock(queue_mutex);
    //printf("is queue empty? size = %d\n", data_queue.size());
    if (!data_queue.empty())
    {
      new_datum_available = true;
      current_datum = data_queue.top();
      data_queue.pop();
      //printf("popped datum from queue, size = %d\n", data_queue.size());      
    }
    else 
    {
      //printf("no datum available\n");
    }
  }
  ///// END LOCKED SECTION

  //if there is a datum to process, continue, else return
  if (!new_datum_available) return;
  
  //printf("Processing a datum of type: %s", current_datum.type_string().c_str());
  //std::cout << " value = " << current_datum.value() << std::endl;
  
  // prediction step
  // calculate dt using current time and datum time stamp    
  double dt = utility::time_tools::dt(t, current_datum.timestamp());
  if (dt < 0)
  {
    printf("WARNING: localization timestep is negative, skipping sensor update\n");
    return;
  }
  
  predict(dt);
  t = current_datum.timestamp();
  
  // convert value std::vector into a column matrix
  std::vector<double> value = current_datum.value();
  if (current_datum.type() == SENSOR_TYPE::GPS)
  {
    containers.heartbeat_gps = 1;

    value.at(0) -= home_x; // use local frame
    value.at(1) -= home_y;
    
    // save gps information for use in gps velocity calculation
    tR = utility::time_tools::dt(t0, t);
    //printf("t = %f seconds\n", tR);
    gps_data_t.push_back(utility::time_tools::dt(t0, current_datum.timestamp()));
    gps_data_x.push_back(value.at(0));
    gps_data_y.push_back(value.at(1));
    // eliminate old gps data
    for (int i = gps_data_t.size()-1; i > -1; i--)
    {
      if (tR - gps_data_t.at(i) > GPS_HISTORY_TIME_WINDOW)
      {
        //printf("gps datum age = %f seconds, deleting it...\n", tR - gps_data_t.at(i));
        gps_data_t.erase(gps_data_t.begin() + i);
        gps_data_x.erase(gps_data_x.begin() + i);
        gps_data_y.erase(gps_data_y.begin() + i);
      }
    }
    // if there are enough gps data remaining, calculate dx/dt and dy/dt
    if (gps_data_t.size() >= GPS_HISTORY_REQUIRED_SIZE)
    {
      //printf("There are at least %d gps data available, calculating velocities...\n", GPS_HISTORY_REQUIRED_SIZE);
      double n, s_t, s_x, s_y, s_tx, s_ty, s_tt, dx_dt, dy_dt;
      n = (double)gps_data_t.size();
      s_t = std::accumulate(gps_data_t.begin(), gps_data_t.end(), 0.0);
      s_x = std::accumulate(gps_data_x.begin(), gps_data_x.end(), 0.0);
      s_y = std::accumulate(gps_data_y.begin(), gps_data_y.end(), 0.0);
      s_tt = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_t.begin(), 0.0);
      s_tx = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_x.begin(), 0.0);        
      s_ty = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_y.begin(), 0.0);
      dx_dt = (n*s_tx - s_t*s_x)/(n*s_tt - s_t*s_t);
      dy_dt = (n*s_ty - s_t*s_y)/(n*s_tt - s_t*s_t);
      //printf("dx_dt = %f m/s,  dy_dt = %f m/s\n", dx_dt, dy_dt);        
      std::vector<double> velocities = {dx_dt, dy_dt};
      Eigen::MatrixXd covariance(2, 2);
      covariance = Eigen::MatrixXd::Identity(2, 2); 
      Datum datum(SENSOR_TYPE::GPS_VELOCITY, SENSOR_CATEGORY::LOCALIZATION, velocities, covariance);       
      new_sensor_update(datum); // put this gps_velocity datum into the queue
    }      
  }
  
  Eigen::Map<Eigen::MatrixXd> z_(value.data(), value.size(), 1);
  z = z_;
  R = current_datum.covariance();
  setH();
  
  K = P*H.transpose()*(H*P*H.transpose() + R).inverse();
  
  dz = z - H*state;
  if (current_datum.type() == SENSOR_TYPE::COMPASS)
  {
    dz(0, 0) = utility::angle_tools::minimum_difference(dz(0, 0)); // use true angular difference, not algebraic difference
  }
          
  S = H*P*H.transpose();
  
  if (S.determinant() < pow(10.0, -12.0))
  {
    printf("WARNING: innovation covariance is singular for sensor type: %s\n", current_datum.type_string().c_str());
    std::cout << "z = " << z << std::endl;
    std::cout << "H = " << H << std::endl;
    std::cout << "R = " << R << std::endl;
    std::cout << "P = " << P << std::endl;
    std::cout << "K = " << K << std::endl;
    std::cout << "S = " << S << std::endl;
    std::cout << "det(S) = " << S.determinant() << " vs. " << pow(10.0, -6.0) << std::endl;
    return;
  }
  
  double d = sqrt((dz.transpose()*S.inverse()*dz)(0, 0));
  //printf("Mahalonobis distance = %f\n", d);
  
  state += K*dz;
  P = (StateSizedSquareMatrix::Identity() - K*H)*P;        
  //std::cout << "Updated state = " << state.transpose() << std::endl;
  
  updateKB();
}