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

Question

Is it possible to find the position of user if the coordinates of A,B and c are known which is taken as A(floorpointx1,floorpointy1),B(floorpointx2,floorpointy2)and C(floorpointx3,floorpointy3) .D1,D2 and D3 can also be provided.With all these details can we find the user's position[User(floorpoint_x,floorpoint_y)]?

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

You can find point of intersection of three circles (centers in A, B, C; radiuses D1, D2, D3) — obywan, Nov 08 '18 at 07:51
@obywan I have seen formulas like where (x1−a1)^2+(x2−a2)^2 =D^2.How to write a method by passing three coordinates and distance values.https://stackoverflow.com/questions/41958970/trilateration-formula-programming — zyonneo, Nov 08 '18 at 17:42
If you want to find `userposition` how could you possibly calculate the distance between each point and `userposition`!? The formula `(x - x0)^2 + (y - y0)^2 == R^2` is to find if point [x,y] is located on the circumference of a circle with center [x0,y0] and radius R. Take a look here: https://stackoverflow.com/questions/19723641/find-intersecting-point-of-three-circles-programmatically — Remi, Nov 09 '18 at 01:02
Do you have the coordinates from A, B, C in a global coordinate system and also the coordinates from A, B, C and User in a local coordiante system? Or from where do you get the distances D1, D2, D3? — xFL, Nov 09 '18 at 07:21
@xFL The coordinates (x1,y1,x2,y2,x3,y3)are preset from the floor plan.That is where I will be placing the models or Point of Interest.After I placed the model/POI then I can get the users distance by using the code distance2 = Vector3.Distance(Cameraposition, modelpos1); I want to plot user position according to the coordinates of the floor plan. — zyonneo, Nov 09 '18 at 07:25
@zyonneo to clarify, you have your coordinates from the floor plane (lets say global coordinate system) and the coordinates from the placed models/POI and the user in the Arkit coordinate system (lets say local coordiante system), right? — xFL, Nov 09 '18 at 07:31
@xFL I am placing the models/POI according to the coordinates of the floor plan.In floorplan say(33,9) I will place the model in that value (almost accurate position).The second model say(120,20) and the third in the (11,20) according to floor plan coordinates.After that I calculate the distance from the device.So coordinates and distance are available.So now I can calculate the user position(u1,u2) according to floor plan coordinates. It is just an idea will it work? — zyonneo, Nov 09 '18 at 08:08
@zyonneo okay, so in floor plan you have your coordinates (say: 33, 9) but in your app, when you place a model the local coordinates from ARKit are stored (say: 2.1, 3.8) right? — xFL, Nov 09 '18 at 08:40
@xFL yes but I dont take that values to plot. Only floor coordinates I use to plot — zyonneo, Nov 09 '18 at 08:43

Grant Shotwell · Answer 1 · 2018-11-09T04:27:13.243

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.

score 0 · Answer 2 · edited Jun 20 '20 at 09:12

After our discussion in the comments, I think the best solution for your problem is a 2D helmert similarity transformation. If you have 2 or more known points in your floor map and place a model/POI on that points, (in other words: you are measuring the coordinates in the ARKit coordinate system!) you can calculate the relationship between the two coordinate system, and then you can transform your user ARKit position to your floor map coordinate system.

The similarity transformation makes a translation, rotation and scale between your two coordiante systems.

Here you find some more infos and an example, how the 2d helmert similarity transformation looks: http://www.geo.itu.edu.tr/dersler/Example%205_2D%20Helmert%20Similarity%20Transformation.pdf Study the document and look at the numerical example on page 6-8, that should be no problem to implement.

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

2 Answers2

Linked