int __EXPORT eulerAnglesTest() { printf("Test EulerAngles\t: "); EulerAngles euler(0.1f, 0.2f, 0.3f); // test ctor ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); ASSERT(equal(euler.getPhi(), 0.1f)); ASSERT(equal(euler.getTheta(), 0.2f)); ASSERT(equal(euler.getPsi(), 0.3f)); // test dcm ctor euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); // test quat ctor euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); // test assignment euler.setPhi(0.4f); euler.setTheta(0.5f); euler.setPsi(0.6f); ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); printf("PASS\n"); return 0; }
GeometryBufferPtr GizmoRotate::generateCircles() { // Vertex data std::vector< Vector3 > pos; std::vector< Vector3 > colors; Matrix4x3 transform; // X axis std::vector< Vector3 > posX; generateCircle(posX, SLICES); generateColors(colors, X); transform = Matrix4x3::createScale( Vector3(0.4f) ); TransformVertices(pos, posX, transform); // Y axis std::vector< Vector3 > posY; generateCircle(posY, SLICES); generateColors(colors, Y); transform = Matrix4x3::createScale( Vector3(0.4f) ); transform = transform*Matrix4x3::createRotation( EulerAngles(0, 90, 0) ); TransformVertices(pos, posY, transform); // Z axis std::vector< Vector3 > posZ; generateCircle(posZ, SLICES); generateColors(colors, Z); transform = Matrix4x3::createScale( Vector3(0.4f) ); transform = transform*Matrix4x3::createRotation( EulerAngles(90, 0, 0) ); TransformVertices(pos, posZ, transform); // Translate it a bit. transform = Matrix4x3::createTranslation( Vector3::UnitY * 0.5f ); for( uint i = 0; i < pos.size(); i++ ) { Vector3& v = pos[i]; v = transform*v; } // Vertex buffer setup GeometryBufferPtr gb = AllocateHeap(GeometryBuffer); assert( pos.size() == colors.size() ); gb->set( VertexAttribute::Position, pos ); gb->set( VertexAttribute::Color, colors ); return gb; }
//! Generates a nondegenerate set of Euler angles with a delta resolution EulerAnglesList get_uniformly_sampled_rotations( double angle_sampling_internal_rad) { double delta = angle_sampling_internal_rad / IMP::PI * 180.0; algebra::Vector3D eu_start(0., 0., 0.); // psi,theta,phi algebra::Vector3D eu_end(360., 180., 360.); algebra::Vector3D eu_range = eu_end - eu_start; double phi_steps = algebra::get_rounded(eu_range[2] / delta + 0.499); double phi_real_dist = eu_range[2] / phi_steps; double theta_steps = algebra::get_rounded(eu_range[1] / delta + 0.499); double theta_real_dist = eu_range[1] / theta_steps; double angle2rad = PI / 180.; double psi_steps, psi_ang_dist, psi_real_dist; EulerAnglesList ret; for (double phi = eu_start[2]; phi < eu_end[2]; phi += phi_real_dist) { for (double theta = eu_start[1]; theta <= eu_end[1]; theta += theta_real_dist) { if (theta == 0.0 || theta == 180.0) { psi_steps = 1; } else { psi_steps = algebra::get_rounded( 360.0 * std::cos((90.0 - theta) * angle2rad) / delta); } psi_ang_dist = 360.0 / psi_steps; psi_real_dist = eu_range[0] / (ceil(eu_range[0] / psi_ang_dist)); for (double psi = eu_start[0]; psi < eu_end[0]; psi += psi_real_dist) { ret.push_back( EulerAngles(psi * angle2rad, theta * angle2rad, phi * angle2rad)); } } } return ret; }
// //############################################################################# //############################################################################# // bool EulerAngles::TestClass() { SPEW((GROUP_STUFF_TEST, "Starting EulerAngle test...")); const EulerAngles a(Identity); EulerAngles b; const EulerAngles c(Pi_Over_4,Pi_Over_6,Pi_Over_3); Test_Assumption(!a.pitch && !a.yaw && !a.roll); Test_Assumption(c.pitch == Pi_Over_4 && c.yaw == Pi_Over_6 && c.roll == Pi_Over_3); Test_Assumption(!a); b = c; Test_Assumption(b == c); Test_Assumption(b != a); Test_Assumption(b[Y_Axis] == b.yaw); Test_Assumption(c[Z_Axis] == c.roll); b.Lerp(a,c,0.5f); Test_Assumption(b == EulerAngles(Stuff::Lerp(a.pitch,c.pitch,0.5f),Stuff::Lerp(a.yaw,c.yaw,0.5f),Stuff::Lerp(a.roll,c.roll,0.5f))); LinearMatrix4D m; m.BuildRotation(c); b = m; Test_Assumption(b == c); return true; }
int __EXPORT quaternionTest() { printf("Test Quaternion\t\t: "); // test default ctor Quaternion q; ASSERT(equal(q.getA(), 1.0f)); ASSERT(equal(q.getB(), 0.0f)); ASSERT(equal(q.getC(), 0.0f)); ASSERT(equal(q.getD(), 0.0f)); // test float ctor q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); ASSERT(equal(q.getA(), 0.1825742f)); ASSERT(equal(q.getB(), 0.3651484f)); ASSERT(equal(q.getC(), 0.5477226f)); ASSERT(equal(q.getD(), 0.7302967f)); // test euler ctor q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); // test dcm ctor q = Quaternion(Dcm()); ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); // TODO test derivative // test accessors q.setA(0.1f); q.setB(0.2f); q.setC(0.3f); q.setD(0.4f); ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); printf("PASS\n"); return 0; }
PlaneObject::PlaneObject(Model *m): GameObject(m,2), m_gunPosition(0, 2.2f, 3.1f), m_enginePosition(0,0.0f, 3.3f), m_propOffset(0.4f), m_isPlaneAlive(true), m_maxTurnRate(kPi * 0.25f), m_maxPitchRate(kPi * 0.25f), m_maxBankRate(kPi * 0.25f), m_planeState(PS_FLYING), m_turnState(TS_STRAIGHT), m_pitchState(PS_LEVEL), m_moveState(MS_STOP), m_hp(4), m_maxHP(4) { assert(m); assert(m->getPartCount() >= 2); setModelOrientation(EulerAngles(kPi, 0.0f, 0.0f)); setPosition(0,m_propOffset,0,1); m_fSpeed = 0.0f; m_maxSpeed = 5.0f; m_className = "Plane"; m_type = ObjectTypes::PLANE; m_pitchRate = m_turnRate = 0.0f; m_smokeID = -1; m_reticleLockOnUpdated = false; m_timeSinceFired = gRenderer.getTime(); // load reticle texture m_reticleTexture = gRenderer.cacheTextureDX("reticle.png"); // load all textures that will be used on the plane. // Multiple textures are used throughout game play so the plane appears to // "take damage" m_allTextures.resize(5); m_allTextures[0] = "plane2life0.tga"; // when hp = 0 m_allTextures[1] = "plane2life1.tga"; // hp = 1 m_allTextures[2] = "plane2life2.tga"; // hp = 2 m_allTextures[3] = "plane2life3.tga"; // hp = 3 m_allTextures[4] = "plane2.tga"; // hp > 3 // cache all these for (int a =0; a < (int)m_allTextures.size(); a++) gRenderer.cacheTextureDX(m_allTextures[a].c_str()); m_allParticles.resize(5); m_allParticles[0] = "smokeveryheavy"; // when hp = 0 m_allParticles[1] = "smokeheavy"; // when hp = 0 m_allParticles[2] = "smokemedium"; // hp = 1 m_allParticles[3] = "smokelight"; // hp = 2 m_allParticles[4] = ""; // hp > 2 setTextureAndSmoke(); }
EulerAngles RotationMatrix3<T>::GetEulerAngles( EulerAngles::EOrder order ) { Assert( order <= EulerAngles::YXZ ); static const int indices[6][3] = { { 0, 1, 2 }, { 1, 2, 0 }, { 2, 0, 1 }, { 0, 2, 1 }, { 2, 1, 0 }, { 1, 0, 2 } }; const int i0 = indices[ order ][ 0 ]; const int i1 = indices[ order ][ 1 ]; const int i2 = indices[ order ][ 2 ]; T c1 = std::sqrt( m_matrix( i0, i0 ) * m_matrix( i0, i0 ) + m_matrix( i0, i1 ) * m_matrix( i0, i1 ) ); Angle angles[3]; if ( order <= EulerAngles::ZXY ) //even permutation { angles[1] = ArcTan( m_matrix( i0, i2 ), c1 ); if ( c1 > 16. * std::numeric_limits<T>::epsilon() ) { angles[0] = ArcTan( - m_matrix( i1, i2 ), m_matrix( i2, i2 ) ); angles[2] = ArcTan( - m_matrix( i0, i1 ), m_matrix( i0, i0 ) ); } else { if ( angles[1].Radians() > 0. ) //"gimble lock", only (angles[0] + angles[2]) determined angles[0] = ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) ); else //"gimble lock", only (angles[0] - angles[2]) determined angles[0] = - ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) ); angles[2] = 0.; } } else //odd permutation { angles[1] = ArcTan( - m_matrix( i0, i2 ), c1 ); if ( c1 > 16. * std::numeric_limits<T>::epsilon() ) { angles[0] = ArcTan( m_matrix( i1, i2 ), m_matrix( i2, i2 ) ); angles[2] = ArcTan( m_matrix( i0, i1 ), m_matrix( i0, i0 ) ); } else { if ( angles[1].Radians() > 0. ) //"gimble lock", only (angles[0] - angles[2]) determined angles[0] = ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) ); else //"gimble lock", only (angles[0] + angles[2]) determined angles[0] = ArcTan( - m_matrix( i1, i0 ), m_matrix( i1, i1 ) ); angles[2] = 0.; } } return EulerAngles( angles ); }
void TrainingDataRenderer::update(float time) { float fps = 60; _currVertStart = 5*((int)(fps*time)) % _mesh->getVertexCount(); // Conversion from Euler angles (in radians) to Quaternion vec3 EulerAngles(time*0.3, time*0.1, 0); quat q = quat(EulerAngles); _m = glm::mat4_cast(q); vec3 translationSpeed(10.0); _position += _fpsController->getVelocity() * GLFWTime::getDT() * translationSpeed;; _orientation = _fpsController->getRotation(); _m = glm::translate(_position) * glm::toMat4(_orientation); _fpsController->update(time); /* //_m = Matrix4::rotationZYX(Vector3(time*0.2f, time*0.3,0)); Point3 EyePosition(0, 0, 5); Vector3 up(0, 1, 0); Point3 target(0); Matrix4 ViewMatrix = Matrix4::lookAt(EyePosition, target, up); _m = Matrix4::rotationZYX(Vector3(time*0.2f, time*0.3,0)); Matrix4 ModelviewMatrix = ViewMatrix * _m; float n = 0.01f; float f = 1000.0f; float fieldOfView = 0.7f; Matrix4 ProjectionMatrix = Matrix4::perspective(fieldOfView, 1, n, f); _mvp = ProjectionMatrix * ViewMatrix * _m; //_m = Matrix4::rotation(-time * 0.25f, Vector3::yAxis()); _mvpRot = ProjectionMatrix * ViewMatrix * _m; */ //Overriding everything. using camera }
void Sphere::buildGeometry( bool fullSphere, byte numSubDiv, VertexData& pos, float dim) { GeometryBufferPtr gb = AllocateThis(GeometryBuffer); setGeometryBuffer(gb); // Rotate the vertices, else the sphere is not properly aligned. Matrix4x3 rot = Matrix4x3::createRotation( EulerAngles(-60, 0, 0) ); for( size_t i = 0; i < ARRAY_SIZE(IcoDomeIndices); i++ ) { const byte* p = IcoDomeIndices[i]; Vector3 v1( IcoVertices[p[0]][0], IcoVertices[p[0]][2], IcoVertices[p[0]][1] ); Vector3 v2( IcoVertices[p[1]][0], IcoVertices[p[1]][2], IcoVertices[p[1]][1] ); Vector3 v3( IcoVertices[p[2]][0], IcoVertices[p[2]][2], IcoVertices[p[2]][1] ); subdivide( rot*v1, rot*v2, rot*v3, numSubDiv, pos ); } // If we don't want a full sphere, we return here. if( fullSphere ) { // These indices are the bottom of the sphere. for( size_t i = 0; i < ARRAY_SIZE(IcoSphereIndices); i++ ) { const byte* p = IcoSphereIndices[i]; Vector3 v1( IcoVertices[p[0]][0], IcoVertices[p[0]][2], IcoVertices[p[0]][1] ); Vector3 v2( IcoVertices[p[1]][0], IcoVertices[p[1]][2], IcoVertices[p[1]][1] ); Vector3 v3( IcoVertices[p[2]][0], IcoVertices[p[2]][2], IcoVertices[p[2]][1] ); subdivide( rot*v1, rot*v2, rot*v3, numSubDiv, pos ); } } // Scale all the vertices. for( size_t i = 0; i < pos.size(); i++ ) { Vector3& vec = pos[i]; vec *= dim; } }
void EulerAngleFileReader::readFile() { // Read in Euler angles from _file_name std::ifstream inFile(_file_name.c_str()); if (!inFile) mooseError("Can't open " << _file_name); // Skip first 4 lines for (unsigned int i = 0; i < 4; ++i) inFile.ignore(std::numeric_limits<std::streamsize>::max(), '\n'); // The angle files contain a fourth column with weights that we ignore in this version Real weight; // Loop over grains EulerAngles a; while (inFile >> a.phi1 >> a.Phi >> a.phi2 >> weight) _angles.push_back(EulerAngles(a)); }
int __EXPORT quaternionTest() { printf("Test Quaternion\t\t: "); // test default ctor Quaternion q; ASSERT(equal(q.getA(), 1)); ASSERT(equal(q.getB(), 0)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test float ctor q = Quaternion(0, 1, 0, 0); ASSERT(equal(q.getA(), 0)); ASSERT(equal(q.getB(), 1)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test euler ctor q = Quaternion(EulerAngles(0, 0, 0)); ASSERT(equal(q.getA(), 1)); ASSERT(equal(q.getB(), 0)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test dcm ctor q = Quaternion(Dcm()); ASSERT(equal(q.getA(), 1)); ASSERT(equal(q.getB(), 0)); ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // TODO test derivative // test accessors q.setA(0.1); q.setB(0.2); q.setC(0.3); q.setD(0.4); ASSERT(equal(q.getA(), 0.1)); ASSERT(equal(q.getB(), 0.2)); ASSERT(equal(q.getC(), 0.3)); ASSERT(equal(q.getD(), 0.4)); printf("PASS\n"); return 0; }
bool AffineTransform::frontendToData() { dataModel()->setRotationEnabled(ui_->btnRotate->isChecked()); dataModel()->setRotation( EulerAngles(ui_->rotation->z(), ui_->rotation->y(), ui_->rotation->x())); dataModel()->setScaleEnabled(ui_->btnScale->isChecked()); dataModel()->setUniformScaleEnabled(ui_->chkUniformScale->isChecked()); if (dataModel()->uniformScaleEnabled()) { dataModel()->setScale(ui_->scale->value()); } else { dataModel()->setScale(QVector3D( ui_->scaleX->value(), ui_->scaleY->value(), ui_->scaleZ->value())); } dataModel()->setTranslationEnabled(ui_->btnTranslate->isChecked()); dataModel()->setTranslation(QVector3D( ui_->offsetX->value(), ui_->offsetY->value(), ui_->offsetZ->value())); return true; }
EulerAngles Rotation::GetEulerAngles() const { OTL_ASSERT(m_type == RotationType::Euler, "Invalid rotation type"); return EulerAngles(m_matrix.eulerAngles(1, 2, 3), 1, 2, 3); }
void Camera::addRoll(float amount){ Quaternion q(EulerAngles(0, 0, amount)); orientation*= q; }
void Camera::addPitch(float amount){ Quaternion q(EulerAngles(amount, 0, 0)); orientation*= q; }
void Camera::addYaw(float amount){ Quaternion q(EulerAngles(0, amount, 0)); orientation*= q; }
// constructor Camera::Camera() { fov = 60.0f; cameraPos = Vector3(0.0f,0.0f,0.0f); cameraOrient = EulerAngles(0.0f,0.0f,0.0f); }
MenuBackground::MenuBackground() : rps(NULL) { Renderer &renderer= Renderer::getInstance(); //load data string data_path = getGameReadWritePath(GameConstants::path_data_CacheLookupKey); XmlTree xmlTree; xmlTree.load(getGameCustomCoreDataPath(data_path, "data/core/menu/menu.xml"),Properties::getTagReplacementValues()); const XmlNode *menuNode= xmlTree.getRootNode(); //water const XmlNode *waterNode= menuNode->getChild("water"); water= waterNode->getAttribute("value")->getBoolValue(); if(water){ waterHeight= waterNode->getAttribute("height")->getFloatValue(); //water texture waterTexture= renderer.newTexture2D(rsMenu); if(waterTexture) { waterTexture->getPixmap()->init(4); waterTexture->getPixmap()->load(getGameCustomCoreDataPath(data_path, "data/core/menu/textures/water.tga")); } } //fog const XmlNode *fogNode= menuNode->getChild("fog"); fog= fogNode->getAttribute("value")->getBoolValue(); if(fog){ fogDensity= fogNode->getAttribute("density")->getFloatValue(); } //rain bool withRainEffect = Config::getInstance().getBool("RainEffectMenu","true"); if(withRainEffect == true) { rain= menuNode->getChild("rain")->getAttribute("value")->getBoolValue(); if(rain) { createRainParticleSystem(); } } else { rain = false; } //camera const XmlNode *cameraNode= menuNode->getChild("camera"); //position const XmlNode *positionNode= cameraNode->getChild("start-position"); Vec3f startPosition; startPosition.x= positionNode->getAttribute("x")->getFloatValue(); startPosition.y= positionNode->getAttribute("y")->getFloatValue(); startPosition.z= positionNode->getAttribute("z")->getFloatValue(); camera.setPosition(startPosition); //rotation const XmlNode *rotationNode= cameraNode->getChild("start-rotation"); Vec3f startRotation; startRotation.x= rotationNode->getAttribute("x")->getFloatValue(); startRotation.y= rotationNode->getAttribute("y")->getFloatValue(); startRotation.z= rotationNode->getAttribute("z")->getFloatValue(); camera.setOrientation(Quaternion(EulerAngles( degToRad(startRotation.x), degToRad(startRotation.y), degToRad(startRotation.z)))); //load main model mainModel= renderer.newModel(rsMenu); if(mainModel) { string mainModelFile = "data/core/menu/main_model/menu_main.g3d"; if(menuNode->hasChild("menu-background-model") == true) { //mainModel->load(data_path + "data/core/menu/main_model/menu_main.g3d"); const XmlNode *mainMenuModelNode= menuNode->getChild("menu-background-model"); mainModelFile = mainMenuModelNode->getAttribute("value")->getRestrictedValue(); } mainModel->load(getGameCustomCoreDataPath(data_path, mainModelFile)); } //models for(int i=0; i<5; ++i){ characterModels[i]= renderer.newModel(rsMenu); if(characterModels[i]) { characterModels[i]->load(getGameCustomCoreDataPath(data_path, "data/core/menu/about_models/character"+intToStr(i)+".g3d")); } } //about position positionNode= cameraNode->getChild("about-position"); aboutPosition.x= positionNode->getAttribute("x")->getFloatValue(); aboutPosition.y= positionNode->getAttribute("y")->getFloatValue(); aboutPosition.z= positionNode->getAttribute("z")->getFloatValue(); rotationNode= cameraNode->getChild("about-rotation"); targetCamera= NULL; t= 0.f; fade= 0.f; anim= 0.f; }
float Quaternion::PitchAngle() const { return EulerAngles().x_; }
EulerAngles::EulerAngles(const Quaternion &q) : Vector(3) { (*this) = EulerAngles(Dcm(q)); }
float Quaternion::YawAngle() const { return EulerAngles().y_; }
bool TestQuaternion( ) { bool ok = true; cout << "Testing Quaternion" << endl; cout << "Quaternion( ) [default constructor]" << endl; QuaternionF quat0; float w = 3.f; float x = 0.f; float y = 4.f; float z = 0.f; cout << "Set( " << w << ", " << x << ", " << y << ", " << z << " )" << endl; quat0.Set( w, x, y, z ); TESTCHECK( quat0.W(), w, &ok ); TESTCHECK( quat0.X(), x, &ok ); TESTCHECK( quat0.Y(), y, &ok ); TESTCHECK( quat0.Z(), z, &ok ); TESTCHECK( quat0[0], w, &ok ); TESTCHECK( quat0[1], x, &ok ); TESTCHECK( quat0[2], y, &ok ); TESTCHECK( quat0[3], z, &ok ); TESTCHECK( quat0.At( 0 ), w, &ok ); TESTCHECK( quat0.At( 1 ), x, &ok ); TESTCHECK( quat0.At( 2 ), y, &ok ); TESTCHECK( quat0.At( 3 ), z, &ok ); try { TESTCHECK( quat0.At(4), 0.f, &ok ); cout << "At(4) should have thrown an exception." << endl; ok = false; } catch( out_of_range & exceptn ) { cout << "Exception here is OK" << endl; cout << exceptn.what() << endl; } cout << "quat0.Array()" << endl; const float * pF = quat0.Array(); TESTCHECK( pF[0], w, &ok ); TESTCHECK( pF[1], x, &ok ); TESTCHECK( pF[2], y, &ok ); TESTCHECK( pF[3], z, &ok ); TESTCHECK( quat0.Real(), w, &ok ); TESTCHECK( quat0.Imaginary().X(), x, &ok ); TESTCHECK( quat0.Imaginary().Y(), y, &ok ); TESTCHECK( quat0.Imaginary().Z(), z, &ok ); cout << "Conjugate( )" << endl; QuaternionF quat1 = quat0.Conjugate( ); TESTCHECK( quat1.W(), w, &ok ); TESTCHECK( quat1.X(), -x, &ok ); TESTCHECK( quat1.Y(), -y, &ok ); TESTCHECK( quat1.Z(), -z, &ok ); TESTCHECK( quat0.Length(), 5.f, &ok ); TESTCHECK( quat0.Norm(), 25.f, &ok ); TESTCHECK( quat1.Length(), 5.f, &ok ); TESTCHECK( quat1.Norm(), 25.f, &ok ); TESTCHECK( (quat0 + quat1).W(), 6.f, &ok ); TESTCHECK( (quat0 + quat1).X(), 0.f, &ok ); TESTCHECK( (quat0 + quat1).Y(), 0.f, &ok ); TESTCHECK( (quat0 + quat1).Z(), 0.f, &ok ); ostringstream ost; cout << "operator<<" << endl; ost << quat0; TESTCHECK( ost.str(), string( "( 3, 0, 4, 0 )" ), &ok ); TESTCHECK( ToJSON( quat0 ), string( "[ +3.00000e+00, +0.00000e+00, +4.00000e+00," " +0.00000e+00 ]" ), &ok ); FromJSON( "[ 3, 0, -4.0, 0. ]", &quat1 ); TESTCHECK( quat1.W(), w, &ok ); TESTCHECK( quat1.X(), -x, &ok ); TESTCHECK( quat1.Y(), -y, &ok ); TESTCHECK( quat1.Z(), -z, &ok ); cout << "Normalize( )" << endl; quat1.Normalize( ); TESTCHECKF( quat1.W(), 0.6f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), -0.8f, &ok ); TESTCHECKF( quat1.Z(), 0.f, &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); cout << "Inverse( )" << endl; quat1 = quat0.Inverse( ); TESTCHECKF( quat1.W(), 0.12f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), -0.16f, &ok ); TESTCHECKF( quat1.Z(), 0.f, &ok ); TESTCHECKF( (quat0 * quat1).W(), 1.f, &ok ); TESTCHECKF( (quat0 * quat1).X(), 0.f, &ok ); TESTCHECKF( (quat0 * quat1).Y(), 0.f, &ok ); TESTCHECKF( (quat0 * quat1).Z(), 0.f, &ok ); float coords[4] = { 0.5f, 0.5f, 0.5f, 0.5f }; cout << "QuaternionF( coords ) [array constructor]" << endl; QuaternionF quat2( coords ); TESTCHECKF( quat2.W(), 0.5f, &ok ); TESTCHECKF( quat2.X(), 0.5f, &ok ); TESTCHECKF( quat2.Y(), 0.5f, &ok ); TESTCHECKF( quat2.Z(), 0.5f, &ok ); TESTCHECKF( quat2.Norm(), 1.f, &ok ); cout << "Normalize( )" << endl; quat2.Normalize( ); TESTCHECKF( quat2.W(), 0.5f, &ok ); TESTCHECKF( quat2.X(), 0.5f, &ok ); TESTCHECKF( quat2.Y(), 0.5f, &ok ); TESTCHECKF( quat2.Z(), 0.5f, &ok ); TESTCHECKF( quat2.Norm(), 1.f, &ok ); cout << "Conjugate( )" << endl; quat1 = quat2.Conjugate( ); TESTCHECKF( quat1.W(), 0.5f, &ok ); TESTCHECKF( quat1.X(), -0.5f, &ok ); TESTCHECKF( quat1.Y(), -0.5f, &ok ); TESTCHECKF( quat1.Z(), -0.5f, &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); TESTCHECKF( (quat1 + quat2).W(), 1.f, &ok ); TESTCHECKF( (quat1 + quat2).X(), 0.f, &ok ); TESTCHECKF( (quat1 + quat2).Y(), 0.f, &ok ); TESTCHECKF( (quat1 + quat2).Z(), 0.f, &ok ); TESTCHECKF( (quat1 - quat2).W(), 0.f, &ok ); TESTCHECKF( (quat1 - quat2).X(), -1.f, &ok ); TESTCHECKF( (quat1 - quat2).Y(), -1.f, &ok ); TESTCHECKF( (quat1 - quat2).Z(), -1.f, &ok ); TESTCHECKF( (quat1 * quat2).W(), 1.f, &ok ); TESTCHECKF( (quat1 * quat2).X(), 0.f, &ok ); TESTCHECKF( (quat1 * quat2).Y(), 0.f, &ok ); TESTCHECKF( (quat1 * quat2).Z(), 0.f, &ok ); cout << "Inverse( )" << endl; quat1 = quat2.Inverse( ); TESTCHECKF( quat1.W(), 0.5f, &ok ); TESTCHECKF( quat1.X(), -0.5f, &ok ); TESTCHECKF( quat1.Y(), -0.5f, &ok ); TESTCHECKF( quat1.Z(), -0.5f, &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); double a = M_PI / 2.; cout << "QuaternionF( AxisAngle( Vector3F::UnitZ, Angle( " << a << " ) ) ) [axis-angle constructor]" << endl; QuaternionF quat3( AxisAngleF( Vector3F::UnitZ, Angle( a ) ) ); TESTCHECKF( quat3.W(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat3.X(), 0.f, &ok ); TESTCHECKF( quat3.Y(), 0.f, &ok ); TESTCHECKF( quat3.Z(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat3.Norm(), 1.f, &ok ); cout << "GetAxisAngle( )" << endl; AxisAngleF axisAngle = quat3.GetAxisAngle( ); TESTCHECKF( axisAngle.Axis().X(), 0.f, &ok ); TESTCHECKF( axisAngle.Axis().Y(), 0.f, &ok ); TESTCHECKF( axisAngle.Axis().Z(), 1.f, &ok ); TESTCHECKF( axisAngle.GetAngle().Radians(), M_PI / 2., &ok ); cout << "Set( 2, a )" << endl; quat1.Set( 2, a ); TESTCHECKF( quat1.W(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); cout << "Log()" << endl; quat1 = quat3.Log( ); TESTCHECKF( quat1.W(), 0.f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), M_PI / 4.f, &ok ); cout << "SetW( 1. )" << endl; quat1.SetW( 1.f ); cout << "Exp()" << endl; quat1 = quat1.Exp( ); TESTCHECKF( quat1.W(), exp( 1.f ) * sqrt( 0.5f ), &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), exp( 1.f ) * sqrt( 0.5f ), &ok ); TESTCHECK( (quat1 == quat3), false, &ok ); cout << "Log()" << endl; quat1 = quat1.Log( ); TESTCHECKF( quat1.W(), 1.f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), M_PI / 4.f, &ok ); a = 2. * M_PI / 3.; float f = static_cast<float>( sqrt( 1./3. ) ); cout << "RotationMatrix3F( AxisAngleF( Vector3F( " << f << ", " << f << ", " << f << "), Angle( " << a << " ) ) )" << endl; axisAngle.Set( Vector3F( f, f, f ), Angle( a ) ); RotationMatrix3F mat0( axisAngle ); cout << "QuaternionF( mat0 ) [rotation matrix constructor]" << endl; QuaternionF quat4( mat0 ); TESTCHECKF( quat4.W(), quat2.W(), &ok ); TESTCHECKF( quat4.X(), quat2.X(), &ok ); TESTCHECKF( quat4.Y(), quat2.Y(), &ok ); TESTCHECKF( quat4.Z(), quat2.Z(), &ok ); TESTCHECKF( quat4.Norm(), 1.f, &ok ); cout << "GetAxisAngle( )" << endl; axisAngle = quat4.GetAxisAngle( ); TESTCHECKF( axisAngle.Axis().X(), f, &ok ); TESTCHECKF( axisAngle.Axis().Y(), f, &ok ); TESTCHECKF( axisAngle.Axis().Z(), f, &ok ); TESTCHECKF( axisAngle.GetAngle().Radians(), a, &ok ); cout << "Matrix( )" << endl; RotationMatrix3F mat1 = quat4.Matrix( ); TESTCHECKF( mat1(0,0), mat0(0,0), &ok ); TESTCHECKF( mat1(0,1), mat0(0,1), &ok ); TESTCHECKF( mat1(0,2), mat0(0,2), &ok ); TESTCHECKF( mat1(1,0), mat0(1,0), &ok ); TESTCHECKF( mat1(1,1), mat0(1,1), &ok ); TESTCHECKF( mat1(1,2), mat0(1,2), &ok ); TESTCHECKF( mat1(2,0), mat0(2,0), &ok ); TESTCHECKF( mat1(2,1), mat0(2,1), &ok ); TESTCHECKF( mat1(2,2), mat0(2,2), &ok ); cout << "(quat4 * Quaternion( Vector3F::UnitX )" << "* quat4.Conjugate()).Imaginary() [rotation]" << endl; Vector3F vec1 = (quat4 * QuaternionF( Vector3F::UnitX ) * quat4.Conjugate()).Imaginary(); TESTCHECKF( vec1.X(), 0.f, &ok ); TESTCHECKF( vec1.Y(), 1.f, &ok ); TESTCHECKF( vec1.Z(), 0.f, &ok ); cout << "Rotate( Vector3F::UnitX )" << endl; vec1 = quat4.Rotate( Vector3F::UnitX ); TESTCHECKF( vec1.X(), 0.f, &ok ); TESTCHECKF( vec1.Y(), 1.f, &ok ); TESTCHECKF( vec1.Z(), 0.f, &ok ); float a0 = -2.9f; float a1 = 1.8f; float a2 = -0.7f; cout << "Quaternion qrot0( 0, " << a0 << " )" << endl; QuaternionF qrot0( 0, Angle( a0 ) ); cout << "Quaternion qrot1( 1, " << a1 << " )" << endl; QuaternionF qrot1( 1, Angle( a1 ) ); cout << "Quaternion qrot2( 2, " << a2 << " )" << endl; QuaternionF qrot2( 2, Angle( a2 ) ); cout << "quat1 = qrot1 * qrot2 * qrot0" << endl; quat1 = qrot1 * qrot2 * qrot0; cout << "GetEulerAngles( EulerAngles::YZX )" << endl; EulerAngles euler = quat1.GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); cout << "Quaternion( EulerAngles( " << a1 << ", " << a2 << ", " << a0 << " ), EulerAngles::YZX ) [Euler angles constructor]" << endl; QuaternionF quat5( EulerAngles( a1, a2, a0 ), EulerAngles::YZX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); a2 = static_cast<float>( - M_PI / 2. ); cout << "qrot2.Set( 2, " << a2 << " )" << endl; qrot2.Set( 2, Angle( a2 ) ); cout << "quat1 = qrot1 * qrot2 * qrot0" << endl; quat1 = qrot1 * qrot2 * qrot0; cout << "GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat1.GetEulerAngles( EulerAngles::YZX ); Angle a10( a1 - a0 ); a10.Normalize(); float a1_a0 = static_cast<float>( a10.Radians() ); TESTCHECKF( euler[0].Radians(), a1_a0, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); cout << "Matrix().GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat1.Matrix().GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1_a0, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); cout << "Set( EulerAngles( " << a1 << ", " << a2 << ", " << a0 << " ), EulerAngles::YZX )" << endl; quat5.Set( EulerAngles( a1, a2, a0 ), EulerAngles::YZX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1_a0, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); a0 = 1.1f; a1 = -1.0f; a2 = 0.5f; cout << "qrot0.Set( 0, " << a0 << " )" << endl; qrot0.Set( 0, Angle( a0 ) ); cout << "qrot1.Set( 1, " << a1 << " )" << endl; qrot1.Set( 1, Angle( a1 ) ); cout << "qrot2.Set( 2, " << a2 << " )" << endl; qrot2.Set( 2, Angle( a2 ) ); cout << "quat1 = qrot2 * qrot1 * qrot0" << endl; quat1 = qrot2 * qrot1 * qrot0; cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat1.GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); cout << "Matrix().GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat1.Matrix().GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); cout << "Set( EulerAngles( " << a2 << ", " << a1 << ", " << a0 << " ), EulerAngles::ZYX )" << endl; quat5.Set( EulerAngles( a2, a1, a0 ), EulerAngles::ZYX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); a1 = static_cast<float>( M_PI / 2. ); cout << "qrot1.Set( 1, " << a1 << " )" << endl; qrot1.Set( 1, Angle( a1 ) ); cout << "quat1 = qrot2 * qrot1 * qrot0" << endl; quat1 = qrot2 * qrot1 * qrot0; cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat1.GetEulerAngles( EulerAngles::ZYX ); Angle a20 = Angle( a2 - a0 ); a20.Normalize(); float a2_a0 = static_cast<float>( a20.Radians() ); TESTCHECKF( euler[0].Radians(), a2_a0, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); cout << "Set( EulerAngles( " << a2 << ", " << a1 << ", " << a0 << " ), EulerAngles::ZYX )" << endl; quat5.Set( EulerAngles( a2, a1, a0 ), EulerAngles::ZYX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2_a0, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); if ( ok ) cout << "Quaternion PASSED." << endl << endl; else cout << "Quaternion FAILED." << endl << endl; return ok; }
float Quaternion::RollAngle() const { return EulerAngles().z_; }
#include "utility.h" #include "vector3.h" #include "euler-angles.h" #include "quaternion.h" #include "matrix3.h" #include "euler-angles.inl" #if MAGICAL_MATH_CACHED_POOL_ENABLE #include "CachePool.h" #endif NS_MAGICAL_BEGIN const EulerAngles EulerAngles::Zero = EulerAngles( 0.0f, 0.0f, 0.0f ); EulerAngles EulerAngles::var = EulerAngles::Zero; EulerAngles::EulerAngles( float yaw, float pitch, float roll ) { this->yaw = yaw; this->pitch = pitch; this->roll = roll; } EulerAngles::EulerAngles( const EulerAngles& ea ) { yaw = ea.yaw; pitch = ea.pitch; roll = ea.roll; }