<commit_before>// Author: François Faure, INRIA-UJF, (C) 2006
//
// Copyright: See COPYING file that comes with this distribution
#include "Sofa/Components/CGImplicitSolver.h"
//#include "Sofa/Core/IntegrationGroup.h"
#include "Sofa/Core/MultiVector.h"
#include "Common/ObjectFactory.h"

#include <math.h>
#include <iostream>
using std::cerr;
using std::endl;

namespace Sofa
{

namespace Components
{

using namespace Common;
using namespace Core;

CGImplicitSolver::CGImplicitSolver()
    : f_maxIter( dataField(&f_maxIter,(unsigned)25,"iterations","maximum number of iterations of the Conjugate Gradient solution") )
    , f_tolerance( dataField(&f_tolerance,1e-5,"tolerance","desired precision of the Conjugate Gradient Solution (ratio of current residual norm over initial residual norm)") )
    , f_smallDenominatorThreshold( dataField(&f_smallDenominatorThreshold,1e-5,"threshold","minimum value of the denominator in the conjugate Gradient solution") )
    , f_rayleighStiffness( dataField(&f_rayleighStiffness,0.1,"rayleighStiffness","Rayleigh damping coefficient related to stiffness") )
    , f_rayleighMass( dataField(&f_rayleighMass,0.1,"rayleighMass","Rayleigh damping coefficient related to mass"))
    , f_velocityDamping( dataField(&f_velocityDamping,0.,"vdamping","Velocity decay coefficient (no decay if null)") )
{
    //     maxCGIter = 25;
    //     smallDenominatorThreshold = 1e-5;
    //     tolerance = 1e-5;
    //     rayleighStiffness = 0.1;
    //     rayleighMass = 0.1;
    //     velocityDamping = 0;

}

// CGImplicitSolver* CGImplicitSolver::setMaxIter( int n )
// {
//     maxCGIter = n;
//     return this;
// }

void CGImplicitSolver::solve(double dt)
{
    CGImplicitSolver* group = this;
    MultiVector pos(group, VecId::position());
    MultiVector vel(group, VecId::velocity());
//     MultiVector dx(group, VecId::dx());
    MultiVector f(group, VecId::force());
    MultiVector b(group, V_DERIV);
    MultiVector p(group, V_DERIV);
    MultiVector q(group, V_DERIV);
    MultiVector q2(group, V_DERIV);
    MultiVector r(group, V_DERIV);
    MultiVector x(group, V_DERIV);
    //MultiVector z(group, V_DERIV);
    double h = dt;
    bool printLog = f_printLog.getValue();

    // compute the right-hand term of the equation system
    group->computeForce(b);             // b = f0
    group->propagateDx(vel);            // dx = v
    group->computeDf(f);                // f = df/dx v
    b.peq(f,h+f_rayleighStiffness.getValue());      // b = f0 + (h+rs)df/dx v

    if (f_rayleighMass.getValue() != 0.0)
    {
        f.clear();
        group->addMdx(f,vel);
        b.peq(f,-f_rayleighMass.getValue());     // b = f0 + (h+rs)df/dx v - rd M v
    }

    b.teq(h);                           // b = h(f0 + (h+rs)df/dx v - rd M v)
    group->projectResponse(b);          // b is projected to the constrained space

    double normb = sqrt(b.dot(b));

    // -- solve the system using a conjugate gradient solution
    double rho, rho_1=0, alpha, beta;
    group->v_clear( x );
    group->v_eq(r,b); // initial residual

    if( printLog )
    {
        cerr<<"CGImplicitSolver, dt = "<< dt <<endl;
        cerr<<"CGImplicitSolver, initial x = "<< pos <<endl;
        cerr<<"CGImplicitSolver, initial v = "<< vel <<endl;
        cerr<<"CGImplicitSolver, f0 = "<< b <<endl;
        cerr<<"CGImplicitSolver, r0 = "<< r <<endl;
    }

    unsigned nb_iter;
    const char* endcond = "iterations";
    for( nb_iter=1; nb_iter<=f_maxIter.getValue(); nb_iter++ )
    {
        cerr<<"r : "<<sqrt(r.dot(r))<<endl;

        //z = r; // no precond
        //rho = r.dot(z);
        rho = r.dot(r);

        if( nb_iter==1 )
            p = r; //z;
        else
        {
            beta = rho / rho_1;
            p *= beta;
            p += r; //z;
        }

// 		cerr<<"p : "<<p<<endl;

        // matrix-vector product
        group->propagateDx(p);          // dx = p

        group->computeDf(q);            // q = df/dx p

// 		cerr<<"computeDf(q) : "<<q<<endl;

        q *= -h*(h+f_rayleighStiffness.getValue());  // q = -h(h+rs) df/dx p
//
// 		cerr<<"-h(h+rs) df/dx p : "<<q<<endl;
// 		cerr<<"f_rayleighMass.getValue() : "<<f_rayleighMass.getValue()<<endl;

        if (f_rayleighMass.getValue()==0.0)
            group->addMdx( q, p);           // q = Mp -h(h+rs) df/dx p
        else
        {
            q2.clear();
            group->addMdx( q2, p);
            q.peq(q2,(1+h*f_rayleighMass.getValue())); // q = Mp -h(h+rs) df/dx p +hr Mp  =  (M + dt(rd M + rs K) + dt2 K) dx
        }
        // filter the product to take the constraints into account
//
// 	   cerr<<"q : "<<q<<endl;

        group->projectResponse(q);     // q is projected to the constrained space

        double den = p.dot(q);

        if( fabs(den)<f_smallDenominatorThreshold.getValue() )
        {
            endcond = "threshold";
            if( printLog )
            {
//                 cerr<<"CGImplicitSolver, den = "<<den<<", smallDenominatorThreshold = "<<f_smallDenominatorThreshold.getValue()<<endl;
            }
            break;
        }
        alpha = rho/den;
        x.peq(p,alpha);                 // x = x + alpha p
        r.peq(q,-alpha);                // r = r - alpha r

        double normr = sqrt(r.dot(r));
        if (normr/normb <= f_tolerance.getValue())
        {
            endcond = "tolerance";
            break;
        }
        rho_1 = rho;
    }
    // x is the solution of the system

    // apply the solution
    vel.peq( x );                       // vel = vel + x
    pos.peq( vel, h );                  // pos = pos + h vel
    if (f_velocityDamping.getValue()!=0.0)
        vel *= exp(-h*f_velocityDamping.getValue());

    if( printLog )
    {
        cerr<<"CGImplicitSolver::solve, nbiter = "<<nb_iter<<" stop because of "<<endcond<<endl;
        cerr<<"CGImplicitSolver::solve, solution = "<<x<<endl;
        cerr<<"CGImplicitSolver, final x = "<< pos <<endl;
        cerr<<"CGImplicitSolver, final v = "<< vel <<endl;
    }
}

void create(CGImplicitSolver*& obj, ObjectDescription* arg)
{
    obj = new CGImplicitSolver();
    obj->parseFields( arg->getAttributeMap() );
}

SOFA_DECL_CLASS(CGImplicit)

Creator<ObjectFactory, CGImplicitSolver> CGImplicitSolverClass("CGImplicit");

} // namespace Components

} // namespace Sofa

<commit_msg>r651/sofa-dev : <commit_after>// Author: François Faure, INRIA-UJF, (C) 2006
//
// Copyright: See COPYING file that comes with this distribution
#include "Sofa/Components/CGImplicitSolver.h"
//#include "Sofa/Core/IntegrationGroup.h"
#include "Sofa/Core/MultiVector.h"
#include "Common/ObjectFactory.h"

#include <math.h>
#include <iostream>
using std::cerr;
using std::endl;

namespace Sofa
{

namespace Components
{

using namespace Common;
using namespace Core;

CGImplicitSolver::CGImplicitSolver()
    : f_maxIter( dataField(&f_maxIter,(unsigned)25,"iterations","maximum number of iterations of the Conjugate Gradient solution") )
    , f_tolerance( dataField(&f_tolerance,1e-5,"tolerance","desired precision of the Conjugate Gradient Solution (ratio of current residual norm over initial residual norm)") )
    , f_smallDenominatorThreshold( dataField(&f_smallDenominatorThreshold,1e-5,"threshold","minimum value of the denominator in the conjugate Gradient solution") )
    , f_rayleighStiffness( dataField(&f_rayleighStiffness,0.1,"rayleighStiffness","Rayleigh damping coefficient related to stiffness") )
    , f_rayleighMass( dataField(&f_rayleighMass,0.1,"rayleighMass","Rayleigh damping coefficient related to mass"))
    , f_velocityDamping( dataField(&f_velocityDamping,0.,"vdamping","Velocity decay coefficient (no decay if null)") )
{
    //     maxCGIter = 25;
    //     smallDenominatorThreshold = 1e-5;
    //     tolerance = 1e-5;
    //     rayleighStiffness = 0.1;
    //     rayleighMass = 0.1;
    //     velocityDamping = 0;

}

// CGImplicitSolver* CGImplicitSolver::setMaxIter( int n )
// {
//     maxCGIter = n;
//     return this;
// }

void CGImplicitSolver::solve(double dt)
{
    CGImplicitSolver* group = this;
    MultiVector pos(group, VecId::position());
    MultiVector vel(group, VecId::velocity());
//     MultiVector dx(group, VecId::dx());
    MultiVector f(group, VecId::force());
    MultiVector b(group, V_DERIV);
    MultiVector p(group, V_DERIV);
    MultiVector q(group, V_DERIV);
    MultiVector q2(group, V_DERIV);
    MultiVector r(group, V_DERIV);
    MultiVector x(group, V_DERIV);
    //MultiVector z(group, V_DERIV);
    double h = dt;
    bool printLog = f_printLog.getValue();

    // compute the right-hand term of the equation system
    group->computeForce(b);             // b = f0
    group->propagateDx(vel);            // dx = v
    group->computeDf(f);                // f = df/dx v
    b.peq(f,h+f_rayleighStiffness.getValue());      // b = f0 + (h+rs)df/dx v

    if (f_rayleighMass.getValue() != 0.0)
    {
        f.clear();
        group->addMdx(f,vel);
        b.peq(f,-f_rayleighMass.getValue());     // b = f0 + (h+rs)df/dx v - rd M v
    }

    b.teq(h);                           // b = h(f0 + (h+rs)df/dx v - rd M v)
    group->projectResponse(b);          // b is projected to the constrained space

    double normb = sqrt(b.dot(b));

    // -- solve the system using a conjugate gradient solution
    double rho, rho_1=0, alpha, beta;
    group->v_clear( x );
    group->v_eq(r,b); // initial residual

    if( printLog )
    {
        cerr<<"CGImplicitSolver, dt = "<< dt <<endl;
        cerr<<"CGImplicitSolver, initial x = "<< pos <<endl;
        cerr<<"CGImplicitSolver, initial v = "<< vel <<endl;
        cerr<<"CGImplicitSolver, f0 = "<< b <<endl;
        cerr<<"CGImplicitSolver, r0 = "<< r <<endl;
    }

    unsigned nb_iter;
    const char* endcond = "iterations";
    for( nb_iter=1; nb_iter<=f_maxIter.getValue(); nb_iter++ )
    {

        //z = r; // no precond
        //rho = r.dot(z);
        rho = r.dot(r);

        if( nb_iter==1 )
            p = r; //z;
        else
        {
            beta = rho / rho_1;
            p *= beta;
            p += r; //z;
        }

// 		cerr<<"p : "<<p<<endl;

        // matrix-vector product
        group->propagateDx(p);          // dx = p

        group->computeDf(q);            // q = df/dx p

// 		cerr<<"computeDf(q) : "<<q<<endl;

        q *= -h*(h+f_rayleighStiffness.getValue());  // q = -h(h+rs) df/dx p
//
// 		cerr<<"-h(h+rs) df/dx p : "<<q<<endl;
// 		cerr<<"f_rayleighMass.getValue() : "<<f_rayleighMass.getValue()<<endl;

        if (f_rayleighMass.getValue()==0.0)
            group->addMdx( q, p);           // q = Mp -h(h+rs) df/dx p
        else
        {
            q2.clear();
            group->addMdx( q2, p);
            q.peq(q2,(1+h*f_rayleighMass.getValue())); // q = Mp -h(h+rs) df/dx p +hr Mp  =  (M + dt(rd M + rs K) + dt2 K) dx
        }
        // filter the product to take the constraints into account
//
// 	   cerr<<"q : "<<q<<endl;

        group->projectResponse(q);     // q is projected to the constrained space

        double den = p.dot(q);

        if( fabs(den)<f_smallDenominatorThreshold.getValue() )
        {
            endcond = "threshold";
            if( printLog )
            {
//                 cerr<<"CGImplicitSolver, den = "<<den<<", smallDenominatorThreshold = "<<f_smallDenominatorThreshold.getValue()<<endl;
            }
            break;
        }
        alpha = rho/den;
        x.peq(p,alpha);                 // x = x + alpha p
        r.peq(q,-alpha);                // r = r - alpha r

        double normr = sqrt(r.dot(r));
        if (normr/normb <= f_tolerance.getValue())
        {
            endcond = "tolerance";
            break;
        }
        rho_1 = rho;
    }
    // x is the solution of the system

    // apply the solution
    vel.peq( x );                       // vel = vel + x
    pos.peq( vel, h );                  // pos = pos + h vel
    if (f_velocityDamping.getValue()!=0.0)
        vel *= exp(-h*f_velocityDamping.getValue());

    if( printLog )
    {
        cerr<<"CGImplicitSolver::solve, nbiter = "<<nb_iter<<" stop because of "<<endcond<<endl;
        cerr<<"CGImplicitSolver::solve, solution = "<<x<<endl;
        cerr<<"CGImplicitSolver, final x = "<< pos <<endl;
        cerr<<"CGImplicitSolver, final v = "<< vel <<endl;
    }
}

void create(CGImplicitSolver*& obj, ObjectDescription* arg)
{
    obj = new CGImplicitSolver();
    obj->parseFields( arg->getAttributeMap() );
}

SOFA_DECL_CLASS(CGImplicit)

Creator<ObjectFactory, CGImplicitSolver> CGImplicitSolverClass("CGImplicit");

} // namespace Components

} // namespace Sofa

