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 */ }