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