imu::Quaternion Adafruit_BNO055::getQuat(void)
{
  uint8_t buffer[8];
  memset (buffer, 0, 8);
  
  int16_t x, y, z, w;
  x = y = z = w = 0;
  
  /* Read quat data (8 bytes) */
  readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
  w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
  x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
  y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
  z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);

  /* Assign to Quaternion */
  imu::Quaternion quat((double)w, (double)x, (double)y, (double)z);
  return quat;
}

void ChiselServer::PublishColorPose()
{
    chisel::Transform lastPose = colorCamera.lastPose;

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = baseTransform;
    pose.header.stamp = colorCamera.lastImageTimestamp;
    pose.pose.position.x = lastPose.translation()(0);
    pose.pose.position.y = lastPose.translation()(1);
    pose.pose.position.z = lastPose.translation()(2);

    chisel::Quaternion quat(lastPose.rotation());
    pose.pose.orientation.x = quat.x();
    pose.pose.orientation.y = quat.y();
    pose.pose.orientation.z = quat.z();
    pose.pose.orientation.w = quat.w();

    colorCamera.lastPosePublisher.publish(pose);
}

        void on_change(const vrpn_TRACKERCB t)
        {
            tf::Vector3 pos(t.pos[0], t.pos[1], t.pos[2]);
            pos = m_transform(pos);
            m_target.transform.translation.x = pos.x() * m_scaling.x();
            m_target.transform.translation.y = pos.y() * m_scaling.y();
            m_target.transform.translation.z = pos.z() * m_scaling.z();

            tf::Quaternion quat(t.quat[0], t.quat[1], t.quat[2], t.quat[3]);
            quat = m_transform * quat;
            m_target.transform.rotation.x = quat.x() * sign(m_scaling.x());
            m_target.transform.rotation.y = quat.y() * sign(m_scaling.y());
            m_target.transform.rotation.z = quat.z() * sign(m_scaling.z());
            m_target.transform.rotation.w = quat.w();

            m_target.header.stamp = ros::Time::now();

            m_br.sendTransform(m_target);
        }

TrackballCamera::TrackballCamera(float w, float h)
{
	SetWindow(w, h);
	vec3 mCenter = vec3(0.0f);
	float mRadius = 1.0f;
	quat mQuat = quat();

	vec3 pos = vec3(0.0f, 0.0f, 4.0f);
	vec3 target = vec3(0.0f);

	setMmworldQuat();
	//mmWorld = scale(mat4(1.0f), vec3(0.5f));
	SetView(pos, target);
	SetProj(45.0f, (float)windowWidth / windowHeight, 0.1f, 100.f);

	mbMouseLButtonDown = false;
	mbMouseWheelRoll = false;
	mbMouseRButtonDown = false;
}

void RigidBody3::Update(real dt)
{
	// linear movement
	vec3 temp = forces * inverseMass * dt;

	linearVelocity = temp + linearVelocity;
	pos = 0.5f * temp * dt + linearVelocity * dt + pos;
	
	// angular movement
	temp = torques * modelMatrix.RotateFromLocalToGlobal(inverseInertiaTensor) * dt;

	angularVelocity = temp + angularVelocity;
	ori =  0.5 * quat(0, 0.5f * temp + angularVelocity) * ori * dt + ori;

	ori.Normalise();
	
	// update derived data
	UpdateWorldMatrix();
}

