Ejemplo n.º 1
0
/*
    offsetIndex:    [0] skeleton
                    [1] spline
                    [2] offset spline
*/
ofVec2f BGGraphics::calculateInternalTexOffset(float t, bool isSourceSpline, bool isSourceSegment, int offsetIndex) {

    const float triangleHeight = .5 * tanf(M_PI / 3.0);

    const float baseSize = sqrtf(3.0);
    const float halfBaseSize = .5 * baseSize;
    const ofVec2f source(0, 1);
    const ofVec2f sink1(-halfBaseSize, -.5);
    const ofVec2f sink2(halfBaseSize, -.5);
    const ofVec2f center = (source + sink1 + sink2) / 3.0;
    const float bezierOffset = 0.5 * baseSize;
    const float maxInternalOffset = (.25 * source - .5 * center + .25 * sink1).length();
    const float centerStretchFactor = (maxInternalOffset + bezierOffset) / bezierOffset;


    ofVec2f focusPt = isSourceSpline ? ofVec2f(baseSize, 1) : ofVec2f(0, -2);
    float fromFocusAngle = M_PI * (isSourceSpline ? (1.0 + t / 3.0) : ((1.0 + t) / 3.0));
    ofVec2f toPtVector(cosf(fromFocusAngle), sinf(fromFocusAngle));

    float offset = (offsetIndex == 2) ? (.5 * baseSize) : baseSize;
    ofVec2f xy = focusPt + offset * toPtVector;

    if(offsetIndex == 0) {
        //project point on base spline
        ofVec2f projBase = isSourceSegment ? ofVec2f(0,1) : ofVec2f(halfBaseSize, -.5);
        xy = dot(xy, projBase) * projBase;
    }

    //in case we are dealing with the center point:
    if(offsetIndex == -1)
        xy = ofVec2f(0,0);
    
    const ofVec2f cornerTL = source + (sink1 - sink2);
    const ofVec2f cornerTR = source + (sink2 - sink1);
    const ofVec2f cornerB = sink1 + (sink2 - source);

    ofVec2f vecSource = (center - source).normalize();
    ofVec2f vecSink1 = (sink1 - center).normalize();
    ofVec2f vecSink2 = (sink2 - center).normalize();
 
    float traversalDistance = 2. * (center - source).length();
 
    float projSource = dot(xy - source, vecSource);
    float projSink1 = dot(xy - sink1, vecSink1);
    float projSink2 = dot(xy - sink2, vecSink2);
 
    float orSource = cross(xy - source, vecSource);
    float orSink1 = cross(xy - sink1, vecSink1);
    float orSink2 = cross(xy - sink2, vecSink2);
 
    float val1 = projSource / traversalDistance;
    float val2 = 1.0 + projSink1 / traversalDistance;
    float val3 = 1.0 + projSink2 / traversalDistance;

    float offsetX = 0;
    if(ABS(projSource) < .0001)
        offsetX = val1;
    else if(ABS(projSink1) < .0001)
        offsetX = val2;
    else if(ABS(projSink2) < .0001)
        offsetX = val3;
    else {
        float power = 2.0;
        float weight1 = powf(1.0 / ABS(projSource), power);
        float weight2 = powf(1.0 / ABS(projSink1), power);
        float weight3 = powf(1.0 / ABS(projSink2), power);
        float sumWeight = weight1 + weight2 + weight3;
    
        offsetX = (weight1 / sumWeight) * val1
                    + (weight2 / sumWeight) * val2
                    + (weight3 / sumWeight) * val3;
    }
 
 
  ofVec2f to = xy - focusPt;
  float toDist = to.length();
  to /= toDist;
 
  float dist = MAX(0.0, toDist - bezierOffset);
 
  float maxAng = M_PI / 6.;
 
  float angle = acos(dot(to, (center - focusPt).normalize()));
  float maxOffset = baseSize / cos(M_PI / 6.0 - angle) - bezierOffset;
 
  float circDistFrac = dist / (baseSize - bezierOffset);
  float projDistFrac = dist / maxOffset;
 
 
  float angleFrac = 1. - angle / maxAng;
 
 
  float offFactor = pow(projDistFrac, 2.0 + abs(angleFrac) * projDistFrac);
  float offsetY = (1. - offFactor) * circDistFrac + offFactor * projDistFrac;
  offsetY = 1. - offsetY;

  if(isnan(offsetX) || isnan(offsetY))
      cout << "OFFSET VALUE is NaN" << endl;

  return ofVec2f(offsetX - .5, offsetY);
}
int main ()
{
    clrscr();
    float a,b,PI;
    int c;
    cout<<endl;
    cout<<"******************************* Calculator *****************************\n";
    cout<<"************************    BY SATISH KUMAR GUPTA    *****************************\n";
    cout<<"================================================================================\n";
    cout<<"Operations\t"<<"\tTrigonometric Functions"<<"\t\tLogarithmic Functions\n";
    cout<<"================================================================================\n";
    cout<<"1: Division\t\t"<<"7: Sin\t\t"<<"\t\t13: Log"<<endl;
    cout<<endl;
    cout<<"2: Multiplication\t"<<"8: Cos\t\t"<<"\t\t14: Log with base 10"<<endl;
    cout<<endl;
    cout<<"3: Subtraction\t\t"<<"9: Tan\t\t"<<endl;
    cout<<endl;
    cout<<"4: Addition\t\t"<<"10: Inverse of Sin"<<endl;
    cout<<endl;
    cout<<"5: Exponent\t\t"<<"11: Inverse of Cos"<<endl;
    cout<<endl;
    cout<<"6: Square root\t\t"<<"12: Inverse of Tan"<<endl;
    cout<<endl;
    cout<<"Enter the function that you want to perform : ";
    cin>>c;
    cout<<endl;
    PI=3.14159265;
    switch(c)
    {
    case 1:
        cout<<"Enter 1st number : ";
        cin>>a;
        cout<<endl;
        cout<<"Enter 2nd number : ";
        cin>>b;
        cout<<endl;
        cout<<"Division = "<<a/b<<endl;
        break;
    case 2:
        cout<<"Enter 1st number : ";
        cin>>a;
        cout<<endl;
        cout<<"Enter 2nd number : ";
        cin>>b;
        cout<<endl;
        cout<<"Multiplication = "<<a*b<<endl;
        break;
    case 3:
        cout<<"Enter 1st number : ";
        cin>>a;
        cout<<endl;
        cout<<"Enter 2nd number : ";
        cin>>b;
        cout<<endl;
        cout<<"Subtraction = "<<a-b<<endl;
        break;
    case 4:
        cout<<"Enter 1st number : ";
        cin>>a;
        cout<<endl;
        cout<<"Enter 2nd number : ";
        cin>>b;
        cout<<endl;
        cout<<"Addition = "<<a+b<<endl;
        break;
    case 5:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Enter the exponent : ";
        cin>>b;
        cout<<endl;
        cout<<"Exponent = "<<pow(a,b)<<endl;
        break;
    case 6:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Square Root = "<<sqrt(a)<<endl;
        break;
    case 7:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Sin = "<<sin(a)<<endl;
        break;
    case 8:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Cos = "<<cos(a)<<endl;
        break;
    case 9:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Tan = "<<tan(a)<<endl;
        break;
    case 10:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Inverse of Sin = "<<asin(a)*180.0/PI<<endl;
        break;

    case 11:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Inverse of Cos = "<<acos(a)*180.0/PI<<endl;
        break;
    case 12:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Inverse of tan = "<<atan(a)*180.0/PI<<endl;
        break;
    case 13:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Log = "<<log(a)<<endl;
        break;
    case 14:
        cout<<"Enter the number : ";
        cin>>a;
        cout<<endl;
        cout<<"Log with base 10 = "<<log10(a)<<endl;
        break;
    default:
        cout<<"Wrong Input"<<endl;
    }
    getch();
    return 0;

}
Ejemplo n.º 3
0
/**
 * The function evalAST(ASTNode_t) evaluates the formula of an
 * Abstract Syntax Tree by simple recursion and returns the result
 * as a double value.
 *
 * If variables (ASTNodeType_t AST_NAME) occur in the formula the user is
 * asked to provide a numerical value.  When evaluating ASTs within an SBML
 * document or simulating an SBML model this node type includes parameters
 * and variables of the model.  Parameters should be retrieved from the
 * SBML file, time and variables from current values of the simulation.
 *
 * Not implemented:
 *
 *  - PIECEWISE, LAMBDA, and the SBML model specific functions DELAY and
 *    TIME and user-defined functions.
 *
 *  - Complex numbers and/or checking for domains of trigonometric and root
 *    functions.
 *
 *  - Checking for precision and rounding errors.
 *
 * The Nodetypes AST_TIME, AST_DELAY and AST_PIECEWISE default to 0.  The
 * SBML DELAY function and unknown functions (SBML user-defined functions)
 * use the value of the left child (first argument to function) or 0 if the
 * node has no children.
 */
