This simple demo shows triangulation given two points and the lengths between those points and a third point. This is a common problem when, for instance, sensing the location of an object with ultrasound transducers.
This code handles the general case. By treating transducer A as being at (0,0) and transducer B at (1,0), obvious simplifications can be made.
void triangulate(
double Ax, double Ay,
double Bx, double By,
double AB, double AC, double BC,
double &Cx1, double &Cy1,
double &Cx2, double &Cy2)
{
double cos1 = (AC * AC + AB * AB - BC * BC) / (2 * AC * AB);
double sin1 = sqrt(1 - cos1 * cos1);
double B1x, B1y;
// get the location of B relative to A
B1x = Bx - Ax;
B1y = By - Ay;
// scale by AC/AB,
B1x *= AC / AB;
B1y *= AC / AB;
// rotate B1 by the calculated angle - plus or minus, and add A
double Solution1x, Solution1y, Solution2x, Solution2y;
Cx1 = B1x * cos1 + B1y * sin1 + Ax;
Cy1 = B1y * cos1 - B1x * sin1 + Ay;
Cx2 = B1x * cos1 - B1y * sin1 + Ax;
Cy2 = B1y * cos1 + B1x * sin1 + Ay;
}
double dist(double x1, double y1, double x2, double y2 ) {
return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}
void printxy(double x, double y) {
Serial.print("(");
Serial.print(x) ;
Serial.print(",");
Serial.print(y);
Serial.print(")");
}
void setup() {
Serial.begin(9600);
while (!Serial);
// seed random with A0, which is floating
randomSeed(analogRead(0));
}
void loop() {
// create some sample data
double Ax, Ay, Bx, By, Cx, Cy, AB, AC, BC;
// position of sensor A
Ax = random(-100, 100);
Ay = random(-100, 100);
// position of sensor B
Bx = random(-100, 100);
By = random(-100, 100);
// the object that we are trying to calculate the position of
Cx = random(-100, 100);
Cy = random(-100, 100);
AB = dist(Ax, Ay, Bx, By); // we know this from where we have positioned the sensors
AC = dist(Ax, Ay, Cx, Cy); // this is given by the ultrasonic
BC = dist(Bx, By, Cx, Cy); // this is given by the ultrasonic
double Solution1x, Solution1y, Solution2x, Solution2y;
triangulate(Ax, Ay, Bx, By, AB, AC, BC, Solution1x, Solution1y, Solution2x, Solution2y);
Serial.print("Sensor A is at ");
printxy(Ax, Ay);
Serial.print(", Sensor B is at ");
printxy(Bx, By);
Serial.println();
Serial.print("Distance AB is ");
Serial.print(AB);
Serial.println();
Serial.print("Sensed object is at ");
printxy(Cx, Cy);
Serial.println();
Serial.print("Distance from sensor A to object is ");
Serial.print(AC);
Serial.println();
Serial.print("Distance from sensor B to object is ");
Serial.print(BC);
Serial.println();
Serial.print("Solution 1 is ");
printxy(Solution1x, Solution1y);
Serial.print(", which is accurate to ");
Serial.print(dist(Cx, Cy, Solution1x, Solution1y));
Serial.println();
Serial.print("Solution 2 is ");
printxy(Solution2x, Solution2y);
Serial.print(", which is accurate to ");
Serial.print(dist(Cx, Cy, Solution2x, Solution2y));
Serial.println();
Serial.println();
delay(1000);
}