void OculusHeadRotation::predict(GVRActivity& gvrActivity, const ovrFrameParms& frameParms, const float time) {
    if (docked_) {
        ovrMobile* ovr = gvrActivity.getOculusContext();
        ovrTracking tracking = vrapi_GetPredictedTracking(ovr,
                vrapi_GetPredictedDisplayTime(ovr, frameParms.FrameIndex));
        tracking = vrapi_ApplyHeadModel(gvrActivity.getOculusHeadModelParms(), &tracking);

        const ovrQuatf& orientation = tracking.HeadPose.Pose.Orientation;
        glm::quat quat(orientation.w, orientation.x, orientation.y, orientation.z);
        gvrActivity.cameraRig_->setRotation(glm::conjugate(glm::inverse(quat)));
        const ovrVector3f& position = tracking.HeadPose.Pose.Position;
        glm::vec3 pos(position.x, position.y, position.z);
        gvrActivity.cameraRig_->setPosition(pos);
    } else if (nullptr != gvrActivity.cameraRig_) {
        gvrActivity.cameraRig_->predict(time);
    } else {
        gvrActivity.cameraRig_->setRotation(glm::quat());
    }
}

 virtual bool handle( const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa )
 {
     switch ( ea.getEventType() )
     {
     case osgGA::GUIEventAdapter::FRAME:  // Update transform values
         if ( _scene.valid() )
         {
             osg::Vec3 pos(position[0], position[1], position[2]);
             osg::Quat quat(rotation[0], rotation[1], rotation[2], rotation[3]);
             _scene->setMatrix( osg::Matrix::rotate(quat) * osg::Matrix::translate(pos) );
         }
         return false;
     }
     
     // As AntTweakBar handle all events within the OpenGL context, we have to record the event here
     // and process it later in the draw callback
     _eventsToHandle.push( &ea );
     return false;
 }

void SplatTestApp::update() {
  if (spaceNav) {
    spaceNav->update();
  }

  {
    const auto &ori = cameraBody.orientation;
    cameraBody.impulse = glm::rotate(ori, cameraTranslation * vec3(1, 1, -1)) * 0.000005f;
    cameraBody.angularImpulse = quat(cameraRotation * vec3(1, 1, -1) * 0.00000333f);
  }

  cameraBody.step();
  cameraBody.applyTransform(camera);

  particleSys->update(getElapsedSeconds(), getElapsedFrames(), cameraBody.position,
                      cameraBody.position - cameraBody.positionPrev, camera.getViewDirection());

  updateGui();
}

quat quat::slerp(const quat &q1,const quat &q2,float t)
{
    const float eps=0.001f;
    float scale0,scale1;

    const float cosom=q1.v.dot(q2.v)+q1.w*q2.w;
    if(cosom<0.0f)
    {
        if(1.0f+cosom>eps)
        {
            const float omega=acosf(-cosom);
            const float sinom_inv=1.0f/sinf(omega);
            scale0=sinf((1.0f-t)*omega)*sinom_inv;
            scale1= -sinf(t*omega)*sinom_inv;
        }
        else
        {
            scale0=1.0f-t;
            scale1= -t;
        }
    }
    else
    {
        if(1.0f-cosom>eps)
        {
            const float omega=acosf(cosom);
            const float sinom_inv=1.0f/sinf(omega);
            scale0=sinf((1.0f-t)*omega)*sinom_inv;
            scale1=sinf(t*omega)*sinom_inv;
        }
        else
        {
            scale0=1.0f-t;
            scale1=t;
        }
    }

    return quat(scale0*q1.v.x+scale1*q2.v.x,
                scale0*q1.v.y+scale1*q2.v.y,
                scale0*q1.v.z+scale1*q2.v.z,
                scale0*q1.w+scale1*q2.w);
}