double
evalAST(ASTNode_t *n)
{
  int    i;
  double result;
  
  int       childnum = ASTNode_getNumChildren(n);
  ASTNode_t  **child = (ASTNode_t **) malloc(childnum * sizeof(ASTNode_t*));


  for (i = 0; i < childnum; i++)
  {
    child[i] = ASTNode_getChild(n, i);
  }

  switch (ASTNode_getType(n))
  {
  case AST_INTEGER:
    result = (double) ASTNode_getInteger(n);
    break;

  case AST_REAL:
    result = ASTNode_getReal(n);
    break;

  case AST_REAL_E:
    result = ASTNode_getReal(n);
    break;

  case AST_RATIONAL:
    result = ASTNode_getReal(n);
    break;
  
  case AST_NAME:
    {
      char *l;
      double var;
      printf("\n-------------MESSAGE FROM EVALUATION FUNCTION-------------\n");
      printf("Please enter a number for the variable!\n");
      printf("If you do not enter a valid number (empty or characters), the \n");
      printf("evaluation will proceed with a current internal value and the \n");
      printf("result will make no sense.\n");
      printf("%s=",ASTNode_getName(n));
      l = get_line(stdin);
      sscanf(l, "%lf", &var);
      free(l);
      printf("%s = %f\n", ASTNode_getName(n), var);
      printf("-----------------------END MESSAGE--------------------------\n\n");
      result = var;
    }
    break;

  case AST_FUNCTION_DELAY:
    printf("\n-------------MESSAGE FROM EVALUATION FUNCTION-------------\n");
    printf("Delays can only be evaluated during a time series simulation.\n");
    printf("The value of the first child (ie. the first argument to the function)\n");
    printf("is used for this evaluation. If the function node has no children the\n");
    printf("value defaults to 0.\n");
    printf("-----------------------END MESSAGE--------------------------\n\n");
    if(i>0)
      result = evalAST(child[0]);
    else
      result = 0.0;
    break;

  case AST_NAME_TIME:
    printf("\n-------------MESSAGE FROM EVALUATION FUNCTION-------------\n");
    printf("The time can only be evaluated during a time series simulation.\n");
    printf("The value of defaults to 0\n");
    printf("-----------------------END MESSAGE--------------------------\n\n");
    result = 0.0;
    break;

  case AST_CONSTANT_E:
    /* exp(1) is used to adjust exponentiale to machine precision */
    result = exp(1);
    break;

  case AST_CONSTANT_FALSE:
    result = 0.0;
    break;

  case AST_CONSTANT_PI:
    /* pi = 4 * atan 1  is used to adjust Pi to machine precision */
    result = 4.*atan(1.);
    break;

  case AST_CONSTANT_TRUE:
    result = 1.0;
    break;

  case AST_PLUS:
    result = evalAST(child[0]) + evalAST(child[1]);
    break;

  case AST_MINUS:
    if(childnum==1)
      result = - (evalAST(child[0]));
    else
      result = evalAST(child[0]) - evalAST(child[1]);
    break;

  case AST_TIMES:
    result = evalAST(child[0]) * evalAST(child[1]) ;
    break;

  case AST_DIVIDE:
    result = evalAST(child[0]) / evalAST(child[1]);
    break;

  case AST_POWER:
    result = pow(evalAST(child[0]),evalAST(child[1]));
    break;

  case AST_LAMBDA:
    printf("\n-------------MESSAGE FROM EVALUATION FUNCTION-------------\n");
    printf("This function is not implemented yet.\n");
    printf("The value defaults to 0.\n");
    printf("-----------------------END MESSAGE--------------------------\n\n");
    result = 0.0;
    break;

  case AST_FUNCTION:
    printf("\n-------------MESSAGE FROM EVALUATION FUNCTION-------------\n");
    printf("This function is not known.\n");
    printf("Within an SBML document new functions can be defined by the user or \n");
    printf("application. The value of the first child (ie. the first argument to \n");
    printf("the function) is used for this evaluation. If the function node has\n");
    printf("no children the value defaults to 0.\n");
    printf("-----------------------END MESSAGE--------------------------\n\n");
    if(childnum>0)
      result = evalAST(child[0]);
    else
      result = 0.0;
    break;

  case AST_FUNCTION_ABS:
    result = (double) fabs(evalAST(child[0]));
    break;

  case AST_FUNCTION_ARCCOS:
    result = acos(evalAST(child[0])) ;
    break;

  case AST_FUNCTION_ARCCOSH:
#ifndef WIN32
    result = acosh(evalAST(child[0]));
#else
	result = log(evalAST(child[0]) + SQR(evalAST(child[0]) * evalAST(child[0]) - 1.));
#endif
    break;
  case AST_FUNCTION_ARCCOT:
    /* arccot x =  arctan (1 / x) */
    result = atan(1./ evalAST(child[0]));
    break;

  case AST_FUNCTION_ARCCOTH:
    /* arccoth x = 1/2 * ln((x+1)/(x-1)) */
    result = ((1./2.)*log((evalAST(child[0])+1.)/(evalAST(child[0])-1.)) );
    break;

  case AST_FUNCTION_ARCCSC:
    /* arccsc(x) = Arctan(1 / sqrt((x - 1)(x + 1))) */
    result = atan( 1. / SQRT( (evalAST(child[0])-1.)*(evalAST(child[0])+1.) ) );
    break;

  case AST_FUNCTION_ARCCSCH:
    /* arccsch(x) = ln((1 + sqrt(1 + x^2)) / x) */
    result = log((1.+SQRT((1+SQR(evalAST(child[0]))))) /evalAST(child[0]));
    break;

  case AST_FUNCTION_ARCSEC:
    /* arcsec(x) = arctan(sqrt((x - 1)(x + 1))) */
    result = atan( SQRT( (evalAST(child[0])-1.)*(evalAST(child[0])+1.) ) );
    break;

  case AST_FUNCTION_ARCSECH:
    /* arcsech(x) = ln((1 + sqrt(1 - x^2)) / x) */
    result = log((1.+pow((1-SQR(evalAST(child[0]))),0.5))/evalAST(child[0]));
    break;

  case AST_FUNCTION_ARCSIN:
    result = asin(evalAST(child[0]));
    break;
  case AST_FUNCTION_ARCSINH:
#ifndef WIN32
    result = asinh(evalAST(child[0]));
#else
	result = log(evalAST(child[0]) + SQR(evalAST(child[0]) * evalAST(child[0]) + 1.));
#endif
    break;

  case AST_FUNCTION_ARCTAN:
    result = atan(evalAST(child[0]));
    break;
  case AST_FUNCTION_ARCTANH:
#ifndef WIN32
    result = atanh(evalAST(child[0]));
#else
	result = log((1. / evalAST(child[0]) + 1.) / (1. / evalAST(child[0]) - 1.)) / 2.;
#endif
    break;

  case AST_FUNCTION_CEILING:
    result = ceil(evalAST(child[0]));
    break;

  case AST_FUNCTION_COS:
    result = cos(evalAST(child[0]));
    break;
  case AST_FUNCTION_COSH:
    result = cosh(evalAST(child[0]));
    break;

  case AST_FUNCTION_COT:
    /* cot x = 1 / tan x */
    result = (1./tan(evalAST(child[0])));
    break;
  case AST_FUNCTION_COTH:
    /* coth x = cosh x / sinh x */
    result = cosh(evalAST(child[0])) / sinh(evalAST(child[0]));
    break;
  case AST_FUNCTION_CSC:
    /* csc x = 1 / sin x */
    result = (1./sin(evalAST(child[0])));
    break;

  case AST_FUNCTION_CSCH:
    /* csch x = 1 / cosh x  */
    result = (1./cosh(evalAST(child[0])));
    break;

  case AST_FUNCTION_EXP:
    result = exp(evalAST(child[0]));
    break;

  case AST_FUNCTION_FACTORIAL:
    {
      printf("\n-------------MESSAGE FROM EVALUATION FUNCTION-------------\n");
      printf("The factorial is only implemented for integer values. If a floating\n");
      printf("point number is passed, the floor value is used for calculation!\n");
      printf("-----------------------END MESSAGE--------------------------\n\n");
      i = (int)floor(evalAST(child[0]));
      for(result=1;i>1;--i)
        result *= i;
    }
    break;

  case AST_FUNCTION_FLOOR:
    result = floor(evalAST(child[0]));
    break;

  case AST_FUNCTION_LN:
    result = log(evalAST(child[0]));
    break;

  case AST_FUNCTION_LOG:
    result = log10(evalAST(child[0]));
    break;

  case AST_FUNCTION_PIECEWISE:
    printf("\n-------------MESSAGE FROM EVALUATION FUNCTION-------------\n");
    printf("This function is not implemented yet.\n");
    printf("The value defaults to 0.\n");
    printf("-----------------------END MESSAGE--------------------------\n\n");
    result = 0.0;
    break;

  case AST_FUNCTION_POWER:
    result = pow(evalAST(child[0]),evalAST(child[1]));
    break;

  case AST_FUNCTION_ROOT:
    result = pow(evalAST(child[1]),(1./evalAST(child[0])));
    break;

  case AST_FUNCTION_SEC:
    /* sec x = 1 / cos x */
    result = 1./cos(evalAST(child[0]));
    break;

  case AST_FUNCTION_SECH:
    /* sech x = 1 / sinh x */
    result = 1./sinh(evalAST(child[0]));
    break;

  case AST_FUNCTION_SIN:
    result = sin(evalAST(child[0]));
    break;

  case AST_FUNCTION_SINH:
    result = sinh(evalAST(child[0]));
    break;

  case AST_FUNCTION_TAN:
    result = tan(evalAST(child[0]));
    break;

  case AST_FUNCTION_TANH:
    result = tanh(evalAST(child[0]));
    break;

  case AST_LOGICAL_AND:
    result = (double) ((evalAST(child[0])) && (evalAST(child[1])));
    break;

  case AST_LOGICAL_NOT:
    result = (double) (!(evalAST(child[0])));
    break;

  case AST_LOGICAL_OR:
    result = (double) ((evalAST(child[0])) || (evalAST(child[1])));
    break;

  case AST_LOGICAL_XOR:
    result = (double) ((!(evalAST(child[0])) && (evalAST(child[1])))
                       || ((evalAST(child[0])) &&  !(evalAST(child[1]))));
    break;

  case AST_RELATIONAL_EQ :
    result = (double) ((evalAST(child[0])) == (evalAST(child[1])));
    break;

  case AST_RELATIONAL_GEQ:
    result = (double) ((evalAST(child[0])) >= (evalAST(child[1])));
    break;

  case AST_RELATIONAL_GT:
    result = (double) ((evalAST(child[0])) > (evalAST(child[1])));
    break;

  case AST_RELATIONAL_LEQ:
    result = (double) ((evalAST(child[0])) <= (evalAST(child[1])));
    break;

  case AST_RELATIONAL_LT :
    result = (double) ((evalAST(child[0])) < (evalAST(child[1])));
    break;

  default:
    result = 0;
    break;
  }

  free(child);

  return result;
}
Ejemplo n.º 4
0
line crosspointCC(const circle& c, const circle& d) {
    double dist = abs(d.o - c.o), th = arg(d.o - c.o);
    double ph = acos((c.r * c.r + dist * dist - d.r * d.r) / (2.0 * c.r * dist));
    return line(c.o + polar(c.r, th - ph), c.o + polar(c.r, th + ph));
}
Ejemplo n.º 5
0
void MobileSimulator::MouseUpEvent(Vec2i screenCoordinates, MouseButtonInput button)
{
	_multiGestureOngoing = false;
	_gestureType = NONE;
	_mouseDown = false;

	TouchList* tl = &TouchListener::GetTouchList();
	
	if (theInput.IsKeyDown(ANGEL_KEY_LEFTCONTROL) || theInput.IsKeyDown(ANGEL_KEY_RIGHTCONTROL))
	{
		TouchList::iterator it = tl->begin();
		while (it != tl->end())
		{
			SendTouchNotifiers((*it), TOUCH_END);
			delete (*it);
			it = tl->erase(it);
		}
	}
	else
	{
		// just a single touch, but we'll iterate anyway
		TouchList::iterator it = tl->begin();
		while (it != tl->end())
		{
			if ( (theWorld.GetCurrentTimeSeconds() - (*it)->MotionStartTime) < SWIPE_MAX_DURATION)
			{
				Vector2 start((*it)->StartingPoint);
				Vector2 end((*it)->CurrentPoint);
				Vector2 motion = end - start;
				if (motion.LengthSquared() >= (SWIPE_MIN_DISTANCE * SWIPE_MIN_DISTANCE))
				{
					float angle = MathUtil::ToDegrees(acos(Vector2::Dot(Vector2::UnitX, Vector2::Normalize(motion))));
					if (motion.Y > 0.0f)
					{
						angle = 360.0f - angle;
					}

					if      ( (angle > 45.0f) && (angle <= 135.0f) )
					{
						// swipe up
						theSwitchboard.Broadcast(new Message("MultiTouchSwipeUp"));
					}
					else if ( (angle > 135.0f) && (angle <= 225.0f) )
					{
						// swipe left
						theSwitchboard.Broadcast(new Message("MultiTouchSwipeLeft"));
					}
					else if ( (angle > 225.0f) && (angle <= 315.0f) )
					{
						// swipe down
						theSwitchboard.Broadcast(new Message("MultiTouchSwipeDown"));
					}
					else
					{
						// swipe right
						theSwitchboard.Broadcast(new Message("MultiTouchSwipeRight"));
					}
				}
			}
			SendTouchNotifiers((*it), TOUCH_END);
			delete (*it);
			it = tl->erase(it);
		}
	}
}
Ejemplo n.º 6
0
# include <math.h>

# include "sparse-adjacency.h"
# include "motif-search.h"
# include "nauty_interface.h"
# include "graphical-models.h"

# include <R.h>
# include <Rinternals.h> 
# include <Rdefines.h>

# include "concentration.h"

# define MSG 0

