vector.h File Reference

#include "support/configCosmos.h"
#include "constants.h"
#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>

Include dependency graph for vector.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	rvector
	3 element generic row vector More...
class	cvector
	3 element cartesian vector More...
struct	svector
	3 element spherical vector More...
struct	gvector
	3 element geodetic vector More...
struct	avector
	3 element attitude vector. More...
struct	quaternion
	Quaternion, scalar last, using x, y, z. More...
struct	qcomplex
	Quaternion, scalar last, using imaginary elements. More...
struct	qlast
	Quaternion, scalar last, using vector elements. More...
struct	qfirst
	Quaternion, scalar first using vector elements. More...
class	Cosmos::Math::Vectors::Vector
	Vector Class. More...
class	Cosmos::Math::Vectors::geodeticVector
class	Cosmos::Math::Vectors::sphericalVector
class	Cosmos::Math::Vectors::eulerVector
class	Cosmos::Math::Quaternions::Quaternion

Namespaces
	Cosmos
	Cosmos::Math
	Cosmos::Math::Vectors
	Cosmos::Math::Quaternions

Functions
std::ostream &	operator<< (std::ostream &out, const rvector &a)
std::ostream &	operator<< (std::ostream &out, const vector< rvector > &a)
std::istream &	operator>> (std::istream &out, rvector &a)
rvector	operator* (rvector v, double scalar)
rvector	operator* (rvector v1, rvector v2)
rvector	operator/ (rvector v, double scalar)
bool	operator== (rvector a, rvector b)
bool	operator!= (rvector a, rvector b)
std::ostream &	operator<< (std::ostream &out, const svector &a)
std::istream &	operator>> (std::istream &out, svector &a)
std::ostream &	operator<< (std::ostream &out, const gvector &a)
std::istream &	operator>> (std::istream &out, gvector &a)
std::ostream &	operator<< (std::ostream &out, const avector &a)
std::istream &	operator>> (std::istream &out, avector &a)
std::ostream &	operator<< (std::ostream &out, const cvector &a)
std::istream &	operator>> (std::istream &in, cvector &a)
gvector	gv_zero ()
	Zero geodetic vector. More...
rvector	rv_zero ()
	Zero row order vector. More...
rvector	rv_shortest (rvector v)
	Shortest vector. More...
rvector	rv_shortest2 (rvector v)
rvector	rv_unitx (double scale=1.)
	Scaled x row vector. More...
rvector	rv_unity (double scale=1.)
	Scaled y row vector. More...
rvector	rv_unitz (double scale=1.)
	Scaled z row vector. More...
rvector	rv_one ()
	Row vector of ones. More...
rvector	rv_one (double x, double y, double z)
	Row vector of values. More...
rvector	rv_smult (double a, rvector b)
	Multiply row vector by scalar. More...
rvector	rv_normal (rvector v)
	Normalize row order vector. More...
rvector	rv_normalto (rvector p0, rvector p1, rvector p2)
	Normal to a polygon. More...
rvector	rv_sadd (double a, rvector b)
	Add scalar to each element of vector. More...
rvector	rv_add (rvector a, rvector b)
	Add two row vectors. More...
rvector	rv_sub (rvector a, rvector b)
	Subtract two vectors. More...
rvector	rv_mult (rvector a, rvector b)
	Multiply two row vectors. More...
rvector	rv_div (rvector a, rvector b)
	Divide two row vectors. More...
rvector	rv_sqrt (rvector a)
	Row vector square root. More...
rvector	rv_cross (rvector a, rvector b)
	Take cross product of two row vectors. More...
rvector	rv_convert (svector from)
	Convert svector to rvector. More...
double	norm_rv (rvector a)
	Infinite norm of row vector. More...
void	normalize_rv (rvector &v)
	Normalize row order vector in place. More...
double	sep_rv (rvector v1, rvector v2)
	Angular separation between row vectors. More...
double	dot_rv (rvector a, rvector b)
	Dot product of two row vectors. More...
double	length_rv (rvector v)
	Length of row vector. More...
bool	equal_rv (rvector v1, rvector v2)
	Boolean equate of row vetor. More...
double	sum_rv (rvector a)
	Sum elements of a row vector. More...
