Finding distance between points in 3D space using correct optimization algorithm C++

Question

It's easy to use the mathematical formula to find distance between any two points in 3D space which is: √((x2-x1)^2 )+(y2-y1)^2+(z2-z1)^2

And it's pretty easy to calculate for finding distance between any two points in space where each point has (x, yz) coordinates in 3D-space.

My question, extends this formula to a large scale where I'm facing optimization hiccups. What I did was create a simple Vector class and use this formula coded in C++ to calculate the distance.

What if there are a 1000 points? How do I go about it? I'm thinking using a simple std::vector to store these 3D points won't be optimized way of calculating the distance between these 1000 points.

Answer 1

There are different level of optimization, each depends on actual situation.

On implementation level , you want to layout the data in a friendly way to the processor. ie.

1. continous
2. proper aligned
3. vector(SSE) friendly (SoA if you use vertical SSE(usually), or AoS for horizontal processing)
4. actually enable vectorisation (compiler option, or hand-craft)

This may give you up to 10% boost.

On algorithm Level , you may want to think again why do you need distances of every point in core loop

1. do you ever need to recalculate it very frequently (required in simulations) or you can live with stale values and possibly re-calc in background (eg in game)
2. Can you do with distance squared, which you can save an sqrt?
3. If your data set do not change frequently, how about convert it to polar coordinate which you can cache the r^2 of each point, the distance squared formula then reduce to about one cos() operation: r1^2 + r2^2 - 2r1r2 cos(a1-a2).

EDIT: Opps in 3D space the complexity grows to same level with Cartesian coordinate, ignore point 3.

GPU Consideration

While 1000 points are too few to trade for the overhead, you may still consider off-load the work to GPU if you have a lot more points in future.

Answer 2

Here is a version of a Vector3 class object that may help you.

Vector3.h

#ifndef VECTOR3_H
#define VECTOR3_H

#include "stdafx.h"
#include "GeneralMath.h"

class Vector3 { 

public:
    union {
        float m_f3[3];
        struct {
            float m_fX;
            float m_fY;
            float m_fZ;
        };  
    };

    inline Vector3();
    inline Vector3( float x, float y, float z );
    inline Vector3( float *pfv );
    ~Vector3();

    // Operators
    inline Vector3  operator+( const Vector3 &v3 ) const;
    inline Vector3  operator+() const;
    inline Vector3& operator+=( const Vector3 &v3 );
    inline Vector3  operator-( const Vector3 &v3 ) const;
    inline Vector3  operator-() const;
    inline Vector3& operator-=( const Vector3 &v3 );
    inline Vector3  operator*( const float &fValue ) const;
    inline Vector3& operator*=( const float &fValue );
    inline Vector3  operator/( const float &fValue ) const;
    inline Vector3& operator/=( const float &fValue );

    // -------------------------------------------------------------------
    // operator*()
    // Pre Multiple Vector By A Scalar
    inline friend Vector3 Vector3::operator*( const float &fValue, const Vector3 v3 ) {

        return Vector3( fValue*v3.m_fX, fValue*v3.m_fY, fValue*v3.m_fZ );

    } // operator*

    // -------------------------------------------------------------------
    // operator/()
    // Pre Divide Vector By A Scalar Value
    inline friend Vector3 Vector3::operator/( const float &fValue, const Vector3 v3 ) {
        Vector3 vec3;
        if ( Math::isZero( v3.m_fX ) ) {
            vec3.m_fX = 0.0f;
        } else {
            vec3.m_fX = fValue / v3.m_fX;
        }

        if ( Math::isZero( v3.m_fY ) ) {
            vec3.m_fY = 0.0f;
        } else {
            vec3.m_fY = fValue / v3.m_fY;
        }

        if ( Math::isZero( v3.m_fZ ) ) {
            vec3.m_fZ = 0.0f;
        } else {
            vec3.m_fZ = fValue / v3.m_fZ;
        }

        return vec3;
    } // operator/

    // Functions    
    inline Vector3 rotateX( float fRadians );
    inline Vector3 rotateY( float fRadians );
    inline Vector3 rotateZ( float fRadians );

    inline void setPerpendicularXZ( Vector3 v3 );
    inline void setPerpendicularXY( Vector3 v3 );
    inline void setPerpendicularYZ( Vector3 v3 );

