How to find the user position on a 2d floor plan if other coordinates (3 or more points) are given in unity?

Floor Plan

Is it possible to find the position of user if the coordinates of A,B and c are known.D1,D2 and D3 can also be provided.With all these details can we find the user's position?

float D1 = Vector3.Distance(userposition, PointAPosition);
float D2 = Vector3.Distance(userposition, PointBPosition);
float D3 = Vector3.Distance(userposition, PointCPosition);

1 answer

  • answered 2018-11-09 02:45 SonicBlue22

    I expected to find a lot on answers on how to do this, but I only could find variations on "here are the equations, do it yourself". So here's how I'd do it using a Geometry class which I like to use because it makes stuff like this easier to understand.

    Most of the math from Intersections(Circle c1, Circle c2) is from here.

    Also, I didn't test this because I wrote it here so if it doesn't work then at least you have somewhere to start. If this has a problem tell me how you fixed it and I'll edit this.

    namespace StuffYouStoleFromStackOverflow {
        public static class Geometry {
            public enum AngleType { Radians, Degrees }
    
            public static float GetAngle(Vector2 v1, Vector2 v2, AngleType units = AngleType.Degrees) { return GetAngle(v1.x, v1.y, v2.x, v2.y, units); }
            public static float GetAngle(float x1, float y1, float x2, float y2, AngleType units = AngleType.Degrees) {
                if(units == AngleType.Radians) return Mathf.Atan2(y1 - y2, x1 - x2);
                else return Mathf.Atan2(y1 - y2, x1 - x2) * Mathf.Rad2Deg;
            }
    
            public static Vector2[] Intersection(Circle c1, Circle c2) {
                float r1 = c1.radius, r2 = c2.radius;
                float d = Vector2.Distance(c1.center, c2.center);
                if(d > c1.radius + c2.radius) {
                Vector2[] i = {   // only gets here if there is no real intersection
                        Vector2.Lerp(c1.center, c2.center, c1.radius / d),
                        Vector2.Lerp(c1.center, c2.center, c2.radius / d)
                    };
                    return i;
                }
    
                // squared versions of the variables, because we use them a lot.
                float d_2 = d * d, r1_2 = r1 * r1, r2_2 = r2 * r2;
    
                float b = d_2 - r1_2 + r2_2;
                float x = b / (2 * d);
                float a = (1 / d) * Mathf.Sqrt((4 * d_2 * r2_2) - (b * b));
                float y = (a / 2);
    
                float angle = GetAngle(c1.center, c2.center, AngleType.Radians);
    
                Vector2[] intersections = new Vector2[2];
                intersections[0] = new Vector2(x, +y).Rotate(angle, AngleType.Radians) + c1.center;
                intersections[1] = new Vector2(x, -y).Rotate(angle, AngleType.Radians) + c1.center;
    
                return intersections;
            }
    
            public static Vector2 Intersection(Circle c1, Circle c2, Circle c3) {
                var i1 = Intersection(c1, c2);
                var i2 = Intersection(c1, c3);
    
                int smallest = 0;
                float[] D = new float[4];
                D[0] = Vector2.Distance(i1[0], i2[0]);
                D[1] = Vector2.Distance(i1[0], i2[1]);
                D[2] = Vector2.Distance(i1[1], i2[0]);
                D[3] = Vector2.Distance(i1[1], i2[1]);
    
                for(int j < 1; j < 4; j++)
                    if(D[smallest] > D[j]) smallest = j;
    
                else return i2[smallest % 2]; //not 100% sure on this part, might be i1 instead?
            }
    
            public class Circle {
                public Vector2 center;
                public float radius;
                public Circle(Vector2 center, float radius) {
                    this.center = center;
                    this.radius = radius;
                }
            }
    
            public static Vector2 Rotate(this Vector2 vector, float angle, AngleType units = AngleType.Degrees) {
                if(units == AngleType.Degrees) angle * Mathf.Deg2Rad;
                float sin = Mathf.Sin(angle), cos = Mathf.Cos(angle);
                vector.x = (cos * vector.x) - (sin * vector.y);
                vector.y = (sin * vector.x) + (cos * vector.y);
                return vector;
            }
        }
    }
    

    I spent too long on this.