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