void ComputeOrientation(const osg::Vec3& lv,const osg::Vec3& up, osg::Quat &rotationDest)
{
osg::Vec3 f(lv);
f.normalize();
osg::Vec3 s(f^up);
s.normalize();
osg::Vec3 u(s^f);
u.normalize();


{
__asm NOP
}


osg::Matrix rotation_matrix(s[0],     u[0],     -f[0],     0.0f,
                            s[1],     u[1],     -f[1],     0.0f,
                            s[2],     u[2],     -f[2],     0.0f,
                            0.0f,     0.0f,     0.0f,      1.0f);
rotationDest.set(rotation_matrix);
//debug_matrix = rotation_matrix;
//debug_quat = rotationDest;
rotationDest = rotationDest.inverse();

} // ComputeOrientation

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

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

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 daeWriter::writeUpdateTransformElements(const osg::Vec3 &pos, const osg::Quat &q,    const osg::Vec3 &s)
{
    // Make a scale place element
    domScale *scale = daeSafeCast< domScale >( currentNode->add( COLLADA_ELEMENT_SCALE ) );
    scale->setSid("scale");
    scale->getValue().append3( s.x(), s.y(), s.z() );

    // Make a three rotate place elements for the euler angles
    // TODO decompose quaternion into three euler angles
    osg::Quat::value_type angle;
    osg::Vec3 axis;
    q.getRotate( angle, axis );

    domRotate *rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
    rot->setSid("rotateZ");
    rot->getValue().append4( 0, 0, 1, osg::RadiansToDegrees(angle) );

    rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
    rot->setSid("rotateY");
    rot->getValue().append4( 0, 1, 0, osg::RadiansToDegrees(angle) );

    rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
    rot->setSid("rotateX");
    rot->getValue().append4( 1, 0, 0, osg::RadiansToDegrees(angle) );

    // Make a translate place element
    domTranslate *trans = daeSafeCast< domTranslate >( currentNode->add( COLLADA_ELEMENT_TRANSLATE ) );
    trans->setSid("translate");
    trans->getValue().append3( pos.x(), pos.y(), pos.z() );
}

bool FrameManager::getTransform(const std::string& frame, ros::Time time, osg::Vec3d& position, osg::Quat& orientation)
{
    boost::mutex::scoped_lock lock(cache_mutex_);

    position = osg::Vec3d(9999999, 9999999, 9999999);
    orientation.set(0,0,0,1);

    if (fixed_frame_.empty())
    {
        return false;
    }

    M_Cache::iterator it = cache_.find(CacheKey(frame, time));
    if (it != cache_.end())
    {
        position = it->second.position;
        orientation = it->second.orientation;
        return true;
    }

    geometry_msgs::Pose pose;
    pose.orientation.w = 1.0f;

    if (!transform(frame, time, pose, position, orientation))
    {
        return false;
    }

    cache_.insert(std::make_pair(CacheKey(frame, time), CacheEntry(position, orientation)));

    return true;
}

/* Public functions */
void OculusHealthAndSafetyWarning::updatePosition(osg::Matrix cameraMatrix, osg::Vec3 position, osg::Quat orientation) {
	if (m_transform.valid()) {
		osg::Matrix matrix;
		matrix.setTrans(osg::Vec3(0.0, 0.0, -m_distance));
		matrix.postMultRotate(orientation.inverse());
		matrix.postMultTranslate(-position);
		m_transform->setMatrix(matrix*cameraMatrix);
	}
}

static void invertOSGTransform(osg::Vec3d& trans, osg::Quat& quat,
        osg::PositionAttitudeTransform*& source, osg::PositionAttitudeTransform*& target,
        std::string& source_frame, std::string& target_frame)
{
    quat = quat.inverse();
    trans = -(quat * trans);
    std::swap(source, target);
    std::swap(source_frame, target_frame);
}

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

inline void copyOsgQuatToLib3dsQuat(float lib3ds_vector[4], const osg::Quat& osg_quat)
{
    //lib3ds_vector[0] = osg_quat[3];        // Not sure
    //lib3ds_vector[1] = osg_quat[0];
    //lib3ds_vector[2] = osg_quat[1];
    //lib3ds_vector[3] = osg_quat[2];
    // 3DS seems to store (angle in radians, axis_x, axis_y, axis_z), but it works with (axis_x, axis_y, axis_z, -angle in radians)!
    osg::Quat::value_type angle, x, y, z;
    osg_quat.getRotate(angle, x, y, z);
    lib3ds_vector[0] = static_cast<float>(x);
    lib3ds_vector[1] = static_cast<float>(y);
    lib3ds_vector[2] = static_cast<float>(z);
    lib3ds_vector[3] = static_cast<float>(-angle);
}

