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));
}
avro::GenericDatum CommonEnum::toAvro() const
{
avro::GenericDatum datum(getSchema());
avro::GenericEnum &enum_ = datum.value<avro::GenericEnum>();
enum_.set(symbol_);
return datum;
}
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));
}
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();
}
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;
}
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();
}
//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;
}
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);
}
// 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;
}
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";
}
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
}
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);
}
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;
}
}
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();
}
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);
}
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
}
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();
}