C++ Rotation类代码示例

// goal = 1/2 sum( wi * ai^2 ) / sum(wi) for WRMS 
// ai == rotation angle between sensor and observation (-Pi:Pi)
int OrientationSensors::calcGoal(const State& state, Real& goal) const {
    const SimbodyMatterSubsystem& matter = getMatterSubsystem();
    goal = 0;
    // Loop over each body that has one or more active osensors.
    Real wtot = 0;
    PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.begin();
    for (; bodyp != bodiesWithOSensors.end(); ++bodyp) {
        const MobilizedBodyIndex    mobodIx      = bodyp->first;
        const Array_<OSensorIx>&    bodyOSensors = bodyp->second;
        const MobilizedBody&        mobod = matter.getMobilizedBody(mobodIx);
        const Rotation&             R_GB  = mobod.getBodyRotation(state);
        assert(bodyOSensors.size());
        // Loop over each osensor on this body.
        for (unsigned m=0; m < bodyOSensors.size(); ++m) {
            const OSensorIx mx = bodyOSensors[m];
            const OSensor&  osensor = osensors[mx];
            assert(osensor.bodyB == mobodIx); // better be on this body!
            const Rotation& R_GO = observations[getObservationIxForOSensor(mx)];
            if (R_GO.isFinite()) { // skip NaNs
                const Rotation R_GS = R_GB * osensor.orientationInB;
                const Rotation R_SO = ~R_GS*R_GO; // error, in S
                const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
                goal += osensor.weight * square(aa_SO[0]);
                wtot += osensor.weight;
            }
        }
    }

    goal /= (2*wtot);

    return 0;
}

void MemSpectrum::flatten(Spectrum * spec, const Rotation & rot)
{
	assert( spec );
	assert( spec->getDimCount() == rot.size() );
	assert( rot.size() > 2 );
	const int cx = spec->getScale( rot[ DimX ] ).getSampleCount();
	const int cy = spec->getScale( rot[ DimY ] ).getSampleCount();
	int x,y;
	Rotation cut( rot.size() - 2 );
	Dimension d;
	for( d = 0; d < cut.size(); d++ )
		cut[ d ] = rot[ d + 2 ];
	Root::Cube cube( rot.size() );
	int c;
	for( d = DimZ; d < rot.size(); d++ )
	{
		c = spec->getScale( rot[ d ] ).getSampleCount();
		cube[ rot[ d ] ].first = 0;
		cube[ rot[ d ] ].second = c - 1;
	}

	Buffer buf;
	for( x = 0; x < cx; x++ )
	{
		cube[ rot[ DimX ] ].first = cube[ rot[ DimX ] ].second = x;
		for( y = 0; y < cy; y++ )
		{
			cube[ rot[ DimY ] ].first = cube[ rot[ DimY ] ].second = y;
			spec->fillBuffer( buf, cut, cube );
			d_buf.setAt( x, y, buf.calcMean() );
		}
	}
}

//-------------------------------------------------------------------
bool  testRotationTwoAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2 ) {

    // Form rotation about specified axes
    Rotation rotationSpecified( bodyOrSpace, angle1, axis1, angle2, axis2 );

    // Form equivalent rotation by another means
    Rotation AB( angle1, axis1 );
    Rotation BC( angle2, axis2 );
    Rotation testRotation = (bodyOrSpace == BodyRotationSequence) ? AB * BC : BC * AB;

    // Test to see if they are the same
    bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );

    // Do the inverse problem to back out the angles
    const Vec2 testVec = rotationSpecified.convertTwoAxesRotationToTwoAngles( bodyOrSpace, axis1, axis2 );
    const Real theta1 = testVec[0];
    const Real theta2 = testVec[1];

    // Create a Rotation matrix with the backed-out angles and compare to the original Rotation matrix
    testRotation.setRotationFromTwoAnglesTwoAxes( bodyOrSpace, theta1, axis1, theta2, axis2 );
    test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );

    // Conversion should produce same angles for for appropriate ranges of angle1 and angle2
    if( axis1.isSameAxis(axis2) ) 
       test = test && testInverseRotation1Angle( angle1+angle2, theta1+theta2 );
    else
       test = test && testInverseRotation2Angle( angle1,theta1, angle2,theta2 );

    return test;
}

