/*
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;
}
/**
* 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;
}
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));
}
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);
}
}
}
# 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;
double Transformation::angle(QVector3D a, QVector3D b)
{
return acos(dotProduct(a, b) / (a.length()*b.length()));
}
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)) ;
}
float Vector3::angle(const Vector3& v) const
{
float angle = dot(v)/(magnitude()*v.magnitude());
return acos(angle);
}
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;
}
#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;
inline double _acos(double arg) { return acos(arg); }
float Vector4::angle (Vector4 const& other) const {
return acos( operator *(other) / (magnitude() * other.magnitude()) );
}
double r_acos(real *x)
#endif
{
return( acos(*x) );
}
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);
}
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;
}
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;
}
}
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);
}
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.");
}
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;
}
//------------------------------------------------------------------------------
// 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);
}
#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)
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;
}
}
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);
}
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));
}
}
}
}
}
double Puckering::CalculateTheta()
{
_Theta = acos( sqrt(_nAtomsInv)*_qz / _Q );
return _Theta;
}
value acos_float(value f) /* ML */
{
return copy_double(acos(Double_val(f)));
}
static int math_acos (lua_State *L) {
lua_pushnumber(L, acos(luaL_checknumber(L, 1)));
return 1;
}
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();
}
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, ¶ms));
}
fclose(pf);
exit(0);
}
*/
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
F1.function = &integrando;
F1.params = ¶ms;
//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;
}