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;
}
Exemple #2
0
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);
        }
Exemple #4
0
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;
}
Exemple #5
0
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;
 }
Exemple #8
0
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();
}
Exemple #9
0
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]));
	}
Exemple #13
0
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;
}
Exemple #14
0
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;
}
Exemple #15
0
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);
}
Exemple #16
0
		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;
}
Exemple #18
0
//------ 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 );
}
Exemple #19
0
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();
}
Exemple #20
0
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;
}
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 */
  /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
     3.6.5.5 Orientation (Quaternion)  */
  const double scale = (1.0 / (1<<14));
  imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
  return quat;
}
Exemple #22
0
glm::mat4 CompositorHelper::getReticleTransform(const glm::mat4& eyePose, const glm::vec3& headPosition) const {
    glm::mat4 result;
    if (isHMD()) {
        vec3 reticleScale = vec3(Cursor::Manager::instance().getScale() * reticleSize);
        auto reticlePosition = getReticlePosition();
        auto spherical = overlayToSpherical(reticlePosition);
        // The pointer transform relative to the sensor
        auto pointerTransform = glm::mat4_cast(quat(vec3(-spherical.y, spherical.x, 0.0f))) * glm::translate(mat4(), vec3(0, 0, -1));
        float reticleDepth = getReticleDepth();
        if (reticleDepth != 1.0f) {
            // Cursor position in UI space
            auto cursorPosition = vec3(pointerTransform[3]) / pointerTransform[3].w;
            // Ray to the cursor, in UI space
            auto cursorRay = glm::normalize(cursorPosition - headPosition) * reticleDepth;
            // Move the ray to be relative to the head pose
            pointerTransform[3] = vec4(cursorRay + headPosition, 1);
            // Scale up the cursor because of distance
            reticleScale *= reticleDepth;
        } 
        glm::mat4 overlayXfm;
        _modelTransform.getMatrix(overlayXfm);
        pointerTransform = overlayXfm * pointerTransform;
        pointerTransform = glm::inverse(eyePose) * pointerTransform;
        result = glm::scale(pointerTransform, reticleScale);
    } else {
        static const float CURSOR_PIXEL_SIZE = 32.0f;
        static auto renderingWidget = PluginContainer::getInstance().getPrimaryWidget();
        const auto canvasSize = vec2(toGlm(renderingWidget->size()));;
        vec2 mousePosition = toGlm(renderingWidget->mapFromGlobal(QCursor::pos()));
        mousePosition /= canvasSize;
        mousePosition *= 2.0;
        mousePosition -= 1.0;
        mousePosition.y *= -1.0f;

        vec2 mouseSize = CURSOR_PIXEL_SIZE / canvasSize;
        return glm::scale(glm::translate(glm::mat4(), vec3(mousePosition, 0.0f)), vec3(mouseSize, 1.0f));
    }
    return result;
}
void CGraphicsCamera::RotateZ( double angleInc, CWindowFilter& filter )
{
	m_rot.z += angleInc;
	if( m_rot.z > M_PI*2 )
		m_rot.z = 0.0;
	else if( m_rot.z < -M_PI*2 )
		m_rot.z = 0.0;
	filter.SetRotations( m_rot );

	//JL note 2/11/2009 - let's try rotating around the global z axis...
	CQuaternion quat( angleInc, CVector3D( 0, 0, 1 ) );
	//CQuaternion quat( angleInc, CVector3D( m_pos, m_focalPt ).normalize() );
	double m[4][4] = {0.};
	quat.QuatToMat( m );
	CVector3D v1( m_pos, m_focalPt );
	//multiply our view vector by the rotation matrix
	v1 = v1*m;
	//update our position
	m_pos = CPoint3D( v1.x + m_focalPt.x, v1.y + m_focalPt.y, v1.z + m_focalPt.z );
	//update our camera's up vector
	m_up = m_up*m;
}
Exemple #24
0
glm::mat4 parseTransform(rapidxml::xml_node<> *node) {
  glm::mat4 trans;
  if(!node)
    return trans;
  rapidxml::xml_node<> *pos = node->first_node("Position");
  rapidxml::xml_node<> *quat = node->first_node("Quaternion");
  if(pos) {
    float x = atof(pos->first_node("X")->value());
    float y = atof(pos->first_node("Y")->value());
    float z = atof(pos->first_node("Z")->value());
    trans = glm::translate(trans, glm::vec3(x, y, z));
  }
  if(quat) {
    float x = atof(quat->first_node("X")->value());
    float y = atof(quat->first_node("Y")->value());
    float z = atof(quat->first_node("Z")->value());
    float w = atof(quat->first_node("W")->value());
    glm::quat quat(x, y, z, w);
    trans = trans * glm::mat4_cast(quat);
  }
  return trans;
}
Exemple #25
0
OVR::Matrix4f GVRActivity::Frame( const OVR::VrFrame & vrFrame )
{
    JNIEnv* jni = app->GetVrJni();
    jni->CallVoidMethod(javaObject, beforeDrawEyesMethodId);
    jni->CallVoidMethod(javaObject, drawFrameMethodId);

    useOculusOrientationReading = canSwitchToOculusHeadTracking && vrFrame.DeviceStatus.DeviceIsDocked;
    if (useOculusOrientationReading && nullptr != cameraRig) {
       const ovrQuatf& orientation = vrFrame.Tracking.HeadPose.Pose.Orientation;
       glm::quat quat(orientation.w, orientation.x, orientation.y, orientation.z);
       cameraRig->owner_object()->transform()->set_rotation(glm::conjugate(glm::inverse(quat)));
    }

	//This is called once while DrawEyeView is called twice, when eye=0 and eye 1.
	//So camera is set in java as one of left and right camera.
	//Centerview camera matrix can be retrieved from its parent, CameraRig
    glm::mat4 vp_matrix = camera->getCenterViewMatrix();

    ovrMatrix4f view2;

    view2.M[0][0] = vp_matrix[0][0];
    view2.M[1][0] = vp_matrix[0][1];
    view2.M[2][0] = vp_matrix[0][2];
    view2.M[3][0] = vp_matrix[0][3];
    view2.M[0][1] = vp_matrix[1][0];
    view2.M[1][1] = vp_matrix[1][1];
    view2.M[2][1] = vp_matrix[1][2];
    view2.M[3][1] = vp_matrix[1][3];
    view2.M[0][2] = vp_matrix[2][0];
    view2.M[1][2] = vp_matrix[2][1];
    view2.M[2][2] = vp_matrix[2][2];
    view2.M[3][2] = vp_matrix[2][3];
    view2.M[0][3] = vp_matrix[3][0];
    view2.M[1][3] = vp_matrix[3][1];
    view2.M[2][3] = vp_matrix[3][2];
    view2.M[3][3] = vp_matrix[3][3];

    return view2;
}
Exemple #26
0
void Particle::initialize()//const float& density
{
	//base class
	mPosition = vec3(0.0f);
	mRenderPos = mPosition;
	mG = mat3::Identity();
	//used for openGL render
	mA = 1.0f; mB = 1.0f; mC = 1.0f;
	mRotation = quat(1.0, 0.0, 0.0, 0.0);
	//base class used for efficient calculation
	mWeightedPos = mPosition;
	mSumWeight = 0.0f;
	mCovariant = mat3::Identity();
	//inherit
	mVelocity = vec3(0.0f, 0.0f, 0.0f);
	mForce = vec3(0.0f, 0.0f, 0.0f);
	mDensity = 0.0f;//TODO: make sure how density should be initialized
	mRestDensity = 0.0f;
	mNormal = vec3(0.0f);
	mKappa = 0.0f;
	mPressure = 0.0f;
	mI = mJ = mK = -1;
}
    double Model::rotateRandom(double maxAngle, double minAngle)
    {
	std::uniform_real_distribution<double> rand_double_min_max(minAngle, maxAngle);
	double angle = rand_double_min_max(m_random);
	std::uniform_int_distribution<short> rand_bool(0, 1);
	bool flipSign = rand_bool(m_random);
	if(flipSign)
	    angle = -angle;
	Eigen::Vector3d axis = Eigen::Vector3d::Random();
	axis.normalize();
	Eigen::AngleAxis<double> aa(angle, axis);
	Eigen::Quaterniond quat(aa);
	for(unsigned int i = 0; i < getAmountOfVertices(); i++)
	{
	    auto coords = getVertex(i).coords;
	    coords = quat * coords;
	    auto normal = getVertex(i).normal;
	    normal = quat * normal;
	    setVertex(i, coords, normal);
	}
	analyzeMesh();
	return angle;
    }
