/// 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;
}