    inline Vector3  cross( const Vector3 v3 ) const;
    inline float    dot( const Vector3 v3 ) const;
    inline float    getAngle( const Vector3 &v3, const bool bNormalized = false, bool bRadians = true );
    inline float    getCosAngle( const Vector3 &v3, const bool bNormalized = false );
    inline float    length() const;
    inline float    length2() const;
    inline void     normalize();
    inline void     zero();
    inline bool     isZero() const;

}; // Vector3

// -----------------------------------------------------------------------
// Vector3()
// Constructor
inline Vector3::Vector3() {
    m_fX = 0.0f;
    m_fY = 0.0f;
    m_fZ = 0.0f;
} // Vector3

// -----------------------------------------------------------------------
// Vector3()
// Constructor
inline Vector3::Vector3( float x, float y, float z ) {
    m_fX = x;
    m_fY = y;
    m_fZ = z;
} // Vector3

// -----------------------------------------------------------------------
// Vector3()
// Constructor
inline Vector3::Vector3( float *pfv ) {
    m_fX = pfv[0];
    m_fY = pfv[1];
    m_fZ = pfv[2];
} // Vector3

// -----------------------------------------------------------------------
// operator+()
// Unary - Operator:
inline Vector3 Vector3::operator+() const {
    return *this;
} // operator+

// -----------------------------------------------------------------------
// operator+()
// Binary - Add Two Vectors Together
inline Vector3 Vector3::operator+( const Vector3 &v3 ) const {
    return Vector3( m_fX + v3.m_fX, m_fY + v3.m_fY, m_fZ + v3.m_fZ );
} // operator+

// -----------------------------------------------------------------------
// operator+=()
// Add Two Vectors Together
inline Vector3 &Vector3::operator+=( const Vector3 &v3 ) {
    m_fX += v3.m_fX;
    m_fY += v3.m_fY;
    m_fZ += v3.m_fZ;
    return *this;
} // operator+=

// -----------------------------------------------------------------------
// operator-()
// Unary - Operator: Negate Each Value
inline Vector3 Vector3::operator-() const {
    return Vector3( -m_fX, -m_fY, -m_fZ );
} // operator-

// -----------------------------------------------------------------------
// operator-()
// Binary - Take This Vector And Subtract Another Vector From It
inline Vector3 Vector3::operator-( const Vector3 &v3 ) const {
    return Vector3( m_fX - v3.m_fX, m_fY - v3.m_fY, m_fZ -v3.m_fZ );
} // operator-

// -----------------------------------------------------------------------
// operator-=()
// Subtract Two Vectors From Each Other
inline Vector3 &Vector3::operator-=( const Vector3 &v3 ) {
    m_fX -= v3.m_fX;
    m_fY -= v3.m_fY;
    m_fZ -= v3.m_fZ;
    return *this;
} // operator-=

// -----------------------------------------------------------------------
// operator*()
// Post Multiply Vector By A Scalar
inline Vector3 Vector3::operator*( const float &fValue ) const {
    return Vector3( m_fX*fValue, m_fY*fValue, m_fZ*fValue );
} // operator*

// -----------------------------------------------------------------------
// operator*=()
// Multiply This Vector By A Scalar
inline Vector3& Vector3::operator*=( const float &fValue ) {
    m_fX *= fValue;
    m_fY *= fValue;
    m_fZ *= fValue;
    return *this;
} // operator*=

// -----------------------------------------------------------------------
// operator/()
// Post Divide Vector By A Scalar
inline Vector3 Vector3::operator/( const float &fValue ) const {
    Vector3 v3;
    if ( Math::IsZero( fValue ) ) {
        v3.m_fX = 0.0f;
        v3.m_fY = 0.0f;
        v3.m_fZ = 0.0f;
    } else {
        float fValue_Inv = 1/fValue;
        v3.m_fX = v3.m_fX * fValue_Inv;
        v3.m_fY = v3.m_fY * fValue_Inv;
        v3.m_fZ = v3.m_fZ * fValue_Inv;
    }
    return v3;
} // operator/

// -----------------------------------------------------------------------
// operator/=()
// Divide This Vector By A Scalar
inline Vector3& Vector3::operator/=( const float &fValue ) {
    if ( Math::isZero( fValue ) ) {
        m_fX = 0.0f;
        m_fY = 0.0f;
        m_fZ = 0.0f;
    } else {
        float fValue_Inv = 1/fValue;
        m_fX *= fValue_Inv;
        m_fY *= fValue_Inv;
        m_fZ *= fValue_Inv;
    }
    return *this;
} // operator/=