<|endoftext|>


<commit_before>//******************************************************************************
// scanner.C
//
// scanner() is used by parser to return the elements of an expression one at
// a time.
//
//******************************************************************************

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include "condor_exprtype.h"
#include "condor_scanner.h"

static int 	MAXVARNAME = 100;

////////////////////////////////////////////////////////////////////////////////
// Implementation of class Token
////////////////////////////////////////////////////////////////////////////////

Token::Token()
{
	length = 0;
	type = NOT_KEYWORD;
	strVal = NULL;
	isString = FALSE;
}

Token::~Token()
{
	if(isString)
	{
		delete []strVal;
	}
}

////////////////////////////////////////////////////////////////////////////////
// This function reads input from a string, returns a token;
////////////////////////////////////////////////////////////////////////////////
void Scanner(char*& s, Token& t)
{
    char	str[MAXVARNAME];
    char*	tmp;		// working variable

    // skip white space
    t.length = 0;
    while(*s == ' ')
    {
        s = s + 1;
		t.length++;
    }

    if(!strncmp(s, "NULL", 4) && !isalpha(*(s+4)) && *(s+4) != '_')
    // NULL
    {
        s = s + 4;
		t.length = t.length + 4;
        t.type = LX_NULL;
		return;
    }
    if(!strncmp(s, "TRUE", 4) && !isalpha(*(s+4)) && *(s+4) != '_')
    // TRUE
    {
        s = s + 4;
		t.length = t.length + 4;
        t.intVal = 1;
        t.type = LX_BOOL;
		return;
    }
    if(!strncmp(s, "FALSE", 5) && !isalpha(*(s+5)) && *(s+5) != '_')
    // FALSE
    {
        s = s + 5;
		t.length = t.length + 5;
        t.intVal = 0;
        t.type = LX_BOOL;
		return;
    }
    if(isalpha(*s) || *s == '_')
    // token is a variable
    {
        tmp = str;
        // prefix, if any.
		if(!strncmp(s, "MY.", 3) && (isalpha(*(s+3)) || *(s+3) == '_'))
		{
			for(int i=0; i<3; i++)
			{
				*tmp = *s;
				s++;
				tmp++;
				t.length++;
			}
		}
		else if(!strncmp(s, "TARGET.", 7) && (isalpha(*(s+7)) || *(s+7) == '_'))
		{
			for(int i=0; i<7; i++)
			{
				*tmp = *s;
				s++;
				tmp++;
				t.length++;
			}
		}
		// the rest of the variable.
        while(isalnum(*s) || *s == '_')
		{
			*tmp = *s;
			s++;
            tmp++;
			t.length++;
        }
        *tmp = '\0';
        t.strVal = new char[strlen(str) + 1];
		strcpy(t.strVal, str);
        t.type = LX_VARIABLE;
		t.isString = TRUE;
		return;
    }
    if(isdigit(*s))
    // token is an integer or a floating point number
    {
		// count the length of the number
        tmp = s;
        while(isdigit(*tmp))
		{
			tmp++;
			t.length++;
		}
        if(*tmp == '.')
        // token is a floating point number
        {
			t.length++;
			for(tmp++; isdigit(*tmp); tmp++) t.length++;
            t.floatVal = strtod(s, &s);
            t.type = LX_FLOAT;
        }
		else
		// token is an integer number
		{
            t.intVal = strtol(s, &s, 10);
            t.type = LX_INTEGER;
        }
		return;
    }
    if(*s == '"' || (*s == '\\' && *(s+1) == '"'))
    // token is a string
    {
        s++;
		tmp = str;
		t.length++;
        while(*s != '"' && *s != '\0')
		{
			*tmp = *s;
			s++;
            tmp++;
			t.length++;
        }
        if(*s == '\0')
		{
			 t.type = LX_ERROR;
			 t.length = 0;
			 return;
		}
        s++;
		t.length++;
        *tmp = '\0';
		t.strVal = new char[strlen(str) + 1];
		strcpy(t.strVal, str);
        t.type = LX_STRING;
		t.isString = TRUE;
		return;
    }

    // token is an operator or a unit
	switch(*s)
	{
		case '(': t.type = LX_LPAREN;
			   s = s + 1;
			   t.length++;
			   break;

		case ')': t.type = LX_RPAREN;
			   s = s + 1;
			   t.length++;
			   break;

		case '+': t.type = LX_ADD;
			   s = s + 1;
			   t.length++;
			   break;

		case '-': t.type = LX_SUB;
			   s = s + 1;
			   t.length++;
			   break;

		case '*': t.type = LX_MULT;
			   s = s + 1;
			   t.length++;
			   break;

		case '/': t.type = LX_DIV;
			   s = s + 1;
			   t.length++;
			   break;

		case '<': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   s = s + 1;
				   t.length++;
				   t.type = LX_LE;
			   }
			   else
			   {
				   t.type = LX_LT;
			   }
			   break;

		case '>': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   s = s + 1;
				   t.length++;
				   t.type = LX_GE;
			   }
			   else
			   {
				   t.type = LX_GT;
			   }
			   break;

		case '&': s = s + 1;
			   t.length++;
			   if(*s == '&')
			   {
				   t.type = LX_AND;
				   s = s + 1;
				   t.length++;
			   }
			   else
			   {
				   t.type = LX_ERROR;
			   }
			   break;

		case '|': s = s + 1;
			   t.length++;
			   if(*s == '|')
			   {
					t.type = LX_OR;
					s = s + 1;
			   }
			   else
			   {
				   t.type = LX_ERROR;
			   }
			   break;

		case '\0':
		case '\n': t.type = LX_EOF;
				break;

		case '!': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   t.type = LX_NEQ;
				   s = s + 1;
				   t.length++;
			   }
			   else
			   {
				   t.type = LX_ERROR;
			   }
			   break;

		case '=': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   t.type = LX_EQ;
				   s = s + 1;
				   t.length++;
			   }
			   else
			   {
				   t.type = LX_ASSIGN;
			   }
			   break;

		case '$': t.type = LX_MACRO;
			   s = s + 1;
			   t.length++;
			   break;

		default:	t.type = LX_ERROR;
					break;
	}
}
<commit_msg>changed MAXVARNAME to 256 instead of 100 which is causing segmentation fault<commit_after>//******************************************************************************
// scanner.C
//
// scanner() is used by parser to return the elements of an expression one at
// a time.
//
//******************************************************************************

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include "condor_exprtype.h"
#include "condor_scanner.h"

static int 	MAXVARNAME = 256;

////////////////////////////////////////////////////////////////////////////////
// Implementation of class Token
////////////////////////////////////////////////////////////////////////////////

Token::Token()
{
	length = 0;
	type = NOT_KEYWORD;
	strVal = NULL;
	isString = FALSE;
}

Token::~Token()
{
	if(isString)
	{
		delete []strVal;
	}
}

////////////////////////////////////////////////////////////////////////////////
// This function reads input from a string, returns a token;
////////////////////////////////////////////////////////////////////////////////
void Scanner(char*& s, Token& t)
{
    char	str[MAXVARNAME];
    char*	tmp;		// working variable

    // skip white space
    t.length = 0;
    while(*s == ' ')
    {
        s = s + 1;
		t.length++;
    }

    if(!strncmp(s, "NULL", 4) && !isalpha(*(s+4)) && *(s+4) != '_')
    // NULL
    {
        s = s + 4;
		t.length = t.length + 4;
        t.type = LX_NULL;
		return;
    }
    if(!strncmp(s, "TRUE", 4) && !isalpha(*(s+4)) && *(s+4) != '_')
    // TRUE
    {
        s = s + 4;
		t.length = t.length + 4;
        t.intVal = 1;
        t.type = LX_BOOL;
		return;
    }
    if(!strncmp(s, "FALSE", 5) && !isalpha(*(s+5)) && *(s+5) != '_')
    // FALSE
    {
        s = s + 5;
		t.length = t.length + 5;
        t.intVal = 0;
        t.type = LX_BOOL;
		return;
    }
    if(isalpha(*s) || *s == '_')
    // token is a variable
    {
        tmp = str;
        // prefix, if any.
		if(!strncmp(s, "MY.", 3) && (isalpha(*(s+3)) || *(s+3) == '_'))
		{
			for(int i=0; i<3; i++)
			{
				*tmp = *s;
				s++;
				tmp++;
				t.length++;
			}
		}
		else if(!strncmp(s, "TARGET.", 7) && (isalpha(*(s+7)) || *(s+7) == '_'))
		{
			for(int i=0; i<7; i++)
			{
				*tmp = *s;
				s++;
				tmp++;
				t.length++;
			}
		}
		// the rest of the variable.
        while(isalnum(*s) || *s == '_')
		{
			*tmp = *s;
			s++;
            tmp++;
			t.length++;
        }
        *tmp = '\0';
        t.strVal = new char[strlen(str) + 1];
		strcpy(t.strVal, str);
        t.type = LX_VARIABLE;
		t.isString = TRUE;
		return;
    }
    if(isdigit(*s))
    // token is an integer or a floating point number
    {
		// count the length of the number
        tmp = s;
        while(isdigit(*tmp))
		{
			tmp++;
			t.length++;
		}
        if(*tmp == '.')
        // token is a floating point number
        {
			t.length++;
			for(tmp++; isdigit(*tmp); tmp++) t.length++;
            t.floatVal = strtod(s, &s);
            t.type = LX_FLOAT;
        }
		else
		// token is an integer number
		{
            t.intVal = strtol(s, &s, 10);
            t.type = LX_INTEGER;
        }
		return;
    }
    if(*s == '"' || (*s == '\\' && *(s+1) == '"'))
    // token is a string
    {
        s++;
		tmp = str;
		t.length++;
        while(*s != '"' && *s != '\0')
		{
			*tmp = *s;
			s++;
            tmp++;
			t.length++;
        }
        if(*s == '\0')
		{
			 t.type = LX_ERROR;
			 t.length = 0;
			 return;
		}
        s++;
		t.length++;
        *tmp = '\0';
		t.strVal = new char[strlen(str) + 1];
		strcpy(t.strVal, str);
        t.type = LX_STRING;
		t.isString = TRUE;
		return;
    }

    // token is an operator or a unit
	switch(*s)
	{
		case '(': t.type = LX_LPAREN;
			   s = s + 1;
			   t.length++;
			   break;

		case ')': t.type = LX_RPAREN;
			   s = s + 1;
			   t.length++;
			   break;

		case '+': t.type = LX_ADD;
			   s = s + 1;
			   t.length++;
			   break;

		case '-': t.type = LX_SUB;
			   s = s + 1;
			   t.length++;
			   break;

		case '*': t.type = LX_MULT;
			   s = s + 1;
			   t.length++;
			   break;

		case '/': t.type = LX_DIV;
			   s = s + 1;
			   t.length++;
			   break;

		case '<': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   s = s + 1;
				   t.length++;
				   t.type = LX_LE;
			   }
			   else
			   {
				   t.type = LX_LT;
			   }
			   break;

		case '>': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   s = s + 1;
				   t.length++;
				   t.type = LX_GE;
			   }
			   else
			   {
				   t.type = LX_GT;
			   }
			   break;

		case '&': s = s + 1;
			   t.length++;
			   if(*s == '&')
			   {
				   t.type = LX_AND;
				   s = s + 1;
				   t.length++;
			   }
			   else
			   {
				   t.type = LX_ERROR;
			   }
			   break;

		case '|': s = s + 1;
			   t.length++;
			   if(*s == '|')
			   {
					t.type = LX_OR;
					s = s + 1;
			   }
			   else
			   {
				   t.type = LX_ERROR;
			   }
			   break;

		case '\0':
		case '\n': t.type = LX_EOF;
				break;

		case '!': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   t.type = LX_NEQ;
				   s = s + 1;
				   t.length++;
			   }
			   else
			   {
				   t.type = LX_ERROR;
			   }
			   break;

		case '=': s = s + 1;
			   t.length++;
			   if(*s == '=')
			   {
				   t.type = LX_EQ;
				   s = s + 1;
				   t.length++;
			   }
			   else
			   {
				   t.type = LX_ASSIGN;
			   }
			   break;

		case '$': t.type = LX_MACRO;
			   s = s + 1;
			   t.length++;
			   break;

		default:	t.type = LX_ERROR;
					break;
	}
}
<|endoftext|>