void SOTCompensator::setSOTCompensation(const std::string& joint_name, const int& idx)
{
    double w,x,y,z;
    nh_.param<double>("/sot_controller/"+joint_name+"/quat/w", w, 1);
    nh_.param<double>("/sot_controller/"+joint_name+"/quat/y", x, 0);
    nh_.param<double>("/sot_controller/"+joint_name+"/quat/y", y, 0);
    nh_.param<double>("/sot_controller/"+joint_name+"/quat/z", z, 0);


    std::cerr << "param server was: " << w << ";;"<< x << ";;"<< y << ";;"<< z << ";;"<<std::endl;


    // converting first to TF then to dynamic graph!
    // this is for sure not cool but does the job for now
    // maybe even operate directly in the sot-controller/sot-device where those paramteres are getting created
    // FIX THAT!

    tf::Quaternion quat(x,y,z,w);
    tf::Matrix3x3 rot;
    rot.setRotation(quat);

    // check if tf rotation is the same as the compensation
    // MAYBE THE COPY IS WRONGLY ADDRESSED !!



    boost::shared_ptr<dynamicgraph::Matrix> compensation
            = boost::shared_ptr<dynamicgraph::Matrix>(new dynamicgraph::Matrix(4,4));

    std::cerr << "compensation for joint: " << joint_name << std::endl;
    compensation->setZero();
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            compensation->elementAt(i,j) = rot[i][j];
            std::cerr << "sot compensation at: " << i
                      << " " <<j << " = " << compensation->elementAt(i,j) << std::endl;
        }
    }

    compensation->elementAt(3,3) = 1;
    compensation_matrix_vec[idx] = compensation;
}

	void llquat_test_object_t::test<13>()
	{
		LLQuaternion quat(23.5f, 34.5f, 16723.4f, 324.7f);
		LLQuaternion result = -quat;
		ensure(
			"1. LLQuaternion operator-(const LLQuaternion &a) failed", 
			(-23.5f == result.mQ[0]) &&
			(-34.5f == result.mQ[1]) &&
			(-16723.4f == result.mQ[2]) &&
			(-324.7f == result.mQ[3]));

		LLQuaternion quat1(-3.5f, -34.5f, -16.4f, -154.7f);
		result = -quat1;
		ensure(
			"2. LLQuaternion operator-(const LLQuaternion &a) failed.", 
			(3.5f == result.mQ[0]) &&
			(34.5f == result.mQ[1]) &&
			(16.4f == result.mQ[2]) &&
			(154.7f == result.mQ[3]));
	}

	void llquat_test_object_t::test<11>()
	{
		LLVector3 vect(12.0f, 5.0f, 60.0f);
		LLQuaternion quat(23.5f, 6.5f, 3.23f, 56.5f);
		LLVector3 result = vect * quat;
		ensure(
			"1. LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot) failed", 
			is_approx_equal(97182.953125f,result.mV[0]) &&
			is_approx_equal(-135405.640625f, result.mV[1]) &&
			is_approx_equal(162986.140f, result.mV[2]));

		LLVector3 vect1(5.0f, 40.0f, 78.1f);
		LLQuaternion quat1(2.034f, 45.5f, 37.23f, 7.5f);
		result = vect1 * quat1;
		ensure(
			"2. LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot) failed", 
			is_approx_equal(33217.703f, result.mV[0]) &&
			is_approx_equal(295383.8125f, result.mV[1]) &&
			is_approx_equal(84718.140f, result.mV[2]));
	}

bool dev::sixense::process_tick(app::event *E)
{
    const double dt = E->data.tick.dt;

    if (::host->root())
        translate();

    if (flying)
    {
        quat o = ::host->get_orientation();
        quat q = normal(inverse(init_q) * curr_q);
        quat r = normal(slerp(quat(), q, 1.0 / 30.0));
        vec3 d = (curr_p - init_p) * dt * move_rate;

        ::host->set_orientation(o * r);
        ::host->offset_position(o * d);
    }

    return false;
}

