void calcDesordem(DNA *arr, int size, int n)
{
for(int i=0; i < size;i++)
{
calcD(arr[i],n);
}
}
Triangle::Triangle(ngl::Vec3 _v1, ngl::Vec3 _v2, ngl::Vec3 _v3,
ngl::Vec3 _n1, ngl::Vec3 _n2, ngl::Vec3 _n3):
m_v1(_v1), m_v2(_v2), m_v3(_v3),
m_n1(_n1), m_n2(_n2), m_n3(_n3)
{
calcN();
calcD();
}
void calcE() {
int k;
k=0;
// cycles through all integers less than phi..
for(i=2;i<phi;i++) {
if(phi%i==0)
continue;
// if phi is not divisible by 'i', checks if prime.
flag=prime(i);
if(flag==1&&i!=p&&i!=q) {
// if prime, assigns to a value of 'e'.
e[k]=i;
flag=calcD(e[k]);
if(flag>0) {
d[k]=flag;
k++;
}
if(k==99)
break;
}
}
}
void Clothoid::getPath(State a, State b) {
cout<<"Start :" <<a.x<<" "<<a.y <<" "<<a.theta<<endl;
cout<<"End"<<b.x<<" "<<b.y<<" "<<b.theta<<endl;
if (fabs(a.x - b.x) < 1 || fabs(a.y - b.y) < 1) {
solution = 0;
return;
}
double beta = atan2((b.y - a.y), (b.x - a.x));
if (fabs(beta - b.theta - a.theta + beta) < 0.001 && fabs(b.theta - a.theta) < PI) {
cout << "Symmetric:\n" << endl;
start = a;
end = b;
double alpha = inRange((-start.theta + end.theta)) / 2;
double D = calcD(fabs(alpha));
path.sigma = 4 * PI * signum(alpha) * D * D / start.getDistance(end)/start.getDistance(end);
if (path.sigma == 0) {
path.lengthOfPath = start.getDistance(end);
for (double s = 0; s < path.lengthOfPath; s += path.lengthOfPath / 1000) {
path.path.push_back(State(start.x + s * cos(start.theta), start.y + s * sin(start.theta), 0));
}
paths.push_back(ClothoidPathSegment(path.path, path.lengthOfPath, path.sigma));
return;
} else {
path.lengthOfPath = 2 * sqrt(fabs(2 * alpha / path.sigma));
getTrajectory();
paths.push_back(ClothoidPathSegment(path.path, path.lengthOfPath, path.sigma));
std::cout << std::endl << std::endl << std::endl;
}
}
else if (a.theta == b.theta) {
State p((a.x + b.x) / 2, (a.y + b.y) / 2, 0);
double beta = atan2((p.y - a.y), (p.x - a.x));
p.theta = 2 * beta - a.theta;
getPath(a, p);
getPath(p, b);
} else {
double alpha = inRange(((-a.theta + b.theta)) / 2);
double cc;
cc = cos(alpha) / sin(alpha);
cout << "c is" << cc << endl;
State p(0, 0, 0);
p.x = (a.x + b.x + cc * (a.y - b.y)) / 2;
p.y = (a.y + b.y + cc * (b.x - a.x)) / 2;
double r = p.getDistance(a);
double deflection1 = (atan2((p.y - a.y), (p.x - a.x)));
double deflection2 = (atan2((p.y - b.y), (p.x - b.x)));
double def;
State c(0, 0, 0);
if (deflection2 > deflection1) {
//swap
double temp = deflection2;
deflection2 = deflection1;
deflection1 = temp;
c.x = a.x;
c.y = a.y;
c.theta = a.theta;
a.x = b.x;
a.y = b.y;
a.theta = b.theta;
b.x = c.x;
b.y = c.y;
b.theta = c.theta;
}
def = ((deflection2 + deflection1)) / 2;
alpha = (((-a.theta + b.theta)) / 2);
if (alpha < 0) {
def = inRange(PI + def);
}
State q(0, 0, 0);
q.x = p.x + r * cos(def);
q.y = p.y + r * sin(def);
double the = atan2((a.y - q.y), (a.x - q.x));
double the2 = atan2((q.y - b.y), (q.x - b.x));
double beta1 = inRange(2 * the - a.theta);
double beta2 = inRange(2 * the2 - b.theta);
q.theta = beta1;
cout << "Intermediate Point " << q.x << " " << q.y << " " << q.theta << endl;
getPath(a, q);
getPath(q, b);
}
}
