Page MenuHome
Differential D6807 Diff 22906 extern/opennurbs/opennurbs_quaternion.cpp

Changeset View

Standalone View

View Options

extern/opennurbs/opennurbs_quaternion.cpp

  • This file was added.
/* $NoKeywords: $ */
/*
//
// Copyright (c) 1993-2012 Robert McNeel & Associates. All rights reserved.
// OpenNURBS, Rhinoceros, and Rhino3D are registered trademarks of Robert
// McNeel & Associates.
//
// THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY.
// ALL IMPLIED WARRANTIES OF FITNESS FOR ANY PARTICULAR PURPOSE AND OF
// MERCHANTABILITY ARE HEREBY DISCLAIMED.
//
// For complete openNURBS copyright information see <http://www.opennurbs.org>.
//
////////////////////////////////////////////////////////////////
*/
#include "opennurbs.h"
#if !defined(ON_COMPILING_OPENNURBS)
// This check is included in all opennurbs source .c and .cpp files to insure
// ON_COMPILING_OPENNURBS is defined when opennurbs source is compiled.
// When opennurbs source is being compiled, ON_COMPILING_OPENNURBS is defined
// and the opennurbs .h files alter what is declared and how it is declared.
#error ON_COMPILING_OPENNURBS must be defined when compiling opennurbs
#endif
ON_Quaternion ON_CrossProduct( const ON_Quaternion& p, const ON_Quaternion& q)
{
return ON_Quaternion(0.0, p.c*q.d - p.d*q.c, p.d*q.b - p.b*q.d, p.b*q.c - p.c*q.d);
}
ON_Quaternion ON_QuaternionProduct( const ON_Quaternion& p, const ON_Quaternion& q)
{
return ON_Quaternion( p.a*q.a - p.b*q.b - p.c*q.c - p.d*q.d,
p.a*q.b + p.b*q.a + p.c*q.d - p.d*q.c,
p.a*q.c - p.b*q.d + p.c*q.a + p.d*q.b,
p.a*q.d + p.b*q.c - p.c*q.b + p.d*q.a);
}
/////////////////////////////////////
const ON_Quaternion ON_Quaternion::Zero(0.0,0.0,0.0,0.0);
const ON_Quaternion ON_Quaternion::Identity(1.0,0.0,0.0,0.0);
const ON_Quaternion ON_Quaternion::I(0.0,1.0,0.0,0.0);
const ON_Quaternion ON_Quaternion::J(0.0,0.0,1.0,0.0);
const ON_Quaternion ON_Quaternion::K(0.0,0.0,0.0,1.0);
void ON_Quaternion::SetRotation(double angle, const ON_3dVector& axis)
{
double s = axis.Length();
s = (s > 0.0) ? sin(0.5*angle)/s : 0.0;
a = cos(0.5*angle);
b = s*axis.x;
c = s*axis.y;
d = s*axis.z;
}
ON_Quaternion ON_Quaternion::Rotation(double angle, const ON_3dVector& axis)
{
double s = axis.Length();
s = (s > 0.0) ? sin(0.5*angle)/s : 0.0;
return ON_Quaternion(cos(0.5*angle),s*axis.x,s*axis.y,s*axis.z);
}
ON_Quaternion ON_Quaternion::Exp(ON_Quaternion q)
{
// Added 8 Jan 2010 - not tested yet
double v = ((const ON_3dVector*)&q.b)->Length();
if ( !(v > ON_DBL_MIN) )
v = 0.0;
double ea = exp(q.a);
double z = (v > 0.0) ? ea*sin(v)/v : 0.0;
return ON_Quaternion( ea*cos(v), z*q.b, z*q.c, z*q.d );
}
ON_Quaternion ON_Quaternion::Log(ON_Quaternion q)
{
// Added 8 Jan 2010 - not tested yet
double lenq = q.Length();
double v = ((const ON_3dVector*)&q.b)->Length();
if ( !(v > ON_DBL_MIN) )
v = 0.0;
double z = (v > 0.0) ? acos(q.a/lenq)/v : 0.0;
return ON_Quaternion(log(lenq), z*q.b, z*q.c, z*q.d );
}
ON_Quaternion ON_Quaternion::Pow(ON_Quaternion q,double t)
{
// Added 8 Jan 2010 - not tested yet
return ON_Quaternion::Exp( t * ON_Quaternion::Log(q) );
}
ON_Quaternion ON_Quaternion::Slerp(ON_Quaternion q0, ON_Quaternion q1, double t)
{
// Added 8 Jan 2010 - not tested yet
ON_Quaternion q;
if ( t <= 0.5 )
{
q = q0.Inverse()*q1;
q = q0*ON_Quaternion::Pow(q,t);
}
else
{
q = q1.Inverse()*q0;
q = q1*ON_Quaternion::Pow(q,1.0-t);
}
return q;
}
void ON_Quaternion::Set(double qa, double qb, double qc, double qd)
{
a = qa;
b = qb;
c = qc;
d = qd;
}
void ON_Quaternion::SetRotation(const ON_Plane& plane0, const ON_Plane& plane1 )
{
double m[3][3], r, s;
double* q;
int i,j,k;
// set m[][] = rotation matrix (acting on the left)
m[0][0] = plane1.xaxis.x*plane0.xaxis.x
+ plane1.yaxis.x*plane0.yaxis.x
+ plane1.zaxis.x*plane0.zaxis.x;
m[0][1] = plane1.xaxis.x*plane0.xaxis.y
+ plane1.yaxis.x*plane0.yaxis.y
+ plane1.zaxis.x*plane0.zaxis.y;
m[0][2] = plane1.xaxis.x*plane0.xaxis.z
+ plane1.yaxis.x*plane0.yaxis.z
+ plane1.zaxis.x*plane0.zaxis.z;
m[1][0] = plane1.xaxis.y*plane0.xaxis.x
+ plane1.yaxis.y*plane0.yaxis.x
+ plane1.zaxis.y*plane0.zaxis.x;
m[1][1] = plane1.xaxis.y*plane0.xaxis.y
+ plane1.yaxis.y*plane0.yaxis.y
+ plane1.zaxis.y*plane0.zaxis.y;
m[1][2] = plane1.xaxis.y*plane0.xaxis.z
+ plane1.yaxis.y*plane0.yaxis.z
+ plane1.zaxis.y*plane0.zaxis.z;
m[2][0] = plane1.xaxis.z*plane0.xaxis.x
+ plane1.yaxis.z*plane0.yaxis.x
+ plane1.zaxis.z*plane0.zaxis.x;
m[2][1] = plane1.xaxis.z*plane0.xaxis.y
+ plane1.yaxis.z*plane0.yaxis.y
+ plane1.zaxis.z*plane0.zaxis.y;
m[2][2] = plane1.xaxis.z*plane0.xaxis.z
+ plane1.yaxis.z*plane0.yaxis.z
+ plane1.zaxis.z*plane0.zaxis.z;
k = 1;
s = ON_SQRT_EPSILON;
for ( i = 0; i < 3 && k; i++ ) for ( j = 0; j < 3; j++ )
{
if ( i == j )
{
if (fabs(m[i][i]-1.0) > s )
{
k = 0;
break;
}
}
else
{
if (fabs(m[i][j]) > s )
{
k = 0;
break;
}
}
}
if ( k )
{
// m[][] is the identity matrix
a = 1.0;
b = c = d = 0.0;
return;
}
i = (m[0][0] >= m[1][1])
? ((m[0][0] >= m[2][2])?0:2)
: ((m[1][1] >= m[2][2])?1:2);
j = (i+1)%3;
k = (i+2)%3;
// Note:
// For any rotation matrix, the diagonal is
// x^2(1-cos(t)) + cos(t), y^2(1-cos(t)) + cos(t), z^2(1-cos(t)) + cos(t),
// where (x,y,z) is the unit vector axis of rotation and "t" is the angle.
// So the trace = 1 + 2cos(t).
//
// When cos(t) >= 0, m[i][i] corresponds to the axis component that has
// the largest absolute value.
//
//
//
// Observe that
// s = 1 + m[i][i] - m[j][j] - m[k][k]
// = 1 + 2*m[i][i] - m[i][i] - m[j][j] - m[k][k]
// = 1 + 2*m[i][i] - trace
// = 2*(m[i][i] - cos(t))
// = 2*(w^2(1-cos(t)^2) + cos(t) - cos(t))
// = 2*w*w*(sin(t)^2)
//
// When cos(t) >= 0, m[i][i] corresponds to the coordinate of
// the rotation axis with largest absolute value.
s = 1.0 + m[i][i] - m[j][j] - m[k][k];
if ( s > ON_DBL_MIN )
{
r = sqrt(s);
s = 0.5/r;
a = s*(m[k][j] - m[j][k]);
q = &b;
q[i] = 0.5*r;
q[j] = s*(m[i][j] + m[j][i]);
q[k] = s*(m[k][i] + m[i][k]);
}
else
{
if ( s < -1.0e-14 )
ON_ERROR("noisy rotation matrix");
a = 1.0;
b = c = d = 0.0;
}
}
ON_Quaternion ON_Quaternion::Rotation(const ON_Plane& plane0, const ON_Plane& plane1)
{
ON_Quaternion q;
q.SetRotation(plane0,plane1);
return q;
}
bool ON_Quaternion::GetRotation(ON_Xform& xform) const
{
bool rc;
ON_Quaternion q(*this);
if ( q.Unitize() )
{
if ( fabs(q.a-a) <= ON_ZERO_TOLERANCE
&& fabs(q.b-b) <= ON_ZERO_TOLERANCE
&& fabs(q.c-c) <= ON_ZERO_TOLERANCE
&& fabs(q.d-d) <= ON_ZERO_TOLERANCE
)
{
// "this" was already unitized - don't tweak bits
q = *this;
}
xform[1][0] = 2.0*(q.b*q.c + q.a*q.d);
xform[2][0] = 2.0*(q.b*q.d - q.a*q.c);
xform[3][0] = 0.0;
xform[0][1] = 2.0*(q.b*q.c - q.a*q.d);
xform[2][1] = 2.0*(q.c*q.d + q.a*q.b);
xform[3][1] = 0.0;
xform[0][2] = 2.0*(q.b*q.d + q.a*q.c);
xform[1][2] = 2.0*(q.c*q.d - q.a*q.b);
xform[3][2] = 0.0;
q.b = q.b*q.b;
q.c = q.c*q.c;
q.d = q.d*q.d;
xform[0][0] = 1.0 - 2.0*(q.c + q.d);
xform[1][1] = 1.0 - 2.0*(q.b + q.d);
xform[2][2] = 1.0 - 2.0*(q.b + q.c);
xform[0][3] = xform[1][3] = xform[2][3] = 0.0;
xform[3][3] = 1.0;
rc = true;
}
else if ( IsZero() )
{
xform = ON_Xform::Zero4x4;
rc = false;
}
else
{
// something is seriously wrong
ON_ERROR("ON_Quaternion::GetRotation(ON_Xform) quaternion is invalid");
xform = ON_Xform::IdentityTransformation;
rc = false;
}
return rc;
}
bool ON_Quaternion::GetRotation(ON_Plane& plane) const
{
plane.xaxis.x = a*a + b*b - c*c - d*d;
plane.xaxis.y = 2.0*(a*d + b*c);
plane.xaxis.z = 2.0*(b*d - a*c);
plane.yaxis.x = 2.0*(b*c - a*d);
plane.yaxis.y = a*a - b*b + c*c - d*d;
plane.yaxis.z = 2.0*(a*b + c*d);
plane.zaxis.x = 2.0*(a*c + b*d);
plane.zaxis.y = 2.0*(c*d - a*b);
plane.zaxis.z = a*a - b*b - c*c + d*d;
plane.xaxis.Unitize();
plane.yaxis.Unitize();
plane.zaxis.Unitize();
plane.origin.Set(0.0,0.0,0.0);
plane.UpdateEquation();
return plane.IsValid();
}
bool ON_Quaternion::GetRotation(double& angle, ON_3dVector& axis) const
{
const double s = Length();
angle = (s > ON_DBL_MIN) ? 2.0*acos(a/s) : 0.0;
axis.x = b;
axis.y = c;
axis.z = d;
return (axis.Unitize() && s > ON_DBL_MIN);
}
ON_3dVector ON_Quaternion::Vector() const
{
return ON_3dVector(b,c,d);
}
double ON_Quaternion::Scalar() const
{
return a;
}
bool ON_Quaternion::IsZero() const
{
return (0.0 == a && 0.0 == b && 0.0 == c && 0.0 == d);
}
bool ON_Quaternion::IsNotZero() const
{
return ( (0.0 != a || 0.0 != b || 0.0 != c || 0.0 != d)
&& ON_IsValid(a)
&& ON_IsValid(b)
&& ON_IsValid(c)
&& ON_IsValid(d)
);
}
bool ON_Quaternion::IsScalar() const
{
return (0.0 == b && 0.0 == c && 0.0 == d);
}
bool ON_Quaternion::IsVector() const
{
return (0.0 == a && (0.0 != b || 0.0 != c || 0.0 != d));
}
bool ON_Quaternion::IsValid() const
{
return ((ON_IS_VALID(a) && ON_IS_VALID(b) && ON_IS_VALID(c) && ON_IS_VALID(d)) ? true : false);
}
ON_Quaternion ON_Quaternion::Conjugate() const
{
return ON_Quaternion(a,-b,-c,-d);
}
ON_Quaternion ON_Quaternion::Inverse() const
{
double x = a*a+b*b+c*c+d*d;
x = ( x > ON_DBL_MIN ) ? 1.0/x : 0.0;
return ON_Quaternion(a*x,-b*x,-c*x,-d*x);
}
bool ON_Quaternion::Invert()
{
double x = a*a+b*b+c*c+d*d;
if ( x <= ON_DBL_MIN )
return false;
x = 1.0/x;
a *= x;
x = -x;
b *= x;
c *= x;
d *= x;
return true;
}
ON_3dVector ON_Quaternion::Rotate(ON_3dVector v) const
{
// returns q*(0,v.x,v.y,v.z)*Inverse(q)
double x = a*a + b*b + c*c + d*d;
x = ( x > ON_DBL_MIN ) ? 1.0/x : 0.0;
const ON_Quaternion qinv(a*x,-b*x,-c*x,-d*x);
const ON_Quaternion qv( -b*v.x - c*v.y - d*v.z,
a*v.x + c*v.z - d*v.y,
a*v.y - b*v.z + d*v.x,
a*v.z + b*v.y - c*v.x);
v.x = qv.a*qinv.b + qv.b*qinv.a + qv.c*qinv.d - qv.d*qinv.c;
v.y = qv.a*qinv.c - qv.b*qinv.d + qv.c*qinv.a + qv.d*qinv.b;
v.z = qv.a*qinv.d + qv.b*qinv.c - qv.c*qinv.b + qv.d*qinv.a;
return v;
}
double ON_Quaternion::DistanceTo(const ON_Quaternion& q) const
{
const ON_Quaternion pq(q.a-a,q.b-b,q.c-c,q.d-d);
return pq.Length();
}
double ON_Quaternion::Distance(const ON_Quaternion& p, const ON_Quaternion& q)
{
const ON_Quaternion pq(q.a-p.a,q.b-p.b,q.c-p.c,q.d-p.d);
return pq.Length();
}
double ON_Quaternion::Length() const
{
double len;
double fa = fabs(a);
double fb = fabs(b);
double fc = fabs(c);
double fd = fabs(d);
if ( fb >= fa && fb >= fc && fb >= fd)
{
len = fa; fa = fb; fb = len;
}
else if ( fc >= fa && fc >= fb && fc >= fd)
{
len = fa; fa = fc; fc = len;
}
else if ( fd >= fa && fd >= fb && fd >= fc)
{
len = fa; fa = fd; fd = len;
}
// 15 September 2003 Dale Lear
// For small denormalized doubles (positive but smaller
// than DBL_MIN), some compilers/FPUs set 1.0/fa to +INF.
// Without the ON_DBL_MIN test we end up with
// microscopic quaternions that have infinte norm!
//
// This code is absolutely necessary. It is a critical
// part of the bug fix for RR 11217.
if ( fa > ON_DBL_MIN )
{
len = 1.0/fa;
fb *= len;
fc *= len;
fd *= len;
len = fa*sqrt(1.0 + fb*fb + fc*fc + fd*fd);
}
else if ( fa > 0.0 && ON_IS_FINITE(fa) )
len = fa;
else
len = 0.0;
return len;
}
double ON_Quaternion::LengthSquared() const
{
return (a*a + b*b + c*c + d*d);
}
ON_Xform ON_Quaternion::MatrixForm() const
{
double m[4][4];
m[0][0] = a; m[0][1] = b; m[0][2] = c; m[0][3] = d;
m[1][0] = -b; m[1][1] = a; m[1][2] = -d; m[1][3] = c;
m[2][0] = -c; m[2][1] = d; m[2][2] = a; m[2][3] = -b;
m[3][0] = -d; m[3][1] = -c; m[3][2] = b; m[3][3] = a;
return ON_Xform(&m[0][0]);
}
bool ON_Quaternion::Unitize()
{
double x = Length();
if (x > ON_DBL_MIN)
{
x = 1.0/x;
a *= x; b *= x; c *= x; d *= x;
}
else if ( x > 0.0 )
{
ON_Quaternion q(a*1.0e300,b*1.0e300,c*1.0e300,c*1.0e300);
if ( !q.Unitize() )
return false;
a = q.a;
b = q.b;
c = q.c;
d = q.d;
}
else
{
return false;
}
return true;
}
ON_Quaternion::ON_Quaternion(double qa, double qb, double qc, double qd) : a(qa),b(qb),c(qc),d(qd) {}
ON_Quaternion::ON_Quaternion(const ON_3dVector& v) : a(0.0),b(v.x),c(v.y),d(v.z) {}
ON_Quaternion& ON_Quaternion::operator=(const ON_3dVector& v)
{
a = 0.0;
b = v.x;
c = v.y;
d = v.z;
return *this;
}
ON_Quaternion ON_Quaternion::operator*(int x) const
{
return ON_Quaternion(a*x,b*x,c*x,d*x);
}
ON_Quaternion ON_Quaternion::operator*(float x) const
{
return ON_Quaternion(a*x,b*x,c*x,d*x);
}
ON_Quaternion ON_Quaternion::operator*(double x) const
{
return ON_Quaternion(a*x,b*x,c*x,d*x);
}
ON_Quaternion ON_Quaternion::operator/(int y) const
{
double x = (0!=y) ? 1.0/((double)y) : 0.0;
return ON_Quaternion(a*x,b*x,c*x,d*x);
}
ON_Quaternion ON_Quaternion::operator/(float y) const
{
double x = (0.0f!=y) ? 1.0/((double)y) : 0.0;
return ON_Quaternion(a*x,b*x,c*x,d*x);
}
ON_Quaternion ON_Quaternion::operator/(double y) const
{
double x = (0.0!=y) ? 1.0/((double)y) : 0.0;
return ON_Quaternion(a*x,b*x,c*x,d*x);
}
ON_Quaternion ON_Quaternion::operator+(const ON_Quaternion& q) const
{
return ON_Quaternion(a+q.a,b+q.b,c+q.c,d+q.d);
}
ON_Quaternion ON_Quaternion::operator-(const ON_Quaternion& q) const
{
return ON_Quaternion(a-q.a,b-q.b,c-q.c,d-q.d);
}
ON_Quaternion ON_Quaternion::operator*(const ON_Quaternion& q) const
{
return ON_Quaternion(a*q.a - b*q.b - c*q.c - d*q.d,
a*q.b + b*q.a + c*q.d - d*q.c,
a*q.c - b*q.d + c*q.a + d*q.b,
a*q.d + b*q.c - c*q.b + d*q.a);
}
ON_Quaternion operator*(int x, const ON_Quaternion& q)
{
return ON_Quaternion(x*q.a,x*q.b,x*q.c,x*q.d);
}
ON_Quaternion operator*(float x, const ON_Quaternion& q)
{
return ON_Quaternion(x*q.a,x*q.b,x*q.c,x*q.d);
}
ON_Quaternion operator*(double x, const ON_Quaternion& q)
{
return ON_Quaternion(x*q.a,x*q.b,x*q.c,x*q.d);
}