<commit_before>/***************************************************************
 *
 * Copyright (C) 1990-2020, Condor Team, Computer Sciences Department,
 * University of Wisconsin-Madison, WI.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You may
 * obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 ***************************************************************/

#include "condor_common.h"
#include "condor_config.h"
#include "match_prefix.h"
#include <termios.h>
#include <unistd.h>
#include <grp.h>
#include <signal.h>
#include <algorithm>

// condor_nsenter
//
// This is a replacement for the linux nsenter command to launch a
// shell inside a container.  We need this when running
// condor_ssh_to_job to a job that has been launched inside singularity
// Docker jobs use docker exec to enter the container, but there is no
// equivalent in singularity.  Standard nsenter isn't sufficient, as it
// does not create a pty for the shell, and we need different command
// line arguments to enter a non-setuid singularity container.  This
// is harder because the starter can't tell if singularity is setuid easily.
//
// The architecture for ssh-to-job to a contained job is to land in an sshd
// the starter forks *outside* the container.  This is important because we
// never want to assume anything about the container, even that there is an sshd
// inside it.  After the sshd starts, a script runs which runs condor_docker_enter
// (even for singularity), which connects to a Unix Domain Socket, passes
// stdin/out/err to the starter, which passes those to condor_nsenter, which
// is runs as root.  Rootly privilege is required to enter a setuid namespace
// so this is how we acquire.
//
// condor_nsenter enters the namespace, taking care to try to enter the user
// namespace, which is only set up for non-setuid singularity, and drops
// privileges to the uid and gid passed on the command line.  It sets up
// a pty for the shell to use, so all interactive processses will work.
//
// A final problem is environment variables.  Singularity, especially when
// managing GPUs, sets some nvidia-related environment variables that the
// condor starter doesn't know about.  So, condor_nsenter extracts the environment
// variables from the contained job process, and sets them in the shell
// it spawns.

void
usage( char *cmd )
{
	fprintf(stderr,"Usage: %s [options] condor_* ....\n",cmd);
	fprintf(stderr,"Where options are:\n");
	fprintf(stderr,"-t target_pid:\n");
	fprintf(stderr,"-S user_id:\n");
	fprintf(stderr,"-G group_id:\n");
	fprintf(stderr,"    -help              Display options\n");
}

// Before we exit, we need to reset the pty back to normal
// if we put it in raw mode

bool pty_is_raw = false;
struct termios old_tio;
void reset_pty_and_exit(int signo) {
	if (pty_is_raw)
		tcsetattr(0, TCSAFLUSH, &old_tio);
	exit(signo);
}

int main( int argc, char *argv[] )
{
	std::string condor_prefix;
	pid_t pid = 0;
	uid_t uid = 0;
	gid_t gid = 0;

	// parse command line args
	for( int i=1; i<argc; i++ ) {
		if(is_arg_prefix(argv[i],"-help")) {
			usage(argv[0]);
			exit(1);
		}

		// target pid to enter
		if(is_arg_prefix(argv[i],"-t")) {
			pid = atoi(argv[i + 1]);
			i++;
		}

		// uid to switch to
		if(is_arg_prefix(argv[i],"-S")) {
			uid = atoi(argv[i + 1]);
			i++;
		}

		// gid to switch to
		if(is_arg_prefix(argv[i],"-G")) {
			gid = atoi(argv[i + 1]);
			i++;
		}
	}

	if (pid < 1) {
		fprintf(stderr, "missing -t argument > 1\n");
		exit(1);
	}

	if (uid == 0) {
		fprintf(stderr, "missing -S argument > 1\n");
		exit(1);
	}

	if (gid == 0) {
		fprintf(stderr, "missing -G argument > 1\n");
		exit(1);
	}

	// slurp the enviroment out of our victim
	std::string env;
	std::string envfile;
	formatstr(envfile, "/proc/%d/environ", pid);

	int e = open(envfile.c_str(), O_RDONLY);
	if (e < 0) {
		fprintf(stderr, "Can't open %s %s\n", envfile.c_str(), strerror(errno));
		exit(1);
	}
	char buf[512];
	int bytesRead;
	while ((bytesRead = read(e, &buf, 512)) > 0) {
		env.append(buf, bytesRead);
	}
	close(e);

	// make a vector to hold all the pointers to env entries
	std::vector<const char *> envp;

	// the first one
	envp.push_back(env.c_str());
	auto it = env.cbegin();
	while (env.cend() != (it = std::find(it, env.cend(), '\0'))) {
		// skip past null terminator
		it++;
		if (& (*it)  != nullptr) {
			envp.push_back(& (*it));
		}
	}
	envp.push_back(nullptr);
	envp.push_back(nullptr);

	// grab the fd for the cwd -- need to get this outside
	// but chdir inside the container
	std::string cwdPath;
	formatstr(cwdPath, "/proc/%d/cwd", pid);
	int rootFd = open(cwdPath.c_str(), O_RDONLY);
	if (rootFd < 0) {
		fprintf(stderr, "Can't open %s\n", cwdPath.c_str());
	}

	std::string filename;

	// start changing namespaces.  Note that once we do this, things
	// get funny in this process
	formatstr(filename, "/proc/%d/ns/uts", pid);
	int fd = open(filename.c_str(), O_RDONLY);
	if (fd < 0) {
		fprintf(stderr, "Can't open uts namespace: %d %s\n", errno, strerror(errno));
		exit(1);
	}
	int r = setns(fd, 0);
	close(fd);
	if (r < 0) {
		// This means an unprivileged singularity, most likely
		// need to set user namespace instead.
		formatstr(filename, "/proc/%d/ns/user", pid);
		fd = open(filename.c_str(), O_RDONLY);
		if (fd < 0) {
			fprintf(stderr, "Can't open user namespace: %d %s\n", errno, strerror(errno));
			exit(1);
		}
		r = setns(fd, 0);
		close(fd);
		if (r < 0) {
			fprintf(stderr, "Can't setns to user namespace: %s\n", strerror(errno));
			exit(1);
		}
	}

	// now the pid namespace
	formatstr(filename, "/proc/%d/ns/pid", pid);
	fd = open(filename.c_str(), O_RDONLY);
	if (fd < 0) {
		fprintf(stderr, "Can't open pid namespace: %s\n", strerror(errno));
		exit(1);
	}
	r = setns(fd, 0);
	close(fd);
	if (r < 0) {
		fprintf(stderr, "Can't setns to pid namespace: %s\n", strerror(errno));
		exit(1);
	}

	// finally the mnt namespace
	formatstr(filename, "/proc/%d/ns/mnt", pid);
	fd = open(filename.c_str(), O_RDONLY);
	if (fd < 0) {
		fprintf(stderr, "Can't open mnt namespace: %s\n", strerror(errno));
		exit(1);
	}
	r = setns(fd, 0);
	close(fd);
	if (r < 0) {
		fprintf(stderr, "Can't setns to mnt namespace: %s\n", strerror(errno));
		exit(1);
	}

	setgroups(0, nullptr);

	// order matters!
	r = setgid(gid);
	if (r < 0) {
		fprintf(stderr, "Can't setgid to %d\n", gid);
		exit(1);
	}
	r = setuid(uid);
	if (r < 0) {
		fprintf(stderr, "Can't setuid to %d\n", uid);
		exit(1);
	}

	// now the pty handling
	int masterPty = -1;
	int workerPty = -1;
	masterPty = open("/dev/ptmx", O_RDWR);
	unlockpt(masterPty);

	if (masterPty < 0) {
		fprintf(stderr, "Can't open master pty %s\n", strerror(errno));
		exit(1);
	} else {
		workerPty = open(ptsname(masterPty), O_RDWR);
		if (workerPty < 0) {
			fprintf(stderr, "Can't open worker pty %s\n", strerror(errno));
			exit(1);
		}
	}
	int childpid = fork();
	if (childpid == 0) {

		// in the child --

		close(0);
		close(1);
		close(2);
		close(masterPty);

		dup2(workerPty, 0);
		dup2(workerPty, 1);
		dup2(workerPty, 2);

		// chdir to existing cwd
		int ret = fchdir(rootFd);
		if (ret < 0) {
			fprintf(stderr, "Can't open %s\n", cwdPath.c_str());
		}
		close(rootFd);

		// make this process group leader so shell job control works
		setsid();

		execle("/bin/sh", "/bin/sh", "-i", nullptr, envp.data());

		// Only get here if exec fails
		fprintf(stderr, "exec failed %d\n", errno);
		exit(errno);

	} else {

		// the parent
		fd_set readfds, writefds, exceptfds;
		bool keepGoing = true;

		// put the pty in raw mode
		struct termios tio;
		tcgetattr(0, &tio);
		pty_is_raw = true;
		old_tio = tio;
		tio.c_oflag &= ~(OPOST);
		tio.c_cflag |= (CS8);
		tio.c_lflag &= ~(ECHO | ICANON | IEXTEN | ISIG);
		tio.c_cc[VMIN] = 1;
		tio.c_cc[VTIME] = 0;
		tcsetattr(0, TCSAFLUSH, &tio);

		struct sigaction handler;
		struct sigaction oldhandler;
		handler.sa_handler = reset_pty_and_exit;

		sigaction(SIGCHLD, &handler, &oldhandler);

		while (keepGoing) {
			FD_ZERO(&readfds);
			FD_ZERO(&writefds);
			FD_ZERO(&exceptfds);

			FD_SET(0, &readfds);
			FD_SET(masterPty, &readfds);

			select(masterPty + 1, &readfds, &writefds, &exceptfds, nullptr);

			if (FD_ISSET(masterPty, &readfds)) {
				char buf;
				int r = read(masterPty, &buf, 1);
				if (r > 0) {
					int ret = write(1, &buf, 1);
					if (ret < 0) {
						reset_pty_and_exit(0);
					}
				} else {
					keepGoing = false;
				}
			}

			if (FD_ISSET(0, &readfds)) {
				char buf;
				int r = read(0, &buf, 1);
				if (r > 0) {
					int ret = write(masterPty, &buf, 1);
					if (ret < 0) {
						reset_pty_and_exit(0);
					}
				} else {
					keepGoing = false;
				}
			}
		}
		int status;
		waitpid(childpid, &status, 0);
	}
	return 0;
}

<commit_msg>Port nsenter to el6 #7666<commit_after>/***************************************************************
 *
 * Copyright (C) 1990-2020, Condor Team, Computer Sciences Department,
 * University of Wisconsin-Madison, WI.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You may
 * obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 ***************************************************************/

#include "condor_common.h"
#include "condor_config.h"
#include "match_prefix.h"
#include <termios.h>
#include <unistd.h>
#include <grp.h>
#include <signal.h>
#include <algorithm>

// condor_nsenter
//
// This is a replacement for the linux nsenter command to launch a
// shell inside a container.  We need this when running
// condor_ssh_to_job to a job that has been launched inside singularity
// Docker jobs use docker exec to enter the container, but there is no
// equivalent in singularity.  Standard nsenter isn't sufficient, as it
// does not create a pty for the shell, and we need different command
// line arguments to enter a non-setuid singularity container.  This
// is harder because the starter can't tell if singularity is setuid easily.
//
// The architecture for ssh-to-job to a contained job is to land in an sshd
// the starter forks *outside* the container.  This is important because we
// never want to assume anything about the container, even that there is an sshd
// inside it.  After the sshd starts, a script runs which runs condor_docker_enter
// (even for singularity), which connects to a Unix Domain Socket, passes
// stdin/out/err to the starter, which passes those to condor_nsenter, which
// is runs as root.  Rootly privilege is required to enter a setuid namespace
// so this is how we acquire.
//
// condor_nsenter enters the namespace, taking care to try to enter the user
// namespace, which is only set up for non-setuid singularity, and drops
// privileges to the uid and gid passed on the command line.  It sets up
// a pty for the shell to use, so all interactive processses will work.
//
// A final problem is environment variables.  Singularity, especially when
// managing GPUs, sets some nvidia-related environment variables that the
// condor starter doesn't know about.  So, condor_nsenter extracts the environment
// variables from the contained job process, and sets them in the shell
// it spawns.

void
usage( char *cmd )
{
	fprintf(stderr,"Usage: %s [options] condor_* ....\n",cmd);
	fprintf(stderr,"Where options are:\n");
	fprintf(stderr,"-t target_pid:\n");
	fprintf(stderr,"-S user_id:\n");
	fprintf(stderr,"-G group_id:\n");
	fprintf(stderr,"    -help              Display options\n");
}

