Ejemplo n.º 1
0
int P2PAccept(int Sockfd, struct sockaddr_in Dest, void(*Function) (void*), void* Arg){
    if(List != NULL){                                               //  如果链表为空
        LinkC_Debug("P2PAccept:LinkC Socket环境没有初始化",LINKC_FAILURE);
        return LINKC_FAILURE;                                       //  返回错误
    }
    LinkC_Socket *Socket = NULL;
    if(IsSocketInList(Sockfd,&Socket)==0){
        LinkC_Debug("P2PAccept:没有这个套接字",LINKC_FAILURE);
        return -1;
    }
    EmptyPackageList(Socket->RecvList);         //  清空发送和接收缓冲区
    EmptyPackageList(Socket->SendList); 
    SetDestAddr(Sockfd,Dest);
    int Length = _LCUDP_Package(NULL,0,Socket,CONNECTION_MESSAGE,Socket->SendBuffer);
    ___LinkC_Send(Socket,Socket->SendBuffer,Length,MSG_DONTWAIT);
    if(Function != NULL){                                   //  执行函数,这里是我发送了无用信息后向服务端确认
        Function(Arg);
    }
    Accept(Sockfd,Dest);
    
    return 0;
}
void HML_GoldenSectionOptimization (double Left, double Right, double (*Function)(double), double Interval, double *VHML_Result_X,double *VHML_Result_Y)
{
/*
Метод золотого сечения. Метод одномерной оптимизации унимодальной функции на интервале. Ищет минимум.
Входные параметры:
 Left - начало интервала поиска;
 b - конец интервала поиска;
 Function - унимодальная функция, минимум которой ищется;
 Interval - длина конечного интервала неопределенности (точность поиска);
 VHML_Result_X - вычисленная точка минимума (сюда записывается результат);
 VHML_Result_Y - значение функции в точке минимума (сюда записывается результат).
Возвращаемое значение: 
 Отсутствует.
*/
double l,v,fl,fv;
l=Left+(1.-HML_TAU)*(Right-Left);
v=Left+HML_TAU*(Right-Left);
fl=Function(l);
fv=Function(v);
while (fabs(Right-Left)>=Interval)
 {
 if (Function(l)<Function(v))
  {
  Right=v;
  v=l;
  fv=fl;
  l=Left+(1.-HML_TAU)*(Right-Left);
  fl=Function(l);
  }
 else
  {
  Left=l;
  l=v;
  fl=fv;
  v=Left+HML_TAU*(Right-Left);
  fv=Function(v);
  }
 }
*VHML_Result_X=(Left+Right)/2.;
*VHML_Result_Y=Function(*VHML_Result_X);
}
Ejemplo n.º 3
0
double
image_to_pl_function(const cimg_library::CImg<double> &image,
		     T &t,
		     std::map<typename T::Face_handle, Function> &fs)
{
  typedef typename T::Point Point;
  std::vector<Point> grid;
  std::map<Point, double> fgrid;
  size_t n = image.width();
  size_t m = image.height();
  double dx = 2/double(n-1), x0=-1.0;
  double dy = 2/double(m-1), y0=-1.0;
  for (size_t i = 0; i < n; ++i)
    {
      for (size_t j = 0; j < m; ++j)
	{
	  Point p(x0 + i * dx,
		  y0 + j * dy);
	  grid.push_back(p);
	  fgrid[p] = image(i,m-j-1)/double(255) + 1e-3;
	}
    }
  t = T(grid.begin(), grid.end());

  double tot_orig(0);
  for (auto f = t.finite_faces_begin(); 
       f != t.finite_faces_end(); ++f)
    {
      Point p = f->vertex(0)->point(), 
	q = f->vertex(1)->point(), 
	r = f->vertex(2)->point();
      fs[f] = Function(p, fgrid[p], 
		       q, fgrid[q],
		       r, fgrid[r]);
      tot_orig += CGAL::to_double(MA::integrate_centroid(p,q,r, fs[f]));
    }
  return tot_orig;
}
Ejemplo n.º 4
0
EXPORT_C TInt TFsPluginRequest::FileName(TDes& aName)
	{
	//Special handling required for directories.
	switch(Function())
		{
		case EFsDirOpen:
		case EFsSetEntry:
			{
			aName.Copy(Request()->Src().FullName());
			break;
			}
		case EFsDirReadOne:
		case EFsDirReadPacked:
		case EFsDirSubClose:
			{
			//Get the name from CDirCB::iName
			CDirCB* dir = (CDirCB*) ScratchValue();
			__ASSERT_ALWAYS(dir!= NULL, Fault(EPluginOpError));
			TName name = dir->Name();
			if(name.Size() == 0)
				{
				return KErrNotFound;
				}
			aName.Copy(name);
			break;
			}
		default:
			{
			CFileShare* share;
			TInt err = ShareFromClientHandle(share);
			if(err != KErrNone || share == NULL)
				return(err);
			
			NameFromShare(*share, aName);
			}
		}
	return KErrNone;
	}
Ejemplo n.º 5
0
typename
std::enable_if<
	Index
	==
	Size,
	mizuiro::array<
		typename
		std::result_of<
			Function(
				mizuiro::mpl::integral_size<
					0
				>
			)
		>::type,
		Size
	>
>::type
array_init_each_impl(
	Function const &,
	Args const & ..._args
)
{
	return
		mizuiro::array<
			typename
			std::result_of<
				Function(
					mizuiro::mpl::integral_size<
						0
					>
				)
			>::type,
			Size
		>{{
			_args...
		}};
}
Ejemplo n.º 6
0
 TermFactory::TermFactory() : ConstructionFactory<Term*>("Term") {
     registerConstructor("", fl::null);
     registerConstructor(Bell().className(), &(Bell::constructor));
     registerConstructor(Binary().className(), &(Binary::constructor));
     registerConstructor(Concave().className(), &(Concave::constructor));
     registerConstructor(Constant().className(), &(Constant::constructor));
     registerConstructor(Cosine().className(), &(Cosine::constructor));
     registerConstructor(Discrete().className(), &(Discrete::constructor));
     registerConstructor(Function().className(), &(Function::constructor));
     registerConstructor(Gaussian().className(), &(Gaussian::constructor));
     registerConstructor(GaussianProduct().className(), &(GaussianProduct::constructor));
     registerConstructor(Linear().className(), &(Linear::constructor));
     registerConstructor(PiShape().className(), &(PiShape::constructor));
     registerConstructor(Ramp().className(), &(Ramp::constructor));
     registerConstructor(Rectangle().className(), &(Rectangle::constructor));
     registerConstructor(SShape().className(), &(SShape::constructor));
     registerConstructor(Sigmoid().className(), &(Sigmoid::constructor));
     registerConstructor(SigmoidDifference().className(), &(SigmoidDifference::constructor));
     registerConstructor(SigmoidProduct().className(), &(SigmoidProduct::constructor));
     registerConstructor(Spike().className(), &(Spike::constructor));
     registerConstructor(Trapezoid().className(), &(Trapezoid::constructor));
     registerConstructor(Triangle().className(), &(Triangle::constructor));
     registerConstructor(ZShape().className(), &(ZShape::constructor));
 }
