PROTECTED void NunchuckFilterJoystickSetCalibration( pNunchuckCtlCalibration Calibration )
{
pNunchuckCtlJsAxisCal jsCal;
jsCal = &Calibration->Joystick.X;
normalizationCal.X.neutral = jsCal->Neutral;
normalizationCal.X.yIntMin = calculateYIntercept((int32)jsCal->Min, (int32)jsCal->Neutral, -127L);
normalizationCal.X.yIntMax = calculateYIntercept((int32)jsCal->Max, (int32)jsCal->Neutral, 127L);
normalizationCal.X.mMin = calculateSlope((int32)jsCal->Min, (int32)jsCal->Neutral, -127L);
normalizationCal.X.mMax = calculateSlope((int32)jsCal->Max, (int32)jsCal->Neutral, 127L);
jsCal = &Calibration->Joystick.Y;
normalizationCal.Y.neutral = jsCal->Neutral;
normalizationCal.Y.yIntMin = calculateYIntercept((int32)jsCal->Min, (int32)jsCal->Neutral, 127L);
normalizationCal.Y.yIntMax = calculateYIntercept((int32)jsCal->Max, (int32)jsCal->Neutral, -127L);
normalizationCal.Y.mMin = calculateSlope((int32)jsCal->Min, (int32)jsCal->Neutral, 127L);
normalizationCal.Y.mMax = calculateSlope((int32)jsCal->Max, (int32)jsCal->Neutral, -127L);
LOG_Printf("Joystick Normalized Calibration:\n");
LOG_Printf("\tX neutral: %d\n", normalizationCal.X.neutral);
LOG_Printf("\tX yIntMin: %d\n", normalizationCal.X.yIntMin);
LOG_Printf("\tX yIntMax: %d\n", normalizationCal.X.yIntMax);
LOG_Printf("\tX mMin: %d\n", normalizationCal.X.mMin);
LOG_Printf("\tX mMax: %d\n", normalizationCal.X.mMax);
LOG_Printf("\tY neutral: %d\n", normalizationCal.Y.neutral);
LOG_Printf("\tY yIntMin: %d\n", normalizationCal.Y.yIntMin);
LOG_Printf("\tY yIntMax: %d\n", normalizationCal.Y.yIntMax);
LOG_Printf("\tY mMin: %d\n", normalizationCal.Y.mMin);
LOG_Printf("\tY mMa: %d\n", normalizationCal.Y.mMax);
}
// initialize the group class to have sufficient space such that the largest nodeNumber can still be stored
FAGeneral::FAGeneral(double* x,int K, double* ngroup, NumericVector xv, std::string weights, double gamma, NumericMatrix W, int mxSize, bool verb, double eps):K(K),Weights(K,vector<double>(K)),graphsize(K){
verbose = verb;
mxSplitSize = mxSize;
epsilon = eps;
// calculate the weights we need
calculateSlope(x,ngroup,xv,weights,gamma,W);
calculateWeights(x,ngroup,xv,weights,gamma,W);
// print_weights();
if (verbose){
Rprintf("Graph Construction \n");
}
myGraph.Construct(K,Weights,epsilon);
// myGraph.printGraph(cout);
// set groups, prefuse and set events
initializeGroups(&x[0],&ngroup[0]);
initializeEvents();
// print_groups();
// print_events();
run();
}
float HolisticFeatureExtractor::computeOrientation(const LTKTrace& inTrace)
{
int numOfPoints; // total number of points in the trace
float orient = 0.0f; // orientation
floatVector xData, yData; // coordinates of the points in the trace
xData = inTrace.getChannelValues("X");
yData = inTrace.getChannelValues("Y");
numOfPoints = xData.size();
orient = calculateSlope(xData.at(0),yData.at(0),xData.at(numOfPoints-1),yData.at(numOfPoints-1));
// convert all angles to a range 0 to pi
if (orient<0)
{
orient += PI;
}
// normalizing it from 0 to 1
orient /= PI;
// make horizontal stroke as 180 degrees instead of 2 values
if (orient<(m_horizStrokeAngleThresh/180))
{
orient = 1 - orient;
}
return orient;
}
int cTrendLine::Create(flarb_mapbuilder::MapImage &msg, int x, int y)
{
yAxisValuesSum = 0;
xAxisValuesSum = 0;
//arrray Max points from laser
int yAxisValues[1081];
int xAxisValues[1081];
int ptr = 0;
//from here till line 52 must be rebuild to levels
//now he tries the whole level(Image)
// Iterator
vector<uint8_t>::const_iterator itr = msg.data.begin();
// Write the image to the buffer
for( int y = 0; y < 512; y++)
{
for( int x = 0; x < (512 / 8); x++)
{
//Get value and advance the iterator
int val = *(itr);
itr++;
//note all the x and y locations and count them
for(int i = 0; i < 7; i++){
if(1 == (val & ( 1 << i )))
{
yAxisValues[ptr] = y;
yAxisValuesSum += y;
xAxisValues[ptr] = x * 8;
xAxisValuesSum += x;
ptr++;
}
}
}
}
//unnecessary pointer
count = ptr;
//sum of x and y
xxSum = 0;
xySum = 0;
for (int i = 0; i < count; i++)
{
xySum += ( xAxisValues[i] * yAxisValues[i] );
xxSum += ( xAxisValues[i] * xAxisValues[i] );
}
Slope = calculateSlope();
Intercept = calculateIntercept();
Start = calculateStart();
End = calculateEnd();
return 1;
}
float HolisticFeatureExtractor::computeSweptAngle(const LTKTrace& inTrace)
{
float swAng=0.0f; // Swept Angle
int numOfPoints; // total number of points in the trace
int pointIndex; // Index to loop over all the points
floatVector angles; // angles made by each line with x-axis
floatVector angleDiff; // difference of angles between consecutive lines
floatVector xData, yData; // coordinates of the points in the trace
xData = inTrace.getChannelValues("X");
yData = inTrace.getChannelValues("Y");
numOfPoints = xData.size();
for(pointIndex = 0; pointIndex < numOfPoints-1; ++pointIndex)
{
float angle;
angle = calculateSlope(xData.at(pointIndex),yData.at(pointIndex),xData.at(pointIndex+1),yData.at(pointIndex+1));
// converting the angles from 0 to 360 degrees
if (angle < 0)
{
angle += 2*PI;
}
angles.push_back(angle);
}
for(pointIndex = 0; pointIndex < numOfPoints-2; ++pointIndex)
{
int sign_angle;
angleDiff.push_back(angles.at(pointIndex)-angles.at(pointIndex+1));
sign_angle = (angleDiff.at(pointIndex) < 0) ? -1:1;
// if greater than 180 then correcting it to a value less than 180
if (fabs(angleDiff.at(pointIndex)) > PI)
{
angleDiff.at(pointIndex) = -sign_angle * (2*PI - fabs(angleDiff.at(pointIndex)));
}
// if greater than swAngHookThresh then assuming it to be a hook
// and disregarding the angle
if (fabs(angleDiff.at(pointIndex)) > (m_swAngHookThresh*PI/180))
{
angleDiff.at(pointIndex) = 0;
}
swAng += angleDiff.at(pointIndex);
}
// returning the absolute value of swept angle
swAng = fabs(swAng);
return swAng;
}
