Distance Formula in 6 DOF

How to find the distance between two 3d points with angular movement(yaw,pitch,roll.

For simple 3d points use


but how to find with yaw pitch roll involve in it.


Yaw, pitch, roll are “orientations”. Your distance is query on “positions”.

Ignore the orientations and compute distance as normal.

Now if you have orientation “and” position transforms in a stream (e.g. to define a modeling transform), that’s a different story. But keep in mind that the aggregate translate is stored in the right column of the 4x4 matrix :slight_smile:

but i have problem is that i have lots of orientation values at same position i want to find minimum orientation value

for example

x=3 y=4 z=9 yaw=70 pitch=50 roll=40

i have following values in DB i want closest value

x1=3 y1=4 z1=9 yaw1=80 pitch1=40 roll1=150
x2=3 y2=4 z2=9 yaw2=170 pitch2=50 roll2=140
x3=3 y3=4 z3=9 yaw3=80 pitch3=40 roll3=50

All above values are same position but with respect to orientation yaw3,pitch3.roll3 is closest i want this values which formula or algorithm i apply for this problem.

You can not find distance between two points in 3D using there orientations only. You shall be needing radius so that position of each point is encoded in polar form.

P1(x1,y1) would be : x1= Rcos (theta1) - R sin (theta1) and similarly for x2, y2 and y1.

D = sqrt( sqr(Y2-Y1) - sqr(X2-X1))