static double Pi        = acos(-1.0);
static double TwoPi     =  2*Pi;
static double FourPi    =  4*Pi;
static double SixteenPi = 16*Pi;


// Struture to handle the sort on the canonic form
typedef struct{
  int *canonic;
  int *occurrence;
} canonic_list_t;

// Structure to handle the 2nd sort on the occurrences U 
typedef struct{
  int *occurrence;
  int remove;
Ejemplo n.º 7
0
double Transformation::angle(QVector3D a, QVector3D b)
{
	return acos(dotProduct(a, b) / (a.length()*b.length()));
}
Ejemplo n.º 8
0
static MATRIX *
compute_pca(MRI *mri_in, MRI *mri_ref) {
  int    row, col, i ;
  float  dot ;
  MATRIX *m_ref_evectors = NULL, *m_in_evectors = NULL ;
  float  in_evalues[3], ref_evalues[3] ;
  double  ref_means[3], in_means[3] ;

  if (!m_ref_evectors)
    m_ref_evectors = MatrixAlloc(3,3,MATRIX_REAL) ;
  if (!m_in_evectors)
    m_in_evectors = MatrixAlloc(3,3,MATRIX_REAL) ;

  if (binarize) {
    MRIbinaryPrincipleComponents(mri_ref, m_ref_evectors, ref_evalues,
                                 ref_means, thresh_low);
    MRIbinaryPrincipleComponents(mri_in,m_in_evectors,in_evalues,in_means,
                                 thresh_low);
  } else {
    MRIprincipleComponents(mri_ref, m_ref_evectors, ref_evalues, ref_means,
                           thresh_low);
    MRIprincipleComponents(mri_in,m_in_evectors,in_evalues,in_means,
                           thresh_low);
  }

  order_eigenvectors(m_in_evectors, m_in_evectors) ;
  order_eigenvectors(m_ref_evectors, m_ref_evectors) ;

  /* check to make sure eigenvectors aren't reversed */
  for (col = 1 ; col <= 3 ; col++) {
#if 0
    float theta ;
#endif

    for (dot = 0.0f, row = 1 ; row <= 3 ; row++)
      dot += m_in_evectors->rptr[row][col] * m_ref_evectors->rptr[row][col] ;

    if (dot < 0.0f) {
      fprintf(stderr, "WARNING: mirror image detected in eigenvector #%d\n",
              col) ;
      dot *= -1.0f ;
      for (row = 1 ; row <= 3 ; row++)
        m_in_evectors->rptr[row][col] *= -1.0f ;
    }
#if 0
    theta = acos(dot) ;
    fprintf(stderr, "angle[%d] = %2.1f\n", col, DEGREES(theta)) ;
#endif
  }
  fprintf(stderr, "ref_evectors = \n") ;
  for (i = 1 ; i <= 3 ; i++)
    fprintf(stderr, "\t\t%2.2f    %2.2f    %2.2f\n",
            m_ref_evectors->rptr[i][1],
            m_ref_evectors->rptr[i][2],
            m_ref_evectors->rptr[i][3]) ;

  fprintf(stderr, "\nin_evectors = \n") ;
  for (i = 1 ; i <= 3 ; i++)
    fprintf(stderr, "\t\t%2.2f    %2.2f    %2.2f\n",
            m_in_evectors->rptr[i][1],
            m_in_evectors->rptr[i][2],
            m_in_evectors->rptr[i][3]) ;

  return(pca_matrix(m_in_evectors, in_means,m_ref_evectors, ref_means)) ;
}
Ejemplo n.º 9
0
    float Vector3::angle(const Vector3& v) const
    {
        float angle = dot(v)/(magnitude()*v.magnitude());

        return acos(angle);
    }
Ejemplo n.º 10
0
D3DXMATRIX Interpolate( const D3DXMATRIX& MatrixA, const D3DXMATRIX& MatrixB, float lamda)
{
	D3DXMATRIX iMat = MatrixA;
	D3DXMATRIX result = MatrixB;

	// Inverse of MatrixA
	FLOAT determinant = D3DXMatrixDeterminant(&iMat);
	D3DXMatrixInverse(&iMat, &determinant, &iMat);

	// Remove MatrixA's transformation from MatrixB
	result *= iMat;

	// iMat is now the intermediary transformation from MatrixA to MatrixB
	// ie: iMat * MatrixA = MatrixB
	iMat = result;

	// The trace of our matrix
	float trace = 1.0f + iMat._11 + iMat._22 + iMat._33;

	float quatResult[4];

	// Calculate the quaternion of iMat
	// If trace is greater than 0, but consider small values that
	// might result in 0 when operated upon due to floating point error
	if( trace > 0.00000001 )
	{
		float S = sqrt(trace)*2;
		quatResult[0] = (iMat._32 - iMat._23) / S;
		quatResult[1] = (iMat._13 - iMat._31) / S;
		quatResult[2] = (iMat._21 - iMat._12) / S;
		quatResult[3] = 0.25f * S;
	}
	else
	{
		if( iMat._11 > iMat._22 && iMat._11 > iMat._33 )
		{
			float S = float(sqrt( 1.0 + iMat._11 - iMat._22 - iMat._33 ) * 2);
			quatResult[0] = 0.25f * S;
			quatResult[1] = (iMat._21 + iMat._12) / S;
			quatResult[2] = (iMat._13 + iMat._31) / S;
			quatResult[3] = (iMat._32 - iMat._23) / S;
		}
		else if( iMat._22 > iMat._33 )
		{
			float S = float(sqrt( 1.0 + iMat._22 - iMat._11 - iMat._33 ) * 2);
			quatResult[0] = (iMat._21 + iMat._12) / S;
			quatResult[1] = 0.25f * S;
			quatResult[2] = (iMat._32 + iMat._23) / S;
			quatResult[3] = (iMat._13 - iMat._31) / S;
		}
		else
		{
			float S = float(sqrt( 1.0 + iMat._33 - iMat._11 - iMat._22 ) * 2);
			quatResult[0] = (iMat._13 + iMat._31) / S;
			quatResult[1] = (iMat._32 + iMat._23) / S;
			quatResult[2] = 0.25f * S;
			quatResult[3] = (iMat._21 - iMat._12) / S;
		}
	}

	// Get the magnitude of our quaternion
	float quatMagnitude = sqrt( quatResult[0]*quatResult[0] + quatResult[1]*quatResult[1] + quatResult[2]*quatResult[2] + quatResult[3]*quatResult[3] );

	// Normalize our quaternion
	float quatNormalized[4] = { quatResult[0]/quatMagnitude, quatResult[1]/quatMagnitude, quatResult[2]/quatMagnitude, quatResult[3]/quatMagnitude }; 

	// Calculate the angles relevant to our quaternion
	float cos_a = quatNormalized[3];
	float angle = acos( cos_a ) * 2;
	float sin_a = float(sqrt( 1.0 - cos_a * cos_a ));
	
	// If there was no rotation between matrices, calculation
	// of the rotation matrix will end badly. So just do the linear
	// interpolation of the translation component and return
	if( angle == 0.0 )
	{
		result = MatrixA;

		result.m[3][0] = MatrixA.m[3][0] + ((MatrixB.m[3][0]-MatrixA.m[3][0])*lamda);
		result.m[3][1] = MatrixA.m[3][1] + ((MatrixB.m[3][1]-MatrixA.m[3][1])*lamda);
		result.m[3][2] = MatrixA.m[3][2] + ((MatrixB.m[3][2]-MatrixA.m[3][2])*lamda);

		return result;
	}


	// Our axis of abitrary rotation
	D3DXVECTOR3 axis;

	if( fabs( sin_a ) < 0.0005 )
		sin_a = 1;

	axis.x = quatNormalized[0] / sin_a;
	axis.y = quatNormalized[1] / sin_a;
	axis.z = quatNormalized[2] / sin_a;

	// Get the portion of the angle to rotate by
	angle *= lamda;

	D3DXVec3Normalize(&axis, &axis);

	// Calculate the quaternion for our new (partial) angle of rotation
	sin_a = sin( angle / 2 );
	cos_a = cos( angle / 2 );
	quatNormalized[0] = axis.x * sin_a;
	quatNormalized[1] = axis.y * sin_a;
	quatNormalized[2] = axis.z * sin_a;
	quatNormalized[3] = cos_a;

	quatMagnitude = sqrt( quatNormalized[0]*quatNormalized[0] + quatNormalized[1]*quatNormalized[1] + quatNormalized[2]*quatNormalized[2] + quatNormalized[3]*quatNormalized[3] );		
	quatNormalized[0] /= quatMagnitude;
	quatNormalized[1] /= quatMagnitude;
	quatNormalized[2] /= quatMagnitude;
	quatNormalized[3] /= quatMagnitude;

	// Calculate our partial rotation matrix
	float xx      = quatNormalized[0] * quatNormalized[0];
	float xy      = quatNormalized[0] * quatNormalized[1];
	float xz      = quatNormalized[0] * quatNormalized[2];
	float xw      = quatNormalized[0] * quatNormalized[3];
	float yy      = quatNormalized[1] * quatNormalized[1];
	float yz      = quatNormalized[1] * quatNormalized[2];
	float yw      = quatNormalized[1] * quatNormalized[3];
	float zz      = quatNormalized[2] * quatNormalized[2];
	float zw      = quatNormalized[2] * quatNormalized[3];

	result._11  = 1 - 2 * ( yy + zz );
	result._12  =     2 * ( xy - zw );
	result._13  =     2 * ( xz + yw );
	result._21  =     2 * ( xy + zw );
	result._22  = 1 - 2 * ( xx + zz );
	result._23  =     2 * ( yz - xw );
	result._31  =     2 * ( xz - yw );
	result._32  =     2 * ( yz + xw );
	result._33 = 1 - 2 * ( xx + yy );
	result._14  = result._24 = result._34 = result._41 = result._42 = result._43 = 0;
	result._44 = 1;

	// Combine our partial rotation with MatrixA
	result *= MatrixA;

	// Linear interpolation of the translation components of the matrices
	result.m[3][0] = MatrixA.m[3][0] + ((MatrixB.m[3][0]-MatrixA.m[3][0])*lamda);
	result.m[3][1] = MatrixA.m[3][1] + ((MatrixB.m[3][1]-MatrixA.m[3][1])*lamda);
	result.m[3][2] = MatrixA.m[3][2] + ((MatrixB.m[3][2]-MatrixA.m[3][2])*lamda);

	return result;
}
Ejemplo n.º 11
0
#include <cstdio>
#include <cstring>
#include <cmath>

typedef long long Long;
const int MAXN=32768;
const double pi=acos(-1.0);
const Long MOD=100000;
const int TEN=5;

double ra[MAXN];
double ia[MAXN];
double rb[MAXN];
double ib[MAXN];
double rc[MAXN];
double ic[MAXN];
char a[MAXN];
char b[MAXN];
int slena;
int slenb;
int lena;
int lenb;
int n,logn;
Long ans[MAXN];

int rev(int x,int bit)
{
    int ans=0;
    for (int i=0; i<bit; i++)
    {
        ans<<=1;
Ejemplo n.º 12
0
inline double _acos(double arg)            { return  acos(arg); }
 float Vector4::angle (Vector4 const& other) const {
     return acos( operator *(other) / (magnitude() * other.magnitude()) );
 }
Ejemplo n.º 14
0
double r_acos(real *x)
#endif
{
return( acos(*x) );
}
Ejemplo n.º 15
0
void
HlaConvertRprFomOrientationToQualNetOrientation(
    double lat,
    double lon,
    float float_psiRadians,
    float float_thetaRadians,
    float float_phiRadians,
    short& azimuth,
    short& elevation)
{
    // Notes: -----------------------------------------------------------------

    // This function converts a DIS / RPR-FOM 1.0 orientation into QualNet
    // azimuth and elevation angles.
    //
    // When the entity is located exactly at the north pole, this function
    // will return an azimuth of 0 (facing north) when the entity is pointing
    // toward 180 degrees longitude.
    //
    // When the entity is located exactly at the south pole, this function
    // will return an azimuth of 0 (facing north) when the entity is pointing
    // toward 0 degrees longitude.
    //
    // This function have not been optimized at all.
    // (e.g., the phi angle doesn't affect the results, some vector components
    // end up being multipled by 0, etc.)

    assert(lat >= -90.0 && lat <= 90.0);
    assert(lon >= -180.0 && lon <= 180.0);

    // Introduction: ----------------------------------------------------------

    // There are two coordinate systems considered:
    //
    // (1) the GCC coordinate system, and
    // (2) the entity coordinate system.
    //
    // Both are right-handed coordinate systems.
    //
    // The GCC coordinate system has well-defined axes.
    // In the entity coordinate system, the x-axis points in the direction the
    // entity is facing (the entity orientation).
    //
    // psi, theta, and phi are the angles by which one rotates the GCC axes
    // so that they match in direction with the entity axes.
    //
    // Start with the GCC axes, and rotate them in the following order:
    //
    // (1) psi, a right-handed rotation about the z-axis
    // (2) theta, a right-handed rotation about the new y-axis
    // (3) phi, a right-handed rotation about the new x-axis

    double psiRadians   = (double) float_psiRadians;
    double thetaRadians = (double) float_thetaRadians;
    double phiRadians   = (double) float_phiRadians;

    // Convert latitude and longitude into a unit vector.
    // If one imagines the earth as a unit sphere, the vector will point
    // to the location of the entity on the surface of the sphere.

    double latRadians = lat * HLA_RADIANS_PER_DEGREE;
    double lonRadians = lon * HLA_RADIANS_PER_DEGREE;

    double entityLocationX = cos(lonRadians) * cos(latRadians);
    double entityLocationY = sin(lonRadians) * cos(latRadians);
    double entityLocationZ = sin(latRadians);

    // Discussion of basic theory: --------------------------------------------

    // Start with a point b in the initial coordinate system.  b is represented
    // as a vector.
    //
    // Rotate the axes of the initial coordinate system by angle theta to
    // obtain a new coordinate system.
    //
    // Representation of point b in the new coordinate system is given by:
    //
    // c = ab
    //
    // where a is a rotation matrix:
    //
    // a = [ cos(theta)        sin(theta)
    //       -sin(theta)       cos(theta) ]
    //
    // and c is the same point, but in the new coordinate system.
    //
    // Note that the coordinate system changes; the point itself does not move.
    // Also, matrix a is for rotating the axes; the matrix is different when
    // rotating the vector while not changing the axes.
    //
    // Applying this discussion to three dimensions, and using psi, theta, and
    // phi as described previously, three rotation matrices can be created:
    //
    // Rx =
    // [ 1           0           0
    //   0           cos(phi)    sin(phi)
    //   0           -sin(phi)   cos(phi) ]
    //
    // Ry =
    // [ cos(theta)  0           -sin(theta)
    //   0           1           0
    //   sin(theta)  0            cos(theta) ]
    //
    // Rz =
    // [ cos(psi)    sin(psi)    0
    //   -sin(psi)   cos(psi)    0
    //   0           0           1 ]
    //
    // where
    //
    // c = ab
    // a = (Rx)(Ry)(Rz)
    //
    // b is the point as represented in the GCC coordinate system;
    // c is the point as represented in the entity coordinate system.
    //
    // Note that matrix multiplication is not commutative, so the order of
    // the factors in a is important (rotate by Rz first, so it's on the right
    // side).

    // Determine elevation angle: ---------------------------------------------

    // In the computation of the elevation angle below, the change is in the
    // opposite direction, from the entity coordinate system to the GCC system.
    // So, for:
    //
    // c = ab
    //
    // Vector b represents the entity orientation as expressed in the entity
    // coordinate system.
    // Vector c represents the entity orientation as expressed in the GCC
    // coordinate system.
    //
    // It turns out that:
    //
    // a = (Rz)'(Ry)'(Rx)'
    //
    // where Rx, Ry, and Rz are given earlier, and the ' symbol indicates the
    // transpose of each matrix.
    //
    // The ordering of the matrices is reversed, since one is going from the
    // entity coordinate system to the GCC system.  The negative of psi, theta,
    // and phi angles are used, and the transposed matrices end up being the
    // correct ones.

    double a11 = cos(psiRadians) * cos(thetaRadians);
    double a12 = -sin(psiRadians) * cos(phiRadians)
                 + cos(psiRadians) * sin(thetaRadians) * sin(phiRadians);
    double a13 = -sin(psiRadians) * -sin(phiRadians)
                 + cos(psiRadians) * sin(thetaRadians) * cos(phiRadians);

    double a21 = sin(psiRadians) * cos(thetaRadians);
    double a22 = cos(psiRadians) * cos(phiRadians)
                 + sin(psiRadians) * sin(thetaRadians) * sin(phiRadians);
    double a23 = cos(psiRadians) * -sin(phiRadians)
                 + sin(psiRadians) * sin(thetaRadians) * cos(phiRadians);

    double a31 = -sin(thetaRadians);
    double a32 = cos(thetaRadians) * sin(phiRadians);
    double a33 = cos(thetaRadians) * cos(phiRadians);

    // Vector b is chosen such that it is the unit vector pointing along the
    // positive x-axis of the entity coordinate system.  I.e., vector b points
    // in the same direction the entity is facing.

    double b1 = 1.0;
    double b2 = 0.0;
    double b3 = 0.0;

    // The values below are the components of vector c, which represent the
    // entity orientation in the GCC coordinate system.

    double entityOrientationX = a11 * b1 + a12 * b2 + a13 * b3;
    double entityOrientationY = a21 * b1 + a22 * b2 + a23 * b3;
    double entityOrientationZ = a31 * b1 + a32 * b2 + a33 * b3;

    // One now has two vectors:
    //
    // (1) an entity-position vector, and
    // (2) an entity-orientation vector.
    //
    // Note that the position vector is normal to the sphere at the point where
    // the entity is located on the sphere.
    //
    // One can determine the angle between the two vectors using dot product
    // formulas.  The computed angle is the deflection from the normal.

    double dotProduct
        = entityLocationX * entityOrientationX
          + entityLocationY * entityOrientationY
          + entityLocationZ * entityOrientationZ;

    double entityLocationMagnitude
        = sqrt(pow(entityLocationX, 2)
               + pow(entityLocationY, 2)
               + pow(entityLocationZ, 2));

    double entityOrientationMagnitude
        = sqrt(pow(entityOrientationX, 2)
               + pow(entityOrientationY, 2)
               + pow(entityOrientationZ, 2));

    double deflectionFromNormalToSphereRadians
        = acos(dotProduct / (entityLocationMagnitude
                             * entityOrientationMagnitude));

    // The elevation angle is 90 degrees minus the angle for deflection from
    // normal.  (Note that the elevation angle can range from -90 to +90
    // degrees.)

    double elevationRadians
        = (HLA_PI / 2.0) - deflectionFromNormalToSphereRadians;

    elevation = (short) roundToInt(elevationRadians * HLA_DEGREES_PER_RADIAN);

    assert(elevation >= -90 && elevation <= 90);

    // Determine azimuth angle: -----------------------------------------------

    // To determine the azimuth angle, for:
    //
    // c = ab
    //
    // b is the entity orientation as represented in the GCC coordinate system.
    //
    // c is the entity orientation as expressed in a new coordinate system.
    // This is a coordinate system where the yz plane is tangent to the sphere
    // at the point on the sphere where the entity is located.  The z-axis
    // points towards true north; the y-axis points towards east; the x-axis
    // points in the direction of the normal to the sphere.
    //
    // The rotation matrix turns is then:
    //
    // a = (Ry)(Rz)
    //
    // where longitude is used for Rz and the negative of latitude is used for
    // Ry (since right-handed rotations of the y-axis are positive).

    a11 = cos(-latRadians) * cos(lonRadians);
    a12 = cos(-latRadians) * sin(lonRadians);
    a13 = -sin(-latRadians);

    a21 = -sin(lonRadians);
    a22 = cos(lonRadians);
    a23 = 0;

    a31 = sin(-latRadians) * cos(lonRadians);
    a32 = sin(-latRadians) * sin(lonRadians);
    a33 = cos(-latRadians);

    b1 = entityOrientationX;
    b2 = entityOrientationY;
    b3 = entityOrientationZ;

    // Variable unused.
    //double c1 = a11 * b1 + a12 * b2 + a13 * b3;

    double c2 = a21 * b1 + a22 * b2 + a23 * b3;
    double c3 = a31 * b1 + a32 * b2 + a33 * b3;

    // To determine azimuth, project c against the yz plane (the plane tangent
    // to the sphere at the entity location), creating a new vector without an
    // x component.
    //
    // Determine the angle between this vector and the unit vector pointing
    // north, using dot-product formulas.

    double vectorInTangentPlaneX = 0;
    double vectorInTangentPlaneY = c2;
    double vectorInTangentPlaneZ = c3;

    double northVectorInTangentPlaneX = 0;
    double northVectorInTangentPlaneY = 0;
    double northVectorInTangentPlaneZ = 1;

    dotProduct
        = vectorInTangentPlaneX * northVectorInTangentPlaneX
          + vectorInTangentPlaneY * northVectorInTangentPlaneY
          + vectorInTangentPlaneZ * northVectorInTangentPlaneZ;

    double vectorInTangentPlaneMagnitude
        = sqrt(pow(vectorInTangentPlaneX, 2)
               + pow(vectorInTangentPlaneY, 2)
               + pow(vectorInTangentPlaneZ, 2));

    double northVectorInTangentPlaneMagnitude
        = sqrt(pow(northVectorInTangentPlaneX, 2)
               + pow(northVectorInTangentPlaneY, 2)
               + pow(northVectorInTangentPlaneZ, 2));

    double azimuthRadians
        = acos(dotProduct / (vectorInTangentPlaneMagnitude
                             * northVectorInTangentPlaneMagnitude));

    // Handle azimuth values between 180 and 360 degrees.

    if (vectorInTangentPlaneY < 0.0)
    {
        azimuthRadians = (2.0 * HLA_PI) - azimuthRadians;
    }

    azimuth = (short) roundToInt(azimuthRadians * HLA_DEGREES_PER_RADIAN);
    if (azimuth == 360) { azimuth = 0; }

    assert(azimuth >= 0 && azimuth <= 359);
}
Ejemplo n.º 16
0
fiducial_detector_error_t fiducial_detector_project_fiducial(fiducial_detector_t* self, fiducial_pose_t fd_pose)
{
 
  
  if(!self->camera_models_set)
  {
    fprintf(stderr,"camera models not set\n");
    return FIDUCIAL_DETECTOR_ERR;
  }

  // reset projection
  self->fiducial_projected = 0;

  // Check if fiducial is visible
  double T[4][4];
  fiducial_pose_to_transform(fd_pose, T);
  fiducial_vec_t camera_to_fiducial = fiducial_vec_unit(fd_pose.pos);
  fiducial_vec_t fiducial_normal = {T[0][2], T[1][2], T[2][2]};

  double angle = 180 * acos( fiducial_vec_dot(camera_to_fiducial,fiducial_normal) / ( fiducial_vec_mag(fiducial_normal) * fiducial_vec_mag(camera_to_fiducial) ) ) / M_PI;

  double min_viewing_angle = self->params.min_viewing_angle;
  if( (angle > (270 + min_viewing_angle) ) || (angle < (90 + min_viewing_angle)) )
  {
    return FIDUCIAL_DETECTOR_ERR;
  }

  int in_bounds;
  int err;
  double col;
  double row;
  int cols, rows;
  double image_point[2];
  double world_point[3];
  fiducial_vec_t world_template_pt;

  //-----------------------------------------------------------------------------
  //  inner circle
  //-----------------------------------------------------------------------------
  // get image dimensions
  cols = self->camera.cols;
  rows = self->camera.rows;


  // Center point
  fiducial_vec_t center_pt = {0,0,0};
  world_template_pt = fiducial_vec_transform(fd_pose, center_pt);
  world_point[0] = world_template_pt.x;
  world_point[1] = world_template_pt.y;
  world_point[2] = world_template_pt.z;

  // reproject the point into the image frame
  err = world_point_reproject(&self->camera, &world_point[0], &image_point[0]);
  col = image_point[0];
  row = image_point[1];

  if( ((int)col < 0) || ((int)col > (cols)) ||
      ((int)row < 0) || ((int)row > (rows)) || (err != 0) )
    in_bounds = false;
  else
    in_bounds = true;

  if(in_bounds)
  {
    self->proj_center_pt.x = (double) col;
    self->proj_center_pt.y = (double) row;
  }

  // Remaining points
  int num_proj_inner_circle_pts = 0;
  int i = 0;

  for(i = 0; i < self->num_inner_circle_pts; i++)
  {
    world_template_pt = fiducial_vec_transform(fd_pose, self->inner_circle_pts[i]);
    world_point[0] = world_template_pt.x;
    world_point[1] = world_template_pt.y;
    world_point[2] = world_template_pt.z;

    err = world_point_reproject(&self->camera, &world_point[0], &image_point[0]);
    col = image_point[0];
    row = image_point[1];

    if( ((int)col < 0) || ((int)col > (cols)) ||
        ((int)row < 0) || ((int)row > (rows)) || (err != 0) )
      in_bounds = false;
    else
      in_bounds = true;

    if(in_bounds)
    {
      self->proj_inner_circle_pts[num_proj_inner_circle_pts].x = (double) col;
      self->proj_inner_circle_pts[num_proj_inner_circle_pts].y = (double) row;
      num_proj_inner_circle_pts = num_proj_inner_circle_pts + 1;
              
    }
  }

  if(num_proj_inner_circle_pts != self->num_inner_circle_pts)
  {
    return FIDUCIAL_DETECTOR_ERR;
  }
  else
  {
    self->num_proj_inner_circle_pts = self->num_inner_circle_pts;
  }


  //-----------------------------------------------------------------------------
  //  outer circle
  //-----------------------------------------------------------------------------
  // get image dimensions
  cols = self->camera.cols;
  rows = self->camera.rows;

  // Remaining points
  int num_proj_outer_circle_pts = 0;

  for(i = 0; i < self->num_outer_circle_pts; i++)
  {
    world_template_pt = fiducial_vec_transform(fd_pose, self->outer_circle_pts[i]);
    world_point[0] = world_template_pt.x;
    world_point[1] = world_template_pt.y;
    world_point[2] = world_template_pt.z;

    err = world_point_reproject(&self->camera, &world_point[0], &image_point[0]);
    col = image_point[0];
    row = image_point[1];

    if( ((int)col < 0) || ((int)col > (cols)) ||
        ((int)row < 0) || ((int)row > (rows)) || (err != 0) )
      in_bounds = false;
    else
      in_bounds = true;

    if(in_bounds)
    {
      self->proj_outer_circle_pts[num_proj_outer_circle_pts].x = (double) col;
      self->proj_outer_circle_pts[num_proj_outer_circle_pts].y = (double) row;
      num_proj_outer_circle_pts = num_proj_outer_circle_pts + 1;
    }
  }

  if(num_proj_outer_circle_pts != self->num_outer_circle_pts)
  {
    return FIDUCIAL_DETECTOR_ERR;
  }
  else
  {
    self->num_proj_outer_circle_pts = self->num_outer_circle_pts;
  }


  //-----------------------------------------------------------------------------
  //  edges
  //-----------------------------------------------------------------------------
  // get image dimensions
  cols = self->camera.cols;
  rows = self->camera.rows;

  // Remaining points
  int num_proj_edge_pts = 0;

  for(i = 0; i < self->num_edge_pts; i++)
  {
    world_template_pt = fiducial_vec_transform(fd_pose, self->edge_pts[i]);
    world_point[0] = world_template_pt.x;
    world_point[1] = world_template_pt.y;
    world_point[2] = world_template_pt.z;

    err = world_point_reproject(&self->camera, &world_point[0], &image_point[0]);
    col = image_point[0];
    row = image_point[1];

    if( ((int)col < 0) || ((int)col > (cols)) ||
        ((int)row < 0) || ((int)row > (rows)) || (err != 0) )
      in_bounds = false;
    else
      in_bounds = true;

    if(in_bounds)
    {
      self->proj_edge_pts[num_proj_edge_pts].x = (double) col;
      self->proj_edge_pts[num_proj_edge_pts].y = (double) row;
      num_proj_edge_pts = num_proj_edge_pts + 1;

    }
  }

  if(num_proj_edge_pts != self->num_edge_pts)
  {
    return FIDUCIAL_DETECTOR_ERR;
  }
  else
  {
    self->num_proj_edge_pts = self->num_edge_pts;
  }


  self->fiducial_projected = 1;

  return FIDUCIAL_DETECTOR_OK;
}
Ejemplo n.º 17
0
void Parser::optimise_call(ExprNode *node)
{
	DBL result = 0.0;
	bool have_result = true;;

	if(node->op != OP_CALL)
		return;
	if(node->child == NULL)
		return;
	if(node->child->op != OP_CONSTANT)
		return;

	switch(node->call.token)
	{
		case SIN_TOKEN:
			result = sin(node->child->number);
			break;
		case COS_TOKEN:
			result = cos(node->child->number);
			break;
		case TAN_TOKEN:
			result = tan(node->child->number);
			break;
		case ASIN_TOKEN:
			result = asin(node->child->number);
			break;
		case ACOS_TOKEN:
			result = acos(node->child->number);
			break;
		case ATAN_TOKEN:
			result = atan(node->child->number);
			break;
		case SINH_TOKEN:
			result = sinh(node->child->number);
			break;
		case COSH_TOKEN:
			result = cosh(node->child->number);
			break;
		case TANH_TOKEN:
			result = tanh(node->child->number);
			break;
		case ASINH_TOKEN:
			result = asinh(node->child->number);
			break;
		case ACOSH_TOKEN:
			result = acosh(node->child->number);
			break;
		case ATANH_TOKEN:
			result = atanh(node->child->number);
			break;
		case ABS_TOKEN:
			result = fabs(node->child->number);
			break;
		case RADIANS_TOKEN:
			result = node->child->number * M_PI / 180.0;
			break;
		case DEGREES_TOKEN:
			result = node->child->number * 180.0 / M_PI;
			break;
		case FLOOR_TOKEN:
			result = floor(node->child->number);
			break;
		case INT_TOKEN:
			result = (int)(node->child->number);
			break;
		case CEIL_TOKEN:
			result = ceil(node->child->number);
			break;
		case SQRT_TOKEN:
			result = sqrt(node->child->number);
			break;
		case EXP_TOKEN:
			result = exp(node->child->number);
			break;
		case LN_TOKEN:
			if(node->child->number > 0.0)
				result = log(node->child->number);
			else
				Error("Domain error in 'ln'.");
			break;
		case LOG_TOKEN:
			if(node->child->number > 0.0)
				result = log10(node->child->number);
			else
				Error("Domain error in 'log'.");
			break;
		case MIN_TOKEN:
			have_result = false;
			break;
		case MAX_TOKEN:
			have_result = false;
			break;
		case ATAN2_TOKEN:
			have_result = false;
			break;
		case POW_TOKEN:
			have_result = false;
			break;
		case MOD_TOKEN:
			have_result = false;
			break;
		case SELECT_TOKEN:
			have_result = false;
			break;
		case FUNCT_ID_TOKEN:
			have_result = false;
			break;
		case VECTFUNCT_ID_TOKEN:
			have_result = false;
			break;
		default:
			have_result = false;
			break;
	}

	if(have_result == true)
	{
		POV_FREE(node->call.name);
		node->number = result;
		node->op = OP_CONSTANT;
		POV_FREE(node->child);
		node->child = NULL;
	}
}
Ejemplo n.º 18
0
double* CMath::Normal2Euler(double x, double y, double z) {
   double phi = acos(y);// - 3.14159/2;
   double theta = atan(z / x) ;//- 3.14159/2;       
   return MatrixZYZ(-phi, -theta, -phi);
}
Ejemplo n.º 19
0
static long double getnumcore()
{
    /*if (*str=='!' && false) // defines.
    {
        string find;
        if (defines.find(str+1, find)) {
            str = find;
            return getnumcore(); // reiterate on define contents
        }
    }*/
    if (*str=='(')
	{
		str++;
		long double rval=eval(0);
		if (*str!=')') error("Mismatched parentheses.");
		str++;
		return rval;
	}
	if (*str=='$')
	{
		if (!isxdigit(str[1])) error("Invalid hex value.");
		if (tolower(str[2])=='x') return -42;//let str get an invalid value so it'll throw an invalid operator later on
		return strtoul(str+1, (char**)&str, 16);
	}
	if (*str=='%')
	{
		if (str[1]!='0' && str[1]!='1') error("Invalid binary value.");
		return strtoul(str+1, (char**)&str, 2);
	}
	if (*str=='\'')
	{
		if (!str[1] || str[2]!='\'') error("Invalid character.");
		unsigned int rval=table[(unsigned char)str[1]];
		str+=3;
		return rval;
	}
	if (isdigit(*str))
	{
		return strtod(str, (char**)&str);
	}
	if (isalpha(*str) || *str=='_' || *str=='.' || *str=='?')
	{
		const char * start=str;
		while (isalnum(*str) || *str=='_') str++;
		int len=str-start;
		while (*str==' ') str++;
		if (*str=='(')
		{
			str++;
			while (*str==' ') str++;
			autoarray<long double> params;
			int numparams=0;
			if (*str!=')')
			{
				while (true)
				{
					while (*str==' ') str++;
					params[numparams++]=eval(0);
					while (*str==' ') str++;
					if (*str==',')
					{
						str++;
						continue;
					}
					if (*str==')')
					{
						str++;
						break;
					}
					error("Malformed function call.");
				}
			}
			long double rval;
			for (int i=0;i<numuserfunc;i++)
			{
				if ((int)strlen(userfunc[i].name)==len && !strncmp(start, userfunc[i].name, len))
				{
					if (userfunc[i].numargs!=numparams) error("Wrong number of parameters to function.");
					char ** oldfuncargnames=funcargnames;
					long double * oldfuncargvals=funcargvals;
					const char * oldstr=str;
					int oldnumuserfuncargs=numuserfuncargs;
					funcargnames=userfunc[i].arguments;
					funcargvals=params;
					str=userfunc[i].content;
					numuserfuncargs=numparams;
					rval=eval(0);
					funcargnames=oldfuncargnames;
					funcargvals=oldfuncargvals;
					str=oldstr;
					numuserfuncargs=oldnumuserfuncargs;
					return rval;
				}
			}
			if (*str=='_') str++;
#define func(name, numpar, code)                                   \
					if (!strncasecmp(start, name, len))                      \
					{                                                        \
						if (numparams==numpar) return (code);                  \
						else error("Wrong number of parameters to function."); \
					}
#define varfunc(name, code)                                   \
					if (!strncasecmp(start, name, len))                      \
					{                                                        \
						code; \
					}
			func("sqrt", 1, sqrt(params[0]));
			func("sin", 1, sin(params[0]));
			func("cos", 1, cos(params[0]));
			func("tan", 1, tan(params[0]));
			func("asin", 1, asin(params[0]));
			func("acos", 1, acos(params[0]));
			func("atan", 1, atan(params[0]));
			func("arcsin", 1, asin(params[0]));
			func("arccos", 1, acos(params[0]));
			func("arctan", 1, atan(params[0]));
			func("log", 1, log(params[0]));
			func("log10", 1, log10(params[0]));
			func("log2", 1, log(params[0])/log(2.0));
			func("read1", 1, read1(params[0]));
			func("read2", 1, read2(params[0]));
			func("read3", 1, read3(params[0]));
			func("read4", 1, read4(params[0]));
			func("read1", 2, read1s(params[0], params[1]));
			func("read2", 2, read2s(params[0], params[1]));
			func("read3", 2, read3s(params[0], params[1]));
			func("read4", 2, read4s(params[0], params[1]));
			func("canread1", 1, validaddr(params[0], 1));
			func("canread2", 1, validaddr(params[0], 2));
			func("canread3", 1, validaddr(params[0], 3));
			func("canread4", 1, validaddr(params[0], 4));
			func("canread", 2, validaddr(params[0], params[1]));
			//varfunc("min", {
			//		if (!numparams) error("Wrong number of parameters to function.");
			//		double minval=params[0];
			//		for (int i=1;i<numparams;i++)
			//		{
			//			if (params[i]<minval) minval=params[i];
			//		}
			//		return minval;
			//	});
			//varfunc("max", {
			//		if (!numparams) error("Wrong number of parameters to function.");
			//		double maxval=params[0];
			//		for (int i=1;i<numparams;i++)
			//		{
			//			if (params[i]>maxval) maxval=params[i];
			//		}
			//		return maxval;
			//	});
#undef func
#undef varfunc
			error("Unknown function.");
		}
		else
		{
			for (int i=0;i<numuserfuncargs;i++)
			{
				if (!strncmp(start, funcargnames[i], len)) return funcargvals[i];
			}
			foundlabel=true;
			int i=labelval(&start);
			str=start;
			//if (start!=str) error("Internal error. Send this patch to Alcaro.");//not gonna add sublabel/macrolabel support here
			if (i==-1) forwardlabel=true;
			return (int)i&0xFFFFFF;
//#define const(name, val) if (!strncasecmp(start, name, len)) return val
//			const("pi", 3.141592653589793238462);
//			const("\xCF\x80", 3.141592653589793238462);
//			const("\xCE\xA0", 3.141592653589793238462);//case insensitive pi, yay
//			const("e", 2.718281828459045235360);
//#undef const
//			error("Unknown constant.");
		}
	}
	error("Invalid number.");
}
Ejemplo n.º 20
0
void CKeeperBot::BotAdjustPos()
{
	float modifier = KEEPER_MID_COEFF;
	QAngle ang = m_oldcmd.viewangles;
	Vector target = GetTeam()->m_vGoalCenter;

	if (m_vBallVel.Length2D() > 750 && m_flAngToBallVel < 60)
	{
		float yDist = GetTeam()->m_vGoalCenter.GetY() - m_vBallPos.y;
		float vAng = acos(Sign(yDist) * m_vBallDir2D.y);
		float xDist = Sign(m_vBallDir2D.x) * abs(yDist) * tan(vAng);
		target.x = clamp(m_vBallPos.x + xDist, GetTeam()->m_vGoalCenter.GetX() - 150, GetTeam()->m_vGoalCenter.GetX() + 150);
	}

	if (m_pBall->State_Get() == BALL_STATE_KEEPERHANDS && m_pBall->GetCurrentPlayer() == this)
	{
		if (ShotButtonsReleased())
		{
			modifier = KEEPER_CLOSE_COEFF;
			//m_cmd.buttons |= (IN_ATTACK2 | IN_ATTACK);
			m_cmd.buttons |= IN_ALT1;
			CSDKPlayer *pPl = FindClosestPlayerToSelf(true, true);
			if (!pPl && SDKGameRules()->IsIntermissionState())
				pPl = FindClosestPlayerToSelf(false, true);

			if (pPl)
			{
				VectorAngles(pPl->GetLocalOrigin() - GetLocalOrigin(), ang);
				ang[PITCH] = g_IOSRand.RandomFloat(-40, 0);
				//m_flBotNextShot = gpGlobals->curtime + 1;
			}
			else
			{
				VectorAngles(Vector(0, GetTeam()->m_nForward, 0), ang);
				ang[YAW] += g_IOSRand.RandomFloat(-45, 45);
				ang[PITCH] = g_IOSRand.RandomFloat(-40, 0);
				//m_flBotNextShot = gpGlobals->curtime + 1;
			}
		}
	}
	else// if (gpGlobals->curtime >= m_flBotNextShot)
	{
		VectorAngles(m_vDirToBall, ang);
		float ballDistToGoal = (m_vBallPos - GetTeam()->m_vGoalCenter).Length2D();
		CSDKPlayer *pClosest = FindClosestPlayerToBall();
		m_cmd.buttons |= IN_ATTACK;

		if (ballDistToGoal < 750 && m_vDirToBall.Length2D() < 200 && m_vDirToBall.z < 200 && (m_vDirToBall.z < 80 || m_vBallVel.z <= 0))
		{
			modifier = KEEPER_CLOSE_COEFF;// max(0.15f, 1 - ballDistToGoal / 750);
			VectorAngles(Vector(0, GetTeam()->m_nForward, 0), ang);
			bool diving = false;

			if (m_flAngToBallVel < 60 && m_flAngToBallVel > 15)
			{
				if (abs(m_vDirToBall.x) > 50 && abs(m_vDirToBall.x) < 200 && m_vDirToBall.z < 150 && abs(m_vDirToBall.x) < 150 && m_vBallVel.Length() > 200)
				{
					m_cmd.buttons |= IN_JUMP;
					m_cmd.buttons |= Sign(m_vDirToBall.x) == GetTeam()->m_nRight ? IN_MOVERIGHT : IN_MOVELEFT;
					//m_cmd.buttons |= (IN_ATTACK2 | IN_ATTACK);
					diving = true;
				}
				else if (m_vDirToBall.z > 100 && m_vDirToBall.z < 150 && m_vDirToBall.Length2D() < 100)
				{
					m_cmd.buttons |= IN_JUMP;
					//m_cmd.buttons |= (IN_ATTACK2 | IN_ATTACK);
					diving = true;
				}
				else if (abs(m_vDirToBall.y) > 50 && abs(m_vDirToBall.y) < 200 && m_vDirToBall.z < 100 && abs(m_vDirToBall.x) < 50 && m_vBallVel.Length() < 200 && pClosest != this)
				{
					m_cmd.buttons |= IN_JUMP;
					m_cmd.buttons |= Sign(m_vLocalDirToBall.x) == GetTeam()->m_nForward ? IN_FORWARD : IN_BACK;
					//m_cmd.buttons |= (IN_ATTACK2 | IN_ATTACK);
					diving = true;
				}
			}

			if (!diving)
			{
				if (m_vDirToBall.Length2D() < 50)
				{
					modifier = KEEPER_CLOSE_COEFF;
					//m_cmd.buttons |= (IN_ATTACK2 | IN_ATTACK);
					CSDKPlayer *pPl = FindClosestPlayerToSelf(true, true);
					if (!pPl && SDKGameRules()->IsIntermissionState())
						pPl = FindClosestPlayerToSelf(false, true);

					if (pPl)
					{
						VectorAngles(pPl->GetLocalOrigin() - GetLocalOrigin(), ang);
						ang[PITCH] = g_IOSRand.RandomFloat(-40, 0);
						//m_flBotNextShot = gpGlobals->curtime + 1;
					}
					else
					{
						VectorAngles(Vector(0, GetTeam()->m_nForward, 0), ang);
						ang[YAW] += g_IOSRand.RandomFloat(-45, 45);
						ang[PITCH] = g_IOSRand.RandomFloat(-40, 0);
						//m_flBotNextShot = gpGlobals->curtime + 1;
					}
				}
				else
					modifier = KEEPER_CLOSE_COEFF;
			}
		}
		else if (ballDistToGoal < 1250 && m_flAngToBallVel < 60 && m_vBallVel.Length2D() > 300 && (m_vBallVel.z > 100 || m_vDirToBall.z > 100))
		{
			modifier = KEEPER_FAR_COEFF;
		}
		else if (ballDistToGoal < 1000 && m_vDirToBall.z < 80 && m_vBallVel.Length2D() < 300 && m_vBallVel.z < 100)
		{
			if (pClosest == this)
				modifier = KEEPER_CLOSE_COEFF;
			else
				modifier = max(KEEPER_FAR_COEFF, 1 - pow(min(1, ballDistToGoal / 750.0f), 2));
		}
		else
		{
			modifier = KEEPER_MID_COEFF;
		}

		m_cmd.viewangles = ang;
		m_LastAngles = m_cmd.viewangles;
		SetLocalAngles(m_cmd.viewangles);
		SnapEyeAngles(ang);

		Vector targetPosDir = target + modifier * (m_vBallPos - target) - GetLocalOrigin();
		targetPosDir.z = 0;
		float dist = targetPosDir.Length2D();
		VectorNormalizeFast(targetPosDir);
		Vector localDir;
		VectorIRotate(targetPosDir, EntityToWorldTransform(), localDir);
		//float speed;
		//if (dist < 10)
		//	speed = 0;
		//else if (dist < 100)
		//	speed = mp_runspeed.GetInt();
		//else
		//	speed = mp_sprintspeed.GetInt();
		//float speed = clamp(dist - 10, 0, mp_runspeed.GetInt());
		float speed = 0;

		if (dist > 30)
			speed = clamp(5 * dist, 0, mp_sprintspeed.GetInt() * (mp_botkeeperskill.GetInt() / 100.0f));

		if (speed > mp_runspeed.GetInt())
			m_cmd.buttons |= IN_SPEED;

		m_cmd.forwardmove = localDir.x * speed;
		m_cmd.sidemove = -localDir.y * speed;

		if (m_cmd.forwardmove > 0)
			m_cmd.buttons |= IN_FORWARD;
		else if (m_cmd.forwardmove < 0)
			m_cmd.buttons |= IN_BACK;

		if (m_cmd.sidemove > 0)
			m_cmd.buttons |= IN_RIGHT;
		else if (m_cmd.sidemove < 0)
			m_cmd.buttons |= IN_MOVELEFT;
	}

	m_cmd.viewangles = ang;
}
Ejemplo n.º 21
0
//------------------------------------------------------------------------------
// onRfEmissionEventAntenna() -- process events for RF Emission not sent by us.
//
// 1) Build a list of emission packets from the queue and compute the
//    Line-Of-Sight (LOS) vectors back to the transmitter.
//
// 2) Transform LOS vectors to antenna coordinates
//
// 3) Compute antenna gains in the direction of the transmitter
//
// 4) Compute Antenna Effective Gains
//
// 5) Compute Antenna Effective Area and Polarization Gains
//
// 6) Compute total receiving antenaa gain and send the emission to our sensor
//------------------------------------------------------------------------------
bool Antenna::onRfEmissionEvent(Emission* const em)
{
   // Is this emission from a player of interest?
   if (fromPlayerOfInterest(em)) {

      Player* ownship = getOwnship();
      RfSystem* sys1 = getSystem();
      if (ownship != 0 && sys1 != 0) {
         sys1->ref();

         // Line-Of-Sight (LOS) vectors back to the transmitter.
         osg::Vec3d xlos = em->getTgtLosVec();
         osg::Vec4d los0( xlos.x(), xlos.y(), xlos.z(), 0.0);

         // 2) Transform local NED LOS vectors to antenna coordinates
         osg::Matrixd mm = getRotMat();
         mm *= ownship->getRotMat();
         osg::Vec4d losA = mm * los0;

         // ---
         // Compute antenna gains in the direction of the transmitter
         // ---
         double rGainDb = 0.0f;
         if (gainPattern != 0) {

            Basic::Func1* gainFunc1 = dynamic_cast<Basic::Func1*>(gainPattern);
            Basic::Func2* gainFunc2 = dynamic_cast<Basic::Func2*>(gainPattern);
            if (gainFunc2 != 0) {
               // ---
               // 3-a) Antenna pattern: 2D table (az & el off antenna boresight)
               // ---

               // Get component arrays and ground range squared
               double xa =  losA.x();
               double ya =  losA.y();
               double za = -losA.z();
               double ra2 = xa*xa + ya*ya;

               // Compute range along antenna x-y plane
               double ra = sqrt(ra2);

               // Compute azimuth off boresight
               double aazr = atan2(ya,xa);

               // Compute elevation off boresight
               double aelr = atan2(za,ra);

               // Lookup gain in 2D table and convert from dB
               if (gainPatternDeg)
                  rGainDb = gainFunc2->f( aazr * Basic::Angle::R2DCC, aelr * Basic::Angle::R2DCC );
               else
                  rGainDb = gainFunc2->f( aazr, aelr );

            }

            else if (gainFunc1 != 0) {
               // ---
                  // 3-b) Antenna Pattern: 1D table (off antenna boresight only
                  // ---

                  // Compute angle off antenna boresight
                  double aar = acos(losA.x());

                  // Lookup gain in 1D table and convert from dB
                  if (gainPatternDeg)
                     rGainDb = gainFunc1->f( aar * Basic::Angle::R2DCC );
                  else
                     rGainDb = gainFunc1->f(aar);

            }
         }

         // Compute off-boresight gain
         double rGain = pow(10.0,rGainDb/10.0);

         // Compute Antenna Effective Gain
         double aeGain = rGain * getGain();
         double lambda = em->getWavelength();
         double aea = getEffectiveArea(aeGain, lambda);

         double pGain = getPolarizationGain(em->getPolarization());
         double raGain = aea * pGain;

         sys1->rfReceivedEmission(em, this, LCreal(raGain));

         sys1->unref();
      }

   }

   return BaseClass::onRfEmissionEvent(em);
}
Ejemplo n.º 22
0
#include <cmath>
#include <cstdio>
#include <cstring>
#include <algorithm>
typedef long long LL;
const int maxn = 100001, maxm = 131072, mod = 313;
const long double pi = acos(-1.0);
struct complex
{
    long double r, i;
    complex() {}
    complex(long double x, long double y) : r(x), i(y) {}
    friend complex operator + (const complex &a, const complex &b) { return complex(a.r + b.r, a.i + b.i); }
    friend complex operator - (const complex &a, const complex &b) { return complex(a.r - b.r, a.i - b.i); }
    friend complex operator * (const complex &a, const complex &b) { return complex(a.r * b.r - a.i * b.i, a.r * b.i + a.i * b.r); }
} x[maxm], y[maxm];
void FFT(complex a[], int n, int flag)
{
    for(int i = 1, j = n >> 1, k; i < n - 1; ++i)
    {
        if(i < j)
            std::swap(a[i], a[j]);
        for(k = n >> 1; j >= k; k >>= 1)
            j -= k;
        if(j < k)
            j += k;
    }
    for(int i = 1; i < n; i <<= 1)
    {
        complex wn(cos(pi / i), flag * sin(pi / i));
        for(int j = 0; j < n; j += i << 1)
Ejemplo n.º 23
0
static void sunmodel_ae_0(double slat, double slon, double lat, double lon, 
        double *azm, double *elv, char use_arc) {
    double azimuth;
    double omega, csz, elevation, az_denom;
    double sinlat, coslat, sinslat, cosslat;
    
    omega = lon - slon;
    sinlat = sin(lat);
    coslat = cos(lat);
    sinslat = sin(slat);
    cosslat = cos(slat);
    
    csz = sinlat * sinslat + coslat * cosslat * cos(omega);
    
    if (csz > 1.0) {
        csz = 1.0;
    }
    else {
        if (csz < -1.0) { 
            csz = -1.0;
        }
    }
    
    elevation = asin(csz);
    az_denom = coslat * cos(elevation);
    
    if (fabs(az_denom) > DBL_EPSILON) { // TODO check relatyvely nominator
        azimuth = (sinlat * csz - sinslat) / az_denom;
    
        if (fabs(azimuth) > 1.0) {
            if (azimuth < 0.0) {
                azimuth = -1.0;
            } 
            else {
                azimuth = 1.0;
            }
        }
        
        azimuth = M_PI - acos(azimuth);
        if (omega > 0.0) {
            azimuth = -azimuth;
        }
    } 
    else {
        if (lat > 0.0) {
            azimuth = M_PI;
        } else { 
            azimuth = 0.0;
        }
    }
    
    if (azimuth < 0.0) {
        azimuth += 2.0 * M_PI;
    }
    
    if (use_arc == 1 || use_arc == 'y') {
        elevation += sunmodel_atmo_refraction_correction(elevation);
    }

//    if (elevation < DEG2RAD(-18.0)) {
//        puts("A Night at the Roxbury");
//    }
    
    if (azm != NULL) {
        (*azm) = azimuth;
    }
    
    if (elv != NULL) {
        (*elv) = elevation;
    }
}
Ejemplo n.º 24
0
int compute_abf(double x, double y, long *mat, double *a, double *b, double *f)
{
  long i, j, substeps, type0, half ;
  double angle ;
  double x1, y1 ;
  double m11,m22,m12, m21, lambda_small, lambda, lambda1, lambda2 ;
  double rs1, rs2, q1, q2, n11, n12, n22, r1, r2 ;
  double x4   = 0.0 ;
	double y4   = 0.0 ;
	double x2   = 0.0 ;
	double y2   = 0.0 ;
	double x2y2 = 0.0 ;
	double a_down, a_top ;
	double b_down, b_top ;
	double tx, ty ;

  type0 = get_mat_type(x, y) ;
  *mat = type0;

  angle = (2*PI) / ((double) lines) ;
  substeps = (radius / step) + 1 ; 

#if 0
  fprintf (msgout,"angle = %e substeps = %li (%e|%e)\n", angle, substeps, radius, step);
#endif

  for  (i=0; i<lines; i++) 
  {
    for  (j=0; j<substeps; j++) 
    {
      get_cyl_dir(step*((double)j), ((double) i)*angle, x, y, &x1, &y1) ;

      xi[i] = x1 ;
      yi[i] = y1 ;
      ri[i] = step*((double)j) ;

      /*printf("[%e, %e] => [%e, %e]\n",step*((double)j), ((double)i)*angle, x1, y1);*/

      if (get_mat_type(x1, y1) != type0) 
      { 
        break ; 
      }
    }
  }

  half = (long) (lines/2) ;

  for  (i=0; i<half; i++) 
  {
    if (ri[i] >= ri[i+half])
    {
      ri[i] = ri[i+half] ;
      get_cyl_dir(ri[i], angle*(double)(i),x,y,&xi[i],&yi[i]) ;
    }
    else 
    {
      ri[i+half] = ri[i] ;
      get_cyl_dir(ri[i], angle*(double)(i+half),x,y,&xi[i+half],&yi[i+half]) ;
    }
  }

#if 0
  for  (i=0; i<lines; i++) { printf("%e %e\n",xi[i], yi[i]); }
#endif
#if 0
  for  (i=0; i<lines; i++) { printf("%li %e \n",i, ri[i]); }
#endif


  m11 = 0.0 ;
  m22 = 0.0 ;
  m12 = 0.0 ;
  m21 = 0.0 ;

  for (i=0; i<lines; i++) 
  {
    m11 += pow ( (xi[i] - x), 2 ) ;
    m22 += pow ( (yi[i] - y), 2 ) ;

    m12 +=  pow((xi[i] - x),2) * pow((yi[i] - y),2) ;
  }

  m11 = m11 / lines ;
  m22 = m22 / lines ;
  m12 = m12 / lines ;
  m21 = m12 ;

  q1 = (m11+m22) ;
	q2 = sqrt( pow(m11+m22, 2) + 4.0*(m12*m21-m11*m22) );

	lambda1 = 0.5 * (q1 + q2 ) ;
	lambda2 = 0.5 * (q1 - q2 ) ;


	/* getting larger one: */
	if (lambda2 > lambda1) 
	{ 
		lambda       = lambda2 ; 
		lambda_small = lambda1 ; 
	}
	else                   
	{ 
		lambda       = lambda1 ; 
		lambda_small = lambda2 ; 
	}

#if 0
	/* eigenvector for larger eigenvalue */
	n11 = m11 - lambda ;
	n22 = m22 - lambda ;
	n12 = m12 ;

	r1 = (n11*n22)/n12 - n12 ;
	r2 = (-1.0)* (n11/n12) ;

	/* eigenvector for smaller eigenvalue */
	n11 = m11 - lambda_small ;
	n22 = m22 - lambda_small ;
	n12 = m12 ;

	rs1 = 1 ;
	rs2 = 0 ;
	/*
	rs1 = (n11*n22)/n12 - n12 ;
	rs2 = (-1.0)* (n11/n12) ;
	*/
#else
		r1 = lambda -m22 ;
		r2 = m12 ;
		rs1 = 1 ;
		rs2 = 0 ;
		/*
		rs1 = lambda_small -m22 ;
		rs2 =  m12 ;
		*/

		/*
		rs1 = -m22 ;
		rs2 = m11 - lambda_small ;
		*/
#endif

	if ( ( sqrt(r1*r1 + r2*r2) * sqrt(rs1*rs1 + rs2*rs2) ) == 0.0 )
	{
		*f = 0.0 ;
	}
	else 
	{
		*f = acos (
		 	(r1*rs1 + r2*rs2) / ( sqrt(r1*r1 + r2*r2) * sqrt(rs1*rs1 + rs2*rs2) )
			) ;
	}


  /* ################################ */
  *a = 0.0 ;
	*b = 0.0 ;

	/* transform coordinates: */
	for (i=0; i<lines; i++) 
	{
		xi[i] -= x ;
		yi[i] -= y ;
	}

	for (i=0; i<lines; i++) 
	{
		tx = xi[i] ;
		ty = yi[i] ;
		
		xi[i] = tx * cos ((*f)) + ty * sin ((*f)) ;
		yi[i] = -tx * sin ((*f)) + ty * cos ((*f)) ;
	}

	for (i=0; i<lines; i++)
	{
		x2   += pow(xi[i],2) ;
		x4   += pow(xi[i],4) ;

		y2   += pow(yi[i],2) ;
		y4   += pow(yi[i],4) ;

		x2y2 += (pow(xi[i],2)*pow(yi[i],2)) ;
	}

	a_top  = (x2y2*x2y2) - (x4*y4) ;
	a_down = (x2y2*y2)   - (x2*y4) ;

	if ((a_down == 0.0))
	{
		fprintf(msgout,"[E] %s: a =%f / %f!\n",
				("Computation of tensor scale failed"),
			 	a_top, a_down);
    *a = 0 ;
	}
  else
  {
    if ((a_top/a_down) > 0)
    {
	    *a = sqrt ( (a_top / a_down) ) ;
    }
    else
    {
      *a = 0 ;
    }
  }

	b_top  = (x2y2*x2y2) - (x4*y4) ;
	b_down = (x2*x2y2)   - (x4*y2) ;

	if ((b_down == 0.0))
	{
		fprintf(msgout,"[E] %s: b =%f / %f!\n",
				("Computation of tensor scale failed"),
			 	b_top, b_down);
    *b = 0 ;
	}
  else
  {
    if ((b_top/b_down) > 0)
    {
	    *b = sqrt ( (b_top / b_down) ) ;
    }
    else
    {
      *b = 0 ;
    }
  }

  return(0);
}
Ejemplo n.º 25
0
void MobileSimulator::MouseMotionEvent(Vec2i screenCoordinates)
{
	Vector2 pos = MathUtil::ScreenToWorld(screenCoordinates);
	_fingerGhost1->SetPosition(pos);
	_fingerGhost2->SetPosition(-pos);

	if (_mouseDown)
	{
		//move touch(es)
		TouchList *tl = &TouchListener::GetTouchList();
		if (tl->size() > 0)
		{
			(*tl)[0]->CurrentPoint = screenCoordinates;
			if ( (*tl)[0]->MotionStartTime < 0.0f )
			{
				(*tl)[0]->MotionStartTime = theWorld.GetCurrentTimeSeconds();
			}
		}
		if (tl->size() > 1)
		{
			Vector2 negCoordsVec = MathUtil::WorldToScreen(-pos);
			Vec2i negCoords((int)negCoordsVec.X, (int)negCoordsVec.Y);
			(*tl)[1]->CurrentPoint = negCoords;
			if ( (*tl)[1]->MotionStartTime < 0.0f )
			{
				(*tl)[1]->MotionStartTime = theWorld.GetCurrentTimeSeconds();
			}

			Touch* t1 = (*tl)[0];
			Touch* t2 = (*tl)[1];

			Vector2 start1(t1->StartingPoint);
			Vector2 current1(t1->CurrentPoint);
			Vector2 start2(t2->StartingPoint);
			Vector2 current2(t2->CurrentPoint);

			Vector2 initialVector = start2 - start1;
			Vector2 currentVector = current2 - current1;

			Vector2 initNorm = Vector2::Normalize(initialVector);
			Vector2 currentNorm = Vector2::Normalize(currentVector);
			float radiansRotated = acos(Vector2::Dot(initNorm, currentNorm));

			if (!_multiGestureOngoing)
			{					
				Vector2 motion = current1 - start1;

				if (motion.LengthSquared() >= (MULTI_MIN_DISTANCE * MULTI_MIN_DISTANCE) )
				{
					_multiGestureOngoing = true;
					
					// figure out if it's a rotate or a pinch
					if (radiansRotated > MULTI_ROTATE_ANGLE)
					{
						_gestureType = ROTATE;
					}
					else
					{
						_gestureType = PINCH;
					}
				}
			}

			if (_multiGestureOngoing)
			{
				GestureData gd;
				gd.Velocity = 0.0f; // don't want to store all the extra datums
									//  needed to actually calculate this

				if      (_gestureType == ROTATE)
				{
					float cross = Vector2::Cross(initNorm, currentNorm);
					if (cross > 0.0f)
					{
						radiansRotated = -radiansRotated;
					}
					gd.GestureMagnitude = radiansRotated;
					theSwitchboard.Broadcast(new TypedMessage<GestureData>("MultiTouchRotate", gd));
				}
				else if (_gestureType == PINCH)
				{
					gd.GestureMagnitude = currentVector.Length() / initialVector.Length();
					theSwitchboard.Broadcast(new TypedMessage<GestureData>("MultiTouchPinch", gd));	
				}
			}
		}
	}
}
Ejemplo n.º 26
0
double Puckering::CalculateTheta()
{
    _Theta = acos( sqrt(_nAtomsInv)*_qz / _Q );
    return _Theta;
}
Ejemplo n.º 27
0
Archivo: floats.c Proyecto: Athas/mosml
value acos_float(value f)             /* ML */
{
  return copy_double(acos(Double_val(f)));
}
Ejemplo n.º 28
0
static int math_acos (lua_State *L) {
  lua_pushnumber(L, acos(luaL_checknumber(L, 1)));
  return 1;
}
Ejemplo n.º 29
0
	void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){

		pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
		pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
		pcl::PCLPointCloud2 cloud_filtered;

		pcl_conversions::toPCL(*cloud_msg, *cloud);

		pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
		sor.setInputCloud(cloudPtr);

		float leaf = 0.005;
		sor.setLeafSize(leaf, leaf, leaf);
		sor.filter(cloud_filtered);

		sensor_msgs::PointCloud2 sensor_pcl;


		pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl);
		//publish pcl data 
		pub_voxel.publish(sensor_pcl);
		global_counter++;


		if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){

		// part for planar segmentation starts here  ..
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>); 
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);

			sensor_msgs::PointCloud2  plane_sensor, plane_transf_sensor;

			//convert sen
			pcl::fromROSMsg(*cloud_msg, *cloud1);
			pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
			pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

			pcl::SACSegmentation<pcl::PointXYZ> seg;

			seg.setOptimizeCoefficients(true);
			seg.setModelType (pcl::SACMODEL_PLANE);
	  		seg.setMethodType (pcl::SAC_RANSAC);
	  		seg.setMaxIterations (100);
			seg.setDistanceThreshold (0.01);

			seg.setInputCloud(cloud1);
			seg.segment (*inliers, *coefficients);

			Eigen::Matrix<float, 1, 3> surface_normal;
			Eigen::Matrix<float, 1, 3> floor_normal;
			surface_normal[0] = coefficients->values[0];
			surface_normal[1] = coefficients->values[1];
			surface_normal[2] = coefficients->values[2];
			std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2];

			floor_normal[0] = 0.0;
			floor_normal[1] = 1.0;
			floor_normal[2] = 0.0;

			theta = acos(surface_normal.dot(floor_normal));
			//extract the inliers - copied from tutorials...

			pcl::ExtractIndices<pcl::PointXYZ> extract;
			extract.setInputCloud(cloud1);
	    	extract.setIndices (inliers);
	    	extract.setNegative(true);
	    	extract.filter(*cloud_p);

	    	ROS_INFO("print cloud info %d",  cloud_p->height);
	    	pcl::toROSMsg(*cloud_p, plane_sensor);
	    	pub_plane_simple.publish(plane_sensor);

	    	// anti rotate the point cloud by first finding the angle of rotation 

	    	Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
	        transform_1.translation() << 0.0, 0.0, 0.0;
	        transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));

	        pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1);
			double y_sum = 0;
			// int num_points = 
			for (int i = 0; i < 20; i++){
				y_sum = cloud_p_rotated->points[i].y;
			}


			y_offset = y_sum / 20;

			Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	        transform_2.translation() << 0.0, -y_offset, 0.0;
	        transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));

			pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2);
	        pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2);

	        //now remove the y offset because of the camera rotation 

	        pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor);
	        // pcl::toROSMsg(*cloud_transformed, plane_transf_sensor);
	        // pcl::toROSMsg(*cloud1, plane_transf_sensor);
	        pub_plane_transf.publish(plane_transf_sensor);


		}


		ras_msgs::Cam_transform cft;

		cft.theta = theta;
		cft.y_offset = y_offset;	
		pub_ctf.publish(cft);	
		// pub_tf.publish();

	}
