/// Spherical Linear Interpolation /// As t goes from 0 to 1, the Quat object goes from "from" to "to" /// Reference: Shoemake at SIGGRAPH 89 /// See also /// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm void ofQuaternion::slerp( float t, const ofQuaternion& from, const ofQuaternion& to ) { const double epsilon = 0.00001; double omega, cosomega, sinomega, scale_from, scale_to ; ofQuaternion quatTo(to); // this is a dot product cosomega = from.asVec4().dot(to.asVec4()); if ( cosomega < 0.0 ) { cosomega = -cosomega; quatTo = -to; } if ( (1.0 - cosomega) > epsilon ) { omega = acos(cosomega) ; // 0 <= omega <= Pi (see man acos) sinomega = sin(omega) ; // this sinomega should always be +ve so // could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()? scale_from = sin((1.0 - t) * omega) / sinomega ; scale_to = sin(t * omega) / sinomega ; } else { /* -------------------------------------------------- The ends of the vectors are very close we can use simple linear interpolation - no need to worry about the "spherical" interpolation -------------------------------------------------- */ scale_from = 1.0 - t ; scale_to = t ; } *this = (from * scale_from) + (quatTo * scale_to); // so that we get a Vec4 }
void ofxRotate(ofQuaternion q) { //rotation float angle; ofVec3f axis; q.getRotate(angle, axis); ofRotate(angle/TWO_PI*360,axis.x,axis.y,axis.z); }
//------------------------------------------------------------ void Hammer::setRotation(ofQuaternion rotation){ btTransform transform; btRigidBody* rigidBody = body.getRigidBody(); rigidBody->getMotionState()->getWorldTransform( transform ); btQuaternion originRot; originRot.setX(rotation.x()); originRot.setY(rotation.y()); originRot.setZ(rotation.z()); originRot.setW(rotation.w()); transform.setRotation(originRot); rigidBody->getMotionState()->setWorldTransform( transform ); }
void KalmanEuler_<T>::update(const ofQuaternion& q) { // warp to appropriate dimension ofVec3f euler = q.getEuler(); for( int i = 0; i < 3; i++ ) { float rev = floorf((eulerPrev[i] + 180) / 360.f) * 360; euler[i] += rev; if( euler[i] < -90 + rev && eulerPrev[i] > 90 + rev ) euler[i] += 360; else if( euler[i] > 90 + rev && eulerPrev[i] < -90 + rev ) euler[i] -= 360; } KalmanPosition_<T>::update(euler); eulerPrev = euler; }
ofxLatLon ofxToLatLon(ofQuaternion q) { ofVec3f c; ofVec4f v(0,0,-1,0); ofMatrix4x4 m; q.get(m); ofVec4f mv = m*v; c.set(mv.x,mv.y,-mv.z); c.rotate(90, 0, 0); float lat = ofRadToDeg(asin(c.z)); float lon = ofRadToDeg(-atan2(c.y,c.x))-90; if (lon<-180) lon+=360; return ofxLatLon(lat,lon); }
//-------------------------------------------------------------- void ofxBulletCapsule::create( btDiscreteDynamicsWorld* a_world, ofVec3f a_loc, ofQuaternion a_rot, float a_mass, float a_radius, float a_height ) { btTransform tr = ofGetBtTransformFromVec3f( a_loc ); tr.setRotation( btQuaternion(btVector3(a_rot.x(), a_rot.y(), a_rot.z()), a_rot.w()) ); create( a_world, tr, a_mass, a_radius, a_height ); }
ofQuaternion testApp::lerpQuat(float t, ofQuaternion qa, ofQuaternion qb) { ofQuaternion qm; //dot product float cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z(); if (abs(cosHalfTheta) >= 1.0) { return qa; } else { // Calculate temporary values. float halfTheta = acos(cosHalfTheta); float sinHalfTheta = sqrt(1.0 - cosHalfTheta*cosHalfTheta); // if theta = 180 degrees then result is not fully defined // we could rotate around any axis normal to qa or qb if (fabs(sinHalfTheta) < 0.001){ // fabs is floating point absolute qm.set( (qa.x() * 0.5 + qb.x() * 0.5), (qa.y() * 0.5 + qb.y() * 0.5), (qa.z() * 0.5 + qb.z() * 0.5), (qa.w() * 0.5 + qb.w() * 0.5) ); return qm; } float ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; float ratioB = sin(t * halfTheta) / sinHalfTheta; //calculate Quaternion qm.set( (qa.x() * ratioA + qb.x() * ratioB), (qa.y() * ratioA + qb.y() * ratioB), (qa.z() * ratioA + qb.z() * ratioB), (qa.w() * ratioA + qb.w() * ratioB) ); return qm; } }
ofVec3f ofxToCartesian(ofQuaternion q) { float angle; ofVec3f vec; q.getRotate(angle, vec); return ofVec3f(0,0,1).rotated(angle, vec); }
string ofxToString(ofQuaternion q) { return ofToString(q.x()) + "," + ofToString(q.y()) + "," + ofToString(q.z()) + "," + ofToString(q.w()); }
string DebugUtil::toString(ofQuaternion quat) { ofVec3f vec = quat.getEuler(); return "alpha:" + ofToString(vec.x) + ", beta:" + ofToString(vec.y) + ", gamma:" + ofToString(vec.z); }
//-------------------------------------------------------------------------- void game::getMatrix( GLfloat * m, ofQuaternion quat ) { float x2 = quat.x() * quat.x(); float y2 = quat.y() * quat.y(); float z2 = quat.z() * quat.z(); float xy = quat.x() * quat.y(); float xz = quat.x() * quat.z(); float yz = quat.y() * quat.z(); float wx = quat.w() * quat.x(); float wy = quat.w() * quat.y(); float wz = quat.w() * quat.z(); m[0] = 1.0f - 2.0f * (y2 + z2); m[1] = 2.0f * (xy - wz); m[2] = 2.0f * (xz + wy); m[3] = 0.0f; m[4] = 2.0f * (xy + wz); m[5] = 1.0f - 2.0f * (x2 + z2); m[6] = 2.0f * (yz - wx); m[7] = 0.0f; m[8] = 2.0f * (xz - wy); m[9] = 2.0f * (yz + wx); m[10] = 1.0f - 2.0f * (x2 + y2); m[11] = 0.0f; m[12] = 0.0f; m[13] = 0.0f; m[14] = 0.0f; m[15] = 1.0f; }