// -----------------------------------------------------------------------
// rotateX()
// Rotate This Vector About The X Axis
inline Vector3 Vector3::rotateX( float fRadians ) {
    Vector3 v3;
    v3.m_fX =  m_fX;
    v3.m_fY =  m_fY * cos( fRadians ) - m_fZ * sin( fRadians );
    v3.m_fZ =  m_fY * sin( fRadians ) + m_fZ * cos( fRadians );
    return v3;
} // rotateX

// -----------------------------------------------------------------------
// rotateY()
// Rotate This Vector About The Y Axis
inline Vector3 Vector3::rotateY( float fRadians ) {
    Vector3 v3;
    v3.m_fX =  m_fX * cos( fRadians ) + m_fZ * sin( fRadians );
    v3.m_fY =  m_fY;
    v3.m_fZ = -m_fX * sin( fRadians ) + m_fZ * cos( fRadians );
    return v3;
} // rotateY

// -----------------------------------------------------------------------
// rotateZ()
// Rotate This Vector About The Z Axis
inline Vector3 Vector3::rotateZ( float fRadians ) {
    Vector3 v3;
    v3.m_fX = m_fX * cos( fRadians ) - m_fY * sin( fRadians );
    v3.m_fY = m_fX * sin( fRadians ) + m_fY * cos( fRadians );
    v3.m_fZ = m_fZ;
    return v3;
} // rotateZ

// -----------------------------------------------------------------------
// setPerpendicularXY()
// Make This Vector Perpendicular To Vector3
inline void Vector3::setPerpendicularXY( Vector3 v3 ) {
    m_fX = -v3.m_fY;
    m_fY =  v3.m_fX;
    m_fZ =  v3.m_fZ;
} // setPerpendicularXY

// -----------------------------------------------------------------------
// setPerpendicularXZ()
// Make This Vector Perpendicular To Vector3
inline void Vector3::setPerpendicularXZ( Vector3 v3 ) {
    m_fX = -v3.m_fZ;
    m_fY =  v3.m_fY;
    m_fZ =  v3.m_fX;
} // setPerpendicularXZ

// -----------------------------------------------------------------------
// setPerpendicularYZ()
// Make This Vector Perpendicular To Vector3
inline void Vector3::setPerpendicularYZ( Vector3 v3 ) {
    m_fX =  v3.m_fX;
    m_fY = -v3.m_fZ;
    m_fZ =  v3.m_fY;
} // setPerpendicularYZ

// -----------------------------------------------------------------------
// cross()
// Get The Cross Product Of Two Vectors
inline Vector3 Vector3::cross( const Vector3 v3 ) const {
    return Vector3( m_fY*v3.m_fZ - m_fZ*v3.m_fY,
                    v3.m_fX*m_fZ - m_fX*v3.m_fZ,
                    m_fX*v3.m_fY - m_fY*v3.m_fX );
} // cross

// -----------------------------------------------------------------------
// dot()
// Return The Dot Product Between This Vector And Another One
inline float Vector3::dot( const Vector3 v3 ) const {
    return ( m_fX * v3.m_fX + 
             m_fY * v3.m_fY +
             m_fZ * v3.m_fZ );
} // dot

// -----------------------------------------------------------------------
// normalize()
// Make The Length Of This Vector Equal To One
inline void Vector3::normalize() {
    float fMag;
    fMag = sqrt( m_fX*m_fX + m_fY*m_fY + m_fZ*m_fZ );
    if ( fMag <= Math::ZERO ) {
        m_fX = 0.0f;
        m_fY = 0.0f;
        m_fZ = 0.0f;
        return;
    }

    fMag = 1/fMag;
    m_fX *= fMag;
    m_fY *= fMag;
    m_fZ *= fMag;
} // normalize

// -----------------------------------------------------------------------
// isZero()
// Return True if Vector Is (0,0,0)
inline bool Vector3::isZero() const {
    if ( Math::isZero( m_fX ) && 
         Math::isZero( m_fY ) && 
         Math::isZero( m_fZ ) ) {
        return true;
    } else {
        return false;
    }
} // isZero

// -----------------------------------------------------------------------
// zero()
// Set The Vector To (0,0,0)
inline void Vector3::zero() {
    m_fX = 0.0f;
    m_fY = 0.0f;
    m_fZ = 0.0f;
} // zero