TYPED_TEST(EulerAnglesXyzDiffTest, testFiniteDifference)
{
  typedef typename TestFixture::Scalar Scalar;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::RotationDiff RotationDiff;
  typedef  typename TestFixture::Vector3 Vector3;

  const  double dt = 1.0e-3;
  for (auto rotation : this->rotations) {
    for (auto angularVelocity2 : this->angularVelocities) {
      RotationDiff rotationDiff(angularVelocity2.toImplementation());
      rot::LocalAngularVelocity<Scalar> angularVelocity(rotation, rotationDiff);
      // Finite difference method for checking derivatives

      Rotation rotationNext = rotation.boxPlus(dt*rotation.rotate(angularVelocity.vector()));
      rotationNext.setUnique();
      rotation.setUnique();
      Vector3 dn = (rotationNext.toImplementation()-rotation.toImplementation())/dt;

      ASSERT_NEAR(rotationDiff.x(),dn(0),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext  << " diff: " << rotationDiff  << " approxdiff: " << dn.transpose();
      ASSERT_NEAR(rotationDiff.y(),dn(1),1e-3)  << " angular velocity: " << angularVelocity  <<  "rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
      ASSERT_NEAR(rotationDiff.z(),dn(2),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext  << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();

    }
  }
}

Placement Command::getPlacement (void)
{
    std::string x = "X";
    std::string y = "Y";
    std::string z = "Z";
    std::string a = "A";
    std::string b = "B";
    std::string c = "C";
    double xval = 0.0;
    double yval = 0.0;
    double zval = 0.0;
    double aval = 0.0;
    double bval = 0.0;
    double cval = 0.0;
    if (Parameters.count(x))
        xval = Parameters[x];
    if (Parameters.count(y))
        yval = Parameters[y];
    if (Parameters.count(z))
        zval = Parameters[z];
    if (Parameters.count(a))
        aval = Parameters[a];
    if (Parameters.count(b))
        bval = Parameters[b];
    if (Parameters.count(c))
        cval = Parameters[c];
    Vector3d vec(xval,yval,zval);
    Rotation rot;
    rot.setYawPitchRoll(aval,bval,cval);
    Placement plac(vec,rot);
    return plac;
}

void Camera::ProjectPoints(const CvMat* object_points, double gl[16], CvMat* image_points) const
{
	double glm[4][4] = {
		gl[0], gl[4], gl[8],  gl[12],
		gl[1], gl[5], gl[9],  gl[13],
		gl[2], gl[6], gl[10], gl[14],
		gl[3], gl[7], gl[11], gl[15],
	};
	CvMat glm_mat = cvMat(4, 4, CV_64F, glm);
	
	// For some reason we need to mirror both y and z ???
	double cv_mul_data[4][4];
	CvMat cv_mul = cvMat(4, 4, CV_64F, cv_mul_data);
	cvSetIdentity(&cv_mul);
	cvmSet(&cv_mul, 1, 1, -1);
	cvmSet(&cv_mul, 2, 2, -1);
	cvMatMul(&cv_mul, &glm_mat, &glm_mat);
	
	// Rotation
	Rotation r;
	r.SetMatrix(&glm_mat);
	double rod[3]; 
	CvMat rod_mat=cvMat(3, 1, CV_64F, rod);
	r.GetRodriques(&rod_mat);
	// Translation
	double tra[3] = { glm[0][3], glm[1][3], glm[2][3] };
	CvMat tra_mat = cvMat(3, 1, CV_64F, tra);
	// Project points
	ProjectPoints(object_points, &rod_mat, &tra_mat, image_points);
}

/**
 * Convert angles to direction cosines.
 * @param aE1 1st Euler angle.
 * @param aE2 2nd Euler angle.
 * @param aE3 3rd Euler angle.
 * @param rDirCos Vector of direction cosines.
 */
void SimbodyEngine::convertAnglesToDirectionCosines(double aE1, double aE2, double aE3, double rDirCos[3][3]) const
{
    Vec3 angs(aE1, aE2, aE3);
    Rotation aRot; 
    aRot.setRotationToBodyFixedXYZ(angs);
    Mat33::updAs(&rDirCos[0][0]) = aRot.asMat33();
}

void CameraController::rotateTowardRobot(Vector3d r_pos)
{
  m_my->setPosition(m_rotatePos);
  // カメラの位置を得る
  m_my->getPosition(m_pos);

  // カメラの位置からロボットを結ぶベクトル
  Vector3d tmpp = r_pos;
  tmpp -= m_pos;

  // y方向は考えない
  tmpp.y(0);

  // カメラの回転角度を得る
  Rotation myRot;
  m_my->getRotation(myRot);

  // カメラのy軸の回転角度を得る(x,z方向の回転は無いと仮定)
  double qw = myRot.qw();
  double qy = myRot.qy();
  double theta = 2*acos(fabs(qw));

  if(qw*qy < 0)
    theta = -1*theta;

  // ロボットまでの回転角度を得る
  double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
  double targetAngle = acos(tmp);
  if(tmpp.x() > 0) targetAngle = -1*targetAngle;

  // 角度差から回転量を得る
  targetAngle += theta;
  m_my->setAxisAndAngle(0, 1, 0, -targetAngle, 0);
}

Actor* addMap()
{
	Actor* sphere = new Actor;

	TransformComponent* trans = new TransformComponent();

	trans->setPos(vec3(0, -0.01, 0));
	Rotation rot;
	rot.setEulerAngles(vec3(0, 0.785, 0));
	trans->setRotation(rot);
	sphere->addComponent(trans);
	glDebug();
	sphere->addComponent(new StaticMeshStaticPhysicsComponent(AssetManager::getBasePath() + "Data/Model/mappa4.obj"));
	glDebug();
	MeshGraphicComponent* mesh = new MeshGraphicComponent(AssetManager::getBasePath() + "Data/Model/mappa4.obj");
	glDebug();
	mesh->setTextureMatrix(mat2(scale(mat4(1), vec3(10))));
	glDebug();
	Material *mat = new Material();
	glDebug();
	mat->setDiffuse(AssetManager::getBasePath() + "Data/Texture/floor1_d.png");
	mat->setNormal(AssetManager::getBasePath() + "Data/Texture/floor1_n.png");
	mat->setSpecular(AssetManager::getBasePath() + "Data/Texture/floor1_s.png");
	mat->setShininess(20);
	glDebug();
	mesh->addMaterial(mat);
	glDebug();
	sphere->addComponent(mesh);
	glDebug();
	return sphere;
}

Actor* addAxis3()
{
	Actor* actor = new Actor;

	TransformComponent* trans = new TransformComponent();
	trans->setPos(vec3(-5, 0, 5));

	Rotation rot;
	rot.setNormalDirection(vec3(1, 0, 0));
	trans->setRotation(rot);

	actor->addComponent(trans);
	MeshGraphicComponent* mesh = new MeshGraphicComponent(AssetManager::getBasePath() + "Data/Model/axis.obj");
	Material *mat = new Material();
	mat->setDiffuse("#FF0000");
	mesh->addMaterial(mat);
	Material *mat1 = new Material();
	mat1->setDiffuse("#00FF00");
	mesh->addMaterial(mat1);
	mesh->addMaterial(mat1);
	Material *mat2 = new Material();
	mat2->setDiffuse("#0000FF");
	mesh->addMaterial(mat2);
	actor->addComponent(mesh);
	return actor;
}

/**
 * Convert quaternions to direction cosines.
 * @param aQ1 1st Quaternion.
 * @param aQ2 2nd Quaternion.
 * @param aQ3 3rd Quaternion.
 * @param aQ4 4th Quaternion.
 * @param rDirCos Vector of direction cosines.
 */
void SimbodyEngine::convertQuaternionsToDirectionCosines(double aQ1, double aQ2, double aQ3, double aQ4, double rDirCos[3][3]) const
{
    Rotation R;
    R.setRotationFromQuaternion(Quaternion(Vec4(aQ1, aQ2, aQ3, aQ4)));

    Mat33::updAs(&rDirCos[0][0]) = R.asMat33();
}

void Drawable::SetGLMatTraQuat(double *tra, double *quat, bool flip)
{
	Rotation r;
	if (quat != 0)
	{
		CvMat cv_mat;
		cvInitMatHeader(&cv_mat, 4, 1, CV_64F, quat);
		r.SetQuaternion(&cv_mat);
	}

	int flp = 1;
	if (flip)
	{
		r.Transpose();
		//flp=-1;
	}

	CvMat cv_gl_mat;
	cvInitMatHeader(&cv_gl_mat, 4, 4, CV_64F, gl_mat); cvZero(&cv_gl_mat);
	r.GetMatrix(&cv_gl_mat);
	cvSet2D(&cv_gl_mat, 0, 3, cvScalar(flp*tra[0]));
	cvSet2D(&cv_gl_mat, 1, 3, cvScalar(flp*tra[1]));
	cvSet2D(&cv_gl_mat, 2, 3, cvScalar(flp*tra[2]));
	cvSet2D(&cv_gl_mat, 3, 3, cvScalar(1));

	cvTranspose(&cv_gl_mat, &cv_gl_mat);
}

double DemoRobotController::rotateTowardObj(Vector3d pos)
{
	// "pos" means target position
	// get own position
	Vector3d ownPosition;
	m_robotObject->getPartsPosition(ownPosition,"RARM_LINK2");

	// pointing vector for target
	Vector3d l_pos = pos;
	l_pos -= ownPosition;

	// ignore variation on y-axis
	l_pos.y(0);

	// get own rotation matrix
	Rotation ownRotation;
	m_robotObject->getRotation(ownRotation);

	// get angles arround y-axis
	double qw = ownRotation.qw();
	double qy = ownRotation.qy();
	double theta = 2*acos(fabs(qw));

	if(qw*qy < 0) theta = -1.0*theta;

	// rotation angle from z-axis to x-axis
	double tmp = l_pos.angle(Vector3d(0.0, 0.0, 1.0));
	double targetAngle = acos(tmp);

	// direction
	if(l_pos.x() > 0) targetAngle = -1.0*targetAngle;
	targetAngle += theta;

	double angVelFac = 3.0;
	double l_angvel = m_angularVelocity/angVelFac;

	if(targetAngle == 0.0) {
		return 0.0;
	}
	else {
		// circumferential distance for the rotation
		double l_distance = m_distance*M_PI*fabs(targetAngle)/(2.0*M_PI);

		// Duration of rotation motion (micro second)
		double l_time = l_distance / (m_movingSpeed/angVelFac);

		// Start the rotation
		if(targetAngle > 0.0) {
			m_robotObject->setWheelVelocity(l_angvel, -l_angvel);
		}
		else{
			m_robotObject->setWheelVelocity(-l_angvel, l_angvel);
		}

		return l_time;
	}
}

void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle)
{
	if (dynamics()) { return; }

	Rotation r;
	r.setAxisAndAngle(ax, ay, az, angle);
	const dReal *q = r.q();
	setQ(q);
}

AbstractObject& Obj::rotate(double ax, double ay, double az)
{
    debug("EXPR ROTATE!!!");
    Rotation* t = new Rotation(ax,ay,az);
    t->set_dynamic(true);
    t->add(*this);

    return *t;
}