Ejemplo n.º 30
0
int evaluate_integral(double R, double beta, double gamma, double a_dm, double a_s, double M_dm, double M_s, double *sig_p, double *R_arcmin)
{
  
  int Nint = 10000;    // NUMBER OF INTERVALS
  double result, error, s, aux, X_s, I_R;
  
  gsl_integration_workspace *z = gsl_integration_workspace_alloc(Nint);
  gsl_function F1;
  
  struct param params;
  
  params.beta  =  beta;
  params.a_s   =  a_s;
  params.a_dm  =  a_dm;
  params.M_s   =  M_s;
  params.M_dm  =  M_dm;       
  params.R     =  R;

  //////////////////////////////////////////////////////////////
  //  THIS IS JUST TO PRINT THE INTEGRAND TO SEE ITS BEHAVIOR  //
  ///////////////////////////////////////////////////////////////
  /* 
    {
    FILE *pf=NULL;
    double r;
    pf=fopen("puto_archivo.dat","w");
    for(r=0; r<LIMIT_RADIUS; r=r+1)
    {
    fprintf(pf,"%16.8e %16.8e\n",r, integrando(r, &params));
    }
    fclose(pf);
    exit(0);
    } 
  */
  ///////////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////////
  
  F1.function = &integrando;
  F1.params = &params;
  
  //gsl_integration_qags(&F1, R, LIMIT_RADIUS, 1.0e-8, 1e-6, Nint, z, &result, &error); 
  gsl_integration_qag(&F1, R, LIMIT_RADIUS, 1.0e-5, 1e-5, Nint, 1, z, &result, &error); 
  //printf("R = %lf I=%lf\n",R, result);
  
  s = R/a_s;
  
  if (s < (1.0-ZERO))
    { 
      aux = 1.0 - s*s;
      X_s = (1.0 / sqrt(aux)) * log((1.0 + sqrt(aux))/s);
      I_R = (M_s/(2.0*M_PI*a_s*a_s*gamma*aux*aux))*( (2.0+s*s)*X_s - 3.0 );
    }
  
  if(s >= (1.0-ZERO) && s <= (1.0+ZERO) )
    {
      X_s = 1.0;
      I_R = (2.0*M_s)/(15.0*M_PI*a_s*a_s*gamma);
    }
  
  if (s > (1.0 + ZERO))
    {
      X_s = (1.0 / sqrt(s*s - 1)) * acos(1.0/s);
      I_R = (M_s/(2.0*M_PI*a_s*a_s*gamma*(1.0-s*s)*(1.0-s*s)))*((2.0+s*s)*X_s - 3.0);
    }      
  
  *sig_p = sqrt( (G*result) / (gamma*I_R*M_PI) ) ;
  
  //THIS LINE CONVERTS FROM PARSECS TO ARCMIN SINCE THE OBSERVATIONAL DATA IS IN THOSE UNITS
  *R_arcmin = R*3437.75/DISTANCE;
  
  gsl_integration_workspace_free(z);
  
  return 0;  
}