cqrlib.h | cqrlib.h | |||
---|---|---|---|---|
skipping to change at line 146 | skipping to change at line 146 | |||
return( y ); | return( y ); | |||
} | } | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline DistanceType GetZ( void ) const | inline DistanceType GetZ( void ) const | |||
{ | { | |||
return( z ); | return( z ); | |||
} | } | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR GetIm( void ) const | ||||
{ | ||||
return( CPPQR(0.,x,y,z) ); | ||||
} | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR GetAxis( void ) const | ||||
{ | ||||
DistanceType anorm=sqrt((double)(x*x + y*y + z*z)); | ||||
if (anorm < DBL_MIN) return(CPPQR(0.,0.,0.,0.)); | ||||
return( CPPQR(0.,x/anorm,y/anorm,z/anorm) ); | ||||
} | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline double GetAngle( void ) const | ||||
{ | ||||
double cosangle, sinangle; | ||||
DistanceType radius, anorm; | ||||
radius = sqrt( (double) (w*w + x*x + y*y + z*z) ); | ||||
anorm = sqrt( (double) (x*x + y*y + z*z) ); | ||||
if ( anorm <= DBL_MIN) { | ||||
return 0.; | ||||
} | ||||
cosangle = w/radius; | ||||
sinangle = anorm/radius; | ||||
return(atan2(sinangle,cosangle)); | ||||
} | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR log( void ) const | ||||
{ | ||||
CPPQR axis = (*this).GetAxis(); | ||||
double PI; | ||||
double ipnormsq; | ||||
double angle = (*this).GetAngle(); | ||||
if ( w < 0. ) { | ||||
ipnormsq = (double) (x*x + y*y + z*z); | ||||
if (ipnormsq <= DBL_MIN ) { | ||||
PI = std::atan2(1.,1.)*4.; | ||||
axis = CPPQR(0.,1.,0.,0.); | ||||
angle = PI; | ||||
} | ||||
} | ||||
return (CPPQR(std::log((double)((*this).Norm())),axis.x*angle,axis.y*an | ||||
gle,axis.z*angle)); | ||||
} | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR exp( void ) const | ||||
{ | ||||
const CPPQR impart = (*this).GetIm( ); | ||||
const double angle = (double)impart.Norm(); | ||||
if (angle <= DBL_MIN) { | ||||
return (CPPQR(cos(angle)*std::exp(w),0.,0.,0.)); | ||||
} | ||||
const double rat = std::exp(w)*sin(angle)/angle; | ||||
return(CPPQR(cos(angle)*std::exp(w),rat*x,rat*y,rat*z)); | ||||
} | ||||
template <typename powertype> | ||||
inline CPPQR pow( const powertype p) const { | ||||
return(((*this).log()*p).exp()); | ||||
} | ||||
inline CPPQR pow( const int p) const { | ||||
CPPQR qtemp, qaccum; | ||||
unsigned int ptemp; | ||||
if ( p == 0 ) return (CPPQR(1.0,0.,0.,0.)); | ||||
else if ( p > 0 ) { | ||||
qtemp = *this; | ||||
ptemp = p; | ||||
} else { | ||||
qtemp = (*this).Inverse(); | ||||
ptemp = -p; | ||||
} | ||||
qaccum = CPPQR(1.0,0.,0.,0.); | ||||
while(1) { | ||||
if ((ptemp&1)!= 0) { | ||||
qaccum *= qtemp; | ||||
} | ||||
ptemp >>= 1; | ||||
if (ptemp==0) break; | ||||
qtemp *= qtemp; | ||||
} | ||||
return qaccum; | ||||
} | ||||
/* root -- take the given integer root of a quaternion, returning | ||||
the indicated mth choice from among multiple roots. | ||||
For reals the cycle runs through first the i-based | ||||
roots, then the j-based roots and then the k-based roots, | ||||
out of the infinite number of possible roots of negative reals. */ | ||||
inline CPPQR root( const int r, const int m) const | ||||
{ | ||||
const double PI = 4.*atan2(1.,1.); | ||||
CPPQR qlog,qlogoverr,qaxis; | ||||
double recip, qaxisnormsq; | ||||
int cycle; | ||||
if (r == 0) return CPPQR(DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX); | ||||
recip = 1./((double)r); | ||||
qlog = (*this).log(); | ||||
if (m != 0) { | ||||
qaxis =(*this).GetAxis(); | ||||
qaxisnormsq = qaxis.Normsq(); | ||||
if (qaxisnormsq <= DBL_MIN) { | ||||
cycle = (m/r)%3; | ||||
switch (cycle) { | ||||
case 1: | ||||
qaxis = CPPQR(0.,0.,1.,0.); | ||||
break; | ||||
case 2: | ||||
qaxis = CPPQR(0.,0.,0.,1.); | ||||
break; | ||||
default: | ||||
qaxis = CPPQR(0.,1.,0.,0.); | ||||
break; | ||||
} | ||||
} | ||||
qaxis *= 2.*PI*((double)m); | ||||
qlog += qaxis; | ||||
} | ||||
qlogoverr = qlog*recip; | ||||
return qlogoverr.exp(); | ||||
} | ||||
/* Dot product of 2 quaternions as 4-vectors */ | /* Dot product of 2 quaternions as 4-vectors */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline DistanceType Dot( const CPPQR& q) const | inline DistanceType Dot( const CPPQR& q) const | |||
{ | { | |||
return (w*q.w+x*q.x+y*q.y+z*q.z); | return (w*q.w+x*q.x+y*q.y+z*q.z); | |||
} | } | |||
/* Add -- add a quaternion (q1) to a quaternion (q2) */ | /* Add -- add a quaternion (q1) to a quaternion (q2) */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
skipping to change at line 173 | skipping to change at line 312 | |||
return( temp ); | return( temp ); | |||
} | } | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline CPPQR& operator+= ( const CPPQR& q ) | inline CPPQR& operator+= ( const CPPQR& q ) | |||
{ | { | |||
w += q.w; | w += q.w; | |||
x += q.x; | x += q.x; | |||
y += q.y; | y += q.y; | |||
z += q.z; | z += q.z; | |||
return *this; | ||||
} | } | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline CPPQR& operator-= ( const CPPQR& q ) | inline CPPQR& operator-= ( const CPPQR& q ) | |||
{ | { | |||
w -= q.w; | w -= q.w; | |||
x -= q.x; | x -= q.x; | |||
y -= q.y; | y -= q.y; | |||
z -= q.z; | z -= q.z; | |||
return *this; | ||||
} | } | |||
/* Subtract -- subtract a quaternion (q2) from a quaternion (q1) */ | /* Subtract -- subtract a quaternion (q2) from a quaternion (q1) */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline CPPQR operator- ( const CPPQR& q ) const | inline CPPQR operator- ( const CPPQR& q ) const | |||
{ | { | |||
CPPQR temp; | CPPQR temp; | |||
temp.w = w - q.w; | temp.w = w - q.w; | |||
temp.x = x - q.x; | temp.x = x - q.x; | |||
temp.y = y - q.y; | temp.y = y - q.y; | |||
skipping to change at line 220 | skipping to change at line 361 | |||
inline CPPQR operator* ( const CPPQR& q ) const // multiply two quaternions | inline CPPQR operator* ( const CPPQR& q ) const // multiply two quaternions | |||
{ | { | |||
CPPQR temp; | CPPQR temp; | |||
temp.w = -z*q.z - y*q.y - x*q.x + w*q.w; | temp.w = -z*q.z - y*q.y - x*q.x + w*q.w; | |||
temp.x = y*q.z - z*q.y + w*q.x + x*q.w; | temp.x = y*q.z - z*q.y + w*q.x + x*q.w; | |||
temp.y = -x*q.z + w*q.y + z*q.x + y*q.w; | temp.y = -x*q.z + w*q.y + z*q.x + y*q.w; | |||
temp.z = w*q.z + x*q.y - y*q.x + z*q.w; | temp.z = w*q.z + x*q.y - y*q.x + z*q.w; | |||
return( temp ); | return( temp ); | |||
} | } | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR& operator*= ( const CPPQR& q ) | ||||
{ | ||||
CPPQR temp; | ||||
temp.w = -z*q.z - y*q.y - x*q.x + w*q.w; | ||||
temp.x = y*q.z - z*q.y + w*q.x + x*q.w; | ||||
temp.y = -x*q.z + w*q.y + z*q.x + y*q.w; | ||||
temp.z = w*q.z + x*q.y - y*q.x + z*q.w; | ||||
w = temp.w; | ||||
x = temp.x; | ||||
y = temp.y; | ||||
z = temp.z; | ||||
return *this; | ||||
} | ||||
/* Divide -- Divide a quaternion (q1) by quaternion (q2) */ | /* Divide -- Divide a quaternion (q1) by quaternion (q2) */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline CPPQR operator/ ( const CPPQR& q2 ) const | inline CPPQR operator/ ( const CPPQR& q2 ) const | |||
{ | { | |||
const DistanceType norm2sq = q2.w*q2.w + q2.x*q2.x + q2.y*q2.y + q2.z*q 2.z; | const DistanceType norm2sq = q2.w*q2.w + q2.x*q2.x + q2.y*q2.y + q2.z*q 2.z; | |||
if ( norm2sq == 0.0 ) | if ( norm2sq == 0.0 ) | |||
{ | { | |||
return( CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ) ); | return( CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ) ); | |||
} | } | |||
CPPQR q; | CPPQR q; | |||
q.w = z*q2.z + y*q2.y + x*q2.x + w*q2.w; | q.w = z*q2.z + y*q2.y + x*q2.x + w*q2.w; | |||
q.x = -y*q2.z + z*q2.y - w*q2.x + x*q2.w; | q.x = -y*q2.z + z*q2.y - w*q2.x + x*q2.w; | |||
q.y = x*q2.z - w*q2.y - z*q2.x + y*q2.w; | q.y = x*q2.z - w*q2.y - z*q2.x + y*q2.w; | |||
q.z = -w*q2.z - x*q2.y + y*q2.x + z*q2.w; | q.z = -w*q2.z - x*q2.y + y*q2.x + z*q2.w; | |||
return( q / norm2sq ); | return( q / norm2sq ); | |||
} | } | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR& operator/= ( const CPPQR& q2 ) | ||||
{ | ||||
const DistanceType norm2sq = q2.w*q2.w + q2.x*q2.x + q2.y*q2.y + q2.z*q | ||||
2.z; | ||||
if ( norm2sq == 0.0 ) | ||||
{ | ||||
return( CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ) ); | ||||
} | ||||
CPPQR q; | ||||
q.w = z*q2.z + y*q2.y + x*q2.x + w*q2.w; | ||||
q.x = -y*q2.z + z*q2.y - w*q2.x + x*q2.w; | ||||
q.y = x*q2.z - w*q2.y - z*q2.x + y*q2.w; | ||||
q.z = -w*q2.z - x*q2.y + y*q2.x + z*q2.w; | ||||
w = q.w/norm2sq; | ||||
x = q.x/norm2sq; | ||||
y = q.y/norm2sq; | ||||
z = q.z/norm2sq; | ||||
return *this; | ||||
} | ||||
/* ScalarMultiply -- multiply a quaternion (q) by scalar (s) */ | /* ScalarMultiply -- multiply a quaternion (q) by scalar (s) */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline CPPQR operator* ( const DistanceType& d ) const // multiply by a con stant | inline CPPQR operator* ( const DistanceType& d ) const // multiply by a con stant | |||
{ | { | |||
CPPQR temp; | CPPQR temp; | |||
temp.w = w*d; | temp.w = w*d; | |||
temp.x = x*d; | temp.x = x*d; | |||
temp.y = y*d; | temp.y = y*d; | |||
temp.z = z*d; | temp.z = z*d; | |||
return( temp ); | return( temp ); | |||
} | } | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline CPPQR& operator*= ( const DistanceType& d ) | ||||
{ | ||||
w *= d; | ||||
x *= d; | ||||
y *= d; | ||||
z *= d; | ||||
return *this; | ||||
} | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR& operator/= ( const DistanceType& d ) | ||||
{ | ||||
if ( std::abs((double)d) <= DBL_MIN ) { | ||||
w = DBL_MAX; | ||||
x = DBL_MAX; | ||||
y = DBL_MAX; | ||||
z = DBL_MAX; | ||||
} else { | ||||
w /= d; | ||||
x /= d; | ||||
y /= d; | ||||
z /= d; | ||||
} | ||||
return *this; | ||||
} | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline CPPQR operator/ ( const DistanceType& d ) const // divide by a const ant | inline CPPQR operator/ ( const DistanceType& d ) const // divide by a const ant | |||
{ | { | |||
CPPQR temp; | CPPQR temp; | |||
temp.w = w/d; | temp.w = w/d; | |||
temp.x = x/d; | temp.x = x/d; | |||
temp.y = y/d; | temp.y = y/d; | |||
temp.z = z/d; | temp.z = z/d; | |||
return( temp ); | return( temp ); | |||
} | } | |||
skipping to change at line 282 | skipping to change at line 491 | |||
return( conjugate ); | return( conjugate ); | |||
} | } | |||
/* Normsq -- Form the normsquared of a quaternion */ | /* Normsq -- Form the normsquared of a quaternion */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline DistanceType Normsq ( void ) const | inline DistanceType Normsq ( void ) const | |||
{ | { | |||
return( w*w + x*x + y*y + z*z ); | return( w*w + x*x + y*y + z*z ); | |||
} | } | |||
/* Normsq -- Form the norm of a quaternion */ | /* Norm -- Form the norm of a quaternion */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline DistanceType Norm ( void ) const | inline DistanceType Norm ( void ) const | |||
{ | { | |||
return sqrt( w*w + x*x + y*y + z*z ); | return sqrt( w*w + x*x + y*y + z*z ); | |||
} | } | |||
/* Distsq -- Form the distance squared from a quaternion */ | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline DistanceType Distsq ( const CPPQR& q ) const | ||||
{ | ||||
return( (w-q.w)*(w-q.w) + (x-q.x)*(x-q.x) + (y-q.y)*(y-q.y) + (z-q.z)*( | ||||
z-q.z) ); | ||||
} | ||||
/* Dist -- Form the distance from a quaternion */ | ||||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | ||||
++++++++++++*/ | ||||
inline DistanceType Dist ( const CPPQR& q ) const | ||||
{ | ||||
return sqrt( (w-q.w)*(w-q.w) + (x-q.x)*(x-q.x) + (y-q.y)*(y-q.y) + (z-q | ||||
.z)*(z-q.z) ); | ||||
} | ||||
/* Inverse -- Form the inverse of a quaternion */ | /* Inverse -- Form the inverse of a quaternion */ | |||
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ++++++++++++*/ | |||
inline CPPQR Inverse ( void ) const | inline CPPQR Inverse ( void ) const | |||
{ | { | |||
CPPQR inversequaternion = (*this).Conjugate(); | CPPQR inversequaternion = (*this).Conjugate(); | |||
const DistanceType normsq = (*this).Normsq( ); | const DistanceType normsq = (*this).Normsq( ); | |||
if ( normsq > DistanceType( 0.0 ) ) | if ( normsq > DistanceType( 0.0 ) ) | |||
{ | { | |||
inversequaternion = inversequaternion * DistanceType( 1.0 ) / norms q; | inversequaternion = inversequaternion * DistanceType( 1.0 ) / norms q; | |||
skipping to change at line 1031 | skipping to change at line 1254 | |||
typedef struct { | typedef struct { | |||
double w; | double w; | |||
double x; | double x; | |||
double y; | double y; | |||
double z; } CQRQuaternion; | double z; } CQRQuaternion; | |||
typedef CQRQuaternion CQR_FAR * CQRQuaternionHandle; | typedef CQRQuaternion CQR_FAR * CQRQuaternionHandle; | |||
/* CQR Macros */ | /* CQR Macros */ | |||
#define CQRMIm(impart,q) \ | ||||
(impart).w = 0.; (impart).x = (q).x; (impart).y = (q).y; (impart).z = (q).z | ||||
; | ||||
#define CQRMCopy(copy,orig) \ | #define CQRMCopy(copy,orig) \ | |||
(copy).w = (orig).w; (copy).x = (orig).x; (copy).y = (orig).y; (copy).z = ( orig).z; | (copy).w = (orig).w; (copy).x = (orig).x; (copy).y = (orig).y; (copy).z = ( orig).z; | |||
#define CQRMSet(q,qw,qx,qy,qz) \ | #define CQRMSet(q,qw,qx,qy,qz) \ | |||
(q).w = (qw); (q).x = (qx); (q).y = (qy); (q).z = (qz); | (q).w = (qw); (q).x = (qx); (q).y = (qy); (q).z = (qz); | |||
#define CQRMAdd(sum,q1,q2) \ | #define CQRMAdd(sum,q1,q2) \ | |||
(sum).w = (q1).w + (q2).w; sum.x = (q1).x + (q2).x; sum.y = (q1).y + (q2).y ; sum.z = (q1).z + (q2).z; | (sum).w = (q1).w + (q2).w; (sum).x = (q1).x + (q2).x; (sum).y = (q1).y + (q 2).y; (sum).z = (q1).z + (q2).z; | |||
#define CQRMSubtract(sum,q1,q2) \ | #define CQRMSubtract(sum,q1,q2) \ | |||
(sum).w = (q1).w - (q2).w; sum.x = (q1).x - (q2).x; sum.y = (q1).y - (q2).y ; sum.z = (q1).z - (q2).z; | (sum).w = (q1).w - (q2).w; (sum).x = (q1).x - (q2).x; (sum).y = (q1).y - (q 2).y; (sum).z = (q1).z - (q2).z; | |||
#define CQRMMultiply(product,q1,q2 ) \ | #define CQRMMultiply(product,q1,q2 ) \ | |||
(product).w = -(q1).z*(q2).z - (q1).y*(q2).y - (q1).x*(q2).x + (q1).w*(q2). w; \ | (product).w = -(q1).z*(q2).z - (q1).y*(q2).y - (q1).x*(q2).x + (q1).w*(q2). w; \ | |||
(product).x = (q1).y*(q2).z - (q1).z*(q2).y + (q1).w*(q2).x + (q1).x*(q2). w; \ | (product).x = (q1).y*(q2).z - (q1).z*(q2).y + (q1).w*(q2).x + (q1).x*(q2). w; \ | |||
(product).y = -(q1).x*(q2).z + (q1).w*(q2).y + (q1).z*(q2).x + (q1).y*(q2). w; \ | (product).y = -(q1).x*(q2).z + (q1).w*(q2).y + (q1).z*(q2).x + (q1).y*(q2). w; \ | |||
(product).z = (q1).w*(q2).z + (q1).x*(q2).y - (q1).y*(q2).x + (q1).z*(q2). w; | (product).z = (q1).w*(q2).z + (q1).x*(q2).y - (q1).y*(q2).x + (q1).z*(q2). w; | |||
#define CQRMDot(dotprod,q1,q2 ) \ | #define CQRMDot(dotprod,q1,q2 ) \ | |||
dotprod = (q1).w*(q2).w + (q1).x*(q2).x + (q1).y*(q2).y + (q1).z*(q2).z; | dotprod = (q1).w*(q2).w + (q1).x*(q2).x + (q1).y*(q2).y + (q1).z*(q2).z; | |||
skipping to change at line 1067 | skipping to change at line 1293 | |||
#define CQRMConjugate(conjugate,q ) \ | #define CQRMConjugate(conjugate,q ) \ | |||
(conjugate).w = (q).w; \ | (conjugate).w = (q).w; \ | |||
(conjugate).x = -(q).x; \ | (conjugate).x = -(q).x; \ | |||
(conjugate).y = -(q).y; \ | (conjugate).y = -(q).y; \ | |||
(conjugate).z = -(q).z; | (conjugate).z = -(q).z; | |||
#define CQRMNormsq(normsq,q) \ | #define CQRMNormsq(normsq,q) \ | |||
normsq = (q).w*(q).w + (q).x*(q).x + (q).y*(q).y + (q).z*(q).z; | normsq = (q).w*(q).w + (q).x*(q).x + (q).y*(q).y + (q).z*(q).z; | |||
#define CQRMNorm(norm,q) \ | ||||
norm = sqrt((q).w*(q).w + (q).x*(q).x + (q).y*(q).y + (q).z*(q).z); | ||||
#define CQRMDistsq(distsq,q1,q2) \ | ||||
distsq = ((q1).w-(q2).w)*((q1).w-(q2).w) + ((q1).x-(q2).x)*((q1).x-(q2).x) | ||||
+ ((q1).y-(q2).y)*((q1).y-(q2).y) + ((q1).z-(q2).z)*((q1).z-(q2).z); | ||||
#define CQRMDist(dist,q1,q2) \ | ||||
dist = sqrt(((q1).w-(q2).w)*((q1).w-(q2).w) + ((q1).x-(q2).x)*((q1).x-(q2). | ||||
x) + ((q1).y-(q2).y)*((q1).y-(q2).y) + ((q1).z-(q2).z)*((q1).z-(q2).z)); | ||||
#define CQRMInverse(inverseq,q) \ | #define CQRMInverse(inverseq,q) \ | |||
{ double normsq; \ | { double normsq; \ | |||
CQRMConjugate(inverseq,q); \ | CQRMConjugate(inverseq,q); \ | |||
CQRMNormsq(normsq,q); \ | CQRMNormsq(normsq,q); \ | |||
if (normsq > 0.) { \ | if (normsq > 0.) { \ | |||
CQRMScalarMultiply(inverserq,1./normsq); \ | CQRMScalarMultiply(inverseq,inverseq,1./normsq); \ | |||
} \ | ||||
} | } | |||
/* CQRCreateQuaternion -- create a quaternion = w +ix+jy+kz */ | /* CQRCreateQuaternion -- create a quaternion = w +ix+jy+kz */ | |||
int CQRCreateQuaternion(CQRQuaternionHandle CQR_FAR * quaternion, doubl e w, double x, double y, double z); | int CQRCreateQuaternion(CQRQuaternionHandle CQR_FAR * quaternion, doubl e w, double x, double y, double z); | |||
/* CQRCreateEmptyQuaternion -- create a quaternion = 0 +i0+j0+k0 */ | /* CQRCreateEmptyQuaternion -- create a quaternion = 0 +i0+j0+k0 */ | |||
int CQRCreateEmptyQuaternion(CQRQuaternionHandle CQR_FAR * quaternion) ; | int CQRCreateEmptyQuaternion(CQRQuaternionHandle CQR_FAR * quaternion) ; | |||
/* CQRFreeQuaternion -- free a quaternion */ | /* CQRFreeQuaternion -- free a quaternion */ | |||
int CQRFreeQuaternion(CQRQuaternionHandle CQR_FAR * quaternion); | int CQRFreeQuaternion(CQRQuaternionHandle CQR_FAR * quaternion); | |||
/* CQRSetQuaternion -- create an existing quaternion = w +ix+jy+kz */ | /* CQRSetQuaternion -- create an existing quaternion = w +ix+jy+kz */ | |||
int CQRSetQuaternion( CQRQuaternionHandle quaternion, double w, double x, double y, double z); | int CQRSetQuaternion( CQRQuaternionHandle quaternion, double w, double x, double y, double z); | |||
/* CQRGetQuaternionW -- get the w component of a quaternion */ | ||||
int CQRGetQuaternionW( double CQR_FAR * qw, CQRQuaternionHandle q ); | ||||
/* CQRGetQuaternionX -- get the x component of a quaternion */ | ||||
int CQRGetQuaternionX( double CQR_FAR * qx, CQRQuaternionHandle q ); | ||||
/* CQRGetQuaternionY -- get the y component of a quaternion */ | ||||
int CQRGetQuaternionY( double CQR_FAR * qy, CQRQuaternionHandle q ); | ||||
/* CQRGetQuaternionZ -- get the z component of a quaternion */ | ||||
int CQRGetQuaternionZ( double CQR_FAR * qz, CQRQuaternionHandle q ); | ||||
/* CQRGetQuaternionIm -- get the imaginary component of a quaternion */ | ||||
int CQRGetQuaternionIm( CQRQuaternionHandle quaternion, CQRQuaternionHa | ||||
ndle q ); | ||||
/* CQRGetQuaternionAxis -- get the axis for the polar representation of | ||||
a quaternion */ | ||||
int CQRGetQuaternionAxis( CQRQuaternionHandle quaternion, CQRQuaternion | ||||
Handle q ); | ||||
/* CQRGetQuaternionAngle -- get the angular component of the polar repr | ||||
esentation | ||||
of aquaternion */ | ||||
int CQRGetQuaternionAngle( double CQR_FAR * angle, CQRQuaternionHandle | ||||
q ); | ||||
/* CQRLog -- get the natural logarithm of a quaternion */ | ||||
int CQRLog( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); | ||||
/* CQRExp -- get the exponential (exp) of a quaternion */ | ||||
int CQRExp( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); | ||||
/* CQRQuaternionPower -- take a quarernion to a quaternion power */ | ||||
int CQRQuaternionPower( CQRQuaternionHandle quaternion, CQRQuaternionHa | ||||
ndle q, CQRQuaternionHandle p); | ||||
/* CQRDoublePower -- take a quarernion to a double power */ | ||||
int CQRDoublePower( CQRQuaternionHandle quaternion, CQRQuaternionHandle | ||||
q, double p); | ||||
/* CQRIntegerPower -- take a quaternion to an integer power */ | ||||
int CQRIntegerPower( CQRQuaternionHandle quaternion, CQRQuaternionHandl | ||||
e q, int p); | ||||
/* CQRIntegerRoot -- take the given integer root of a quaternion, retu | ||||
rning | ||||
the indicated mth choice from among multiple roots. | ||||
For reals the cycle runs through first the i-based | ||||
roots, then the j-based roots and then the k-based roots, | ||||
out of the infinite number of possible roots of reals. */ | ||||
int CQRIntegerRoot( CQRQuaternionHandle quaternion, CQRQuaternionHandle | ||||
q, int r, int m); | ||||
/* CQRAdd -- add a quaternion (q1) to a quaternion (q2) */ | /* CQRAdd -- add a quaternion (q1) to a quaternion (q2) */ | |||
int CQRAdd (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQ RQuaternionHandle q2 ); | int CQRAdd (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQ RQuaternionHandle q2 ); | |||
/* CQRSubtract -- subtract a quaternion (q2) from a quaternion (q1) * / | /* CQRSubtract -- subtract a quaternion (q2) from a quaternion (q1) * / | |||
int CQRSubtract (CQRQuaternionHandle quaternion, CQRQuaternionHandle q 1, CQRQuaternionHandle q2 ); | int CQRSubtract (CQRQuaternionHandle quaternion, CQRQuaternionHandle q 1, CQRQuaternionHandle q2 ); | |||
/* CQRMultiply -- multiply a quaternion (q1) by quaternion (q2) */ | /* CQRMultiply -- multiply a quaternion (q1) by quaternion (q2) */ | |||
skipping to change at line 1131 | skipping to change at line 1424 | |||
int CQRConjugate (CQRQuaternionHandle qconjgate, CQRQuaternionHandle qu aternion); | int CQRConjugate (CQRQuaternionHandle qconjgate, CQRQuaternionHandle qu aternion); | |||
/* CQRNormsq -- Form the normsquared of a quaternion */ | /* CQRNormsq -- Form the normsquared of a quaternion */ | |||
int CQRNormsq (double CQR_FAR * normsq, CQRQuaternionHandle quaternion ) ; | int CQRNormsq (double CQR_FAR * normsq, CQRQuaternionHandle quaternion ) ; | |||
/* CQRNorm -- Form the norm of a quaternion */ | /* CQRNorm -- Form the norm of a quaternion */ | |||
int CQRNorm (double CQR_FAR * norm, CQRQuaternionHandle quaternion ) ; | int CQRNorm (double CQR_FAR * norm, CQRQuaternionHandle quaternion ) ; | |||
/* CQRDistsq -- Form the distance squared between two quaternions */ | ||||
int CQRDistsq (double CQR_FAR * distsq, CQRQuaternionHandle q1, CQRQuat | ||||
ernionHandle q2) ; | ||||
/* CQRDist -- Form the distance between two quaternions */ | ||||
int CQRDist (double CQR_FAR * dist, CQRQuaternionHandle q1, CQRQuaterni | ||||
onHandle q2 ) ; | ||||
/* CQRInverse -- Form the inverse of a quaternion */ | /* CQRInverse -- Form the inverse of a quaternion */ | |||
int CQRInverse (CQRQuaternionHandle inversequaternion, CQRQuaternionHan dle quaternion ); | int CQRInverse (CQRQuaternionHandle inversequaternion, CQRQuaternionHan dle quaternion ); | |||
/* CQRRotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* * / | /* CQRRotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* * / | |||
int CQRRotateByQuaternion(double CQR_FAR * w, CQRQuaternionHandle rotqu aternion, double CQR_FAR * v); | int CQRRotateByQuaternion(double CQR_FAR * w, CQRQuaternionHandle rotqu aternion, double CQR_FAR * v); | |||
/* CQRAxis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ | /* CQRAxis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ | |||
End of changes. 15 change blocks. | ||||
4 lines changed or deleted | 335 lines changed or added | |||