// @param RunCount should be around 10 but can be adjusted for precision 
// @param Function should run for about 3 ms
static FTimeSample RunBenchmark(float WorkScale, float (*Function)())
{
	float Sum = 0;

	// this test doesn't support fractional WorkScale
	uint32 RunCount = FMath::Max((int32)1, (int32)WorkScale);

	for(uint32 i = 0; i < RunCount; ++i)
	{
		FPlatformMisc::MemoryBarrier();
		// todo: compiler reorder might be an issue, use _ReadWriteBarrier or something like http://svn.openimageio.org/oiio/tags/Release-0.6.3/src/include/tbb/machine/windows_em64t.h
		const double StartTime = FPlatformTime::Seconds();

		FPlatformMisc::MemoryBarrier();

		GGlobalStateObject += Function();

		FPlatformMisc::MemoryBarrier();
		Sum += (float)(FPlatformTime::Seconds() - StartTime);
		FPlatformMisc::MemoryBarrier();
	}
	
	return FTimeSample(Sum, Sum / RunCount);
}
Ejemplo n.º 8
0
ScriptContext::ScriptContext() : m_VariableIndex(0) , m_DataIndex(0) {
	m_Functions.push_back(Function("+",OP_ADD,12,2,scriptAddOperation));
	m_Functions.push_back(Function("*",OP_MUL,16,2,scriptMul));
	//m_Functions.push_back(Function("/",OP_DIV,16,2));
	//m_Functions.push_back(Function("-",OP_SUB,12,2));
	//m_Functions.push_back(Function("sin",OP_SIN,17,1));
	//m_Functions.push_back(Function("cos",OP_COS,17,1));
	m_Functions.push_back(Function("=",OP_ASSIGN,1,1,scriptAssign));
	m_Functions.push_back(Function("random",OP_RND,20,2,scriptRandom));
	m_Functions.push_back(Function("print",OP_RND,19,1,scriptPrint));
	m_Functions.push_back(Function("vector2",OP_NEW_VEC2,20,2,scriptNewVec2));

	m_Constants.push_back(ConstantValue("PI",3.14159265359f));

	m_Declarations.push_back(TypeDeclaration("int",DT_INT));
	m_Declarations.push_back(TypeDeclaration("float",DT_FLOAT));
	m_Declarations.push_back(TypeDeclaration("vec2",DT_VEC2));
	m_Declarations.push_back(TypeDeclaration("vec3",DT_VEC3));
	m_Declarations.push_back(TypeDeclaration("color",DT_COLOR));
}
Ejemplo n.º 9
0
bool Origin410Parser::parse()
{
	unsigned int dataIndex = 0;

#ifndef NO_LOG_FILE
	// append progress in log file
	logfile = fopen("opjfile.log", "a");
#endif

	/////////////////// find column ///////////////////////////////////////////////////////////
	skipLine();
	unsigned int size;
	file >> size;
	file.seekg(1 + size + 1 + 5, ios_base::cur);

	file >> size;
	file.seekg(1, ios_base::cur);
	LOG_PRINT(logfile, "	[column found = %d/0x%X @ 0x%X]\n", size, size, (unsigned int) file.tellg());

	unsigned int colpos = file.tellg();
	unsigned int current_col = 1, nr = 0, nbytes = 0;

	while(size > 0 && size <= 0x8B){// should be 0x72, 0x73 or 0x83 ?
		//////////////////////////////// COLUMN HEADER /////////////////////////////////////////////

		short data_type;
		char data_type_u;
		unsigned int oldpos = (unsigned int)file.tellg();

		file.seekg(oldpos + 0x16, ios_base::beg);
		file >> data_type;

		file.seekg(oldpos + 0x3F, ios_base::beg);
		file >> data_type_u;

		char valuesize;
		file.seekg(oldpos + 0x3D, ios_base::beg);
		file >> valuesize;

		LOG_PRINT(logfile, "	[valuesize = %d @ 0x%X]\n", (int)valuesize, ((unsigned int) file.tellg()-1));
		if(valuesize <= 0)
		{
			LOG_PRINT(logfile, "	WARNING : found strange valuesize of %d\n", (int)valuesize);
			valuesize = 10;
		}

		file.seekg(oldpos + 0x58, ios_base::beg);
		LOG_PRINT(logfile, "	[Spreadsheet @ 0x%X]\n", (unsigned int) file.tellg());

		string name(25, 0);
		file >> name;

		string::size_type pos = name.find_last_of("_");
		string columnname;
		if(pos != string::npos){
			columnname = name.substr(pos + 1);
			name.resize(pos);
		}

		LOG_PRINT(logfile, "	NAME: %s\n", name.c_str());

		unsigned int spread = 0;
		pos = name.find("DPk");//multipeak fits generate these tables
		if(columnname.empty() && pos == string::npos){
			LOG_PRINT(logfile, "NO COLUMN NAME FOUND! Must be a Matrix or Function.");
			////////////////////////////// READ matrices or functions ////////////////////////////////////
			LOG_PRINT(logfile, "	[position @ 0x%X]\n", (unsigned int) file.tellg());

			file.seekg(oldpos + 0x18, ios_base::beg);
			short signature;
			file >> signature;
			LOG_PRINT(logfile, "	SIGNATURE : %02X @ 0x%X\n", signature, (unsigned int) file.tellg());

			file.seekg(oldpos + size + 1, ios_base::beg);
			file >> size;

			file.seekg(1, ios_base::cur);
			size /= valuesize;
			LOG_PRINT(logfile, "	SIZE = %d\n", size);

			if (size == 1){
				functions.push_back(Function(name, dataIndex));
				++dataIndex;
				readFunction(colpos, valuesize, &oldpos);
			} else if (size > 1){
				if (signature == 1){
					matrices.push_back(Matrix(name));
					matrices.back().sheets.push_back(MatrixSheet(name, dataIndex));
					++dataIndex;
					readMatrixValues(data_type, data_type_u, valuesize, size);
				} else {
					LOG_PRINT(logfile, "UNKNOWN SIGNATURE, SKIP DATA\n");
					file.seekg(valuesize*size, ios_base::cur);
					++dataIndex;

					if(valuesize != 8 && valuesize <= 16)
						file.seekg(2, ios_base::cur);
				}
			}
		}
		else
		{	// worksheet
			if(speadSheets.size() == 0 || findSpreadByName(name) == -1)
Ejemplo n.º 10
0
/**
  Initialize debug agent.

  This function is used to set up debug environment for SEC and PEI phase.

  If InitFlag is DEBUG_AGENT_INIT_PREMEM_SEC, it will overirde IDT table entries
  and initialize debug port. It will enable interrupt to support break-in feature.
  It will set up debug agent Mailbox in cache-as-ramfrom. It will be called before
  physical memory is ready.
  If InitFlag is DEBUG_AGENT_INIT_POSTMEM_SEC, debug agent will build one GUIDed
  HOB to copy debug agent Mailbox. It will be called after physical memory is ready.

  This function is used to set up debug environment to support source level debugging.
  If certain Debug Agent Library instance has to save some private data in the stack,
  this function must work on the mode that doesn't return to the caller, then
  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one
  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is
  responsible to invoke the passing-in function at the end of InitializeDebugAgent().

  If the parameter Function is not NULL, Debug Agent Libary instance will invoke it by
  passing in the Context to be its parameter.

  If Function() is NULL, Debug Agent Library instance will return after setup debug
  environment.

  @param[in] InitFlag     Init flag is used to decide the initialize process.
  @param[in] Context      Context needed according to InitFlag; it was optional.
  @param[in] Function     Continue function called by debug agent library; it was
                          optional.

**/
VOID
EFIAPI
InitializeDebugAgent (
    IN UINT32                InitFlag,
    IN VOID                  *Context, OPTIONAL
    IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
)
{
    DEBUG_AGENT_MAILBOX              *Mailbox;
    DEBUG_AGENT_MAILBOX              MailboxInStack;
    DEBUG_AGENT_PHASE2_CONTEXT       Phase2Context;
    DEBUG_AGENT_CONTEXT_POSTMEM_SEC  *DebugAgentContext;

    DisableInterrupts ();

    switch (InitFlag) {

    case DEBUG_AGENT_INIT_PREMEM_SEC:

        InitializeDebugIdt ();

        Mailbox = &MailboxInStack;
        ZeroMem ((VOID *) Mailbox, sizeof (DEBUG_AGENT_MAILBOX));

        //
        // Get and save debug port handle and set the length of memory block.
        //
        SetMailboxPointerInIdtEntry ((VOID *) Mailbox);

        InitializeDebugTimer ();

        Phase2Context.Context  = Context;
        Phase2Context.Function = Function;
        DebugPortInitialize ((VOID *) &Phase2Context, InitializeDebugAgentPhase2);

        //
        // If reaches here, it means Debug Port initialization failed.
        //
        DEBUG ((EFI_D_ERROR, "Debug Agent: Debug port initialization failed.\n"));

        break;

    case DEBUG_AGENT_INIT_POSTMEM_SEC:

        //
        // Memory has been ready
        //
        if (IsHostConnected()) {
            //
            // Trigger one software interrupt to inform HOST
            //
            TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
        }

        DebugAgentContext = (DEBUG_AGENT_CONTEXT_POSTMEM_SEC *) Context;

        Mailbox = (DEBUG_AGENT_MAILBOX *) GetMailboxPointerInIdtEntry ();
        Mailbox->DebugPortHandle = Mailbox->DebugPortHandle + DebugAgentContext->StackMigrateOffset;

        Mailbox = BuildMailboxHob ();
        Mailbox = (DEBUG_AGENT_MAILBOX *) ((UINTN) Mailbox + DebugAgentContext->HeapMigrateOffset);

        SetMailboxPointerInIdtEntry ((VOID *) Mailbox);

        EnableInterrupts ();

        break;

    default:

        //
        // Only DEBUG_AGENT_INIT_PREMEM_SEC and DEBUG_AGENT_INIT_POSTMEM_SEC are allowed for this
        // Debug Agent library instance.
        //
        DEBUG ((EFI_D_ERROR, "Debug Agent: The InitFlag value is not allowed!\n"));
        CpuDeadLoop ();
        break;

    }

    //
    // If Function is not NULL, invoke it always whatever debug agent was initialized sucesssfully or not.
    //
    if (Function != NULL) {
        Function (Context);
    }
}
Ejemplo n.º 11
0
/* Common function to test asserts safely (i.e without aborting) */
SNM_TEST_RESULT snm_test_debug_safe_assert(void(*Function)(void),
                                           SNM_SAFE_ASSERT ExpectedResult,
                                           const char *ExpectedString) {
    int Status;     /* Exit status of the child */
    int Pipe[2];    /* Pipe from child to parent, STDOUT redirects to this */
    FILE *Read;     /* File handle used to read from the pipe */
    char *String;   /* Pointer to string (if any) captured from the child */
    char Buffer[SNM_DEBUG_BUFFER_SIZE] = { '\0' };

    /* Create a pipe to capture STDOUT from child and pipe it to the parent */
    if(pipe(&Pipe[0]) != 0) {
        snm_debug_critical("Failed to create pipe", NULL);
        return SNM_TEST_INCONCLUSIVE;
    }

    /* Create a child process to use for asserts */
    switch(fork()) {
        case -1:
            /* Problem creating the child process */
            snm_debug_critical("Failed to fork child process", NULL);
            return SNM_TEST_INCONCLUSIVE;
        case 0:
            /*
             * This is the child process (fork returns 0 to the child)
             */
            snm_debug_trivial("Child: forked successfully", NULL);

            /* Redirect STDOUT to the pipe */
            if(dup2(Pipe[1], STDOUT_FILENO) != STDOUT_FILENO) {
                snm_debug_critical("Failed to redirect STDOUT=>pipe, error %i",
                                   errno);
            }

            /* Close unused file descriptors */
            if( !( (close(Pipe[0]) == 0) &&
                   (close(Pipe[1]) == 0)    ) ) {
                return SNM_TEST_INCONCLUSIVE;
            }

            /*
             * So now any debug or assert that writes to STDOUT will
             * actually be redirected through the pipe to the parent
             */

            /* Perform the assert */
            if(Function != NULL) {
                Function();
            }

            /* Exit normally, if we got this far (assert test was true) */
            exit(EXIT_SUCCESS);
            break;
        default:
            /*
             * This is the parent process (fork returns child pid to parent)
             */
            snm_debug_trivial("Parent: forked successfully", NULL);

            /* Use the pipe for reading */
            if((Read = fdopen(Pipe[0], "r")) == NULL) {
                snm_debug_critical("Parent: failed to read from pipe, error %i",
                                   errno);
                return SNM_TEST_INCONCLUSIVE;
            }

            /* Close unused file descriptors */
            if(close(Pipe[1]) != 0) {
                snm_debug_critical("Parent: failed to close Pipe[1], error %i",
                                   errno);
                return SNM_TEST_INCONCLUSIVE;
            }

            /* Read assert text (if any) from the pipe */
            String = fgets(Buffer, SNM_DEBUG_BUFFER_SIZE, Read);

            /* Wait for the child process to terminate */
            wait(&Status);

            /* Close the pipe */
            if(fclose(Read) != 0) {
                snm_debug_critical("Parent: failed to close Read, error %i",
                                   errno);
                return SNM_TEST_INCONCLUSIVE;
            }
            break;
    }

    /*
     * This code is only executed by the parent process
     */
    snm_debug_trivial("Returned from fork, only parent executing this", NULL);

    /* Did the child process terminate normally, with no assert? */
    if(WIFEXITED(Status) && (EXIT_SUCCESS == WEXITSTATUS(Status))) {
        snm_debug_trivial("Child process terminated normally, no assert", NULL);
        return (SNM_SAFE_ASSERT_NO_ASSERT == ExpectedResult) ? SNM_TEST_PASS
                                                             : SNM_TEST_FAIL;
    }

    /* Did the child process abort due to an assert? */
    if(WIFSIGNALED(Status) && (SIGABRT == WTERMSIG(Status))) {
        snm_debug_trivial("Child process aborted, with an assert", NULL);

        /* We had an assert, but were we expecting one? */
        if(SNM_SAFE_ASSERT_ASSERTED != ExpectedResult) {
                snm_debug_trivial("FAIL! Asserted, but not expected", NULL);
                return SNM_TEST_FAIL;
        }

        if((NULL == ExpectedString) && (NULL == String)) {
            /* Pass if no assert text expected, and none received */
            snm_debug_trivial("PASS! Asserted, no text expected or received",
                              NULL);
            return SNM_TEST_PASS;
        }
        else if((NULL != ExpectedString) && (NULL != String)) {
            /* Pass if assert text was expected, and we got a match */
            if(NULL != strstr(String, ExpectedString)) {
                /* The string exactly matches */
                snm_debug_trivial("PASS! Asserted, text matches", NULL);
                return SNM_TEST_PASS;
            }
            else if(strlen(String) == (SNM_DEBUG_BUFFER_SIZE -1)) {
                /* No exact match, but max length - it got truncated */
                snm_debug_trivial("PASS! Asserted, text truncated", NULL);
                return SNM_TEST_PASS;
            }
        }
        else {
            snm_debug_trivial("FAIL! Asserted, but text mismatch", NULL);
            return SNM_TEST_FAIL;
        }
    }

    /* If we get this far, then something unexpected must have happened */
    snm_debug_important("Something unexpected happened", NULL);
    if(WIFEXITED(Status)) {
        snm_debug_important("Child exited with %i", WEXITSTATUS(Status));
    }
    else if(WIFSIGNALED(Status)) {
        snm_debug_important("Child terminated upon signal %i",WTERMSIG(Status));
    }
    else if(WIFSTOPPED(Status)) {
        snm_debug_important("Child stopped upon signal %i", WSTOPSIG(Status));
    }
    return SNM_TEST_INCONCLUSIVE;
}
Ejemplo n.º 12
0
/*****************************************************************************
  Function name: RootBrent()

  Purpose      : Calculate the surface temperature in the absence of snow

  Required     :
    int y                 - Row number of current pixel
    int x                 - Column number of current pixel
    float LowerBound      - Lower bound for root
    float UpperBound      - Upper bound for root
    float (*Function)(float Estimate, va_list ap)
    ...                   - Variable arguments
                            The number and order of arguments has to be
                            appropriate for the Function pointed to, since
                            the list of arguments after Nargs will be passed
                            on to Function.
                            See the appropriate Function for the correct
                            arguments.

  Returns      :
    float b               - Effective surface temperature (C)

  Modifies     : none

  Comments     :
*****************************************************************************/
float RootBrent(int y, int x, float LowerBound, float UpperBound,
                float (*Function) (float Estimate, va_list ap), ...)
{
    const char *Routine = "RootBrent";
    char ErrorString[MAXSTRING + 1];
    va_list ap;			/* Used in traversing variable argument list
				 */
    float a;
    float b;
    float c=0;
    float d=0;
    float e=0;
    float fa;
    float fb;
    float fc;
    float m;
    float p;
    float q;
    float r;
    float s;
    float tol;
    int i;
    int j;
    int eval = 0;

    sprintf(ErrorString, "%s: y = %d, x = %d", Routine, y, x);

    /* initialize variable argument list */
    a = LowerBound;
    b = UpperBound;;
    va_start(ap, Function);
    fa = Function(a, ap);
    eval++;
    va_start(ap, Function);
    fb = Function(b, ap);
    eval++;

    /*  if root not bracketed attempt to bracket the root */

    j = 0;
    while ((fa * fb) >= 0 && j < MAXTRIES) {
        a -= TSTEP;
        b += TSTEP;
        va_start(ap, Function);
        fa = Function(a, ap);
        eval++;
        va_start(ap, Function);
        fb = Function(b, ap);
        eval++;
        j++;
    }
    if ((fa * fb) >= 0) {
        // printf("Error in RootBrent%s: y = %d, x = %d, tries = %d,\na:b = %f:%f  fa:fb = %f:%f\n", Routine, y, x, j, a,b,fa,fb);
        //  ReportError(ErrorString, 34);
        return(-99);
    }
    fc = fb;

    for (i = 0; i < MAXITER; i++) {

        if (fb * fc > 0) {
            c = a;
            fc = fa;
            d = b - a;
            e = d;
        }

        if (fabs(fc) < fabs(fb)) {
            a = b;
            b = c;
            c = a;
            fa = fb;
            fb = fc;
            fc = fa;
        }

        tol = 2 * MACHEPS * fabs(b) + T;
        m = 0.5 * (c - b);

        if (fabs(m) <= tol || fequal(fb, 0.0)) {
            va_end(ap);
            return b;
        }

        else {
            if (fabs(e) < tol || fabs(fa) <= fabs(fb)) {
                d = m;
                e = d;
            }
            else {
                s = fb / fa;

                if (fequal(a, c)) {

                    /* linear interpolation */

                    p = 2 * m * s;
                    q = 1 - s;
                }

                else {

                    /* inverse quadratic interpolation */

                    q = fa / fc;
                    r = fb / fc;
                    p = s * (2 * m * q * (q - r) - (b - a) * (r - 1));
                    q = (q - 1) * (r - 1) * (s - 1);
                }

                if (p > 0)
                    q = -q;
                else
                    p = -p;
                s = e;
                e = d;
                if ((2 * p) < (3 * m * q - fabs(tol * q)) && p < fabs(0.5 * s * q))
                    d = p / q;
                else {
                    d = m;
                    e = d;
                }
            }
            a = b;
            fa = fb;
            b += (fabs(d) > tol) ? d : ((m > 0) ? tol : -tol);
            va_start(ap, Function);
            fb = Function(b, ap);
            eval++;
        }
    }
    // printf("Error in RootBrent%s: y = %d, x = %d, tries = %d,\na:b = %f:%f  fa:fb = %f:%f", Routine, y, x, j, a,b,fa,fb);
    //  ReportError(ErrorString, 33);
    return(-99);
}
Ejemplo n.º 13
0
RTF::Reader::Reader()
	: m_in_block(true),
	m_codec(0)
{
	if (functions.isEmpty()) {
		functions["\\"] = Function(&Reader::insertSymbol, '\\');
		functions["_"] = Function(&Reader::insertSymbol, 0x2011);
		functions["{"] = Function(&Reader::insertSymbol, '{');
		functions["|"] = Function(&Reader::insertSymbol, 0x00b7);
		functions["}"] = Function(&Reader::insertSymbol, '}');
		functions["~"] = Function(&Reader::insertSymbol, 0x00a0);
		functions["-"] = Function(&Reader::insertSymbol, 0x00ad);

		functions["bullet"] = Function(&Reader::insertSymbol, 0x2022);
		functions["emdash"] = Function(&Reader::insertSymbol, 0x2014);
		functions["emspace"] = Function(&Reader::insertSymbol, 0x2003);
		functions["endash"] = Function(&Reader::insertSymbol, 0x2013);
		functions["enspace"] = Function(&Reader::insertSymbol, 0x2002);
		functions["ldblquote"] = Function(&Reader::insertSymbol, 0x201c);
		functions["lquote"] = Function(&Reader::insertSymbol, 0x2018);
		functions["line"] = Function(&Reader::insertSymbol, 0x000a);
		functions["ltrmark"] = Function(&Reader::insertSymbol, 0x200e);
		functions["qmspace"] = Function(&Reader::insertSymbol, 0x2004);
		functions["rdblquote"] = Function(&Reader::insertSymbol, 0x201d);
		functions["rquote"] = Function(&Reader::insertSymbol, 0x2019);
		functions["rtlmark"] = Function(&Reader::insertSymbol, 0x200f);
		functions["tab"] = Function(&Reader::insertSymbol, 0x0009);
		functions["zwj"] = Function(&Reader::insertSymbol, 0x200d);
		functions["zwnj"] = Function(&Reader::insertSymbol, 0x200c);

		functions["\'"] = Function(&Reader::insertHexSymbol);
		functions["u"] = Function(&Reader::insertUnicodeSymbol);
		functions["uc"] = Function(&Reader::setSkipCharacters);
		functions["par"] = Function(&Reader::endBlock);
		functions["\n"] = Function(&Reader::insertBlock);
		functions["\r"] = Function(&Reader::insertBlock);

		functions["pard"] = Function(&Reader::resetBlockFormatting);
		functions["plain"] = Function(&Reader::resetTextFormatting);

		functions["qc"] = Function(&Reader::setBlockAlignment, Qt::AlignHCenter);
		functions["qj"] = Function(&Reader::setBlockAlignment, Qt::AlignJustify);
		functions["ql"] = Function(&Reader::setBlockAlignment, Qt::AlignLeft | Qt::AlignAbsolute);
		functions["qr"] = Function(&Reader::setBlockAlignment, Qt::AlignRight | Qt::AlignAbsolute);

		functions["li"] = Function(&Reader::setBlockIndent);

		functions["ltrpar"] = Function(&Reader::setBlockDirection, Qt::LeftToRight);
		functions["rtlpar"] = Function(&Reader::setBlockDirection, Qt::RightToLeft);

		functions["b"] = Function(&Reader::setTextBold, true);
		functions["i"] = Function(&Reader::setTextItalic, true);
		functions["strike"] = Function(&Reader::setTextStrikeOut, true);
		functions["striked"] = Function(&Reader::setTextStrikeOut, true);
		functions["ul"] = Function(&Reader::setTextUnderline, true);
		functions["uld"] = Function(&Reader::setTextUnderline, true);
		functions["uldash"] = Function(&Reader::setTextUnderline, true);
		functions["uldashd"] = Function(&Reader::setTextUnderline, true);
		functions["uldb"] = Function(&Reader::setTextUnderline, true);
		functions["ulnone"] = Function(&Reader::setTextUnderline, false);
		functions["ulth"] = Function(&Reader::setTextUnderline, true);
		functions["ulw"] = Function(&Reader::setTextUnderline, true);
		functions["ulwave"] = Function(&Reader::setTextUnderline, true);
		functions["ulhwave"] = Function(&Reader::setTextUnderline, true);
		functions["ululdbwave"] = Function(&Reader::setTextUnderline, true);

		functions["sub"] = Function(&Reader::setTextVerticalAlignment, QTextCharFormat::AlignSubScript);
		functions["super"] = Function(&Reader::setTextVerticalAlignment, QTextCharFormat::AlignSuperScript);
		functions["nosupersub"] = Function(&Reader::setTextVerticalAlignment, QTextCharFormat::AlignNormal);

		functions["ansicpg"] = Function(&Reader::setCodepage);
		functions["ansi"] = Function(&Reader::setCodepage, 1252);
		functions["mac"] = Function(&Reader::setCodepageMac);
		functions["pc"] = Function(&Reader::setCodepage, 850);
		functions["pca"] = Function(&Reader::setCodepage, 850);

		functions["deff"] = Function(&Reader::setFont);
		functions["f"] = Function(&Reader::setFont);
		functions["cpg"] = Function(&Reader::setFontCodepage);
		functions["fcharset"] = Function(&Reader::setFontCharset);

		functions["filetbl"] = Function(&Reader::ignoreGroup); // ignoreGroup insertColor,850
		functions["colortbl"] = Function(&Reader::ignoreGroup);
		functions["stylesheet"] = Function(&Reader::ignoreGroup);
		functions["info"] = Function(&Reader::ignoreGroup);
		functions["*"] = Function(&Reader::ignoreGroup);
	}

	m_state.ignore_control_word = false;
	m_state.ignore_text = false;
	m_state.skip = 1;
	m_state.active_codepage = 0;

	setCodepage(1252);
}
Ejemplo n.º 14
0
 static T In(T normalizedTime)
 {
     return Function(normalizedTime);
 }
Ejemplo n.º 15
0
  Function simpleIRK(Function f, int N, int order, const std::string& scheme,
                       const std::string& solver,
                       const Dict& solver_options) {
    // Consistency check
    casadi_assert_message(N>=1, "Parameter N (number of steps) must be at least 1, but got "
                          << N << ".");
    casadi_assert_message(f.n_in()==2, "Function must have two inputs: x and p");
    casadi_assert_message(f.n_out()==1, "Function must have one outputs: dot(x)");

    // Obtain collocation points
    std::vector<double> tau_root = collocation_points(order, scheme);
    tau_root.insert(tau_root.begin(), 0);

    // Retrieve collocation interpolating matrices
    std::vector < std::vector <double> > C;
    std::vector < double > D;
    collocationInterpolators(tau_root, C, D);

    // Inputs of constructed function
    MX x0 = MX::sym("x0", f.sparsity_in(0));
    MX p = MX::sym("p", f.sparsity_in(1));
    MX h = MX::sym("h");

    // Time step
    MX dt = h/N;

    // Implicitly defined variables
    MX v = MX::sym("v", repmat(x0.sparsity(), order));
    std::vector<MX> x = vertsplit(v, x0.size1());
    x.insert(x.begin(), x0);

    // Collect the equations that implicitly define v
    std::vector<MX> V_eq, f_in(2), f_out;
    for (int j=1; j<order+1; ++j) {
      // Expression for the state derivative at the collocation point
      MX xp_j = 0;
      for (int r=0; r<=order; ++r) xp_j+= C[j][r]*x[r];

      // Collocation equations
      f_in[0] = x[j];
      f_in[1] = p;
      f_out = f(f_in);
      V_eq.push_back(dt*f_out.at(0)-xp_j);
    }

    // Root-finding function
    Function rfp("rfp", {v, x0, p, h}, {vertcat(V_eq)});

    // Create a implicit function instance to solve the system of equations
    Function ifcn = rootfinder("ifcn", solver, rfp, solver_options);

    // Get state at end time
    MX xf = x0;
    for (int k=0; k<N; ++k) {
      std::vector<MX> ifcn_out = ifcn({repmat(xf, order), xf, p, h});
      x = vertsplit(ifcn_out[0], x0.size1());

      // State at end of step
      xf = D[0]*x0;
      for (int i=1; i<=order; ++i) {
        xf += D[i]*x[i-1];
      }
    }

    // Form discrete-time dynamics
    return Function("F", {x0, p, h}, {xf}, {"x0", "p", "h"}, {"xf"});
  }
Ejemplo n.º 16
0
/// Processes the data using contractors and bissections. Classifies the boxes in outside (grey), back_in(yellow) and unsafe (red)
void Sivia::do_Sivia(Ctc& tubeConstraints, Data &data, Function gdot, bool calcInner){

    QTime tSivia;
    tSivia.start();

    if (calcInner)                  //inner approximation calculation
    {
        int count=0;
        while (!data.boxes.empty()) {
            IntervalVector currentBox = data.boxes.front();                 //start from the first one
            data.boxes.pop_front();                                         //once it has been copied remove the first box

            IntervalVector auxBox=currentBox;                               //store it in aux variable to compare later

            tubeConstraints.contract(currentBox);                           //contract the current box using the previously calculated constraints
            if (currentBox!=auxBox){                                        //if the box has been contracted
                IntervalVector* removedByContractorInner;
                int setDiff=auxBox.diff(currentBox, removedByContractorInner);   //set difference between the contracted box and the original box
                for (int i = 0; i < setDiff; ++i) {

                    bool testInside=true;
                    IntervalVector gg=data.g->eval_vector(removedByContractorInner[i]);

                    for(int j = 0; j<gg.size(); j++){
                        testInside = testInside && (gg[j].ub()<=0);
                    }
                    if (testInside) {
                        data.boxesInside.append(removedByContractorInner[i]);
                    }
                }
                delete[] removedByContractorInner;
            }

            if(data.realTimeDraw){                                          //draw the boxes processing in real time
                draw_update(data, auxBox, currentBox);
            }


            bool allBoxesLessEpsilon=true;                                                                              //check if all the boxes are smaler than epsilon
            for (int i=0;(i<(currentBox.size()-1));i++){
                allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i]));
            }
            allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt));         //check the time box also


            bool boxesLessEpsilon=false;                                                                                //check if at least one box is smaller than epsilon
            for (int i=0;(i<(currentBox.size()-1));i++){
                boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]);
            }
            boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt);                   //check time box

            if (allBoxesLessEpsilon) {                                                          //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop
                (data.boxesInsideUnsafe).push_back(currentBox);
                count++;
                if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){         //If I have more boxes than nbPerhaps I stop the loop and I display the results
                    break;
                }
            }
            else {                                                                              //Otherwise we bissect following the widest diameter
                double l = 0;
                double l_temp = 0;
                int v = -1;
                for(int i = 0; i<currentBox.size()-1; i++){                                     //test that the diameter of the boxes doesnt depend on time
                    if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){
                        l_temp = currentBox[i].diam();
                        if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){
                            l = l_temp/(data.epsilons[i]);
                            v = i;
                        }
                    }
                }

                l_temp = currentBox[currentBox.size()-1].diam();                                //test the time interval
                if(l_temp>=data.dt && l_temp/(data.dt) > l){
                    v = currentBox.size()-1;
                }
                if(v != -1 && currentBox[v].is_bisectable()){                                   // then the test interval of the state variables, and then it bisects the interval which has the largest diameter
                    pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5);
                    (data.boxes).push_back(boxes.first);
                    (data.boxes).push_back(boxes.second);
                }
                else{
                    if (data.myDebug){
                        std::cout<<"Cannot be bisected \n";
                    }
                }
            }
        }
    }


    else                            //outer approximation
    {
        int count=0;
        //process all the boxes in data
        while (!data.boxes.empty()) {
            IntervalVector currentBox = data.boxes.front();                 //start from the first one
            data.boxes.pop_front();                                         //once it has been copied remove the first box

            IntervalVector auxBox=currentBox;                               //store it in aux variable to compare later

            tubeConstraints.contract(currentBox);                           //contract the current box using the previously calculated constraints
            if (currentBox!=auxBox){                                        //if the box has been contracted
                IntervalVector* removedByContractor;
                int setDiff=auxBox.diff(currentBox, removedByContractor);   //set difference between the contracted box and the original box
                for (int i = 0; i < setDiff; ++i) {
                    data.boxesOutside.push_back(removedByContractor[i]);    //add the areas removed by the contractor to the outside set
                }
                delete[] removedByContractor;
            }

            if(data.realTimeDraw){                                          //draw the boxes processing in real time
                draw_update(data, auxBox, currentBox);
            }


            bool allBoxesLessEpsilon=true;                                                                              //check if all the boxes are smaler than epsilon
            for (int i=0;(i<(currentBox.size()-1));i++){
                allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i]));
            }
            allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt));         //check the time box also

            bool boxesLessEpsilon=false;                                                                                //check if at least one box is smaller than epsilon
            for (int i=0;(i<(currentBox.size()-1));i++){
                boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]);
            }
            boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt);                   //check time box

            if (boxesLessEpsilon && !allBoxesLessEpsilon){
                IntervalVector xnext = currentBox.subvector(0, data.numVarF-1).mid();   //using the middle point of the box calculate the future positions using euler method
                IntervalVector x = currentBox.mid();
                bool testBackIn;
                for (int i = 0;i<data.numFuturePos;i++){                                // Euler method: x(n+1)=x(n)+dt*fx
                    x[data.numVarF]= x[data.numVarF].mid();
                    testBackIn = true;
                    xnext=xnext+(data.dt)*data.f->eval_vector(x);
                    x.put(0, xnext);
                    x[data.numVarF] = x[data.numVarF]+(data.dt);
                    IntervalVector gg=data.g->eval_vector(x);
                    for(int j = 0; j<gg.size(); j++){
                        testBackIn = testBackIn && (gg[j].ub()<0);                      //test if it comes back to the bubble

                    }
                    if(testBackIn == true){
                        break;
                    }
                }

                if(testBackIn == true && data.enableBackIn){                                                 //If my box was back in the bubble after integration, I store it in boxesbackin
                    (data.boxesBackIn).append(currentBox);

                    continue;
                }
            }


            if (allBoxesLessEpsilon) {                                                          //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop
                (data.boxesUnsafe).push_back(currentBox);
                count++;
                if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){         //If I have more boxes than nbPerhaps I stop the loop and I display the results
                    break;
                }
            }
            else {                                                                              //Otherwise we bissect following the widest diameter
                double l = 0;
                double l_temp = 0;
                int v = -1;
                for(int i = 0; i<currentBox.size()-1; i++){                                     //test that the diameter of the boxes doesnt depend on time
                    if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){
                        l_temp = currentBox[i].diam();
                        if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){
                            l = l_temp/(data.epsilons[i]);
                            v = i;
                        }
                    }
                }

                l_temp = currentBox[currentBox.size()-1].diam();                                //test the time interval
                if(l_temp>=data.dt && l_temp/(data.dt) > l){
                    v = currentBox.size()-1;
                }
                if(v != -1 && currentBox[v].is_bisectable()){                                   // then the test interval of the state variables, and then it bisects the interval which has the largest diameter
                    pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5);
                    (data.boxes).push_back(boxes.first);
                    (data.boxes).push_back(boxes.second);
                }
                else{
                    if (data.myDebug){
                        std::cout<<"Can not be bisected \n";
                    }
                }
            }
        }


        double maxGValues[data.numVarG-1];                              //init vector to store the max values of G

        for (int i = 0; i < data.numVarG-1; ++i) {
            maxGValues[i]=0; }


        for(int i=0; i<data.boxesUnsafe.size();i++) {                   //process unsafe boxes

            IntervalVector currentBox=data.boxesUnsafe.at(i);
            IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1);

            if (data.intMethod==0){                                     //Guaranteed integration

                // State variables
                Variable y(data.numVarF);

                // Initial conditions
                IntervalVector yinit(data.numVarF);

                for (int i = 0; i < data.numVarF; ++i) {
                    yinit[i] = currentBox[i];
                    cout<<currentBox[i]<<endl;
                }

                // system fn has to be re entered here, cannot be loaded directly from text file

                //pendulum
                Function ydot = Function (y,Return (y[1], -sin(y[0])-0.15*y[1]));

                //non holonomic
                //                Interval t = currentBox[data.numVarF];
                //                Interval xd = 7*t;
                //                Interval xdd = 7;
                //                Interval yd = sin(0.1*t);
                //                Interval ydd = 0.1*cos(0.1*t);
                //                Interval xdiff = (xd-y[0]+xdd);
                //                Interval ydiff = (yd-y[1]+ydd);
                //                Interval norm =  ( sqrt((xdiff)^2 +(ydiff)^2) );

                //                Function ydot = Function (y,Return (( sqrt((xd-y[0]+xdd)*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*cos(y[2]), ( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*sin(y[2]), 10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )));           // Ivp contruction (initial time is 0.0)

                QTime t1;
                t1.start();

                ivp_ode problem = ivp_ode (ydot, 0.0 , yinit);

                // Simulation construction and run
                simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__);          //uses Runge-kutta4 method
                data.boxesUnsafeFuture.append(simu.run_simulation());                                           //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one

                double timeSiviaCalculations1=t1.elapsed()/1000.0;
                double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0;
                cout<<endl<<"Unsafe # "<<i<<"  , Box time = "<<timeSiviaCalculations1<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl;
            }

            if (data.intMethod==1){                                     //euler method

                for (int i = 0;i<data.numFuturePos;i++){

                    IntervalVector gdotValue=gdot.eval_vector(currentBox);                                      //evaluate the g and gdot functions to inspect the constraints
                    IntervalVector gValue=data.g->eval_vector(currentBox);

                    if (data.myDebug){
                        cout<<"box = "<<currentBox<<endl;

                        for(int j = 0; j<gValue.size(); j++){
                            cout<<"gdot"<<j<<" = "<<gdotValue<<"  /  "<<(gdotValue[j].lb()>0) <<endl;                       //gdot i values
                        }
                    }

                    for(int j = 0; j<gValue.size(); j++){
                        if (data.myDebug){
                            cout<<"g"<<j<<" = "<<gValue[j]<<"  /  "<<((gValue[j].ub()>0)&&(gValue[j].lb()<0)) <<endl;}      //print gi values

                        if((gValue[j].ub()>maxGValues[j])&&(gValue[j].ub()<999999)){                                        //check max values for each gi, ignore if system goes to infinity
                            maxGValues[j]=gValue[j].ub();}
                    }

                    nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox);              //euler method
                    data.boxesUnsafeFuture.append(nextBox);

                    currentBox.put(0, nextBox);
                    currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt);          //increase time for the next step
                }
            }
        }


        for(int i=0; i<data.boxesBackIn.size();i++){                                        //process back_in boxes

            IntervalVector currentBox=data.boxesBackIn.at(i);
            IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1);

            if (data.intMethod==0){                 //guaranteed integration                                                       //Guaranteed integration

                // State variables
                Variable y(data.numVarF);

                // Initial conditions
                IntervalVector yinit(data.numVarF);

                for (int i = 0; i < data.numVarF; ++i) {
                    yinit[i] = currentBox[i];
                    cout<<currentBox[i]<<endl;
                }

                QTime t2;
                t2.start();

                // system fn has to be re entered here, cannot be loaded directly from text file

                //pendulum
                Function ydot = Function (y,Return (y[1], -sin(y[0])-0.15*y[1]));

                //non holonomic
                //                Interval t = currentBox[data.numVarF];
                //                Interval xd = 7*t;
                //                Interval xdd = 7;
                //                Interval yd = sin(0.1*t);
                //                Interval ydd = 0.1*cos(0.1*t);
                //                Interval xdiff = (xd-y[0]+xdd);
                //                Interval ydiff = (yd-y[1]+ydd);
                //                Interval norm =  ( sqrt((xdiff)^2 +(ydiff)^2) );

                //                Function ydot = Function (y,Return (( sqrt((xd-y[0]+xdd)*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*cos(y[2]), ( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*sin(y[2]), 10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )));           // Ivp contruction (initial time is 0.0)


                ivp_ode problem = ivp_ode (ydot, currentBox[data.numVarF].lb() , yinit);

                // Simulation construction and run
                simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__);          //uses Runge-kutta4 method
                data.boxesUnsafeFuture.append(simu.run_simulation());                       //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one

                double timeSiviaCalculations2=t2.elapsed()/1000.0;
                double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0;

                cout<<endl<<"Back_in # "<<i<<"  , Box time = "<<timeSiviaCalculations2<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl;
            }

            for (int i = 0;i<data.numFuturePos;i++){                                        //euler method

                if (data.intMethod==1){
                    IntervalVector gdotValue=gdot.eval_vector(currentBox);                  //evaluate the g and gdot functions to inspect the constraints
                    IntervalVector gValue=data.g->eval_vector(currentBox);

                    if (data.myDebug){
                        cout<<"box = "<<currentBox<<endl;

                        for(int j = 0; j<gValue.size(); j++){
                            cout<<"gdot"<<j<<" = "<<gdotValue<<"  /  "<<(gdotValue[j].lb()>0) <<endl;               //gdoti values
                        }
                    }

                    bool testBackIn= true;
                    for(int j = 0; j<gValue.size(); j++){
                        if (data.myDebug){
                            cout<<"g"<<j<<" = "<<gValue[j]<<"  /  "<<((gValue[j].ub()>0)&&(gValue[j].lb()<0)) <<endl;}          //print gi values

                        testBackIn = testBackIn && (gValue[j].ub()<0);                                              //test if it comes back to the bubble

                        if((gValue[j].ub()>maxGValues[j])&&(gValue[j].ub()<999999)){                                //check max values for each gi, ignore if system goes to infinity
                            maxGValues[j]=gValue[j].ub();
                        }
                    }


                    nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox);                                      // euler method
                    if (!testBackIn){
                        data.boxesBackInFuture.append(nextBox);
                    }
                    currentBox.put(0, nextBox);
                    currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt);                                  //increase time for the next step
                }
            }
        }


        if (data.myDebug){
            for (int i = 0; i < data.numVarG-1; ++i) {
                cout<<"Max G"<<i<<" = "<<maxGValues[i]<<endl;}
        }
    }
}
Ejemplo n.º 17
0
  Function implicitRK(Function& f, const std::string& impl, const Dictionary& impl_options,
                      const MX& tf, int order, const std::string& scheme, int ne) {
    casadi_assert_message(ne>=1, "Parameter ne (number of elements must be at least 1), "
                          "but got " << ne << ".");
    casadi_assert_message(order==4, "Only RK order 4 is supported now.");
    casadi_assert_message(f.getNumInputs()==DAE_NUM_IN && f.getNumOutputs()==DAE_NUM_OUT,
                          "Supplied function must adhere to dae scheme.");
    casadi_assert_message(f.output(DAE_QUAD).isEmpty(),
                          "Supplied function cannot have quadrature states.");

    // Obtain collocation points
    std::vector<double> tau_root = collocationPoints(order, "legendre");

    // Retrieve collocation interpolating matrices
    std::vector < std::vector <double> > C;
    std::vector < double > D;
    collocationInterpolators(tau_root, C, D);

    // Retrieve problem dimensions
    int nx = f.input(DAE_X).size();
    int nz = f.input(DAE_Z).size();
    int np = f.input(DAE_P).size();

    //Variables for one finite element
    MX X = MX::sym("X", nx);
    MX P = MX::sym("P", np);
    MX V = MX::sym("V", order*(nx+nz)); // Unknowns

    MX X0 = X;

    // Components of the unknowns that correspond to states at collocation points
    std::vector<MX> Xc;Xc.reserve(order);
    Xc.push_back(X0);

    // Components of the unknowns that correspond to algebraic states at collocation points
    std::vector<MX> Zc;Zc.reserve(order);

    // Splitting the unknowns
    std::vector<int> splitPositions = range(0, order*nx, nx);
    if (nz>0) {
      std::vector<int> Zc_pos = range(order*nx, order*nx+(order+1)*nz, nz);
      splitPositions.insert(splitPositions.end(), Zc_pos.begin(), Zc_pos.end());
    } else {
      splitPositions.push_back(order*nx);
    }
    std::vector<MX> Vs = vertsplit(V, splitPositions);

    // Extracting unknowns from Z
    for (int i=0;i<order;++i) {
      Xc.push_back(X0+Vs[i]);
    }
    if (nz>0) {
      for (int i=0;i<order;++i) {
        Zc.push_back(Vs[order+i]);
      }
    }

    // Get the collocation Equations (that define V)
    std::vector<MX> V_eq;

    // Local start time
    MX t0_l=MX::sym("t0");
    MX h = MX::sym("h");

    for (int j=1;j<order+1;++j) {
      // Expression for the state derivative at the collocation point
      MX xp_j = 0;
      for (int r=0;r<order+1;++r) {
        xp_j+= C[j][r]*Xc[r];
      }
      // Append collocation equations & algebraic constraints
      std::vector<MX> f_out;
      MX t_l = t0_l+tau_root[j]*h;
      if (nz>0) {
        f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P, "z", Zc[j-1]));
      } else {
        f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P));
      }
      V_eq.push_back(h*f_out[DAE_ODE]-xp_j);
      V_eq.push_back(f_out[DAE_ALG]);

    }

    // Root-finding function, implicitly defines V as a function of X0 and P
    std::vector<MX> vfcn_inputs;
    vfcn_inputs.push_back(V);
    vfcn_inputs.push_back(X);
    vfcn_inputs.push_back(P);
    vfcn_inputs.push_back(t0_l);
    vfcn_inputs.push_back(h);

    Function vfcn = MXFunction(vfcn_inputs, vertcat(V_eq));
    vfcn.init();

    try {
      // Attempt to convert to SXFunction to decrease overhead
      vfcn = SXFunction(vfcn);
      vfcn.init();
    } catch(CasadiException & e) {
      //
    }

    // Create a implicit function instance to solve the system of equations
    ImplicitFunction ifcn(impl, vfcn, Function(), LinearSolver());
    ifcn.setOption(impl_options);
    ifcn.init();

    // Get an expression for the state at the end of the finite element
    std::vector<MX> ifcn_call_in(5);
    ifcn_call_in[0] = MX::zeros(V.sparsity());
    std::copy(vfcn_inputs.begin()+1, vfcn_inputs.end(), ifcn_call_in.begin()+1);
    std::vector<MX> ifcn_call_out = ifcn.call(ifcn_call_in, true);
    Vs = vertsplit(ifcn_call_out[0], splitPositions);

    MX XF = 0;
    for (int i=0;i<order+1;++i) {
      XF += D[i]*(i==0? X : X + Vs[i-1]);
    }


    // Get the discrete time dynamics
    ifcn_call_in.erase(ifcn_call_in.begin());
    MXFunction F = MXFunction(ifcn_call_in, XF);
    F.init();

    // Loop over all finite elements
    MX h_ = tf/ne;
    MX t0_ = 0;

    for (int i=0;i<ne;++i) {
      std::vector<MX> F_in;
      F_in.push_back(X);
      F_in.push_back(P);
      F_in.push_back(t0_);
      F_in.push_back(h_);
      t0_+= h_;
      std::vector<MX> F_out = F.call(F_in);
      X = F_out[0];
    }

    // Create a ruturn function with Integrator signature
    MXFunction ret = MXFunction(integratorIn("x0", X0, "p", P), integratorOut("xf", X));
    ret.init();

    return ret;

  }
void DirectMultipleShootingInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();

  // Create an integrator instance
  integratorCreator integrator_creator = getOption("integrator");
  integrator_ = integrator_creator(ffcn_,Function());
  if(hasSetOption("integrator_options")){
    integrator_.setOption(getOption("integrator_options"));
  }

  // Set t0 and tf
  integrator_.setOption("t0",0);
  integrator_.setOption("tf",tf_/nk_);
  integrator_.init();
  
  // Path constraints present?
  bool path_constraints = nh_>0;
  
  // Count the total number of NLP variables
  int NV = np_ + // global parameters
           nx_*(nk_+1) + // local state
           nu_*nk_; // local control
           
  // Declare variable vector for the NLP
  // The structure is as follows:
  // np x 1  (parameters)
  // ------
  // nx x 1  (states at time i=0)
  // nu x 1  (controls in interval i=0)
  // ------
  // nx x 1  (states at time i=1)
  // nu x 1  (controls in interval i=1)
  // ------
  // .....
  // ------
  // nx x 1  (states at time i=nk)
  
  MX V = MX::sym("V",NV);

  // Global parameters
  MX P = V(Slice(0,np_));

  // offset in the variable vector
  int v_offset=np_; 
  
  // Disretized variables for each shooting node
  vector<MX> X(nk_+1), U(nk_);
  for(int k=0; k<=nk_; ++k){ // interior nodes
    // Local state
    X[k] = V[Slice(v_offset,v_offset+nx_)];
    v_offset += nx_;
    
    // Variables below do not appear at the end point
    if(k==nk_) break;
    
    // Local control
    U[k] = V[Slice(v_offset,v_offset+nu_)];
    v_offset += nu_;
  }
  
  // Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
  casadi_assert(v_offset==NV);

  // Input to the parallel integrator evaluation
  vector<vector<MX> > int_in(nk_);
  for(int k=0; k<nk_; ++k){
    int_in[k].resize(INTEGRATOR_NUM_IN);
    int_in[k][INTEGRATOR_P] = vertcat(P,U[k]);
    int_in[k][INTEGRATOR_X0] = X[k];
  }

  // Input to the parallel function evaluation
  vector<vector<MX> > fcn_in(nk_);
  for(int k=0; k<nk_; ++k){
    fcn_in[k].resize(DAE_NUM_IN);
    fcn_in[k][DAE_T] = (k*tf_)/nk_;
    fcn_in[k][DAE_P] = vertcat(P,U.at(k));
    fcn_in[k][DAE_X] = X[k];
  }

  // Options for the parallelizer
  Dictionary paropt;
  
  // Transmit parallelization mode
  if(hasSetOption("parallelization"))
    paropt["parallelization"] = getOption("parallelization");
  
  // Evaluate function in parallel
  vector<vector<MX> > pI_out = integrator_.callParallel(int_in,paropt);

  // Evaluate path constraints in parallel
  vector<vector<MX> > pC_out;
  if(path_constraints)
    pC_out = cfcn_.callParallel(fcn_in,paropt);
  
  //Constraint function
  vector<MX> gg(2*nk_);

  // Collect the outputs
  for(int k=0; k<nk_; ++k){
    //append continuity constraints
    gg[2*k] = pI_out[k][INTEGRATOR_XF] - X[k+1];
    
    // append the path constraints
    if(path_constraints)
      gg[2*k+1] = pC_out[k][0];
  }

  // Terminal constraints
  MX g = vertcat(gg);

  // Objective function
  MX f;
  if (mfcn_.getNumInputs()==1) {
    f = mfcn_(X.back()).front();
  } else {
    vector<MX> mfcn_argin(MAYER_NUM_IN); 
    mfcn_argin[MAYER_X] = X.back();
    mfcn_argin[MAYER_P] = P;
    f = mfcn_.call(mfcn_argin).front();
  }

  // NLP
  nlp_ = MXFunction(nlpIn("x",V),nlpOut("f",f,"g",g));
  nlp_.setOption("ad_mode","forward");
  nlp_.init();
  
  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(nlp_);
  
  // Pass user options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