#if (__GLIBC__ == 2) && (__GLIBC_MINOR__ < 15)
// el6 doesn't have setns.  What are we going to do?
// what can we do?  Just not enter the container
int setns(int , int) {
        return 0;
}
#endif

// Before we exit, we need to reset the pty back to normal
// if we put it in raw mode

bool pty_is_raw = false;
struct termios old_tio;
void reset_pty_and_exit(int signo) {
	if (pty_is_raw)
		tcsetattr(0, TCSAFLUSH, &old_tio);
	exit(signo);
}

int main( int argc, char *argv[] )
{
	std::string condor_prefix;
	pid_t pid = 0;
	uid_t uid = 0;
	gid_t gid = 0;

	// parse command line args
	for( int i=1; i<argc; i++ ) {
		if(is_arg_prefix(argv[i],"-help")) {
			usage(argv[0]);
			exit(1);
		}

		// target pid to enter
		if(is_arg_prefix(argv[i],"-t")) {
			pid = atoi(argv[i + 1]);
			i++;
		}

		// uid to switch to
		if(is_arg_prefix(argv[i],"-S")) {
			uid = atoi(argv[i + 1]);
			i++;
		}

		// gid to switch to
		if(is_arg_prefix(argv[i],"-G")) {
			gid = atoi(argv[i + 1]);
			i++;
		}
	}

	if (pid < 1) {
		fprintf(stderr, "missing -t argument > 1\n");
		exit(1);
	}

	if (uid == 0) {
		fprintf(stderr, "missing -S argument > 1\n");
		exit(1);
	}

	if (gid == 0) {
		fprintf(stderr, "missing -G argument > 1\n");
		exit(1);
	}

	// slurp the enviroment out of our victim
	std::string env;
	std::string envfile;
	formatstr(envfile, "/proc/%d/environ", pid);

	int e = open(envfile.c_str(), O_RDONLY);
	if (e < 0) {
		fprintf(stderr, "Can't open %s %s\n", envfile.c_str(), strerror(errno));
		exit(1);
	}
	char buf[512];
	int bytesRead;
	while ((bytesRead = read(e, &buf, 512)) > 0) {
		env.append(buf, bytesRead);
	}
	close(e);

	// make a vector to hold all the pointers to env entries
	std::vector<const char *> envp;

	// the first one
	envp.push_back(env.c_str());
	auto it = env.begin();
	while (env.end() != (it = std::find(it, env.end(), '\0'))) {
		// skip past null terminator
		it++;
		if (& (*it)  != 0) {
			envp.push_back(& (*it));
		}
	}
	envp.push_back(0);
	envp.push_back(0);

	// grab the fd for the cwd -- need to get this outside
	// but chdir inside the container
	std::string cwdPath;
	formatstr(cwdPath, "/proc/%d/cwd", pid);
	int rootFd = open(cwdPath.c_str(), O_RDONLY);
	if (rootFd < 0) {
		fprintf(stderr, "Can't open %s\n", cwdPath.c_str());
	}

	std::string filename;

	// start changing namespaces.  Note that once we do this, things
	// get funny in this process
	formatstr(filename, "/proc/%d/ns/uts", pid);
	int fd = open(filename.c_str(), O_RDONLY);
	if (fd < 0) {
		fprintf(stderr, "Can't open uts namespace: %d %s\n", errno, strerror(errno));
		exit(1);
	}
	int r = setns(fd, 0);
	close(fd);
	if (r < 0) {
		// This means an unprivileged singularity, most likely
		// need to set user namespace instead.
		formatstr(filename, "/proc/%d/ns/user", pid);
		fd = open(filename.c_str(), O_RDONLY);
		if (fd < 0) {
			fprintf(stderr, "Can't open user namespace: %d %s\n", errno, strerror(errno));
			exit(1);
		}
		r = setns(fd, 0);
		close(fd);
		if (r < 0) {
			fprintf(stderr, "Can't setns to user namespace: %s\n", strerror(errno));
			exit(1);
		}
	}

	// now the pid namespace
	formatstr(filename, "/proc/%d/ns/pid", pid);
	fd = open(filename.c_str(), O_RDONLY);
	if (fd < 0) {
		fprintf(stderr, "Can't open pid namespace: %s\n", strerror(errno));
		exit(1);
	}
	r = setns(fd, 0);
	close(fd);
	if (r < 0) {
		fprintf(stderr, "Can't setns to pid namespace: %s\n", strerror(errno));
		exit(1);
	}

	// finally the mnt namespace
	formatstr(filename, "/proc/%d/ns/mnt", pid);
	fd = open(filename.c_str(), O_RDONLY);
	if (fd < 0) {
		fprintf(stderr, "Can't open mnt namespace: %s\n", strerror(errno));
		exit(1);
	}
	r = setns(fd, 0);
	close(fd);
	if (r < 0) {
		fprintf(stderr, "Can't setns to mnt namespace: %s\n", strerror(errno));
		exit(1);
	}

	setgroups(0, 0);

	// order matters!
	r = setgid(gid);
	if (r < 0) {
		fprintf(stderr, "Can't setgid to %d\n", gid);
		exit(1);
	}
	r = setuid(uid);
	if (r < 0) {
		fprintf(stderr, "Can't setuid to %d\n", uid);
		exit(1);
	}

	// now the pty handling
	int masterPty = -1;
	int workerPty = -1;
	masterPty = open("/dev/ptmx", O_RDWR);
	unlockpt(masterPty);

	if (masterPty < 0) {
		fprintf(stderr, "Can't open master pty %s\n", strerror(errno));
		exit(1);
	} else {
		workerPty = open(ptsname(masterPty), O_RDWR);
		if (workerPty < 0) {
			fprintf(stderr, "Can't open worker pty %s\n", strerror(errno));
			exit(1);
		}
	}
	int childpid = fork();
	if (childpid == 0) {

		// in the child --

		close(0);
		close(1);
		close(2);
		close(masterPty);

		dup2(workerPty, 0);
		dup2(workerPty, 1);
		dup2(workerPty, 2);

		// chdir to existing cwd
		int ret = fchdir(rootFd);
		if (ret < 0) {
			fprintf(stderr, "Can't open %s\n", cwdPath.c_str());
		}
		close(rootFd);

		// make this process group leader so shell job control works
		setsid();

		execle("/bin/sh", "/bin/sh", "-i", 0, envp.data());

		// Only get here if exec fails
		fprintf(stderr, "exec failed %d\n", errno);
		exit(errno);

	} else {

		// the parent
		fd_set readfds, writefds, exceptfds;
		bool keepGoing = true;

		// put the pty in raw mode
		struct termios tio;
		tcgetattr(0, &tio);
		pty_is_raw = true;
		old_tio = tio;
		tio.c_oflag &= ~(OPOST);
		tio.c_cflag |= (CS8);
		tio.c_lflag &= ~(ECHO | ICANON | IEXTEN | ISIG);
		tio.c_cc[VMIN] = 1;
		tio.c_cc[VTIME] = 0;
		tcsetattr(0, TCSAFLUSH, &tio);

		struct sigaction handler;
		struct sigaction oldhandler;
		handler.sa_handler = reset_pty_and_exit;

		sigaction(SIGCHLD, &handler, &oldhandler);

		while (keepGoing) {
			FD_ZERO(&readfds);
			FD_ZERO(&writefds);
			FD_ZERO(&exceptfds);

			FD_SET(0, &readfds);
			FD_SET(masterPty, &readfds);

			select(masterPty + 1, &readfds, &writefds, &exceptfds, 0);

			if (FD_ISSET(masterPty, &readfds)) {
				char buf;
				int r = read(masterPty, &buf, 1);
				if (r > 0) {
					int ret = write(1, &buf, 1);
					if (ret < 0) {
						reset_pty_and_exit(0);
					}
				} else {
					keepGoing = false;
				}
			}

			if (FD_ISSET(0, &readfds)) {
				char buf;
				int r = read(0, &buf, 1);
				if (r > 0) {
					int ret = write(masterPty, &buf, 1);
					if (ret < 0) {
						reset_pty_and_exit(0);
					}
				} else {
					keepGoing = false;
				}
			}
		}
		int status;
		waitpid(childpid, &status, 0);
	}
	return 0;
}

<|endoftext|>


<commit_before>#include "ClockWindow.h"
#include <vector>

using namespace Gdiplus;

#ifdef _DEBUG
static void SaveBitmap(Bitmap* bitmap)
{
	CLSID guid;
	::CLSIDFromString(L"{557CF406-1A04-11D3-9A73-0000F81EF32E}", &guid);
	Status saveStatus = bitmap->Save(L"C:\\Users\\Hugo\\bitmap.png", &guid);
	if (saveStatus != Status::Ok)
	{
		Beep(200, 100);
		Beep(200, 100);
	}
}
#endif

static std::wstring TextTime(SYSTEMTIME& time, DWORD flag, LPCWSTR format = nullptr)
{
	int textLength = ::GetTimeFormatEx(LOCALE_NAME_USER_DEFAULT, flag, &time, nullptr, nullptr, 0);
	wchar_t* textBuffer = new wchar_t[textLength];
	::GetTimeFormatEx(LOCALE_NAME_USER_DEFAULT, flag, &time, nullptr, textBuffer, textLength);
	std::wstring text(textBuffer);
	delete textBuffer;
	return text;
}

static std::wstring TextDate(SYSTEMTIME& time, DWORD flag, LPCWSTR format = nullptr)
{
	int textLength = ::GetDateFormatEx(LOCALE_NAME_USER_DEFAULT, flag | DATE_AUTOLAYOUT, &time, format, nullptr, 0, nullptr);
	wchar_t* textBuffer = new wchar_t[textLength];
	::GetDateFormatEx(LOCALE_NAME_USER_DEFAULT, flag | DATE_AUTOLAYOUT, &time, format, textBuffer, textLength, nullptr);
	std::wstring text(textBuffer);
	delete textBuffer;
	return text;
}

void ClockWindow::RenderClickedState(Gdiplus::Graphics* graphics, int width, int height) const
{
	Rect stateRect(1, 0, width - 1, height);
	SolidBrush brush(Color(5, 255, 255, 255));
	graphics->FillRectangle(&brush, stateRect);
}

void ClockWindow::RenderHighlight(Gdiplus::Graphics* graphics, int width, int height) const
{
	int otherColorsCount = 1;

	Color centerBarColor = Color(192, 200, 200, 200);
	Color otherBarColors[] = { Color(0, 200, 200, 200) };
	// Draw left highlight bar...
	const Rect leftHightlightRect(1, 0, 2, height);
	GraphicsPath leftHighlightPath;
	leftHighlightPath.AddRectangle(leftHightlightRect);

	PathGradientBrush leftHighlightBrush(&leftHighlightPath);
	leftHighlightBrush.SetCenterColor(centerBarColor);
	leftHighlightBrush.SetSurroundColors(otherBarColors, &otherColorsCount);

	graphics->FillPath(&leftHighlightBrush, &leftHighlightPath);

	// ... and draw the right one...
	const Rect rightHightlightRect(width - 3, 0, 2, height);
	GraphicsPath rightHighlightPath;
	rightHighlightPath.AddRectangle(rightHightlightRect);

	PathGradientBrush rightHighlightBrush(&rightHighlightPath);
	rightHighlightBrush.SetCenterColor(centerBarColor);
	rightHighlightBrush.SetSurroundColors(otherBarColors, &otherColorsCount);

	graphics->FillPath(&rightHighlightBrush, &rightHighlightPath);

	// ... and the blue highlight dot
	Color centerDotColor = Color(200, 177, 211, 255);
	Color otherDotColors[] = { Color(0, 255, 255, 255) };
	GraphicsPath dotPath;
	dotPath.AddEllipse(width / 3.f, height * 0.66f, width / 3.f, (REAL)height);
	PathGradientBrush dotBursh(&dotPath);
	dotBursh.SetCenterColor(centerDotColor);
	dotBursh.SetSurroundColors(otherDotColors, &otherColorsCount);
	graphics->FillPath(&dotBursh, &dotPath);

	const Rect dotLineRect(0, height - 1, width, 1);
	Color colors[] =
	{
		otherDotColors[0],
		centerDotColor,
		otherDotColors[0]
	};
	REAL positions[] = {
		0.0f,
		0.5f,
		1.0f };
	LinearGradientBrush dotLineBrush(
		Point(dotLineRect.X, dotLineRect.Y),
		Point(dotLineRect.GetRight(), dotLineRect.GetBottom()),
		Color(255, 0, 0, 0),
		Color(255, 255, 255, 255));
	dotLineBrush.SetInterpolationColors(colors, positions, 3);
	graphics->FillRectangle(&dotLineBrush, dotLineRect);
}