void Node::quat_to_euler(osg::Quat& q, osg::Vec3& euler) {
	//To calculate back the euler angles from a quaternion we use this formulas
	//but since we want the postmultiplication order we use the conjugate of the
	//quaternion, also there has to be a change in the sign of the angles
	//http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
	osg::Quat q_c = q.conj();
	euler[0] = -std::atan2(2.0 * (q_c.y() * q_c.z() + q_c.w() * q_c.x()),
			q_c.w() * q_c.w() - q_c.x() * q_c.x() - q_c.y() * q_c.y()
					+ q_c.z() * q_c.z());
	euler[1] = -std::asin(-2.0 * (q_c.x() * q_c.z() - q_c.w() * q_c.y()));
	euler[2] = -std::atan2(2.0 * (q_c.x() * q_c.y() + q_c.w() * q_c.z()),
			q_c.w() * q_c.w() + q_c.x() * q_c.x() - q_c.y() * q_c.y()
					- q_c.z() * q_c.z());
}

osg::PositionAttitudeTransform* creation_CHARRR(float posx, float posy, osg::Node* terrain){
	osg::Node* LECHARRR = osgDB::readNodeFile("t72-tank_des.flt");
	osg::PositionAttitudeTransform* pos_tank = new osg::PositionAttitudeTransform;
	osg::Vec3 pos, normal;
	intersection_terrain(posx, posy, terrain, pos, normal);
	pos_tank->setPosition(pos);
	osg::Quat rotation;
	rotation.makeRotate(osg::Vec3f(0, 0, 1), normal);
	pos_tank->setAttitude(rotation);
	pos_tank->addChild(LECHARRR);
	pos_tank->setUpdateCallback(new Deplacement);
	
	return pos_tank;
}

bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, osg::Vec3d& position, osg::Quat& orientation)
{
    position.set(0,0,0);
    orientation.set(0,0,0,1);

    // put all pose data into a tf stamped pose
    btQuaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
    btVector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z);

    if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0)
    {
        bt_orientation.setW(1.0);
    }

    tf::Stamped<tf::Pose> pose_in(btTransform(bt_orientation,bt_position), time, frame);
    tf::Stamped<tf::Pose> pose_out;

    // convert pose into new frame
    try
    {
        tf_->transformPose( fixed_frame_, pose_in, pose_out );
    }
    catch(tf::TransformException& e)
    {
        ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what());
        return false;
    }

#if ROS_VERSION_MINIMUM(1, 8, 0)
    //ROS Fuerte version
    bt_position = pose_out.asBt().getOrigin();
    bt_orientation = pose_out.asBt().getRotation();
#else
    bt_position = pose_out.getOrigin();
    bt_orientation = pose_out.getRotation();
#endif
    position = osg::Vec3d(bt_position.x(), bt_position.y(), bt_position.z());
    orientation = osg::Quat( bt_orientation.x(), bt_orientation.y(), bt_orientation.z(), bt_orientation.w() );

    return true;
}

void FlexDrawable::ConvexManager::add( const osg::Vec3& pos, const osg::Quat& q,
                                       const std::vector<osg::Plane>& faces,
                                       const osg::BoundingBox& bb, bool isStatic )
{
    unsigned int startIndex = starts.size();
    for ( unsigned int i=0; i<faces.size(); ++i )
    {
        const osg::Plane& plane = faces[i];
        planes.push_back( osg::Vec4f(plane[0], plane[1], plane[2], plane[3]) );
    }
    
    starts.push_back( startIndex );
    lengths.push_back( faces.size() );
    flags.push_back( isStatic ? 0 : 1 );
    
    positions.push_back( osg::Vec4(pos, 0.0f) );
    rotations.push_back( q.asVec4() );
    prevPositions.push_back( positions.back() );
    prevRotations.push_back( rotations.back() );
    
    aabbMin.push_back( osg::Vec4(bb._min, 0.0f) );
    aabbMax.push_back( osg::Vec4(bb._max, 0.0f) );
}

int InterpretCameraForUse(NVAnimObject *Cam, double Moment, osg::Vec3 &EyePoint, osg::Quat &Orientation, float &HFOV)
{
int NumValid;

float HFOV_S, HFOV_N, HFOV_E;

NumValid = Cam->GetBracketingKeyFrames(Moment, PrevKey, NextKey, false); // don't use caching yet, we're not set up for it

if(NumValid)
	{
	if(NumValid == 2)
		{ // interpolate
		double KeyTimeDif, MomentTimeDif;

		//determine interpolation fraction
		KeyTimeDif = NextKey->GetTimeValue() - PrevKey->GetTimeValue();
		MomentTimeDif = Moment - PrevKey->GetTimeValue();

		if(KeyTimeDif > 0.0 && MomentTimeDif >= 0.0) // do rationality checking before a risky divide
			{
			double MomentFrac;
			osg::Vec3 EyePoint_S, EyePoint_E;
			osg::Quat Orientation_S, Orientation_E;

			MomentFrac = MomentTimeDif / KeyTimeDif; // divide by zero prevented by rationality checking above
			MiscReadout = MomentFrac;
			if(MomentFrac > 1.0)
				{
				Cam->GetBracketingKeyFrames(Moment, PrevKey, NextKey, false); // trace only
				} // if

			// HFOV
			HFOV_S = PrevKey->ChannelValues[NVAnimObject::CANONHANGLE];
			HFOV_E = NextKey->ChannelValues[NVAnimObject::CANONHANGLE];
			HFOV_N = flerp(MomentFrac, HFOV_S, HFOV_E);
			HFOV = HFOV_N;

			// create start and end eye/orient pairs
			InterpretCameraConfiguration(PrevKey, EyePoint_S, Orientation_S);
			InterpretCameraConfiguration(NextKey, EyePoint_E, Orientation_E);

			// Eye position
			EyePoint = EyePoint_S + ((EyePoint_E - EyePoint_S) * MomentFrac);

			// Orientation quat via slerp
			Orientation.slerp(MomentFrac, Orientation_S, Orientation_E);

			return(1);
			} // if
		else
			{ // drop back ten and punt by going with NumValid == 1 code below
			NumValid = 1;
			} // else
		} // if
	if(NumValid == 1) // not an else-if, this is a fail-safe case if the above NumValid=2 fails somehow
		{ // only have one, only use PrevKey, NextKey is identical and tweening would be a waste of time
		InterpretCameraConfiguration(PrevKey, EyePoint, Orientation);
		HFOV = PrevKey->ChannelValues[NVAnimObject::CANONHANGLE];
		} // else
	} // if
return(0);
} // InterpretCameraForUse

