vector.cpp File Reference

#include "vector.h"
#include "matrix.h"

Include dependency graph for vector.cpp:

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

Functions
double	sep_rv (rvector v1, rvector v2)
	Angular separation between row vectors. More...
svector	s_convert (rvector from)
	Convert rvector to svector. More...
rvector	rv_convert (svector from)
	Convert svector to rvector. More...
rvector	rv_zero ()
	Zero row order vector. More...
rvector	rv_unitx (double scale)
	Scaled x row vector. More...
rvector	rv_unity (double scale)
	Scaled y row vector. More...
rvector	rv_unitz (double scale)
	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_shortest (rvector v)
	Shortest vector. More...
rvector	rv_shortest2 (rvector v)
rvector	rv_normal (rvector v)
	Normalize row order vector. More...
void	normalize_rv (rvector &v)
	Normalize row order vector in place. More...
double	normVector3 (double x, double y, double z)
	basic function to compute the L2 norm of a 3d generic vector with separate entries More...
void	normalizeVector3 (double &x, double &y, double &z)
	basic function to normalize any 3d vector with separate entries More...
rvector	rv_smult (double a, rvector b)
	Multiply row vector by scalar. 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_div (rvector a, rvector b)
	Divide two row vectors. More...
rvector	rv_mult (rvector a, rvector b)
	Multiply two row vectors. More...
rvector	rv_cross (rvector a, rvector b)
	Take cross product of two row vectors. More...
double	dot_rv (rvector a, rvector b)
	Dot product of two row vectors. More...
double	sep_cv (cvector v1, cvector v2)
	Angular separation between vectors. 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...
void	normalize_cv (cvector &v)
	Normalize cartesian vector in place, i.e. divides it by its own norm. More...
cvector	cv_smult (double a, cvector b)
	Multiply vector by scalar. 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_div (cvector a, cvector b)
	Divide two vectors. More...
cvector	cv_mult (cvector a, cvector b)
	Multiply two vectors. More...
cvector	cv_cross (cvector a, cvector b)
	Take cross product of two vectors. More...
double	dot_cv (cvector a, cvector b)
double	length_cv (cvector v)
double	cv_norm (cvector v)
double	norm_cv (cvector v)
double	sum_cv (cvector vec)
cvector	cv_sqrt (cvector a)
bool	equal_rv (rvector v1, rvector v2)
	Boolean equate of row vetor. More...
double	length_rv (rvector v)
	Length of row vector. More...
double	norm_rv (rvector vec)
	Infinite norm of row vector. More...
double	sum_rv (rvector vec)
	Sum elements of a row vector. More...
rvector	rv_sqrt (rvector vec)
	Row vector square root. More...
std::ostream &	operator<< (std::ostream &out, const rvector &a)
std::ostream &	operator<< (std::ostream &out, const vector< rvector > &a)
std::istream &	operator>> (std::istream &in, rvector &a)
rvector	operator* (rvector v, double scalar)
rvector	operator* (double scalar, rvector v)
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 cvector &a)
std::istream &	operator>> (std::istream &in, cvector &a)
std::ostream &	operator<< (std::ostream &out, const svector &a)
std::istream &	operator>> (std::istream &in, svector &a)
std::ostream &	operator<< (std::ostream &out, const gvector &a)
std::istream &	operator>> (std::istream &in, gvector &a)
std::ostream &	operator<< (std::ostream &out, const avector &a)
std::istream &	operator>> (std::istream &in, avector &a)
gvector	gv_zero ()
	Zero geodetic vector. More...
double	norm_q (quaternion q)
void	normalize_q (quaternion *q)
double	sep_q (quaternion q1, quaternion q2)
	Angular separation between quaternions. More...
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...
double	inner_q (quaternion q1, quaternion q2)
	Inner product of two 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 b)
	Multiply quaternion by scalar. More...
quaternion	q_add (quaternion a, quaternion b)
	Add two quaternions. More...
quaternion	q_sub (quaternion a, quaternion b)
	Subtract two quaternions. More...
void	qrotate (double ipos[3], double rpos[3], double angle, double *opos)
quaternion	q_euler2quaternion (avector rpw)
avector	a_quaternion2euler (quaternion q)
quaternion	q_axis2quaternion_cv (cvector v)
quaternion	q_eye ()
	Identity quaternion. More...
quaternion	q_identity ()
double	length_q (quaternion q)
	Length of quaternion. More...
cvector	cv_quaternion2axis (quaternion q)
quaternion	q_change_around_cv (cvector around, double angle)
	Create rotation quaternion from 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_change_around (int axis, double angle)
	Rotation quaternion for indicated axis. More...
std::ostream &	operator<< (std::ostream &out, const quaternion &a)
std::istream &	operator>> (std::istream &in, quaternion &a)
std::ostream &	operator<< (std::ostream &out, const qcomplex &a)
std::istream &	operator>> (std::istream &in, qcomplex &a)
std::ostream &	operator<< (std::ostream &out, const qlast &a)
std::istream &	operator>> (std::istream &in, qlast &a)
std::ostream &	operator<< (std::ostream &out, const qfirst &a)
std::istream &	operator>> (std::istream &in, qfirst &a)
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* (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::operator* (double scale, const Quaternion &q)
	Reverse scalar product. More...
Quaternion	Cosmos::Math::Quaternions::drotate_between (Vectors::Vector a, Vectors::Vector b)
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_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 &	in,
		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*	(	double	scalar,
		rvector	v
	)

{
    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 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;
}

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

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

std::istream& operator>>	(	std::istream &	in,
		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 &	in,
		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 &	in,
		avector &	a
	)

{
    char junk;
    in >> junk >> a.h >> junk >> a.e >> junk >> a.b >> 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);
}

double norm_q

(

quaternion

q

)

{
    return length_q(q);
}

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;
    }
}

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);
}

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);
}

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;
}

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);
}

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));

}

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);
}

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);
}

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_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);
}

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);
}

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_change_around	(	int	axis,
		double	angle
	)

Rotation quaternion for indicated axis.

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

Parameters

axis	Axis of rotation: 1=X, 2=Y, 3=Z
angle	Angle of rotation in radians

Returns

Resulting quaternion

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

    switch (axis)
    {
    case 1:
        q_change_around_x(angle);
        break;
    case 2:
        q_change_around_y(angle);
        break;
    case 3:
        q_change_around_z(angle);
        break;
    }

    return (a);
}

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 &	in,
		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 &	in,
		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 &	in,
		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 &	in,
		qfirst &	a
	)

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