osg::Quat lerp_Quat(osg::Quat v0, osg::Quat v1, double t)
{
osg::Quat vec;
vec.x() = lerp_lf(v0.x(), v1.x(), t);
vec.y() = lerp_lf(v0.y(), v1.y(), t);
vec.z() = lerp_lf(v0.z(), v1.z(), t);
vec.w() = lerp_lf(v0.w(), v1.w(), t);
return vec;
}
// ================================================
// updateMotionTracker
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
void PluginMotionTrackerRecvr::updateMotionTracker( int trackerID, int viewID,
osg::Quat orientation )
{
if( trackerParamsMap == NULL )
return;
TrackerParams *trackerParams = (*trackerParamsMap)[trackerID];
if( trackerParams == NULL )
{
// The tracker ID specified doesn't yet have an entry in
// trackerParamsMap. It will be created and added to the map.
// The default values set by TrackerParams constructor should be good
// enough.
(*trackerParamsMap)[trackerID] = trackerParams = new TrackerParams;
}
// this tracker plugin generates quaternions
trackerParams->useQuaternion = true;
trackerParams->trackerRotateQuat[0] = orientation.x();
trackerParams->trackerRotateQuat[1] = orientation.y();
trackerParams->trackerRotateQuat[2] = orientation.z();
trackerParams->trackerRotateQuat[3] = orientation.w();
trackerParams->trackerAbsoluteYaw = 0.0;
trackerParams->trackerOffset[0] = 0.0;
trackerParams->trackerOffset[1] = 0.0;
trackerParams->trackerOffset[2] = 0.0;
}
unsigned int PhysicsEngine::addUserVehicle(const osg::Vec3f& pos, const osg::Vec3f& sizes, const osg::Quat& orient, float mass)
{
assert(OpenThreads::Thread::CurrentThread() == m_physicsThread);
if (! m_vehicleShape)
m_vehicleShape = new btBoxShape(btVector3(sizes.x() / 2, sizes.y() / 2, sizes.z() / 2));
//m_collisionShapes.push_back(m_vehicleShape);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setRotation(btQuaternion(orient.x(), orient.y(), orient.z(), orient.w()));
startTransform.setOrigin(btVector3(pos.x(), pos.y(), pos.z()));
VehicleMotionState* motionState = new VehicleMotionState(this, m_nextUsedId, startTransform);
btVector3 inertia;
m_vehicleShape->calculateLocalInertia(mass, inertia);
btRigidBody* body = new btRigidBody(mass, motionState, m_vehicleShape, inertia);
m_vehicles[m_nextUsedId] = body;
++m_nextUsedId;
body->setDamping(0.5f, 0.7f);
m_physicsWorld->addRigidBody(body, COLLISION_SHIPGROUP, COLLISION_SHIPGROUP | COLLISION_ASTEROIDGROUP);
return m_nextUsedId - 1;
}
osg::Vec3d Math::fromQuat(const osg::Quat& quat)
{
// From: http://guardian.curtin.edu.au/cga/faq/angles.html
// Except OSG exchanges pitch & roll from what is listed on that page
double qx = quat.x();
double qy = quat.y();
double qz = quat.z();
double qw = quat.w();
double sqx = qx * qx;
double sqy = qy * qy;
double sqz = qz * qz;
double sqw = qw * qw;
double term1 = 2 * (qx*qy + qw*qz);
double term2 = sqw + sqx - sqy - sqz;
double term3 = -2 * (qx*qz - qw*qy);
double term4 = 2 * (qw*qx + qy*qz);
double term5 = sqw - sqx - sqy + sqz;
double heading = atan2(term1, term2);
double pitch = atan2(term4, term5);
double roll = asin(term3);
return osg::Vec3d(heading, pitch, roll);
}
void DataOutputStream::writeQuat(const osg::Quat& q){
writeFloat(q.x());
writeFloat(q.y());
writeFloat(q.z());
writeFloat(q.w());
if (_verboseOutput) std::cout<<"read/writeQuat() ["<<q<<"]"<<std::endl;
}
void CamInfo::setCamRot(osg::Quat rot)
{
CamRot.set(rot.x(),rot.y(),rot.z(),rot.w());
}
bool Node::equivalent(const osg::Quat& q0, const osg::Quat& q1) {
return osg::equivalent(q0.x(), q1.x(), 1e-4)
&& osg::equivalent(q0.y(), q1.y(), 1e-4)
&& osg::equivalent(q0.z(), q1.z(), 1e-4)
&& osg::equivalent(q0.w(), q1.w(), 1e-4);
}
