C++ EulerAngles类代码示例

TEST( Transform, planeTransformation )
{
  // construct a transformation we'll use for testing
   Transform trans;
   Matrix transformMtx;
   {
      Matrix m1, m2;
      EulerAngles angles;
      angles.set( FastFloat::fromFloat( 45.0f ), FastFloat::fromFloat( 60.0f ), FastFloat::fromFloat( -10.0f ) );
      m1.setTranslation( Vector( 1, 2, 3 ) );
      m2.setRotation( angles );
      transformMtx.setMul( m2, m1 );

      trans.set( transformMtx );
   }

   // create a test plane
   Plane testPlane;
   testPlane.set( Float_1, Float_0, Float_0, FastFloat::fromFloat( 10.0f ) );

   // transform the plane
   Plane tamyTransformedPlane;
   trans.transform( testPlane, tamyTransformedPlane );

   // let DX help us generate a transformed plane we can compare our results against
   D3DXPLANE dxTransformedPlane;
   D3DXMATRIX dxMtx = ( const D3DXMATRIX& )transformMtx;
   D3DXMatrixInverse( &dxMtx, NULL, &dxMtx );
   D3DXMatrixTranspose( &dxMtx, &dxMtx );
   D3DXPlaneTransform( &dxTransformedPlane, ( const D3DXPLANE* )&testPlane, &dxMtx );

   COMPARE_PLANE( dxTransformedPlane, tamyTransformedPlane );
}

/// \param angles The orientation (unit) vector that you want to reflect
/// \return Euler angles that are reflected about the slope of the plane.
EulerAngles Plane::reflectOrientation(const EulerAngles& angles) const
{
  EulerAngles out;
  Vector3 look, up, right;  
  RotationMatrix rotMat;
    
  // get look and up vector from euler angles
  rotMat.setup(angles); 
  look = rotMat.objectToInertial(Vector3(0.0f,0.0f,1.0f));
  up = rotMat.objectToInertial(Vector3(0.0f,1.0f,0.0f));

  // reflect the look and up vectors over the plane
  look = reflectOrientation(look);
  up = -reflectOrientation(up); 
  
  // calculate right vector
  right = Vector3::crossProduct( up, look);
  right.normalize();
    
  // create a rotation matrix from right, up, and look
  rotMat.m11 = right.x; rotMat.m12 = right.y; rotMat.m13 = right.z;
  rotMat.m21 = up.x; rotMat.m22 = up.y; rotMat.m23 = up.z;
  rotMat.m31 = look.x; rotMat.m32 = look.y; rotMat.m33 = look.z;
  
  // calculate new euler angles from the matrix
  out.fromRotationMatrix(rotMat);

  return out;
}

 /// rotation specified by Euler angles
 Transformation Transformation::rotation(const EulerAngles& angles)
 {
   Transformation result = Transformation::rotation(Vector3d(0,0,1), angles.phi()) * 
                           Transformation::rotation(Vector3d(0,1,0), angles.theta()) *
                           Transformation::rotation(Vector3d(1,0,0), angles.psi());
   return result;
 }

TEST_F(QuaternionTest, ToEulerAngles) {
	set(0.966, 0.259, 0, 0);
	EulerAngles ea = quaternion.uprightToEulerAngles();
	ASSERT_THAT(ea.heading(), FloatNear(0, EA_DELTA));
	ASSERT_THAT(ea.pitch(), FloatNear(30, EA_DELTA));
	ASSERT_THAT(ea.bank(), FloatNear(0, EA_DELTA));
}

  bool GlareSensor_Impl::aimAt(const Point3d& target)
  {
    Point3d position = this->position();
    Vector3d vector = target - position;

    if (!vector.normalize()){
      return false;
    }

    Vector3d yAxis(0,1,0);
    Vector3d rotationAxis = yAxis.cross(vector);

    if (!rotationAxis.normalize()){
      return false;
    }

    double angle = getAngle(yAxis, vector);
    Transformation transformation = Transformation::rotation(rotationAxis, angle);
    EulerAngles eulerAngles = transformation.eulerAngles();

    this->setPsiRotationAroundXAxis(eulerAngles.psi());
    this->setThetaRotationAroundYAxis(eulerAngles.theta());
    this->setPhiRotationAroundZAxis(eulerAngles.phi());

    return true;
  }

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

 void Rotation::setRotation(EulerAngles const& _angles) {
   {
     QSignalBlocker blocker(this);
     x_->setValue(_angles.roll().degrees());
     y_->setValue(_angles.pitch().degrees());
     z_->setValue(_angles.yaw().degrees());
   }
   emit rotationChanged();
 }

void
RandomEulerAngleProvider::initialize()
{
  EulerAngles angle;
  auto grain_num = _grain_tracker.getTotalFeatureCount();
  for (auto i = _angles.size(); i < grain_num; ++i)
  {
    angle.random(_random);
    _angles.push_back(angle);
  }
}