void ClockWindow::RenderTime(HDC context, int maxWidth, int maxHeight) const
{
	// Get the current system font...
	NONCLIENTMETRICS metrics;
	metrics.cbSize = sizeof(NONCLIENTMETRICS);
	::SystemParametersInfo(SPI_GETNONCLIENTMETRICS, sizeof(NONCLIENTMETRICS), &metrics, 0);

	HFONT font = ::CreateFontIndirect(&metrics.lfStatusFont);
	HFONT oldFont = (HFONT)::SelectObject(context, font);
	int oldBkMode = ::SetBkMode(context, TRANSPARENT);
	int oldTextColor = ::SetTextColor(context, GetSysColor(COLOR_3DFACE));

	std::wstring timeText;
	{
		RECT textRect;
		int textRectWidth;

		SYSTEMTIME time;
		GetLocalTime(&time);

		std::wstring dateText;
		std::wstring dayText;

		timeText = TextTime(time, TIME_NOSECONDS);
		if (maxHeight >= 36)
		{
			if (maxHeight >= 53)
			{
				dayText = TextDate(time, 0, L"dddd");
				textRect = { 0, 0, maxWidth, maxHeight };

				std::vector<wchar_t> textBuffer(dayText.begin(), dayText.end());
				textBuffer.push_back('\0');
				::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CALCRECT | DT_CENTER | DT_NOCLIP, nullptr);
				textRectWidth = textRect.right - textRect.left;
				if (textRectWidth <= maxWidth)
				{
					timeText += L"\n";
					timeText += dayText;
				}
			}
			dateText = TextDate(time, DATE_SHORTDATE);
			textRect = { 0, 0, maxWidth, maxHeight };

			std::vector<wchar_t> textBuffer(dayText.begin(), dayText.end());
			textBuffer.push_back('\0');
			::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CALCRECT | DT_CENTER | DT_NOCLIP, nullptr);
			textRectWidth = textRect.right - textRect.left;
			if (textRectWidth <= maxWidth)
			{
				timeText += L"\n";
				timeText += dateText;
			}
		}
	}

	RECT textRect = { 0, 0, maxWidth, maxHeight };

	std::vector<wchar_t> textBuffer(timeText.begin(), timeText.end());
	textBuffer.push_back('\0');
	::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CALCRECT | DT_CENTER | DT_NOCLIP, nullptr);

	// move text rect to (0,0)
	::OffsetRect(&textRect, -textRect.left, -textRect.top);
	// center text rect
	::OffsetRect(&textRect, (int)((maxWidth - textRect.right) / 2.0), (int)((maxHeight - textRect.bottom) / 2.0));
	// draw
	::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CENTER | DT_NOCLIP, nullptr);

	::SetTextColor(context, oldTextColor);
	::SetBkMode(context, oldBkMode);
	::SelectObject(context, oldFont);
	::DeleteObject(font);
}

void ClockWindow::RenderHighlighting(Graphics* graphics, int width, int height) const
{
	SolidBrush burshClear(Color::Transparent);
	graphics->FillRectangle(&burshClear, 0, 0, width, height);
	if (isClicked)
	{
		RenderClickedState(graphics, width, height);
	}
	if (isHighlighted)
	{
		RenderHighlight(graphics, width, height);
	}
}

bool ClockWindow::IsVisible(const RECT& clientRect)
{
	HDC parent = ::GetDC(this->GetParent());
	RECT clip;
	int result = ::GetClipBox(parent, &clip);
	::ReleaseDC(this->GetParent(), parent);

	bool isVisible;
	switch (result)
	{
	case SIMPLEREGION:
	case COMPLEXREGION:
		{
			OUTPUT_DEBUG_STRING(L"Clock is partially or fully visible");
			RECT inParentRect(clientRect);
			MapWindowPoints(this->GetParent(), &inParentRect);

			RECT intersectRect;
			::IntersectRect(&intersectRect, &clip, &inParentRect);

			isVisible = !::IsRectEmpty(&intersectRect);
		}
		break;
	case NULLREGION:
	default:
		OUTPUT_DEBUG_STRING(L"Clock is completly covered");
		isVisible = false;
		break;
	}

	if (!isVisible)
	{
		OUTPUT_DEBUG_STRING(L"Clock not visible, ignoring refresh request.");
	}
	return isVisible;
}

void ClockWindow::Refresh(bool force)
{
	OUTPUT_DEBUG_STRING(L"Clock refresh requested. Force =", force);

	RECT clientRect;
	GetClientRect(&clientRect);

	if (!force && !IsVisible(clientRect))
	{
		return;
	}

	int width = clientRect.right - clientRect.left;
	int height = clientRect.bottom - clientRect.top;

	Bitmap* bitmap = new Bitmap(width, height, PixelFormat32bppPARGB);
	Status status = bitmap->GetLastStatus();
	if (status != Status::Ok)
	{
		delete bitmap;
		return;
	}

	Graphics* g = Graphics::FromImage(bitmap);
	RenderHighlighting(g, width, height);
	delete g;

	HBITMAP hbitmap;
	bitmap->GetHBITMAP(NULL, &hbitmap);

	HDC hdcScreen = ::GetDC(nullptr);
	HDC hDC = ::CreateCompatibleDC(hdcScreen);
	HBITMAP hbitmapOld = (HBITMAP) ::SelectObject(hDC, hbitmap);

	RenderTime(hDC, width, height);

	SIZE sizeWnd = { width, height };
	POINT ptSrc = { 0, 0 };
	BLENDFUNCTION blend = { 0 };
	blend.BlendOp = AC_SRC_OVER;
	blend.SourceConstantAlpha = 255;
	blend.AlphaFormat = AC_SRC_ALPHA;
	::UpdateLayeredWindow(this->m_hWnd, hdcScreen, nullptr, &sizeWnd, hDC, &ptSrc, 0, &blend, ULW_ALPHA);

	::SelectObject(hDC, hbitmapOld);
	::DeleteObject(hbitmap);
	::DeleteDC(hDC);
	::ReleaseDC(NULL, hdcScreen);

	delete bitmap;

	OUTPUT_DEBUG_STRING(L"Clock has been refreshed.");
}
<commit_msg>dont check visibility... doesnt work<commit_after>#include "ClockWindow.h"
#include <vector>

using namespace Gdiplus;

#ifdef _DEBUG
static void SaveBitmap(Bitmap* bitmap)
{
	CLSID guid;
	::CLSIDFromString(L"{557CF406-1A04-11D3-9A73-0000F81EF32E}", &guid);
	Status saveStatus = bitmap->Save(L"C:\\Users\\Hugo\\bitmap.png", &guid);
	if (saveStatus != Status::Ok)
	{
		Beep(200, 100);
		Beep(200, 100);
	}
}
#endif

static std::wstring TextTime(SYSTEMTIME& time, DWORD flag, LPCWSTR format = nullptr)
{
	int textLength = ::GetTimeFormatEx(LOCALE_NAME_USER_DEFAULT, flag, &time, nullptr, nullptr, 0);
	wchar_t* textBuffer = new wchar_t[textLength];
	::GetTimeFormatEx(LOCALE_NAME_USER_DEFAULT, flag, &time, nullptr, textBuffer, textLength);
	std::wstring text(textBuffer);
	delete textBuffer;
	return text;
}

static std::wstring TextDate(SYSTEMTIME& time, DWORD flag, LPCWSTR format = nullptr)
{
	int textLength = ::GetDateFormatEx(LOCALE_NAME_USER_DEFAULT, flag | DATE_AUTOLAYOUT, &time, format, nullptr, 0, nullptr);
	wchar_t* textBuffer = new wchar_t[textLength];
	::GetDateFormatEx(LOCALE_NAME_USER_DEFAULT, flag | DATE_AUTOLAYOUT, &time, format, textBuffer, textLength, nullptr);
	std::wstring text(textBuffer);
	delete textBuffer;
	return text;
}

void ClockWindow::RenderClickedState(Gdiplus::Graphics* graphics, int width, int height) const
{
	Rect stateRect(1, 0, width - 1, height);
	SolidBrush brush(Color(5, 255, 255, 255));
	graphics->FillRectangle(&brush, stateRect);
}

void ClockWindow::RenderHighlight(Gdiplus::Graphics* graphics, int width, int height) const
{
	int otherColorsCount = 1;

	Color centerBarColor = Color(192, 200, 200, 200);
	Color otherBarColors[] = { Color(0, 200, 200, 200) };
	// Draw left highlight bar...
	const Rect leftHightlightRect(1, 0, 2, height);
	GraphicsPath leftHighlightPath;
	leftHighlightPath.AddRectangle(leftHightlightRect);

	PathGradientBrush leftHighlightBrush(&leftHighlightPath);
	leftHighlightBrush.SetCenterColor(centerBarColor);
	leftHighlightBrush.SetSurroundColors(otherBarColors, &otherColorsCount);

	graphics->FillPath(&leftHighlightBrush, &leftHighlightPath);

	// ... and draw the right one...
	const Rect rightHightlightRect(width - 3, 0, 2, height);
	GraphicsPath rightHighlightPath;
	rightHighlightPath.AddRectangle(rightHightlightRect);

	PathGradientBrush rightHighlightBrush(&rightHighlightPath);
	rightHighlightBrush.SetCenterColor(centerBarColor);
	rightHighlightBrush.SetSurroundColors(otherBarColors, &otherColorsCount);

	graphics->FillPath(&rightHighlightBrush, &rightHighlightPath);

	// ... and the blue highlight dot
	Color centerDotColor = Color(200, 177, 211, 255);
	Color otherDotColors[] = { Color(0, 255, 255, 255) };
	GraphicsPath dotPath;
	dotPath.AddEllipse(width / 3.f, height * 0.66f, width / 3.f, (REAL)height);
	PathGradientBrush dotBursh(&dotPath);
	dotBursh.SetCenterColor(centerDotColor);
	dotBursh.SetSurroundColors(otherDotColors, &otherColorsCount);
	graphics->FillPath(&dotBursh, &dotPath);

	const Rect dotLineRect(0, height - 1, width, 1);
	Color colors[] =
	{
		otherDotColors[0],
		centerDotColor,
		otherDotColors[0]
	};
	REAL positions[] = {
		0.0f,
		0.5f,
		1.0f };
	LinearGradientBrush dotLineBrush(
		Point(dotLineRect.X, dotLineRect.Y),
		Point(dotLineRect.GetRight(), dotLineRect.GetBottom()),
		Color(255, 0, 0, 0),
		Color(255, 255, 255, 255));
	dotLineBrush.SetInterpolationColors(colors, positions, 3);
	graphics->FillRectangle(&dotLineBrush, dotLineRect);
}

void ClockWindow::RenderTime(HDC context, int maxWidth, int maxHeight) const
{
	// Get the current system font...
	NONCLIENTMETRICS metrics;
	metrics.cbSize = sizeof(NONCLIENTMETRICS);
	::SystemParametersInfo(SPI_GETNONCLIENTMETRICS, sizeof(NONCLIENTMETRICS), &metrics, 0);

	HFONT font = ::CreateFontIndirect(&metrics.lfStatusFont);
	HFONT oldFont = (HFONT)::SelectObject(context, font);
	int oldBkMode = ::SetBkMode(context, TRANSPARENT);
	int oldTextColor = ::SetTextColor(context, GetSysColor(COLOR_3DFACE));

	std::wstring timeText;
	{
		RECT textRect;
		int textRectWidth;

		SYSTEMTIME time;
		GetLocalTime(&time);

		std::wstring dateText;
		std::wstring dayText;

		timeText = TextTime(time, TIME_NOSECONDS);
		if (maxHeight >= 36)
		{
			if (maxHeight >= 53)
			{
				dayText = TextDate(time, 0, L"dddd");
				textRect = { 0, 0, maxWidth, maxHeight };

				std::vector<wchar_t> textBuffer(dayText.begin(), dayText.end());
				textBuffer.push_back('\0');
				::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CALCRECT | DT_CENTER | DT_NOCLIP, nullptr);
				textRectWidth = textRect.right - textRect.left;
				if (textRectWidth <= maxWidth)
				{
					timeText += L"\n";
					timeText += dayText;
				}
			}
			dateText = TextDate(time, DATE_SHORTDATE);
			textRect = { 0, 0, maxWidth, maxHeight };

			std::vector<wchar_t> textBuffer(dayText.begin(), dayText.end());
			textBuffer.push_back('\0');
			::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CALCRECT | DT_CENTER | DT_NOCLIP, nullptr);
			textRectWidth = textRect.right - textRect.left;
			if (textRectWidth <= maxWidth)
			{
				timeText += L"\n";
				timeText += dateText;
			}
		}
	}

	RECT textRect = { 0, 0, maxWidth, maxHeight };

	std::vector<wchar_t> textBuffer(timeText.begin(), timeText.end());
	textBuffer.push_back('\0');
	::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CALCRECT | DT_CENTER | DT_NOCLIP, nullptr);

	// move text rect to (0,0)
	::OffsetRect(&textRect, -textRect.left, -textRect.top);
	// center text rect
	::OffsetRect(&textRect, (int)((maxWidth - textRect.right) / 2.0), (int)((maxHeight - textRect.bottom) / 2.0));
	// draw
	::DrawTextEx(context, textBuffer.data(), -1, &textRect, DT_CENTER | DT_NOCLIP, nullptr);

	::SetTextColor(context, oldTextColor);
	::SetBkMode(context, oldBkMode);
	::SelectObject(context, oldFont);
	::DeleteObject(font);
}

