#include "2d.h"
/*******************************
all angles are in rads less polar, which is in degs
*******************************/
double dist2d(point2d_t *this, point2d_t *that) {
return sqrt(SQR((this->x)-(that->x)) + SQR((this->y)-(that->y)) );
}
double inscribed (point2d_t this, point2d_t that, point2d_t other){
point2d_t t1, t2, tc;
point2d_t c1, c2;
double a;
setPoint (&t1, this);
setPoint (&t2, other);
setPoint (&tc, that);
tc.x = -tc.x;
tc.y = -tc.y;
translate (&t1, tc);
translate (&t2, tc);
// x <-> radius y <-> theta
cart2polar(&c1, t1);
cart2polar(&c2, t2);
a = c2.y - c1.y;
if (a > 180.0) {
a = 360-a;
}
return a;
}
point2d_t * setPoint(point2d_t *p, point2d_t to) {
p->x = to.x;
p->y = to.y;
return p;
}
point2d_t * translate(point2d_t *p, point2d_t by) {
p->x += by.x;
p->y += by.y;
return p;
}
// oops, fixed xy error Aug 11/2015
point2d_t * rotatez(point2d_t *p, double by) {
double xtemp; //, ytemp;
double cosTheta, sinTheta;
cosTheta = cos(by);
sinTheta = sin(by);
xtemp = (p->x * cosTheta) - (p->y * sinTheta);
/*ytemp =*/ p->y = (p->x * sinTheta) + (p->y * cosTheta);
p->x = xtemp;
/* p->y = ytemp;*/
return p;
}
point2d_t * rotateZAround(point2d_t *p, point2d_t *around, double by) {
double xtemp; //, ytemp;
double cosTheta, sinTheta;
cosTheta = cos(by);
sinTheta = sin(by);
xtemp = around->x + (((p->x) - (around->x)) * cosTheta) - (((p->y) - (around->y)) * sinTheta);
p->y = around->y + (((p->x) - (around->x)) * sinTheta) + (((p->y) - (around->y)) * cosTheta);
p->x = xtemp;
return p;
}
// scale p by by around 0
point2d_t * scale(point2d_t *p, point2d_t by) {
p->x *= by.x;
p->y *= by.y;
return p;
}
// point to string.
char * point2s(char *s, int bl, point2d_t * this){
snprintf(s, bl, "%0.3f, %0.3f", this->x, this->y);
return s;
}
//
// x <-> radius y <-> theta
//
point2d_t * cart2polar(point2d_t *p, point2d_t cart){
(*p).x = sqrt(SQR(cart.x) + SQR(cart.y));
(*p).y = rad2deg(atan2(cart.y, cart.x));
return p;
}
//
// x <-> radius y <-> theta
//
point2d_t * polar2cart(point2d_t *p, point2d_t polar){
(*p).y = polar.x * sin(deg2rad(polar.y));
(*p).x = polar.x * cos(deg2rad(polar.y));
return p;
}
line_t *initLine(line_t *l) {
l->p1.x = 0;
l->p1.y = 0;
l->p2.x = 1;
l->p2.y = 0;
return l;
}
line_t *setLine(line_t *l, double x1, double y1, double x2, double y2) {
l->p1.x = x1;
l->p1.y = y1;
l->p2.x = x2;
l->p2.y = y2;
return l;
}
line_t * setLinePts(line_t *l, point2d_t *this, point2d_t *that){
setPoint(&(l->p1), *this);
setPoint(&(l->p2), *that);
return l;
}
line_t *transLine(line_t *l, point2d_t by) {
translate(&l->p1, by);
translate(&l->p2, by);
return l;
}
line_t *rotLine(line_t *l, double by) {
rotatez(&l->p1, by);
rotatez(&l->p2, by);
return l;
}
line_t * rotLineAround(line_t *l, point2d_t * around, double by) {
rotateZAround(&l->p1, around, by) ;
rotateZAround(&l->p2, around, by) ;
return l;
}
line_t *scaleLine(line_t *l, point2d_t by) {
scale(&l->p1, by);
scale(&l->p2, by);
return l;
}
char * line2s(char *s, int bl, line_t * this){
char t1[255];
char t2[255];
snprintf(s, bl, "%s -> %s", point2s( t1, 254 , &(this->p1)), point2s( t2, 254, &(this->p2)));
return s;
}
point2d_t * getMidPoint(point2d_t *mp, line_t *l){
mp->x = ((l->p1.x)+(l->p2.x))/2;
mp->y = ((l->p1.y)+(l->p2.y))/2;
return mp;
}
/*
A*x + B*y = C
*/
lineGF_t * line2General(lineGF_t *g1, line_t *this){
// convert one line
if (0) {
// are points a vert line?
} else if (this->p1.x == this->p2.x) {
// X = k
// printf("vl\n");
g1->A = 1;
g1->B = 0;
g1->C = this->p1.x;
// are points a horiz line?
} else if (this->p1.y == this->p2.y) {
// Y = k
// printf("hl\n");
g1->A = 0;
g1->B = 1;
g1->C = this->p1.y;
// regular line
} else {
// printf("sl\n");
g1->B = (this->p2.x - this->p1.x); // xb - xa
g1->A = (this->p1.y - this->p2.y); // yb - ya
g1->C = -((this->p2.y) * ((this->p1.x) - (this->p2.x)) - (this->p2.x) * ((this->p1.y) - (this->p2.y)));
}
return g1;
}
/*
General form of line in A*x + B*y - C = 0
returns NULL if the lines are parallel
*/
point2d_t * getIntersection(point2d_t * is, line_t * this, line_t * that) {
lineGF_t g1;
lineGF_t g2;
char s[255];
// find out of lines are parallel
if ((this->p1.x - this->p2.x)*(that->p1.y - that->p2.y) - (this->p1.y - this->p2.y)*(that->p1.x - that->p2.x) == 0) {
return NULL;
}
// to general form
line2General(&g1, this); // printf("A = %f, B = %f, C = %f\n", g1.A, g1.B, g1.C );
line2General(&g2, that); // printf("A = %f, B = %f, C = %f\n", g2.A, g2.B, g2.C );
// automagic intersection formula,
is->y = ((g2.A * g1.C) - (g2.C * g1.A)) / ((g2.A * g1.B) - ( g2.B * g1.A ));
if (g1.A != 0.0) {
is->x = (g1.C - g1.B*is->y)/g1.A;
} else {
is->x = (g2.C - g2.B*is->y)/g2.A;
}
return is;
}
/*
find the arc that hits the 3 given points
*/
arc_t * getArcOn( arc_t * a, point2d_t * this, point2d_t * that, point2d_t * other) {
line_t s1, s2;
point2d_t m1, m2;
char s[255];
// create lines between the two points
setLinePts(&s1, this, that);
setLinePts(&s2, that, other);
// rotate them 90 degrees around their midpoint
rotLineAround(&s1, getMidPoint(&m1, &s1), deg2rad(90.0));
rotLineAround(&s2, getMidPoint(&m2, &s2), deg2rad(90.0));
// find the intersection -> arc centre
getIntersection(&(a->c), &s1, &s2);
// find the radius
a->r = dist2d(&(a->c), that);
// start angle 0, end angle 0
a->start = 0;
a->end = 0;
return a;
}
// tell me about it.
char * arc2s (char *s, int bl, arc_t * this) {
char t[255];
snprintf(s, bl, "c -> %s, r -> %f, start angle: %f, stop angle: %f", point2s( t, 254 , &(this->c)), this->r, this->start, this->end);
return s;
}
// main
int main(void) {
point2d_t p, p2, p3;
line_t l, l2;
char s[255];
lineGF_t g;
arc_t a;
point2d_t A, B, C;
double ARB, ARC, BRC;
double ABC, BCA, CAB;
double ACR, ABR;
double AC, CB, BA;
double M, D;
setLine(&l, 2, 5, 8, 3);
getMidPoint(&p, &l);
printf("Line is : %s\n", line2s( s, 254, &l));
printf("midpoint is: %s\n", point2s( s, 254, &p));
printf("----- 1 -----\n");
p.x = 4; p.y = 4;
printf("before rotation point is: %s\n", point2s( s, 254, &p));
p2.x = 2; p2.y = 2;
rotateZAround(&p, &p2, deg2rad(90.0));
printf("after rotation point is: %s\n", point2s( s, 254, &p));
printf("----- 2 -----\n");
setLine(&l, 2, 2, 4, 4);
p.x = 2; p.y = 2;
rotLineAround(&l, &p, deg2rad(90.0));
printf("Line is : %s\n", line2s( s, 254, &l));
printf("----- 3 -----\n");
setLine(&l, -2, 4, 3, -2);
line2General(&g, &l);
printf("(-2,4)-(3,-2) => A = %f, B = %f, C = %f\n", g.A, g.B, g.C );
printf("----- 4 -----\n");
setLine(&l, -6, 2, 8, 2);
setLine(&l2, 16, 24, 8, 2);
printf("Line is : %s\n", line2s( s, 254, &l));
printf("Line is : %s\n", line2s( s, 254, &l2));
printf("-> %p\n", getIntersection(&p, &l, &l2));
printf("Intersection: %s \n", point2s( s, 254, &p));
printf("----- 5 -----\n");
p.x = 0; p.y = 1;
p2.x = 0; p2.y = 0;
p3.x = 5; p3.y = 0;
getArcOn( &a, &p, &p2, &p3);
printf("Arc is : %s\n", arc2s( s, 254, &a));
printf("----- 6 -----\n");
p.x = -7; p.y = -8;
p2.x = 1; p2.y = 0;
p3.x = -7; p3.y = 8;
printf("angle is : %f\n", inscribed (p, p2, p3));
printf("----- 7 -----\n");
/*
point2d_t A, B, C;
double AC, CB, BA;
double ARB, ARC, BRC;
double ABC, BCA, CAB;
double ACR;
double M, D;
*/
// givens
// these are the beacon locations
A.x = 483.99; A.y = -65.57;
B.x = -8.34; B.y = 27.42;
C.x = 261.07; C.y = 287.29;
//ARC = deg2rad(128.124);
//BRC = deg2rad(105.835);
//ARB = deg2rad(126.04);
// these are the angles around the robot to the beacons
ARC = deg2rad(92.368);
BRC = deg2rad(121.113);
ARB = deg2rad(146.519);
// statics
AC = dist2d(&A, &C);
CB = dist2d(&C, &B);
BA = dist2d(&B, &A);
ABC = deg2rad(inscribed (A, B, C)); // 54.663
BCA = deg2rad(inscribed (B, C, A)); // 78.315
CAB = deg2rad(inscribed (C, A, B)); // 47.022 not used for this
printf("Distance AC is %f\n", AC);
printf("Distance CB is %f\n", CB);
printf("Distance BA is %f\n", BA);
printf("Angle ABC is %f\n", rad2deg(ABC));
printf("Angle BCA is %f\n", rad2deg(BCA));
printf("Angle CAB is %f\n", rad2deg(CAB));
// calculations
D = ABC + BCA + BRC - deg2rad(180) ; // = 58.813 this is angle ACR + ABR
M = (AC/BA) * (sin(ARB)/sin(ARC));
printf("M is %f\n", M);
printf("D is %f\n", rad2deg(D));
ACR = atan( sin(D)/ (M+cos(D)) ); // should be 31.905
printf("angle ACR is : %f\n", rad2deg(ACR));
// for the next set of calculations, we swap things to get
ABR = atan(sin(D)/((1/M)+cos(D)));
printf("angle ABR is : %f\n", rad2deg(ABR));
// we need to rotate AC cw around C by ACR
setLinePts(&l, &A, &C);
rotLineAround(&l, &C, -ACR);
// rotate AB ccw around B by ABR
setLinePts(&l2, &A, &B);
rotLineAround(&l2, &B, ABR);
// and find the intersection
getIntersection(&p, &l, &l2);
printf("Intersection: %s \n", point2s( s, 254, &p));
return 0;
}