static boost::python::tuple getRotate_7cd433cce0eabe855a9b774688d2845e( ::osg::Quat const & inst ){
    double angle2;
    osg::Vec3f vec2;
    inst.getRotate(angle2, vec2);
    return bp::make_tuple( angle2, vec2 );
}

void CamInfo::setCamRot(osg::Quat rot)
{
	CamRot.set(rot.x(),rot.y(),rot.z(),rot.w());
}

bool CameraFlight::buttonEvent(int type/*, const osg::Matrix & mat*/)
{
//    osg::Matrix curMatrix = SceneManager::instance()->getObjectTransform()->getMatrix();
//    double curScale = SceneManager::instance()->getObjectScale();

 //   osg::Matrix w2o = SceneManager::instance()->getWorldToObjectTransform();
 //   osg::Matrix o2w = SceneManager::instance()->getObjectToWorldTransform();

    if(type == 'p') {
	std::cerr<<"curMatrix"<<endl;
	printMat(curMatrix, curScale);
	
	cout<<"x = "<<latLonHeight.x()<<", y = "<<latLonHeight.y()<<", z = "<<latLonHeight.z()<<endl;
//	std::cerr<<"WorldToObject"<<endl;
//	printMat(w2o, curScale);

//	std::cerr<<"ObjectToWorld"<<endl;
//	printMat(o2w, curScale);
    }

    else if(type == 'd') {
	curMatrix.decompose(trans1, rot1, scale1, so1);

	std::cerr<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
	cout<<"Trans = ";
	printVec(trans1);

	cout<<"Scale = ";
	printVec(scale1);

	cout<<"Rotate = ";
	printQuat(rot1);

	cout<<"Scale Orient =";
	printQuat(so1);

	std::cerr<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
	    //osg::Matrix _trans = osg::Matrix::rotate(oldPos,currentPos) * osg::Matrix::translate(trans);
	
	    
	    //osg::Matrix rotMat = osg::Matrix::rotate(oldPos,currentPos);
	    //osg::Matrix wr = o2w * rotMat* w2o; 
//	    osg::Matrix _tmp = w2o * osg::Matrix::rotate(oldPos,currentPos) * o2w;
	    //osg::Matrix _scale = osg::Matrix::scale(scale);
	    //osg::Matrix _temp = osg::Matrix::translate(-trans) * _scale * _trans;

    }

    else if(type == 't') {
	curMatrix.setTrans(osg::Vec3(0.0,1e+09/*6.41844e+09*/,0));
	SceneManager::instance()->setObjectMatrix(curMatrix);
    }

    else if(type == 's') {
	_origMatrix = SceneManager::instance()->getObjectTransform()->getMatrix();
	_origScale = SceneManager::instance()->getObjectScale();
    }

    else if(type == 'z') {
	tstart = time(0);
//	zIn = 1e+10; 
//	zOut = 1e+10;
	cout<<"zpressed"<<endl;
	if (flagZoom == false)
	    flagZoom = true;
    }

    else if(type == 'r') {

	if (flagRot == false) {
	    flagRot = true;
	}
	else {
	    flagRot = false;
	}

	//cout<<"Old Matrix"<<endl;
	//curMatrix = SceneManager::instance()->getObjectTransform()->getMatrix();
	//curScale = SceneManager::instance()->getObjectScale();

//	printMat(curMatrix, curScale);

	
	/*osg::Matrix mat2 = */

        osg::Matrix rotM;	
	rotM.makeRotate(DegreesToRadians(1.0),osg::Vec3(0,1,0)); 
//	printMat(rotM, curScale);
	
	curMatrix= o2w * rotM * w2o;
	
  //      printMat(curMatrix, curScale);

	//curMatrix.setTrans(trans);
	//cout<<"New Matrix"<<endl;
	//printMat(curMatrix, curScale);
	cout<<"x = "<<origPlanetPoint[0]<<", y = "<<origPlanetPoint[1]<<", z = "<<origPlanetPoint[2]<<endl;

	osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();

//.........这里部分代码省略.........

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