# 6 DOF Distance formula

## Recommended Posts

How to find the distance between two 3d points with angular movement(yaw,pitch,roll. For simple 3d points use sqrt((x1-x2)(x1-x2)+(y1-y2)(y1-y2)+(z1-z2)(z1-z2)) but how to find with yaw pitch roll involve in it. ----------------------- wwww.ashscholar.com

#### Share this post

##### Share on other sites
Yaw, pitch, and roll are rotations, they don't have anything to do with distance. Please restate your question more clearly if this answer doesn't satisfy you.

#### Share this post

##### Share on other sites
Any transformation that does not change the distance between 2 points is a rotation, by definition. This is true if both points are defined in the same space. Thus if your two points were in the same space to begin with, the rotation will not change the distance between them.

If the points are in two different spaces, then a rotation of one of the spaces can affect the distance between the points. For instance, if one point is in world space, and the other point is specified in the local space of a model, then if the model rotates, then the world space representation of the model point rotates as well, and the distance between the two points changes.

If this is the type of setup that you are referring to, you need to get both of your points into the same space before you can apply the distance formula.

Here is a possible function if you want to find the distance between a point in world space (W), and a point in model space (M). This assumes that the yaw (y), pitch (p) and roll (r) angles are in the correct domains, and are given in radians. Also, this function allows the origin of the model space to be translated (T) from the origin in world space. If the model space and the world space share the same origin, then T is just zero.

float distance(const Vector& W, cont Vector& M, float y, float p, float r, const Vector& T){     //evaluate all of the sines and cosines     float cy = cos(y);     float cp = cos(p);     float cr = cos(r);          float sy = sin(y);     float sp = sin(p);     float sr = sin(r);     //calculate the components of the rotation matrix     float m11 = cr * cp;     float m12 = cr * sp * sy - sr * cy;     float m13 = cr * sp * cy + sr * sy;     float m21 = sr * cp;     float m22 = sr * sp * sy + cr * cy;     float m23 = sr * sp * cy - cr * sy;     float m31 = -sr;     float m32 = cp * sy;     float m33 = cr * cy;     //calculate the deltas     float dx = W.x - M.x * m11 - M.y * m12 - M.z * m13 - T.x;     float dy = W.y - M.x * m21 - M.y * m22 - M.z * m23 - T.y;     float dz = W.z - M.x * m31 - M.y * m32 - M.z * m33 - T.z;     //return the distance     return sqrt(dx*dx + dy*dy + dz*dz);}

Of course, if I were going to do this, I would prefer the rotation to be represented by a matrix, rather than angles, so that I wouldn't have to do all of the sines and cosines every time I wanted to check a distance between a point in world space, and a point in the model space.

#### Share this post

##### Share on other sites
There is a natural way to talk about distance between "points with angular movement."

A position together with an orientation -- a frame, positioned somewhere in space -- is called an element of SE(3), the "Special Euclidean Group." You can think of a point G in SE(3) as a rotation matrix R together with a point p; i.e., G=(R,p). The notion of distance is the geodesic distance on this manifold (in the same way that there is a shortest distance on the sphere between two points on the sphere).

Here is how you compute it. Given two elements G1=(R1,p1), G2=(R2,p2) of SE(3), the geodesic distance between them is

dist(G1, G2) = sqrt(dist(R1,R2)^2 + dist(p1, p2)^2)

where

dist(p1, p2) = ||p2 - p1||

is the usual Euclidean distance, and dist(R1, R2) is the angle through which the rotation matrix R2 R1^T rotates. To compute it, just convert (R2 R1^T) to axis-angle coordinates; the distance is the angle. For how to do that, see here; in particular, the angle is,
.

#### Share this post

##### Share on other sites
Quote:
 Original post by Eric_BrownAny transformation that does not change the distance between 2 points is a rotation, by definition.

A rotation is an isometry, but not all isometries are rotations.

#### Share this post

##### Share on other sites
Quote:
 Original post by nilknA rotation is an isometry, but not all isometries are rotations.

To expand/clarify: There are also reflections.

#### Share this post

##### Share on other sites
I have an objection to Emergent's distance definition: It depends on the units used. The usual euclidean distance has units of length, but the distance in the space of rotations has units of radians (or "doesn't have units", more correctly). You need to multiply the angle part by a proportionality constant with units of length.

#### Share this post

##### Share on other sites
Quote:
 Original post by alvaroI have an objection to Emergent's distance definition: It depends on the units used. The usual euclidean distance has units of length, but the distance in the space of rotations has units of radians (or "doesn't have units", more correctly). You need to multiply the angle part by a proportionality constant with units of length.

Hmmmm.... yeah, that throws a wrench in...

If you're thinking about SE(3) as the configuration manifold for a solid body, then the natural Riemannian metric on this space is the Kinetic Energy metric, which represented in coordinates is just the generalized mass matrix...

Unfortunately, that makes what the geodesics are and hence what the distances are less obvious, so I'd need to think about it...

#### Share this post

##### Share on other sites
My 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 to find minimum values of orientation

#### Share this post

##### Share on other sites
Oh, I see.

If all of the points are at the same position, then you only need to worry about the angular peice.

Find the rotation matrix, or quaternion that represents the set of angles. I prefer quaternions. You construct the quaternion like this

$q = \left(\cos\left(\frac{\phi}{2}\right)+\hat{\mathbf{x}}\sin\left(\frac{\phi}{2}\right)\right)\left(\cos\left(\frac{\theta}{2}\right)+\hat{\mathbf{y}}\sin\left(\frac{\theta}{2}\right)\right)\left(\cos\left(\frac{\psi}{2}\right)+\hat{\mathbf{z}}\sin\left(\frac{\psi}{2}\right)\right)$

Here ψ is the yaw angle, θ is the pitch angle, and φ is the roll angle. Of course, these angles are given within their canonical ranges.

If you have 2 quaternions that represent 2 different rotations, then you can always construct a single quaternion that will take you from one rotation to the other

$\Delta q = q_2\overline{q}_1$

If you want to find the angle that this delta quaternion represents, you use the scalar part of Δq in the following formula.

$\alpha = 2\arccos\left(\Delta q_s\right)$

Note, that you don't have to perform a full blown quaternion multiply if you want to find the scalar part of Δ q. The scalar part of this product is just the 4D dot product of the two quaternions q1 and q2.

$\alpha = 2\arccos\left({\rm dot}_4\left(q_1, q_2\right)\right)$

It seems to me that what you are looking for is the set of coordinates that minimizes the angle α.

I don't recommend using matrices to solve this problem, because it is more expensive all around. But if you were using matrices instead, you would find the delta matrix

$\Delta R = R\left(\phi_2; \theta_2; \psi_2\right) R\left(\phi_1; \theta_1; \psi_1)^{-1}$

Then you would use the formula posted by Emergent

$\alpha = \arccos\left(\frac{{\rm Trace}\left(\Delta R\right) - 1}{2}\right)$

[Edited by - Eric_Brown on April 1, 2010 11:38:44 AM]

#### Share this post

##### Share on other sites
On top of what Eric said:
(1) Since q and -q represent the same orientation, you should take the absolute value of the dot product before you take the arccos.
(2) You don't need to compute the expensive arccos at all if all you need to know is which of them is closest. The smallest angle will be the one with the highest cosine, so simply find the one that maximizes that.

#### Share this post

##### Share on other sites
Quote:
 Original post by alvaroOn top of what Eric said:(1) Since q and -q represent the same orientation, you should take the absolute value of the dot product before you take the arccos.(2) You don't need to compute the expensive arccos at all if all you need to know is which of them is closest. The smallest angle will be the one with the highest cosine, so simply find the one that maximizes that.

Agreed!

#### Share this post

##### Share on other sites
Thank for reply all of user,any c++ example code that implement above formulas.

#### Share this post

##### Share on other sites
Quote:
 Original post by usman1266Thank for reply all of user,any c++ example code that implement above formulas.

No: Do your own work. If you can't figure it out, post what you've tried, and we'll help you.

## Create an account or sign in to comment

You need to be a member in order to leave a comment

## Create an account

Sign up for a new account in our community. It's easy!

Register a new account

## Sign in

Already have an account? Sign in here.

Sign In Now

• ## Partner Spotlight

• ### Forum Statistics

• Total Topics
627674
• Total Posts
2978558

• 11
• 11
• 10
• 12
• 22