Exemple #28
0
//Mouse drag, calculate rotation
void Trackball::drag(const vec2& coords) noexcept
{
  //Map the point to the sphere
  _end = _mapToSphere(coords);

  //Return the quaternion equivalent to the rotation
  //Compute the vector perpendicular to the begin and end vectors
  vec3 cross = glm::cross(_start, _end);

  if (glm::length2(cross) > std::numeric_limits<float>::epsilon())
  {
    //Compute the length of the perpendicular vector, and if it's non-zero...
    //We're ok, so return the perpendicular vector as the transform after all
    //In the quaternion values, w is cosine (theta / 2), where theta is rotation angle
    _rotation = glm::normalize(quat(glm::dot(_start, _end), cross));
  }
  else {
    //The begin and end vectors coincide, so return an identity transform
    _rotation = {};
  }

  _matrix = static_cast<mat4>(_rotation);
}
	void llquat_test_object_t::test<7>()
	{
		F32 radian = 60.0f;
		LLQuaternion quat(3.0f, 2.0f, 6.0f, 0.0f);
		LLQuaternion quat1;
		quat1 = quat.constrain(radian);
		ensure("1. LLQuaternion::constrain(F32 radians) failed", 
									is_approx_equal_fraction(-0.423442f, quat1.mQ[0], 8) &&
									is_approx_equal_fraction(-0.282295f, quat1.mQ[1], 8) &&
									is_approx_equal_fraction(-0.846884f, quat1.mQ[2], 8) &&				
									is_approx_equal_fraction(0.154251f, quat1.mQ[3], 8));				
											

		radian = 30.0f;
		LLQuaternion quat0(37.50f, 12.0f, 86.023f, 40.32f);
		quat1 = quat0.constrain(radian);
	
		ensure("2. LLQuaternion::constrain(F32 radians) failed", 
									is_approx_equal_fraction(37.500000f, quat1.mQ[0], 8) &&
									is_approx_equal_fraction(12.0000f, quat1.mQ[1], 8) &&
									is_approx_equal_fraction(86.0230f, quat1.mQ[2], 8) &&				
									is_approx_equal_fraction(40.320000f, quat1.mQ[3], 8));				
	}