Eigen::Quaterniond Filter::sampleQuasiQuTEM(Eigen::Quaterniond mean, double sigma, double sigma1, double sigma2, double sigma3)
{
	int n[4];
	double x[4];
	
	//Quasi Gaussian law
	for(int i=0 ; i<4 ; i++)
	{
		n[i] = (int)floor(mVectorQMC[mIndexQMC+i]/mPasUnite);
		
		/*if (n[i]==0 || n[i]==mNbEchantillons-2)
		{
			cout << "Error in function sampleQuasiQuTEM" << endl;
			cout << n[i] << endl;
		}*/
		
		x[i] = mGaussCDFInv[n[i]] + ( (mGaussCDFInv[n[i]+1] - mGaussCDFInv[n[i]]) * (mVectorQMC[mIndexQMC+i] - ((double)n[i]*mPasUnite)) / mPasUnite  );
	}
	mIndexQMC += 4;
	
	Eigen::Vector4d axis(0, x[1], x[2], x[3]);
	
	//N
	axis.normalize();
	axis[1]=axis[1]*sigma1;
	axis[2]=axis[2]*sigma2;
	axis[3]=axis[3]*sigma3;
	
	//theta
	double theta = x[0]*sigma;
	
	//exp(N*theta)
	axis=axis*(double)sin(theta);
	axis[0]=(double)cos(theta);
	
	//to quaternion
	Eigen::Quaterniond quat(axis[0], axis[1], axis[2], axis[3]);
	quat = mean*quat;
	
	return quat;
}

void PR2EihMapping::display_trajectory(const StdVectorJ& J) {
	geometry_msgs::PoseArray pose_array;
	pose_array.poses.resize(J.size());
	for(int t=0; t < J.size(); ++t) {
		Matrix4d pose = arm_sim->fk(J[t]);
		pose_array.poses[t].position.x = pose(0,3);
		pose_array.poses[t].position.y = pose(1,3);
		pose_array.poses[t].position.z = pose(2,3);

		Quaterniond quat(pose.block<3,3>(0,0));
		pose_array.poses[t].orientation.w = quat.w();
		pose_array.poses[t].orientation.x = quat.x();
		pose_array.poses[t].orientation.y = quat.y();
		pose_array.poses[t].orientation.z = quat.z();
	}

	pose_array.header.frame_id = "/base_link";
	pose_array.header.stamp = ros::Time::now();

	display_trajectory_pub.publish(pose_array);
}

		void RenderContext::initiate()
		{
			this->renderer.initiate(*this);
			this->renderer.setCamera(this->camera);

			///Testing
			//Resources::Texture& tex = this->resource_manager.newTextureFromFile("/home/barry/Documents/Projects/starclock/mickey.bmp");
			Resources::Mesh& mesh = this->resource_manager.newMeshFromFile("/home/barry/Documents/Projects/voxeltest/icosahedron.obj");

			Figures::Part& part = this->figure_manager.newFigure().newPart();
			part.setMesh(&mesh);
			//part.setTexture(&tex);
			part.bufferAll();
			
			this->figure_manager.getFigure(0).getState().position = v3(9.0, 0.0, 0.0);
			this->figure_manager.getFigure(0).getState().spin = quat(v3(0.1, 0.0, 0.0));
			this->figure_manager.getFigure(0).getState().scale = v3(3.0, 3.0, 3.0);
			this->figure_manager.getFigure(0).getPart(0).getState().update();
			this->figure_manager.getFigure(0).getState().update();
			///Testing
		}

void stateListenerCallBack(const nav_msgs::Odometry msg){
	currentState = msg;
	position[0] = msg.pose.pose.position.x;
	position[1] = msg.pose.pose.position.y;
	position[2] = msg.pose.pose.position.z;
	tfScalar roll,pitch,yaw;
	tf::Quaternion	quat(msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w);
	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
	position[3] = roll;
	position[4] = pitch;
	position[5] = yaw;
	//position[3]=msg.pose.pose.orientation.x;position[4]=msg.pose.pose.orientation.y;position[5]=msg.pose.pose.orientation.z;
	//printf("x=%lf y=%lf z=%lf\n",position[0],position[1],position[2]);	
	//printf("roll=%lf pitch=%lf yaw=%lf\n",position[3],position[4],position[5]);
	vel[0] = msg.twist.twist.linear.x;
	vel[1] = msg.twist.twist.linear.y;
	vel[2] = msg.twist.twist.linear.z;
	vel[3] = msg.twist.twist.angular.x;
	vel[4] = msg.twist.twist.angular.y;
	vel[5] = msg.twist.twist.angular.z;
}

