///=======================================================================================
float CPSFloatCurveFunctor::getValue(float date) const
{
if (_CtrlPoints.empty()) return 0.f;
NLMISC::clamp(date, 0, 1);
// find a key that has a higher value
CPSVector<CCtrlPoint>::V::const_iterator it = _CtrlPoints.begin();
while ( it != _CtrlPoints.end() && it->Date <= date ) ++it;
if (it == _CtrlPoints.begin()) return _CtrlPoints[0].Value;
if (it == _CtrlPoints.end()) return _CtrlPoints[_CtrlPoints.size() - 1].Value;
CPSVector<CCtrlPoint>::V::const_iterator precIt = it - 1;
if (precIt->Date == it->Date) return 0.5f * (precIt->Value + it->Value);
const float lambda = (date - precIt->Date) / (it->Date - precIt->Date);
if (!_Smoothing) // linear interpolation
{
return lambda * it->Value + (1.f - lambda) * precIt->Value;
}
else // hermite interpolation
{
float width = it->Date - precIt->Date;
uint index = (uint)(precIt - _CtrlPoints.begin());
float t1 = getSlope(index) * width, t2 = getSlope(index + 1) * width;
const float lambda2 = NLMISC::sqr(lambda);
const float lambda3 = lambda2 * lambda;
const float h1 = 2 * lambda3 - 3 * lambda2 + 1;
const float h2 = - 2 * lambda3 + 3 * lambda2;
const float h3 = lambda3 - 2 * lambda2 + lambda;
const float h4 = lambda3 - lambda2;
return h1 * precIt->Value + h2 * it->Value + h3 * t1 + h4 * t2;
}
}
int main (void){
double K1, K2;
double a1 , a2;
/****************************
K1(x1 - x0) + K2(y1-y0) = a
******************************/
/*L1_x_diff = "(x1 - x0)"*/
/*L1_y_diff = "(y1 - y0)"*/
double L1_x1 = 603.0;
double L1_y1 = -395.0;
double L1_x2 = 789.0;
double L1_y2 = -626.0;
double L2_x1 = 615.0;
double L2_y1 = -357.0;
double L2_x2 = 813.0;
double L2_y2 = -532.0;
double L3[] = {583.0, -383.0, 754.0, -607.0};
double L1_x_diff = L1_x1 - X_CONST; /* corresponds w/ a1*/
double L1_y_diff = L1_y1 - Y_CONST; /* corresponds w/ a1*/
/*L2_x_diff = "(x1 - x0)"*/
/*L2_y_diff = "(y1 - y0)"*/
double L2_x_diff = L2_x1 - X_CONST; /* corresponds w/ a2*/
double L2_y_diff = L2_y1 - Y_CONST; /* corresponds w/ a2*/
a1 = getSlope(L1_x1 , L1_y1, L1_x2, L1_y2);
a2 = getSlope(L2_x1 , L2_y1, L2_x2, L2_y2);
/* NOTE: -L2_x_diff * L1_x_diff) / L2_x_diff; /* redundant step */
K2 = ( (a1 * -L2_x_diff / L1_x_diff) + a2) / ( (L1_y_diff * -L2_x_diff / L1_x_diff) + L2_y_diff );
K1 = (a1 - (K2 * L1_y_diff)) / L1_x_diff;
printf("X_CONST is: %0.2f\n", X_CONST );
printf("Y_CONST is: %0.2f\n\n", Y_CONST );
printf("LINE 1 is: (%0.2f ,%0.2f ) , (%0.2f ,%0.2f) \n", L1_x1 , L1_y1, L1_x2, L1_y2 );
printf("\tLINE 1 has a slope of: %0.5f \n\n", a1 );
printf("LINE 2 is: (%0.2f ,%0.2f ) , (%0.2f ,%0.2f) \n", L2_x1 , L2_y1, L2_x2, L2_y2 );
printf("\tLINE 2 has a slope of: %0.5f \n\n", a2);
printf("K1 is: %0.7f\n", K1 );
printf("K2 is: %0.5f\n", K2 );
printf("\n\n*****************************\n\tCHECK\n*****************************\n", K2 );
printf("CHECK LINE 1 slope given K1 and K2: %0.5f \n", getSlopeWithK(K1, K2, L1_x1, L1_y1) );
printf("CHECK LINE 2 slope given K1 and K2: %0.5f \n", getSlopeWithK(K1, K2, L2_x1, L2_y1) );
printf("LINE 3 is: (%0.2f ,%0.2f ) , (%0.2f ,%0.2f) \n", L3[0] , L3[1], L3[2], L3[3] );
printf("CHECK NEW LINE 3 slope given two points: %0.5f \n", getSlope(L3[0], L3[1], L3[2], L3[3]) );
printf("CHECK NEW LINE 3 slope given K1 and K2: %0.5f \n", getSlopeWithK(K1, K2, L3[0], L3[1]) );
} /*end of Main*/
//orients based on the slope calculated by the centroid
void OrientationBehavior::orientByCentroid(){
int argc; char **argv;
ros::init(argc, argv, "orienting_using_centroids");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("centroid", 1, centroidCallback);
while(centroid_x == INVALID_VAL && centroid_y == INVALID_VAL)
ros::spinOnce();
float last_centroid_x = centroid_x;
float last_centroid_y = centroid_y;
NavigationBehavior::moveStraight(true); //move straight
ros::spinOnce();
float slope = getSlope(last_centroid_x, last_centroid_y, centroid_x, centroid_y);
float acceptMargin = 0.2;
NavigationBehavior::backUp(true); //move back from moving straight
while( (slope != ALIGNED_VERTICAL) &&
(slope < -acceptMargin || slope > acceptMargin)){
//move according to the slope
if(slope > 0) { //positive slope
turn(5.0);
}
else if(slope == 0.0){ //0 slope
turn(90.0);
}
else { //negative slope
turn(5.0);
}
//update current centroid to last
last_centroid_x = centroid_x;
last_centroid_y = centroid_y;
//get new values
ros::spinOnce();
//get new slope
slope = getSlope(last_centroid_x, last_centroid_y, centroid_x, centroid_y);
}
motor_utility::stop_wheels();
}
bool
Person::handleVariable(const std::string& objID, const int variable, VariableWrapper* wrapper) {
switch (variable) {
case TRACI_ID_LIST:
return wrapper->wrapStringList(objID, variable, getIDList());
case ID_COUNT:
return wrapper->wrapInt(objID, variable, getIDCount());
case VAR_POSITION:
return wrapper->wrapPosition(objID, variable, getPosition(objID));
case VAR_POSITION3D:
return wrapper->wrapPosition(objID, variable, getPosition(objID, true));
case VAR_ANGLE:
return wrapper->wrapDouble(objID, variable, getAngle(objID));
case VAR_SLOPE:
return wrapper->wrapDouble(objID, variable, getSlope(objID));
case VAR_SPEED:
return wrapper->wrapDouble(objID, variable, getSpeed(objID));
case VAR_ROAD_ID:
return wrapper->wrapString(objID, variable, getRoadID(objID));
case VAR_LANEPOSITION:
return wrapper->wrapDouble(objID, variable, getLanePosition(objID));
case VAR_COLOR:
return wrapper->wrapColor(objID, variable, getColor(objID));
case VAR_WAITING_TIME:
return wrapper->wrapDouble(objID, variable, getWaitingTime(objID));
case VAR_TYPE:
return wrapper->wrapString(objID, variable, getTypeID(objID));
case VAR_NEXT_EDGE:
return wrapper->wrapString(objID, variable, getNextEdge(objID));
case VAR_STAGES_REMAINING:
return wrapper->wrapInt(objID, variable, getRemainingStages(objID));
case VAR_VEHICLE:
return wrapper->wrapString(objID, variable, getVehicle(objID));
default:
return false;
}
}
int maxPoints(vector<Point> &points) {
if (points.size() > 0) {
int maxNum = 0;
unordered_map<double, int> slope;
for (int i = 0; i < points.size(); ++i) {
slope.clear();
int samePoint = 1;
for (int j = i + 1; j < points.size(); ++j) {
if (isSame(points[i], points[j])) {
samePoint++;
} else {
double s = getSlope(points[i], points[j]);
slope[s] += 1;
}
}
maxNum = max(maxNum, samePoint);
for (unordered_map<double, int>::iterator iter = slope.begin(); iter != slope.end(); ++iter) {
maxNum = max(maxNum, samePoint + iter->second);
}
}
return maxNum;
}
return 0;
}
void data::runPermutationExtended(string fout, vector < int > nPermutations) {
//0. Prepare genotypes
vector < double > genotype_sd = vector < double > (genotype_count, 0.0);
vector < double > phenotype_sd = vector < double > (phenotype_count, 0.0);
if (covariate_count > 0) {
LOG.println("\nCorrecting genotypes for covariates");
covariate_engine->residualize(genotype_orig);
}
for (int g = 0 ; g < genotype_count ; g ++) genotype_sd[g] = RunningStat(genotype_orig[g]).StandardDeviation();
normalize(genotype_orig);
//1. Loop over phenotypes
ofile fdo (fout);
for (int p = 0 ; p < phenotype_count ; p ++) {
LOG.println("\nProcessing gene [" + phenotype_id[p] + "]");
//1.1. Enumerate all genotype-phenotype pairs within cis-window
vector < int > targetGenotypes, targetDistances;
for (int g = 0 ; g < genotype_count ; g ++) {
int cisdistance;
int startdistance = genotype_pos[g] - phenotype_start[p];
int enddistance = genotype_end[g] - phenotype_start[p];
// for INVs ignore the span and define the cisdistance
// as the distance from the breakpoints to the phenotype_start
if (genotype_vartype[g].compare("INV") == 0) {
if (abs(startdistance) <= abs(enddistance))
cisdistance = startdistance;
else
cisdistance = enddistance;
}
// for the variants with span (DEL, DUP, MEI), cisdistance is zero
// if the phenotype_start falls within the span, and the distance to
// the closest edge otherwise
// BNDs get processed here as well, but their END coordinate is the
// same as the START coordinate.
else {
if (startdistance < 0 && enddistance > 0) { // if gene is within SV, then cis distance is 0
cisdistance = 0;
}
else if (startdistance >= 0)
cisdistance = startdistance;
else
cisdistance = enddistance;
}
if (abs(cisdistance) <= cis_window) {
targetGenotypes.push_back(g);
targetDistances.push_back(cisdistance);
}
}
LOG.println(" * Number of variants in cis = " + sutils::int2str(targetGenotypes.size()));
//1.2. Copy original data
vector < float > phenotype_curr = phenotype_orig[p];
if (covariate_count > 0) covariate_engine->residualize(phenotype_curr);
phenotype_sd[p] = RunningStat(phenotype_curr).StandardDeviation();
normalize(phenotype_curr);
//1.3. Nominal pass: scan cis-window & compute statistics
double bestCorr = 0.0;
vector < double > targetCorrelations;
int bestDistance = ___LI___, bestIndex = -1;
for (int g = 0 ; g < targetGenotypes.size() ; g ++) {
double corr = getCorrelation(genotype_orig[targetGenotypes[g]], phenotype_curr);
targetCorrelations.push_back(corr);
if (abs(targetCorrelations[g]) > abs(bestCorr) || (abs(targetCorrelations[g]) == abs(bestCorr) && abs(targetDistances[g]) < bestDistance)) {
bestCorr = targetCorrelations[g];
bestDistance = targetDistances[g];
bestIndex = targetGenotypes[g];
}
}
if (targetGenotypes.size() > 0) LOG.println(" * Best correlation = " + sutils::double2str(bestCorr, 4));
//1.4. Permutation pass:
bool done = false;
int countPermutations = 0, nBetterCorrelation = 0;
vector < double > permCorr;
do {
double bestCperm = 0.0;
phenotype_curr = phenotype_orig[p];
random_shuffle(phenotype_curr.begin(), phenotype_curr.end());
if (covariate_count > 0) covariate_engine->residualize(phenotype_curr);
normalize(phenotype_curr);
for (int g = 0 ; g < targetGenotypes.size() ; g ++) {
double corr = getCorrelation(genotype_orig[targetGenotypes[g]], phenotype_curr);
if (abs(corr) > abs(bestCperm)) bestCperm = corr;
}
if (abs(bestCperm) >= abs(bestCorr)) nBetterCorrelation++;
permCorr.push_back(bestCperm);
countPermutations++;
if (nPermutations.size() == 1 && countPermutations >= nPermutations[0]) done = true;
if (nPermutations.size() == 2 && (nBetterCorrelation >= nPermutations[0] || countPermutations >= nPermutations[1])) done = true;
if (nPermutations.size() == 3 && (countPermutations >= nPermutations[0]) && (nBetterCorrelation >= nPermutations[1] || countPermutations >= nPermutations[2])) done = true;
} while (!done);
if (targetGenotypes.size() > 0) LOG.println(" * Number of permutations = " + sutils::int2str(nBetterCorrelation) + " / " + sutils::int2str(countPermutations));
//1.5. Calculate effective DFs & Beta distribution parameters
vector < double > permPvalues;
double true_df = sample_count - 2 - ((covariate_count>0)?covariate_engine->nCovariates():0);
double mean = 0.0, variance = 0.0, beta_shape1 = 1.0, beta_shape2 = 1.0;
if (targetGenotypes.size() > 0) {
//Estimate number of degrees of freedom
if (putils::variance(permCorr, putils::mean(permCorr)) != 0.0) {
learnDF(permCorr, true_df);
//LOG.println(" * Effective degree of freedom = " + sutils::double2str(true_df, 4));
}
//Compute mean and variance of p-values
for (int c = 0 ; c < permCorr.size() ; c ++) permPvalues.push_back(getPvalue(permCorr[c], true_df));
for (int pv = 0 ; pv < permPvalues.size() ; pv++) mean += permPvalues[pv];
mean /= permPvalues.size();
for (int pv = 0 ; pv < permPvalues.size() ; pv++) variance += (permPvalues[pv] - mean) * (permPvalues[pv] - mean);
variance /= (permPvalues.size() - 1);
//Estimate shape1 & shape2
if (targetGenotypes.size() > 1 && mean != 1.0) {
beta_shape1 = mean * (mean * (1 - mean ) / variance - 1);
beta_shape2 = beta_shape1 * (1 / mean - 1);
if (targetGenotypes.size() > 10) mleBeta(permPvalues, beta_shape1, beta_shape2); //ML estimate if more than 10 variant in cis
}
LOG.println(" * Beta distribution parameters = " + sutils::double2str(beta_shape1, 4) + " " + sutils::double2str(beta_shape2, 4));
}
//1.6. Writing results
if (targetGenotypes.size() > 0 && bestIndex >= 0) {
for (int g = 0 ; g < targetGenotypes.size() ; g ++) {
fdo << phenotype_id[p] << " " << targetGenotypes.size();
fdo << " " << beta_shape1 << " " << beta_shape2 << " " << true_df;
double pval_fdo = getPvalue(targetCorrelations[g], true_df);
double pval_nom = getPvalue(targetCorrelations[g], sample_count - 2 - ((covariate_count>0)?covariate_engine->nCovariates():0));
double pval_slope = getSlope(targetCorrelations[g], phenotype_sd[p], genotype_sd[bestIndex]);
fdo << " " << genotype_id[targetGenotypes[g]];
fdo << " " << targetDistances[g];
fdo << " " << pval_nom;
fdo << " " << pval_slope;
fdo << " " << (nBetterCorrelation + 1) * 1.0 / (countPermutations + 1.0);
fdo << " " << pbeta(pval_fdo, beta_shape1, beta_shape2, 1, 0);
fdo << endl;
}
}
else fdo << phenotype_id[p] << " NA NA NA NA NA NA NA NA NA" << endl;
LOG.println(" * Progress = " + sutils::double2str((p+1) * 100.0 / phenotype_count, 1) + "%");
}
fdo.close();
}
double Utility::getSlope(const VisualLine& line) {
return getSlope(line.start.x,line.start.y,line.end.x,line.end.y);
}
void loop() {
test = getSlope(dhistory);
}
int main(int argc, char *argv[]) {
/*
printf("Argument count: %d\n", argc);
for (int i = 0; i < argc; i++) {
printf("Argument vector values:%s at %p memory\n", argv[i], argv[i]);
for (char *j=argv[i]; *j!='\0'; j++) {
printf("Another way to print argument vector values: "
"%c at %p memory\n", *j, j);
}
}
*/
struct Line line1;
double m,c;
m = -1.00;
c = 6.00;
setLine(&line1,m,c);
printf("Line1 has slope: %f and y-intercept: %f\n",line1.slope,line1.yintercept);
struct Line line2;
m = 1.00;
c = 6.00;
setLine(&line2,m,c);
printf("Line2 has slope: %f and y-intercept: %f\n",line2.slope,line2.yintercept);
struct Point p1;
double xval,yval;
xval = 2;
yval = 1;
setPoint(&p1,xval,yval);
printf("Point1 has x: %f and y: %f\n",p1.x,p1.y);
struct Point p2;
xval = 2;
yval = 1;
setPoint(&p2,xval,yval);
printf("Point2 has x: %f and y: %f\n",p2.x,p2.y);
m = getSlope(p1,p2);
printf("Slope of Line joining p1 and p2: %f\n",m);
p2 = getIntersectPoint(line1,line2);
printf("Intersection Point of Line1 and Line2 has x: %f and y: %f\n",p2.x,p2.y);
struct Square sq1;
double xl,xr,yt,yb;
xl = 2.00;
xr = 4.00;
yt = 4.00;
yb = 2.00;
setSquare(&sq1, xl, xr, yt, yb);
printf("Sqaure1 has xleft: %f, xright: %f and ytop: %f, ybottom: %f\n",sq1.xleft,sq1.xright,sq1.ytop,sq1.ybottom);
struct Point sqCentre1;
sqCentre1 = getSquareCentre(sq1);
printf("Centre Point of Square1 has x: %f and y: %f\n",sqCentre1.x,sqCentre1.y);
struct Square sq2;
xl = 4.00;
xr = 6.00;
yt = 6.00;
yb = 4.00;
setSquare(&sq2, xl, xr, yt, yb);
printf("Sqaure2 has xleft: %f, xright: %f and ytop: %f, ybottom: %f\n",sq2.xleft,sq2.xright,sq2.ytop,sq2.ybottom);
struct Point sqCentre2;
sqCentre2 = getSquareCentre(sq2);
printf("Centre Point of Square2 has x: %f and y: %f\n",sqCentre2.x,sqCentre2.y);
m = getSlope(sqCentre1,sqCentre2);
printf("Slope of Line joining Square 1 Centre and Square 2 Centre: %f\n",m);
return 0;
}