svector	s_convert (rvector from)
	Convert rvector to svector. More...
cvector	cv_zero ()
	Zero cartesian vector. More...
cvector	cv_unitx ()
	Unit x vector. More...
cvector	cv_unity ()
	Unit y vector. More...
cvector	cv_unitz ()
	Unit z vector. More...
cvector	cv_one ()
	Vector of ones. More...
cvector	cv_normal (cvector v)
	Normalize cartesian vector. More...
cvector	cv_cross (cvector a, cvector b)
	Take cross product of two vectors. More...
cvector	cv_sadd (double a, cvector b)
	Add scalar to each element of vector. More...
cvector	cv_add (cvector a, cvector b)
	Add two vectors. More...
cvector	cv_sub (cvector a, cvector b)
	Subtract two vectors. More...
cvector	cv_mult (cvector a, cvector b)
	Multiply two vectors. More...
cvector	cv_div (cvector a, cvector b)
	Divide two vectors. More...
cvector	cv_smult (double a, cvector b)
	Multiply vector by scalar. More...
cvector	cv_sqrt (cvector a)
void	normalize_cv (cvector &v)
	Normalize cartesian vector in place, i.e. divides it by its own norm. More...
double	sep_cv (cvector v1, cvector v2)
	Angular separation between vectors. More...
double	dot_cv (cvector a, cvector b)
double	length_cv (cvector v)
double	norm_cv (cvector v)
double	cv_norm (cvector v)
double	sum_cv (cvector a)
std::ostream &	operator<< (std::ostream &out, const quaternion &a)
std::istream &	operator>> (std::istream &out, quaternion &a)
std::ostream &	operator<< (std::ostream &out, const qcomplex &a)
std::istream &	operator>> (std::istream &out, qcomplex &a)
std::ostream &	operator<< (std::ostream &out, const qlast &a)
std::istream &	operator>> (std::istream &out, qlast &a)
std::ostream &	operator<< (std::ostream &out, const qfirst &a)
std::istream &	operator>> (std::istream &out, qfirst &a)
cvector	cv_quaternion2axis (quaternion q)
void	normalize_q (quaternion *q)
quaternion	q_zero ()
	Zero quaternion. More...
quaternion	q_conjugate (quaternion q)
quaternion	q_times (quaternion q1, quaternion q2)
	Multiply the elements of 2 quaternions. More...
quaternion	q_sqrt (quaternion q1)
	Square root of the elements of a quaternion. More...
quaternion	q_fmult (rvector r1, quaternion q2)
	rvector quaternion multiply More...
quaternion	q_fmult (quaternion q1, quaternion q2)
quaternion	q_mult (quaternion q1, quaternion q2)
quaternion	q_rmult (quaternion q1, quaternion q2)
quaternion	q_smult (double a, quaternion q)
	Multiply quaternion by scalar. More...
quaternion	q_add (quaternion q1, quaternion q2)
	Add two quaternions. More...
quaternion	q_sub (quaternion q1, quaternion q2)
	Subtract two quaternions. More...
quaternion	q_euler2quaternion (avector rpw)
quaternion	q_axis2quaternion_cv (cvector v)
quaternion	q_axis2quaternion_rv (rvector v)
	Row vector axis and angle to Quaternion. More...
quaternion	q_drotate_between_rv (rvector from, rvector to)
	Create rotation quaternion from 2 row vectors. More...
quaternion	q_change_around_cv (cvector around, double angle)
	Create rotation quaternion from axis and angle. More...
quaternion	q_change_around_rv (rvector around, double angle)
	Create rotation quaternion from row vector axis and angle. More...
quaternion	q_change_around_x (double angle)
	Rotation quaternion for X axis. More...
quaternion	q_change_around_y (double angle)
	Rotation quaternion for Y axis. More...
quaternion	q_change_around_z (double angle)
	Rotation quaternion for Z axis. More...
quaternion	q_irotate_for (rvector sourcea, rvector sourceb, rvector targeta, rvector targetb)
	Create irotate quaternion from two orthogonal vectors. More...
quaternion	q_eye ()
	Identity quaternion. More...