void ClockWindow::RenderHighlighting(Graphics* graphics, int width, int height) const
{
	SolidBrush burshClear(Color::Transparent);
	graphics->FillRectangle(&burshClear, 0, 0, width, height);
	if (isClicked)
	{
		RenderClickedState(graphics, width, height);
	}
	if (isHighlighted)
	{
		RenderHighlight(graphics, width, height);
	}
}

bool ClockWindow::IsVisible(const RECT& clientRect)
{
	return true;
}

void ClockWindow::Refresh(bool force)
{
	OUTPUT_DEBUG_STRING(L"Clock refresh requested. Force =", force);

	RECT clientRect;
	GetClientRect(&clientRect);

	if (!force && !IsVisible(clientRect))
	{
		return;
	}

	int width = clientRect.right - clientRect.left;
	int height = clientRect.bottom - clientRect.top;

	Bitmap* bitmap = new Bitmap(width, height, PixelFormat32bppPARGB);
	Status status = bitmap->GetLastStatus();
	if (status != Status::Ok)
	{
		delete bitmap;
		return;
	}

	Graphics* g = Graphics::FromImage(bitmap);
	RenderHighlighting(g, width, height);
	delete g;

	HBITMAP hbitmap;
	bitmap->GetHBITMAP(NULL, &hbitmap);

	HDC hdcScreen = ::GetDC(nullptr);
	HDC hDC = ::CreateCompatibleDC(hdcScreen);
	HBITMAP hbitmapOld = (HBITMAP) ::SelectObject(hDC, hbitmap);

	RenderTime(hDC, width, height);

	SIZE sizeWnd = { width, height };
	POINT ptSrc = { 0, 0 };
	BLENDFUNCTION blend = { 0 };
	blend.BlendOp = AC_SRC_OVER;
	blend.SourceConstantAlpha = 255;
	blend.AlphaFormat = AC_SRC_ALPHA;
	::UpdateLayeredWindow(this->m_hWnd, hdcScreen, nullptr, &sizeWnd, hDC, &ptSrc, 0, &blend, ULW_ALPHA);

	::SelectObject(hDC, hbitmapOld);
	::DeleteObject(hbitmap);
	::DeleteDC(hDC);
	::ReleaseDC(NULL, hdcScreen);

	delete bitmap;

	OUTPUT_DEBUG_STRING(L"Clock has been refreshed.");
}
<|endoftext|>


<commit_before>/**
 * Clever programming language
 * Copyright (c) 2011-2012 Clever Team
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include "core/cthread.h"
#include "core/value.h"
#include "types/function.h"
#include "core/pkgmanager.h"
#include "modules/std/io/io.h"

namespace clever { namespace packages { namespace std {

namespace io {

// print(object a, [ ...])
// Prints the object values without trailing newline
static CLEVER_FUNCTION(print) {
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
	}
}

// println(object a, [ ...])
// Prints the object values with trailing newline
static CLEVER_FUNCTION(println) {
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
		::std::cout << '\n';
	}
}

// printf(string format, [...])
// Prints and formats a string to standard output without trailing newline
static CLEVER_FUNCTION(printf) {
	const CString *format = args[0]->getStr();
	if (format) {
		const char *delim = "{}";
		char *tokenize = (char*) format->c_str();
#ifndef _WIN32
		char *tokenized;
		char *point = strtok_r(tokenize, delim, &tokenized);
#else
		char *pointer = strtok(tokenize, delim);
#endif
		if (point) {
			do {
				unsigned int arg = atoi(point);
				if (arg) {
					if (args.size() > arg) {
						args[arg]->dump();
					}
				} else ::std::cout << point;
#ifndef _WIN32
			} while((point = strtok_r(NULL, delim, &tokenized)));
#else
			} while((point = strtok(NULL, delim)));
#endif
		}
	}
}

} // clever::packages::std::io

/// Initializes Standard module
CLEVER_MODULE_INIT(IOModule) {
	using namespace io;

	BEGIN_DECLARE_FUNCTION();

	addFunction(new Function("print",       &CLEVER_FUNC_NAME(print)));
	addFunction(new Function("println",     &CLEVER_FUNC_NAME(println)));
	addFunction(new Function("printf",		&CLEVER_FUNC_NAME(printf)));
	END_DECLARE();
}

}}} // clever::packages::std
<commit_msg>stupid mistake<commit_after>/**
 * Clever programming language
 * Copyright (c) 2011-2012 Clever Team
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include "core/cthread.h"
#include "core/value.h"
#include "types/function.h"
#include "core/pkgmanager.h"
#include "modules/std/io/io.h"

namespace clever { namespace packages { namespace std {

namespace io {

// print(object a, [ ...])
// Prints the object values without trailing newline
static CLEVER_FUNCTION(print) {
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
	}
}

// println(object a, [ ...])
// Prints the object values with trailing newline
static CLEVER_FUNCTION(println) {
	for (size_t i = 0, size = args.size(); i < size; ++i) {
		args[i]->dump();
		::std::cout << '\n';
	}
}

// printf(string format, [...])
// Prints and formats a string to standard output without trailing newline
static CLEVER_FUNCTION(printf) {
	const CString *format = args[0]->getStr();
	if (format) {
		const char *delim = "{}";
		char *tokenize = (char*) format->c_str();
#ifndef _WIN32
		char *tokenized;
		char *point = strtok_r(tokenize, delim, &tokenized);
#else
		char *point = strtok(tokenize, delim);
#endif
		if (point) {
			do {
				unsigned int arg = atoi(point);
				if (arg) {
					if (args.size() > arg) {
						args[arg]->dump();
					}
				} else ::std::cout << point;
#ifndef _WIN32
			} while((point = strtok_r(NULL, delim, &tokenized)));
#else
			} while((point = strtok(NULL, delim)));
#endif
		}
	}
}

} // clever::packages::std::io

/// Initializes Standard module
CLEVER_MODULE_INIT(IOModule) {
	using namespace io;

	BEGIN_DECLARE_FUNCTION();

	addFunction(new Function("print",       &CLEVER_FUNC_NAME(print)));
	addFunction(new Function("println",     &CLEVER_FUNC_NAME(println)));
	addFunction(new Function("printf",		&CLEVER_FUNC_NAME(printf)));
	END_DECLARE();
}

}}} // clever::packages::std
<|endoftext|>


<commit_before>
#include "Globals.h"
#include "FireworksSerializer.h"
#include "WorldStorage/FastNBT.h"

void cFireworkItem::WriteToNBTCompound(const cFireworkItem & a_FireworkItem, cFastNBTWriter & a_Writer, const ENUM_ITEM_ID a_Type)
{
	switch (a_Type)
	{
		case E_ITEM_FIREWORK_ROCKET:
		{
			a_Writer.BeginCompound("Fireworks");
			a_Writer.AddByte("Flight", a_FireworkItem.m_FlightTimeInTicks / 20);
			a_Writer.BeginList("Explosions", TAG_Compound);
			a_Writer.BeginCompound("");
			a_Writer.AddByte("Flicker", a_FireworkItem.m_HasFlicker);
			a_Writer.AddByte("Trail", a_FireworkItem.m_HasTrail);
			a_Writer.AddByte("Type", a_FireworkItem.m_Type);
			a_Writer.AddIntArray("Colors", &(a_FireworkItem.m_Colours[0]), a_FireworkItem.m_Colours.size());
			a_Writer.AddIntArray("FadeColors", &(a_FireworkItem.m_FadeColours[0]), a_FireworkItem.m_FadeColours.size());
			a_Writer.EndCompound();
			a_Writer.EndList();
			a_Writer.EndCompound();
			break;
		}
		case E_ITEM_FIREWORK_STAR:
		{
			a_Writer.BeginCompound("Explosion");
			a_Writer.AddByte("Flicker", a_FireworkItem.m_HasFlicker);
			a_Writer.AddByte("Trail", a_FireworkItem.m_HasTrail);
			a_Writer.AddByte("Type", a_FireworkItem.m_Type);
			if (!a_FireworkItem.m_Colours.empty())
			{
				a_Writer.AddIntArray("Colors", &(a_FireworkItem.m_Colours[0]), a_FireworkItem.m_Colours.size());
			}
			if (!a_FireworkItem.m_FadeColours.empty())
			{
				a_Writer.AddIntArray("FadeColors", &(a_FireworkItem.m_FadeColours[0]), a_FireworkItem.m_FadeColours.size());
			}
			a_Writer.EndCompound();
			break;
		}
		default: ASSERT(!"Unhandled firework item!"); break;
	}
}

void cFireworkItem::ParseFromNBT(cFireworkItem & a_FireworkItem, const cParsedNBT & a_NBT, int a_TagIdx, const ENUM_ITEM_ID a_Type)
{
	if (a_TagIdx < 0)
	{
		return;
	}

	switch (a_Type)
	{
		case E_ITEM_FIREWORK_STAR:
		{
			for (int explosiontag = a_NBT.GetFirstChild(a_TagIdx); explosiontag >= 0; explosiontag = a_NBT.GetNextSibling(explosiontag))
			{
				eTagType TagType = a_NBT.GetType(explosiontag);
				if (TagType == TAG_Byte) // Custon name tag
				{
					AString ExplosionName = a_NBT.GetName(explosiontag);

					if (ExplosionName == "Flicker")
					{
						a_FireworkItem.m_HasFlicker = (a_NBT.GetByte(explosiontag) == 1);
					}
					else if (ExplosionName == "Trail")
					{
						a_FireworkItem.m_HasTrail = (a_NBT.GetByte(explosiontag) == 1);
					}
					else if (ExplosionName == "Type")
					{
						a_FireworkItem.m_Type = a_NBT.GetByte(explosiontag);
					}
				}
				else if (TagType == TAG_IntArray)
				{
					AString ExplosionName = a_NBT.GetName(explosiontag);

					if (ExplosionName == "Colors")
					{
						// Divide by four as data length returned in bytes
						int DataLength = a_NBT.GetDataLength(explosiontag);
						// round to the next highest multiple of four
						DataLength -= DataLength % 4;
						if (DataLength == 0)
						{
							continue;
						}

						const char * ColourData = (a_NBT.GetData(explosiontag));
						for (int i = 0; i < DataLength; i += 4 /* Size of network int*/)
						{
							a_FireworkItem.m_Colours.push_back(GetBEInt(ColourData + i));
						}
					}
					else if (ExplosionName == "FadeColors")
					{
						int DataLength = a_NBT.GetDataLength(explosiontag) / 4;
						// round to the next highest multiple of four
						DataLength -= DataLength % 4;
						if (DataLength == 0)
						{
							continue;
						}

						const char * FadeColourData = (a_NBT.GetData(explosiontag));
						for (int i = 0; i < DataLength; i += 4 /* Size of network int*/)
						{
							a_FireworkItem.m_FadeColours.push_back(GetBEInt(FadeColourData + i));
						}
					}
				}
			}
			break;
		}
		case E_ITEM_FIREWORK_ROCKET:
		{
			for (int fireworkstag = a_NBT.GetFirstChild(a_TagIdx); fireworkstag >= 0; fireworkstag = a_NBT.GetNextSibling(fireworkstag))
			{
				eTagType TagType = a_NBT.GetType(fireworkstag);
				if (TagType == TAG_Byte) // Custon name tag
				{
					if (a_NBT.GetName(fireworkstag) == "Flight")
					{
						a_FireworkItem.m_FlightTimeInTicks = a_NBT.GetByte(fireworkstag) * 20;
					}
				}
				else if ((TagType == TAG_List) && (a_NBT.GetName(fireworkstag) == "Explosions"))
				{
					int ExplosionsChild = a_NBT.GetFirstChild(fireworkstag);
					if ((a_NBT.GetType(ExplosionsChild) == TAG_Compound) && (a_NBT.GetName(ExplosionsChild).empty()))
					{
						ParseFromNBT(a_FireworkItem, a_NBT, ExplosionsChild, E_ITEM_FIREWORK_STAR);
					}
				}
			}
			break;
		}
		default: ASSERT(!"Unhandled firework item!"); break;
	}
}

AString cFireworkItem::ColoursToString(const cFireworkItem & a_FireworkItem)
{
	AString Result;

	for (std::vector<int>::const_iterator itr = a_FireworkItem.m_Colours.begin(); itr != a_FireworkItem.m_Colours.end(); ++itr)
	{
		AppendPrintf(Result, "%i;", *itr);
	}

	return Result;
}