Exemple #30
0
	void xform_test_object_t::test<7>()	
	{
		LLXformMatrix formMatrix_obj;

		LLXformMatrix parent;
		LLVector3 llvecpos(1.0, 2.0, 3.0);
		LLVector3 llvecpospar(10.0, 20.0, 30.0);
		formMatrix_obj.setPosition(llvecpos);
		parent.setPosition(llvecpospar);

		LLVector3 llvecparentscale(1.0, 2.0, 0);
		parent.setScaleChildOffset(TRUE);
		parent.setScale(llvecparentscale);

		LLQuaternion quat(1, 2, 3, 4);
		LLQuaternion quatparent(5, 6, 7, 8);
		formMatrix_obj.setRotation(quat);
		parent.setRotation(quatparent);
		formMatrix_obj.setParent(&parent);

		parent.update();
		formMatrix_obj.update();

		LLVector3 worldPos = llvecpos;
		worldPos.scaleVec(llvecparentscale);
		worldPos *= quatparent;
		worldPos += llvecpospar;

		LLQuaternion worldRot = quat * quatparent; 

		ensure("getWorldPosition failed: ", formMatrix_obj.getWorldPosition() == worldPos);
		ensure("getWorldRotation failed: ", formMatrix_obj.getWorldRotation() == worldRot);

		ensure("getWorldPosition for parent failed: ", parent.getWorldPosition() == llvecpospar);
		ensure("getWorldRotation for parent failed: ", parent.getWorldRotation() == quatparent);
	}