Skip to content

Instantly share code, notes, and snippets.

@vidhyasgr

vidhyasgr/gist:dd6638512de4dd07389537ab170803ee

Last active May 19, 2017 15:23
Show Gist options
  • Save vidhyasgr/dd6638512de4dd07389537ab170803ee to your computer and use it in GitHub Desktop.
Save vidhyasgr/dd6638512de4dd07389537ab170803ee to your computer and use it in GitHub Desktop.
Download ZIP
Trilateration using Geo. Solving linear equation, reducing error by tuning the radius, adjusting the overlapped and enveloped circle to find the best position.
Raw
gistfile1.txt
package com.kianaanalyitcs.location.algorithm;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.concurrent.ConcurrentHashMap;
public abstract class OwnTrilaterationAlgorithm {
/**
* @param rssi
* @return
*/
static double getRadiusFromRSSISimple(double rssi) {
// double base = 10;
double base = 8;
double exponent = -(rssi + 37.57) / 27.251999999997867;
double distance = Math.pow(base, exponent);
return distance;
}
/*
* checking circle collision
*/
static boolean checkingCircleIntersection(double x1, double y1, double distance, double x2, double y2, double distance2) {
return Math.abs((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)) < (distance + distance2) * (distance + distance2);
}
/**
* @return
*/
static double gettingDistanceBetweenTwoCircles(double x1, double y1, double distance, double x2, double y2, double distance2) {
return Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}
/**
* @param distance
* @param r1
* @param r2
* @return
*/
static boolean checkInsideOtherCircle(double distance, double r1, double r2) {
if ((distance <= Math.abs(r1 - r2))) {
// Inside
System.out.println("Circle2 is inside Circle1");
return true;
} else {
return false;
}
}
/**
*
*/
/**
* @return
*/
public static double[] calculatingTrilaterationUsingAlgebra(double[][] points, double[] distance, double[] latitudes, double[] longitudes) {
// calculate coordinates of U using the formula for y and x
double a1 = points[0][0], a2 = points[1][0], a3 = points[2][0], b1 = points[0][1], b2 = points[1][1], b3 = points[2][1], x, y;
double r1 = distance[0], r2 = distance[1], r3 = distance[2];
double a1Sq = a1 * a1, a2Sq = a2 * a2, a3Sq = a3 * a3, b1Sq = b1 * b1, b2Sq = b2 * b2, b3Sq = b3 * b3,
r1Sq = r1 * r1, r2Sq = r2 * r2, r3Sq = r3 * r3;
double numerator1 = (a2 - a1) * (a3Sq + b3Sq - r3Sq) + (a1 - a3) * (a2Sq + b2Sq - r2Sq)
+ (a3 - a2) * (a1Sq + b1Sq - r1Sq);
double denominator1 = 2 * (b3 * (a2 - a1) + b2 * (a1 - a3) + b1 * (a3 - a2));
// lng value
y = numerator1 / denominator1;
double numerator2 = r2Sq - r1Sq + a1Sq - a2Sq + b1Sq - b2Sq - 2 * (b1 - b2) * y;
double denominator2 = 2 * (a1 - a2);
// lat value
x = numerator2 / denominator2;
System.out.println("The Coordinates of point U are " + x + " " + y);
double point[] = new double[2];
point[0] = x;
point[1] = y;
CoordinateConverter.Hemisphere hemisphere = CoordinateConverter.getHemisphere(latitudes[0]);
int longZone = CoordinateConverter.getLongZone(longitudes[1]);
double positions[] = CoordinateConverter.utm2GPS(point, longZone, hemisphere);
System.out.println("positions " + positions[0]);
System.out.println("positions " + positions[1]);
return positions;
}
/**
*
* @param closestPoint
* @param points
* @param distance
* @return
*/
private static double[] makingCircleIntersectionNew(double[][] closestPoint, double[][] points, double[] distance) {
// TODO Auto-generated method stub
double circleOneXvalue = closestPoint[0][0];
double circleOneYvalue = closestPoint[0][1];
double circleTwoXvalue = points[0][0];
double circleTwoYvalue = points[0][1];
double circleThreeXvalue = points[1][0];
double circleThreeYvalue = points[1][1];
double circleOneDistance = distance[0];
double circleTwoDistance = distance[1];
double circleThreeDistance = distance[2];
double[] tunedDistance = new double[3];
//Checking circleOne and circleTwo intersecting points. If no, then make it intersect by increasing radius size.
// checking circle intersection between One and Two.
boolean circleOneAndTwoIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
// checking circle intersection between One and Three.
boolean circleOneAndThreeIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
// checking circle intersection between Three and Two.
boolean circleThreeAndTwoIntersect = checkingCircleIntersection(circleThreeXvalue, circleThreeYvalue, circleThreeDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
System.out.println("intersection between circles " + circleOneAndTwoIntersect + circleOneAndThreeIntersect + circleThreeAndTwoIntersect);
if (circleOneAndTwoIntersect == true && circleOneAndThreeIntersect == true && circleThreeAndTwoIntersect == true) {
tunedDistance = distance;
} else {
tunedDistance = makeAllThreeCirclesIntersect(points, distance);
}
return tunedDistance;
}
/**
* @param points
* @param distance
*/
private static double[] makingCircleIntersection(double[][] points, double[] distance) {
// TODO Auto-generated method stub
double[] tunedDistance = new double[3];
tunedDistance = makeAllThreeCirclesIntersect(points, distance);
return tunedDistance;
}
/**
* @param points
* @param distance
*/
private static double[] makeAllThreeCirclesIntersect(double[][] points, double[] distance) {
// TODO Auto-generated method stub
double[] tunedDistance = new double[3];
double circleOneXvalue = points[0][0];
double circleOneYvalue = points[0][1];
double circleTwoXvalue = points[1][0];
double circleTwoYvalue = points[1][1];
double circleThreeXvalue = points[2][0];
double circleThreeYvalue = points[2][1];
double circleOneDistance = distance[0];
double circleTwoDistance = distance[1];
double circleThreeDistance = distance[2];
boolean allThreeCirclesIntersected = false;
int maxEvaluation = 100;
while (allThreeCirclesIntersected == false && maxEvaluation > 0) {
// checking circle intersection between One and Two.
boolean circleOneAndTwoIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
// checking circle intersection between One and Three.
boolean circleOneAndThreeIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
// checking circle intersection between Three and Two.
boolean circleThreeAndTwoIntersect = checkingCircleIntersection(circleThreeXvalue, circleThreeYvalue, circleThreeDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
if (circleOneAndTwoIntersect == true && circleOneAndThreeIntersect == true && circleThreeAndTwoIntersect == true) {
allThreeCirclesIntersected = true;
}else if(circleOneAndTwoIntersect == true && circleOneAndThreeIntersect == false && circleThreeAndTwoIntersect == true){
circleThreeDistance = circleThreeDistance + 0.3;
}else if(circleOneAndTwoIntersect == false && circleOneAndThreeIntersect == true && circleThreeAndTwoIntersect == true){
circleTwoDistance = circleTwoDistance + 0.3;
}
else {
circleTwoDistance = circleTwoDistance + 0.3;
circleThreeDistance = circleThreeDistance + 0.3;
}
--maxEvaluation;
}
if (maxEvaluation != 0) {
tunedDistance[0] = circleOneDistance;
tunedDistance[1] = circleTwoDistance;
tunedDistance[2] = circleThreeDistance;
} else {
tunedDistance[0] = 0.0;
tunedDistance[1] = 0.0;
tunedDistance[2] = 0.0;
}
System.out.println("during tuning radius one " + tunedDistance[0]);
System.out.println("during tuning radius two " + tunedDistance[1]);
System.out.println("during tuning radius three " + tunedDistance[2]);
return tunedDistance;
}
/**
* @param points
* @param distance
*/
private static boolean checkAllThreeCirclesIntersect(double[][] points, double[] distance) {
// TODO Auto-generated method stub
double circleOneXvalue = points[0][0];
double circleOneYvalue = points[0][1];
double circleTwoXvalue = points[1][0];
double circleTwoYvalue = points[1][1];
double circleThreeXvalue = points[2][0];
double circleThreeYvalue = points[2][1];
double circleOneDistance = distance[0];
double circleTwoDistance = distance[1];
double circleThreeDistance = distance[2];
System.out.println("x values " + circleOneXvalue + " " + circleTwoXvalue + " " + circleThreeXvalue);
System.out.println("y values " + circleOneYvalue + " " + circleTwoYvalue + " " + circleThreeYvalue);
System.out.println("Distances " + circleOneDistance + " " + circleTwoDistance + " " + circleThreeDistance);
boolean allThreeCirclesIntersected = false;
boolean circleOneAndTwoIntersect = false;
boolean circleOneAndThreeIntersect = false;
boolean circleTwoAndThreeIntersect = false;
// checking circle intersection between One and Two.
double distanceBetweenOneAndTwo = gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
if (circleOneDistance > circleTwoDistance) {
System.out.println("testing circle one and two " + isInside(distanceBetweenOneAndTwo, circleTwoDistance, circleOneDistance));
if (isInside(distanceBetweenOneAndTwo, circleTwoDistance, circleOneDistance) == false) {
circleOneAndTwoIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
}
} else {
System.out.println("testing circle one and two " + circleOneDistance + circleTwoDistance);
if (isInside(distanceBetweenOneAndTwo, circleOneDistance, circleTwoDistance) == false) {
circleOneAndTwoIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
}
}
double distanceBetweenOneAndThree = gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
if (circleOneDistance > circleThreeDistance) {
if (isInside(distanceBetweenOneAndThree, circleThreeDistance, circleOneDistance) == false) {
circleOneAndThreeIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
}
} else {
System.out.println("ya three is bigger " + isInside(distanceBetweenOneAndThree, circleOneDistance, circleThreeDistance)
+ "distance " + circleOneDistance + circleThreeDistance);
if (isInside(distanceBetweenOneAndThree, circleOneDistance, circleThreeDistance) == false) {
circleOneAndThreeIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
}
}
// checking circle intersection between Three and Two.
double distanceBetweenTwoAndThree = gettingDistanceBetweenTwoCircles(circleTwoXvalue, circleTwoYvalue, circleTwoDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
if (circleTwoDistance > circleThreeDistance) {
System.out.println("testing inside three and two " + isInside(distanceBetweenTwoAndThree, circleThreeDistance, circleTwoDistance));
if (isInside(distanceBetweenTwoAndThree, circleThreeDistance, circleTwoDistance) == false) {
circleTwoAndThreeIntersect = checkingCircleIntersection(circleTwoXvalue, circleTwoYvalue, circleTwoDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
System.out.println(" checking two and three intersection " + circleTwoAndThreeIntersect);
}
} else {
System.out.println("testing is inside two and three " + isInside(distanceBetweenOneAndThree, circleTwoDistance, circleThreeDistance));
if (isInside(distanceBetweenTwoAndThree, circleTwoDistance, circleThreeDistance) == false) {
circleTwoAndThreeIntersect = checkingCircleIntersection(circleTwoXvalue, circleTwoYvalue, circleTwoDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
System.out.println(" checking two and three intersection second condition " + circleTwoAndThreeIntersect);
}
}
System.out.println(" checking all circle intersection " + circleOneAndTwoIntersect + circleOneAndThreeIntersect + circleTwoAndThreeIntersect);
if (circleOneAndTwoIntersect == true && circleOneAndThreeIntersect == true && circleTwoAndThreeIntersect == true) {
allThreeCirclesIntersected = true;
}
return allThreeCirclesIntersected;
}
/**
* @param points
* @param distance
*/
private static boolean checkForCircleOverlap(double[][] points, double[] distance) {
// TODO Auto-generated method stub
double circleOneXvalue = points[0][0];
double circleOneYvalue = points[0][1];
double circleTwoXvalue = points[1][0];
double circleTwoYvalue = points[1][1];
double circleThreeXvalue = points[2][0];
double circleThreeYvalue = points[2][1];
double circleOneDistance = distance[0];
double circleTwoDistance = distance[1];
double circleThreeDistance = distance[2];
boolean circleOverlapped = false;
// checking circle intersection between One and Two.
boolean circleOneAndTwoIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
// checking circle intersection between One and Three.
boolean circleOneAndThreeIntersect = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneDistance, circleThreeXvalue, circleThreeYvalue, circleThreeDistance);
// checking circle intersection between Three and Two.
boolean circleThreeAndTwoIntersect = checkingCircleIntersection(circleThreeXvalue, circleThreeYvalue, circleThreeDistance, circleTwoXvalue, circleTwoYvalue, circleTwoDistance);
if (circleOneAndTwoIntersect == false && circleOneAndThreeIntersect == false && circleThreeAndTwoIntersect == false) {
circleOverlapped = false;
} else {
circleOverlapped = true;
}
return circleOverlapped;
}
/**
* @param points
* @param distance
*/
private static boolean checkForCircleEnvelope(double[][] points, double[] distance) {
// TODO Auto-generated method stub
double circleOneXvalue = points[0][0];
double circleOneYvalue = points[0][1];
double circleTwoXvalue = points[1][0];
double circleTwoYvalue = points[1][1];
double circleThreeXvalue = points[2][0];
double circleThreeYvalue = points[2][1];
double circleOneDistance = distance[0];
double circleTwoDistance = distance[1];
double circleThreeDistance = distance[2];
boolean circleEnvelopped = false;
boolean circleOneAndTwoEnvelopped = false;
boolean circleTwoAndThreeEnvelopped = false;
boolean circleOneAndThreeEnvelopped = false;
if (circleTwoDistance > circleOneDistance) {
circleOneAndTwoEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance,
circleTwoXvalue, circleTwoYvalue, circleTwoDistance), circleOneDistance, circleTwoDistance);
} else {
if (isInside(gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance,
circleTwoXvalue, circleTwoYvalue, circleTwoDistance), circleTwoDistance, circleOneDistance))
circleOneAndTwoEnvelopped = true;
else circleOneAndTwoEnvelopped = false;
}
if (circleThreeDistance > circleTwoDistance) {
circleTwoAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleTwoXvalue, circleTwoYvalue, circleTwoDistance,
circleThreeXvalue, circleThreeYvalue, circleThreeDistance), circleTwoDistance, circleThreeDistance);
} else {
circleTwoAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleTwoXvalue, circleTwoYvalue, circleTwoDistance,
circleThreeXvalue, circleThreeYvalue, circleThreeDistance), circleThreeDistance, circleTwoDistance);
}
if (circleOneDistance > circleThreeDistance) {
circleOneAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance,
circleThreeXvalue, circleThreeYvalue, circleThreeDistance), circleThreeDistance, circleOneDistance);
} else {
circleOneAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleThreeXvalue, circleThreeYvalue, circleThreeDistance,
circleOneXvalue, circleOneYvalue, circleOneDistance), circleOneDistance, circleThreeDistance);
}
if (circleOneAndTwoEnvelopped || circleTwoAndThreeEnvelopped == true || circleOneAndThreeEnvelopped == true) {
circleEnvelopped = true;
}
System.out.println("checking circle envelope " + circleEnvelopped);
return circleEnvelopped;
}
/**
* @param points
* @param distance
*/
private static double[][] makeCircleIntersectionInEnvelope(double[][] points, double[] distance) {
// TODO Auto-generated method stub
double circleOneXvalue = points[0][0];
double circleOneYvalue = points[0][1];
double circleTwoXvalue = points[1][0];
double circleTwoYvalue = points[1][1];
double circleThreeXvalue = points[2][0];
double circleThreeYvalue = points[2][1];
double circleOneDistance = distance[0];
double circleTwoDistance = distance[1];
double circleThreeDistance = distance[2];
List<Integer> listOfEnvelopedCircle = new ArrayList<Integer>();
int envelopeCircle = 0;
envelopeCircle = getEnvelopeCircle(circleOneXvalue, circleOneYvalue, circleTwoXvalue, circleTwoYvalue, circleThreeXvalue, circleThreeYvalue, circleOneDistance, circleTwoDistance, circleThreeDistance, listOfEnvelopedCircle, envelopeCircle);
System.out.println("enveloped circle " + envelopeCircle);
System.out.println("enveloped circle size " + listOfEnvelopedCircle.size());
double[][] tunedPointsAndDistance = new double[3][3];
if (listOfEnvelopedCircle.size() >= 2) {
tunedPointsAndDistance = tuningEnvelopedCircleAndNormalCircles( points, distance, listOfEnvelopedCircle);
} else {
tunedPointsAndDistance = tuningEnvelopedCircleAndNormalCircles(envelopeCircle, points, distance);
}
return tunedPointsAndDistance;
}
/**
*
* @param circleOneXvalue
* @param circleOneYvalue
* @param circleTwoXvalue
* @param circleTwoYvalue
* @param circleThreeXvalue
* @param circleThreeYvalue
* @param circleOneDistance
* @param circleTwoDistance
* @param circleThreeDistance
* @param listOfEnvelopedCircle
* @param envelopeCircle
* @return
*/
private static int getEnvelopeCircle(double circleOneXvalue, double circleOneYvalue, double circleTwoXvalue, double circleTwoYvalue, double circleThreeXvalue, double circleThreeYvalue, double circleOneDistance, double circleTwoDistance, double circleThreeDistance, List<Integer> listOfEnvelopedCircle, int envelopeCircle) {
boolean circleOneAndTwoEnvelopped;
boolean circleTwoAndThreeEnvelopped;
boolean circleOneAndThreeEnvelopped;
if (circleTwoDistance > circleOneDistance) {
circleOneAndTwoEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance,
circleTwoXvalue, circleTwoYvalue, circleTwoDistance), circleOneDistance, circleTwoDistance);
if (circleOneAndTwoEnvelopped == true) {
envelopeCircle = 2;
listOfEnvelopedCircle.add(2);
}
} else {
circleOneAndTwoEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance,
circleTwoXvalue, circleTwoYvalue, circleTwoDistance), circleTwoDistance, circleOneDistance);
if (circleOneAndTwoEnvelopped == true) {
envelopeCircle = 1;
listOfEnvelopedCircle.add(1);
}
}
if (circleThreeDistance > circleTwoDistance) {
circleTwoAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleTwoXvalue, circleTwoYvalue, circleTwoDistance,
circleThreeXvalue, circleThreeYvalue, circleThreeDistance), circleTwoDistance, circleThreeDistance);
if (circleTwoAndThreeEnvelopped == true) {
envelopeCircle = 3;
listOfEnvelopedCircle.add(3);
}
} else {
circleTwoAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleTwoXvalue, circleTwoYvalue, circleTwoDistance,
circleThreeXvalue, circleThreeYvalue, circleThreeDistance), circleThreeDistance, circleTwoDistance);
if (circleTwoAndThreeEnvelopped == true) {
envelopeCircle = 2;
boolean duplicate = checkingDuplicateValues(listOfEnvelopedCircle, envelopeCircle);
if (duplicate == true) {
} else {
listOfEnvelopedCircle.add(2);
}
}
}
if (circleOneDistance > circleThreeDistance) {
circleOneAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleOneXvalue, circleOneYvalue, circleOneDistance,
circleThreeXvalue, circleThreeYvalue, circleThreeDistance), circleThreeDistance, circleOneDistance);
if (circleOneAndThreeEnvelopped == true) {
envelopeCircle = 1;
boolean duplicate = checkingDuplicateValues(listOfEnvelopedCircle, envelopeCircle);
if (duplicate == true) {
} else {
listOfEnvelopedCircle.add(1);
}
}
} else {
circleOneAndThreeEnvelopped = isInside(gettingDistanceBetweenTwoCircles(circleThreeXvalue, circleThreeYvalue, circleThreeDistance,
circleOneXvalue, circleOneYvalue, circleOneDistance), circleOneDistance, circleThreeDistance);
if (circleOneAndThreeEnvelopped == true) {
envelopeCircle = 3;
boolean duplicate = checkingDuplicateValues(listOfEnvelopedCircle, envelopeCircle);
if (duplicate == true) {
} else {
listOfEnvelopedCircle.add(3);
}
}
}
return envelopeCircle;
}
/**
*
* @param listOfEnvelopedCircle
* @param circleToCheck
* @return
*/
private static boolean checkingDuplicateValues(List<Integer> listOfEnvelopedCircle, int circleToCheck) {
boolean duplicate = false;
for (int envelopedCircle : listOfEnvelopedCircle) {
if (envelopedCircle == circleToCheck) {
duplicate = true;
}
}
return duplicate;
}
/**
*
* @param points
* @param distance
* @param listOfEnvelopedCircle
* @return
*/
private static double[][] tuningEnvelopedCircleAndNormalCircles( double[][] points,
double[] distance, List<Integer> listOfEnvelopedCircle) {
// TODO Auto-generated method stub
double circleOneXvalue;
double circleOneYvalue;
double circleOneRadius;
double envelopeCircleOneXvalue;
double envelopeCircleOneYvalue;
double envelopeCircleOneRadius;
double envelopeCircleTwoXvalue;
double envelopeCircleTwoYvalue;
double envelopeCircleTwoRadius;
double[][] newPointsAndDistance = new double[5][5];
ConcurrentHashMap<String, Double> circleWithRssi = new ConcurrentHashMap<String, Double>();
circleWithRssi.put("circleOne", distance[0]);
circleWithRssi.put("circleTwo", distance[1]);
circleWithRssi.put("circleThree", distance[2]);
int smallestCircle = 0;
if (listOfEnvelopedCircle.size() >= 2) {
Double min = Collections.min(circleWithRssi.values());
System.out.println("minimum distance " + min);
for (int i = 0; i < 3; i++) {
if (distance[i] == min) {
smallestCircle = i;
}
}
}
System.out.println("smallest circle is " + smallestCircle);
if (smallestCircle == 0) {
circleOneXvalue = points[0][0];
circleOneYvalue = points[0][1];
circleOneRadius = distance[0];
envelopeCircleOneXvalue = points[1][0];
envelopeCircleOneYvalue = points[1][1];
envelopeCircleOneRadius = distance[1];
envelopeCircleTwoXvalue = points[2][0];
envelopeCircleTwoYvalue = points[2][1];
envelopeCircleTwoRadius = distance[2];
} else if (smallestCircle == 1) {
circleOneXvalue = points[1][0];
circleOneYvalue = points[1][1];
circleOneRadius = distance[1];
envelopeCircleOneXvalue = points[0][0];
envelopeCircleOneYvalue = points[0][1];
envelopeCircleOneRadius = distance[0];
envelopeCircleTwoXvalue = points[2][0];
envelopeCircleTwoYvalue = points[2][1];
envelopeCircleTwoRadius = distance[2];
} else {
circleOneXvalue = points[2][0];
circleOneYvalue = points[2][1];
circleOneRadius = distance[2];
envelopeCircleOneXvalue = points[0][0];
envelopeCircleOneYvalue = points[0][1];
envelopeCircleOneRadius = distance[0];
envelopeCircleTwoXvalue = points[1][0];
envelopeCircleTwoYvalue = points[1][1];
envelopeCircleTwoRadius = distance[1];
}
// checking one and two circle intersection..
boolean twoCirclesIntersected = checkingCircleIntersection(envelopeCircleOneXvalue, envelopeCircleOneYvalue, envelopeCircleOneRadius
, envelopeCircleTwoXvalue, envelopeCircleTwoYvalue, envelopeCircleTwoRadius);
System.out.println("two circles intersected in envelope " + twoCirclesIntersected);
double[] pointsDistances = new double[4];
double[][] newPoints = new double[4][4];
boolean allThreeCirclesIntersected = false;
int maximumEvaluation = 100;
if (twoCirclesIntersected == true) {
while (!allThreeCirclesIntersected && maximumEvaluation > 0) {
newPoints[0][0] = circleOneXvalue;
newPoints[0][1] = circleOneYvalue;
newPoints[1][0] = envelopeCircleOneXvalue;
newPoints[1][1] = envelopeCircleOneYvalue;
newPoints[2][0] = envelopeCircleTwoXvalue;
newPoints[2][1] = envelopeCircleTwoYvalue;
pointsDistances[0] = circleOneRadius;
pointsDistances[1] = envelopeCircleOneRadius;
pointsDistances[2] = envelopeCircleTwoRadius;
allThreeCirclesIntersected = checkAllThreeCirclesIntersect(newPoints, pointsDistances);
System.out.println("checking all three circles intersected " + allThreeCirclesIntersected);
if (allThreeCirclesIntersected == false) {
envelopeCircleOneRadius = envelopeCircleOneRadius - 0.2;
envelopeCircleTwoRadius = envelopeCircleTwoRadius - 0.2;
}
--maximumEvaluation;
}
}
System.out.println("last two enveloped circle radius " + envelopeCircleOneRadius + " " + envelopeCircleTwoRadius);
if (maximumEvaluation != 0) {
newPointsAndDistance[0][0] = circleOneXvalue;
newPointsAndDistance[0][1] = circleOneYvalue;
newPointsAndDistance[0][2] = circleOneRadius;
newPointsAndDistance[1][0] = envelopeCircleOneXvalue;
newPointsAndDistance[1][1] = envelopeCircleOneYvalue;
newPointsAndDistance[1][2] = envelopeCircleOneRadius;
newPointsAndDistance[2][0] = envelopeCircleTwoXvalue;
newPointsAndDistance[2][1] = envelopeCircleTwoYvalue;
newPointsAndDistance[2][2] = envelopeCircleTwoRadius;
} else {
newPointsAndDistance[0][0] = circleOneXvalue;
newPointsAndDistance[0][1] = circleOneYvalue;
newPointsAndDistance[0][2] = 0.0;
newPointsAndDistance[1][0] = envelopeCircleOneXvalue;
newPointsAndDistance[1][1] = envelopeCircleOneYvalue;
newPointsAndDistance[1][2] = 0.0;
newPointsAndDistance[2][0] = envelopeCircleTwoXvalue;
newPointsAndDistance[2][1] = envelopeCircleTwoYvalue;
newPointsAndDistance[2][2] = 0.0;
}
return newPointsAndDistance;
}
/**
* @param envelopeCircle
* @param points
* @param distance
* @return
*/
private static double[][] tuningEnvelopedCircleAndNormalCircles(int envelopeCircle, double[][] points,
double[] distance) {
// TODO Auto-generated method stub
double circleOneXvalue;
double circleOneYvalue;
double circleOneRadius;
double circleTwoXvalue;
double circleTwoYvalue;
double circleTwoRadius;
double envelopeCircleXvalue;
double envelopeCircleYvalue;
double envelopeCircleRadius;
double[][] newPointsAndDistance = new double[5][5];
if (envelopeCircle == 1) {
circleOneXvalue = points[1][0];
circleOneYvalue = points[1][1];
circleOneRadius = distance[1];
circleTwoXvalue = points[2][0];
circleTwoYvalue = points[2][1];
circleTwoRadius = distance[2];
envelopeCircleXvalue = points[0][0];
envelopeCircleYvalue = points[0][1];
envelopeCircleRadius = distance[0];
} else if (envelopeCircle == 2) {
circleOneXvalue = points[0][0];
circleOneYvalue = points[0][1];
circleOneRadius = distance[0];
circleTwoXvalue = points[2][0];
circleTwoYvalue = points[2][1];
circleTwoRadius = distance[2];
envelopeCircleXvalue = points[1][0];
envelopeCircleYvalue = points[1][1];
envelopeCircleRadius = distance[1];
} else {
circleOneXvalue = points[0][0];
circleOneYvalue = points[0][1];
circleOneRadius = distance[0];
circleTwoXvalue = points[1][0];
circleTwoYvalue = points[1][1];
circleTwoRadius = distance[1];
envelopeCircleXvalue = points[2][0];
envelopeCircleYvalue = points[2][1];
envelopeCircleRadius = distance[2];
}
// checking one and two circle intersection..
//TODO check whether enveloped circle intersected with other circle.
boolean twoCirclesIntersected = checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneRadius
, circleTwoXvalue, circleTwoYvalue, circleTwoRadius);
System.out.println("two circles intersected in envelope " + twoCirclesIntersected);
int maxEvaluationThirdCircleIntersection = 20;
if (twoCirclesIntersected == true) {
envelopeCircleRadius = makingThirdCircleIntersectInEnvelope(points, circleOneRadius, circleTwoRadius,
envelopeCircleRadius);
} else {
// need to find the strongest signal, then intersect normal two circles first. finally reduce the
//size of enveloped circle.
boolean allThreeCirclesIntersected = false;
double[][] newPoints = new double[4][4];
double[] intersectedDistance = new double[4];
newPoints[0][0] = circleOneXvalue;
newPoints[0][1] = circleOneYvalue;
newPoints[1][0] = circleTwoXvalue;
newPoints[1][1] = circleTwoYvalue;
newPoints[2][0] = envelopeCircleXvalue;
newPoints[2][1] = envelopeCircleYvalue;
boolean twoNormalCirclesIntersected = false;
int maxEvaluation = 20;
while (twoNormalCirclesIntersected == false && maxEvaluation > 0) {
if (checkingCircleIntersection(circleOneXvalue, circleOneYvalue, circleOneRadius
, circleTwoXvalue, circleTwoYvalue, circleTwoRadius) == false) {
circleOneRadius = circleOneRadius + 0.2;
circleTwoRadius = circleTwoRadius + 0.2;
} else {
System.out.println("two circles intersected at " + circleOneRadius + circleTwoRadius);
twoNormalCirclesIntersected = true;
}
--maxEvaluation;
}
while (allThreeCirclesIntersected == false && maxEvaluationThirdCircleIntersection > 0) {
intersectedDistance[0] = circleOneRadius;
intersectedDistance[1] = circleTwoRadius;
intersectedDistance[2] = envelopeCircleRadius;
System.out.println(" checking intersection after subtraction " + checkAllThreeCirclesIntersect(points, intersectedDistance));
if (checkAllThreeCirclesIntersect(points, intersectedDistance) == true) {
allThreeCirclesIntersected = true;
} else {
System.out.println("subtracting the envelope circle " + envelopeCircleRadius);
envelopeCircleRadius = envelopeCircleRadius - 0.2;
if (envelopeCircleRadius <= 2.0) {
allThreeCirclesIntersected = true;
}
}
--maxEvaluationThirdCircleIntersection;
}
}
if (maxEvaluationThirdCircleIntersection != 0) {
assingingValues(circleOneXvalue, circleOneYvalue, circleOneRadius, circleTwoXvalue, circleTwoYvalue,
circleTwoRadius, envelopeCircleXvalue, envelopeCircleYvalue, envelopeCircleRadius, newPointsAndDistance);
} else {
newPointsAndDistance[0][0] = circleOneXvalue;
newPointsAndDistance[0][1] = circleOneYvalue;
newPointsAndDistance[0][2] = 0.0;
newPointsAndDistance[1][0] = circleTwoXvalue;
newPointsAndDistance[1][1] = circleTwoYvalue;
newPointsAndDistance[1][2] = 0.0;
newPointsAndDistance[2][0] = envelopeCircleXvalue;
newPointsAndDistance[2][1] = envelopeCircleYvalue;
newPointsAndDistance[2][2] = 0.0;
}
return newPointsAndDistance;
}
/**
*
* @param points
* @param circleOneRadius
* @param circleTwoRadius
* @param envelopeCircleRadius
* @return
*/
private static double makingThirdCircleIntersectInEnvelope(double[][] points, double circleOneRadius,
double circleTwoRadius, double envelopeCircleRadius) {
boolean allThreeCirclesIntersected = false;
int maxEvaluation = 30;
int indexToTake = 0;
if(circleOneRadius > circleTwoRadius){
indexToTake =1;
}else{
indexToTake =0;
}
while (allThreeCirclesIntersected == false && maxEvaluation > 0) {
double[] intersectedDistance = new double[4];
intersectedDistance[0] = circleOneRadius;
intersectedDistance[1] = circleTwoRadius;
intersectedDistance[2] = envelopeCircleRadius;
System.out.println("checking all three circles intersect " + checkAllThreeCirclesIntersect(points, intersectedDistance));
if (checkAllThreeCirclesIntersect(points, intersectedDistance) == true) {
allThreeCirclesIntersected = true;
} else {
System.out.println("subtracting the envelope circle " + envelopeCircleRadius);
intersectedDistance[indexToTake] = intersectedDistance[indexToTake] - 0.2;
// envelopeCircleRadius = envelopeCircleRadius - 0.2;
if (intersectedDistance[indexToTake] <= 2.0) {
allThreeCirclesIntersected = true;
}
}
--maxEvaluation;
}
return envelopeCircleRadius;
}
/**
*
* @param circleOneXvalue
* @param circleOneYvalue
* @param circleOneRadius
* @param circleTwoXvalue
* @param circleTwoYvalue
* @param circleTwoRadius
* @param envelopeCircleXvalue
* @param envelopeCircleYvalue
* @param envelopeCircleRadius
* @param newPointsAndDistance
*/
private static void assingingValues(double circleOneXvalue, double circleOneYvalue, double circleOneRadius,
double circleTwoXvalue, double circleTwoYvalue, double circleTwoRadius, double envelopeCircleXvalue,
double envelopeCircleYvalue, double envelopeCircleRadius, double[][] newPointsAndDistance) {
newPointsAndDistance[0][0] = circleOneXvalue;
newPointsAndDistance[0][1] = circleOneYvalue;
newPointsAndDistance[0][2] = circleOneRadius;
newPointsAndDistance[1][0] = circleTwoXvalue;
newPointsAndDistance[1][1] = circleTwoYvalue;
newPointsAndDistance[1][2] = circleTwoRadius;
newPointsAndDistance[2][0] = envelopeCircleXvalue;
newPointsAndDistance[2][1] = envelopeCircleYvalue;
newPointsAndDistance[2][2] = envelopeCircleRadius;
}
/**
* @param
*/
public static boolean isInside(double distanceBetweenTwoCircles, double radiusOne, double radiusTwo) {
return distanceBetweenTwoCircles + radiusOne <= radiusTwo;
}
/**
* @param points
* @param distance
* @param latitudes
* @param longitudes
*/
public static double[] evaluateTrilateration(double[][] points, double[] distance, double[] latitudes, double[] longitudes) {
double xa = points[0][0]; //lat
double ya = points[0][1]; //lng
double xb = points[1][0]; //lat
double yb = points[1][1]; //lng
double xc = points[2][0]; //lat
double yc = points[2][1]; //lng
double ra = distance[0]; //distance
double rb = distance[1]; //distance
double rc = distance[2]; //distance
double S = (Math.pow(xc, 2.) - Math.pow(xb, 2.) + Math.pow(yc, 2.) - Math.pow(yb, 2.) + Math.pow(rb, 2.) - Math.pow(rc, 2.)) / 2.0;
double T = (Math.pow(xa, 2.) - Math.pow(xb, 2.) + Math.pow(ya, 2.) - Math.pow(yb, 2.) + Math.pow(rb, 2.) - Math.pow(ra, 2.)) / 2.0;
double y = ((T * (xb - xc)) - (S * (xb - xa))) / (((ya - yb) * (xb - xc)) - ((yc - yb) * (xb - xa)));
double x = ((y * (ya - yb)) - T) / (xb - xa);
double point[] = new double[2];
point[0] = x;
point[1] = y;
CoordinateConverter.Hemisphere hemisphere = CoordinateConverter.getHemisphere(latitudes[0]);
int longZone = CoordinateConverter.getLongZone(longitudes[1]);
double positions[] = CoordinateConverter.utm2GPS(point, longZone, hemisphere);
System.out.println("positions " + positions[0]);
System.out.println("positions " + positions[1]);
return positions;
}
/**
*
* @param oldLat
* @param oldLng
* @param newLat
* @param newLng
* @return
*/
protected static double findingBearingAngle(double oldLat, double oldLng, double newLat, double newLng){
double longDiff= newLng-oldLng;
System.out.println("lng difference " + longDiff);
double y =Math.cos(Math.toRadians(newLat)) * Math.sin(Math.toRadians(longDiff));
double x = Math.cos(Math.toRadians(oldLat))*Math.sin(Math.toRadians(newLat))-Math.sin(Math.toRadians(oldLat))*Math.cos(Math.toRadians(newLat))*Math.cos(Math.toRadians(longDiff));
System.out.println("y value " + y + " x value " + x);
return (Math.toDegrees(Math.atan2(y, x)) + 360 ) % 360;
}
/**
* @param args
*/
public static void main(String[] args) {
List<Double> latAndLongValue = new ArrayList<Double>();
double AP1LatValue = 51.14980372;
double AP2LatValue = 51.14978676;
double AP3LatValue = 51.14976488;
double AP1LngValue = 14.9963615;
double AP2LngValue = 14.99640299;
double AP3LngValue = 14.9963004;
double AP1RssiValue = -61;
double AP2RssiValue = -62;
double AP3RssiValue = -67;
System.out.println("bearing angle " + findingBearingAngle(AP1LatValue, AP1LngValue, AP2LatValue, AP2LngValue));
double[] distance = new double[3];
double[] intersectedDistance = new double[3];
double[] latitudes = new double[3];
latitudes[0] = AP1LatValue;
latitudes[1] = AP2LatValue;
latitudes[2] = AP3LatValue;
double[] longitudes = new double[3];
longitudes[0] = AP1LngValue;
longitudes[1] = AP2LngValue;
longitudes[2] = AP3LngValue;
double[] positions;
double[][] points = new double[3][3];
for (int i = 0; i < points.length; i++) {
points[i] = CoordinateConverter.gps2UTM(latitudes[i], longitudes[i]);
}
distance[0] = getRadiusFromRSSISimple(AP1RssiValue);
System.out.println(distance[0]);
distance[1] = getRadiusFromRSSISimple(AP2RssiValue);
System.out.println(distance[1]);
distance[2] = getRadiusFromRSSISimple(AP3RssiValue);
System.out.println(distance[2]);
// here we check whether three circles touch each other.
boolean circleOverlapped = checkForCircleOverlap(points, distance);
System.out.println("checking overlapped circle " + circleOverlapped);
System.out.println("checking enveloped circle " + checkForCircleEnvelope(points, distance));
if (circleOverlapped == false || (checkForCircleEnvelope(points, distance) == false)) {
// making circle intersection.
intersectedDistance = makeAllThreeCirclesIntersect(points, distance);
positions = evaluateTrilateration(points, intersectedDistance, latitudes, longitudes);
// }
} else {
double[][] newPoints = new double[4][4];
double[][] pointsForCalculation = new double[4][4];
double[] newDistances = new double[4];
newPoints = makeCircleIntersectionInEnvelope(points, distance);
pointsForCalculation[0][0] = newPoints[0][0];
pointsForCalculation[0][1] = newPoints[0][1];
newDistances[0] = newPoints[0][2];
pointsForCalculation[1][0] = newPoints[1][0];
pointsForCalculation[1][1] = newPoints[1][1];
newDistances[1] = newPoints[1][2];
pointsForCalculation[2][0] = newPoints[2][0];
pointsForCalculation[2][1] = newPoints[2][1];
newDistances[2] = newPoints[2][2];
positions = evaluateTrilateration(pointsForCalculation, newDistances, latitudes, longitudes);
// }
}
intersectedDistance = makingCircleIntersection(points, distance);
System.out.println("circle one radius " + intersectedDistance[0] + "circle two radius " + intersectedDistance[1] + "circle three radius " + intersectedDistance[2]);
System.out.println("checking all three circle intersected after tuned " + checkAllThreeCirclesIntersect(points, intersectedDistance));
// checking overlapped circle.
double latValue = positions[0];
double longValue = positions[1];
System.out.println("lat and lng value " + latValue + " " + longValue);
double locationDifference = DistanceFilterAlgorithm.distance(AP1LatValue,
AP1LngValue, latValue, longValue, "K");
if ((locationDifference * 1000) > 40) {
System.out.println("error is high");
} else {
latAndLongValue.add(latValue);
latAndLongValue.add(longValue);
System.out.println("lat and long " + latAndLongValue.get(0) + " , " + latAndLongValue.get(1));
}
}
}
@vidhyasgr
Copy link
Author

vidhyasgr commented May 19, 2017 •
edited
Loading

This is developed for triangle access points placement. If the access points or wifi are not placed in triangle, then it will not work. I am still updating some improvements, but it will work for 90 percent of cases.

Any improvements are welcome.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment