FGFS-4.1/OpenSceneGraph
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed problems with osg::Matrix::makeRot(from,to) and osg::Quat::makeRot(from,to)

Browse Source 
so that they both use the same implementation (the Quat code now) and the
code has been corrected to work from and to vectors which directly opposite
to one another.
This commit is contained in:
Robert Osfield 2001-12-12 15:09:11 +00:00
parent 05e4a0b4ce
commit 79c1fb531d
2 changed files with 38 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
src/osg/Matrix.cpp
View File

@ -159,79 +159,28 @@ void Matrix::makeTrans( float x, float y, float z )
void Matrix::makeRot( const Vec3& from, const Vec3& to )
{
double d = from * to; // dot product == cos( angle between from & to )
if( d < 0.9999 ) {
double angle = acos(d);
// For right-handed rotations, cross product must be from x to, not
// to x from
//Vec3 axis = to ^ from; //we know ((to) x (from)) is perpendicular to both
Vec3 axis = from ^ to; //we know ((from) x (to)) is perpendicular to both
makeRot( inRadians(angle) , axis );
}
else
makeIdent();
Quat quat;
quat.makeRot(from,to);
quat.get(*this);
}
void Matrix::makeRot( float angle, const Vec3& axis )
{
makeRot( angle, axis.x(), axis.y(), axis.z() );
Quat quat;
quat.makeRot( angle, axis);
quat.get(*this);
}
void Matrix::makeRot( float angle, float x, float y, float z )
{
float d = sqrt( x*x + y*y + z*z );
if( d == 0 )
return;
#ifdef USE_DEGREES_INTERNALLY
angle = DEG2RAD(angle);
#endif
#if 0
float sin_half = sin( angle/2 );
float cos_half = cos( angle/2 );
Quat q( sin_half * (x/d),
sin_half * (y/d),
sin_half * (z/d),
cos_half );//NOTE: original used a private quaternion made of doubles
#endif
Quat q;
q.makeRot( angle, x, y, z);
makeRot( q ); // but Quat stores the values in a Vec4 made of floats.
Quat quat;
quat.makeRot( angle, x, y, z);
quat.get(*this);
}
void Matrix::makeRot( const Quat& q ) {
// taken from Shoemake/ACM SIGGRAPH 89
Vec4 v = q.asVec4();
double xs = 2 * v.x(); //assume q is already normalized? assert?
double ys = 2 * v.y(); // if not, xs = 2 * v.x() / d, ys = 2 * v.y() / d
double zs = 2 * v.z(); // and zs = 2 * v.z() /d where d = v.length2()
double xx = xs * v.x();
double xy = ys * v.x();
double xz = zs * v.x();
double yy = ys * v.y();
double yz = zs * v.y();
double zz = zs * v.z();
double wx = xs * v.w();
double wy = ys * v.w();
double wz = zs * v.w();
/*
* This is inverted - Don Burns
SET_ROW(0, 1.0-(yy+zz), xy - wz, xz + wy, 0.0 )
SET_ROW(1, xy + wz, 1.0-(xx+zz),yz - wx, 0.0 )
SET_ROW(2, xz - wy, yz + wx, 1.0-(xx+yy),0.0 )
SET_ROW(3, 0.0, 0.0, 0.0, 1.0 )
*/
SET_ROW(0, 1.0-(yy+zz), xy + wz, xz - wy, 0.0 )
SET_ROW(1, xy - wz, 1.0-(xx+zz),yz + wx, 0.0 )
SET_ROW(2, xz + wy, yz - wx, 1.0-(xx+yy),0.0 )
SET_ROW(3, 0.0, 0.0, 0.0, 1.0 )
void Matrix::makeRot( const Quat& q )
{
q.get(*this);
fully_realized = true;
}
@ -256,7 +205,8 @@ void Matrix::makeRot( float yaw, float pitch, float roll)
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
makeRot( q );
q.get(*this);
}
void Matrix::mult( const Matrix& lhs, const Matrix& rhs )

46
src/osg/Quat.cpp
View File

@ -42,15 +42,15 @@ void Quat::makeRot( const float angle, const Vec3& vec )
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void Quat::makeRot( const Vec3& vec1, const Vec3& vec2 )
void Quat::makeRot( const Vec3& from, const Vec3& to )
{
const float epsilon = 0.00001f;
float length1 = vec1.length();
float length2 = vec2.length();
float length1 = from.length();
float length2 = to.length();
// dot product vec1*vec2
float cosangle = vec1*vec2/(length1*length2);
float cosangle = from*to/(length1*length2);
if ( fabs(cosangle - 1) < epsilon )
{
@ -60,30 +60,32 @@ void Quat::makeRot( const Vec3& vec1, const Vec3& vec2 )
makeRot( 0.0, 1.0, 0.0, 0.0 );
}
else
#if 0 /// This is broken and entirely unecessary. - Don Burns.
if ( fabs(cosangle + 1) < epsilon )
if ( fabs(cosangle + 1.0) < epsilon )
{
// vectors are close to being opposite, so will need to find a
// vector orthongonal to from to rotate about.
osg::Vec3 tmp;
if (fabs(from.x())<fabs(from.y()))
if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
else tmp.set(0.0,0.0,1.0);
else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
else tmp.set(0.0,0.0,1.0);
// find orthogonal axis.
Vec3 axis(from^tmp);
axis.normalize();
_fv[0] = axis[0]; // sin of half angle of PI is 1.0.
_fv[1] = axis[1]; // sin of half angle of PI is 1.0.
_fv[2] = axis[2]; // sin of half angle of PI is 1.0.
_fv[3] = 0; // cos of half angle of PI is zero.
// cosangle is close to -1, so the vectors are close to being opposite
// The angle of rotation is going to be Pi, but around which axis?
// Basically, any one perpendicular to vec1 = (x,y,z) is going to work.
// Choose a vector to cross product vec1 with. Find the biggest
// in magnitude of x, y and z and then put a zero in that position.
float biggest = fabs(vec1[0]); int bigposn = 0;
if ( fabs(vec1[1]) > biggest ) { biggest=fabs(vec1[1]); bigposn = 1; }
if ( fabs(vec1[2]) > biggest ) { biggest=fabs(vec1[2]); bigposn = 2; }
Vec3 temp = Vec3( 1.0, 1.0, 1.0 );
temp[bigposn] = 0.0;
Vec3 axis = vec1^temp; // this is a cross-product to generate the
// axis around which to rotate
makeRot( (float)M_PI, axis );
}
else
#endif
{
// This is the usual situation - take a cross-product of vec1 and vec2
// and that is the axis around which to rotate.
Vec3 axis = vec1^vec2;
Vec3 axis(from^to);
float angle = acos( cosangle );
makeRot( angle, axis );
}
@ -224,7 +226,7 @@ void Quat::get( Matrix& m ) const
//
//http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
// calculate coefficients
x2 = QX + QX;