quaternion	q_identity ()
quaternion	q_evaluate_poly (double x, vector< vector< double > > parms)
	Evaluate quaternion polynomial. More...
quaternion	q_evaluate_poly_slope (double x, vector< vector< double > > parms)
	Evaluate quaternion polynomial slope. More...
quaternion	q_evaluate_poly_accel (double x, vector< vector< double > > parms)
	Evaluate quaternion polynomial acceleration. More...
quaternion	q_evaluate_poly_jerk (double x, vector< vector< double > > parms)
	Evaluate quaternion polynomial jerk. More...
double	length_q (quaternion q)
	Length of quaternion. More...
double	norm_q (quaternion q)
double	sep_q (quaternion q1, quaternion q2)
	Angular separation between quaternions. More...
double	inner_q (quaternion q1, quaternion q2)
	Inner product of two quaternions. More...
void	qrotate (double ipos[3], double rpos[3], double angle, double *opos)
avector	a_quaternion2euler (quaternion q)
quaternion	q_change_between_cv (cvector from, cvector to)
	Create rotation quaternion from 2 vectors. More...
quaternion	q_change_between_rv (rvector from, rvector to)
Vector	Cosmos::Math::Vectors::operator* (const double scale, const Vector &v)
	Reverse scalar product. More...
std::ostream &	Cosmos::Math::Vectors::operator<< (std::ostream &out, const Vector &v)
Vector	Cosmos::Math::Vectors::eye (double scale)
Vector	Cosmos::Math::Vectors::unitxV (double scale)
Vector	Cosmos::Math::Vectors::unityV (double scale)
Vector	Cosmos::Math::Vectors::unitzV (double scale)
Quaternion	Cosmos::Math::Quaternions::operator* (double scale, const Quaternion &q)
	Reverse scalar product. More...
Quaternion	Cosmos::Math::Quaternions::operator* (const Vectors::Vector &v, const Quaternion &q)
std::ostream &	Cosmos::Math::Quaternions::operator<< (std::ostream &os, const Quaternion &q)
	Scalar division. More...
Quaternion	Cosmos::Math::Quaternions::irotate_for (Vectors::Vector sourcea, Vectors::Vector sourceb, Vectors::Vector targeta, Vectors::Vector targetb)
	Create irotate quaternion from two orthogonal vectors. More...
Quaternion	Cosmos::Math::Quaternions::drotate_between (Vectors::Vector a, Vectors::Vector b)
Quaternion	Cosmos::Math::Quaternions::drotate_around_x (double angle)
	Rotation Quaternion for X axis. More...
Quaternion	Cosmos::Math::Quaternions::drotate_around_y (double angle)
	Rotation Quaternion for Y axis. More...
Quaternion	Cosmos::Math::Quaternions::drotate_around_z (double angle)
	Rotation Quaternion for Z axis. More...
Quaternion	Cosmos::Math::Quaternions::drotate_around (int axis, double angle)
	Rotation Quaternion for indicated axis. More...
Quaternion	Cosmos::Math::Quaternions::eye (double scale)

Function Documentation

std::ostream& operator<<	(	std::ostream &	out,
		const rvector &	a
	)

{

    //    // fancy formating
    //    //out.precision(15);
    //    out << std::fixed;
    //    out << std::setprecision(6);
    //    out<< "["
    //       << std::setw(10) << a.col[0] << ","
    //       << std::setw(10) << a.col[1] << ","
    //       << std::setw(10) << a.col[2];
    //       << "]";

    //    // remove formating for floatfield (not set)
    //    std::cout.unsetf ( std::ios::floatfield );

    // simple formating
    out << a.col[0] << ","
                    << a.col[1] << ","
                    << a.col[2];

    return out;
}

std::ostream& operator<<	(	std::ostream &	out,
		const vector< rvector > &	a
	)

{
    for(vector<rvector>::const_iterator I = a.begin(); I != a.end(); ++I)
        out<<*I<<"\t";
    return out;
}

std::istream& operator>>	(	std::istream &	out,
		rvector &	a
	)

{
    char junk;
    in >> junk >> a.col[0] >> junk >> a.col[1] >> junk >> a.col[2] >> junk;
    return in;
}

