3D Vector operations

Collaboration diagram for 3D Vector operations:

Data Structures
struct	vec_3d_
	3D vector type More...

Typedefs
typedef struct vec_3d_	vec_3d
	3D vector type

Functions
vec_3d	vector_sub (const vec_3d *a, const vec_3d *b)
	Subtract one vector from another. More...
vec_3d	vector_add (const vec_3d *a, const vec_3d *b)
	Add one vector to another. More...
float	dot_prod (const vec_3d *a, const vec_3d *b)
	Obtain the dot product of two 3D vectors. More...
vec_3d	vector_prod (const vec_3d *a, const vec_3d *b)
	Compute the vector product of two 3d vectors. More...
const char *	print_vector (const vec_3d *a, const char *name)
	Print formatted vector on stdout. More...
float	vector_norm (const vec_3d *a)
	Compute the norm a vector. More...
vec_3d	unit_vec (const vec_3d *a)
	Obtain unit vector in the same direction as given vector. More...
mat_3x3	get_cross_matrix (const vec_3d *a)
	The cross product of vectors can be represented as a matrix multiplication operation. More...
double	get_angle (const vec_3d *a, const vec_3d *b)
	Obtain the angle between two given vectors. More...

Detailed Description

Function Documentation

dot_prod()

float dot_prod	(	const vec_3d *	a,
		const vec_3d *	b
	)

Obtain the dot product of two 3D vectors.

\[ \vec{a}\cdot\vec{b}=a_xb_x + a_yb_y + a_zb_z \]

Parameters

[in]	a	first vector
[in]	b	second vector

Returns

resulting dot product

{
    float dot;
#ifdef LIBQUAT_ARM
    arm_dot_prod_f32((float *)a, (float *)b, &dot);
#else
    dot = a->x * b->x;
    dot += a->y * b->y;
    dot += a->z * b->z;
#endif
 
    return dot;
}

get_angle()

double get_angle	(	const vec_3d *	a,
		const vec_3d *	b
	)

Obtain the angle between two given vectors.

\[\alpha=acos\left(\frac{\vec{a} \cdot \vec{b}}{\lVert\vec{a}\rVert \cdot \lVert\vec{b}\rVert}\right)\]

Parameters

[in]	a	first input vector
[in]	b	second input vector

Returns

angle between \(\vec{a}\) and \(\vec{b}\) in radians

< The norm of vector a

< The norm of vector b

detect possible division by 0 - the angle is not defined in this case

{
    double alpha, cos_alpha;
    float norm_a = vector_norm(a); ///< The norm of vector a
    float norm_b = vector_norm(b); ///< The norm of vector b
    if (fabsf(norm_a) < EPSILON || fabsf(norm_b) < EPSILON) /// detect possible division by 0 - the angle is not defined in this case
    {  
        return NAN; 
    }
 
    cos_alpha = dot_prod(a, b) / (norm_a * norm_b);
    alpha = acos(cos_alpha); // delivers the radian
    return alpha; // in range from -1 to 1
}

Here is the call graph for this function:

get_cross_matrix()

mat_3x3 get_cross_matrix

(

const vec_3d *

a

)

The cross product of vectors can be represented as a matrix multiplication operation.

This function obtains the 3x3 matrix of the cross-product operator from the first vector.

\[\begin{align*} \left(\vec{a}\times\right)\vec{b} &= \tilde{A}_a\vec{b}\\ \tilde{A}_a &= \begin{bmatrix}0&-a_z&a_y\\a_z&0&-a_x\\-a_y&a_x&0\end{bmatrix} \end{align*}\]

Parameters

[in]

a

input vector

Returns

the 3x3 matrix for the cross product operator \(\left(\vec{a}\times\right)\)

{
    mat_3x3 A = {0., -a->z, a->y, a->z, 0., -a->x, -a->y, a->x, 0.};
    return A;
}

print_vector()

const char * print_vector	(	const vec_3d *	a,
		const char *	name
	)

Print formatted vector on stdout.

Parameters

[in]	a	vector to print
[in]	name	name of the vector

Returns

string representation of vector

