//{{{ site_failure void site_failure(int sig) { if (_verbose) fprintf (stderr, "/* FAILURE OF A SITE SPOTTED */\n"); int i=0; int sortie = 0; msg_t msg; time_t timeStart, timeCur; pthread_mutex_lock(&mut_check); if (check) check = 0; pthread_mutex_unlock(&mut_check); int flags = fcntl(this_site.sdRecv, F_GETFL); int flags2 = flags | O_NONBLOCK; fcntl(this_site.sdRecv, F_SETFL, flags2); // Sends ARE_YOU_ALIVE to other pred if (_verbose) fprintf (stdout, "Checking predecessor validity\n"); for (i=1; i<TOLERANCE+1; i++) { if (!strcmp ("0.0.0.0", inet_ntoa(predec[i].sin_addr))) { if (_verbose) fprintf (stdout, "Bad ip address\n"); continue; } type(msg) = ARE_YOU_ALIVE; char *tmp = inet_ntoa(predec[i].sin_addr); strncpy (ip(msg), tmp, IPLONG *sizeof(char)); unsigned long int ipa = (unsigned long int) inet_addr(ip(msg)); sendMessageWithAdd(msg); pthread_mutex_lock(&mut_check); if (check) check = 0; pthread_mutex_unlock(&mut_check); timeStart = time(&timeStart); timeCur = time(&timeCur); while(timeCur - timeStart < (2*TMESG)) { timeCur = time(&timeCur); memset (&msg, 0, SIZE); if(recvMessage(&msg, NULL) == -1) continue; // Receive I_AM_ALIVE --> Connection if (_verbose) fprintf (stdout, "Received I_AM_ALIVE\n"); if (type(msg) == I_AM_ALIVE && !isMe(ips(msg))) { handleIAmAlive(msg); type(msg) = CONNECTION; strncpy(ip(msg), ips(msg), IPLONG * sizeof(char)); sendMessageWithAdd(msg); if (_verbose) fprintf (stdout, "Asking for connection\n"); last = getNeighbour(ipa); if (_verbose) fprintf (stdout, "New last after receiving I_AM_ALIVE message : %d\n", last); state = IDLE; critSectionRequest(); sortie++; break; } else if (type(msg) == REQUEST) {} else handleMessage(msg); } } // No pred is alive, broadcast SEARCH_PREV fcntl(this_site.sdRecv, F_SETFL, flags); if (_verbose) fprintf (stdout, "No alive predecessor found, looking for other sites\n"); if (!sortie) { msg_t min; type(msg) = SEARCH_PREV; pos(msg) = position; if (_verbose) fprintf (stdout, "Broadcasting SEARCH_PREV.\n"); broadcast(msg); fcntl(this_site.sdRecv, F_SETFL, flags2); timeStart = time(&timeStart); timeCur = time(&timeCur); // Wait for ACK_SEARCH_PREV while(timeCur - timeStart < (2*TMESG)) { timeCur = time(&timeCur); memset (&msg, 0, SIZE); if(recvMessage(&msg, NULL) == -1) continue; pos(min) = -1; if (type(msg) == ACK_SEARCH_PREV) { if (pos(min) == -1) memcpy (&min, &msg, SIZE); else if (pos(min) < pos(msg)) memcpy (&min, &msg, SIZE); if (_verbose) fprintf (stdout, "Got an answer.\n"); } else if (type(msg) == REQUEST) {} else handleMessage(msg); } fcntl(this_site.sdRecv, F_SETFL, flags); // No answers, regenerate the TOKEN if (pos(min) == -1) { if (_verbose) fprintf (stdout, "No answers, regenerating TOKEN\n"); tokenPresent = 1; last = next; takeCriticalSection(); sortie ++; } // Got an answer, ask for connection if (!sortie) { if (_verbose) fprintf (stdout, "Asking for connection\n"); type(msg) = CONNECTION; strncpy(ip(msg), ips(min), IPLONG * sizeof(char)); last = getNeighbour((unsigned long int)inet_addr(ips(min))); sendMessageWithAdd(msg); state = IDLE; critSectionRequest(); } } }
int main(int argc, char ** argv) { MPI_Init(&argc, &argv); QUESO::EnvOptionsValues envOptions; envOptions.m_numSubEnvironments = 1; envOptions.m_subDisplayFileName = "test_outputNoInputFile/display"; envOptions.m_subDisplayAllowAll = 1; envOptions.m_displayVerbosity = 2; envOptions.m_syncVerbosity = 0; envOptions.m_seed = 0; QUESO::FullEnvironment env(MPI_COMM_WORLD, "", "", &envOptions); unsigned int dim = 2; QUESO::VectorSpace<> paramSpace(env, "param_", dim, NULL); QUESO::GslVector paramMins(paramSpace.zeroVector()); QUESO::GslVector paramMaxs(paramSpace.zeroVector()); double min_val = -10.0; double max_val = 10.0; paramMins.cwSet(min_val); paramMaxs.cwSet(max_val); QUESO::BoxSubset<> paramDomain("param_", paramSpace, paramMins, paramMaxs); QUESO::UniformVectorRV<> priorRv("prior_", paramDomain); Likelihood<> lhood("llhd_", paramDomain); QUESO::GenericVectorRV<> postRv("post_", paramSpace); QUESO::SipOptionsValues sipOptions; sipOptions.m_computeSolution = 1; sipOptions.m_dataOutputFileName = "test_outputNoInputFile/sipOutput"; sipOptions.m_dataOutputAllowedSet.clear(); sipOptions.m_dataOutputAllowedSet.insert(0); QUESO::StatisticalInverseProblem<> ip("", &sipOptions, priorRv, lhood, postRv); QUESO::GslVector paramInitials(paramSpace.zeroVector()); paramInitials[0] = 0.0; paramInitials[1] = 0.0; QUESO::GslMatrix proposalCovMatrix(paramSpace.zeroVector()); proposalCovMatrix(0, 0) = 1.0; proposalCovMatrix(0, 1) = 0.0; proposalCovMatrix(1, 0) = 0.0; proposalCovMatrix(1, 1) = 1.0; QUESO::MhOptionsValues mhOptions; mhOptions.m_dataOutputFileName = "test_outputNoInputFile/sipOutput"; mhOptions.m_dataOutputAllowAll = 1; mhOptions.m_rawChainGenerateExtra = 0; mhOptions.m_rawChainDisplayPeriod = 50000; mhOptions.m_rawChainMeasureRunTimes = 1; mhOptions.m_rawChainDataOutputFileName = "test_outputNoInputFile/ip_raw_chain"; mhOptions.m_rawChainDataOutputAllowAll = 1; mhOptions.m_displayCandidates = 0; mhOptions.m_tkUseLocalHessian = 0; mhOptions.m_tkUseNewtonComponent = 1; mhOptions.m_filteredChainGenerate = 0; mhOptions.m_rawChainSize = 1000; mhOptions.m_putOutOfBoundsInChain = false; mhOptions.m_drMaxNumExtraStages = 1; mhOptions.m_drScalesForExtraStages.resize(1); mhOptions.m_drScalesForExtraStages[0] = 5.0; mhOptions.m_amInitialNonAdaptInterval = 100; mhOptions.m_amAdaptInterval = 100; mhOptions.m_amEta = (double) 2.4 * 2.4 / dim; // From Gelman 95 mhOptions.m_amEpsilon = 1.e-8; mhOptions.m_doLogitTransform = false; ip.solveWithBayesMetropolisHastings(&mhOptions, paramInitials, &proposalCovMatrix); MPI_Finalize(); return 0; }
void ip(struct pktinfo *pktinfo) { register const struct ip *ipv4 = NULL; register int ipv4_p = 0; /* next header */ const struct in_addr *ipv4_src = NULL; const struct in_addr *ipv4_dst = NULL; struct addr_4 addr_ipv4; struct udphdr *udphdr = NULL; struct tcphdr *tcphdr = NULL; struct pktinfo n_hdr; u_int32_t src = 0; u_int32_t dst = 0; char *addr = NULL; const u_char *pkt = pktinfo->pkt; bpf_u_int32 len = pktinfo->len; ipv4 = (const struct ip *)pkt; /* next header type code */ ipv4_p = ipv4->ip_p; /* IPv4 address */ ipv4_src = &ipv4->ip_src; ipv4_dst = &ipv4->ip_dst; src = htonl(ipv4_src->s_addr); dst = htonl(ipv4_dst->s_addr); addr_ipv4.src = src; addr_ipv4.dst = dst; len -= sizeof(struct ip); pkt += sizeof(struct ip); n_hdr.len = len; n_hdr.pkt = pkt; addr_ipv4.p_tcp = NULL; addr_ipv4.p_udp = NULL; addr_ipv4.nxtflag = 0; if (ipv4_p == IPPROTO_TCP) { tcphdr = (struct tcphdr *)pkt; addr_ipv4.p_tcp = tcphdr; addr_ipv4.nxtflag = 1; } else if (ipv4_p == IPPROTO_UDP) { udphdr = (struct udphdr *)pkt; addr_ipv4.p_udp = udphdr; addr_ipv4.nxtflag = 2; } addr = v4_addr(&addr_ipv4, encap); encap ++; switch(ipv4_p) { case IPPROTO_IPV4 : /* IP header */ ip(&n_hdr); break; case IPPROTO_TCP : /* tcp */ if (rflag != 1) tcp(&n_hdr); break; case IPPROTO_UDP : /* user datagram protocol */ if (bflag != 1) udp(&n_hdr); break; case IPPROTO_IPV6 : /* IP6 header */ ipv6(&n_hdr); break; default: } /* end of switch */ } /* end of ip() */
// if this function changes, please also check SupportsReShading, PreShade and PostShade // end - ke/mjm - 03.16.00 - merge reshading code // [attilas|29.5.2000] if this function changes, please also check EvalColorStdChannel void Matte::Shade(ShadeContext& sc) { Color c,t, shadowClr; float atten; float reflA; // > 6/15/02 - 11:12am --MQM-- // for renderer prepass, we need to at least call // illuminate so that Light Tracer can cache shading if ( SHADECONTEXT_IS_PREPASS( sc ) ) { Color lightCol; Point3 L; float NL = 0.0f, diffCoef = 0.0f; LightDesc *l = NULL; for ( int i = 0; i < sc.nLights; i++ ) { l = sc.Light( i ); if ( NULL != l ) l->Illuminate( sc, sc.Normal(), lightCol, L, NL, diffCoef ); } return; } #ifdef _DEBUG IPoint2 sp = sc.ScreenCoord(); if ( sp.x == stopX && sp.y == stopY ) sp.x = stopX; #endif if (gbufID) sc.SetGBufferID(gbufID); IllumParams ip( 1, &shadowIllumOutStr); IllumParams ipNS(0, NULL); ip.ClearInputs(); ip.ClearOutputs(); ipNS.ClearInputs(); ipNS.ClearOutputs(); ip.hasComponents = ipNS.hasComponents = HAS_MATTE_MTL; // get background color & transparency if (!opaque) sc.Execute(0x1000); // DS: 6/24/99:use black bg when AA filtering (#192348) sc.GetBGColor(c, t, fogBG&&(!fogObjDepth) ); if (!opaque) sc.Execute(0x1001); // DS: 6/24/99:use black bg when AA filtering (#192348) if (shadowBG && sc.shadow) { /******** sc.shadow = 0; Color col0 = sc.DiffuseIllum(); sc.shadow = 1; Color scol = sc.DiffuseIllum(); float f = Intens(col0); atten = (f>0.0f)?Intens(scol)/f:1.0f; if (atten>1.0f) atten = 1.0f/atten; ********/ atten = IllumShadow( sc, shadowClr ); atten = amblev + (1.0f-amblev) * atten; // key on black user-set shadow clr if( gUseLocalShadowClr || col.r != 0.0f || col.g != 0.0f || col.b != 0.0f ) shadowClr = col; ipNS.finalC = ipNS.diffIllumOut = c; c *= atten; ip.diffIllumOut = c; shadowClr *= 1.0f - atten; ip.finalC = sc.out.c = c + shadowClr; ip.SetUserIllumOutput( 0, shadowClr ); if (shadowAlpha) t *= atten; } else { sc.out.c = ipNS.finalC = ipNS.diffIllumOut = ip.finalC = ip.diffIllumOut = c; } // add the reflections if (reflmap && useReflMap) { AColor rcol; if (reflmap->HandleOwnViewPerturb()) { sc.TossCache(reflmap); rcol = reflmap->EvalColor(sc); } else rcol = sc.EvalEnvironMap(reflmap, sc.ReflectVector()); Color rc; rc = Color(rcol.r,rcol.g,rcol.b)*reflAmt; ip.reflIllumOut = ipNS.reflIllumOut = rc; if( additiveReflection ) { // additive compositing of reflections sc.out.c += rc; ip.finalC += rc; ipNS.finalC += rc; } else { reflA = Intens( rc ); // over compositing of reflections sc.out.c = (1.0f - reflA) * sc.out.c + rc; ip.finalC = (1.0f - reflA) * ip.finalC + rc; ipNS.finalC = (1.0f - reflA) * ipNS.finalC + rc; } } // render elements Clamp( t ); Clamp( reflA ); ip.finalT = ipNS.finalT = sc.out.t = opaque ? black: additiveReflection? t : Color(reflA,reflA,reflA) ; int nEles = sc.NRenderElements(); if( nEles != 0 ){ ip.pShader = ipNS.pShader = NULL; // no shader on matte mtl ip.stdIDToChannel = ipNS.stdIDToChannel = NULL; ip.pMtl = ipNS.pMtl = this; ip.finalAttenuation = ipNS.finalAttenuation = 1.0f; for( int i=0; i < nEles; ++i ){ IRenderElement* pEle = sc.GetRenderElement(i); if( pEle->IsEnabled() ){ MaxRenderElement* pMaxEle = (MaxRenderElement*)pEle->GetInterface( MaxRenderElement::IID ); if( pEle->ShadowsApplied() ) pMaxEle->PostIllum( sc, ip ); else pMaxEle->PostIllum( sc, ipNS ); } } } }
// ---------------------------------------------------------------------------- // 6to4FlowInfo::ApplyL // Final stage where the 6to4 touches the outgoing packet. Space is made for // the IP header and the IP header is initialized. // ---------------------------------------------------------------------------- TInt C6to4FlowInfo::ApplyL (RMBufSendPacket & aPacket, RMBufSendInfo & aInfo) { // Apply the tunnelling // Prepend the fixed content (if any) below the IP header if (iOffset > 0) { RMBufChain work; // Note: assumed that if CopyL leaves, then work is empty iPacket.CopyL(work); aPacket.Prepend(work); aInfo.iLength += iOffset; } switch(iOuterIp.Version()) { case 6: { aPacket.PrependL(sizeof(TInet6HeaderIP)); aInfo.iLength += sizeof(TInet6HeaderIP); aInfo.iProtocol = KProtocolInet6Ip; TInet6Packet<TInet6HeaderIP> ip(aPacket); if (ip.iHdr == NULL) User::Leave(KErrGeneral); // Build outer IPv6 header (already in iOuterIp, just copy) *ip.iHdr = iOuterIp; // Note: Certain packets must not have ECN ECT bits set. tcp_sap.cpp sets // KIpNoEcnEct for those packets. if (aInfo.iFlags & KIpNoEcnEct) { ip.iHdr->SetTrafficClass(ip.iHdr->TrafficClass() & 0xfc); } ip.iHdr->SetPayloadLength(aInfo.iLength); break; } case 4: { // Make space for IPv4 header aPacket.PrependL (20); aInfo.iLength += 20; aInfo.iProtocol = KProtocolInetIp; TInet6Checksum < TInet6HeaderIP4 > ip (aPacket); if (ip.iHdr == NULL) User::Leave (KErrGeneral); // Build the outer IPv4 header ip.iHdr->Init(iOuterIp.TrafficClass() & ((aInfo.iFlags & KIpNoEcnEct) ? 0xfc : 0xff)); // Raw Address load. Someone else must have already checked that // both src and dst are IPv4 mapped addresses at the end of the // ReadyL phase! This just does a blind copy. ip.iHdr->DstAddrRef() = iOuterIp.DstAddr().u.iAddr32[3]; ip.iHdr->SrcAddrRef() = iOuterIp.SrcAddr().u.iAddr32[3]; // Somewhat ad hoc thing: if DontFragment flag is set, then // set the DF bit to the IPv4 header... if (aInfo.iFlags & KIpDontFragment) ip.iHdr->SetFlags((TUint8)(ip.iHdr->Flags() | KInet4IP_DF)); ip.iHdr->SetHeaderLength (20); ip.iHdr->SetTotalLength (aInfo.iLength); ip.iHdr->SetTtl(iOuterIp.HopLimit()); // NOTE!!! Something might be done for this. Now it is theoretically // possible that a same host receives protocol 41 packets from us and // we have two or more flows with protocol 41 AND two packets get // fragmented and are reassembled in the same time by the remote host // AND the IPv4 identifier happens to be the same. That would break the // reassembly process, but since this is practically "impossible", it is // left now without a fix. ip.iHdr->SetIdentification (iPacketID++); ip.iHdr->SetProtocol (iOuterIp.NextHeader()); // Checksum of the outer header. ip.ComputeChecksum (); break; } default: User::Leave(KErrGeneral); } // Pass the packet return 0; }
int LuSolve( size_t n , size_t m , const FloatVector &A , const FloatVector &B , FloatVector &X , Float &logdet ) { // check numeric type specifications CheckNumericType<Float>(); // check simple vector class specifications CheckSimpleVector<Float, FloatVector>(); size_t p; // index of pivot element (diagonal of L) int signdet; // sign of the determinant Float pivot; // pivot element // the value zero const Float zero(0); // pivot row and column order in the matrix std::vector<size_t> ip(n); std::vector<size_t> jp(n); // ------------------------------------------------------- CPPAD_ASSERT_KNOWN( size_t(A.size()) == n * n, "Error in LuSolve: A must have size equal to n * n" ); CPPAD_ASSERT_KNOWN( size_t(B.size()) == n * m, "Error in LuSolve: B must have size equal to n * m" ); CPPAD_ASSERT_KNOWN( size_t(X.size()) == n * m, "Error in LuSolve: X must have size equal to n * m" ); // ------------------------------------------------------- // copy A so that it does not change FloatVector Lu(A); // copy B so that it does not change X = B; // Lu factor the matrix A signdet = LuFactor(ip, jp, Lu); // compute the log of the determinant logdet = Float(0); for(p = 0; p < n; p++) { // pivot using the max absolute element pivot = Lu[ ip[p] * n + jp[p] ]; // check for determinant equal to zero if( pivot == zero ) { // abort the mission logdet = Float(0); return 0; } // update the determinant if( LeqZero ( pivot ) ) { logdet += log( - pivot ); signdet = - signdet; } else logdet += log( pivot ); } // solve the linear equations LuInvert(ip, jp, Lu, X); // return the sign factor for the determinant return signdet; }
int CallFrame::line(STATE) { if(!compiled_code) return -2; // trampoline context return compiled_code->line(state, ip()); }
void compute(const QUESO::FullEnvironment& env, unsigned int numModes) { //////////////////////////////////////////////////////// // Step 1 of 5: Instantiate the parameter space //////////////////////////////////////////////////////// #ifdef APPLS_MODAL_USES_CONCATENATION QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> paramSpaceA(env, "paramA_", 2, NULL); QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> paramSpaceB(env, "paramB_", 1, NULL); #endif QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> paramSpace(env, "param_", 3, NULL); //////////////////////////////////////////////////////// // Step 2 of 5: Instantiate the parameter domain //////////////////////////////////////////////////////// #ifdef APPLS_MODAL_USES_CONCATENATION QUESO::GslVector paramMinsA(paramSpaceA.zeroVector()); paramMinsA[0] = 0.; paramMinsA[1] = 0.; QUESO::GslVector paramMaxsA(paramSpaceA.zeroVector()); paramMaxsA[0] = 3.; paramMaxsA[1] = 3.; QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix> paramDomainA("paramA_",paramSpaceA,paramMinsA,paramMaxsA); QUESO::GslVector paramMinsB(paramSpaceB.zeroVector()); paramMinsB[0] = 0.; QUESO::GslVector paramMaxsB(paramSpaceB.zeroVector()); paramMaxsB[0] = INFINITY; QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix> paramDomainB("paramB_",paramSpaceB,paramMinsB,paramMaxsB); QUESO::ConcatenationSubset<QUESO::GslVector,QUESO::GslMatrix> paramDomain("",paramSpace,paramDomainA,paramDomainB); #else QUESO::GslVector paramMins(paramSpace.zeroVector()); paramMins[0] = 0.; paramMins[1] = 0.; paramMins[2] = 0.; QUESO::GslVector paramMaxs(paramSpace.zeroVector()); paramMaxs[0] = 3.; paramMaxs[1] = 3.; paramMaxs[2] = .3; QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix> paramDomain("param_",paramSpace,paramMins,paramMaxs); #endif //////////////////////////////////////////////////////// // Step 3 of 5: Instantiate the likelihood function object //////////////////////////////////////////////////////// likelihoodRoutine_DataType likelihoodRoutine_Data; likelihoodRoutine_Data.numModes = numModes; QUESO::GenericScalarFunction<QUESO::GslVector,QUESO::GslMatrix> likelihoodFunctionObj("like_", paramDomain, likelihoodRoutine, (void *) &likelihoodRoutine_Data, true); // routine computes [-2.*ln(function)] //////////////////////////////////////////////////////// // Step 4 of 5: Instantiate the inverse problem //////////////////////////////////////////////////////// #ifdef APPLS_MODAL_USES_CONCATENATION QUESO::UniformVectorRV<QUESO::GslVector,QUESO::GslMatrix> priorRvA("priorA_", paramDomainA); QUESO::GslVector alpha(paramSpaceB.zeroVector()); alpha[0] = 3.; QUESO::GslVector beta(paramSpaceB.zeroVector()); if (numModes == 1) { beta[0] = 0.09709133373799; } else { beta[0] = 0.08335837191688; } QUESO::InverseGammaVectorRV<QUESO::GslVector,QUESO::GslMatrix> priorRvB("priorB_", paramDomainB,alpha,beta); QUESO::ConcatenatedVectorRV<QUESO::GslVector,QUESO::GslMatrix> priorRv("prior_", priorRvA, priorRvB, paramDomain); #else QUESO::UniformVectorRV<QUESO::GslVector,QUESO::GslMatrix> priorRv("prior_", paramDomain); #endif QUESO::GenericVectorRV<QUESO::GslVector,QUESO::GslMatrix> postRv("post_", paramSpace); QUESO::StatisticalInverseProblem<QUESO::GslVector,QUESO::GslMatrix> ip("", NULL, priorRv, likelihoodFunctionObj, postRv); //////////////////////////////////////////////////////// // Step 5 of 5: Solve the inverse problem //////////////////////////////////////////////////////// ip.solveWithBayesMLSampling(); //////////////////////////////////////////////////////// // Print some statistics //////////////////////////////////////////////////////// unsigned int numPosTotal = postRv.realizer().subPeriod(); if (env.subDisplayFile()) { *env.subDisplayFile() << "numPosTotal = " << numPosTotal << std::endl; } QUESO::GslVector auxVec(paramSpace.zeroVector()); unsigned int numPosTheta1SmallerThan1dot5 = 0; for (unsigned int i = 0; i < numPosTotal; ++i) { postRv.realizer().realization(auxVec); if (auxVec[0] < 1.5) numPosTheta1SmallerThan1dot5++; } if (env.subDisplayFile()) { *env.subDisplayFile() << "numPosTheta1SmallerThan1dot5 = " << numPosTheta1SmallerThan1dot5 << ", ratio = " << ((double) numPosTheta1SmallerThan1dot5)/((double) numPosTotal) << std::endl; } return; }
void Elastic::FAC::pack_strain(double* buffer, const SAMRAI::hier::Patch& patch, const SAMRAI::hier::Box& region, const int &depth) const { boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_ptr= boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch.getPatchData(v_id)); SAMRAI::pdat::SideData<double>& v = *v_ptr; boost::shared_ptr<SAMRAI::pdat::CellData<double> > dv_diagonal; boost::shared_ptr<SAMRAI::pdat::SideData<double> > dv_mixed; if(!faults.empty()) { dv_diagonal=boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> > (patch.getPatchData(dv_diagonal_id)); dv_mixed=boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch.getPatchData(dv_mixed_id)); } const Gamra::Dir dim=d_dim.getValue(); if(have_embedded_boundary()) { boost::shared_ptr<SAMRAI::pdat::SideData<double> > level_set_ptr; if(have_embedded_boundary()) level_set_ptr=boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> > (patch.getPatchData(level_set_id)); } Gamra::Dir ix(Gamra::Dir::from_int(depth/dim)), iy(Gamra::Dir::from_int(depth%dim)); const SAMRAI::hier::Index zero(SAMRAI::hier::Index::getZeroIndex(d_dim)); SAMRAI::hier::Index ip(zero), jp(zero); ip[ix]=1; jp[iy]=1; const double *dx(boost::dynamic_pointer_cast <SAMRAI::geom::CartesianPatchGeometry> (patch.getPatchGeometry())->getDx()); SAMRAI::pdat::CellIterator iend(SAMRAI::pdat::CellGeometry::end(region)); for(SAMRAI::pdat::CellIterator icell(SAMRAI::pdat::CellGeometry::begin(region)); icell!=iend; ++icell) { const SAMRAI::pdat::SideIndex s(*icell,ix,SAMRAI::pdat::SideIndex::Lower); if(ix==iy) { double diff(v(s+ip)-v(s)); if(!faults.empty()) diff-=(*dv_diagonal)(*icell,ix); *buffer=diff/dx[ix]; } else { const int ix_iy(index_map(ix,iy,dim)); double diff(- v(s-jp) - v(s+ip-jp) + v(s+jp) + v(s+ip+jp)); if(!faults.empty()) diff+=(*dv_mixed)(s,ix_iy+1) - (*dv_mixed)(s-jp,ix_iy) + (*dv_mixed)(s+ip,ix_iy+1) - (*dv_mixed)(s+ip-jp,ix_iy) + (*dv_mixed)(s+jp,ix_iy+1) - (*dv_mixed)(s,ix_iy) + (*dv_mixed)(s+ip+jp,ix_iy+1) - (*dv_mixed)(s+ip,ix_iy); *buffer=diff/(4*dx[iy]); } ++buffer; } }
const char * ipv6(const char *interface) { return ip(interface, AF_INET6); }
void NodeConnection::Listener() { try { while(this->Socket != SOCKET_ERROR) { Packet packet; packet.getCommand(this->Socket); //printf("%s\n", packet.command); //this printf are not really thread save, and they are only needed for debug if (!strcmp(packet.command, "pong")) //just a keep alive { packet.sendData(this->Socket); //resending it }else if(!strcmp(packet.command,"version")) { packet_version version(packet); this->Version = version.version; }else if(!strcmp(packet.command,"verack")) { packet_verack verack(packet); verack.sendData(this->Socket); //empty addr for now packet_addr addr; addr.addr_list.clear(); addr.encodeData(); addr.setChecksum_Lenght_Magic(); addr.sendData(this->Socket); //empty inv for now, this should be the whole list of object hash bool finished = false; vector<sTag> inventory; std::shared_lock<std::shared_timed_mutex> mlock(bitmrc->sharedObj.mutex_); for (int i = 0; i < bitmrc->sharedObj.Dim; i++) { hash_table<ustring>::linked_node * cur = bitmrc->sharedObj.Table[i]; while (cur != NULL) { sTag hash; memcpy(hash.ch, cur->hash.c_str(), 32); inventory.push_back(hash); cur = cur->next; } } mlock.unlock(); unsigned int i = 0; while (!finished) { packet_inv inv; while (i < 50000 && i < inventory.size()) { inv.inventory.push_back(inventory[i]); i++; } inv.encodeData(); inv.setChecksum_Lenght_Magic(); inv.sendData(this->Socket); if (i == inventory.size()) finished = true; } this->state = 2; }else if(!strcmp(packet.command,"addr")) { packet_addr addr(packet); for(unsigned int i=0;i<addr.addr_list.size();i++) { if(addr.addr_list[i].IPv6_4[11] == 0xff && addr.addr_list[i].IPv6_4[10] == 0xff) { char buf[40]; sprintf(buf,"%i.%i.%i.%i", addr.addr_list[i].IPv6_4[12], addr.addr_list[i].IPv6_4[13], addr.addr_list[i].IPv6_4[14], addr.addr_list[i].IPv6_4[15]); string ip(buf); //printf("%s\n",buf); sprintf(buf, "%i", addr.addr_list[i].port); string port(buf); NodeConnection *tmp_node= new NodeConnection(ip,port, this->bitmrc); this->bitmrc->connectNode(tmp_node); } } }else if(!strcmp(packet.command,"inv")) { packet_inv inv(packet); packet_getdata needed; for (unsigned int i = 0; i < inv.inventory.size(); i++) { ustring tag; for (int j = 0; j < 32; j++) { tag += inv.inventory[i].ch[j]; } if (this->bitmrc->sharedObj.searchByHash(tag).empty()) { needed.inventory.push_back(inv.inventory[i]); } } if (!needed.inventory.empty()) { needed.encodeData(); needed.setChecksum_Lenght_Magic(); Packets.push(needed); } }else if(!strcmp(packet.command,"getdata")) { packet_getdata getdata(packet); for (unsigned int i = 0; i < getdata.inventory.size(); i++) { ustring tag; for (int j = 0; j < 32; j++) { tag += getdata.inventory[i].ch[j]; } ustring ObjPayload = this->bitmrc->sharedObj.searchByHash(tag); if (!ObjPayload.empty()) { object obj; memset(obj.command, 0x00, sizeof obj.command); strncpy(obj.command, "object", 7); obj.message_payload = ObjPayload; obj.setChecksum_Lenght_Magic(); Packets.push(obj); } } }else if(!strcmp(packet.command,"object")) { object obj(packet); bool check = checkPow(obj.message_payload, obj.Time); //if not ignore if (check) { ustring invHash = this->bitmrc->inventoryHash(obj.message_payload); int present = this->bitmrc->sharedObj.insert(obj.message_payload, invHash); sTag tag; memcpy(tag.ch, invHash.c_str(), 32); this->bitmrc->new_inv.push(tag); if (obj.objectType == type_getpubkey) { packet_getpubkey getpubkey(obj); std::shared_lock<std::shared_timed_mutex> mlock(this->bitmrc->mutex_priv); for (unsigned int i = 0; i < this->bitmrc->PrivAddresses.size(); i++) { ustring tag = this->bitmrc->PrivAddresses[i].getTag(); if (getpubkey.tag == tag) { if(this->bitmrc->PrivAddresses[i].getLastPubKeyRequest() + 60 * 60 * 24 * 4 < time(NULL)) this->bitmrc->sendObj(this->bitmrc->PrivAddresses[i].encodePubKey(),true); //else //printf("PubKey already shared recently"); } } mlock.unlock(); } else if (obj.objectType == type_pubkey) { packet_pubkey pubkey(obj); std::shared_lock<std::shared_timed_mutex> mlock(this->bitmrc->mutex_pub); for (unsigned int i = 0; i < this->bitmrc->PubAddresses.size(); i++) { if (!this->bitmrc->PubAddresses[i].waitingPubKey()) continue; ustring tag = this->bitmrc->PubAddresses[i].getTag(); if (pubkey.tag == tag) { this->bitmrc->PubAddresses[i].decodeFromObj(pubkey); } } mlock.unlock(); } else if (obj.objectType == type_msg) { packet_msg msg(obj); if (this->bitmrc->decryptMsg(msg)) //it takes like 1-4 milliseconds { //printf("Message accepted\n"); } } else if (obj.objectType == type_broadcast) { packet_broadcast broadcast(obj); if (this->bitmrc->decryptMsg(broadcast)) { //printf("broadcast decrypted\n"); } } } } } }catch(int e) { switch(e) { case CONNECTION_CLOSED: case CONNECTION_ERROR: this->Close(); return; } } }
const char * ipv4(const char *interface) { return ip(interface, AF_INET); }
void compute(const QUESO::FullEnvironment& env) { struct timeval timevalNow; gettimeofday(&timevalNow, NULL); std::cout << std::endl << "Beginning run of 'Hysteretic' example at " << ctime(&timevalNow.tv_sec); //------------------------------------------------------ // Step 1 of 5: Instantiate the parameter space //------------------------------------------------------ QUESO::VectorSpace<> paramSpaceA(env, "paramA_", 1, NULL); QUESO::VectorSpace<> paramSpaceB(env, "paramB_", 14, NULL); QUESO::VectorSpace<> paramSpace (env, "param_", 15, NULL); //------------------------------------------------------ // Step 2 of 5: Instantiate the parameter domain //------------------------------------------------------ QUESO::GslVector paramMinsA(paramSpaceA.zeroVector()); paramMinsA.cwSet(0); QUESO::GslVector paramMaxsA(paramSpaceA.zeroVector()); paramMaxsA.cwSet(5); QUESO::BoxSubset<> paramDomainA("paramA_",paramSpaceA,paramMinsA,paramMaxsA); QUESO::GslVector paramMinsB(paramSpaceB.zeroVector()); paramMinsB.cwSet(-INFINITY); QUESO::GslVector paramMaxsB(paramSpaceB.zeroVector()); paramMaxsB.cwSet( INFINITY); QUESO::BoxSubset<> paramDomainB("paramB_",paramSpaceB,paramMinsB,paramMaxsB); QUESO::ConcatenationSubset<> paramDomain("",paramSpace,paramDomainA,paramDomainB); //------------------------------------------------------ // Step 3 of 5: Instantiate the likelihood function object //------------------------------------------------------ std::cout << "\tInstantiating the Likelihood; calling internally the hysteretic model" << std::endl; Likelihood<> likelihood("like_", paramDomain); likelihood.floor.resize(4,NULL); unsigned int numTimeSteps = 401; for (unsigned int i = 0; i < 4; ++i) { likelihood.floor[i] = new std::vector<double>(numTimeSteps,0.); } likelihood.accel.resize(numTimeSteps,0.); FILE *inp; inp = fopen("an.txt","r"); unsigned int numObservations = 0; double tmpA; while (fscanf(inp,"%lf",&tmpA) != EOF) { likelihood.accel[numObservations] = tmpA; numObservations++; } numObservations=1; FILE *inp1_1; inp1_1=fopen("measured_data1_1.txt","r"); while (fscanf(inp1_1,"%lf",&tmpA) != EOF) { (*likelihood.floor[0])[numObservations]=tmpA; numObservations++; } numObservations=0; FILE *inp1_2; inp1_2=fopen("measured_data1_2.txt","r"); while (fscanf(inp1_2,"%lf",&tmpA) != EOF) { (*likelihood.floor[1])[numObservations]=tmpA; numObservations++; } numObservations=0; FILE *inp1_3; inp1_3=fopen("measured_data1_3.txt","r"); while (fscanf(inp1_3,"%lf",&tmpA) != EOF) { (*likelihood.floor[2])[numObservations]=tmpA; numObservations++; } numObservations=0; FILE *inp1_4; inp1_4=fopen("measured_data1_4.txt","r"); while (fscanf(inp1_4,"%lf",&tmpA) != EOF) { (*likelihood.floor[3])[numObservations]=tmpA; numObservations++; } //------------------------------------------------------ // Step 4 of 5: Instantiate the inverse problem //------------------------------------------------------ std::cout << "\tInstantiating the SIP" << std::endl; QUESO::UniformVectorRV<> priorRvA("priorA_", paramDomainA); QUESO::GslVector meanVec(paramSpaceB.zeroVector()); QUESO::GslVector diagVec(paramSpaceB.zeroVector()); diagVec.cwSet(0.6*0.6); QUESO::GslMatrix covMatrix(diagVec); QUESO::GaussianVectorRV<> priorRvB("priorB_", paramDomainB,meanVec,covMatrix); QUESO::ConcatenatedVectorRV<> priorRv("prior_", priorRvA, priorRvB, paramDomain); QUESO::GenericVectorRV<> postRv("post_", paramSpace); QUESO::StatisticalInverseProblem<> ip("", NULL, priorRv, likelihood, postRv); //------------------------------------------------------ // Step 5 of 5: Solve the inverse problem //------------------------------------------------------ std::cout << "\tSolving the SIP with Multilevel method" << std::endl; ip.solveWithBayesMLSampling(); gettimeofday(&timevalNow, NULL); std::cout << "Ending run of 'Hysteretic' example at " << ctime(&timevalNow.tv_sec) << std::endl; return; }
//{{{ handleAreYouAlive int handleAreYouAlive(msg_t msg){ type(msg) = I_AM_ALIVE; if (_verbose) fprintf (stdout, "ARE_YOU_ALIVE received, sending I_AM_ALIVE\n"); strncpy(ip(msg), ips(msg), IPLONG * sizeof(char)); return (sendMessageWithAdd(msg)); }
void networkSetup(uint8_t *srvMac, uint8_t *srvIp) { IPAddress ip(srvIp); Ethernet.begin(srvMac, ip); server.begin(); delay(1000); }
int CVODEModel::evaluateRHSFunction( double time, SundialsAbstractVector* y, SundialsAbstractVector* y_dot) { /* * Convert Sundials vectors to SAMRAI vectors */ std::shared_ptr<SAMRAIVectorReal<double> > y_samvect( Sundials_SAMRAIVector::getSAMRAIVector(y)); std::shared_ptr<SAMRAIVectorReal<double> > y_dot_samvect( Sundials_SAMRAIVector::getSAMRAIVector(y_dot)); std::shared_ptr<PatchHierarchy> hierarchy(y_samvect->getPatchHierarchy()); /* * Compute max norm of solution vector. */ //std::shared_ptr<HierarchyDataOpsReal<double> > hierops( // new HierarchyCellDataOpsReal<double>(hierarchy)); //double max_norm = hierops->maxNorm(y_samvect-> // getComponentDescriptorIndex(0)); if (d_print_solver_info) { pout << "\t\tEval RHS: " << "\n \t\t\ttime = " << time << "\n \t\t\ty_maxnorm = " << y_samvect->maxNorm() << endl; } /* * Allocate scratch space and fill ghost cells in the solution vector * 1) Create a refine algorithm * 2) Register with the algorithm the current & scratch space, along * with a refine operator. * 3) Use the refine algorithm to construct a refine schedule * 4) Use the refine schedule to fill data on fine level. */ std::shared_ptr<RefineAlgorithm> bdry_fill_alg( new RefineAlgorithm()); std::shared_ptr<RefineOperator> refine_op(d_grid_geometry-> lookupRefineOperator(d_soln_var, "CONSERVATIVE_LINEAR_REFINE")); bdry_fill_alg->registerRefine(d_soln_scr_id, // dest y_samvect-> getComponentDescriptorIndex(0), // src d_soln_scr_id, // scratch refine_op); for (int ln = hierarchy->getFinestLevelNumber(); ln >= 0; --ln) { std::shared_ptr<PatchLevel> level(hierarchy->getPatchLevel(ln)); if (!level->checkAllocated(d_soln_scr_id)) { level->allocatePatchData(d_soln_scr_id); } // Note: a pointer to "this" tells the refine schedule to invoke // the setPhysicalBCs defined in this class. std::shared_ptr<RefineSchedule> bdry_fill_alg_schedule( bdry_fill_alg->createSchedule(level, ln - 1, hierarchy, this)); bdry_fill_alg_schedule->fillData(time); } /* * Step through the levels and compute rhs */ for (int ln = hierarchy->getFinestLevelNumber(); ln >= 0; --ln) { std::shared_ptr<PatchLevel> level(hierarchy->getPatchLevel(ln)); for (PatchLevel::iterator ip(level->begin()); ip != level->end(); ++ip) { const std::shared_ptr<Patch>& patch = *ip; std::shared_ptr<CellData<double> > y( SAMRAI_SHARED_PTR_CAST<CellData<double>, PatchData>( patch->getPatchData(d_soln_scr_id))); std::shared_ptr<SideData<double> > diff( SAMRAI_SHARED_PTR_CAST<SideData<double>, PatchData>( patch->getPatchData(d_diff_id))); std::shared_ptr<CellData<double> > rhs( SAMRAI_SHARED_PTR_CAST<CellData<double>, PatchData>( patch->getPatchData(y_dot_samvect->getComponentDescriptorIndex(0)))); TBOX_ASSERT(y); TBOX_ASSERT(diff); TBOX_ASSERT(rhs); const Index ifirst(patch->getBox().lower()); const Index ilast(patch->getBox().upper()); const std::shared_ptr<CartesianPatchGeometry> patch_geom( SAMRAI_SHARED_PTR_CAST<CartesianPatchGeometry, PatchGeometry>( patch->getPatchGeometry())); TBOX_ASSERT(patch_geom); const double* dx = patch_geom->getDx(); IntVector ghost_cells(y->getGhostCellWidth()); /* * 1 eqn radiation diffusion */ if (d_dim == Dimension(2)) { SAMRAI_F77_FUNC(comprhs2d, COMPRHS2D) ( ifirst(0), ilast(0), ifirst(1), ilast(1), ghost_cells(0), ghost_cells(1), dx, y->getPointer(), diff->getPointer(0), diff->getPointer(1), rhs->getPointer()); } else if (d_dim == Dimension(3)) { SAMRAI_F77_FUNC(comprhs3d, COMPRHS3D) ( ifirst(0), ilast(0), ifirst(1), ilast(1), ifirst(2), ilast(2), ghost_cells(0), ghost_cells(1), ghost_cells(2), dx, y->getPointer(), diff->getPointer(0), diff->getPointer(1), diff->getPointer(2), rhs->getPointer()); } } // loop over patches } // loop over levels /* * Deallocate scratch space. */ for (int ln = hierarchy->getFinestLevelNumber(); ln >= 0; --ln) { hierarchy->getPatchLevel(ln)->deallocatePatchData(d_soln_scr_id); } /* * record current time and increment counter for number of RHS * evaluations. */ d_current_soln_time = time; ++d_number_rhs_eval; return 0; }
int CallFrame::line(STATE) { if(!cm) return -2; // trampoline context return cm->line(state, ip()); }
double cv::pointPolygonTest( InputArray _contour, Point2f pt, bool measureDist ) { CV_INSTRUMENT_REGION(); double result = 0; Mat contour = _contour.getMat(); int i, total = contour.checkVector(2), counter = 0; int depth = contour.depth(); CV_Assert( total >= 0 && (depth == CV_32S || depth == CV_32F)); bool is_float = depth == CV_32F; double min_dist_num = FLT_MAX, min_dist_denom = 1; Point ip(cvRound(pt.x), cvRound(pt.y)); if( total == 0 ) return measureDist ? -DBL_MAX : -1; const Point* cnt = contour.ptr<Point>(); const Point2f* cntf = (const Point2f*)cnt; if( !is_float && !measureDist && ip.x == pt.x && ip.y == pt.y ) { // the fastest "purely integer" branch Point v0, v = cnt[total-1]; for( i = 0; i < total; i++ ) { v0 = v; v = cnt[i]; if( (v0.y <= ip.y && v.y <= ip.y) || (v0.y > ip.y && v.y > ip.y) || (v0.x < ip.x && v.x < ip.x) ) { if( ip.y == v.y && (ip.x == v.x || (ip.y == v0.y && ((v0.x <= ip.x && ip.x <= v.x) || (v.x <= ip.x && ip.x <= v0.x)))) ) return 0; continue; } int64 dist = static_cast<int64>(ip.y - v0.y)*(v.x - v0.x) - static_cast<int64>(ip.x - v0.x)*(v.y - v0.y); if( dist == 0 ) return 0; if( v.y < v0.y ) dist = -dist; counter += dist > 0; } result = counter % 2 == 0 ? -1 : 1; } else { Point2f v0, v; Point iv; if( is_float ) { v = cntf[total-1]; } else { v = cnt[total-1]; } if( !measureDist ) { for( i = 0; i < total; i++ ) { double dist; v0 = v; if( is_float ) v = cntf[i]; else v = cnt[i]; if( (v0.y <= pt.y && v.y <= pt.y) || (v0.y > pt.y && v.y > pt.y) || (v0.x < pt.x && v.x < pt.x) ) { if( pt.y == v.y && (pt.x == v.x || (pt.y == v0.y && ((v0.x <= pt.x && pt.x <= v.x) || (v.x <= pt.x && pt.x <= v0.x)))) ) return 0; continue; } dist = (double)(pt.y - v0.y)*(v.x - v0.x) - (double)(pt.x - v0.x)*(v.y - v0.y); if( dist == 0 ) return 0; if( v.y < v0.y ) dist = -dist; counter += dist > 0; } result = counter % 2 == 0 ? -1 : 1; } else { for( i = 0; i < total; i++ ) { double dx, dy, dx1, dy1, dx2, dy2, dist_num, dist_denom = 1; v0 = v; if( is_float ) v = cntf[i]; else v = cnt[i]; dx = v.x - v0.x; dy = v.y - v0.y; dx1 = pt.x - v0.x; dy1 = pt.y - v0.y; dx2 = pt.x - v.x; dy2 = pt.y - v.y; if( dx1*dx + dy1*dy <= 0 ) dist_num = dx1*dx1 + dy1*dy1; else if( dx2*dx + dy2*dy >= 0 ) dist_num = dx2*dx2 + dy2*dy2; else { dist_num = (dy1*dx - dx1*dy); dist_num *= dist_num; dist_denom = dx*dx + dy*dy; } if( dist_num*min_dist_denom < min_dist_num*dist_denom ) { min_dist_num = dist_num; min_dist_denom = dist_denom; if( min_dist_num == 0 ) break; } if( (v0.y <= pt.y && v.y <= pt.y) || (v0.y > pt.y && v.y > pt.y) || (v0.x < pt.x && v.x < pt.x) ) continue; dist_num = dy1*dx - dx1*dy; if( dy < 0 ) dist_num = -dist_num; counter += dist_num > 0; } result = std::sqrt(min_dist_num/min_dist_denom); if( counter % 2 == 0 ) result = -result; } } return result; }
int main(int argc, char* argv[]) { try { // initialize logging - this reads the file log.xml from the current directory log_init(); // read the command line options cxxtools::Arg<std::string> ip(argc, argv, 'i'); // option -i <ip-addres> defines the address where to find the server cxxtools::Arg<bool> binary(argc, argv, 'b'); cxxtools::Arg<bool> json(argc, argv, 'j'); cxxtools::Arg<bool> jsonhttp(argc, argv, 'J'); cxxtools::Arg<std::size_t> timeout(argc, argv, 't', cxxtools::RemoteClient::WaitInfinite); cxxtools::Arg<unsigned short> port(argc, argv, 'p', binary ? 7003 : json ? 7004 : 7002); // we need a selector, which controls the network activity cxxtools::Selector selector; // Normally we would define just one rpc client for the protocol we use but // here we want to demonstrate, that it is just up to the client, which protocol // is used for the remote call. // One client can run just one request at a time. To run parallel requests // we need 2 clients. So we define 2 clients for each protocol. // define a xlmrpc client cxxtools::xmlrpc::HttpClient xmlrpcClient1(selector, ip, port, "/xmlrpc"); cxxtools::xmlrpc::HttpClient xmlrpcClient2(selector, ip, port, "/xmlrpc"); // and a binary rpc client cxxtools::bin::RpcClient binaryClient1(selector, ip, port); cxxtools::bin::RpcClient binaryClient2(selector, ip, port); // and a tcp json rpc client cxxtools::json::RpcClient jsonClient1(selector, ip, port); cxxtools::json::RpcClient jsonClient2(selector, ip, port); // and a http json rpc client cxxtools::json::HttpClient httpJsonClient1(selector, ip, port, "/jsonrpc"); cxxtools::json::HttpClient httpJsonClient2(selector, ip, port, "/jsonrpc"); // now se select the client depending on the command line flags cxxtools::RemoteClient& client1( binary ? static_cast<cxxtools::RemoteClient&>(binaryClient1) : json ? static_cast<cxxtools::RemoteClient&>(jsonClient1) : jsonhttp ? static_cast<cxxtools::RemoteClient&>(httpJsonClient1) : static_cast<cxxtools::RemoteClient&>(xmlrpcClient1)); cxxtools::RemoteClient& client2( binary ? static_cast<cxxtools::RemoteClient&>(binaryClient2) : json ? static_cast<cxxtools::RemoteClient&>(jsonClient2) : jsonhttp ? static_cast<cxxtools::RemoteClient&>(httpJsonClient2) : static_cast<cxxtools::RemoteClient&>(xmlrpcClient2)); // define remote procedure with dobule return value and two double parameters cxxtools::RemoteProcedure<double, double, double> add1(client1, "add"); cxxtools::RemoteProcedure<double, double, double> add2(client2, "add"); // initiate the execution of our method add1.begin(5, 6); add2.begin(1, 2); // Calling RemoteProcedure::end will run the underlying event loop until // the remote procedure is finished and return the result. // In case of a error, an exception is thrown. // Note that waiting for the end of one remote procedure will also start // and maybe finish the second remote procedure. double result1 = add1.end(timeout); std::cout << "result1=" << result1 << std::endl; // Here we run the loop again until the second procedure is finished. It // may well be, that the procedure is already finished and we get the // result immediately. double result2 = add2.end(timeout); std::cout << "result2=" << result2 << std::endl; } catch (const std::exception& e) { std::cerr << "error: " << e.what() << std::endl; } }
void run() { m_mutex.lock(); result=ip(X1,X2,N); m_finished=true; m_mutex.unlock(); }
int main(int argc, char ** argv) { #ifdef QUESO_HAS_MPI MPI_Init(&argc, &argv); QUESO::FullEnvironment env(MPI_COMM_WORLD, argv[1], "", NULL); #else QUESO::FullEnvironment env(argv[1], "", NULL); #endif QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> paramSpace(env, "param_", 1, NULL); double min_val = 0.0; double max_val = 1.0; QUESO::GslVector paramMins(paramSpace.zeroVector()); paramMins.cwSet(min_val); QUESO::GslVector paramMaxs(paramSpace.zeroVector()); paramMaxs.cwSet(max_val); QUESO::BoxSubset<QUESO::GslVector, QUESO::GslMatrix> paramDomain("param_", paramSpace, paramMins, paramMaxs); QUESO::UniformVectorRV<QUESO::GslVector, QUESO::GslMatrix> priorRv("prior_", paramDomain); // Set up observation space QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> obsSpace(env, "obs_", 2, NULL); // Fill up observation vector QUESO::GslVector observations(obsSpace.zeroVector()); observations[0] = 1.0; observations[1] = 1.0; // Pass in observations to Gaussian likelihood object Likelihood<QUESO::GslVector, QUESO::GslMatrix> lhood("llhd_", paramDomain, observations, 1.0); QUESO::GenericVectorRV<QUESO::GslVector, QUESO::GslMatrix> postRv("post_", paramSpace); QUESO::StatisticalInverseProblem<QUESO::GslVector, QUESO::GslMatrix> ip("", NULL, priorRv, lhood, postRv); QUESO::GslVector paramInitials(paramSpace.zeroVector()); paramInitials[0] = 0.0; QUESO::GslMatrix proposalCovMatrix(paramSpace.zeroVector()); for (unsigned int i = 0; i < 1; i++) { proposalCovMatrix(i, i) = 0.1; } ip.solveWithBayesMetropolisHastings(NULL, paramInitials, &proposalCovMatrix); #ifdef QUESO_HAS_MPI MPI_Finalize(); #endif return 0; }
void normalize(float *X,long N) { double norm=sqrt(ip(X,X,N)); if (norm) { for (long j=0; j<N; j++) X[j]/=norm; } }
void PDFwin::pageChanged() { int ww=w()-2; int hh=h()-2; if(!mModel) return; if(!mModel->cacheValid(ww, hh, mCurrPage)) { mState="load"; redraw(); Fl::check(); mState="none"; } CImage* bmp_orig=mModel->getPage(ww, hh, mCurrPage); if(mLayout->findCheckButton("Use automatic segmentation")->value()) { CImage temp; temp.CopyFrom(*bmp_orig); #ifdef DEBUG_FONT_DETECTION CImage* bmp=bmp_orig; #else CImage* bmp=&temp; #endif { intmatrixn& textCache=*mModel->_textCache[mCurrPage]; #ifdef USE_FONT_DETECTION const bool drawLetterBoundingBox=true; const bool drawDetectedSpace=true; #else const bool drawLetterBoundingBox=false; const bool drawDetectedSpace=false; #endif if(drawLetterBoundingBox) { CImagePixel ip(bmp); for(int i=0; i<textCache.rows()-1; i++) { int* info=textCache[i]; int c=info[TC_c]; int x=info[TC_x]; int y=info[TC_y]; int w=info[TC_w]; int h=info[TC_h]; int fx=info[TC_fx]; int fy=info[TC_fy]; if (std::abs(y-textCache[i+1][TC_y])<=3) // draw excluding trailing space { #ifdef DEBUG_FONT_DETECTION ip.DrawLineBox(TRect(x-fx,y-fy, x-fx+w, y-fy+h), CPixelRGB8(0,0,0)); #else ip.DrawBox(TRect(x-fx,y-fy, x-fx+w, y-fy+h), CPixelRGB8(0,0,0)); #endif ip.DrawHorizLine(x, y, w, CPixelRGB8(255,0,0)); } // else printf("%d %d %d %d\n", x,y, textCache[i+1][TC_x], textCache[i+1][TC_y]); } } FlLayout* layout=mLayout->findLayout("Automatic segmentation"); double min_text_gap=layout->findSlider("MIN text-gap")->value(); if(drawDetectedSpace) { CImagePixel ip(bmp); for(int i=1; i<textCache.rows(); i++) { int* pinfo=textCache[i-1]; int* info=textCache[i]; if(pinfo[TC_y]==info[TC_y]) { int prevEnd=pinfo[TC_x]-pinfo[TC_fx]+pinfo[TC_w]-1; int curStart=info[TC_x]-info[TC_fx]; int bottom=MIN(pinfo[TC_y]-pinfo[TC_fy], info[TC_y]-info[TC_fy]); int top=MAX(pinfo[TC_y]-pinfo[TC_fy]+pinfo[TC_h], info[TC_y]-info[TC_fy]+info[TC_h]); int space_thr=MAX(pinfo[TC_h], pinfo[TC_w]); space_thr=MAX(space_thr, info[TC_h]); space_thr=MAX(space_thr, info[TC_w]); space_thr*=min_text_gap; if(curStart-prevEnd<=space_thr && curStart>prevEnd) { // mark as space. ip.DrawBox(TRect(prevEnd, bottom, curStart, top), CPixelRGB8(0,0,0)); } } } } } SummedAreaTable t(*bmp);//bmp->getWidth(), bmp->getHeight(), bmp->getDataPtr(), bmp->getRowSize()); FlLayout* layout=mLayout->findLayout("Automatic segmentation"); double min_gap_percentage=layout->findSlider("MIN gap")->value(); double margin_percentage=layout->findSlider("Margin")->value(); int thr_white=layout->findSlider("white point")->value(); double max_width=1.0/layout->findSlider("N columns")->value(); double cropT=layout->findSlider("Crop T")->value()/100.0; double cropB=layout->findSlider("Crop B")->value()/100.0; double cropL=layout->findSlider("Crop L")->value()/100.0; double cropR=layout->findSlider("Crop R")->value()/100.0; // TRect domain(0,0, bmp->GetWidth(), bmp->GetHeight()); TRect domain(cropL*bmp->GetWidth(),cropT*bmp->GetHeight(), (1-cropR)*bmp->GetWidth() , (1-cropB)*bmp->GetHeight()); ImageSegmentation s(t, true, domain, 0, min_gap_percentage, thr_white); s.segment(); std::list<TRect> results; s.getResult(results, max_width, margin_percentage); /* std::list<TRect> results; int min_gap=int((double)bmp->GetWidth()*min_gap_percentage*0.01 +0.5); int min_box_size=100; // 10�ȼ������� �ڽ��� ������. CImagePixel orig(bmp_orig); std::list<index2> LRcorners; std::list<index2> ULcorners; for(int i=0, ni=bmp->GetWidth()-min_box_size-min_gap; i<ni; i++) { for(int j=0, nj=bmp->GetHeight()-min_box_size-min_gap; j<nj; j++) { // search for LR corner TRect outerBox(i, j, i+min_box_size+min_gap, j+min_box_size+min_gap); TRect innerBox(i, j, i+min_box_size, j+min_box_size); int sum1=t.sum(outerBox); int sum2=t.sum(innerBox); int area=outerBox.Width()*outerBox.Height()- innerBox.Width()*innerBox.Height(); if(sum1-sum2 >= thr_white*area) { //orig.SetPixel(i+min_box_size+min_gap/2,j+min_box_size+min_gap/2,CPixelRGB8(255,0,0)); LRcorners.push_back(index2(i+min_box_size+min_gap/2,j+min_box_size+min_gap/2)); } // search for UL corner { TRect outerBox(i, j, i+min_box_size+min_gap, j+min_box_size+min_gap); TRect innerBox(i+min_gap, j+min_gap, i+min_box_size+min_gap, j+min_box_size+min_gap); int sum1=t.sum(outerBox); int sum2=t.sum(innerBox); int area=outerBox.Width()*outerBox.Height()- innerBox.Width()*innerBox.Height(); if(sum1-sum2 >= thr_white*area) { // orig.SetPixel(i+min_gap/2,j+min_gap/2,CPixelRGB8(0,0,255)); ULcorners.push_back(index2(i+min_gap/2,j+min_gap/2)); } } } } { std::list<index2>::iterator i; for(i=ULcorners.begin(); i!=ULcorners.end(); ++i) { orig.SetPixel((*i).x(), (*i).y(), CPixelRGB8(0,0,255)); } } */ mRects.clear(); int x=toWindowCoord(0,0).x; int y=toWindowCoord(0,0).y; std::list<TRect>::iterator i; for(i=results.begin(); i!=results.end(); i++) { mRects.push_back(SelectionRectangle()); mSelectedRect=mRects.end(); mSelectedRect--; SelectionRectangle& mRect=selectedRect(); mRect.p1=toDocCoord((*i).left+x, (*i).top+y); mRect.p2=toDocCoord((*i).right+x, (*i).bottom+y); mRect.updateScreenCoord(*this); } } }
int main(int argc, char* argv[]) { try { log_init(); cxxtools::Arg<std::string> ip(argc, argv, 'i'); cxxtools::Arg<unsigned short> port(argc, argv, 'p', 1234); cxxtools::Arg<unsigned> bufsize(argc, argv, 'b', 8192); cxxtools::Arg<bool> listen(argc, argv, 'l'); cxxtools::Arg<bool> read_reply(argc, argv, 'r'); cxxtools::Arg<bool> ssl(argc, argv, 's'); cxxtools::Arg<std::string> cert(argc, argv, "--cert"); cxxtools::Arg<std::string> ca(argc, argv, "--CA"); if (listen) { // I'm a server // listen to a port cxxtools::net::TcpServer server(ip.getValue(), port); // accept a connetion cxxtools::net::TcpStream worker(server, bufsize); if (ssl) { if (cert.isSet()) worker.loadSslCertificateFile(cert); if (ca.isSet()) worker.setSslVerify(2, ca); worker.sslAccept(); } // copy to stdout std::cout << worker.rdbuf(); } else { // I'm a client // connect to server cxxtools::net::TcpStream peer(ip, port, bufsize); if (ssl) { if (cert.isSet()) peer.loadSslCertificateFile(cert); peer.sslConnect(); } if (argc > 1) { for (int a = 1; a < argc; ++a) { std::ifstream in(argv[a]); peer << in.rdbuf() << std::flush; } } else { // copy stdin to server peer << std::cin.rdbuf() << std::flush; } if (read_reply) // copy answer to stdout std::cout << peer.rdbuf() << std::flush; } } catch (const std::exception& e) { std::cerr << e.what() << std::endl; } }
HierarchyNeighbors::HierarchyNeighbors( const PatchHierarchy& hierarchy, int coarsest_level, int finest_level, bool do_same_level_nbrs, int width) : d_coarsest_level(coarsest_level), d_finest_level(finest_level), d_do_same_level_nbrs(do_same_level_nbrs), d_neighbor_width(hierarchy.getDim(), width) { int num_levels = hierarchy.getNumberOfLevels(); TBOX_ASSERT(coarsest_level >= 0); TBOX_ASSERT(coarsest_level <= finest_level); TBOX_ASSERT(finest_level < num_levels); TBOX_ASSERT(width >= 1); d_same_level_nbrs.resize(num_levels); d_finer_level_nbrs.resize(num_levels); d_coarser_level_nbrs.resize(num_levels); for (int ln = finest_level; ln >= coarsest_level; --ln) { const std::shared_ptr<PatchLevel>& current_level = hierarchy.getPatchLevel(ln); /* * If there is a finer level than ln, find the finer level neighbors */ if (ln != finest_level) { std::shared_ptr<PatchLevel> finer_level( hierarchy.getPatchLevel(ln+1)); hier::IntVector connector_width( IntVector::getOne(hierarchy.getDim()), hierarchy.getNumberBlocks()); if (d_neighbor_width != IntVector::getOne(hierarchy.getDim())) { connector_width *= d_neighbor_width; connector_width.ceilingDivide(finer_level->getRatioToCoarserLevel()); } const Connector& coarse_to_fine = current_level->findConnector( *finer_level, connector_width, CONNECTOR_IMPLICIT_CREATION_RULE, true); for (PatchLevel::iterator ip(current_level->begin()); ip != current_level->end(); ++ip) { const std::shared_ptr<Patch>& patch = *ip; const Box& box = patch->getBox(); const BoxId& box_id = box.getBoxId(); BoxContainer& finer_nbrs = d_finer_level_nbrs[ln][box_id]; if (coarse_to_fine.hasNeighborSet(box_id)) { coarse_to_fine.getNeighborBoxes(box_id, finer_nbrs); } } } /* * If there is a coarser level than ln, find the coarser level neighbors */ if (ln != coarsest_level) { std::shared_ptr<PatchLevel> coarser_level( hierarchy.getPatchLevel(ln-1)); const Connector& fine_to_coarse = current_level->findConnector( *coarser_level, d_neighbor_width, CONNECTOR_IMPLICIT_CREATION_RULE, true); for (PatchLevel::iterator ip(current_level->begin()); ip != current_level->end(); ++ip) { const std::shared_ptr<Patch>& patch = *ip; const Box& box = patch->getBox(); const BoxId& box_id = box.getBoxId(); BoxContainer& coarser_nbrs = d_coarser_level_nbrs[ln][box_id]; if (fine_to_coarse.hasNeighborSet(box_id)) { fine_to_coarse.getNeighborBoxes(box_id, coarser_nbrs); } } } /* * Find the neighbors on the current level */ if (do_same_level_nbrs) { const Connector& current_to_current = current_level->findConnector( *current_level, d_neighbor_width, CONNECTOR_IMPLICIT_CREATION_RULE, true); for (PatchLevel::iterator ip(current_level->begin()); ip != current_level->end(); ++ip) { const std::shared_ptr<Patch>& patch = *ip; const Box& box = patch->getBox(); const BoxId& box_id = box.getBoxId(); BoxContainer& same_level_nbrs = d_same_level_nbrs[ln][box_id]; BoxContainer same_nbr_boxes; if (current_to_current.hasNeighborSet(box_id)) { current_to_current.getNeighborBoxes(box_id, same_nbr_boxes); } for (BoxContainer::iterator itr = same_nbr_boxes.begin(); itr != same_nbr_boxes.end(); ++itr) { if (box_id != itr->getBoxId()) { same_level_nbrs.insert(same_level_nbrs.end(), *itr); } } } } } }
void UdpServerTransport::open() { RCF_LOG_4()(mIpAddress.string()) << "UdpServerTransport - creating server socket."; int mPort = mIpAddress.getPort(); // create and bind a socket for receiving UDP messages if (mFd == -1 && mPort >= 0) { int ret = 0; int err = 0; mIpAddress.resolve(); mFd = mIpAddress.createSocket(SOCK_DGRAM, IPPROTO_UDP); // enable reception of broadcast messages int enable = 1; ret = setsockopt(mFd, SOL_SOCKET, SO_BROADCAST, (char *) &enable, sizeof(enable)); err = Platform::OS::BsdSockets::GetLastError(); if (ret) { RCF_LOG_1()(ret)(err) << "setsockopt() - failed to set SO_BROADCAST on listening udp socket."; } // Share the address binding, if appropriate. if (mEnableSharedAddressBinding) { enable = 1; // Set SO_REUSEADDR socket option. ret = setsockopt(mFd, SOL_SOCKET, SO_REUSEADDR, (char *) &enable, sizeof(enable)); err = Platform::OS::BsdSockets::GetLastError(); if (ret) { RCF_LOG_1()(ret)(err) << "setsockopt() - failed to set SO_REUSEADDR on listening udp multicast socket."; } // On OS X and BSD variants, need to set SO_REUSEPORT as well. #if (defined(__MACH__) && defined(__APPLE__)) || defined(__FreeBSD__) || defined(__OpenBSD__) || defined(__NetBSD__) ret = setsockopt(mFd, SOL_SOCKET, SO_REUSEPORT, (char *) &enable, sizeof(enable)); err = Platform::OS::BsdSockets::GetLastError(); if (ret) { RCF_LOG_1()(ret)(err) << "setsockopt() - failed to set SO_REUSEPORT on listening udp multicast socket."; } #endif } sockaddr * pServerAddr = NULL; Platform::OS::BsdSockets::socklen_t serverAddrSize = 0; mIpAddress.getSockAddr(pServerAddr, serverAddrSize); // bind the socket ret = ::bind(mFd, pServerAddr, serverAddrSize); if (ret < 0) { err = Platform::OS::BsdSockets::GetLastError(); Exception e(_RcfError_Socket("bind()"), err, RcfSubsystem_Os); RCF_THROW(e)(mFd)(mIpAddress.string())(ret); } RCF_ASSERT_NEQ( mFd , -1 ); if (!mMulticastIpAddress.empty()) { // set socket option for receiving multicast messages mMulticastIpAddress.resolve(); // TODO: implement for IPv6. RCF_ASSERT( mIpAddress.getType() == IpAddress::V4 && mMulticastIpAddress.getType() == IpAddress::V4); std::string ip = mMulticastIpAddress.getIp(); ip_mreq imr; imr.imr_multiaddr.s_addr = inet_addr(ip.c_str()); imr.imr_interface.s_addr = INADDR_ANY; int ret = setsockopt(mFd,IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char*) &imr, sizeof(imr)); int err = Platform::OS::BsdSockets::GetLastError(); RCF_VERIFY( ret == 0, Exception( _RcfError_Socket("setsockopt() with IPPROTO_IP/IP_ADD_MEMBERSHIP"), err, RcfSubsystem_Os)) (mMulticastIpAddress.string())(mIpAddress.string()); // TODO: enable source-filtered multicast messages //ip_mreq_source imr; //imr.imr_multiaddr.s_addr = inet_addr("232.5.6.7"); //imr.imr_sourceaddr.s_addr = INADDR_ANY;//inet_addr("10.1.1.2"); //imr.imr_interface.s_addr = INADDR_ANY; //int ret = setsockopt(mFd,IPPROTO_IP, IP_ADD_SOURCE_MEMBERSHIP, (const char*) &imr, sizeof(imr)); //int err = Platform::OS::BsdSockets::GetLastError(); } // set the socket to nonblocking mode Platform::OS::BsdSockets::setblocking(mFd, false); // retrieve the port number, if it's generated by the system if (mPort == 0) { IpAddress ip(mFd, mIpAddress.getType()); mPort = ip.getPort(); mIpAddress.setPort(mPort); } RCF_LOG_2() << "UdpServerTransport - listening on port " << mPort << "."; } }
int main(int argc, const char* argv[]) { std::string opt_ip = "131.254.10.126"; bool opt_language_english = true; bool opt_debug = false; for (unsigned int i=0; i<argc; i++) { if (std::string(argv[i]) == "--ip") opt_ip = argv[i+1]; else if (std::string(argv[i]) == "--fr") opt_language_english = false; else if (std::string(argv[i]) == "--debug") opt_debug = true; else if (std::string(argv[i]) == "--help") { std::cout << "Usage: " << argv[0] << "[--ip <robot address>] [--fr]" << std::endl; return 0; } } std::string camera_name = "CameraTopPepper"; // Open the grabber for the acquisition of the images from the robot vpNaoqiGrabber g; if (! opt_ip.empty()) g.setRobotIp(opt_ip); g.setFramerate(30); g.setCamera(0); g.open(); vpCameraParameters cam = vpNaoqiGrabber::getIntrinsicCameraParameters(AL::kQVGA,camera_name, vpCameraParameters::perspectiveProjWithDistortion); vpHomogeneousMatrix eMc = vpNaoqiGrabber::getExtrinsicCameraParameters(camera_name,vpCameraParameters::perspectiveProjWithDistortion); std::cout << "eMc:" << std::endl << eMc << std::endl; std::cout << "cam:" << std::endl << cam << std::endl; vpNaoqiRobot robot; // Connect to the robot if (! opt_ip.empty()) robot.setRobotIp(opt_ip); robot.open(); if (robot.getRobotType() != vpNaoqiRobot::Pepper) { std::cout << "ERROR: You are not connected to Pepper, but to a different Robot. Check the IP. " << std::endl; return 0; } std::vector<std::string> jointNames_head = robot.getBodyNames("Head"); // Open Proxy for the speech AL::ALTextToSpeechProxy tts(opt_ip, 9559); std::string phraseToSay; if (opt_language_english) { tts.setLanguage("English"); phraseToSay = " \\emph=2\\ Hi,\\pau=200\\ How are you ?"; } else { tts.setLanguage("French"); phraseToSay = " \\emph=2\\ Bonjour,\\pau=200\\ comment vas tu ?"; } // Inizialize PeoplePerception AL::ALPeoplePerceptionProxy people_proxy(opt_ip, 9559); AL::ALMemoryProxy m_memProxy(opt_ip, 9559); people_proxy.subscribe("People", 30, 0.0); std::cout << "period: " << people_proxy.getCurrentPeriod() << std::endl; // Open Proxy for the recognition speech AL::ALSpeechRecognitionProxy asr(opt_ip, 9559); // asr.unsubscribe("Test_ASR"); // return 0 ; asr.setVisualExpression(false); asr.setLanguage("English"); std::vector<std::string> vocabulary; vocabulary.push_back("follow me"); vocabulary.push_back("stop"); // Set the vocabulary asr.setVocabulary(vocabulary,false); // Start the speech recognition engine with user Test_ASR asr.subscribe("Test_ASR"); std::cout << "Speech recognition engine started" << std::endl; // Proxy to control the leds AL::ALLedsProxy led_proxy(opt_ip, 9559); //Declare plots vpPlot * plotter_diff_vel; vpPlot * plotter_vel; vpPlot * plotter_error; vpPlot * plotter_distance; if (opt_debug) { // Plotting plotter_diff_vel = new vpPlot (2); plotter_diff_vel->initGraph(0, 2); plotter_diff_vel->initGraph(1, 2); plotter_diff_vel->setTitle(0, "HeadYaw"); plotter_diff_vel->setTitle(1, "HeadPitch"); plotter_vel= new vpPlot (1); plotter_vel->initGraph(0, 5); plotter_vel->setLegend(0, 0, "vx"); plotter_vel->setLegend(0, 1, "vy"); plotter_vel->setLegend(0, 2, "wz"); plotter_vel->setLegend(0, 3, "q_yaw"); plotter_vel->setLegend(0, 4, "q_pitch"); plotter_error = new vpPlot(1); plotter_error->initGraph(0, 3); plotter_error->setLegend(0, 0, "x"); plotter_error->setLegend(0, 1, "y"); plotter_error->setLegend(0, 2, "Z"); plotter_distance = new vpPlot (1); plotter_distance->initGraph(0, 1); plotter_distance->setLegend(0, 0, "dist"); } try { vpImage<unsigned char> I(g.getHeight(), g.getWidth()); vpDisplayX d(I); vpDisplay::setTitle(I, "ViSP viewer"); vpFaceTrackerOkao face_tracker(opt_ip,9559); double dist = 0.0; // Distance between person detected from peoplePerception and faceDetection // Set Visual Servoing: vpServo task; task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ; task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE); // vpAdaptiveGain lambda_adapt; // lambda_adapt.initStandard(1.6, 1.8, 15); vpAdaptiveGain lambda_base(1.2, 1.0, 10); // 2.3, 0.7, 15 vpAdaptiveGain lambda_nobase(5, 2.9, 15); // 4, 0.5, 15 task.setLambda(lambda_base) ; double Z = 0.9; double Zd = 0.9; bool stop_vxy = false; bool move_base = true; bool move_base_prev = true; // Create the desired visual feature vpFeaturePoint s; vpFeaturePoint sd; vpImagePoint ip(I.getHeight()/2, I.getWidth()/2); // Create the current x visual feature vpFeatureBuilder::create(s, cam, ip); vpFeatureBuilder::create(sd, cam, ip); // sd.buildFrom( I.getWidth()/2, I.getHeight()/2, Zd); AL::ALValue limit_yaw = robot.getProxy()->getLimits("HeadYaw"); std::cout << limit_yaw[0][0] << " " << limit_yaw[0][1] << std::endl; // Add the feature task.addFeature(s, sd) ; vpFeatureDepth s_Z, s_Zd; s_Z.buildFrom(s.get_x(), s.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0 s_Zd.buildFrom(sd.get_x(), sd.get_y(), Zd , 0); // log(Z/Z*) = 0 that's why the last parameter is 0 // Add the feature task.addFeature(s_Z, s_Zd); // Jacobian 6x5 (vx,vy,wz,q_yaq,q_pitch) vpMatrix tJe(6,5); tJe[0][0]= 1; tJe[1][1]= 1; tJe[5][2]= 1; vpMatrix eJe(6,5); double servo_time_init = 0; vpImagePoint head_cog_cur; vpImagePoint head_cog_des(I.getHeight()/2, I.getWidth()/2); vpColVector q_dot; bool reinit_servo = true; unsigned long loop_iter = 0; std::vector<std::string> recognized_names; std::map<std::string,unsigned int> detected_face_map; bool detection_phase = true; unsigned int f_count = 0; // AL::ALValue leg_names = AL::ALValue::array("HipRoll","HipPitch", "KneePitch" ); // AL::ALValue values = AL::ALValue::array(0.0, 0.0, 0.0 ); // robot.getProxy()->setMoveArmsEnabled(false,false); std::vector<std::string> arm_names = robot.getBodyNames("LArm"); std::vector<std::string> rarm_names = robot.getBodyNames("RArm"); std::vector<std::string> leg_names(3); leg_names[0]= "HipRoll"; leg_names[1]= "HipPitch"; leg_names[2]= "KneePitch"; arm_names.insert( arm_names.end(), rarm_names.begin(), rarm_names.end() ); arm_names.insert( arm_names.end(), leg_names.begin(), leg_names.end() ); std::vector<float> arm_values(arm_names.size(),0.0); for (unsigned int i = 0; i < arm_names.size(); i++ ) std::cout << arm_names[i]<< std::endl; robot.getPosition(arm_names, arm_values,false); vpImagePoint best_cog_face_peoplep; bool person_found = false; double t_prev = vpTime::measureTimeSecond(); while(1) { if (reinit_servo) { servo_time_init = vpTime::measureTimeSecond(); t_prev = vpTime::measureTimeSecond(); reinit_servo = false; led_proxy.fadeRGB("FaceLeds","white",0.1); } double t = vpTime::measureTimeMs(); if (0) // (opt_debug) { g.acquire(I); } vpDisplay::display(I); // Detect face bool face_found = face_tracker.detect(); stop_vxy = false; //std::cout << "Loop time face_tracker: " << vpTime::measureTimeMs() - t << " ms" << std::endl; // Check speech recognition result AL::ALValue result_speech = m_memProxy.getData("WordRecognized"); if ( ((result_speech[0]) == vocabulary[0]) && (double (result_speech[1]) > 0.4 )) //move { //std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl; task.setLambda(lambda_base) ; move_base = true; } else if ( (result_speech[0] == vocabulary[1]) && (double(result_speech[1]) > 0.4 )) //stop { //std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl; task.setLambda(lambda_nobase) ; move_base = false; } if (move_base != move_base_prev) { if (move_base) { phraseToSay = "Ok, I will follow you."; tts.post.say(phraseToSay); } else { phraseToSay = "Ok, I will stop."; tts.post.say(phraseToSay); } } //robot.setStiffness(arm_names,0.0); //robot.getProxy()->setAngles(arm_names,arm_values,1.0); //robot.getProxy()->setAngles("HipRoll",0.0,1.0); //std::cout << "Loop time check_speech: " << vpTime::measureTimeMs() - t << " ms" << std::endl; move_base_prev = move_base; if (face_found) { std::ostringstream text; text << "Found " << face_tracker.getNbObjects() << " face(s)"; vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red); for(size_t i=0; i < face_tracker.getNbObjects(); i++) { vpRect bbox = face_tracker.getBBox(i); if (i == 0) vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 2); else vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1); vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), face_tracker.getMessage(i) , vpColor::red); } led_proxy.post.fadeRGB("FaceLeds","blue",0.1); double u = face_tracker.getCog(0).get_u(); double v = face_tracker.getCog(0).get_v(); if (u<= g.getWidth() && v <= g.getHeight()) head_cog_cur.set_uv(u,v); vpRect bbox = face_tracker.getBBox(0); std::string name = face_tracker.getMessage(0); //std::cout << "Loop time face print " << vpTime::measureTimeMs() - t << " ms" << std::endl; } AL::ALValue result = m_memProxy.getData("PeoplePerception/VisiblePeopleList"); //std::cout << "Loop time get Data PeoplePerception " << vpTime::measureTimeMs() - t << " ms" << std::endl; person_found = false; if (result.getSize() > 0) { AL::ALValue info = m_memProxy.getData("PeoplePerception/PeopleDetected"); int num_people = info[1].getSize(); std::ostringstream text; text << "Found " << num_people << " person(s)"; vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red); person_found = true; if (face_found) // Try to find the match between two detection { vpImagePoint cog_face; double dist_min = 1000; unsigned int index_person = 0; for (unsigned int i = 0; i < num_people; i++) { float alpha = info[1][i][2]; float beta = info[1][i][3]; //Z = Zd; // Centre of face into the image float x = g.getWidth()/2 - g.getWidth() * beta; float y = g.getHeight()/2 + g.getHeight() * alpha; cog_face.set_uv(x,y); dist = vpImagePoint::distance(cog_face, head_cog_cur); if (dist < dist_min) { dist_min = dist; best_cog_face_peoplep = cog_face; index_person = i; } } vpDisplay::displayCross(I, best_cog_face_peoplep, 10, vpColor::white); if (dist_min < 55.) { Z = info[1][index_person][1]; // Current distance } } else // Take the first one on the list { float alpha = info[1][0][2]; float beta = info[1][0][3]; //Z = Zd; // Centre of face into the image float x = g.getWidth()/2 - g.getWidth() * beta; float y = g.getHeight()/2 + g.getHeight() * alpha; head_cog_cur.set_uv(x,y); Z = info[1][0][1]; // Current distance } } else { std::cout << "No distance computed " << std::endl; stop_vxy = true; robot.getProxy()->setAngles("HipRoll",0.0,1.0); //Z = Zd; } // float alpha = info[1][0][2]; // float beta = info[1][0][3]; // //Z = Zd; // // Centre of face into the image // float x = g.getWidth()/2 - g.getWidth() * beta; // float y = g.getHeight()/2 + g.getHeight() * alpha; // vpImagePoint cog_face(y,x); // dist = vpImagePoint::distance(cog_face,head_cog_cur); // if (dist < 55.) // Z = info[1][0][1]; // Current distance // else // stop_vxy = true; // } // else // { // std::cout << "No distance computed " << std::endl; // stop_vxy = true; // //Z = Zd; // } //std::cout << "Loop time before VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl; if (face_found || person_found ) { // Get Head Jacobian (6x2) vpMatrix torso_eJe_head; robot.get_eJe("Head",torso_eJe_head); // Add column relative to the base rotation (Wz) // vpColVector col_wz(6); // col_wz[5] = 1; for (unsigned int i = 0; i < 6; i++) for (unsigned int j = 0; j < torso_eJe_head.getCols(); j++) tJe[i][j+3] = torso_eJe_head[i][j]; // std::cout << "tJe" << std::endl << tJe << std::endl; // vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform(jointNames_head[jointNames_head.size()-1], 0, true));// get transformation matrix between torso and HeadRoll vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform("HeadPitch", 0, true));// get transformation matrix between torso and HeadRoll vpVelocityTwistMatrix HeadPitchVLtorso(torsoMHeadPith.inverse()); for(unsigned int i=0; i< 3; i++) for(unsigned int j=0; j< 3; j++) HeadPitchVLtorso[i][j+3] = 0; //std::cout << "HeadPitchVLtorso: " << std::endl << HeadPitchVLtorso << std::endl; // Transform the matrix eJe = HeadPitchVLtorso *tJe; // std::cout << "eJe" << std::endl << eJe << std::endl; task.set_eJe( eJe ); task.set_cVe( vpVelocityTwistMatrix(eMc.inverse()) ); vpDisplay::displayCross(I, head_cog_des, 10, vpColor::blue); vpDisplay::displayCross(I, head_cog_cur, 10, vpColor::green); // std::cout << "head_cog_des:" << std::endl << head_cog_des << std::endl; // std::cout << "head_cog_cur:" << std::endl << head_cog_cur << std::endl; // Update the current x feature double x,y; vpPixelMeterConversion::convertPoint(cam, head_cog_cur, x, y); s.buildFrom(x, y, Z); //s.set_xyZ(head_cog_cur.get_u(), head_cog_cur.get_v(), Z); // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix s_Z.buildFrom(s.get_x(), s.get_y(), Z, log(Z/Zd)) ; q_dot = task.computeControlLaw(vpTime::measureTimeSecond() - servo_time_init); //std::cout << "Loop time compute VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl; vpMatrix P = task.getI_WpW(); double alpha = -3.3; double min = limit_yaw[0][0]; double max = limit_yaw[0][1]; vpColVector z_q2 (q_dot.size()); vpColVector q_yaw = robot.getPosition(jointNames_head[0]); z_q2[3] = 2 * alpha * q_yaw[0]/ pow((max - min),2); vpColVector q3 = P * z_q2; //if (q3.euclideanNorm()<10.0) q_dot = q_dot + q3; std::vector<float> vel(jointNames_head.size()); vel[0] = q_dot[3]; vel[1] = q_dot[4]; // Compute the distance in pixel between the target and the center of the image double distance = vpImagePoint::distance(head_cog_cur, head_cog_des); //if (distance > 0.03*I.getWidth()) std::cout << "q:" << std::endl << q_dot << std::endl; // std::cout << "vel" << std::endl << q_dot << std::endl; //std::cout << "distance" << std::endl << distance <<" -- " << 0.03*I.getWidth() << " ++ " << I.getWidth() << std::endl; // if (distance > 0.1*I.getWidth()) robot.setVelocity(jointNames_head,vel); // else // { // std::cout << "Setting hipRoll to zero" << std::endl; // robot.getProxy()->setAngles("HipRoll",0.0,1.0); // } // std::cout << "errorZ: " << task.getError()[2] << std::endl; // std::cout << "stop_vxy: " << stop_vxy << std::endl; if (std::fabs(Z -Zd) < 0.05 || stop_vxy || !move_base) robot.setBaseVelocity(0.0, 0.0, q_dot[2]); else robot.setBaseVelocity(q_dot[0], q_dot[1], q_dot[2]); if (opt_debug) { vpColVector vel_head = robot.getJointVelocity(jointNames_head); for (unsigned int i=0 ; i < jointNames_head.size() ; i++) { plotter_diff_vel->plot(i, 1, loop_iter, q_dot[i+3]); plotter_diff_vel->plot(i, 0, loop_iter, vel_head[i]); } plotter_error->plot(0,loop_iter,task.getError()); plotter_vel->plot(0,loop_iter, q3); plotter_distance->plot(0,0,loop_iter,Z); } // if (detection_phase) // { // //if (score >= 0.4 && distance < 0.06*I.getWidth() && bbox.getSize() > 3000) // if (distance < 0.06*I.getWidth() && bbox.getSize() > 3000) // { // if (opt_debug) // { // vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 1); // vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::red); // } // detected_face_map[name]++; // f_count++; // } // else // { // if (opt_debug) // { // vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1); // vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::green); // } // } // if (f_count>10) // { // detection_phase = false; // f_count = 0; // } // } // else // { // std::string recognized_person_name = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->first; // unsigned int times = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->second; // if (!in_array(recognized_person_name, recognized_names) && recognized_person_name != "Unknown") { // if (opt_language_english) // { // phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ How are you ?"; // } // else // { // phraseToSay = "\\emph=2\\ Salut \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ comment vas tu ?";; // } // std::cout << phraseToSay << std::endl; // tts.post.say(phraseToSay); // recognized_names.push_back(recognized_person_name); // } // if (!in_array(recognized_person_name, recognized_names) && recognized_person_name == "Unknown" // && times > 15) // { // if (opt_language_english) // { // phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\. I don't know you! \\emph=2\\ What's your name?"; // } // else // { // phraseToSay = " \\emph=2\\ Salut \\wait=200\\ \\emph=2\\. Je ne te connais pas! \\emph=2\\ Comment t'appelles-tu ?"; // } // std::cout << phraseToSay << std::endl; // tts.post.say(phraseToSay); // recognized_names.push_back(recognized_person_name); // } // detection_phase = true; // detected_face_map.clear(); // } } else { robot.stop(jointNames_head); robot.stopBase(); std::cout << "Stop!" << std::endl; reinit_servo = true; } //if (opt_debug) vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) break; loop_iter ++; std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl; } robot.stop(jointNames_head); robot.stopBase(); tts.setLanguage("French"); // tts.exit(); people_proxy.unsubscribe("People"); // people_proxy.exit(); asr.unsubscribe("Test_ASR"); asr.setVisualExpression(true); // asr.exit(); tts.setLanguage("French"); // tts.exit(); led_proxy.fadeRGB("FaceLeds","white",0.1); // led_proxy.exit(); vpDisplay::getClick(I, true); } catch(vpException &e) { std::cout << e.getMessage() << std::endl; } catch (const AL::ALError& e) { std::cerr << "Caught exception " << e.what() << std::endl; } std::cout << "The end: stop the robot..." << std::endl; //tts.setLanguage("French"); // tts.exit(); robot.stop(jointNames_head); robot.stopBase(); return 0; }
int main(int argc, char ** argv) { // Step 0: Set up some variables unsigned int numExperiments = 10; // Number of experiments unsigned int numUncertainVars = 5; // Number of things to calibrate unsigned int numSimulations = 50; // Number of simulations unsigned int numConfigVars = 1; // Dimension of configuration space unsigned int numEta = 1; // Number of responses the model is returning unsigned int experimentSize = 1; // Size of each experiment #ifdef QUESO_HAS_MPI MPI_Init(&argc, &argv); // Step 1: Set up QUESO environment QUESO::FullEnvironment env(MPI_COMM_WORLD, argv[1], "", NULL); #else QUESO::FullEnvironment env(argv[1], "", NULL); #endif // Step 2: Set up prior for calibration parameters QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> paramSpace(env, "param_", numUncertainVars, NULL); // Parameter (theta) bounds: // descriptors 'k_tmasl' 'k_xkle' 'k_xkwew' 'k_xkwlx' 'k_cd' // upper_bounds 1.05 1.1 1.1 1.1 1.1 // lower_bounds 0.95 0.9 0.9 0.9 0.9 // // These bounds are dealt with when reading in the data QUESO::GslVector paramMins(paramSpace.zeroVector()); QUESO::GslVector paramMaxs(paramSpace.zeroVector()); paramMins.cwSet(0.0); paramMaxs.cwSet(1.0); QUESO::BoxSubset<QUESO::GslVector, QUESO::GslMatrix> paramDomain("param_", paramSpace, paramMins, paramMaxs); QUESO::UniformVectorRV<QUESO::GslVector, QUESO::GslMatrix> priorRv("prior_", paramDomain); // Step 3: Instantiate the 'scenario' and 'output' spaces for simulation QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> configSpace(env, "scenario_", numConfigVars, NULL); QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> nEtaSpace(env, "output_", numEta, NULL); // Step 4: Instantiate the 'output' space for the experiments QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> experimentSpace(env, "experimentspace_", experimentSize, NULL); QUESO::VectorSpace<QUESO::GslVector, QUESO::GslMatrix> totalExperimentSpace(env, "experimentspace_", experimentSize * numExperiments, NULL); // Step 5: Instantiate the Gaussian process emulator object // // Regarding simulation scenario input values, the user should standardise // them so that they exist inside a hypercube. // // Regarding simulation output data, the user should transform it so that the // mean is zero and the variance is one. // // Regarding experimental scenario input values, the user should standardize // them so that they exist inside a hypercube. // // Regarding experimental data, the user should transformed it so that it has // zero mean and variance one. // GPMSA stores all the information about our simulation // data and experimental data. It also stores default information about the // hyperparameter distributions. QUESO::GPMSAFactory<QUESO::GslVector, QUESO::GslMatrix> gpmsaFactory(env, NULL, priorRv, configSpace, paramSpace, nEtaSpace, experimentSpace, numSimulations, numExperiments); // std::vector containing all the points in scenario space where we have // simulations std::vector<QUESO::GslVector *> simulationScenarios(numSimulations, (QUESO::GslVector *) NULL); // std::vector containing all the points in parameter space where we have // simulations std::vector<QUESO::GslVector *> paramVecs(numSimulations, (QUESO::GslVector *) NULL); // std::vector containing all the simulation output data std::vector<QUESO::GslVector *> outputVecs(numSimulations, (QUESO::GslVector *) NULL); // std::vector containing all the points in scenario space where we have // experiments std::vector<QUESO::GslVector *> experimentScenarios(numExperiments, (QUESO::GslVector *) NULL); // std::vector containing all the experimental output data std::vector<QUESO::GslVector *> experimentVecs(numExperiments, (QUESO::GslVector *) NULL); // The experimental output data observation error covariance matrix QUESO::GslMatrix experimentMat(totalExperimentSpace.zeroVector()); // Instantiate each of the simulation points/outputs for (unsigned int i = 0; i < numSimulations; i++) { simulationScenarios[i] = new QUESO::GslVector(configSpace.zeroVector()); // 'x_{i+1}^*' in paper paramVecs [i] = new QUESO::GslVector(paramSpace.zeroVector()); // 't_{i+1}^*' in paper outputVecs [i] = new QUESO::GslVector(nEtaSpace.zeroVector()); // 'eta_{i+1}' in paper } for (unsigned int i = 0; i < numExperiments; i++) { experimentScenarios[i] = new QUESO::GslVector(configSpace.zeroVector()); // 'x_{i+1}' in paper experimentVecs[i] = new QUESO::GslVector(experimentSpace.zeroVector()); } // Read in data and store the standard deviation of the simulation data. We // will need the standard deviation when we pass the experiment error // covariance matrix to QUESO. double stdsim = readData(simulationScenarios, paramVecs, outputVecs, experimentScenarios, experimentVecs); for (unsigned int i = 0; i < numExperiments; i++) { // Passing in error of experiments (standardised). experimentMat(i, i) = (0.025 / stdsim) * (0.025 / stdsim); } // Add simulation and experimental data gpmsaFactory.addSimulations(simulationScenarios, paramVecs, outputVecs); gpmsaFactory.addExperiments(experimentScenarios, experimentVecs, &experimentMat); QUESO::GenericVectorRV<QUESO::GslVector, QUESO::GslMatrix> postRv( "post_", gpmsaFactory.prior().imageSet().vectorSpace()); QUESO::StatisticalInverseProblem<QUESO::GslVector, QUESO::GslMatrix> ip("", NULL, gpmsaFactory, postRv); QUESO::GslVector paramInitials( gpmsaFactory.prior().imageSet().vectorSpace().zeroVector()); // Initial condition of the chain // Have to set each of these by hand, *and* the sampler is sensitive to these // values paramInitials[0] = 0.5; // param 1 paramInitials[1] = 0.5; // param 2 paramInitials[2] = 0.5; // param 3 paramInitials[3] = 0.5; // param 4 paramInitials[4] = 0.5; // param 5 paramInitials[5] = 0.4; // not used. emulator mean paramInitials[6] = 0.4; // emulator precision paramInitials[7] = 0.97; // emulator corr str paramInitials[8] = 0.97; // emulator corr str paramInitials[9] = 0.97; // emulator corr str paramInitials[10] = 0.97; // emulator corr str paramInitials[11] = 0.20; // emulator corr str paramInitials[12] = 0.80; // emulator corr str paramInitials[13] = 10.0; // discrepancy precision paramInitials[14] = 0.97; // discrepancy corr str paramInitials[15] = 8000.0; // emulator data precision QUESO::GslMatrix proposalCovMatrix( gpmsaFactory.prior().imageSet().vectorSpace().zeroVector()); // Setting the proposal covariance matrix by hand. This requires great // forethough, and can generally be referred to as a massive hack. These // values were taken from the gpmsa matlab code and fiddled with. double scale = 600.0; proposalCovMatrix(0, 0) = 3.1646 / 10.0; // param 1 proposalCovMatrix(1, 1) = 3.1341 / 10.0; // param 2 proposalCovMatrix(2, 2) = 3.1508 / 10.0; // param 3 proposalCovMatrix(3, 3) = 0.3757 / 10.0; // param 4 proposalCovMatrix(4, 4) = 0.6719 / 10.0; // param 5 proposalCovMatrix(5, 5) = 0.1 / scale; // not used. emulator mean proposalCovMatrix(6, 6) = 0.4953 / scale; // emulator precision proposalCovMatrix(7, 7) = 0.6058 / scale; // emulator corr str proposalCovMatrix(8, 8) = 7.6032e-04 / scale; // emulator corr str proposalCovMatrix(9, 9) = 8.3815e-04 / scale; // emulator corr str proposalCovMatrix(10, 10) = 7.5412e-04 / scale; // emulator corr str proposalCovMatrix(11, 11) = 0.2682 / scale; // emulator corr str proposalCovMatrix(12, 12) = 0.0572 / scale; // emulator corr str proposalCovMatrix(13, 13) = 1.3417 / scale; // discrepancy precision proposalCovMatrix(14, 14) = 0.3461 / scale; // discrepancy corr str proposalCovMatrix(15, 15) = 495.3 / scale; // emulator data precision // Square to get variances for (unsigned int i = 0; i < 16; i++) { proposalCovMatrix(i, i) = proposalCovMatrix(i, i) * proposalCovMatrix(i, i); } ip.solveWithBayesMetropolisHastings(NULL, paramInitials, &proposalCovMatrix); #ifdef QUESO_HAS_MPI MPI_Finalize(); #endif return 0; }
Vector Rosen34( Fun &F , size_t M , const Scalar &ti , const Scalar &tf , const Vector &xi , Vector &e ) { CPPAD_ASSERT_FIRST_CALL_NOT_PARALLEL; // check numeric type specifications CheckNumericType<Scalar>(); // check simple vector class specifications CheckSimpleVector<Scalar, Vector>(); // Parameters for Shampine's Rosenbrock method // are static to avoid recalculation on each call and // do not use Vector to avoid possible memory leak static Scalar a[3] = { Scalar(0), Scalar(1), Scalar(3) / Scalar(5) }; static Scalar b[2 * 2] = { Scalar(1), Scalar(0), Scalar(24) / Scalar(25), Scalar(3) / Scalar(25) }; static Scalar ct[4] = { Scalar(1) / Scalar(2), - Scalar(3) / Scalar(2), Scalar(121) / Scalar(50), Scalar(29) / Scalar(250) }; static Scalar cg[3 * 3] = { - Scalar(4), Scalar(0), Scalar(0), Scalar(186) / Scalar(25), Scalar(6) / Scalar(5), Scalar(0), - Scalar(56) / Scalar(125), - Scalar(27) / Scalar(125), - Scalar(1) / Scalar(5) }; static Scalar d3[3] = { Scalar(97) / Scalar(108), Scalar(11) / Scalar(72), Scalar(25) / Scalar(216) }; static Scalar d4[4] = { Scalar(19) / Scalar(18), Scalar(1) / Scalar(4), Scalar(25) / Scalar(216), Scalar(125) / Scalar(216) }; CPPAD_ASSERT_KNOWN( M >= 1, "Error in Rosen34: the number of steps is less than one" ); CPPAD_ASSERT_KNOWN( e.size() == xi.size(), "Error in Rosen34: size of e not equal to size of xi" ); size_t i, j, k, l, m; // indices size_t n = xi.size(); // number of components in X(t) Scalar ns = Scalar(double(M)); // number of steps as Scalar object Scalar h = (tf - ti) / ns; // step size Scalar zero = Scalar(0); // some constants Scalar one = Scalar(1); Scalar two = Scalar(2); // permutation vectors needed for LU factorization routine CppAD::vector<size_t> ip(n), jp(n); // vectors used to store values returned by F Vector E(n * n), Eg(n), f_t(n); Vector g(n * 3), x3(n), x4(n), xf(n), ftmp(n), xtmp(n), nan_vec(n); // initialize e = 0, nan_vec = nan for(i = 0; i < n; i++) { e[i] = zero; nan_vec[i] = nan(zero); } xf = xi; // initialize solution for(m = 0; m < M; m++) { // time at beginning of this interval Scalar t = ti * (Scalar(int(M - m)) / ns) + tf * (Scalar(int(m)) / ns); // value of x at beginning of this interval x3 = x4 = xf; // evaluate partial derivatives at beginning of this interval F.Ode_ind(t, xf, f_t); F.Ode_dep(t, xf, E); // E = f_x if( hasnan(f_t) || hasnan(E) ) { e = nan_vec; return nan_vec; } // E = I - f_x * h / 2 for(i = 0; i < n; i++) { for(j = 0; j < n; j++) E[i * n + j] = - E[i * n + j] * h / two; E[i * n + i] += one; } // LU factor the matrix E # ifndef NDEBUG int sign = LuFactor(ip, jp, E); # else LuFactor(ip, jp, E); # endif CPPAD_ASSERT_KNOWN( sign != 0, "Error in Rosen34: I - f_x * h / 2 not invertible" ); // loop over integration steps for(k = 0; k < 3; k++) { // set location for next function evaluation xtmp = xf; for(l = 0; l < k; l++) { // loop over previous function evaluations Scalar bkl = b[(k-1)*2 + l]; for(i = 0; i < n; i++) { // loop over elements of x xtmp[i] += bkl * g[i*3 + l] * h; } } // ftmp = F(t + a[k] * h, xtmp) F.Ode(t + a[k] * h, xtmp, ftmp); if( hasnan(ftmp) ) { e = nan_vec; return nan_vec; } // Form Eg for this integration step for(i = 0; i < n; i++) Eg[i] = ftmp[i] + ct[k] * f_t[i] * h; for(l = 0; l < k; l++) { for(i = 0; i < n; i++) Eg[i] += cg[(k-1)*3 + l] * g[i*3 + l]; } // Solve the equation E * g = Eg LuInvert(ip, jp, E, Eg); // save solution and advance x3, x4 for(i = 0; i < n; i++) { g[i*3 + k] = Eg[i]; x3[i] += h * d3[k] * Eg[i]; x4[i] += h * d4[k] * Eg[i]; } } // Form Eg for last update to x4 only for(i = 0; i < n; i++) Eg[i] = ftmp[i] + ct[3] * f_t[i] * h; for(l = 0; l < 3; l++) { for(i = 0; i < n; i++) Eg[i] += cg[2*3 + l] * g[i*3 + l]; } // Solve the equation E * g = Eg LuInvert(ip, jp, E, Eg); // advance x4 and accumulate error bound for(i = 0; i < n; i++) { x4[i] += h * d4[3] * Eg[i]; // cant use abs because cppad.hpp may not be included Scalar diff = x4[i] - x3[i]; if( diff < zero ) e[i] -= diff; else e[i] += diff; } // advance xf for this step using x4 xf = x4; } return xf; }
// {{{ critSectionRequest // Ask for critical section int critSectionRequest() { msg_t msg; type(msg) = REQUEST; char *ip_tmp = inet_ntoa(this_site.neighbours[0].sin_addr); strncpy (ask(msg), ip_tmp, IPLONG); // If already in SC just return -1 if (state != IDLE) { if (_verbose) fprintf (stderr,"Already in critical section.\n"); return -1; } state = WAITING; // Owns Token ? if(tokenPresent) return takeCriticalSection(); // Sends request to last else if(last != -1) { // Get ip of last char *tmpter = getIPstrFromNb (last); if (_verbose) fprintf (stdout, "Last is %d, sending it a request.\n", last); strncpy(ip(msg), tmpter, IPLONG); free (tmpter); strncpy (ask(msg), inet_ntoa(this_site.neighbours[0].sin_addr), IPLONG); if(sendMessageWithAdd(msg) == -1){ if (_verbose) fprintf (stderr, "======> Sending request failure... <======\n"); return -1; } // Arms a timer time_t timeStart, timeCur; int flags = fcntl(this_site.sdRecv, F_GETFL); int flags2 = flags | O_NONBLOCK; fcntl(this_site.sdRecv, F_SETFL, flags2); timeStart = time(&timeStart); timeCur = time(&timeCur); // Wait for an answer if (_verbose) fprintf (stdout, "Waiting for COMMIT.\n"); while(timeCur - timeStart < (2*TMESG)) { timeCur = time(&timeCur); if(recvMessage(&msg, NULL) == -1) continue; if (type(msg) == COMMIT) return handleCommit(msg); else { // TODO: Work on reaction when receiving request while waiting commit if (type(msg) == REQUEST) { if (_verbose) fprintf (stdout, "Received a REQUEST instead of a COMMIT.\n"); } else if (type(msg) == TOKEN){ last = -1; return handleToken(msg); } else handleMessage(msg); } } // If receive no answers if (_verbose) fprintf (stdout, "I didn't get any COMMIT, looking for the queue...\n"); fcntl(this_site.sdRecv, F_SETFL, flags); memset (&msg, 0, SIZE); type(msg) = SEARCH_QUEUE; nb_acc(msg) = acces; // Broadcast Search_Queue if (_verbose) fprintf (stdout, "Broadcasting SEARCH_QUEUE.\n"); broadcast(msg); msg_t max; pos(max) = -1; fcntl(this_site.sdRecv, F_SETFL, flags2); timeStart = time(&timeStart); timeCur = time(&timeCur); // Wait for ACK_SEARCH_QUEUE if (_verbose) fprintf (stdout, "Expecting ACK_SEARCH_QUEUE.\n"); while(timeCur - timeStart < 2*TMESG) { timeCur = time(&timeCur); memset (&msg, 0, SIZE); if(recvMessage(&msg, NULL) == -1) continue; switch(type(msg)) { case ACK_SEARCH_QUEUE: if (_verbose) fprintf (stdout, "Got an ACK_SEARCH_QUEUE.\n"); // Save greatest position in the queue if (pos(msg) > pos(max)) { if (_verbose) fprintf (stdout, "New position in QUEUE : %d.\n", pos(msg)); memcpy(&max, &msg, SIZE); } break; // Another site discovers the failure case SEARCH_QUEUE: if (isMe(ips(msg))) continue; if (_verbose) fprintf (stdout, "Another site discovered the failure.\n"); // The other site hasn't priority, just continue if (nb_acc(msg) > acces) continue; // Both of sites have the same numbers of access, defines priority with ip if (nb_acc(msg) == acces) { char *ip_pers = inet_ntoa(this_site.neighbours[0].sin_addr); if (strcmp(ip_pers, ips(msg)) <= 0) continue; } // If the other has the priority, just change last and ask it for CS unsigned long int ipa = (unsigned long int) inet_addr(ips(msg)); last = getNeighbour(ipa); if (_verbose) fprintf (stdout, "I don't have priority, changing my last (%d) and asking for CS again.\n", last); state = IDLE; return critSectionRequest(); // TODO: Request processing case REQUEST: break; default: handleMessage(msg); break; } } if (_verbose) fprintf (stdout, "Waiting time passed.\n"); fcntl(this_site.sdRecv, F_SETFL, flags); // No other site in the queue, juste regenerate TOKEN if (pos(max) < 0) { if (_verbose) fprintf (stdout, "No other sites in the queue, regenerate TOKEN (last = next)\n"); last = next; tokenPresent = 1; takeCriticalSection(); } else { // Ask for connection to the site with the highest position unsigned long int ipa = (unsigned long int) inet_addr(ips(max)); last = getNeighbour(ipa); if (_verbose) fprintf (stdout, "Ask for Connection, new last = %d\n", last); strncpy(ip(msg), ips(max),IPLONG *sizeof(char)); if (next(max)) { type(msg) = CONNECTION; if (sendMessageWithAdd(msg) == -1) return -1; } state = IDLE; critSectionRequest(); } } else if (_verbose) fprintf (stderr, "======> Last = -1, but don't have TOKEN <======\n"); return 0; }