rvector operator*	(	rvector	v,
		double	scalar
	)

{
    return rv_smult(scalar, v);
}

rvector operator*	(	rvector	v1,
		rvector	v2
	)

{
    return rv_mult(v1, v2);
}

rvector operator/	(	rvector	v,
		double	scalar
	)

{
    return rv_smult(1./scalar, v);
}

bool operator==	(	rvector	a,
		rvector	b
	)

{
    if(a.col[0] == b.col[0] && a.col[1] == b.col[1] && a.col[2] == b.col[2])
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool operator!=	(	rvector	a,
		rvector	b
	)

{
    if(a.col[0] != b.col[0] || a.col[1] != b.col[1] || a.col[2] != b.col[2])
    {
        return true;
    }
    else
    {
        return false;
    }
}

std::ostream& operator<<	(	std::ostream &	out,
		const svector &	a
	)

{
    out << "[" << a.phi << "," << a.lambda << "," << a.r << "]";
    return out;
}

std::istream& operator>>	(	std::istream &	out,
		svector &	a
	)

{
    char junk;
    in >> junk >> a.phi >> junk >> a.lambda >> junk >> a.r >> junk;
    return in;
}

std::ostream& operator<<	(	std::ostream &	out,
		const gvector &	a
	)

{
    out<<"["<<a.lat<<","<<a.lon<<","<<a.h<<"]";
    return out;
}

std::istream& operator>>	(	std::istream &	out,
		gvector &	a
	)

{
    char junk;
    in >> junk >> a.lat >> junk >> a.lon >> junk >> a.h >> junk;
    return in;
}

std::ostream& operator<<	(	std::ostream &	out,
		const avector &	a
	)

{
    out << "[" << a.h << "," << a.e << "," << a.b << "]";
    return out;
}

std::istream& operator>>	(	std::istream &	out,
		avector &	a
	)

{
    char junk;
    in >> junk >> a.h >> junk >> a.e >> junk >> a.b >> junk;
    return in;
}

std::ostream& operator<<	(	std::ostream &	out,
		const cvector &	a
	)

{
    out << "[" << a.x << "," << a.y << "," << a.z << "]";
    return out;
}

std::istream& operator>>	(	std::istream &	in,
		cvector &	a
	)

{
    char junk;
    in >> junk >> a.x >> junk >> a.y >> junk >> a.z >> junk;
    return in;
}

gvector gv_zero

(

)

Zero geodetic vector.

Creates a zero length geodetic vector.

Returns

a gvector of zero length

{
    gvector v={0.,0.,0.};

    return (v);
}

std::ostream& operator<<	(	std::ostream &	out,
		const quaternion &	a
	)

{
    //out << std::fixed;
    //out << std::setprecision(5);
    //out<< "[("
    //out << std::setw(6);
    out << a.d.x << ",";
    //out << std::setw(6);
    out << a.d.y << ",";
    //out << std::setw(6);
    out << a.d.z << ", ";
    //out << std::setw(6)
    out << a.w;
    //   << "]";

    // remove formating for floatfield (not set)
    // std::cout.unsetf ( std::ios::floatfield );

    return out;
}

std::istream& operator>>	(	std::istream &	out,
		quaternion &	a
	)

{
    char junk;
    in >> junk >> a.d.x >> junk >> a.d.y >> junk >> a.d.z >> junk >> a.w >> junk;
    return in;
}

std::ostream& operator<<	(	std::ostream &	out,
		const qcomplex &	a
	)

{
    out<< "[" << a.i << "," << a.j << "," << a.k << "," << a.r << "]";
    return out;
}

std::istream& operator>>	(	std::istream &	out,
		qcomplex &	a
	)

{
    char junk;
    in >> junk >> a.i >> junk >> a.j >> junk >> a.k >> junk >> a.r >> junk;
    return in;
}

std::ostream& operator<<	(	std::ostream &	out,
		const qlast &	a
	)

{
    out << "[" << a.q1 << "," << a.q2 << "," << a.q3 << "," << a.q4 << "]";
    return out;
}

std::istream& operator>>	(	std::istream &	out,
		qlast &	a
	)

{
    char junk;
    in >> junk >> a.q1 >> junk >> a.q2 >> junk >> a.q3 >> junk >> a.q4 >> junk;
    return in;
}

std::ostream& operator<<	(	std::ostream &	out,
		const qfirst &	a
	)

{
    out << "[" << a.q0 << "," << a.q1 << "," << a.q2 << "," << a.q3 << "]";
    return out;
}

std::istream& operator>>	(	std::istream &	out,
		qfirst &	a
	)

{
    char junk;
    in >> junk >> a.q0 >> junk >> a.q1 >> junk >> a.q2 >> junk >> a.q3 >> junk;
    return in;
}

cvector cv_quaternion2axis

(

quaternion

q

)

{
    cvector v;
    double ca, sa;

    ca = 2.*acos(q.w);
    if (ca > 0.)
    {
        sa = sin(ca/2.);
        v.x = ca*(q.d.x/sa);
        v.y = ca*(q.d.y/sa);
        v.z = ca*(q.d.z/sa);
    }
    else
        v = {0.,0.,0.};

    return (v);
}

void normalize_q

(

quaternion *

q

)

{
    double mag;

    q->w = round(q->w/D_SMALL)*D_SMALL;
    q->d.x = round(q->d.x/D_SMALL)*D_SMALL;
    q->d.y = round(q->d.y/D_SMALL)*D_SMALL;
    q->d.z = round(q->d.z/D_SMALL)*D_SMALL;

    mag = q->w * q->w + q->d.x * q->d.x + q->d.y * q->d.y + q->d.z * q->d.z;

    if (fabs(mag - (double)0.) > D_SMALL && fabs(mag - (double)1.) > D_SMALL)
    {
        mag = sqrt(mag);
        q->w /= mag;
        q->d.x /= mag;
        q->d.y /= mag;
        q->d.z /= mag;
    }
}

quaternion q_zero

(

)

Zero quaternion.

Creates a quaternion filled with zeros.

Returns

a quaternion of zeros

{
    quaternion q={{0.,0.,0.},0.};

    return (q);
}

quaternion q_conjugate

(

quaternion

q

)

{
    quaternion o;

    o.w = q.w;
    o.d.x = -q.d.x;
    o.d.y = -q.d.y;
    o.d.z = -q.d.z;

    return (o);
}

quaternion q_times	(	quaternion	q1,
		quaternion	q2
	)

Multiply the elements of 2 quaternions.

Multiply each element of one quaternion by the same element of the other.

Parameters

q1	First quaternion
q2	second quaternion

Returns

Quaternion result

{
    quaternion o;

    o.d.x = q1.d.x * q2.d.x;
    o.d.y = q1.d.y * q2.d.y;
    o.d.z = q1.d.z * q2.d.z;
    o.w = q1.w * q2.w;

    return (o);
}

quaternion q_sqrt

(

quaternion

q1

)

Square root of the elements of a quaternion.

Take the square root of each element of one quaternion.

Parameters

q1	quaternion

Returns

Quaternion result

{
    quaternion o;

    o.d.x = sqrt(q1.d.x);
    o.d.y = sqrt(q1.d.y);
    o.d.z = sqrt(q1.d.z);
    o.w = sqrt(q1.w);

    return (o);
}

quaternion q_fmult	(	rvector	r1,
		quaternion	q2
	)

rvector quaternion multiply

Treat an rvector as a quaternion with scalar set to zero, then quaternion multiply the rvector by the quaternion.

Parameters

r1	rvector as first quaternion.
q2	second quaternion

Returns

Quaternion result

{
    quaternion o;

    o.d.x = r1.col[0] * q2.w + r1.col[1] * q2.d.z - r1.col[2] * q2.d.y;
    o.d.y = r1.col[1] * q2.w + r1.col[2] * q2.d.x - r1.col[0] * q2.d.z;
    o.d.z = r1.col[2] * q2.w + r1.col[0] * q2.d.y - r1.col[1] * q2.d.x;
    o.w = - r1.col[0] * q2.d.x - r1.col[1] * q2.d.y - r1.col[2] * q2.d.z;

    return (o);
}

quaternion q_fmult	(	quaternion	q1,
		quaternion	q2
	)

Quaternion multiplication. The result of multiplying two quaternions is the composition of two rotations (not commutative). This function creates a result that would behave like rotating first by q1, then by q2.

Quaternion multiply two quaternions.

Parameters

q1	First quaternion
q2	Second quaternion

Returns

Quaternion result.

{
    quaternion o;

    o.d.x = q1.w * q2.d.x + q1.d.x * q2.w + q1.d.y * q2.d.z - q1.d.z * q2.d.y;
    o.d.y = q1.w * q2.d.y + q1.d.y * q2.w + q1.d.z * q2.d.x - q1.d.x * q2.d.z;
    o.d.z = q1.w * q2.d.z + q1.d.z * q2.w + q1.d.x * q2.d.y - q1.d.y * q2.d.x;
    o.w = q1.w * q2.w - q1.d.x * q2.d.x - q1.d.y * q2.d.y - q1.d.z * q2.d.z;

    return (o);
}

quaternion q_mult	(	quaternion	q1,
		quaternion	q2
	)

Quaternion multiplication. The result of multiplying two quaternions is the composition of two rotations (not commutative). This function creates a result that would behave like rotating first by q1, then by q2.

Quaternion multiply two quaternions.

Parameters

q1	First quaternion
q2	Second quaternion

Returns

Quaternion result.

{
    return q_fmult(q1, q2);
}

quaternion q_rmult	(	quaternion	q1,
		quaternion	q2
	)

Quaternion multiplication. The result of multiplying two quaternions is the composition of two rotations (not commutative). This function creates a result that would behave like rotating first by q2, then by q1. It is the conjugate of q_fmult().

Quaternion multiply two quaternions.

Parameters

q1	First quaternion
q2	Second quaternion

Returns

Quaternion result.

{
    quaternion o;

    o.d.x = q1.w * q2.d.x + q1.d.x * q2.w + q1.d.y * q2.d.z - q1.d.z * q2.d.y;
    o.d.y = q1.w * q2.d.y + q1.d.y * q2.w + q1.d.z * q2.d.x - q1.d.x * q2.d.z;
    o.d.z = q1.w * q2.d.z + q1.d.z * q2.w + q1.d.x * q2.d.y - q1.d.y * q2.d.x;
    o.w = -q1.w * q2.w + q1.d.x * q2.d.x + q1.d.y * q2.d.y + q1.d.z * q2.d.z;

    return (o);
}

quaternion q_smult	(	double	a,
		quaternion	b
	)

Multiply quaternion by scalar.

Multiply a 3 element quaternion by a double precision scalar.

Parameters

b	quaternion to be tranformed, in quaternion form
a	double precision scalar to multiply by

Returns

the transformed quaternion, in quaternion form

{
    quaternion c;

    c.w = a * b.w;
    c.d.x = a * b.d.x;
    c.d.y = a * b.d.y;
    c.d.z = a * b.d.z;
    return (c);
}

quaternion q_add	(	quaternion	a,
		quaternion	b
	)

Add two quaternions.

Add two quaternions in quaternion form, returning a quaternion.

Parameters

a	first quaternion to be added, in quaternion form
b	second quaternion to be added, in quaternion form

Returns

the transformed quaternion, in quaternion form

{
    quaternion c;

    c.w = a.w + b.w;
    c.d.x = a.d.x + b.d.x;
    c.d.y = a.d.y + b.d.y;
    c.d.z = a.d.z + b.d.z;
    return (c);
}

quaternion q_sub	(	quaternion	a,
		quaternion	b
	)

Subtract two quaternions.

Subtract two quaternions in quaternion form, returning a quaternion.

Parameters

a	quaternion to be subtracted from, in quaternion form
b	quaternion to be subtracted, in quaternion form

Returns

the transformed quaternion, in quaternion form

{
    quaternion c;

    c.w = a.w - b.w;
    c.d.x = a.d.x - b.d.x;
    c.d.y = a.d.y - b.d.y;
    c.d.z = a.d.z - b.d.z;
    return (c);
}

quaternion q_euler2quaternion

(

avector

rpw

)

{
    quaternion q;
    double sr, sp, sy, cr, cp, cy;

    sr = sin(rpw.b/2.);
    sp = sin(rpw.e/2.);
    sy = sin(rpw.h/2.);
    cr = cos(rpw.b/2.);
    cp = cos(rpw.e/2.);
    cy = cos(rpw.h/2.);

    q.d.x = sr * cp * cy - cr * sp * sy;
    q.d.y = cr * sp * cy + sr * cp * sy;
    q.d.z = cr * cp * sy - sr * sp * cy;
    q.w = (cr * cp * cy + sr * sp * sy);

    normalize_q(&q);

    return (q);
}

quaternion q_axis2quaternion_cv

(

cvector

v

)

{
    double length, s2;
    quaternion q;

    length = sqrt(v.x*v.x+v.y*v.y+v.z*v.z);
    s2 = sin(length/2.)/length;
    if (length)
    {
        q.d.x = s2*v.x;
        q.d.y = s2*v.y;
        q.d.z = s2*v.z;
    }
    else
        q.d.x = q.d.y = q.d.z = 0.;
    q.w =cos(length/2);

    normalize_q(&q);
    return (q);
}

quaternion q_change_around_cv	(	cvector	around,
		double	angle
	)

Create rotation quaternion from axis and angle.

Generate the quaternion that represents a rotation of the specified angle around the specified axis.

Parameters

around	cartesian vector around which the rotation will occur
angle	amount of rotation in radians

Returns

quaternion that can be used to rotate points

{
    double sa;
    quaternion rq;

    angle /= 2.;
    sa = sin(angle);

    // normalize vector
    // before it was normalize_cv(around);

    double mag = around.x*around.x
                 + around.y*around.y
                 + around.z*around.z;

    if (fabs(mag - (double)0.) > D_SMALL && fabs(mag - (double)1.) > D_SMALL)
    {
        mag = sqrt(mag);
        around.x /= mag;
        around.y /= mag;
        around.z /= mag;
    }

    rq.d.x = around.x * sa;
    rq.d.y = around.y * sa;
    rq.d.z = around.z * sa;
    rq.w = cos(angle);
    normalize_q(&rq);
    return (rq);
}

quaternion q_change_around_x

(

double

angle

)

Rotation quaternion for X axis.

Create the quaternion that represents a rotation of the given angle around the X axis.

Parameters

angle

Angle of rotation in radians

Returns

Resulting quaternion

{
    quaternion a = {{1.,0.,0.},0.};

    // removed cv_mult to clean the dependency from mathlib
    // previously was: a.d = cv_smult(sin(angle/2.),a.d);

    // new
    double sa = sin(angle/2.);

    a.d.x = sa * a.d.x;
    a.d.y = sa * a.d.y;
    a.d.z = sa * a.d.z;

    a.w = cos(angle/2.);

    return (a);
}

quaternion q_change_around_y

(

double

angle

)

Rotation quaternion for Y axis.

Create the quaternion that represents a rotation of the given angle around the Y axis.

Parameters

angle

Angle of rotation in radians

Returns

Resulting quaternion

{
    quaternion a = {{0.,1.,0.},0.};

    // removed cv_mult to clean the dependency from mathlib
    // previously was: a.d = cv_smult(sin(angle/2.),a.d);

    // new
    double sa = sin(angle/2.);

    a.d.x = sa * a.d.x;
    a.d.y = sa * a.d.y;
    a.d.z = sa * a.d.z;

    a.w = cos(angle/2.);

    return (a);
}

quaternion q_change_around_z

(

double

angle

)

Rotation quaternion for Z axis.

Create the quaternion that represents a rotation of the given angle around the Z axis.

Parameters

angle

Angle of rotation in radians

Returns

Resulting quaternion

{
    quaternion a = {{0.,0.,1.},0.};

    // removed cv_mult to clean the dependency from mathlib
    // previously was: a.d = cv_smult(sin(angle/2.),a.d);

    // new
    double sa = sin(angle/2.);

    a.d.x = sa * a.d.x;
    a.d.y = sa * a.d.y;
    a.d.z = sa * a.d.z;
    a.w = cos(angle/2.);

    return (a);
}

quaternion q_eye

(

)

Identity quaternion.

Returns a quaternion that will cause no rotation when multiplied by a vector.

Returns

Identity quaternion

{
    quaternion q = {{0.,0.,0.},1.};

    return (q);
}

quaternion q_identity

(

)

{
    return q_eye();
}

double length_q

(

quaternion

q

)

Length of quaternion.

Calculate the length of a quaternion by summing the squares of its elements.

Parameters

q	Quaternion to find the length of.

Returns

Length of quaternion.

{
    double length;

    length = q.w * q.w + q.d.x * q.d.x + q.d.y * q.d.y + q.d.z * q.d.z;
    length = sqrt(length);

    if (length < D_SMALL)
        return (0.);
    else
        return (length);
}

double norm_q

(

quaternion

q

)

{
    return length_q(q);
}

double sep_q	(	quaternion	q1,
		quaternion	q2
	)

Angular separation between quaternions.

Calculates the separation angle between two quaternions, in radians.

Parameters

q1	the first quaternion
q2	the second quaternion

Returns

The separation angle in radians as a double precision

{

    normalize_q(&q1);
    normalize_q(&q2);

    double inner = inner_q(q1, q2);
    double sepangle = acos(2. * inner * inner - 1);
    return (sepangle);
}

double inner_q	(	quaternion	q1,
		quaternion	q2
	)

Inner product of two quaternions.

Multiply each element of one quaternion by the same element of the other, and sum.

Parameters

q1	First quaternion
q2	second quaternion

Returns

Double result

{
    quaternion q = q_times(q1, q2);
    double result = q.d.x + q.d.y + q.d.z + q.w;

    return result;
}

void qrotate	(	double	ipos[3],
		double	rpos[3],
		double	angle,
		double *	opos
	)

{
    double q1, q2, q3, q4, length, sa;
    double q11, q12, q13, q14, q22, q23, q24, q33, q34;

    length = sqrt(rpos[0]*rpos[0]+rpos[1]*rpos[1]+rpos[2]*rpos[2]);
    if (length>0.)
    {
        rpos[0] /= length;
        rpos[1] /= length;
        rpos[2] /= length;
    }

    sa = sin(angle/2.);
    q1 = rpos[0] * sa;
    q2 = rpos[1] * sa;
    q3 = rpos[2] * sa;
    q4 = cos(angle/2.);
    q11 = q1 * q1;
    q12 = q1 * q2;
    q13 = q1 * q3;
    q14 = q1 * q4;
    q22 = q2 * q2;
    q23 = q2 * q3;
    q24 = q2 * q4;
    q33 = q3 * q3;
    q34 = q3 * q4;

    opos[0] = ipos[0] *(1.-2.*(q22+q33)) + ipos[1] * 2.*(q12-q34) + ipos[2] * 2.*(q13+q24);
    opos[1] = ipos[0]*2.*(q12+q34)+ipos[1]*(1.-2.*(q11+q33))+ipos[2]*2.*(q23-q14);
    opos[2] = ipos[0]*2.*(q13-q24)+ipos[1]*2.*(q14+q23)+ipos[2]*(1.-2.*(q11+q22));

}

avector a_quaternion2euler

(

quaternion

q

)

{
    avector rpw;

    normalize_q(&q);
    /*
rpw.b = atan2(q.d.y*q.d.z+q.w*q.d.x,q.w*q.w+q.d.z*q.d.z-.5);
rpw.e = asin(-2.*(q.d.x*q.d.z-q.w*q.d.y));
rpw.h = atan2(q.d.x*q.d.y+q.w*q.d.z,q.w*q.w+q.d.x*q.d.x-.5);
*/
    //  double sqw = q.w * q.w;
    double sqx = q.d.x * q.d.x;
    double sqy = q.d.y * q.d.y;
    double sqz = q.d.z * q.d.z;
    rpw.b = atan2(2.*(q.d.z*q.d.y + q.d.x*q.w), 1. - 2.*(sqx + sqy));
    rpw.e = asin(-2.*(q.d.x*q.d.z - q.d.y*q.w));
    rpw.h = atan2(2.*(q.d.x*q.d.y + q.d.z*q.w), 1. - 2.*(sqy + sqz));

    return (rpw);
}