geometry_msgs::Pose JacoPose::constructPoseMsg()
{
geometry_msgs::Pose pose;
tf::Quaternion position_quaternion;
// TODO: QUICK FIX, bake this as a quaternion:
tf::Matrix3x3 mx( 1, 0, 0,
0, cos(ThetaX), -sin(ThetaX),
0, sin(ThetaX), cos(ThetaX));
tf::Matrix3x3 my( cos(ThetaY), 0, sin(ThetaY),
0, 1, 0,
-sin(ThetaY), 0, cos(ThetaY));
tf::Matrix3x3 mz( cos(ThetaZ), -sin(ThetaZ), 0,
sin(ThetaZ), cos(ThetaZ), 0,
0, 0, 1);
tf::Matrix3x3 mg = mx * my * mz;
mg.getRotation(position_quaternion);
// NOTE: This doesn't work, as angles reported by the API are not fixed.
// position_quaternion.setRPY(ThetaX, ThetaY, ThetaZ);
tf::quaternionTFToMsg(position_quaternion, pose.orientation);
pose.position.x = X;
pose.position.y = Y;
pose.position.z = Z;
return pose;
}
//-----------------------------------------------------------------------------
void DatPanel::squize()
{
QString mx("1"), my("1"), mz("1");
if(sizesDialog(tr("UDAV - Squeeze data"), tr("Enter step of saved points. For example, '1' save all, '2' save each 2nd point, '3' save each 3d and so on."), tr("X-direction"), tr("Y-direction"), tr("Z-direction"), mx, my, mz))
{
mglData *d = dynamic_cast<mglData *>(var);
if(d) d->Squeeze(mx.toInt(), my.toInt(), mz.toInt());
refresh(); updateDataItems();
}
}
//-----------------------------------------------------------------------------
void DatPanel::create()
{
QString mx, my("1"), mz("1");
if(sizesDialog(tr("UDAV - Clear data"), tr("Enter new data sizes"), tr("X-size"), tr("Y-size"), tr("Z-size"), mx, my, mz))
{
mglData *d = dynamic_cast<mglData *>(var);
if(d) d->Create(mx.toInt(), my.toInt(), mz.toInt());
mglDataC *c = dynamic_cast<mglDataC *>(var);
if(c) c->Create(mx.toInt(), my.toInt(), mz.toInt());
refresh(); updateDataItems();
}
}
QImage *AxisRotate::setZ(int z) {
double rz = static_cast<double>(z)*M_PI/180.0;
mz(0,0) = mz(1,1) = cos(rz);
mz(0,1) = -sin(rz);
mz(1,0) = -mz(0,1);
mz(2,2) = mz(3,3) = 1;
return translate();//const_cast<QImage>(image);
}
AxisRotate::AxisRotate(const QImage &im, const int newW, const int newH): image(im)
{
mx.fill(0.0);
my.fill(0.0);
mz.fill(0.0);
per.fill(0.0);
minusTran.fill(0.0);
tran.fill(0.0);
minusTran(0,0) = minusTran(1,1) = minusTran(2,2) = minusTran(3,3) =
tran(0,0) = tran(1,1) = tran(2,2) = tran(3,3) = 1.0;
mx(0,0)=mx(1,1)=mx(2,2)=mx(3,3)=1.0;
my(0,0)=my(1,1)=my(2,2)=my(3,3)=1.0;
mz(0,0)=mz(1,1)=mz(2,2)=mz(3,3)=1.0;
move(0,0)=move(1,1)=move(2,2)=move(3,3)=1.0;
move(0,3)=newW/2-im.width()/2;
move(1,3)=newH/2-im.height()/2;
minusTran(0,3) = im.width()/2.0;
minusTran(1,3) = im.height()/2.0;
//minusTran(2,3) = 0;
nw = newW;
nh = newH;
tran(0,3) = -im.width() /2.0;
tran(1,3) = -im.height()/2.0;
//tran(2,3) = 0;
per(0,0) = per(1,1) = per(3,3) = 1.0;
per(3,2) = 1.0/-200.0;
rotatedImage = NULL;
}
Vector Camera::rotateAxisAngle( Vector v, Vector n, float a)
{
float co = cos(a);
float si = sin(a);
Vector o;
Vector mx( n.X*n.X*(1.0f-co)+co, n.X*n.Y*(1.0f-co)-n.Z*si,n.X*n.Z*(1.0f-co)+n.Y*si );
Vector my( n.X*n.Y*(1.0f-co)+n.Z*si, n.Y*n.Y*(1.0f-co)+co, n.Y*n.Z*(1.0f-co)-n.X*si );
Vector mz( n.X*n.Z*(1.0f-co)-n.Y*si, n.Z*n.Y*(1.0f-co)+n.X*si, n.Z*n.Z*(1.0f-co)+co);
o.X = mx.dot(v);
o.Y = my.dot(v);
o.Z = mz.dot(v);
return o;
}
void InitBase(
__in const void* base
)
{
if (!IsInitialized())
{
CMdl mapped_img(base, 0x1000);
void* img = mapped_img.Map();
if (img)
{
CPE mz(img);
if (mz.IsValid())
m_addrToHook = reinterpret_cast<void*>(reinterpret_cast<BYTE*>(base) + mz.Entrypoint());
}
}
}
double ChLcpIterativePCG::Solve(
ChLcpSystemDescriptor& sysd ///< system description with constraints and variables
)
{
std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
std::vector<ChLcpVariables*>& mvariables = sysd.GetVariablesList();
tot_iterations = 0;
double maxviolation = 0.;
// Update auxiliary data in all constraints before starting,
// that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]'
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
mconstraints[ic]->Update_auxiliary();
// Allocate auxiliary vectors;
int nc = sysd.CountActiveConstraints();
if (verbose) GetLog() <<"\n-----Projected CG, solving nc=" << nc << "unknowns \n";
ChMatrixDynamic<> ml(nc,1);
ChMatrixDynamic<> mb(nc,1);
ChMatrixDynamic<> mu(nc,1);
ChMatrixDynamic<> mp(nc,1);
ChMatrixDynamic<> mw(nc,1);
ChMatrixDynamic<> mz(nc,1);
ChMatrixDynamic<> mNp(nc,1);
ChMatrixDynamic<> mtmp(nc,1);
double graddiff= 0.00001; // explorative search step for gradient
// ***TO DO*** move the following thirty lines in a short function ChLcpSystemDescriptor::ShurBvectorCompute() ?
// Compute the b_shur vector in the Shur complement equation N*l = b_shur
// with
// N_shur = D'* (M^-1) * D
// b_shur = - c + D'*(M^-1)*k = b_i + D'*(M^-1)*k
// but flipping the sign of lambdas, b_shur = - b_i - D'*(M^-1)*k
// Do this in three steps:
// Put (M^-1)*k in q sparse vector of each variable..
for (unsigned int iv = 0; iv< mvariables.size(); iv++)
if (mvariables[iv]->IsActive())
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb()); // q = [M]'*fb
// ...and now do b_shur = - D' * q ..
int s_i = 0;
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
if (mconstraints[ic]->IsActive())
{
mb(s_i, 0) = - mconstraints[ic]->Compute_Cq_q();
++s_i;
}
// ..and finally do b_shur = b_shur - c
sysd.BuildBiVector(mtmp); // b_i = -c = phi/h
mb.MatrDec(mtmp);
// Optimization: backup the q sparse data computed above,
// because (M^-1)*k will be needed at the end when computing primals.
ChMatrixDynamic<> mq;
sysd.FromVariablesToVector(mq, true);
// Initialize lambdas
if (warm_start)
sysd.FromConstraintsToVector(ml);
else
ml.FillElem(0);
// Initial projection of ml ***TO DO***?
// ...
std::vector<bool> en_l(nc);
// Initially all constraints are enabled
for (int ie= 0; ie < nc; ie++)
en_l[ie] = true;
// u = -N*l+b
sysd.ShurComplementProduct(mu, &ml, &en_l); // 1) u = N*l ... #### MATR.MULTIPLICATION!!!###
mu.MatrNeg(); // 2) u =-N*l
mu.MatrInc(mb); // 3) u =-N*l+b
mp = mu;
//
// THE LOOP
//
std::vector<double> f_hist;
for (int iter = 0; iter < max_iterations; iter++)
{
// alpha = u'*p / p'*N*p
sysd.ShurComplementProduct(mNp, &mp, &en_l);// 1) Np = N*p ... #### MATR.MULTIPLICATION!!!###
double pNp = mp.MatrDot(&mp,&mNp); // 2) pNp = p'*N*p
double up = mu.MatrDot(&mu,&mp); // 3) up = u'*p
double alpha = up/pNp; // 4) alpha = u'*p / p'*N*p
if (fabs(pNp)<10e-10) GetLog() << "Rayleygh quotient pNp breakdown \n";
// l = l + alpha * p;
mtmp.CopyFromMatrix(mp);
mtmp.MatrScale(alpha);
ml.MatrInc(mtmp);
double maxdeltalambda = mtmp.NormInf();
// l = Proj(l)
sysd.ConstraintsProject(ml); // 5) l = P(l)
// u = -N*l+b
sysd.ShurComplementProduct(mu, &ml, 0); // 6) u = N*l ... #### MATR.MULTIPLICATION!!!###
mu.MatrNeg(); // 7) u =-N*l
mu.MatrInc(mb); // 8) u =-N*l+b
// w = (Proj(l+lambda*u) -l) /lambda;
mw.CopyFromMatrix(mu);
mw.MatrScale(graddiff);
mw.MatrInc(ml);
sysd.ConstraintsProject(mw); // 9) w = P(l+lambda*u) ...
mw.MatrDec(ml);
mw.MatrScale(1.0/graddiff); //10) w = (P(l+lambda*u)-l)/lambda ...
// z = (Proj(l+lambda*p) -l) /lambda;
mz.CopyFromMatrix(mp);
mz.MatrScale(graddiff);
mz.MatrInc(ml);
sysd.ConstraintsProject(mz); //11) z = P(l+lambda*u) ...
mz.MatrDec(ml);
mz.MatrScale(1.0/graddiff); //12) z = (P(l+lambda*u)-l)/lambda ...
// beta = w'*Np / pNp;
double wNp = mw.MatrDot(&mw, &mNp);
double beta = wNp / pNp;
// p = w + beta * z;
mp.CopyFromMatrix(mz);
mp.MatrScale(beta);
mp.MatrInc(mw);
// METRICS - convergence, plots, etc
double maxd = mu.NormInf(); // ***TO DO*** should be max violation, but just for test...
// For recording into correction/residuals/violation history, if debugging
if (this->record_violation_history)
AtIterationEnd(maxd, maxdeltalambda, iter);
tot_iterations++;
}
// Resulting DUAL variables:
// store ml temporary vector into ChLcpConstraint 'l_i' multipliers
sysd.FromVectorToConstraints(ml);
// Resulting PRIMAL variables:
// compute the primal variables as v = (M^-1)(k + D*l)
// v = (M^-1)*k ... (by rewinding to the backup vector computed ad the beginning)
sysd.FromVectorToVariables(mq);
// ... + (M^-1)*D*l (this increment and also stores 'qb' in the ChLcpVariable items)
for (unsigned int ic = 0; ic < mconstraints.size(); ic++)
{
if (mconstraints[ic]->IsActive())
mconstraints[ic]->Increment_q( mconstraints[ic]->Get_l_i() );
}
if (verbose) GetLog() <<"-----\n";
return maxviolation;
}
bool csDIntersect3::Planes (
const csDPlane &p1,
const csDPlane &p2,
const csDPlane &p3,
csDVector3 &isect)
{
//To find the one point that is on all three planes, we need to solve
//the following equation system (we need to find the x, y and z which
//are true for all equations):
// A1*x+B1*y+C1*z+D1=0 //plane1
// A2*x+B2*y+C2*z+D2=0 //plane2
// A3*x+B3*y+C3*z+D3=0 //plane3
//This can be solved according to Cramers rule by looking at the
//determinants of the equation system.
csDMatrix3 mdet (
p1.A (),
p1.B (),
p1.C (),
p2.A (),
p2.B (),
p2.C (),
p3.A (),
p3.B (),
p3.C ());
double det = mdet.Determinant ();
if (det == 0) return false; //some planes are parallel.
csDMatrix3 mx (
-p1.D (),
p1.B (),
p1.C (),
-p2.D (),
p2.B (),
p2.C (),
-p3.D (),
p3.B (),
p3.C ());
double xdet = mx.Determinant ();
csDMatrix3 my (
p1.A (),
-p1.D (),
p1.C (),
p2.A (),
-p2.D (),
p2.C (),
p3.A (),
-p3.D (),
p3.C ());
double ydet = my.Determinant ();
csDMatrix3 mz (
p1.A (),
p1.B (),
-p1.D (),
p2.A (),
p2.B (),
-p2.D (),
p3.A (),
p3.B (),
-p3.D ());
double zdet = mz.Determinant ();
isect.x = xdet / det;
isect.y = ydet / det;
isect.z = zdet / det;
return true;
}
void PDFDocumentTools::CalculateCameraPropertiesFromInventorCamera(
Vector3 inventorCameraPosition, Vector4 inventorCameraOrientation, float inventorCameraFocalDistance, float inventorCameraHeight, // inputs
Vector3& camCenterOfOrbit, Vector3& camCenterToCamera, float& camRadiusOfOrbit, float& camRollAngle, float& camFOVAngle
)
{
SbVec3f cameraRotationAxis(inventorCameraOrientation[0], inventorCameraOrientation[1], inventorCameraOrientation[2]);
SbRotation cameraOrientation(cameraRotationAxis, inventorCameraOrientation[3]);
SbMatrix cameraRotationMatrix;
cameraOrientation.getValue(cameraRotationMatrix);
//Vector3 kosXAxis(1,0,0);
//Vector3 kosYAxis(0,1,0);
Vector3 kosZAxis(0,0,1);
Vector3 camXAxis(cameraRotationMatrix[0][0], cameraRotationMatrix[0][1], cameraRotationMatrix[0][2]);
Vector3 camYAxis(cameraRotationMatrix[1][0], cameraRotationMatrix[1][1], cameraRotationMatrix[1][2]);
Vector3 camZAxis(cameraRotationMatrix[2][0], cameraRotationMatrix[2][1], cameraRotationMatrix[2][2]);
Vector3 camYAxisInverted = camYAxis * -1;
Vector3 centerToCamera = camZAxis * inventorCameraFocalDistance;
Vector3 centerOfOrbit = inventorCameraPosition - centerToCamera;
centerToCamera.normalize(); // Must be AFTER calculation of centerOfOrbit!
Vector4 mx(cameraRotationMatrix[0][0], cameraRotationMatrix[0][1], cameraRotationMatrix[0][2], cameraRotationMatrix[0][3]);
Vector4 my(cameraRotationMatrix[1][0], cameraRotationMatrix[1][1], cameraRotationMatrix[1][2], cameraRotationMatrix[1][3]);
Vector4 mz(cameraRotationMatrix[2][0], cameraRotationMatrix[2][1], cameraRotationMatrix[2][2], cameraRotationMatrix[2][3]);
Vector4 mr(cameraRotationMatrix[3][0], cameraRotationMatrix[3][1], cameraRotationMatrix[3][2], cameraRotationMatrix[3][3]);
Matrix4 camMatrix(mx, my, mz, mr);
Vector3 projectionAxis1 = camZAxis.cross(kosZAxis);
Vector3 projectionAxis2 = projectionAxis1.cross(camZAxis);
projectionAxis2.normalize();
// Calc roll angle
double rollAangleRadians = acos(camYAxisInverted.dot(projectionAxis2));
double rollAngleDegrees = (rollAangleRadians / M_PI * 180.0) + 180.0;
double angleRadBetweenProjectionAxisAndCamXAxis = acos(camXAxis.dot(projectionAxis2));
if (angleRadBetweenProjectionAxisAndCamXAxis < (M_PI/2.0))
{
rollAngleDegrees = 360.0 - rollAngleDegrees;
}
// Calc FOV angle
double fieldOfViewAngle = 45.0;
if (inventorCameraHeight != 0)
{
fieldOfViewAngle = 180.0/(M_PI/inventorCameraHeight);
}
if (fieldOfViewAngle > 180.0) // Can happen if Orthographic camera mode is selected in inventor
{
fieldOfViewAngle = 45.0;
}
// Return result values
camCenterOfOrbit = centerOfOrbit;
camCenterToCamera = centerToCamera;
camRadiusOfOrbit = inventorCameraFocalDistance;
camRollAngle = rollAngleDegrees;
camFOVAngle = fieldOfViewAngle;
}
