void EntityAdapter::setOrientation( Config::Real xAngle, Config::Real yAngle, Config::Real zAngle )
{
Ogre::Quaternion qx(Ogre::Radian(xAngle), Ogre::Vector3(1.0,0.0,0.0)) ;
Ogre::Quaternion qy(Ogre::Radian(yAngle), Ogre::Vector3(0.0,1.0,0.0)) ;
Ogre::Quaternion qz(Ogre::Radian(zAngle), Ogre::Vector3(0.0,0.0,1.0)) ;
m_entity->setOrientation(qx*qy*qz) ;
}
void ZipRW::SetFileText(QString ZipFile, QString File, QString Text)
{
QFile(ZipFile).rename(ZipFile + ".old");
QuaZip qz2(ZipFile);
qz2.open(QuaZip::mdCreate);
QuaZipFile qzf(&qz2);
qzf.open(QFile::WriteOnly, QuaZipNewInfo(File));
qzf.write(Text.toLatin1());
qzf.close();
QuaZip qz(ZipFile + ".old");
qz.open(QuaZip::mdUnzip);
QList<QuaZipFileInfo> lst = qz.getFileInfoList();
foreach (QuaZipFileInfo itm, lst) {
if (itm.name != File) {
qz.setCurrentFile(itm.name, QuaZip::csInsensitive);
QuaZipFile qf(&qz);
qf.open(QFile::ReadOnly);
QByteArray ba = qf.readAll();
qf.close();
QuaZipFile qzf2(&qz2);
qzf2.open(QFile::WriteOnly, QuaZipNewInfo(itm.name));
qzf2.write(ba);
qzf2.close();
}
}
qz.close();
qz2.close();
QFile(ZipFile + ".old").remove();
}
void ZipRW::CpFileText(QString source, QString dest, QString File, QString Text)
{
QuaZip qz2(dest);
qz2.open(QuaZip::mdCreate);
QuaZipFile qzf(&qz2);
qzf.open(QFile::WriteOnly, QuaZipNewInfo(File));
qzf.write(Text.toUtf8().data());
qzf.close();
QuaZip qz(source);
qz.open(QuaZip::mdUnzip);
QList<QuaZipFileInfo> lst = qz.getFileInfoList();
foreach (QuaZipFileInfo itm, lst) {
if (itm.name != File) {
qz.setCurrentFile(itm.name, QuaZip::csInsensitive);
QuaZipFile qf(&qz);
qf.open(QFile::ReadOnly);
QByteArray ba = qf.readAll();
qf.close();
QuaZipFile qzf2(&qz2);
qzf2.open(QFile::WriteOnly, QuaZipNewInfo(itm.name));
qzf2.write(ba);
qzf2.close();
}
}
qz.close();
qz2.close();
}
inline PxQuat EulerAngleToQuat(const PxVec3 &rot)
{
PxQuat qx(degToRad(rot.x), PxVec3(1.0f, 0.0f, 0.0f));
PxQuat qy(degToRad(rot.y), PxVec3(0.0f, 1.0f, 0.0f));
PxQuat qz(degToRad(rot.z), PxVec3(0.0f, 0.0f, 1.0f));
return qz * qy * qx;
}
void
EditorIkSolver::_checkPivotXZ(ArRef<Joint> joint, Vector3d /* requestedTranslation */, Quaterniond requestedRotation) {
double prx, pry, prz;
joint->getDeltaRotation().getEulerAngles(prx, pry, prz);
requestedRotation = joint->getDeltaRotation() * requestedRotation;
double rx, ry, rz;
requestedRotation.getEulerAngles(rx, ry, rz);
double* max[2];
double* min[2];
ar_down_cast<JointConstraint2DOF>(joint->getConstraint())->getLimitValues(min, max);
double xmin = *min[0], xmax = *max[0], zmin = *min[1], zmax = *max[1];
rx = _clampWithPreviousValue(xmin, xmax, prx, rx);
rz = _clampWithPreviousValue(zmin, zmax, prz, rz);
Quaterniond qx(Vector3d(1.0, 0.0, 0.0), rx);
Quaterniond qz(Vector3d(0.0, 0.0, 1.0), rz);
joint->setDeltaRotation(qx * qz);
}
Transform Road::getRoadTransform(const double &s, const double &t)
{
Vector3D xyPoint = ((--xyMap.upper_bound(s))->second)->getPoint(s);
Vector2D zPoint = ((--zMap.upper_bound(s))->second)->getPoint(s);
//std::cerr << "Center: x: " << center.x() << ", y: " << center.y() << ", z: " << center.z() << std::endl;
double alpha = ((--lateralProfileMap.upper_bound(s))->second)->getAngle(s, t);
//std::cerr << "s: " << s << "Alpha: " << alpha << ", Beta: " << beta << ", Gamma: " << gamma << std::endl;
//double sinalpha = sin(alpha); double cosalpha=cos(alpha);
//double sinbeta = sin(beta); double cosbeta=cos(beta);
//double singamma = sin(gamma); double cosgamma=cos(gamma);
Quaternion qz(xyPoint[2], Vector3D(0, 0, 1));
Quaternion qya(zPoint[1], Vector3D(0, 1, 0));
Quaternion qxaa(alpha, Vector3D(1, 0, 0));
Quaternion q = qz * qya * qxaa;
/*
return RoadTransform( xyPoint.x() + (sinalpha*sinbeta*cosgamma-cosalpha*singamma)*t,
xyPoint.y() + (sinalpha*sinbeta*singamma+cosalpha*cosgamma)*t,
zPoint.z() + (sinalpha*cosbeta)*t,
alpha, beta, gamma);
*/
return Transform(Vector3D(xyPoint.x(), xyPoint.y(), zPoint[0]) + (q * Vector3D(0, t, 0) * q.T()).getVector(), q);
}
void interpolateSurface(const Mesh & mesh, Mesh & qmesh, bool verbose){
RVector z(mesh.nodeCount());
for (uint i = 0; i < z.size(); i ++) z[i] = mesh.node(i).pos()[2];
RVector qz(qmesh.nodeCount());
interpolate(mesh, z, qmesh, qz, verbose);
for (uint i = 0; i < qz.size(); i ++) qmesh.node(i).pos()[2] = qz[i];
}
void SimObjBase::setQ(const dReal *q)
{
int i = 0;
qw(q[i]); i++;
qx(q[i]); i++;
qy(q[i]); i++;
qz(q[i]); i++;
}
void euler2quaternion(Eigen::Quaternion<scalar> &res,
const scalar& phi, const scalar& theta, const scalar& psi) {
Eigen::Quaternion<scalar>
qx(std::cos(phi/2), std::sin(phi/2), 0, 0),
qy(std::cos(theta/2), 0, std::sin(theta/2), 0),
qz(std::cos(psi/2), 0, 0, std::sin(psi/2));
res = qz * qy * qx;
}
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double degAngle)
{
Sgv::Quaternion q(degAngle*180/M_PI, ax, ay, az);
qw(q.qw());
qx(q.qx());
qy(q.qy());
qz(q.qz());
}
void Quat :: setRotXYZ( const float a, const float b, const float c )
{
Quat qx(ROT_X, a);
Quat qy(ROT_Y, b);
Quat qz(ROT_Z, c);
*this = qx * qy * qz;
} /* RotXIdent */
QString ZipRW::FileText(QString ZipFile, QString File)
{
QuaZip qz(ZipFile);
qz.open(QuaZip::mdUnzip);
qz.setCurrentFile(File, QuaZip::csInsensitive);
QuaZipFile qf(&qz);
qf.open(QFile::ReadOnly);
QString txt = qf.readAll();
qf.close();
qz.close();
return txt;
}
/***********************************************************
constructor from 3 angles
***********************************************************/
LbaQuaternion::LbaQuaternion(float anglex, float angley, float anglez)
{
LbaQuaternion qx(anglex, LbaVec3(1, 0, 0));
LbaQuaternion qy(angley, LbaVec3(0, 1, 0));
LbaQuaternion qz(anglez, LbaVec3(0, 0, 1));
LbaQuaternion res = (qy * qx * qz);
X = res.X;
Y = res.Y;
Z = res.Z;
W = res.W;
}
/*!
* @brief It rotates for the specification of the relative angle.
* @param[in] x-axis rotation weather(i of quaternion complex part)
* @param[in] y-axis rotation weather(j of quaternion complex part)
* @param[in] z-axis rotation weather(k of quaternion complex part)
* @param[in] flag for ansolute / relational (1.0=absolute, else=relational)
*/
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle, double direct)
{
// The angle is used now at the relative angle specification.
if (dynamics()) { return; }
Rotation r;
if (direct != 1.0) r.setQuaternion(qw(), qx(), qy(), qz());
// alculate relational angle
r.setAxisAndAngle(ax, ay, az, angle, direct);
const dReal *q = r.q();
setQ(q);
}
//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button){
if (button==0){
ofQuaternion qy((x-ofGetPreviousMouseX()), ofVec3f(0,1,0));
ofQuaternion qx((y-ofGetPreviousMouseY()), ofVec3f(1,0,0));
qrot *= qx*qy;
}
if (button==2){
ofQuaternion qz((x-ofGetPreviousMouseX()), ofVec3f(0,0,1));
qrot *= qz;
scale += (float)(ofGetPreviousMouseY()-y)/(ofGetWindowHeight()/2);
if (scale<0) scale = 0.0001;
}
}
Z H2(e2){
I z;
I isWritableFile();
CX cx=Cx;
ND2;
z=*a->p;
if (It==a->t||qz(a))
{
if(dbg_txeq)xeqtrc((C *)w->p,3);
if(w->c||isWritableFile((I)w))z=pexm((I)w->p,APL);
else
{
C *buf=(C *)mab(w->n+1);
memmove(buf,w->p,w->n+1);
z=pexm((I)buf,APL);
mf((I *)buf);
}
if(dbg_txeq)xeqtrc((C *)w->p,2);
R z;
}
else R QS(z)?(Cx=cxi(XS(z)),z=e1(w),Cx=cx,z):(q=6,0);
}
int main(int argc, char** argv)
{
char port[] = "27015";
char ip[] = "10.0.0.67";
int numbytes;
char buf[MAXDATASIZE];
// Get input parameters
for(int a=0; a < argc; a++)
{
if( strcmp( argv[a], "--ip" ) == 0 )
{
strcpy(ip, argv[a+1]);
}
if( strcmp( argv[a], "--port" ) == 0 )
{
strcpy(port, argv[a+1]);
}
}
TCP myClient = TCP(port, ip);
ros::init(argc, argv, "joints");
ros::NodeHandle n;
std::string receivedString;
std::string attribute;
std::stringstream ss;
std::istringstream iss;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate loop_rate(30);
if( myClient.Connect() == 0 )
{
// Receive data
numbytes = recv(myClient.s,buf,MAXDATASIZE-1,0);
while (numbytes != 0 && ros::ok())
{
numbytes = recv(myClient.s,buf,MAXDATASIZE-1,0);
buf[numbytes]='\0';
receivedString.assign(buf);
// debug
// std::cout << "Ahoy! Received " << numbytes << " bytes. " << receivedString << std::endl;
std::istringstream iss(receivedString);
std::vector<double> x (25, 0.0);
std::vector<double> y (25, 0.0);
std::vector<double> z (25, 0.0);
std::vector<std::vector<double> > X (6, x);
std::vector<std::vector<double> > Y (6, y);
std::vector<std::vector<double> > Z (6, z);
std::vector<double> qx (25, 0.0);
std::vector<double> qy (25, 0.0);
std::vector<double> qz (25, 0.0);
std::vector<double> qw (25, 1.0);
std::vector<std::vector<double> > QX (6, qx);
std::vector<std::vector<double> > QY (6, qy);
std::vector<std::vector<double> > QZ (6, qz);
std::vector<std::vector<double> > QW (6, qw);
int j;
int count = 6;
int id;
std::vector<int> foundIdx;
std::string frame;
std::string joint;
/* code */
while(!iss.eof())
{
// joint = "joint_";
iss >> attribute;
frame = "person_";
if( strcmp(attribute.c_str(), "count:") == 0 )
{
iss >> count;
// ROS_INFO("count %d",count);
}
else if( strcmp(attribute.c_str(), "id:") == 0 )
{
iss >> id;
foundIdx.push_back(id);
frame.append(boost::lexical_cast<std::string>(id));
// ROS_INFO("id %s",frame);
// std::cout << id << " " << frame << " ";
}
int main(int argc, char **argv)
{
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution<> dis(-1, 1);
// AX = ZB
Eigen::Matrix4f Z = Eigen::Matrix4f::Identity();
Eigen::Vector4f qz_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
Eigen::Quaternionf qz( qz_use[0], qz_use[1], qz_use[2], qz_use[3] );
qz.normalize();
Eigen::Vector3f tz = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );
Z.block<3,3>(0,0) = qz.toRotationMatrix();
Z.block<3,1>(0,3) = tz;
//std::cout << "Z: " << std::endl << Z << std::endl;
Eigen::Matrix4f X = Eigen::Matrix4f::Identity();
Eigen::Vector4f qx_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
Eigen::Quaternionf qx( qx_use[0], qx_use[1], qx_use[2], qx_use[3] );
qx.normalize();
Eigen::Vector3f tx = Eigen::Vector3f(dis(gen),dis(gen), dis(gen));
X.block<3,3>(0,0) = qx.toRotationMatrix();
X.block<3,1>(0,3) = tx;
//std::cout << "X: " << std::endl << X << std::endl;
int NPoses = 6;
std::vector<Matrix4f> A, B;
Eigen::Matrix4f a = Eigen::Matrix4f::Identity();
Eigen::Vector4f q_use = Eigen::Vector4f::Random();
Eigen::Quaternionf q( q_use[0], q_use[1], q_use[2], q_use[3]);
q.normalize();
Eigen::Vector3f t = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );
a.block<3,3>(0,0) = q.toRotationMatrix();
a.block<3,1>(0,3) = t;
for (int i = 0; i < NPoses; i++)
{
// test data
Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
b = Z.inverse()*a*X;
Eigen::Matrix4f d = Eigen::Matrix4f::Identity();
Eigen::Vector3f t = 0.001*Eigen::Vector3f( dis(gen),dis(gen), dis(gen) );
float roll = dis(gen)*M_PI/512;
float pitch = dis(gen)*M_PI/512;
float yaw = dis(gen)*M_PI/512;
Eigen::Matrix3f R;
R = AngleAxisf(roll, Vector3f::UnitX())
*AngleAxisf(pitch, Vector3f::UnitY())
*AngleAxisf(yaw, Vector3f::UnitZ());
d.block<3,3>(0,0) = R;
d.block<3,1>(0,3) = t;
a = d*a;
A.push_back(a);
B.push_back(b);
}
// first, create an object of your class
calibration::Calibration mycalib;
mycalib.setInput(A, B);
mycalib.computeNonLinOpt();
Eigen::Matrix4f Tx, Tz;
Tx = Eigen::Matrix4f::Identity();
Tz = Eigen::Matrix4f::Identity();
mycalib.getOutput(Tz, Tx);
// print out the result
std::cout << "Input Z: " << std::endl << Z << std::endl;
std::cout << "Estimated Z:" << std::endl << Tz << std::endl;
std::cout << "Error over Z estimation: " << std::endl << Z-Tz << std::endl << std::endl;
std::cout << "Input X: " << std::endl << X << std::endl;
std::cout << "Estimated X:" << std::endl << Tx << std::endl;
std::cout << "Error over X estimation: " << std::endl << X-Tx << std::endl;
return 0;
}
void ql(void)
{
char buf[10];
while(1)
{
sen.pro = 2;
system("clear");
printf("-----------群聊----------\n\n");
printf("1--------加入群组--------\n");
printf("2-----查看群聊天记录-----\n");
// printf("-----加入私密群----\n");
// printf("---------创建私密群----\n")
printf("3-------返回上一层-------\n");
printf("4----------注销----------\n");
printf("5----------退出----------\n");
printf("-------------------------\n\n");
printf("请输入你的选择:");
memset(buf,0,10);
fgets(buf,10,stdin);
if(strlen(buf) > 2)
{
continue;
}
if(buf[0] == '1')
{
qz();
}
else if(buf[0] == '2')
{
system("clear");
printf("--------群聊天记录--------\n\n");
FILE *fp;
struct data_info buff;
fp = fopen(".MTqunzu","r");
while(fread(&buff,len_data,1,fp) > 0)
{
printf("%s >>> %-30s%s\n",buff.I_name,buff.message,buff.time);
}
printf("\n\n请按任意键返回...\n");
getchar();
}
else if(buf[0] == '3')
{
break;
}
else if(buf[0] == '4')
{
sen.pro = 6;
strcpy(sen.message,"ZX");
send(fd,&sen,len_data,0);
MAIN();
}
else if(buf[0] == '5')
{
sen.pro = 6;
strcpy(sen.message,"MT");
send(fd,&sen,len_data,0);
exit(0);
}
}
}
