void write_to_file(VecT vec, std::string filename)
{
std::ofstream fout;
fout.open(filename.c_str());
for (size_t i = 0; i < vec.size(); i++) {
// fout << i << "\t" << std::setprecision(10) << vec[i] << std::endl;
fout << std::setprecision(10) << vec[i] << std::endl;
}
fout.close();
std::cout << "Wrote " << filename << std::endl;
}
void pointFromFrameJacEuler(VecT const& t, VecPt const& pt, MatJT& JT, MatJPt& JPt) {
JFR_PRECOND(t.size() == 6, "t3d::pointFromFrameJacEuler: t3d is [x,y,z,roll,pitch,yaw]");
JFR_PRECOND(pt.size() == 3, "t3d::pointFromFrameJacEuler: pt is [x,y,z]");
JFR_PRECOND(JT.size1() == 3 && JT.size2() == 6,
"t3d::pointFromFrameJacEuler: invalid size");
JFR_PRECOND(JPt.size1() == 3 && JPt.size2() == 3,
"t3d::pointFromFrameJacEuler: invalid size");
// double const& x = t(0);
// double const& y = t(1);
// double const& z = t(2);
double const& yaw = t(3);
double const& pitch = t(4);
double const& roll = t(5);
double const& pt_x = pt(0);
double const& pt_y = pt(1);
double const& pt_z = pt(2);
/* begin Maple */
double t1 = sin(yaw);
double t2 = cos(pitch);
double t3 = t1 * t2;
double t6 = sin(pitch);
double t7 = t1 * t6;
double t8 = sin(roll);
double t10 = cos(yaw);
double t11 = cos(roll);
double t14 = -t7 * t8 - 0.10e1 * t10 * t11;
double t19 = -t7 * t11 + 0.10e1 * t10 * t8;
double t22 = t10 * t6;
double t25 = t10 * t2;
double t26 = t8 * pt_y;
double t28 = t11 * pt_z;
double t34 = t22 * t11 + 0.10e1 * t1 * t8;
double t39 = -t22 * t8 + 0.10e1 * t1 * t11;
double t66 = t2 * t11;
double t69 = t2 * t8;
JT(0,0) = 0.10e1;
JT(0,1) = 0.0e0;
JT(0,2) = 0.0e0;
JT(0,3) = -0.10e1 * t3 * pt_x + t14 * pt_y + t19 * pt_z;
JT(0,4) = -0.10e1 * t22 * pt_x + t25 * t26 + t25 * t28;
JT(0,5) = t34 * pt_y + t39 * pt_z;
JT(1,0) = 0.0e0;
JT(1,1) = 0.10e1;
JT(1,2) = 0.0e0;
JT(1,3) = 0.10e1 * t25 * pt_x - t39 * pt_y + t34 * pt_z;
JT(1,4) = -0.10e1 * t7 * pt_x + t3 * t26 + t3 * t28;
JT(1,5) = -t19 * pt_y + t14 * pt_z;
JT(2,0) = 0.0e0;
JT(2,1) = 0.0e0;
JT(2,2) = 0.10e1;
JT(2,3) = 0.0e0;
JT(2,4) = -0.100e1 * t2 * pt_x - 0.10e1 * t6 * t8 * pt_y - 0.10e1 * t6 * t11 * pt_z;
JT(2,5) = 0.10e1 * t66 * pt_y - 0.10e1 * t69 * pt_z;
JPt(0,0) = 0.10e1 * t25;
JPt(0,1) = -t39;
JPt(0,2) = t34;
JPt(1,0) = 0.10e1 * t3;
JPt(1,1) = -t14;
JPt(1,2) = -t19;
JPt(2,0) = -0.100e1 * t6;
JPt(2,1) = 0.10e1 * t69;
JPt(2,2) = 0.10e1 * t66;
/* end Maple */
}
void pointToFrameJacEuler(VecT const& t, VecPt const& pt, MatJT& JT, MatJPt& JPt) {
JFR_PRECOND(t.size() == 6, "t3d::pointToFrameJacEuler: t3d is [x,y,z,roll,pitch,yaw]");
JFR_PRECOND(pt.size() == 3, "t3d::pointToFrameJacEuler: pt is [x,y,z]");
JFR_PRECOND(JT.size1() == 3 && JT.size2() == 6,
"t3d::pointToFrameJacEuler: invalid size");
JFR_PRECOND(JPt.size1() == 3 && JPt.size2() == 3,
"t3d::pointToFrameJacEuler: invalid size");
double const& x = t(0);
double const& y = t(1);
double const& z = t(2);
double const& yaw = t(3);
double const& pitch = t(4);
double const& roll = t(5);
double const& pt_x = pt(0);
double const& pt_y = pt(1);
double const& pt_z = pt(2);
/* begin Maple */
double t1 = cos(yaw);
double t2 = cos(pitch);
double t3 = t1 * t2;
double t5 = sin(yaw);
double t6 = t5 * t2;
double t8 = sin(pitch);
double t19 = t1 * t8;
double t22 = t5 * t8;
double t37 = sin(roll);
double t38 = t19 * t37;
double t40 = cos(roll);
double t41 = t5 * t40;
double t44 = t22 * t37;
double t46 = t1 * t40;
double t49 = t2 * t37;
double t52 = -t44 - 0.10e1 * t46;
double t55 = t38 - 0.10e1 * t41;
double t66 = t8 * t37;
double t78 = t19 * t40;
double t79 = t5 * t37;
double t81 = t78 + 0.10e1 * t79;
double t83 = t22 * t40;
double t84 = t1 * t37;
double t86 = t83 - 0.10e1 * t84;
double t88 = t2 * t40;
double t117 = t8 * t40;
JT(0,0) = -0.100e1 * t3;
JT(0,1) = -0.100e1 * t6;
JT(0,2) = 0.1000e1 * t8;
JT(0,3) = -0.10e1 * t6 * pt_x + 0.10e1 * t3 * pt_y + 0.100e1 * t6 * x - 0.100e1 * t3 * y;
JT(0,4) = -0.10e1 * t19 * pt_x - 0.10e1 * t22 * pt_y - 0.100e1 * t2 * pt_z + 0.100e1 * t19 * x + 0.100e1 * t22 * y + 0.1000e1 * t2 * z;
JT(0,5) = 0.0e0;
JT(1,0) = -0.10e1 * t38 + 0.100e1 * t41;
JT(1,1) = -0.10e1 * t44 - 0.100e1 * t46;
JT(1,2) = -0.100e1 * t49;
JT(1,3) = t52 * pt_x + t55 * pt_y - 0.10e1 * t52 * x - 0.10e1 * t55 * y;
JT(1,4) = t3 * t37 * pt_x + t6 * t37 * pt_y - 0.10e1 * t66 * pt_z - 0.10e1 * t3 * t37 * x - 0.10e1 * t6 * t37 * y + 0.100e1 * t66 * z;
JT(1,5) = t81 * pt_x + t86 * pt_y + 0.10e1 * t88 * pt_z - 0.10e1 * t81 * x - 0.10e1 * t86 * y - 0.100e1 * t88 * z;
JT(2,0) = -0.10e1 * t78 - 0.100e1 * t79;
JT(2,1) = -0.10e1 * t83 + 0.100e1 * t84;
JT(2,2) = -0.100e1 * t88;
JT(2,3) = -t86 * pt_x + t81 * pt_y + 0.10e1 * t86 * x - 0.10e1 * t81 * y;
JT(2,4) = t3 * t40 * pt_x + t6 * t40 * pt_y - 0.10e1 * t117 * pt_z - 0.10e1 * t3 * t40 * x - 0.10e1 * t6 * t40 * y + 0.100e1 * t117 * z;
JT(2,5) = -t55 * pt_x + t52 * pt_y - 0.10e1 * t49 * pt_z + 0.10e1 * t55 * x - 0.10e1 * t52 * y + 0.100e1 * t49 * z;
JPt(0,0) = 0.10e1 * t3;
JPt(0,1) = 0.10e1 * t6;
JPt(0,2) = -0.100e1 * t8;
JPt(1,0) = t55;
JPt(1,1) = -t52;
JPt(1,2) = 0.10e1 * t49;
JPt(2,0) = t81;
JPt(2,1) = t86;
JPt(2,2) = 0.10e1 * t88;
/* end MAple */
}