Ejemplo n.º 19
0
 static T Out(T normalizedTime)
 {
     return 1 - Function(1 - normalizedTime);
 }
Ejemplo n.º 20
0
 inline explicit fused_procedure(func_const_fwd_t f = Function())
     : fnc_transformed(f)
 { }
Ejemplo n.º 21
0
 static bool parse(State& s, UserState& us)
 {
     return symbols_detail::aux(s, us, PairAssocContainerFtor()(us), Function());
 }
Ejemplo n.º 22
0
int main(int argc, char **argv) {
  int SomeLocal = argc * 2;
  return Function(SomeLocal, 'a');
}
Ejemplo n.º 23
0
 inline void reset_function(Function& f)
 {
     f = Function();
 }
Ejemplo n.º 24
0
s32 Swi_Handler(u32 arg0, u32 arg1, u32 arg2, u32 arg3)
{
	u8 cmd;

	/* Check alignment */
	SwiAddr -= (SwiAddr[-4] == 0xDF) ? 3 : 1;

	/* Get command */
	cmd = SwiAddr[0];

	/* Check command */
	if(SwiAddr[0] != 0xcc) {
		if (SwiTable[cmd])
			return SwiTable[cmd](arg0, arg1, arg2, arg3);
		else
			return arg0;
	}

	/* Check argument */
	switch (arg0) {
	/** Add SWI handler **/
	case 0: {
		SwiTable[arg1]= (void *)arg2;
		break;
	}
	
	/** Memcpy (cached to cached) **/
	case 2: {
		void *src = (void *)arg2;
		void *dst = (void *)arg1;
		u32   len = arg3;

		u32 perms;

		/* Apply permissions */
		perms = Perms_Read();
		Perms_Write(0xFFFFFFFF);

		/* Copy data */
		memcpy(dst, src, len);
		DCFlushRange(dst, len);

		/* Restore permissions */
		Perms_Write(perms);

		break;
	}
	
	/** Get register **/
	case 3:
		return *(vu32 *)arg1;

	/** Set register **/
	case 4:
		*(vu32 *)arg1 = arg2;
		break;

	/** Set register **/
	case 5:
		*(vu32 *)arg1 |= arg2;
		break;

	/** Clear register **/
	case 6:
		*(vu32 *)arg1 &= ~arg2;
		break;

	/** Memcpy (uncached to cached) **/
	case 9: {
		void *src = (void *)arg2;
		void *dst = (void *)arg1;
		u32   len = arg3;

		u32 perms;

		/* Apply permissions */
		perms = Perms_Read();
		Perms_Write(0xFFFFFFFF);

		/* Copy data */
		DCInvalidateRange(src, len);
		memcpy(dst, src, len);
		DCFlushRange(dst, len);

		/* Restore permissions */
		Perms_Write(perms);

		break;
	}

	/** Call function **/
	case 16: {
		s32 (*Function)(void *in, void *out);

		/* Set function */
		Function = (void *)arg1;

		/* Call function */
		return Function((void *)arg2, (void *)arg3);
	}

	/** Get syscall base */
	case 17:
		return ios.syscall;

	/** Get IOS info **/
	case 18: {
		iosInfo *buffer = (iosInfo *)arg1;

		/* Copy IOS info */
		memcpy(buffer, &ios, sizeof(ios));
		DCFlushRange(buffer, sizeof(ios));

		break;
	}

	/** Get MLOAD version **/
	case 19:
		return (MLOAD_VER << 4) | MLOAD_SUBVER;

	/** Led on **/
	case 128:
		*(vu32 *)0x0d8000c0 |= 0x20;
		break;
	
	/** Led off **/
	case 129:
		*(vu32 *)0x0d8000c0 &=~ 0x20;
		break;

	/** Led blink **/
	case 130:
		*(vu32 *)0x0d8000c0 ^= 0x20;
		break;
		
	default:
		break;
	}

	return 0;
}
Ejemplo n.º 25
0
Function GslInternal::getJacobian() {return Function();}
Ejemplo n.º 26
0
ContextBindings * AcquireContextBindings() {
	ContextBindings * result = AllocateContextBindings(NULL);
	int lenFunctions = sizeof(globalFunctionNames)/sizeof(globalFunctionNames[0]);
	int lenLazyFunctions = sizeof(globalLazyFunctionNames)/sizeof(globalLazyFunctionNames[0]);
	int lenConstants = sizeof(globalConstantNames)/sizeof(globalConstantNames[0]);
	assert((sizeof(globalFunctionPointers)/sizeof(globalFunctionPointers[0])) == lenFunctions);
	while (lenFunctions-- > 0)
		result->dictionary = SetConstantStr(result->dictionary, globalFunctionNames[lenFunctions], Function(globalFunctionPointers[lenFunctions]));
	assert((sizeof(globalLazyFunctionPointers)/sizeof(globalLazyFunctionPointers[0])) == lenLazyFunctions);
	assert((sizeof(globalAcquireLazyEvaluationContextPointers)/sizeof(globalAcquireLazyEvaluationContextPointers[0])) == lenLazyFunctions);
	while (lenLazyFunctions-- > 0)
		result->dictionary = SetConstantStr(result->dictionary, globalLazyFunctionNames[lenLazyFunctions], LazyFunction(globalLazyFunctionPointers[lenLazyFunctions], globalAcquireLazyEvaluationContextPointers[lenLazyFunctions]));	
	assert((sizeof(globalConstantFunctionPointers)/sizeof(globalConstantFunctionPointers[0])) == lenConstants);
	while (lenConstants-- > 0)
		result->dictionary = SetConstantStr(result->dictionary, globalConstantNames[lenConstants], (globalConstantFunctionPointers[lenConstants])());
	return result;
}
Ejemplo n.º 27
0
/**
  Initialize debug agent.

  This function is used to set up debug environment for SEC and PEI phase.

  If InitFlag is DEBUG_AGENT_INIT_PREMEM_SEC, it will overirde IDT table entries
  and initialize debug port. It will enable interrupt to support break-in feature.
  It will set up debug agent Mailbox in cache-as-ramfrom. It will be called before
  physical memory is ready.
  If InitFlag is DEBUG_AGENT_INIT_POSTMEM_SEC, debug agent will build one GUIDed
  HOB to copy debug agent Mailbox. It will be called after physical memory is ready.

  This function is used to set up debug environment to support source level debugging.
  If certain Debug Agent Library instance has to save some private data in the stack,
  this function must work on the mode that doesn't return to the caller, then
  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one
  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is
  responsible to invoke the passing-in function at the end of InitializeDebugAgent().

  If the parameter Function is not NULL, Debug Agent Library instance will invoke it by
  passing in the Context to be its parameter.

  If Function() is NULL, Debug Agent Library instance will return after setup debug
  environment.

  @param[in] InitFlag     Init flag is used to decide the initialize process.
  @param[in] Context      Context needed according to InitFlag; it was optional.
  @param[in] Function     Continue function called by debug agent library; it was
                          optional.

**/
VOID
EFIAPI
InitializeDebugAgent (
  IN UINT32                InitFlag,
  IN VOID                  *Context, OPTIONAL
  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
  )
{
  DEBUG_AGENT_MAILBOX              *Mailbox;
  DEBUG_AGENT_MAILBOX              *NewMailbox;
  DEBUG_AGENT_MAILBOX              MailboxInStack;
  DEBUG_AGENT_PHASE2_CONTEXT       Phase2Context;
  DEBUG_AGENT_CONTEXT_POSTMEM_SEC  *DebugAgentContext;
  EFI_STATUS                       Status;
  IA32_DESCRIPTOR                  *Ia32Idtr;
  IA32_IDT_ENTRY                   *Ia32IdtEntry;
  UINT64                           DebugPortHandle;
  UINT64                           MailboxLocation;
  UINT64                           *MailboxLocationPointer;
  EFI_PHYSICAL_ADDRESS             Address;
  UINT32                           DebugTimerFrequency;
  BOOLEAN                          CpuInterruptState;

  //
  // Disable interrupts and save current interrupt state
  //
  CpuInterruptState = SaveAndDisableInterrupts();

  switch (InitFlag) {

  case DEBUG_AGENT_INIT_PREMEM_SEC:

    InitializeDebugIdt ();

    MailboxLocation = (UINT64)(UINTN)&MailboxInStack;
    Mailbox = &MailboxInStack;
    ZeroMem ((VOID *) Mailbox, sizeof (DEBUG_AGENT_MAILBOX));
    //
    // Get and save debug port handle and set the length of memory block.
    //
    SetLocationSavedMailboxPointerInIdtEntry (&MailboxLocation);
    //
    // Force error message could be printed during the first shakehand between Target/HOST.
    //
    SetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL, DEBUG_AGENT_ERROR);
    //
    // Save init arch type when debug agent initialized
    //
    SetDebugFlag (DEBUG_AGENT_FLAG_INIT_ARCH, DEBUG_ARCH_SYMBOL);
    //
    // Initialize Debug Timer hardware and save its frequency
    //
    InitializeDebugTimer (&DebugTimerFrequency, TRUE);
    UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_TIMER_FREQUENCY, DebugTimerFrequency);

    Phase2Context.InitFlag = InitFlag;
    Phase2Context.Context  = Context;
    Phase2Context.Function = Function;
    DebugPortInitialize ((VOID *) &Phase2Context, InitializeDebugAgentPhase2);
    //
    // If reaches here, it means Debug Port initialization failed.
    //
    DEBUG ((EFI_D_ERROR, "Debug Agent: Debug port initialization failed.\n"));

    break;

  case DEBUG_AGENT_INIT_POSTMEM_SEC:
    Mailbox = GetMailboxPointer ();
    //
    // Memory has been ready
    //
    SetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY, 1);
    if (IsHostAttached ()) {
      //
      // Trigger one software interrupt to inform HOST
      //
      TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
    }
    //
    // Install Vector Handoff Info PPI to persist vectors used by Debug Agent
    //
    Status = PeiServicesInstallPpi (&mVectorHandoffInfoPpiList[0]);
    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to install Vector Handoff Info PPI!\n"));
      CpuDeadLoop ();
    }
    //
    // Fix up Debug Port handle address and mailbox address
    //
    DebugAgentContext = (DEBUG_AGENT_CONTEXT_POSTMEM_SEC *) Context;
    if (DebugAgentContext != NULL) {
      DebugPortHandle = (UINT64)(UINT32)(Mailbox->DebugPortHandle + DebugAgentContext->StackMigrateOffset);
      UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, DebugPortHandle);
      Mailbox = (DEBUG_AGENT_MAILBOX *) ((UINTN) Mailbox + DebugAgentContext->StackMigrateOffset);
      MailboxLocation = (UINT64)(UINTN)Mailbox;
      //
      // Build mailbox location in HOB and fix-up its address
      //
      MailboxLocationPointer = BuildGuidDataHob (
                                 &gEfiDebugAgentGuid,
                                 &MailboxLocation,
                                 sizeof (UINT64)
                                 );
      MailboxLocationPointer = (UINT64 *) ((UINTN) MailboxLocationPointer + DebugAgentContext->HeapMigrateOffset);
    } else {
      //
      // DebugAgentContext is NULL. Then, Mailbox can directly be copied into memory.
      // Allocate ACPI NVS memory for new Mailbox and Debug Port Handle buffer
      //
      Status = PeiServicesAllocatePages (
                 EfiACPIMemoryNVS,
                 EFI_SIZE_TO_PAGES (sizeof(DEBUG_AGENT_MAILBOX) + PcdGet16(PcdDebugPortHandleBufferSize)),
                 &Address
                 );
      if (EFI_ERROR (Status)) {
        DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to allocate pages!\n"));
        CpuDeadLoop ();
      }
      NewMailbox = (DEBUG_AGENT_MAILBOX *) (UINTN) Address;
      //
      // Copy Mailbox and Debug Port Handle buffer to new location in ACPI NVS memory, because original Mailbox
      // and Debug Port Handle buffer in the allocated pool that may be marked as free by DXE Core after DXE Core
      // reallocates the HOB.
      //
      CopyMem (NewMailbox, Mailbox, sizeof (DEBUG_AGENT_MAILBOX));
      CopyMem (NewMailbox + 1, (VOID *)(UINTN)Mailbox->DebugPortHandle, PcdGet16(PcdDebugPortHandleBufferSize));
      UpdateMailboxContent (NewMailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, (UINT64)(UINTN)(NewMailbox + 1));
      MailboxLocation = (UINT64)(UINTN)NewMailbox;
      //
      // Build mailbox location in HOB
      //
      MailboxLocationPointer = BuildGuidDataHob (
                                 &gEfiDebugAgentGuid,
                                 &MailboxLocation,
                                 sizeof (UINT64)
                                 );
    }
    //
    // Update IDT entry to save the location saved mailbox pointer
    //
    SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationPointer);
    break;

  case DEBUG_AGENT_INIT_PEI:
    if (Context == NULL) {
      DEBUG ((EFI_D_ERROR, "DebugAgent: Input parameter Context cannot be NULL!\n"));
      CpuDeadLoop ();
    }
    //
    // Check if Debug Agent has initialized before
    //
    if (IsDebugAgentInitialzed()) {
      DEBUG ((EFI_D_WARN, "Debug Agent: It has already initialized in SEC Core!\n"));
      break;
    }
    //
    // Install Vector Handoff Info PPI to persist vectors used by Debug Agent
    //
    Status = PeiServicesInstallPpi (&mVectorHandoffInfoPpiList[0]);
    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to install Vector Handoff Info PPI!\n"));
      CpuDeadLoop ();
    }
    //
    // Set up IDT entries
    //
    InitializeDebugIdt ();
    //
    // Build mailbox in HOB and setup Mailbox Set In Pei flag
    //
    Mailbox = AllocateZeroPool (sizeof (DEBUG_AGENT_MAILBOX));
    if (Mailbox == NULL) {
      DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to allocate memory!\n"));
      CpuDeadLoop ();
    } else {
      MailboxLocation = (UINT64)(UINTN)Mailbox;
      MailboxLocationPointer = BuildGuidDataHob (
                                 &gEfiDebugAgentGuid,
                                 &MailboxLocation,
                                 sizeof (UINT64)
                                 );
      //
      // Initialize Debug Timer hardware and save its frequency
      //
      InitializeDebugTimer (&DebugTimerFrequency, TRUE);
      UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_TIMER_FREQUENCY, DebugTimerFrequency);
      //
      // Update IDT entry to save the location pointer saved mailbox pointer
      //
      SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationPointer);
    }
    //
    // Save init arch type when debug agent initialized
    //
    SetDebugFlag (DEBUG_AGENT_FLAG_INIT_ARCH, DEBUG_ARCH_SYMBOL);
    //
    // Register for a callback once memory has been initialized.
    // If memery has been ready, the callback funtion will be invoked immediately
    //
    Status = PeiServicesNotifyPpi (&mMemoryDiscoveredNotifyList[0]);
    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to register memory discovered callback function!\n"));
      CpuDeadLoop ();
    }
    //
    // Set HOB check flag if memory has not been ready yet
    //
    if (GetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY) == 0) {
      SetDebugFlag (DEBUG_AGENT_FLAG_CHECK_MAILBOX_IN_HOB, 1);
    }

    Phase2Context.InitFlag = InitFlag;
    Phase2Context.Context  = Context;
    Phase2Context.Function = Function;
    DebugPortInitialize ((VOID *) &Phase2Context, InitializeDebugAgentPhase2);

    FindAndReportModuleImageInfo (4);

    break;

  case DEBUG_AGENT_INIT_THUNK_PEI_IA32TOX64:
    if (Context == NULL) {
      DEBUG ((EFI_D_ERROR, "DebugAgent: Input parameter Context cannot be NULL!\n"));
      CpuDeadLoop ();
    } else {
      Ia32Idtr =  (IA32_DESCRIPTOR *) Context;
      Ia32IdtEntry = (IA32_IDT_ENTRY *)(Ia32Idtr->Base);
      MailboxLocationPointer = (UINT64 *) (UINTN) (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetLow +
                                         (UINT32) (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh << 16));
      Mailbox = (DEBUG_AGENT_MAILBOX *) (UINTN)(*MailboxLocationPointer);
      //
      // Mailbox should valid and setup before executing thunk code
      //
      VerifyMailboxChecksum (Mailbox);

      DebugPortHandle = (UINT64) (UINTN)DebugPortInitialize ((VOID *)(UINTN)Mailbox->DebugPortHandle, NULL);
      UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, DebugPortHandle);
      //
      // Set up IDT entries
      //
      InitializeDebugIdt ();
      //
      // Update IDT entry to save location pointer saved the mailbox pointer
      //
      SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationPointer);

      FindAndReportModuleImageInfo (4);
    }
    break;

  default:
    //
    // Only DEBUG_AGENT_INIT_PREMEM_SEC and DEBUG_AGENT_INIT_POSTMEM_SEC are allowed for this
    // Debug Agent library instance.
    //
    DEBUG ((EFI_D_ERROR, "Debug Agent: The InitFlag value is not allowed!\n"));
    CpuDeadLoop ();
    break;
  }

  if (InitFlag == DEBUG_AGENT_INIT_POSTMEM_SEC) {
    //
    // Restore CPU Interrupt state and keep debug timer interrupt state as is
    // in DEBUG_AGENT_INIT_POSTMEM_SEC case
    //
    SetInterruptState (CpuInterruptState);
  } else {
    //
    // Enable Debug Timer interrupt
    //
    SaveAndSetDebugTimerInterrupt (TRUE);
    //
    // Enable CPU interrupts so debug timer interrupts can be delivered
    //
    EnableInterrupts ();
  }
  //
  // If Function is not NULL, invoke it always whatever debug agent was initialized sucesssfully or not.
  //
  if (Function != NULL) {
    Function (Context);
  }
  //
  // Set return status for DEBUG_AGENT_INIT_PEI
  //
  if (InitFlag == DEBUG_AGENT_INIT_PEI && Context != NULL) {
    *(EFI_STATUS *)Context = EFI_SUCCESS;
  }
}
Ejemplo n.º 28
0
//------------------------------------------------------------------------------
// Name: operator=
//------------------------------------------------------------------------------
Function &Function::operator=(const Function &rhs) {
	Function(rhs).swap(*this);
	return *this;
}
Ejemplo n.º 29
0
int	User_Program::Execute (int _record)
{
	int stat;
	bool test;

	memset (out_flag, '\0', num_files * sizeof (int));

	stat = 0;
	s = stack [sindex = 1];
	loop = 0;

	if (_record < 0) {
		record++;
	} else {
		record = _record;
	}

	//---- process each command ----

	for (cmd_ptr = command.First (); cmd_ptr; cmd_ptr = command.Next ()) {
		s1 = stack [sindex - 1];

		switch (cmd_ptr->type) {

			case LIMIT:
				if (cmd_ptr->token != EQUALS) {
					exe->Error ("Unrecognized Limit Token = %d (stack=%d)", 
						cmd_ptr->token, command.Record_Index ());
					return (-1);
				}
				cmd_ptr = command.Next ();

				if (!Assign ()) return (-1);
				break;

			case LOGICAL:
				if (cmd_ptr->token == IF || cmd_ptr->token == WHILE) {
					if (s->type == INT_DATA) {
						test = (s->lvalue != 0);
					} else if (s->type == FLOAT_DATA) {
						test = (s->fvalue != 0.0);
					} else if (s->type == STRING_DATA) {
						test = (s->svalue [0] != '\0');
					} else {
						test = false;
					}
					s = stack [sindex = 1];
					if (test) continue;
				}
				cmd_ptr = command [cmd_ptr->value];
				break;

			case RELATION:
				if (!Relation ()) return (-1);
				break;

			case MATH:
				if (!Math ()) return (-1);
				break;

			case FUNCTION:
				if (!Function ()) return (-1);
				break;

			case CONVERT:
				if (!Convert ()) return (-1);
				break;

			case DATETIME:
				if (!Date_Time ()) return (-1);
				break;

			case IN_OUT:
				if (!Input_Output ()) return (-1);
				break;

			case TABLE:
				if (!Table ()) return (-1);
				break;

			case RETURN:
				if (s->type == INT_DATA) {
					stat = s->lvalue;
				} else if (s->type == FLOAT_DATA) {
					stat = (int) s->fvalue;
				}
				return (stat);
				break;

			default:
				if (!Read_Value ()) return (-1);
				break;
		}
	}
	return (stat);
}
Ejemplo n.º 30
0
void OptionDialog::addFunc() {

  tabs->addTab(new FunctionTab(Function()), QString::number(tabs->count() + 1));
  tabs->setCurrentIndex(tabs->count() - 1);
  tabChange(false);
}