//------ EQuat.cpp ------
void LDQuatTest::getAxisTest()
{
	LDQuat quat( 0.0f, 0.0f, 0.0f, 0.0f );
	LDVector3 actual;
	LDVector3 expected;
	//ld_float delta = 0.00001f;

	// Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
	expected.zero();
	actual = quat.getAxis();
	QCOMPARE( expected.x, actual.x );
	QCOMPARE( expected.y, actual.y );
	QCOMPARE( expected.z, actual.z );

	// Input : x = 0.0, y = 0.0, z = 0.0, w = 1.0 (単位クォータニオン)
	quat.x = 0.0f;
	quat.y = 0.0f;
	quat.z = 0.0f;
	quat.w = 1.0f;
	expected.x = 1.0f;
	expected.y = 0.0f;
	expected.z = 0.0f;
	actual = quat.getAxis();
	QCOMPARE( expected.x, actual.x );
	QCOMPARE( expected.y, actual.y );
	QCOMPARE( expected.z, actual.z );

	// Input : x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
	quat.x = 1.0f;
	quat.y = 2.0f;
	quat.z = 3.0f;
	quat.w = 0.5f;
	expected.x = quat.x * 1.0f / sqrt( 1.0f - quat.w * quat.w );
	expected.y = quat.y * 1.0f / sqrt( 1.0f - quat.w * quat.w );
	expected.z = quat.z * 1.0f / sqrt( 1.0f - quat.w * quat.w );
	actual = quat.getAxis();
	QCOMPARE( expected.x, actual.x );
	QCOMPARE( expected.y, actual.y );
	QCOMPARE( expected.z, actual.z );
}

void Grid::draw(int index_load)

{

	GLfloat m[16];
	vector <float > c = getColor();
    vector <float > r = getRotation();
    vector <float > t = getTranslation();
    vector <float > s = getScale();

    glm::quat quat (glm::vec3(r[0]*PI/BASE, r[1]*PI/BASE, r[2]*PI/BASE));
    glm::quat quaternion = quat ; 
    glm::mat4 mat  = glm::toMat4(quaternion);

    int count = 0;
    for (int k = 0; k < 4; ++k){
        for (int j = 0; j < 4; ++j){
            m[count] = mat[k][j];
            count++;
        }   
    }
    glLoadName(index_load); // register object.
    glPushMatrix();

        glColor3f(c[0],c[1],c[2]);
        glScalef(s[0], s[1], s[2]);
        glTranslatef(t[0],t[1],t[2]);
        glMultMatrixf(m);
        glBegin(GL_LINES);
        vector < float * > vertex = getVertex();

        for (int j = 0; j < (int) vertex.size(); ++j){
            
            glVertex3fv(vertex[j]);
        }
        glEnd();
    
    glPopMatrix();
    //glPopName();
}

int main( void ) {
    // init
    time_t seed = time(NULL);
    printf("seed: %ld\n", seed);
    srand((unsigned int)seed); // might break in 2038
    aa_test_ulimit();

    for( size_t i = 0; i < 1000; i++ ) {
        /* Random Data */
        static const size_t k=2;
        double E[2][7], S[2][8], T[2][12], dx[2][6];
        for( size_t j = 0; j < k; j ++ ) {
            rand_tf(E[j], S[j], T[j]);
            aa_vrand(6,dx[j]);
        }
        //printf("%d\n",i);
        /* Run Tests */
        rotvec(E[0]);
        euler(dx[0]);
        euler1(dx[0]);
        eulerzyx(E[0]);
        chain(E,S,T);
        quat(E);
        duqu();
        rel_q();
        rel_d();
        slerp();
        theta2quat();
        rotmat(E[0]);
        tfmat();
        tfmat_inv(T[0]);
        mzlook(dx[0]+0, dx[0]+3, dx[1]+0);
        integrate(E[0], S[0], T[0], dx[0]);
        tf_conj(E, S);
        qdiff(E,dx);
    }


    return 0;
}