void cFireworkItem::ColoursFromString(const AString & a_String, cFireworkItem & a_FireworkItem)
{
	AStringVector Split = StringSplit(a_String, ";");

	for (size_t itr = 0; itr < Split.size(); ++itr)
	{
		if (Split[itr].empty())
		{
			continue;
		}

		a_FireworkItem.m_Colours.push_back(atoi(Split[itr].c_str()));
	}
}

AString cFireworkItem::FadeColoursToString(const cFireworkItem & a_FireworkItem)
{
	AString Result;

	for (std::vector<int>::const_iterator itr = a_FireworkItem.m_FadeColours.begin(); itr != a_FireworkItem.m_FadeColours.end(); ++itr)
	{
		AppendPrintf(Result, "%i;", *itr);
	}

	return Result;
}

void cFireworkItem::FadeColoursFromString(const AString & a_String, cFireworkItem & a_FireworkItem)
{
	AStringVector Split = StringSplit(a_String, ";");

	for (size_t itr = 0; itr < Split.size(); ++itr)
	{
		if (Split[itr].empty())
		{
			continue;
		}

		a_FireworkItem.m_FadeColours.push_back(atoi(Split[itr].c_str()));
	}
}

int cFireworkItem::GetVanillaColourCodeFromDye(short a_DyeMeta)
{
	/*
	Colours are supposed to be calculated via: R << 16 + G << 8 + B
	However, the RGB values fireworks use aren't the same as the ones for dyes (the ones listed in the MC Wiki)
	Therefore, here is a list of numbers gotten via the Protocol Proxy
	*/

	switch (a_DyeMeta)
	{
		case E_META_DYE_BLACK: return 0x1E1B1B;
		case E_META_DYE_RED: return 0xB3312C;
		case E_META_DYE_GREEN: return 0x3B511A;
		case E_META_DYE_BROWN:  return 0x51301A;
		case E_META_DYE_BLUE: return 0x253192;
		case E_META_DYE_PURPLE: return 0x7B2FBE;
		case E_META_DYE_CYAN: return 0x287697;
		case E_META_DYE_LIGHTGRAY: return 0xABABAB;
		case E_META_DYE_GRAY: return 0x434343;
		case E_META_DYE_PINK: return 0xD88198;
		case E_META_DYE_LIGHTGREEN: return 0x41CD34;
		case E_META_DYE_YELLOW: return 0xDECF2A;
		case E_META_DYE_LIGHTBLUE: return 0x6689D3;
		case E_META_DYE_MAGENTA: return 0xC354CD;
		case E_META_DYE_ORANGE: return 0xEB8844;
		case E_META_DYE_WHITE: return 0xF0F0F0;
		default: ASSERT(!"Unhandled dye meta whilst trying to get colour code for fireworks!"); return 0;
	}
}
<commit_msg>Fixed a crash in firework rockets.<commit_after>
#include "Globals.h"
#include "FireworksSerializer.h"
#include "WorldStorage/FastNBT.h"

void cFireworkItem::WriteToNBTCompound(const cFireworkItem & a_FireworkItem, cFastNBTWriter & a_Writer, const ENUM_ITEM_ID a_Type)
{
	switch (a_Type)
	{
		case E_ITEM_FIREWORK_ROCKET:
		{
			a_Writer.BeginCompound("Fireworks");
			a_Writer.AddByte("Flight", a_FireworkItem.m_FlightTimeInTicks / 20);
			a_Writer.BeginList("Explosions", TAG_Compound);
			a_Writer.BeginCompound("");
			a_Writer.AddByte("Flicker", a_FireworkItem.m_HasFlicker);
			a_Writer.AddByte("Trail", a_FireworkItem.m_HasTrail);
			a_Writer.AddByte("Type", a_FireworkItem.m_Type);
			if (!a_FireworkItem.m_Colours.empty())
			{
				a_Writer.AddIntArray("Colors", &(a_FireworkItem.m_Colours[0]), a_FireworkItem.m_Colours.size());
			}
			if (!a_FireworkItem.m_FadeColours.empty())
			{
				a_Writer.AddIntArray("FadeColors", &(a_FireworkItem.m_FadeColours[0]), a_FireworkItem.m_FadeColours.size());
			}
			a_Writer.EndCompound();
			a_Writer.EndList();
			a_Writer.EndCompound();
			break;
		}
		case E_ITEM_FIREWORK_STAR:
		{
			a_Writer.BeginCompound("Explosion");
			a_Writer.AddByte("Flicker", a_FireworkItem.m_HasFlicker);
			a_Writer.AddByte("Trail", a_FireworkItem.m_HasTrail);
			a_Writer.AddByte("Type", a_FireworkItem.m_Type);
			if (!a_FireworkItem.m_Colours.empty())
			{
				a_Writer.AddIntArray("Colors", &(a_FireworkItem.m_Colours[0]), a_FireworkItem.m_Colours.size());
			}
			if (!a_FireworkItem.m_FadeColours.empty())
			{
				a_Writer.AddIntArray("FadeColors", &(a_FireworkItem.m_FadeColours[0]), a_FireworkItem.m_FadeColours.size());
			}
			a_Writer.EndCompound();
			break;
		}
		default: ASSERT(!"Unhandled firework item!"); break;
	}
}

void cFireworkItem::ParseFromNBT(cFireworkItem & a_FireworkItem, const cParsedNBT & a_NBT, int a_TagIdx, const ENUM_ITEM_ID a_Type)
{
	if (a_TagIdx < 0)
	{
		return;
	}

	switch (a_Type)
	{
		case E_ITEM_FIREWORK_STAR:
		{
			for (int explosiontag = a_NBT.GetFirstChild(a_TagIdx); explosiontag >= 0; explosiontag = a_NBT.GetNextSibling(explosiontag))
			{
				eTagType TagType = a_NBT.GetType(explosiontag);
				if (TagType == TAG_Byte) // Custon name tag
				{
					AString ExplosionName = a_NBT.GetName(explosiontag);

					if (ExplosionName == "Flicker")
					{
						a_FireworkItem.m_HasFlicker = (a_NBT.GetByte(explosiontag) == 1);
					}
					else if (ExplosionName == "Trail")
					{
						a_FireworkItem.m_HasTrail = (a_NBT.GetByte(explosiontag) == 1);
					}
					else if (ExplosionName == "Type")
					{
						a_FireworkItem.m_Type = a_NBT.GetByte(explosiontag);
					}
				}
				else if (TagType == TAG_IntArray)
				{
					AString ExplosionName = a_NBT.GetName(explosiontag);

					if (ExplosionName == "Colors")
					{
						// Divide by four as data length returned in bytes
						int DataLength = a_NBT.GetDataLength(explosiontag);
						// round to the next highest multiple of four
						DataLength -= DataLength % 4;
						if (DataLength == 0)
						{
							continue;
						}

						const char * ColourData = (a_NBT.GetData(explosiontag));
						for (int i = 0; i < DataLength; i += 4 /* Size of network int*/)
						{
							a_FireworkItem.m_Colours.push_back(GetBEInt(ColourData + i));
						}
					}
					else if (ExplosionName == "FadeColors")
					{
						int DataLength = a_NBT.GetDataLength(explosiontag) / 4;
						// round to the next highest multiple of four
						DataLength -= DataLength % 4;
						if (DataLength == 0)
						{
							continue;
						}

						const char * FadeColourData = (a_NBT.GetData(explosiontag));
						for (int i = 0; i < DataLength; i += 4 /* Size of network int*/)
						{
							a_FireworkItem.m_FadeColours.push_back(GetBEInt(FadeColourData + i));
						}
					}
				}
			}
			break;
		}
		case E_ITEM_FIREWORK_ROCKET:
		{
			for (int fireworkstag = a_NBT.GetFirstChild(a_TagIdx); fireworkstag >= 0; fireworkstag = a_NBT.GetNextSibling(fireworkstag))
			{
				eTagType TagType = a_NBT.GetType(fireworkstag);
				if (TagType == TAG_Byte) // Custon name tag
				{
					if (a_NBT.GetName(fireworkstag) == "Flight")
					{
						a_FireworkItem.m_FlightTimeInTicks = a_NBT.GetByte(fireworkstag) * 20;
					}
				}
				else if ((TagType == TAG_List) && (a_NBT.GetName(fireworkstag) == "Explosions"))
				{
					int ExplosionsChild = a_NBT.GetFirstChild(fireworkstag);
					if ((a_NBT.GetType(ExplosionsChild) == TAG_Compound) && (a_NBT.GetName(ExplosionsChild).empty()))
					{
						ParseFromNBT(a_FireworkItem, a_NBT, ExplosionsChild, E_ITEM_FIREWORK_STAR);
					}
				}
			}
			break;
		}
		default: ASSERT(!"Unhandled firework item!"); break;
	}
}

AString cFireworkItem::ColoursToString(const cFireworkItem & a_FireworkItem)
{
	AString Result;

	for (std::vector<int>::const_iterator itr = a_FireworkItem.m_Colours.begin(); itr != a_FireworkItem.m_Colours.end(); ++itr)
	{
		AppendPrintf(Result, "%i;", *itr);
	}

	return Result;
}

void cFireworkItem::ColoursFromString(const AString & a_String, cFireworkItem & a_FireworkItem)
{
	AStringVector Split = StringSplit(a_String, ";");

	for (size_t itr = 0; itr < Split.size(); ++itr)
	{
		if (Split[itr].empty())
		{
			continue;
		}

		a_FireworkItem.m_Colours.push_back(atoi(Split[itr].c_str()));
	}
}

AString cFireworkItem::FadeColoursToString(const cFireworkItem & a_FireworkItem)
{
	AString Result;

	for (std::vector<int>::const_iterator itr = a_FireworkItem.m_FadeColours.begin(); itr != a_FireworkItem.m_FadeColours.end(); ++itr)
	{
		AppendPrintf(Result, "%i;", *itr);
	}

	return Result;
}

void cFireworkItem::FadeColoursFromString(const AString & a_String, cFireworkItem & a_FireworkItem)
{
	AStringVector Split = StringSplit(a_String, ";");

	for (size_t itr = 0; itr < Split.size(); ++itr)
	{
		if (Split[itr].empty())
		{
			continue;
		}

		a_FireworkItem.m_FadeColours.push_back(atoi(Split[itr].c_str()));
	}
}

int cFireworkItem::GetVanillaColourCodeFromDye(short a_DyeMeta)
{
	/*
	Colours are supposed to be calculated via: R << 16 + G << 8 + B
	However, the RGB values fireworks use aren't the same as the ones for dyes (the ones listed in the MC Wiki)
	Therefore, here is a list of numbers gotten via the Protocol Proxy
	*/

	switch (a_DyeMeta)
	{
		case E_META_DYE_BLACK: return 0x1E1B1B;
		case E_META_DYE_RED: return 0xB3312C;
		case E_META_DYE_GREEN: return 0x3B511A;
		case E_META_DYE_BROWN:  return 0x51301A;
		case E_META_DYE_BLUE: return 0x253192;
		case E_META_DYE_PURPLE: return 0x7B2FBE;
		case E_META_DYE_CYAN: return 0x287697;
		case E_META_DYE_LIGHTGRAY: return 0xABABAB;
		case E_META_DYE_GRAY: return 0x434343;
		case E_META_DYE_PINK: return 0xD88198;
		case E_META_DYE_LIGHTGREEN: return 0x41CD34;
		case E_META_DYE_YELLOW: return 0xDECF2A;
		case E_META_DYE_LIGHTBLUE: return 0x6689D3;
		case E_META_DYE_MAGENTA: return 0xC354CD;
		case E_META_DYE_ORANGE: return 0xEB8844;
		case E_META_DYE_WHITE: return 0xF0F0F0;
		default: ASSERT(!"Unhandled dye meta whilst trying to get colour code for fireworks!"); return 0;
	}
}
<|endoftext|>


<commit_before>/*! \file EulerParameterVector.cpp
 */

#include "EulerParameterVector.h"

