/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield * * This library is open source and may be redistributed and/or modified under * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or * (at your option) any later version. The full license is in LICENSE file * included with this distribution, and on the openscenegraph.org website. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * OpenSceneGraph Public License for more details. */ #include #include #include #include #include #include using namespace osg; #define SET_ROW(row, v1, v2, v3, v4 ) \ _mat[(row)][0] = (v1); \ _mat[(row)][1] = (v2); \ _mat[(row)][2] = (v3); \ _mat[(row)][3] = (v4); #define INNER_PRODUCT(a,b,r,c) \ ((a)._mat[r][0] * (b)._mat[0][c]) \ +((a)._mat[r][1] * (b)._mat[1][c]) \ +((a)._mat[r][2] * (b)._mat[2][c]) \ +((a)._mat[r][3] * (b)._mat[3][c]) Matrix_implementation::Matrix_implementation( value_type a00, value_type a01, value_type a02, value_type a03, value_type a10, value_type a11, value_type a12, value_type a13, value_type a20, value_type a21, value_type a22, value_type a23, value_type a30, value_type a31, value_type a32, value_type a33) { SET_ROW(0, a00, a01, a02, a03 ) SET_ROW(1, a10, a11, a12, a13 ) SET_ROW(2, a20, a21, a22, a23 ) SET_ROW(3, a30, a31, a32, a33 ) } void Matrix_implementation::set( value_type a00, value_type a01, value_type a02, value_type a03, value_type a10, value_type a11, value_type a12, value_type a13, value_type a20, value_type a21, value_type a22, value_type a23, value_type a30, value_type a31, value_type a32, value_type a33) { SET_ROW(0, a00, a01, a02, a03 ) SET_ROW(1, a10, a11, a12, a13 ) SET_ROW(2, a20, a21, a22, a23 ) SET_ROW(3, a30, a31, a32, a33 ) } #define QX q._v[0] #define QY q._v[1] #define QZ q._v[2] #define QW q._v[3] void Matrix_implementation::setRotate(const Quat& q_in) { Quat q(q_in); double length2 = q.length2(); if (length2!=1.0 && length2!=0) { // normalize quat if required. q /= sqrt(length2); } // Source: Gamasutra, Rotating Objects Using Quaternions // //http://www.gamasutra.com/features/19980703/quaternions_01.htm double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; // calculate coefficients x2 = QX + QX; y2 = QY + QY; z2 = QZ + QZ; xx = QX * x2; xy = QX * y2; xz = QX * z2; yy = QY * y2; yz = QY * z2; zz = QZ * z2; wx = QW * x2; wy = QW * y2; wz = QW * z2; // Note. Gamasutra gets the matrix assignments inverted, resulting // in left-handed rotations, which is contrary to OpenGL and OSG's // methodology. The matrix assignment has been altered in the next // few lines of code to do the right thing. // Don Burns - Oct 13, 2001 _mat[0][0] = 1.0 - (yy + zz); _mat[1][0] = xy - wz; _mat[2][0] = xz + wy; _mat[0][1] = xy + wz; _mat[1][1] = 1.0 - (xx + zz); _mat[2][1] = yz - wx; _mat[0][2] = xz - wy; _mat[1][2] = yz + wx; _mat[2][2] = 1.0 - (xx + yy); #if 0 _mat[0][3] = 0.0; _mat[1][3] = 0.0; _mat[2][3] = 0.0; _mat[3][0] = 0.0; _mat[3][1] = 0.0; _mat[3][2] = 0.0; _mat[3][3] = 1.0; #endif } #if 1 // David Spillings implementation Mk 2 Quat Matrix_implementation::getRotate() const { Quat q; // From http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm QW = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] + _mat[1][1] + _mat[2][2] ) ); QX = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] - _mat[1][1] - _mat[2][2] ) ); QY = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] + _mat[1][1] - _mat[2][2] ) ); QZ = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] - _mat[1][1] + _mat[2][2] ) ); QX = QX * osg::sign( _mat[1][2] - _mat[2][1]) ; QY = QY * osg::sign( _mat[2][0] - _mat[0][2]) ; QZ = QZ * osg::sign( _mat[0][1] - _mat[1][0]) ; return q; } #else #if 1 // David Spillings implementation Mk 1 void Matrix_implementation::get( Quat& q ) const { value_type s; value_type tq[4]; int i, j; // Use tq to store the largest trace tq[0] = 1 + _mat[0][0]+_mat[1][1]+_mat[2][2]; tq[1] = 1 + _mat[0][0]-_mat[1][1]-_mat[2][2]; tq[2] = 1 - _mat[0][0]+_mat[1][1]-_mat[2][2]; tq[3] = 1 - _mat[0][0]-_mat[1][1]+_mat[2][2]; // Find the maximum (could also use stacked if's later) j = 0; for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j; // check the diagonal if (j==0) { /* perform instant calculation */ QW = tq[0]; QX = _mat[1][2]-_mat[2][1]; QY = _mat[2][0]-_mat[0][2]; QZ = _mat[0][1]-_mat[1][0]; } else if (j==1) { QW = _mat[1][2]-_mat[2][1]; QX = tq[1]; QY = _mat[0][1]+_mat[1][0]; QZ = _mat[2][0]+_mat[0][2]; } else if (j==2) { QW = _mat[2][0]-_mat[0][2]; QX = _mat[0][1]+_mat[1][0]; QY = tq[2]; QZ = _mat[1][2]+_mat[2][1]; } else /* if (j==3) */ { QW = _mat[0][1]-_mat[1][0]; QX = _mat[2][0]+_mat[0][2]; QY = _mat[1][2]+_mat[2][1]; QZ = tq[3]; } s = sqrt(0.25/tq[j]); QW *= s; QX *= s; QY *= s; QZ *= s; } #else // Original implementation void Matrix_implementation::get( Quat& q ) const { // Source: Gamasutra, Rotating Objects Using Quaternions // //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm value_type tr, s; value_type tq[4]; int i, j, k; int nxt[3] = {1, 2, 0}; tr = _mat[0][0] + _mat[1][1] + _mat[2][2]+1.0; // check the diagonal if (tr > 0.0) { s = (value_type)sqrt (tr); QW = s / 2.0; s = 0.5 / s; QX = (_mat[1][2] - _mat[2][1]) * s; QY = (_mat[2][0] - _mat[0][2]) * s; QZ = (_mat[0][1] - _mat[1][0]) * s; } else { // diagonal is negative i = 0; if (_mat[1][1] > _mat[0][0]) i = 1; if (_mat[2][2] > _mat[i][i]) i = 2; j = nxt[i]; k = nxt[j]; s = (value_type)sqrt ((_mat[i][i] - (_mat[j][j] + _mat[k][k])) + 1.0); tq[i] = s * 0.5; if (s != 0.0) s = 0.5 / s; tq[3] = (_mat[j][k] - _mat[k][j]) * s; tq[j] = (_mat[i][j] + _mat[j][i]) * s; tq[k] = (_mat[i][k] + _mat[k][i]) * s; QX = tq[0]; QY = tq[1]; QZ = tq[2]; QW = tq[3]; } } #endif #endif int Matrix_implementation::compare(const Matrix_implementation& m) const { const Matrix_implementation::value_type* lhs = reinterpret_cast(_mat); const Matrix_implementation::value_type* end_lhs = lhs+16; const Matrix_implementation::value_type* rhs = reinterpret_cast(m._mat); for(;lhs!=end_lhs;++lhs,++rhs) { if (*lhs < *rhs) return -1; if (*rhs < *lhs) return 1; } return 0; } void Matrix_implementation::setTrans( value_type tx, value_type ty, value_type tz ) { _mat[3][0] = tx; _mat[3][1] = ty; _mat[3][2] = tz; } void Matrix_implementation::setTrans( const Vec3f& v ) { _mat[3][0] = v[0]; _mat[3][1] = v[1]; _mat[3][2] = v[2]; } void Matrix_implementation::setTrans( const Vec3d& v ) { _mat[3][0] = v[0]; _mat[3][1] = v[1]; _mat[3][2] = v[2]; } void Matrix_implementation::makeIdentity() { SET_ROW(0, 1, 0, 0, 0 ) SET_ROW(1, 0, 1, 0, 0 ) SET_ROW(2, 0, 0, 1, 0 ) SET_ROW(3, 0, 0, 0, 1 ) } void Matrix_implementation::makeScale( const Vec3f& v ) { makeScale(v[0], v[1], v[2] ); } void Matrix_implementation::makeScale( const Vec3d& v ) { makeScale(v[0], v[1], v[2] ); } void Matrix_implementation::makeScale( value_type x, value_type y, value_type z ) { SET_ROW(0, x, 0, 0, 0 ) SET_ROW(1, 0, y, 0, 0 ) SET_ROW(2, 0, 0, z, 0 ) SET_ROW(3, 0, 0, 0, 1 ) } void Matrix_implementation::makeTranslate( const Vec3f& v ) { makeTranslate( v[0], v[1], v[2] ); } void Matrix_implementation::makeTranslate( const Vec3d& v ) { makeTranslate( v[0], v[1], v[2] ); } void Matrix_implementation::makeTranslate( value_type x, value_type y, value_type z ) { SET_ROW(0, 1, 0, 0, 0 ) SET_ROW(1, 0, 1, 0, 0 ) SET_ROW(2, 0, 0, 1, 0 ) SET_ROW(3, x, y, z, 1 ) } void Matrix_implementation::makeRotate( const Vec3f& from, const Vec3f& to ) { makeIdentity(); Quat quat; quat.makeRotate(from,to); setRotate(quat); } void Matrix_implementation::makeRotate( const Vec3d& from, const Vec3d& to ) { makeIdentity(); Quat quat; quat.makeRotate(from,to); setRotate(quat); } void Matrix_implementation::makeRotate( value_type angle, const Vec3f& axis ) { makeIdentity(); Quat quat; quat.makeRotate( angle, axis); setRotate(quat); } void Matrix_implementation::makeRotate( value_type angle, const Vec3d& axis ) { makeIdentity(); Quat quat; quat.makeRotate( angle, axis); setRotate(quat); } void Matrix_implementation::makeRotate( value_type angle, value_type x, value_type y, value_type z ) { makeIdentity(); Quat quat; quat.makeRotate( angle, x, y, z); setRotate(quat); } void Matrix_implementation::makeRotate( const Quat& quat ) { makeIdentity(); setRotate(quat); } void Matrix_implementation::makeRotate( value_type angle1, const Vec3f& axis1, value_type angle2, const Vec3f& axis2, value_type angle3, const Vec3f& axis3) { makeIdentity(); Quat quat; quat.makeRotate(angle1, axis1, angle2, axis2, angle3, axis3); setRotate(quat); } void Matrix_implementation::makeRotate( value_type angle1, const Vec3d& axis1, value_type angle2, const Vec3d& axis2, value_type angle3, const Vec3d& axis3) { makeIdentity(); Quat quat; quat.makeRotate(angle1, axis1, angle2, axis2, angle3, axis3); setRotate(quat); } void Matrix_implementation::mult( const Matrix_implementation& lhs, const Matrix_implementation& rhs ) { if (&lhs==this) { postMult(rhs); return; } if (&rhs==this) { preMult(lhs); return; } // PRECONDITION: We assume neither &lhs nor &rhs == this // if it did, use preMult or postMult instead _mat[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0); _mat[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1); _mat[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2); _mat[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3); _mat[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0); _mat[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1); _mat[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2); _mat[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3); _mat[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0); _mat[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1); _mat[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2); _mat[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3); _mat[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0); _mat[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1); _mat[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2); _mat[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3); } void Matrix_implementation::preMult( const Matrix_implementation& other ) { // brute force method requiring a copy //Matrix_implementation tmp(other* *this); // *this = tmp; // more efficient method just use a value_type[4] for temporary storage. value_type t[4]; for(int col=0; col<4; ++col) { t[0] = INNER_PRODUCT( other, *this, 0, col ); t[1] = INNER_PRODUCT( other, *this, 1, col ); t[2] = INNER_PRODUCT( other, *this, 2, col ); t[3] = INNER_PRODUCT( other, *this, 3, col ); _mat[0][col] = t[0]; _mat[1][col] = t[1]; _mat[2][col] = t[2]; _mat[3][col] = t[3]; } } void Matrix_implementation::postMult( const Matrix_implementation& other ) { // brute force method requiring a copy //Matrix_implementation tmp(*this * other); // *this = tmp; // more efficient method just use a value_type[4] for temporary storage. value_type t[4]; for(int row=0; row<4; ++row) { t[0] = INNER_PRODUCT( *this, other, row, 0 ); t[1] = INNER_PRODUCT( *this, other, row, 1 ); t[2] = INNER_PRODUCT( *this, other, row, 2 ); t[3] = INNER_PRODUCT( *this, other, row, 3 ); SET_ROW(row, t[0], t[1], t[2], t[3] ) } } #undef INNER_PRODUCT // orthoNormalize the 3x3 rotation matrix void Matrix_implementation::orthoNormalize(const Matrix_implementation& rhs) { value_type x_colMag = (rhs._mat[0][0] * rhs._mat[0][0]) + (rhs._mat[1][0] * rhs._mat[1][0]) + (rhs._mat[2][0] * rhs._mat[2][0]); value_type y_colMag = (rhs._mat[0][1] * rhs._mat[0][1]) + (rhs._mat[1][1] * rhs._mat[1][1]) + (rhs._mat[2][1] * rhs._mat[2][1]); value_type z_colMag = (rhs._mat[0][2] * rhs._mat[0][2]) + (rhs._mat[1][2] * rhs._mat[1][2]) + (rhs._mat[2][2] * rhs._mat[2][2]); if(!equivalent((double)x_colMag, 1.0) && !equivalent((double)x_colMag, 0.0)) { x_colMag = sqrt(x_colMag); _mat[0][0] = rhs._mat[0][0] / x_colMag; _mat[1][0] = rhs._mat[1][0] / x_colMag; _mat[2][0] = rhs._mat[2][0] / x_colMag; } else { _mat[0][0] = rhs._mat[0][0]; _mat[1][0] = rhs._mat[1][0]; _mat[2][0] = rhs._mat[2][0]; } if(!equivalent((double)y_colMag, 1.0) && !equivalent((double)y_colMag, 0.0)) { y_colMag = sqrt(y_colMag); _mat[0][1] = rhs._mat[0][1] / y_colMag; _mat[1][1] = rhs._mat[1][1] / y_colMag; _mat[2][1] = rhs._mat[2][1] / y_colMag; } else { _mat[0][1] = rhs._mat[0][1]; _mat[1][1] = rhs._mat[1][1]; _mat[2][1] = rhs._mat[2][1]; } if(!equivalent((double)z_colMag, 1.0) && !equivalent((double)z_colMag, 0.0)) { z_colMag = sqrt(z_colMag); _mat[0][2] = rhs._mat[0][2] / z_colMag; _mat[1][2] = rhs._mat[1][2] / z_colMag; _mat[2][2] = rhs._mat[2][2] / z_colMag; } else { _mat[0][2] = rhs._mat[0][2]; _mat[1][2] = rhs._mat[1][2]; _mat[2][2] = rhs._mat[2][2]; } _mat[3][0] = rhs._mat[3][0]; _mat[3][1] = rhs._mat[3][1]; _mat[3][2] = rhs._mat[3][2]; _mat[0][3] = rhs._mat[0][3]; _mat[1][3] = rhs._mat[1][3]; _mat[2][3] = rhs._mat[2][3]; _mat[3][3] = rhs._mat[3][3]; } /****************************************** Matrix inversion technique: Given a matrix mat, we want to invert it. mat = [ r00 r01 r02 a r10 r11 r12 b r20 r21 r22 c tx ty tz d ] We note that this matrix can be split into three matrices. mat = rot * trans * corr, where rot is rotation part, trans is translation part, and corr is the correction due to perspective (if any). rot = [ r00 r01 r02 0 r10 r11 r12 0 r20 r21 r22 0 0 0 0 1 ] trans = [ 1 0 0 0 0 1 0 0 0 0 1 0 tx ty tz 1 ] corr = [ 1 0 0 px 0 1 0 py 0 0 1 pz 0 0 0 s ] where the elements of corr are obtained from linear combinations of the elements of rot, trans, and mat. So the inverse is mat' = (trans * corr)' * rot', where rot' must be computed the traditional way, which is easy since it is only a 3x3 matrix. This problem is simplified if [px py pz s] = [0 0 0 1], which will happen if mat was composed only of rotations, scales, and translations (which is common). In this case, we can ignore corr entirely which saves on a lot of computations. ******************************************/ bool Matrix_implementation::invert_4x3( const Matrix_implementation& mat ) { if (&mat==this) { Matrix_implementation tm(mat); return invert_4x3(tm); } register value_type r00, r01, r02, r10, r11, r12, r20, r21, r22; // Copy rotation components directly into registers for speed r00 = mat._mat[0][0]; r01 = mat._mat[0][1]; r02 = mat._mat[0][2]; r10 = mat._mat[1][0]; r11 = mat._mat[1][1]; r12 = mat._mat[1][2]; r20 = mat._mat[2][0]; r21 = mat._mat[2][1]; r22 = mat._mat[2][2]; // Partially compute inverse of rot _mat[0][0] = r11*r22 - r12*r21; _mat[0][1] = r02*r21 - r01*r22; _mat[0][2] = r01*r12 - r02*r11; // Compute determinant of rot from 3 elements just computed register value_type one_over_det = 1.0/(r00*_mat[0][0] + r10*_mat[0][1] + r20*_mat[0][2]); r00 *= one_over_det; r10 *= one_over_det; r20 *= one_over_det; // Saves on later computations // Finish computing inverse of rot _mat[0][0] *= one_over_det; _mat[0][1] *= one_over_det; _mat[0][2] *= one_over_det; _mat[0][3] = 0.0; _mat[1][0] = r12*r20 - r10*r22; // Have already been divided by det _mat[1][1] = r00*r22 - r02*r20; // same _mat[1][2] = r02*r10 - r00*r12; // same _mat[1][3] = 0.0; _mat[2][0] = r10*r21 - r11*r20; // Have already been divided by det _mat[2][1] = r01*r20 - r00*r21; // same _mat[2][2] = r00*r11 - r01*r10; // same _mat[2][3] = 0.0; _mat[3][3] = 1.0; // We no longer need the rxx or det variables anymore, so we can reuse them for whatever we want. But we will still rename them for the sake of clarity. #define d r22 d = mat._mat[3][3]; if( osg::square(d-1.0) > 1.0e-6 ) // Involves perspective, so we must { // compute the full inverse Matrix_implementation TPinv; _mat[3][0] = _mat[3][1] = _mat[3][2] = 0.0; #define px r00 #define py r01 #define pz r02 #define one_over_s one_over_det #define a r10 #define b r11 #define c r12 a = mat._mat[0][3]; b = mat._mat[1][3]; c = mat._mat[2][3]; px = _mat[0][0]*a + _mat[0][1]*b + _mat[0][2]*c; py = _mat[1][0]*a + _mat[1][1]*b + _mat[1][2]*c; pz = _mat[2][0]*a + _mat[2][1]*b + _mat[2][2]*c; #undef a #undef b #undef c #define tx r10 #define ty r11 #define tz r12 tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2]; one_over_s = 1.0/(d - (tx*px + ty*py + tz*pz)); tx *= one_over_s; ty *= one_over_s; tz *= one_over_s; // Reduces number of calculations later on // Compute inverse of trans*corr TPinv._mat[0][0] = tx*px + 1.0; TPinv._mat[0][1] = ty*px; TPinv._mat[0][2] = tz*px; TPinv._mat[0][3] = -px * one_over_s; TPinv._mat[1][0] = tx*py; TPinv._mat[1][1] = ty*py + 1.0; TPinv._mat[1][2] = tz*py; TPinv._mat[1][3] = -py * one_over_s; TPinv._mat[2][0] = tx*pz; TPinv._mat[2][1] = ty*pz; TPinv._mat[2][2] = tz*pz + 1.0; TPinv._mat[2][3] = -pz * one_over_s; TPinv._mat[3][0] = -tx; TPinv._mat[3][1] = -ty; TPinv._mat[3][2] = -tz; TPinv._mat[3][3] = one_over_s; preMult(TPinv); // Finish computing full inverse of mat #undef px #undef py #undef pz #undef one_over_s #undef d } else // Rightmost column is [0; 0; 0; 1] so it can be ignored { tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2]; // Compute translation components of mat' _mat[3][0] = -(tx*_mat[0][0] + ty*_mat[1][0] + tz*_mat[2][0]); _mat[3][1] = -(tx*_mat[0][1] + ty*_mat[1][1] + tz*_mat[2][1]); _mat[3][2] = -(tx*_mat[0][2] + ty*_mat[1][2] + tz*_mat[2][2]); #undef tx #undef ty #undef tz } return true; } template inline T SGL_ABS(T a) { return (a >= 0 ? a : -a); } #ifndef SGL_SWAP #define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp)) #endif bool Matrix_implementation::invert_4x4( const Matrix_implementation& mat ) { if (&mat==this) { Matrix_implementation tm(mat); return invert_4x4(tm); } unsigned int indxc[4], indxr[4], ipiv[4]; unsigned int i,j,k,l,ll; unsigned int icol = 0; unsigned int irow = 0; double temp, pivinv, dum, big; // copy in place this may be unnecessary *this = mat; for (j=0; j<4; j++) ipiv[j]=0; for(i=0;i<4;i++) { big=0.0; for (j=0; j<4; j++) if (ipiv[j] != 1) for (k=0; k<4; k++) { if (ipiv[k] == 0) { if (SGL_ABS(operator()(j,k)) >= big) { big = SGL_ABS(operator()(j,k)); irow=j; icol=k; } } else if (ipiv[k] > 1) return false; } ++(ipiv[icol]); if (irow != icol) for (l=0; l<4; l++) SGL_SWAP(operator()(irow,l), operator()(icol,l), temp); indxr[i]=irow; indxc[i]=icol; if (operator()(icol,icol) == 0) return false; pivinv = 1.0/operator()(icol,icol); operator()(icol,icol) = 1; for (l=0; l<4; l++) operator()(icol,l) *= pivinv; for (ll=0; ll<4; ll++) if (ll != icol) { dum=operator()(ll,icol); operator()(ll,icol) = 0; for (l=0; l<4; l++) operator()(ll,l) -= operator()(icol,l)*dum; } } for (int lx=4; lx>0; --lx) { if (indxr[lx-1] != indxc[lx-1]) for (k=0; k<4; k++) SGL_SWAP(operator()(k,indxr[lx-1]), operator()(k,indxc[lx-1]),temp); } return true; } void Matrix_implementation::makeOrtho(double left, double right, double bottom, double top, double zNear, double zFar) { // note transpose of Matrix_implementation wr.t OpenGL documentation, since the OSG use post multiplication rather than pre. double tx = -(right+left)/(right-left); double ty = -(top+bottom)/(top-bottom); double tz = -(zFar+zNear)/(zFar-zNear); SET_ROW(0, 2.0/(right-left), 0.0, 0.0, 0.0 ) SET_ROW(1, 0.0, 2.0/(top-bottom), 0.0, 0.0 ) SET_ROW(2, 0.0, 0.0, -2.0/(zFar-zNear), 0.0 ) SET_ROW(3, tx, ty, tz, 1.0 ) } bool Matrix_implementation::getOrtho(double& left, double& right, double& bottom, double& top, double& zNear, double& zFar) const { if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=0.0 || _mat[3][3]!=1.0) return false; zNear = (_mat[3][2]+1.0) / _mat[2][2]; zFar = (_mat[3][2]-1.0) / _mat[2][2]; left = -(1.0+_mat[3][0]) / _mat[0][0]; right = (1.0-_mat[3][0]) / _mat[0][0]; bottom = -(1.0+_mat[3][1]) / _mat[1][1]; top = (1.0-_mat[3][1]) / _mat[1][1]; return true; } void Matrix_implementation::makeFrustum(double left, double right, double bottom, double top, double zNear, double zFar) { // note transpose of Matrix_implementation wr.t OpenGL documentation, since the OSG use post multiplication rather than pre. double A = (right+left)/(right-left); double B = (top+bottom)/(top-bottom); double C = -(zFar+zNear)/(zFar-zNear); double D = -2.0*zFar*zNear/(zFar-zNear); SET_ROW(0, 2.0*zNear/(right-left), 0.0, 0.0, 0.0 ) SET_ROW(1, 0.0, 2.0*zNear/(top-bottom), 0.0, 0.0 ) SET_ROW(2, A, B, C, -1.0 ) SET_ROW(3, 0.0, 0.0, D, 0.0 ) } bool Matrix_implementation::getFrustum(double& left, double& right, double& bottom, double& top, double& zNear, double& zFar) const { if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=-1.0 || _mat[3][3]!=0.0) return false; zNear = _mat[3][2] / (_mat[2][2]-1.0); zFar = _mat[3][2] / (1.0+_mat[2][2]); left = zNear * (_mat[2][0]-1.0) / _mat[0][0]; right = zNear * (1.0+_mat[2][0]) / _mat[0][0]; top = zNear * (1.0+_mat[2][1]) / _mat[1][1]; bottom = zNear * (_mat[2][1]-1.0) / _mat[1][1]; return true; } void Matrix_implementation::makePerspective(double fovy,double aspectRatio, double zNear, double zFar) { // calculate the appropriate left, right etc. double tan_fovy = tan(DegreesToRadians(fovy*0.5)); double right = tan_fovy * aspectRatio * zNear; double left = -right; double top = tan_fovy * zNear; double bottom = -top; makeFrustum(left,right,bottom,top,zNear,zFar); } bool Matrix_implementation::getPerspective(double& fovy,double& aspectRatio, double& zNear, double& zFar) const { double right = 0.0; double left = 0.0; double top = 0.0; double bottom = 0.0; if (getFrustum(left,right,bottom,top,zNear,zFar)) { fovy = RadiansToDegrees(atan(top/zNear)-atan(bottom/zNear)); aspectRatio = (right-left)/(top-bottom); return true; } return false; } void Matrix_implementation::makeLookAt(const Vec3d& eye,const Vec3d& center,const Vec3d& up) { Vec3d f(center-eye); f.normalize(); Vec3d s(f^up); s.normalize(); Vec3d u(s^f); u.normalize(); set( s[0], u[0], -f[0], 0.0, s[1], u[1], -f[1], 0.0, s[2], u[2], -f[2], 0.0, 0.0, 0.0, 0.0, 1.0); preMult(Matrix_implementation::translate(-eye)); } void Matrix_implementation::getLookAt(Vec3f& eye,Vec3f& center,Vec3f& up,value_type lookDistance) const { Matrix_implementation inv; inv.invert(*this); eye = osg::Vec3f(0.0,0.0,0.0)*inv; up = transform3x3(*this,osg::Vec3f(0.0,1.0,0.0)); center = transform3x3(*this,osg::Vec3f(0.0,0.0,-1)); center.normalize(); center = eye + center*lookDistance; } void Matrix_implementation::getLookAt(Vec3d& eye,Vec3d& center,Vec3d& up,value_type lookDistance) const { Matrix_implementation inv; inv.invert(*this); eye = osg::Vec3d(0.0,0.0,0.0)*inv; up = transform3x3(*this,osg::Vec3d(0.0,1.0,0.0)); center = transform3x3(*this,osg::Vec3d(0.0,0.0,-1)); center.normalize(); center = eye + center*lookDistance; } #undef SET_ROW