// Routine to convert the Euler angle specifying a rotation to a quaternion. 
Quaternion eulerAnglesToQuaternion(EulerAngles e)
{
   float alpha, beta, gamma;
   Quaternion *q1, *q2, *q3;

   alpha = e.getAlpha(); beta = e.getBeta(); gamma = e.getGamma(); 

   q1 = new Quaternion( cos( (PI/180.0) * (alpha/2.0) ), sin( (PI/180.0) * (alpha/2.0) ), 0.0, 0.0 );
   q2 = new Quaternion( cos( (PI/180.0) * (beta/2.0) ), 0.0, sin( (PI/180.0) * (beta/2.0) ), 0.0 );
   q3 = new Quaternion( cos( (PI/180.0) * (gamma/2.0) ), 0.0, 0.0, sin( (PI/180.0) * (gamma/2.0) ) );

   return multiplyQuaternions( *q1, multiplyQuaternions(*q2, *q3) );
}

  bool GlareSensor_Impl::setTransformation(const openstudio::Transformation& transformation) 
  {
    Vector3d translation = transformation.translation();
    this->setPositionXCoordinate(translation.x());
    this->setPositionYCoordinate(translation.y());
    this->setPositionZCoordinate(translation.z());

    EulerAngles eulerAngles = transformation.eulerAngles();
    setPsiRotationAroundXAxis(radToDeg(eulerAngles.psi()));
    setThetaRotationAroundYAxis(radToDeg(eulerAngles.theta()));
    setPhiRotationAroundZAxis(radToDeg(eulerAngles.phi()));
    
    return true;  
  }

Quaternion::Quaternion(const EulerAngles &euler) :
	Vector(4)
{
	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
	     sinPhi_2 * sinTheta_2 * sinPsi_2);
	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
	     cosPhi_2 * sinTheta_2 * sinPsi_2);
	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
	     sinPhi_2 * cosTheta_2 * sinPsi_2);
	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
	     sinPhi_2 * sinTheta_2 * cosPsi_2);
}

  bool PlanarSurfaceGroup_Impl::setTransformation(const openstudio::Transformation& transformation) {

    EulerAngles eulerAngles = transformation.eulerAngles();
    if ((eulerAngles.psi() != 0) || (eulerAngles.theta() != 0)){
      return false;
    }

    double dORN = -radToDeg(eulerAngles.phi());
    this->setDirectionofRelativeNorth(dORN, false);

    Vector3d translation = transformation.translation();
    this->setXOrigin(translation.x(), false);
    this->setYOrigin(translation.y(), false);
    this->setZOrigin(translation.z(), false);
    this->emitChangeSignals();

    return true;
  }

  bool IlluminanceMap_Impl::setTransformation(const openstudio::Transformation& transformation) 
  {
    bool test;

    Vector3d translation = transformation.translation();
    this->setOriginXCoordinate(translation.x());
    this->setOriginYCoordinate(translation.y());
    this->setOriginZCoordinate(translation.z());

    EulerAngles eulerAngles = transformation.eulerAngles();
    test = this->setPsiRotationAroundXAxis(radToDeg(eulerAngles.psi()));
    OS_ASSERT(test);
    test = this->setThetaRotationAroundYAxis(radToDeg(eulerAngles.theta()));
    OS_ASSERT(test);
    test = this->setPhiRotationAroundZAxis(radToDeg(eulerAngles.phi()));
    OS_ASSERT(test);

    return true;
  }

RankFourTensor
GrainTrackerElasticity::newGrain(unsigned int new_grain_id)
{
  EulerAngles angles;

  if (new_grain_id < _euler.getGrainNum())
    angles = _euler.getEulerAngles(new_grain_id);
  else
  {
    if (_random_rotations)
      angles.random();
    else
      mooseError("GrainTrackerElasticity has run out of grain rotation data.");
  }

  RankFourTensor C_ijkl = _C_ijkl;
  C_ijkl.rotate(RotationTensor(RealVectorValue(angles)));

  return C_ijkl;
}

void test(string videoName) {
    const int rate = 100;

    ifstream dataFile((videoName + "/TXT_" + videoName + ".txt").c_str());

    int start = 0;
    double tmp, frames;
    dataFile >> frames >> tmp;
    for (int i = 0; i < frames; ++i) {
        dataFile >> tmp;
        if (!start)
            start = tmp;
        dataFile >> tmp;

    }

    //vector<Quaternion> q;
    int i = 0;
    Quaternion p;
    p.identity();


    while (!dataFile.eof()) {
        dataFile >> tmp;
        double x, y, z;
        dataFile >> x >> y >> z;
        EulerAngles eulerAngles(y / rate, x / rate, z / rate);
        Quaternion tq;
        tq.setToRotateInertialToObject(eulerAngles);

        if (i % (100 / rate) == 0)
            p = p * tq;
        i++;
        EulerAngles e;
        e.fromInertialToObjectQuaternion(p);
        cout << tmp - start << " " << e.pitch << " " << e.heading << " " << e.bank << endl;
    }

}