// -----------------------------------------------------------------------
// getCosAngle()
// Returns The cos(Angle) Value Between This Vector And Vector V. This
// Is Less Expensive Than Using GetAngle
inline float Vector3::getCosAngle( const Vector3 &v3, const bool bNormalized ) {
    // a . b = |a||b|cos(angle)
    // -> cos-1((a.b)/(|a||b|))

    // Make Sure We Do Not Divide By Zero
    float fMagA = length();
    if ( fMagA <= Math::ZERO ) {
        // This (A) Is An Invalid Vector
        return 0;
    }

    float fValue = 0;
    if ( bNormalized ) {
        // v3 Is Already Normalized
        fValue = dot(v3)/fMagA;
    } else {
        float fMagB = v3.length();
        if ( fMagB <= Math::ZERO) {
             // B Is An Invalid Vector
             return 0;
        }
        fValue = dot(v3)/(fMagA*fMagB);
    }

    // Correct Value Due To Rounding Problem
    Math::constrain( -1.0f, 1.0f, fValue );

    return fValue;
} // getCosAngle

// -----------------------------------------------------------------------
// getAngle()
// Returns The Angle Between This Vector And Vector V in Radians.
// This Is More Expensive Than Using getCosAngle
inline float Vector3::getAngle( const Vector3 &v3, const bool bNormalized, bool bRadians ) {
    // a . b = |a||b|cos(angle)
    // -> cos-1((a.b)/(|a||b|))

    if ( bRadians ) {
        return acos( this->getCosAngle( v3 ) );
    } else {
        // Convert To Degrees
        return Math::radian2Degree( acos( getCosAngle( v3, bNormalized ) ) );
    }
} // getAngle

// -----------------------------------------------------------------------
// length()
// Return The Absolute Length Of This Vector
inline float Vector3::length() const {
    return sqrtf( m_fX * m_fX + 
                  m_fY * m_fY +
                  m_fZ * m_fZ );
 } // length

 // -----------------------------------------------------------------------
 // length2()
 // Return The Relative Length Of This Vector - Doesn't Use sqrt() Less Expensive
 inline float Vector3::length2() const {
     return ( m_fX * m_fX +
              m_fY * m_fY +
              m_fZ * m_fZ );
 } // length2

#endif // VECTOR3_H

Vector3.cpp

#include "Vector3.h"

GeneralMath.h

#ifndef GENERALMATH_H
#define GENERALMATH_H

#include "stdafx.h"

class Math {
public:
    static const float PI;
    static const float PI_HALVES;
    static const float PI_THIRDS;
    static const float PI_FOURTHS;
    static const float PI_SIXTHS;
    static const float PI_2;
    static const float PI_INVx180;
    static const float PI_DIV180;
    static const float PI_INV;
    static const float ZERO;

    Math();

    inline static bool  isZero( float fValue );
    inline static float sign( float fValue );

    inline static int   randomRange( int iMin, int iMax );
    inline static float randomRange( float fMin, float fMax );

    inline static float degree2Radian( float fDegrees );
    inline static float radian2Degree( float fRadians );
    inline static float correctAngle( float fAngle, bool bDegrees, float fAngleStart = 0.0f );
    inline static float mapValue( float fMinY, float fMaxY, float fMinX, float fMaxX, float fValueX );

    template<class T>
    inline static void constrain( T min, T max, T &value );

    template<class T>
    inline static void swap( T &value1, T &value2 );

}; // Math

// -----------------------------------------------------------------------
// degree2Radian()
// Convert Angle In Degrees To Radians
inline float Math::degree2Radian( float fDegrees ) {
    return fDegrees * PI_DIV180;
} // degree2Radian

// -----------------------------------------------------------------------
// radian2Degree()
// Convert Angle In Radians To Degrees
inline float Math::radian2Degree( float fRadians ) {
    return fRadians * PI_INVx180;
} // radian2Degree

// -----------------------------------------------------------------------
// correctAngle()
// Returns An Angle Value That Is Alway Between fAngleStart And fAngleStart + 360
// If Radians Are Used, Then Range Is fAngleStart To fAngleStart + 2PI
inline float Math::correctAngle( float fAngle, bool bDegrees, float fAngleStart ) {
    if ( bDegrees ) {
        // Using Degrees
        if ( fAngle < fAngleStart ) {
            while ( fAngle < fAngleStart ) {
                fAngle += 360.0f;
            }
        } else if ( fAngle >= (fAngleStart + 360.0f) ) {
            while ( fAngle >= (fAngleStart + 360.0f) ) {
                fAngle -= 360.0f;
            }
        }
        return fAngle;
    } else {
        // Using Radians
        if ( fAngle < fAngleStart ) {
            while ( fAngle < fAngleStart ) {
                fAngle += Math::PI_2;
            }
        } else if ( fAngle >= (fAngleStart + Math::PI_2) ) {
            while ( fAngle >= (fAngleStart + Math::PI_2) ) {
                fAngle -= Math::PI_2;
            }
        }
        return fAngle;
    }
} // correctAngle

