void CQDlgTextures::_setTextureConfig(int index)
{
QLineEdit* ww[6]={ui->qqX,ui->qqY,ui->qqZ,ui->qqAlpha,ui->qqBeta,ui->qqGamma};
if (!isLinkedDataValid())
return;
CTextureProperty* tp=NULL;
CShape* shape=NULL;
if (_identification[1]==0)
{ // texture is linked to a shape
shape=App::ct->objCont->getShape(_identification[2]);
tp=((CGeometric*)shape->geomData->geomInfo)->getTextureProperty();
bool ok;
float newVal=ww[index]->text().toFloat(&ok);
if ((tp!=NULL)&&ok)
{
C7Vector tr(tp->getTextureRelativeConfig());
if (index<3)
{ // position
newVal=tt::getLimitedFloat(-100.0f,100.0f,newVal*gv::userToMeter);
tr.X(index)=newVal;
}
else
{ // orientation
C3Vector euler(tr.Q.getEulerAngles());
euler(index-3)=newVal*gv::userToRad;
tr.Q.setEulerAngles(euler);
}
tp->setTextureRelativeConfig(tr);
App::ct->undoBufferContainer->announceChange(); // **************** UNDO THINGY ****************
}
}
}
void MasterController::slaveOdometryCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
// Pose slave
Eigen::Matrix<double,3,1> euler=Eigen::Quaterniond(msg->pose.pose.orientation.w,
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z).matrix().eulerAngles(2, 1, 0);
double yaw = euler(0,0);
double pitch = euler(1,0);
double roll = euler(2,0);
if(!init_slave_readings)
{
previous_pose_slave << msg->pose.pose.position.x,
msg->pose.pose.position.y,
msg->pose.pose.position.z,
roll-previous_pose_slave(3,0),
pitch-previous_pose_slave(4,0),
yaw; // should be relative
// std::cout << "previous_pose_slave:" << previous_pose_slave(5,0) << " yaw:" << yaw << std::endl;
// std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl;
yaw_slave_previous=yaw;
init_slave_readings=true;
return;
}
else
{
// lastPositionUpdate = ros::Time::now().toSec();
current_pose_slave << msg->pose.pose.position.x,
msg->pose.pose.position.y,
msg->pose.pose.position.z,
roll-previous_pose_slave(3,0),
pitch-previous_pose_slave(4,0),
yaw_slave_previous; // should be relative
// std::cout << "current_pose_slave:" << current_pose_slave(5,0) << " yaw_slave_previous:" << yaw_slave_previous << std::endl;
// std::cout << current_pose_slave(0,0) << std::endl ;
// std::cout << current_pose_slave(1,0) << std::endl ;
// std::cout << current_pose_slave(2,0) << std::endl ;
// std::cout << current_pose_slave(3,0) << std::endl ;
// std::cout << current_pose_slave(4,0) << std::endl ;
// std::cout << current_pose_slave(5,0) << std::endl ;
// double test = current_pose_slave(5,0) ;
// std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl;
// std::cout << "current_pose_slave:" << test << " previous_pose_slave previous:" << previous_pose_slave(5,0) << std::endl;
// std::cout << "yaw TO DEG:" << yaw*RAD_TO_DEG << " yaw previous TO DEG:" << yaw_slave_previous * RAD_TO_DEG << std::endl;
// std::cout << "current_pose_slave TO DEG (((:" << current_pose_slave(5,0)*RAD_TO_DEG << " previous_pose_slave previous TO DEG )))):" << previous_pose_slave(5,0) * RAD_TO_DEG << std::endl;
yaw_slave_previous=yaw;
}
current_velocity_slave << msg->twist.twist.linear.x,
msg->twist.twist.linear.y,
msg->twist.twist.linear.z,
msg->twist.twist.angular.x,
msg->twist.twist.angular.y,
msg->twist.twist.angular.z;
slave_new_readings=true;
feedback();
previous_pose_slave=current_pose_slave;
}
void
FixedwingEstimator::print_status()
{
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
(double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
// State vector:
// 0-3: quaternions (q0, q1, q2, q3)
// 4-6: Velocity - m/sec (North, East, Down)
// 7-9: Position - m (North, East, Down)
// 10-12: Delta Angle bias - rad (X,Y,Z)
// 13: Accelerometer offset
// 14-15: Wind Vector - m/sec (North,East)
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
if (n_states == 23) {
printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]);
} else {
printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
}
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
}
C7Vector CQDlgTranslations::_getNewTransf(const C7Vector& transf,float newValueInUserUnit,bool orientation,int index)
{
C7Vector retVal(transf);
if (orientation)
{
C3Vector euler(retVal.Q.getEulerAngles());
euler(index)=newValueInUserUnit*gv::userToRad;
retVal.Q.setEulerAngles(euler(0),euler(1),euler(2));
}
else
retVal.X(index)=newValueInUserUnit*gv::userToMeter;
return(retVal);
}
// SLAVE MEASUREMENTS
void SlaveController::slaveOdometryCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
// Pose slave
Eigen::Matrix<double,3,1> euler=Eigen::Quaterniond(msg->pose.pose.orientation.w,
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z).matrix().eulerAngles(2, 1, 0);
double yaw = euler(0,0);
double pitch = euler(1,0);
double roll = euler(2,0);
if(!init_slave_readings)
{
previous_pose_slave << msg->pose.pose.position.x,
msg->pose.pose.position.y,
msg->pose.pose.position.z,
roll-previous_pose_slave(3,0),
pitch-previous_pose_slave(4,0),
yaw; // should be relative
//std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl;
yaw_slave_previous=yaw;
init_slave_readings=true;
return;
}
else
{
current_pose_slave << msg->pose.pose.position.x,
msg->pose.pose.position.y,
msg->pose.pose.position.z,
roll-previous_pose_slave(3,0),
pitch-previous_pose_slave(4,0),
yaw-yaw_slave_previous; // should be relative
//std::cout << "yaw:" << yaw << " yaw previous:" << yaw_slave_previous << std::endl;
yaw_slave_previous=yaw;
}
current_velocity_slave << msg->twist.twist.linear.x,
msg->twist.twist.linear.y,
msg->twist.twist.linear.z,
msg->twist.twist.angular.x,
msg->twist.twist.angular.y,
msg->twist.twist.angular.z;
slave_new_readings=true;
feedback();
previous_pose_slave=current_pose_slave;
}
void Ekf::printStates()
{
static int counter = 0;
if (counter % 50 == 0) {
printf("quaternion\n");
for (int i = 0; i < 4; i++) {
printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
}
matrix::Euler<float> euler(_state.quat_nominal);
printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
printf("vel\n");
for (int i = 0; i < 3; i++) {
printf("v %i %.5f\n", i, (double)_state.vel(i));
}
printf("pos\n");
for (int i = 0; i < 3; i++) {
printf("p %i %.5f\n", i, (double)_state.pos(i));
}
printf("gyro_scale\n");
for (int i = 0; i < 3; i++) {
printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
}
printf("mag earth\n");
for (int i = 0; i < 3; i++) {
printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
}
printf("mag bias\n");
for (int i = 0; i < 3; i++) {
printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
}
counter = 0;
}
counter++;
}
void InteractionHandler::keyboardCallback(Key key, KeyModifier modifier, KeyAction action) {
// TODO package in script
const float speed = _controllerSensitivity;
const float dt = static_cast<float>(_deltaTime);
if (action == KeyAction::Press || action == KeyAction::Repeat) {
if ((key == Key::Right) && (modifier == KeyModifier::NoModifier)) {
glm::vec3 euler(0.0, speed * dt*0.4, 0.0);
glm::quat rot = glm::quat(euler);
rotateDelta(rot);
}
if ((key == Key::Left) && (modifier == KeyModifier::NoModifier)) {
glm::vec3 euler(0.0, -speed * dt*0.4, 0.0);
glm::quat rot = glm::quat(euler);
rotateDelta(rot);
}
if ((key == Key::Down) && (modifier == KeyModifier::NoModifier)) {
glm::vec3 euler(speed * dt*0.4, 0.0, 0.0);
glm::quat rot = glm::quat(euler);
rotateDelta(rot);
}
if ((key == Key::Up) && (modifier == KeyModifier::NoModifier)) {
glm::vec3 euler(-speed * dt*0.4, 0.0, 0.0);
glm::quat rot = glm::quat(euler);
rotateDelta(rot);
}
if ((key == Key::KeypadSubtract) && (modifier == KeyModifier::NoModifier)) {
glm::vec2 s = OsEng.renderEngine().camera()->scaling();
s[1] -= 0.5f;
OsEng.renderEngine().camera()->setScaling(s);
}
if ((key == Key::KeypadAdd) && (modifier == KeyModifier::NoModifier)) {
glm::vec2 s = OsEng.renderEngine().camera()->scaling();
s[1] += 0.5f;
OsEng.renderEngine().camera()->setScaling(s);
}
// iterate over key bindings
//_validKeyLua = true;
auto ret = _keyLua.equal_range({ key, modifier });
for (auto it = ret.first; it != ret.second; ++it) {
//OsEng.scriptEngine()->runScript(it->second);
OsEng.scriptEngine().queueScript(it->second);
//if (!_validKeyLua) {
// break;
//}
}
}
}
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;
}
list<int>::iterator euler(int at, int to,
list<int>::iterator it) {
if (at == to) return it;
L.insert(it, at), --it;
while (!adj[at].empty()) {
int nxt = *adj[at].begin();
adj[at].erase(adj[at].find(nxt));
adj[nxt].erase(adj[nxt].find(at));
if (to == -1) {
it = euler(nxt, at, it);
L.insert(it, at);
--it;
} else {
it = euler(nxt, to, it);
to = -1; } }
return it; }
void OutputBase::_calculate_output_angles(const hrt_abstime &t)
{
//take speed into account
float dt = (t - _last_update) / 1.e6f;
for (int i = 0; i < 3; ++i) {
_angle_setpoints[i] += dt * _angle_speeds[i];
}
//get the output angles and stabilize if necessary
vehicle_attitude_s vehicle_attitude;
matrix::Eulerf euler;
if (_stabilize[0] || _stabilize[1] || _stabilize[2]) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
euler = matrix::Quatf(vehicle_attitude.q);
}
for (int i = 0; i < 3; ++i) {
if (_stabilize[i]) {
_angle_outputs[i] = _angle_setpoints[i] - euler(i);
} else {
_angle_outputs[i] = _angle_setpoints[i];
}
//bring angles into proper range [-pi, pi]
while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; }
while (_angle_outputs[i] < -M_PI_F) { _angle_outputs[i] += 2.f * M_PI_F; }
}
}
RSA::RSA(){
long long p=Generar_primo(-1);
long long q=Generar_primo(p);
long long n=p*q;
long long phi=euler(p,q);
}
/*! Construct a list of rotations for a given spacegroup and sampling.
\param spgr The spacegroup.
\param step The search andle step (in degrees, NOT radians). */
std::vector<clipper::RTop_orth> LLK_map_target::rtop_list( const clipper::Spacegroup& spgr, const clipper::ftype& step ) const
{
std::vector<clipper::RTop_orth> ops;
// make a list of rotation ops to try
float glim = 360.0; // gamma
float blim = 180.0; // beta
float alim = 360.0; // alpha
// reduce search angles by symmetry rotations
alim /= float( spgr.order_of_symmetry_about_axis( clipper::Spacegroup::C ) );
if ( spgr.order_of_symmetry_about_axis( clipper::Spacegroup::A ) % 2 == 0 ||
spgr.order_of_symmetry_about_axis( clipper::Spacegroup::B ) % 2 == 0 )
blim /= 2.0;
// do a uniformly sampled search of orientation space
float anglim = clipper::Util::min( alim, glim );
for ( float bdeg=step/2; bdeg < 180.0; bdeg += step ) {
float beta = clipper::Util::d2rad(bdeg);
float spl = anglim/clipper::Util::intf(cos(0.5*beta)*anglim/step+1);
float smi = anglim/clipper::Util::intf(sin(0.5*beta)*anglim/step+1);
for ( float thpl=spl/2; thpl < 720.0; thpl += spl )
for ( float thmi=smi/2; thmi < 360.0; thmi += smi ) {
float adeg = clipper::Util::mod(0.5*(thpl+thmi),360.0);
float gdeg = clipper::Util::mod(0.5*(thpl-thmi),360.0);
if ( adeg <= alim && bdeg <= blim && gdeg <= glim ) {
float alpha = clipper::Util::d2rad(adeg);
float gamma = clipper::Util::d2rad(gdeg);
clipper::Euler_ccp4 euler( alpha, beta, gamma );
ops.push_back(clipper::RTop_orth(clipper::Rotation(euler).matrix()));
}
}
}
// return the rotations
return ops;
}
/* calloc() wrapper
*/
link_t *
ecalloc (void)
{
link_t *e;
if (!euler ())
{
return NULL;
}
/* straight outta cache
*/
e = E.next;
E.next = e->next;
e->next = NULL; /* like calloc() */
e->ptr = NULL;
e->num = 0;
/* yes one less in the E cache
*/
E.num--;
return e;
} /* ecalloc() */
void LightAnimData::animate( LightInfo *lightInfo, LightAnimState *state )
{
PROFILE_SCOPE( LightAnimData_animate );
// Calculate the input time for animation.
F32 time = state->animationPhase +
( (F32)Sim::getCurrentTime() * 0.001f ) /
state->animationPeriod;
MatrixF transform( state->transform );
EulerF euler( Point3F::Zero );
if ( mRot.animate( time, euler ) )
{
euler.x = mDegToRad( euler.x );
euler.y = mDegToRad( euler.y );
euler.z = mDegToRad( euler.z );
MatrixF rot( euler );
transform.mul( rot );
}
Point3F offset( Point3F::Zero );
if ( mOffset.animate( time, offset ) )
transform.displace( offset );
lightInfo->setTransform( transform );
ColorF color = state->color;
mColor.animate( time, color );
lightInfo->setColor( color );
F32 brightness = state->brightness;
mBrightness.animate( time, &brightness );
lightInfo->setBrightness( brightness );
}
std::vector<double> BaxterArmCommander::quat2euler(geometry_msgs::Quaternion quaternion) {
double mData[4];
std::vector<double> euler(3);
const static double PI_OVER_2 = M_PI * 0.5;
const static double EPSILON = 1e-10;
double sqw, sqx, sqy, sqz;
mData[0] = quaternion.x;
mData[1] = quaternion.y;
mData[2] = quaternion.z;
mData[3] = quaternion.w;
// quick conversion to Euler angles to give tilt to user
sqw = mData[3] * mData[3];
sqx = mData[0] * mData[0];
sqy = mData[1] * mData[1];
sqz = mData[2] * mData[2];
euler[1] = asin(2.0 * (mData[3] * mData[1] - mData[0] * mData[2]));
if (PI_OVER_2 - fabs(euler[1]) > EPSILON) {
euler[2] = atan2(2.0 * (mData[0] * mData[1] + mData[3] * mData[2]),
sqx - sqy - sqz + sqw);
euler[0] = atan2(2.0 * (mData[3] * mData[0] + mData[1] * mData[2]),
sqw - sqx - sqy + sqz);
} else {
// compute heading from local 'down' vector
euler[2] = atan2(2 * mData[1] * mData[2] - 2 * mData[0] * mData[3],
2 * mData[0] * mData[2] + 2 * mData[1] * mData[3]);
euler[0] = 0.0;
// If facing down, reverse yaw
if (euler[1] < 0)
euler[2] = M_PI - euler[2];
}
return euler;
}
void OsgNav::preFrame()
{
vpr::Interval cur_time = mWand->getTimeStamp();
vpr::Interval diff_time(cur_time-mLastPreFrameTime);
if (mLastPreFrameTime.getBaseVal() >= cur_time.getBaseVal())
{ diff_time.secf(0.0f); }
float time_delta = diff_time.secf();
// Cluster debug code.
// std::cout << "CLUSTER Delta: " << diff_time.getBaseVal() << std::endl;
// std::cout << "CLUSTER Current: " << cur_time.getBaseVal() << "Last: "
// << mLastTimeStamp.getBaseVal() << "\n" << std::endl;
mLastPreFrameTime = cur_time;
//vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) << "------- preFrame ------\n"
// << vprDEBUG_FLUSH;
// Get wand data
gmtl::Matrix44f wandMatrix = mWand->getData(); // Get the wand matrix
// If we are pressing button 1 then translate in the direction the wand is
// pointing.
if ( mButton0->getData() == gadget::Digital::ON )
{
gmtl::Vec3f direction;
gmtl::Vec3f Zdir = gmtl::Vec3f(0.0f, 0.0f, -10.0f);
gmtl::xform(direction, wandMatrix, Zdir);
mNavigator.setVelocity(direction);
} // Make sure to reset the velocity when we stop pressing the button.
else if ( mButton0->getData() == gadget::Digital::TOGGLE_OFF)
{
mNavigator.setVelocity(gmtl::Vec3f(0.0, 0.0, 0.0));
}
if ( mButton1->getData() == gadget::Digital::TOGGLE_ON)
{
std::cout << "Cur Pos: " << mNavigator.getCurPos() << std::endl;
}
// If we are pressing button 2 then rotate in the direction the wand is
// pointing.
if ( mButton2->getData() == gadget::Digital::ON )
{
// Only allow Yaw (rot y)
gmtl::EulerAngleXYZf euler(0.0f, gmtl::makeYRot(mWand->getData()), 0.0f);
gmtl::Matrix44f rot_mat = gmtl::makeRot<gmtl::Matrix44f>(euler);
mNavigator.setRotationalVelocity(rot_mat);
}
// Make sure to reset the rotational velocity when we stop pressing the
// button.
else if(mButton2->getData() == gadget::Digital::TOGGLE_OFF)
{
mNavigator.setRotationalVelocity(gmtl::Matrix44f());
}
// Update the navigation using the time delta between
mNavigator.update(time_delta);
}
void euler(int u){
for(int v = 0; v < n; v++) if(G[u][v] && !vis[u][v]) {
vis[u][v] = vis[v][u] = 1;
euler(v);
printf("%d %d\n", u, v); // 打印的顺序是逆序的：最先打印的边实际上应该最后经过
}
}
// discrete-logarithm, finding y for equation k = x^y % mod
int discrete_logarithm(int x, int mod, int k) {
if (mod == 1) return 0;
int s = 1, g;
for (int i = 0; i < 64; ++i) {
if (s == k) return i;
s = (1ll * s * x) % mod;
}
while ((g = gcd(x, mod)) != 1) {
if (k % g) return -1;
mod /= g;
}
static unordered_map<int, int> M; M.clear();
int q = int(sqrt(double(euler(mod)))) + 1; // mod-1 is also okay
for (int i = 0, b = 1; i < q; ++i) {
if (M.find(b) == M.end()) M[b] = i;
b = (1ll * b * x) % mod;
}
int p = fpow(x, q, mod);
for (int i = 0, b = 1; i <= q; ++i) {
int v = (1ll * k * inverse(b, mod)) % mod;
if (M.find(v) != M.end()) {
int y = i * q + M[v];
if (y >= 64) return y;
}
b = (1ll * b * p) % mod;
}
return -1;
}
int main ()
{
scanf("%d", &tcase);
for (fcase = 1;fcase <= tcase;fcase++)
{
scanf("%d", &n);
for (i = 1;i <= 26;i++)//初始化
{
in[i] = out[i] = 0;
f[i] = i;
node[i].next = -1;
}
totnode = 26;
totans = 0;
for (i = 1; i <= n; i++)
{
scanf("%s", s);
addpath(s[0] - 'a'+1, s[strlen(s) - 1] - 'a'+1, s);
}
if (!check(26))//如果不是一个欧拉图
{
output(false);//输出。
continue;//继续上面的for循环（直接处理下一组数据了）
}
int flag;
flag = false;
for (i = 1; i <= 26; i++)//首先判断是否存在欧拉路
{
if (out[i] == in[i] + 1)//如果哪个点的出度比入度多1，那么从这个点开始找
{
euler(i, -1);
flag = true;//标记
break;
}
}
if (flag == false)//如果没有欧拉路，那么找欧拉回路
{
for (i = 1; i <= 26; i++)
if (out[i])//随便从一个点开始
{
euler(i, -1);//找
break;
}
}
output(true);//输出
}
}
int main(int argc, char** argv){
parse_args(argc, argv);
if(print_usage_flag) {
print_usage();
return 0;
}
init();
short accel[3], gyro[3], sensors[1];
long quat[4];
unsigned long timestamp;
unsigned char more[0];
struct pollfd fdset[1];
char buf[1];
// File descriptor for the GPIO interrupt pin
int gpio_fd = open(GPIO_INT_FILE, O_RDONLY | O_NONBLOCK);
// Create an event on the GPIO value file
memset((void*)fdset, 0, sizeof(fdset));
fdset[0].fd = gpio_fd;
fdset[0].events = POLLPRI;
while (1){
// Blocking poll to wait for an edge on the interrupt
if(!no_interrupt_flag) {
poll(fdset, 1, -1);
}
if (no_interrupt_flag || fdset[0].revents & POLLPRI) {
// Read the file to make it reset the interrupt
if(!no_interrupt_flag) {
read(fdset[0].fd, buf, 1);
}
int fifo_read = dmp_read_fifo(gyro, accel, quat, ×tamp, sensors, more);
if (fifo_read != 0) {
//printf("Error reading fifo.\n");
continue;
}
float angles[NOSENTVALS];
rescale_l(quat, angles+9, QUAT_SCALE, 4);
// rescale the gyro and accel values received from the IMU from longs that the
// it uses for efficiency to the floats that they actually are and store these values in the angles array
rescale_s(gyro, angles+3, GYRO_SCALE, 3);
rescale_s(accel, angles+6, ACCEL_SCALE, 3);
// turn the quaternation (that is already in angles) into euler angles and store it in the angles array
euler(angles+9, angles);
if(!silent_flag && verbose_flag) {
printf("Yaw: %+5.1f\tPitch: %+5.1f\tRoll: %+5.1f\n", angles[0]*180.0/PI, angles[1]*180.0/PI, angles[2]*180.0/PI);
}
// send the values in angles over UDP as a string (in udp.c/h)
if(!no_broadcast_flag) {
udp_send(angles, 13);
}
}
}
}
main()
{
int n;
while(scanf("%d",&n)==1){
if(!n) return;
printf("%d\n",euler(n));
}
}
int main(void)
{
//stampo le distanze dall'origine dopo 100000 iterazioni con i due metodi: quale sarà la più grande?
std::cout << euler(0.02, 100000) << "\n";
std::cout << leapfrog(0.02,100000)<< "\n";
return 0;
}
DiQuat DiK2Configs::ConvertAngles(const DiVec3& eulerrot)
{
DiQuat q;
DiVec3 euler(DiDegree(eulerrot.x).valueRadians(),
DiDegree(eulerrot.y).valueRadians(), DiDegree(eulerrot.z).valueRadians());
DiEuler::ToQuat(q, euler, DiEuler::YXZ);
return q;
}
Vector print_euler(double h_value)
{
printf("Euler with h = %g\n", h_value);
Vector euler_ys = euler(h_value);
printVector(&euler_ys, stdout);
printf("\n\n");
return euler_ys;
}
void AttitudePositionEstimatorEKF::publishAttitude()
{
// Output results
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
PX4_R(_att.R, i, j) = R(i, j);
}
}
_att.timestamp = _last_sensor_timestamp;
_att.q[0] = _ekf->states[0];
_att.q[1] = _ekf->states[1];
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.R_valid = true;
_att.timestamp = _last_sensor_timestamp;
_att.roll = euler(0);
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
} else {
/* advertise and publish */
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
}
}
void euler(int u)
{
for(int v=1;v<=n;v++) if(g[u][v]&&!vis[u][v]){
vis[u][v]=vis[v][u]=1; //有向图只需改为vis[u][v]=1
euler(v);
top++;
su[top]=u; sv[top]=v;
}
}
void main()
{
int n;
double h;
double *V0;
double *V1;
double *V;
double *F0;
double *F1;
double xmax;
int nmax;
int i,j;
//Récupération des informations générales sur la méthode
scanf("%d %lf %lf",&n,&xmax,&h);
//Création des tableaux dynamiques
V0=(double *) malloc(n*sizeof(double));
V1=(double *) malloc(n*sizeof(double));
V=(double *) malloc(n*sizeof(double));
F0=(double *) malloc(n*sizeof(double));
F1=(double *) malloc(n*sizeof(double));
//Lecture et écriture des conditions initiales
for(i=0;i<n;i++)
{
scanf("%lf",&V0[i]);
printf("%lf ",V0[i]);
}
printf("\n");
//Calcul du nombre d'itérations
nmax=trunc((xmax-V0[0])/h);
//Initialisation avec la méthode d'Euler explicite
f(V0,F0);
euler(n,h,V0,V1,F0);
for(i=0;i<n;i++)
printf("%lf ",V1[i]);
printf("\n");
//Méthode d'Adams-Bashforth
f(V1,F1);
for(i=2;i<(nmax+1);i++)
{
adamsBashforth(n,h,V1,V,F0,F1);
for(j=0;j<n;j++)
{
printf("%lf ",V[j]);
V0[j]=V1[j];
V1[j]=V[j];
f(V0,F0);
f(V1,F1);
}
printf("\n");
}
}
int main(){
scanf("%d",&cn);
for (int i = 0; i < cn; ++i)
{
scanf("%d",&n);
printf("%d\n", euler(n));
}
return 0;
}
int main (void){
a_alt = 1;
n = 1;
double erg = euler(e);
printf ("Ergebnis: %f\n", erg);
return 0;
}
void PositionCommand::getGoal(const geometry_msgs::PoseStamped::ConstPtr & goal)
{
ROS_INFO("GOT NEW GOAL");
Eigen::Matrix<double,3,1> euler=Eigen::Quaterniond(goal->pose.orientation.w,
goal->pose.orientation.x,
goal->pose.orientation.y,
goal->pose.orientation.z).matrix().eulerAngles(2, 1, 0);
double yaw = euler(0,0);
double pitch = euler(1,0);
double roll = euler(2,0);
goal_pose_ << goal->pose.position.x,
goal->pose.position.y,
goal->pose.position.z,
roll,
pitch,
yaw;
goal_=true;
}