{
    static char vec_str[100];  // static to ensure the string life extends the
                               // life of function
 
    snprintf(vec_str, 99, "vec(%s) = (%.3g)i + (%.3g)j + (%.3g)k\n", name, a->x,
             a->y, a->z);
    return vec_str;
}

unit_vec()

vec_3d unit_vec

(

const vec_3d *

a

)

Obtain unit vector in the same direction as given vector.

\[\hat{a}=\frac{\vec{a}}{\lVert\vec{a}\rVert}\]

Parameters

[in]

a

input vector

Returns

n unit vector in the direction of \(\vec{a}\)

{
    vec_3d n = {0};
 
    float norm = vector_norm(a);
    if (fabsf(norm) < EPSILON)
    {  // detect possible divide by 0
        return n;
    }
 
    if (norm != 1.F)  // perform division only if needed
    {
        n.x = a->x / norm;
        n.y = a->y / norm;
        n.z = a->z / norm;
    }
    return n;
}

Here is the call graph for this function:

vector_add()

vec_3d vector_add	(	const vec_3d *	a,
		const vec_3d *	b
	)

Add one vector to another.

\[ \vec{c}=\vec{a}+\vec{b}=\left(a_x+b_x\right)\hat{i}+ \left(a_y+b_y\right)\hat{j}+\left(a_z+b_z\right)\hat{k}\]

Parameters

[in]	a	vector to add to
[in]	b	vector to add

Returns

resultant vector

{
    vec_3d out;
#ifdef LIBQUAT_ARM
    arm_add_f32((float *)a, (float *)b, (float *)&out);
#else
    out.x = a->x + b->x;
    out.y = a->y + b->y;
    out.z = a->z + b->z;
#endif
 
    return out;
}

vector_norm()

float vector_norm

(

const vec_3d *

a

)

Compute the norm a vector.

\[\lVert\vec{a}\rVert = \sqrt{\vec{a}\cdot\vec{a}} \]

Parameters

[in]

a

input vector

Returns

norm of the given vector

{
    float n = dot_prod(a, a);
#ifdef LIBQUAT_ARM
    arm_sqrt_f32(*n, n);
#else
    n = sqrtf(n);
#endif
 
    return n;
}

Here is the call graph for this function:

vector_prod()

vec_3d vector_prod	(	const vec_3d *	a,
		const vec_3d *	b
	)

Compute the vector product of two 3d vectors.

\[\begin{align*} \vec{a}\times\vec{b} &= \begin{vmatrix} \hat{i} & \hat{j} & \hat{k}\\ a_x & a_y & a_z\\ b_x & b_y & b_z \end{vmatrix}\\ &= \left(a_yb_z-b_ya_z\right)\hat{i} - \left(a_xb_z-b_xa_z\right)\hat{j} + \left(a_xb_y-b_xa_y\right)\hat{k} \end{align*} \]

Parameters

[in]	a	first vector \(\vec{a}\)
[in]	b	second vector \(\vec{b}\)

Returns

resultant vector \(\vec{o}=\vec{a}\times\vec{b}\)

{
    vec_3d out;  // better this way to avoid copying results to input
                 // vectors themselves
    out.x = a->y * b->z - a->z * b->y;
    out.y = -a->x * b->z + a->z * b->x;
    out.z = a->x * b->y - a->y * b->x;
 
    return out;
}

vector_sub()

vec_3d vector_sub	(	const vec_3d *	a,
		const vec_3d *	b
	)

Subtract one vector from another.

\[ \vec{c}=\vec{a}-\vec{b}=\left(a_x-b_x\right)\hat{i}+ \left(a_y-b_y\right)\hat{j}+\left(a_z-b_z\right)\hat{k}\]

Parameters

[in]	a	vector to subtract from
[in]	b	vector to subtract

Returns

resultant vector

{
    vec_3d out;
#ifdef LIBQUAT_ARM
    arm_sub_f32((float *)a, (float *)b, (float *)&out);
#else
    out.x = a->x - b->x;
    out.y = a->y - b->y;
    out.z = a->z - b->z;
#endif
 
    return out;
}