// -----------------------------------------------------------------------
// isZero()
// Tests If Input Value Is Close To Zero
inline bool Math::isZero( float fValue ) {
    if ( (fValue > -ZERO) && (fValue < ZERO) ) {
        return true;
    }
    return false;
} // isZero

// -----------------------------------------------------------------------
// sign()
// Returns 1 If Value Is Positive, -1 If Value Is Negative Or 0 Otherwise
inline float Math::sign( float fValue ) {
    if ( fValue > 0 ) {
        return 1.0f;
    } else if ( fValue < 0 ) {
        return -1.0f;
    }
    return 0;
} // sign

// -----------------------------------------------------------------------
// randomRange()
// Return A Random Number Between iMin And iMax Where iMin < iMax
inline int Math::randomRange( int iMin, int iMax ) {
    if ( iMax < iMin ) {
        swap( iMax, iMin );
     }
     return (iMin + ((iMax - iMin +1) * rand()) / (RAND_MAX+1) );
} // randomRange

// -----------------------------------------------------------------------
// randomRange()
// Return A Random Number Between fMin And fMax Where fMin < fMax
inline float Math::randomRange( float fMin, float fMax ) {
    if ( fMax < fMin ) {
        swap( fMax, fMin );
    }
    return (fMin + (rand()/(float)RAND_MAX)*(fMax-fMin));
} // randomRange

// -----------------------------------------------------------------------
// mapValue()
// Returns The fValueY That Corresponds To A Point On The Line Going From Min To Max
inline float Math::mapValue( float fMinY, float fMaxY, float fMinX, float fMaxX, float fValueX ) {
    if ( fValueX >= fMaxX ) {
        return fMaxY;
} else if ( fValueX <= fMinX ) {
        return fMinY;
    } else {
        float fM = (fMaxY - fMinY) / (fMaxX - fMinX);
        float fB = fMaxY - fM * fMaxX;
        return (fM*fValueX + fB);
    }
} // mapValue

// -----------------------------------------------------------------------
// constrain()
// Prevent Value From Going Outside The Min, Max Range.
template<class T>
inline void Math::constrain( T min, T max, T &value ) {
    if ( value < min ) {
        value = min;
        return;
    }
    if ( value > max ) {
        value = max;
    }
} // Constrain

// -----------------------------------------------------------------------
// swap()
//
template<class T>
inline void Math::swap( T &value1, T &value2 ) {
    T temp;
    temp   = value1;
    value1 = value2;
    value2 = temp;
} // swap

#endif // GENERALMATH_H

GeneralMath.cpp

#include "GeneralMath.h"

const float Math::PI           = 4.0f  * atan(1.0f); // tan(pi/4) = 1
const float Math::PI_HALVES    = 0.50f * Math::PI;
const float Math::PI_THIRDS    = Math::PI * 0.3333333333333f;
const float Math::PI_FOURTHS   = 0.25f * Math::PI;
const float Math::PI_SIXTHS    = Math::PI * 0.6666666666667f;
const float Math::PI_2         = 2.00f * Math::PI;
const float Math::PI_DIV180    = Math::PI / 180.0f;
const float Math::PI_INVx180   = 180.0f / Math::PI;
const float Math::PI_INV       = 1.0f / Math::PI;
const float Math::ZERO         = (float)1e-7;

// -----------------------------------------------------------------------
// Math()
// Default Constructor
Math::Math() {
} // Math

If you take a notice at the Vector3 class object it has 2 methods that returns the length of the vector. One uses the sqrt() which will give the absolute length but is more expensive to use. The second method length2() does not use the sqrt() and returns the relative length which is inexpensive. This same exact concept can be applied to finding distances between two points! I hope this class helps you. If you need to know the exact length or distance then yes use the sqrt() version. When it comes to comparisons between multiple points or vectors then the comparison truth table between A & B is the same using either method so it is optimal to use the 2nd version of this function.