namespace numlib{ namespace tensor{

EulerParameterVector::EulerParameterVector(Real q1_, Real q2_, Real q3_, Real q4_):
	 q1(q1_),
	 q2(q2_),
	 q3(q3_),
	 q4(q4_)
{
	 DEBUG_PRINT_VAR( q1 );
	 DEBUG_PRINT_VAR( q2 );
	 DEBUG_PRINT_VAR( q3 );
	 DEBUG_PRINT_VAR( q4 );
}

EulerParameterVector::EulerParameterVector(const TensorR2 & r)
{
	 using std::sqrt;

	 Real r12 = r(1,2);
	 Real r21 = r(2,1);

	 Real r13 = r(1,3);
	 Real r31 = r(3,1);

	 Real r23 = r(2,3);
	 Real r32 = r(3,2);

	 Real tr = trace(r);

	 ASSERT( tr > -1.0 );

	 q4 = sqrt(0.25*(tr + 1.0));

	 Real q44 = 4.0*q4;

	 q1 = (r32 - r23)/q44;
	 q2 = (r13 - r31)/q44;
	 q3 = (r21 - r12)/q44;

	 DEBUG_PRINT_VAR( q1 );
	 DEBUG_PRINT_VAR( q2 );
	 DEBUG_PRINT_VAR( q3 );
	 DEBUG_PRINT_VAR( q4 );
}

EulerParameterVector::EulerParameterVector(const TensorR1 & e, Real theta)
{
	 using std::cos;
	 using std::sin;

	 Real theta2 = 0.5*theta;
	 Real c = cos(theta2);
	 Real s = sin(theta2);

	 q4 = c;
	 q1 = e(1)*s;
	 q2 = e(2)*s;
	 q3 = e(3)*s;
}

EulerParameterVector::EulerParameterVector(const EulerParameterVector & other):
	 q1(other.q1),
	 q2(other.q2),
	 q3(other.q3),
	 q4(other.q4)
{
}

EulerParameterVector::~EulerParameterVector()
{
	 /* nothing to delete */
}

EulerParameterVector & EulerParameterVector::operator=(const EulerParameterVector & other)
{
	 if(&other == this)
		  return *this;

	 q1 = other.q1;
	 q2 = other.q2;
	 q3 = other.q3;
	 q4 = other.q4;

	 return *this;

}

Real EulerParameterVector::rotationAngle() const
{
	 using std::acos;

	 DEBUG_PRINT_VAR( q4 );

	 return 2.0*acos(q4);
}

TensorR1 EulerParameterVector::rotationAxis() const
{
	 using std::sin;

	 Real s = sqrt(q1*q1 + q2*q2 + q3*q3);

	 if( s < 1.0E-12 ) return TensorR1(0,0,0);

	 DEBUG_PRINT_VAR( s );
	 DEBUG_PRINT_VAR( q1 );
	 DEBUG_PRINT_VAR( q2 );
	 DEBUG_PRINT_VAR( q3 );

	 Real e1 = q1/s;
	 Real e2 = q2/s;
	 Real e3 = q3/s;

	 DEBUG_PRINT_VAR( e1*e1 + e2*e2 + e3*e3 );
	 ASSERT( fabs(e1*e1 + e2*e2 + e3*e3 - 1.0) < 1.0E-10 );

	 return TensorR1(e1, e2, e3);
}

Real & EulerParameterVector::operator()(Index i)
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

const Real & EulerParameterVector::operator()(Index i) const
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

std::ostream & operator<<(std::ostream & os, const EulerParameterVector & ep)
{
	 os<<std::scientific
	   <<"( "<<ep.q1
	   <<", "<<ep.q2
	   <<", "<<ep.q3
	   <<", "<<ep.q4
	   <<" )";
}

}}//::numlib::tensor
<commit_msg>Added check for q4 exceeding unity due to round off error<commit_after>/*! \file EulerParameterVector.cpp
 */

#include "EulerParameterVector.h"

namespace numlib{ namespace tensor{

EulerParameterVector::EulerParameterVector(Real q1_, Real q2_, Real q3_, Real q4_):
	 q1(q1_),
	 q2(q2_),
	 q3(q3_),
	 q4(q4_)
{
	 DEBUG_PRINT_VAR( q1 );
	 DEBUG_PRINT_VAR( q2 );
	 DEBUG_PRINT_VAR( q3 );
	 DEBUG_PRINT_VAR( q4 );
}

EulerParameterVector::EulerParameterVector(const TensorR2 & r)
{
	 using std::sqrt;

	 Real r12 = r(1,2);
	 Real r21 = r(2,1);

	 Real r13 = r(1,3);
	 Real r31 = r(3,1);

	 Real r23 = r(2,3);
	 Real r32 = r(3,2);

	 Real tr = trace(r);

	 ASSERT( tr > -1.0 );

	 q4 = sqrt(0.25*(tr + 1.0));

	 /* Sometimes the above expression for q4 results in a value slightly
		greater than 1.0 (e.g. 1.00000000005). If left this way computation
		of rotation angle via acos(q4) will give NaN. So, if q4 > 1.0 we
		assume its due to rounding error and manually fix it. */

	 if(q4 > 1.0) q4 = 1.0;

	 Real q44 = 4.0*q4;

	 q1 = (r32 - r23)/q44;
	 q2 = (r13 - r31)/q44;
	 q3 = (r21 - r12)/q44;

	 DEBUG_PRINT_VAR( q1 );
	 DEBUG_PRINT_VAR( q2 );
	 DEBUG_PRINT_VAR( q3 );
	 DEBUG_PRINT_VAR( q4 );
}

EulerParameterVector::EulerParameterVector(const TensorR1 & e, Real theta)
{
	 using std::cos;
	 using std::sin;

	 Real theta2 = 0.5*theta;
	 Real c = cos(theta2);
	 Real s = sin(theta2);

	 q4 = c;
	 q1 = e(1)*s;
	 q2 = e(2)*s;
	 q3 = e(3)*s;
}

EulerParameterVector::EulerParameterVector(const EulerParameterVector & other):
	 q1(other.q1),
	 q2(other.q2),
	 q3(other.q3),
	 q4(other.q4)
{
}

EulerParameterVector::~EulerParameterVector()
{
	 /* nothing to delete */
}

EulerParameterVector & EulerParameterVector::operator=(const EulerParameterVector & other)
{
	 if(&other == this)
		  return *this;

	 q1 = other.q1;
	 q2 = other.q2;
	 q3 = other.q3;
	 q4 = other.q4;

	 return *this;

}

Real EulerParameterVector::rotationAngle() const
{
	 using std::acos;

	 DEBUG_PRINT_VAR( q4 );

	 return 2.0*acos(q4);
}

TensorR1 EulerParameterVector::rotationAxis() const
{
	 using std::sin;

	 Real s = sqrt(q1*q1 + q2*q2 + q3*q3);

	 if( s < 1.0E-12 ) return TensorR1(0,0,0);

	 DEBUG_PRINT_VAR( s );
	 DEBUG_PRINT_VAR( q1 );
	 DEBUG_PRINT_VAR( q2 );
	 DEBUG_PRINT_VAR( q3 );

	 Real e1 = q1/s;
	 Real e2 = q2/s;
	 Real e3 = q3/s;

	 DEBUG_PRINT_VAR( e1*e1 + e2*e2 + e3*e3 );
	 ASSERT( fabs(e1*e1 + e2*e2 + e3*e3 - 1.0) < 1.0E-10 );

	 return TensorR1(e1, e2, e3);
}

Real & EulerParameterVector::operator()(Index i)
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

const Real & EulerParameterVector::operator()(Index i) const
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

std::ostream & operator<<(std::ostream & os, const EulerParameterVector & ep)
{
	 os<<std::scientific
	   <<"( "<<ep.q1
	   <<", "<<ep.q2
	   <<", "<<ep.q3
	   <<", "<<ep.q4
	   <<" )";
}

}}//::numlib::tensor
<|endoftext|>


<commit_before>/*! \file EulerParameterVector.cpp
 */

#include "EulerParameterVector.h"

namespace numlib{ namespace tensor{

EulerParameterVector::EulerParameterVector(Real q1_, Real q2_, Real q3_, Real q4_):
	 q1(q1_),
	 q2(q2_),
	 q3(q3_),
	 q4(q4_)
{
}

EulerParameterVector::EulerParameterVector(const TensorR2 & r)
{
	 using std::sqrt;

	 Real r12 = r(1,2);
	 Real r21 = r(2,1);

	 Real r13 = r(1,3);
	 Real r31 = r(3,1);

	 Real r23 = r(2,3);
	 Real r32 = r(3,2);

	 q4 = sqrt(0.25*(trace(r) + 1.0));

	 Real q44 = 4.0*q4;

	 q1 = (r32 - r23)/q44;
	 q2 = (r13 - r31)/q44;
	 q3 = (r21 - r12)/q44;
}

EulerParameterVector::EulerParameterVector(const TensorR1 & e, Real theta)
{
	 using std::cos;
	 using std::sin;

	 Real theta2 = 0.5*theta;
	 Real c = cos(theta2);
	 Real s = sin(theta2);

	 q4 = c;
	 q1 = e(1)*s;
	 q2 = e(2)*s;
	 q3 = e(3)*s;
}

EulerParameterVector::EulerParameterVector(const EulerParameterVector & other):
	 q1(other.q1),
	 q2(other.q2),
	 q3(other.q3),
	 q4(other.q4)
{
}

EulerParameterVector::~EulerParameterVector()
{
	 /* nothing to delete */
}

EulerParameterVector & EulerParameterVector::operator=(const EulerParameterVector & other)
{
	 if(&other == this)
		  return *this;

	 q1 = other.q1;
	 q2 = other.q2;
	 q3 = other.q3;
	 q4 = other.q4;

	 return *this;

}

Real EulerParameterVector::rotationAngle() const
{
	 using std::acos;

	 return 2.0*acos(q4);
}

TensorR1 EulerParameterVector::rotationAxis() const
{
	 using std::sin;

	 Real theta = rotationAngle();

	 /* theta should be mod to [0, pi) */

	 if(theta == 0.0)
		  return TensorR1(0.0, 0.0, 0.0);

	 Real s = sin(0.5*theta);

	 Real e1 = q1/s;
	 Real e2 = q2/s;
	 Real e3 = q3/s;

	 return TensorR1(e1, e2, e3);
}

Real & EulerParameterVector::operator()(Index i)
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

const Real & EulerParameterVector::operator()(Index i) const
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

std::ostream & operator<<(std::ostream & os, const EulerParameterVector & ep)
{
	 os<<std::scientific
	   <<"( "<<ep.q1
	   <<", "<<ep.q2
	   <<", "<<ep.q3
	   <<", "<<ep.q4
	   <<" )";
}

}}//::numlib::tensor
<commit_msg>Added assertion to verify unit magnitude of rotation axis vector<commit_after>/*! \file EulerParameterVector.cpp
 */

#include "EulerParameterVector.h"

namespace numlib{ namespace tensor{

EulerParameterVector::EulerParameterVector(Real q1_, Real q2_, Real q3_, Real q4_):
	 q1(q1_),
	 q2(q2_),
	 q3(q3_),
	 q4(q4_)
{
}

EulerParameterVector::EulerParameterVector(const TensorR2 & r)
{
	 using std::sqrt;

	 Real r12 = r(1,2);
	 Real r21 = r(2,1);

	 Real r13 = r(1,3);
	 Real r31 = r(3,1);

	 Real r23 = r(2,3);
	 Real r32 = r(3,2);

	 q4 = sqrt(0.25*(trace(r) + 1.0));

	 Real q44 = 4.0*q4;

	 q1 = (r32 - r23)/q44;
	 q2 = (r13 - r31)/q44;
	 q3 = (r21 - r12)/q44;
}

EulerParameterVector::EulerParameterVector(const TensorR1 & e, Real theta)
{
	 using std::cos;
	 using std::sin;

	 Real theta2 = 0.5*theta;
	 Real c = cos(theta2);
	 Real s = sin(theta2);

	 q4 = c;
	 q1 = e(1)*s;
	 q2 = e(2)*s;
	 q3 = e(3)*s;
}

EulerParameterVector::EulerParameterVector(const EulerParameterVector & other):
	 q1(other.q1),
	 q2(other.q2),
	 q3(other.q3),
	 q4(other.q4)
{
}

EulerParameterVector::~EulerParameterVector()
{
	 /* nothing to delete */
}

EulerParameterVector & EulerParameterVector::operator=(const EulerParameterVector & other)
{
	 if(&other == this)
		  return *this;

	 q1 = other.q1;
	 q2 = other.q2;
	 q3 = other.q3;
	 q4 = other.q4;

	 return *this;

}

Real EulerParameterVector::rotationAngle() const
{
	 using std::acos;

	 return 2.0*acos(q4);
}

TensorR1 EulerParameterVector::rotationAxis() const
{
	 using std::sin;

	 Real theta = rotationAngle();

	 /* theta should be mod to [0, pi) */

	 if(theta == 0.0)
		  return TensorR1(0.0, 0.0, 0.0);

	 Real s = sin(0.5*theta);

	 Real e1 = q1/s;
	 Real e2 = q2/s;
	 Real e3 = q3/s;

	 ASSERT( fabs(e1*e1 + e2*e2 + e3*e3 - 1.0) < 1.0E-10 );

	 return TensorR1(e1, e2, e3);
}

Real & EulerParameterVector::operator()(Index i)
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

const Real & EulerParameterVector::operator()(Index i) const
{
	 switch(i)
	 {
	 case 1:
		  return q1;
	 case 2:
		  return q2;
	 case 3:
		  return q3;
	 case 4:
		  return q4;
	 }
}

std::ostream & operator<<(std::ostream & os, const EulerParameterVector & ep)
{
	 os<<std::scientific
	   <<"( "<<ep.q1
	   <<", "<<ep.q2
	   <<", "<<ep.q3
	   <<", "<<ep.q4
	   <<" )";
}

}}//::numlib::tensor
<|endoftext|>