diff --git a/scripts/symbol_calculation/affine_body_revolute_joint.ipynb b/scripts/symbol_calculation/affine_body_revolute_joint.ipynb index ac3598a6..4bf3b0fb 100644 --- a/scripts/symbol_calculation/affine_body_revolute_joint.ipynb +++ b/scripts/symbol_calculation/affine_body_revolute_joint.ipynb @@ -6,37 +6,92 @@ "metadata": {}, "outputs": [], "source": [ - "import sympy as sp\n", "import sys\n", - "sys.path.append('../')\n", + "\n", + "sys.path.append(\"../\")\n", + "\n", + "import sympy as sp\n", "import pathlib as pl\n", "from SymEigen import *\n", "from sympy import symbols\n", "from project_dir import backend_source_dir\n", + "from affine_body_core import compute_J_point, compute_J_vec\n", "\n", "Gen = EigenFunctionGenerator()\n", "Gen.MacroBeforeFunction(\"__host__ __device__\")\n", + "Gen.DisableLatexComment()\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "ref: [Affine Body Revolute Joint](https://spirimirror.github.io/libuipc-doc/specification/constitutions/affine_body_revolute_joint/)\n", + "\n", + "![Affine Body Revolute Joint](../../docs/specification/constitutions/media/affine_body_revolute_joint_fig1.drawio.svg)\n", + "\n", + "$$\n", + "C_0 = (\\mathbf{q}_i^0 - \\mathbf{q}_j^0 ) = \\mathbf{0}\n", + "$$\n", "\n", + "$$\n", + "C_1 = (\\mathbf{q}_i^1 - \\mathbf{q}_j^1) = \\mathbf{0}\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ "kappa = Eigen.Scalar(\"kappa\")\n", - "x0_bar = Eigen.Vector(\"x0_bar\", 3)\n", - "x1_bar = Eigen.Vector(\"x1_bar\", 3)\n", - "x2_bar = Eigen.Vector(\"x2_bar\", 3)\n", - "x3_bar = Eigen.Vector(\"x3_bar\", 3)\n", - "\n", - "\n", - "def compute_J(xbar):\n", - " J = sp.Matrix.zeros(3,12)\n", - " J[0:3,0:3] = sp.eye(3)\n", - " J[0,3:6] = xbar.T\n", - " J[1,6:9] = xbar.T\n", - " J[2,9:12] = xbar.T\n", - " return J\n", - "J0 = compute_J(x0_bar)\n", - "J1 = compute_J(x1_bar)\n", - "J2 = compute_J(x2_bar)\n", - "J3 = compute_J(x3_bar)\n", - "\n", - "Cl = Gen.Closure(kappa, x0_bar, x1_bar, x2_bar, x3_bar)" + "\n", + "# Basis vectors\n", + "qi0_bar = Eigen.Vector(\"qi0_bar\", 3)\n", + "qi1_bar = Eigen.Vector(\"qi1_bar\", 3)\n", + "qj0_bar = Eigen.Vector(\"qj0_bar\", 3)\n", + "qj1_bar = Eigen.Vector(\"qj1_bar\", 3)\n", + "\n", + "# Affine Body DOF vectors\n", + "qi = Eigen.Vector(\"qi\", 12)\n", + "qj = Eigen.Vector(\"qj\", 12)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "$$\n", + "C_0 = (\\mathbf{q}_i^0 - \\mathbf{q}_j^0 ) = \\mathbf{0}\n", + "$$\n", + "\n", + "$$\n", + "C_1 = (\\mathbf{q}_i^1 - \\mathbf{q}_j^1) = \\mathbf{0}\n", + "$$\n", + "$$\n", + "\\mathbf{F}_{axis} = \\begin{bmatrix}\n", + "\\mathbf{q}_i^0 - \\mathbf{q}_j^0 \\\\\n", + "\\mathbf{q}_i^1 - \\mathbf{q}_j^1 \\\\\n", + "\\end{bmatrix}_{6 \\times 1}\n", + "$$\n", + "\n", + "Frame Affine Body Mapping:\n", + "\n", + "$$\n", + "\\mathbf{J}_{axis} = \\begin{bmatrix}\n", + "\\mathbf{J}(\\bar{\\mathbf{q}}_i^0) & -\\mathbf{J}(\\bar{\\mathbf{q}}_j^0) \\\\\n", + "\\mathbf{J}(\\bar{\\mathbf{q}}_i^1) & -\\mathbf{J}(\\bar{\\mathbf{q}}_j^1) \\\\\n", + "\\end{bmatrix}_{6 \\times 24}\n", + "$$\n", + "\n", + "$$\n", + "\\mathbf{F}_{axis} = \\mathbf{J}_{axis} \\cdot\n", + "\\begin{bmatrix}\n", + "\\mathbf{q}_i \\\\\n", + "\\mathbf{q}_j \\\\\n", + "\\end{bmatrix}\n", + "$$" ] }, { @@ -45,8 +100,78 @@ "metadata": {}, "outputs": [], "source": [ - "Hess = kappa[0,0] * (J0.T@J1 + J2.T@J3)\n", - "Hess" + "# Mapping\n", + "Jaxis = sp.Matrix.zeros(6, 24)\n", + "Jaxis[0:3, 0:12] = compute_J_point(qi0_bar)\n", + "Jaxis[0:3, 12:24] = -compute_J_point(qj0_bar)\n", + "Jaxis[3:6, 0:12] = compute_J_point(qi1_bar)\n", + "Jaxis[3:6, 12:24] = -compute_J_point(qj1_bar)\n", + "\n", + "\n", + "content = \"\"\n", + "\n", + "# from ABD q to F\n", + "Faxis = Jaxis @ sp.Matrix.vstack(qi, qj)\n", + "Cl_Faxis = Gen.Closure(\n", + " qi0_bar,\n", + " qi1_bar,\n", + " qi, # Affine Body i\n", + " qj0_bar,\n", + " qj1_bar,\n", + " qj,\n", + ") # Affine Body j\n", + "\n", + "# from F Gradient to ABD Gradient\n", + "Gaxis = Eigen.Vector(\"Gaxis\", 6)\n", + "JaxisT_Gaxis = Jaxis.T @ Gaxis\n", + "Cl_Gaxis = Gen.Closure(\n", + " Gaxis,\n", + " qi0_bar,\n", + " qi1_bar, # Affine Body i\n", + " qj0_bar,\n", + " qj1_bar,\n", + ") # Affine Body j\n", + "# from F Hessian to ABD Hessian\n", + "Haxis = Eigen.Matrix(\"Haxis\", 6, 6)\n", + "JaxisT_Haxis_Jaxis = Jaxis.T @ Haxis @ Jaxis\n", + "Cl_Haxis = Gen.Closure(\n", + " Haxis,\n", + " qi0_bar,\n", + " qi1_bar, # Affine Body i\n", + " qj0_bar,\n", + " qj1_bar,\n", + ") # Affine Body j\n", + "\n", + "content += f\"\"\" \n", + "// Revolute Joint: C0 C1\n", + "// Mapping between ABD qi qj to Faxis\n", + "\n", + "{Cl_Faxis(\"Faxis\", Faxis)}\n", + "{Cl_Gaxis(\"JaxisT_Gaxis\", JaxisT_Gaxis)}\n", + "{Cl_Haxis(\"JaxisT_Haxis_Jaxis\", JaxisT_Haxis_Jaxis)}\n", + "\n", + "\"\"\"\n", + "\n", + "Faxis = Eigen.Vector(\"Faxis\", 6)\n", + "dij0 = Faxis[0:3, 0]\n", + "dij1 = Faxis[3:6, 0]\n", + "\n", + "Eaxis = 0.5 * kappa * (dij0.dot(dij0) + dij1.dot(dij1))\n", + "# E01 = 0\n", + "Cl_Eaxis = Gen.Closure(kappa, Faxis)\n", + "\n", + "\n", + "dEaxisdFaxis = VecDiff(Eaxis, Faxis)\n", + "ddEaxisddFaxis = VecDiff(dEaxisdFaxis, Faxis)\n", + "\n", + "content += f\"\"\" // Revolute Joint Energy and Derivatives\n", + "\n", + "{Cl_Eaxis(\"Eaxis\", Eaxis)}\n", + "{Cl_Eaxis(\"dEaxisdFaxis\", dEaxisdFaxis)}\n", + "{Cl_Eaxis(\"ddEaxisddFaxis\", ddEaxisddFaxis)}\n", + "\n", + "// -------------------------------------------------------------------------\n", + "\"\"\"" ] }, { @@ -55,20 +180,16 @@ "metadata": {}, "outputs": [], "source": [ - "s = f'''// Affine Body Revolute Joint Energy\n", - "{Cl(\"Hess\",Hess)}\n", - "'''\n", - "print(s)\n", - "\n", - "f = open(backend_source_dir('cuda') / 'affine_body/constitutions/sym/affine_body_revolute_joint.inl', 'w')\n", - "f.write(s)\n", - "f.close()" + "path = backend_source_dir(\"cuda\") / \"affine_body/constitutions/sym\" / \"affine_body_revolute_joint.inl\"\n", + "with open(path, \"w\") as f:\n", + " f.write(content)\n", + "print(f\"Written to {path}\")\n" ] } ], "metadata": { "kernelspec": { - "display_name": "base", + "display_name": ".venv", "language": "python", "name": "python3" }, @@ -82,7 +203,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.12.7" + "version": "3.11.15" } }, "nbformat": 4, diff --git a/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint.cu b/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint.cu index a8f9f56a..89d47da7 100644 --- a/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint.cu +++ b/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint.cu @@ -14,6 +14,7 @@ class AffineBodyRevoluteJoint final : public InterAffineBodyConstitution public: using InterAffineBodyConstitution::InterAffineBodyConstitution; static constexpr SizeT HalfHessianSize = 2 * (2 + 1) / 2; + static constexpr SizeT StencilSize = 2; static constexpr U64 ConstitutionUID = 18; @@ -182,21 +183,20 @@ Edge = ({}, {}))", Vector12 q_i = qs(bids(0)); Vector12 q_j = qs(bids(1)); - ABDJacobi Js[4] = {ABDJacobi{X_bar.segment<3>(0)}, - ABDJacobi{X_bar.segment<3>(3)}, - ABDJacobi{X_bar.segment<3>(6)}, - ABDJacobi{X_bar.segment<3>(9)}}; - - Vector12 X; - X.segment<3>(0) = Js[0].point_x(q_i); - X.segment<3>(3) = Js[1].point_x(q_i); + // qi0_bar, qi1_bar, qj0_bar, qj1_bar + Vector3 qi0_bar = X_bar.segment<3>(0); + Vector3 qi1_bar = X_bar.segment<3>(3); + Vector3 qj0_bar = X_bar.segment<3>(6); + Vector3 qj1_bar = X_bar.segment<3>(9); - X.segment<3>(6) = Js[2].point_x(q_j); - X.segment<3>(9) = Js[3].point_x(q_j); + // Compute constraint violation in F-space + Vector6 F; + RJ::Faxis(F, qi0_bar, qi1_bar, q_i, qj0_bar, qj1_bar, q_j); - // energy = 1/2 * kappa * (||x0 - x2||^2 + ||x1 - x3||^2) - - Es(I) = kappa * RJ::E(X); + // Compute energy: E = 0.5 * kappa * (||d0||^2 + ||d1||^2) + Float E; + RJ::Eaxis(E, kappa, F); + Es(I) = E; }); } @@ -212,6 +212,10 @@ Edge = ({}, {}))", void do_compute_gradient_hessian(ComputeGradientHessianInfo& info) override { using namespace muda; + using Vector24 = Vector; + using Matrix24x24 = Matrix; + using Matrix6x6 = Matrix; + namespace RJ = sym::affine_body_revolute_joint; auto gradient_only = info.gradient_only(); ParallelFor() @@ -233,76 +237,45 @@ Edge = ({}, {}))", Vector12 q_i = qs(bids(0)); Vector12 q_j = qs(bids(1)); - ABDJacobi Js[4] = {ABDJacobi{X_bar.segment<3>(0)}, - ABDJacobi{X_bar.segment<3>(3)}, - ABDJacobi{X_bar.segment<3>(6)}, - ABDJacobi{X_bar.segment<3>(9)}}; + // Extract rest positions + Vector3 qi0_bar = X_bar.segment<3>(0); + Vector3 qi1_bar = X_bar.segment<3>(3); + Vector3 qj0_bar = X_bar.segment<3>(6); + Vector3 qj1_bar = X_bar.segment<3>(9); - Vector12 X; - X.segment<3>(0) = Js[0].point_x(q_i); - X.segment<3>(3) = Js[1].point_x(q_i); - - X.segment<3>(6) = Js[2].point_x(q_j); - X.segment<3>(9) = Js[3].point_x(q_j); - - Vector3 D02 = Js[0].point_x(q_i) - Js[2].point_x(q_j); - Vector3 D13 = Js[1].point_x(q_i) - Js[3].point_x(q_j); - Float K = + Float K = strength_ratio(I) * (body_masses(bids(0)).mass() + body_masses(bids(1)).mass()); - // Fill Body Gradient: - { - // G = 0.5 * kappa * (J0^T * (x0 - x2) + J1^T * (x1 - x3)) - Vector12 G_i = K * (Js[0].T() * D02 + Js[1].T() * D13); - G12s(2 * I + 0).write(bids(0), G_i); - } - { - // G = 0.5 * kappa * (J2^T * (x2 - x0) + J3^T * (x3 - x1)) - Vector12 G_j = K * (Js[2].T() * (-D02) + Js[3].T() * (-D13)); - G12s(2 * I + 1).write(bids(1), G_j); - } + // Compute constraint violation in F-space + Vector6 F; + RJ::Faxis(F, qi0_bar, qi1_bar, q_i, qj0_bar, qj1_bar, q_j); + + // Compute gradient in F-space + Vector6 dEdF; + RJ::dEaxisdFaxis(dEdF, K, F); + + // Map gradient back to ABD space: G24 = J^T * dEdF + Vector24 G24; + RJ::JaxisT_Gaxis(G24, dEdF, qi0_bar, qi1_bar, qj0_bar, qj1_bar); - // Fill Body Hessian: - if(!gradient_only) + // Fill Body Gradient + DoubletVectorAssembler DVA{G12s}; + DVA.segment(StencilSize * I).write(bids, G24); + if(gradient_only) { - { - Matrix12x12 H_ii; - RJ::Hess(H_ii, - K, - Js[0].x_bar(), - Js[0].x_bar(), - Js[1].x_bar(), - Js[1].x_bar()); - H12x12s(HalfHessianSize * I + 0).write(bids(0), bids(0), H_ii); - } - { - Matrix12x12 H; - Vector2i lr = bids; - RJ::Hess(H, - -K, - Js[0].x_bar(), - Js[2].x_bar(), - Js[1].x_bar(), - Js[3].x_bar()); - if(bids(0) > bids(1)) - { - H.transposeInPlace(); - lr = Vector2i{bids(1), bids(0)}; - } - H12x12s(HalfHessianSize * I + 1).write(lr(0), lr(1), H); - } - { - Matrix12x12 H_jj; - RJ::Hess(H_jj, - K, - Js[2].x_bar(), - Js[2].x_bar(), - Js[3].x_bar(), - Js[3].x_bar()); - H12x12s(HalfHessianSize * I + 2).write(bids(1), bids(1), H_jj); - } + return; } + // Fill Body Hessian + Matrix6x6 ddEddF; + RJ::ddEaxisddFaxis(ddEddF, K, F); + + // Map Hessian back to ABD space: H24 = J^T * ddEddF * J + Matrix24x24 H24; + RJ::JaxisT_Haxis_Jaxis(H24, ddEddF, qi0_bar, qi1_bar, qj0_bar, qj1_bar); + + TripletMatrixAssembler TMA{H12x12s}; + TMA.half_block(HalfHessianSize * I).write(bids, H24); }); } @@ -789,7 +762,8 @@ Edge = ({}, {}))", qs = info.qs().cviewer().name("qs"), body_masses = info.body_masses().cviewer().name("body_masses"), G12s = info.gradients().viewer().name("G12s"), - H12x12s = info.hessians().viewer().name("H12x12s")] __device__(int I) + H12x12s = info.hessians().viewer().name("H12x12s"), + gradient_only = info.gradient_only()] __device__(int I) { Vector2i bids = body_ids(I); auto constrained = is_constrained(I); @@ -846,6 +820,10 @@ Edge = ({}, {}))", DoubletVectorAssembler DVA{G12s}; DVA.segment(StencilSize * I).write(bids, J01T_G01); + if(gradient_only) + { + return; + } // H12x12s Matrix12x12 H01; DRJ::ddEddF01(H01, kappa, F01_q, theta_tilde); diff --git a/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint_function.h b/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint_function.h index 72f93299..0d21b536 100644 --- a/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint_function.h +++ b/src/backends/cuda/affine_body/constitutions/affine_body_revolute_joint_function.h @@ -5,13 +5,6 @@ namespace uipc::backend::cuda { namespace sym::affine_body_revolute_joint { - inline UIPC_GENERIC Float E(const Vector12& X) - { - Float E0 = (X.segment<3>(0) - X.segment<3>(6)).squaredNorm(); - Float E1 = (X.segment<3>(3) - X.segment<3>(9)).squaredNorm(); - return (E0 + E1) / 2; - } - #include "sym/affine_body_revolute_joint.inl" } // namespace sym::affine_body_revolute_joint namespace sym::affine_body_driving_revolute_joint diff --git a/src/backends/cuda/affine_body/constitutions/sym/affine_body_revolute_joint.inl b/src/backends/cuda/affine_body/constitutions/sym/affine_body_revolute_joint.inl index daaa895c..825bd0ce 100644 --- a/src/backends/cuda/affine_body/constitutions/sym/affine_body_revolute_joint.inl +++ b/src/backends/cuda/affine_body/constitutions/sym/affine_body_revolute_joint.inl @@ -1,6 +1,9 @@ -// Affine Body Revolute Joint Energy + +// Revolute Joint: C0 C1 +// Mapping between ABD qi qj to Faxis + template -__host__ __device__ void Hess(Eigen::Matrix& R, const T& kappa, const Eigen::Vector& x0_bar, const Eigen::Vector& x1_bar, const Eigen::Vector& x2_bar, const Eigen::Vector& x3_bar) +__host__ __device__ void Faxis(Eigen::Vector& R, const Eigen::Vector& qi0_bar, const Eigen::Vector& qi1_bar, const Eigen::Vector& qi, const Eigen::Vector& qj0_bar, const Eigen::Vector& qj1_bar, const Eigen::Vector& qj) { /***************************************************************************************************************************** Function generated by SymEigen.py @@ -8,186 +11,1076 @@ Author: MuGdxy GitHub: https://github.com/MuGdxy/SymEigen E-Mail: lxy819469559@gmail.com ****************************************************************************************************************************** -LaTeX expression: -//tex:$$R = \left[\begin{array}{cccccccccccc}2 \kappa & 0 & 0 & \kappa \left(x_{1 bar(0)} + x_{3 bar(0)}\right) & \kappa \left(x_{1 bar(1)} + x_{3 bar(1)}\right) & \kappa \left(x_{1 bar(2)} + x_{3 bar(2)}\right) & 0 & 0 & 0 & 0 & 0 & 0\\0 & 2 \kappa & 0 & 0 & 0 & 0 & \kappa \left(x_{1 bar(0)} + x_{3 bar(0)}\right) & \kappa \left(x_{1 bar(1)} + x_{3 bar(1)}\right) & \kappa \left(x_{1 bar(2)} + x_{3 bar(2)}\right) & 0 & 0 & 0\\0 & 0 & 2 \kappa & 0 & 0 & 0 & 0 & 0 & 0 & \kappa \left(x_{1 bar(0)} + x_{3 bar(0)}\right) & \kappa \left(x_{1 bar(1)} + x_{3 bar(1)}\right) & \kappa \left(x_{1 bar(2)} + x_{3 bar(2)}\right)\\\kappa \left(x_{0 bar(0)} + x_{2 bar(0)}\right) & 0 & 0 & \kappa \left(x_{0 bar(0)} x_{1 bar(0)} + x_{2 bar(0)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(0)} x_{1 bar(1)} + x_{2 bar(0)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(0)} x_{1 bar(2)} + x_{2 bar(0)} x_{3 bar(2)}\right) & 0 & 0 & 0 & 0 & 0 & 0\\\kappa \left(x_{0 bar(1)} + x_{2 bar(1)}\right) & 0 & 0 & \kappa \left(x_{0 bar(1)} x_{1 bar(0)} + x_{2 bar(1)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(1)} x_{1 bar(1)} + x_{2 bar(1)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(1)} x_{1 bar(2)} + x_{2 bar(1)} x_{3 bar(2)}\right) & 0 & 0 & 0 & 0 & 0 & 0\\\kappa \left(x_{0 bar(2)} + x_{2 bar(2)}\right) & 0 & 0 & \kappa \left(x_{0 bar(2)} x_{1 bar(0)} + x_{2 bar(2)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(2)} x_{1 bar(1)} + x_{2 bar(2)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(2)} x_{1 bar(2)} + x_{2 bar(2)} x_{3 bar(2)}\right) & 0 & 0 & 0 & 0 & 0 & 0\\0 & \kappa \left(x_{0 bar(0)} + x_{2 bar(0)}\right) & 0 & 0 & 0 & 0 & \kappa \left(x_{0 bar(0)} x_{1 bar(0)} + x_{2 bar(0)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(0)} x_{1 bar(1)} + x_{2 bar(0)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(0)} x_{1 bar(2)} + x_{2 bar(0)} x_{3 bar(2)}\right) & 0 & 0 & 0\\0 & \kappa \left(x_{0 bar(1)} + x_{2 bar(1)}\right) & 0 & 0 & 0 & 0 & \kappa \left(x_{0 bar(1)} x_{1 bar(0)} + x_{2 bar(1)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(1)} x_{1 bar(1)} + x_{2 bar(1)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(1)} x_{1 bar(2)} + x_{2 bar(1)} x_{3 bar(2)}\right) & 0 & 0 & 0\\0 & \kappa \left(x_{0 bar(2)} + x_{2 bar(2)}\right) & 0 & 0 & 0 & 0 & \kappa \left(x_{0 bar(2)} x_{1 bar(0)} + x_{2 bar(2)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(2)} x_{1 bar(1)} + x_{2 bar(2)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(2)} x_{1 bar(2)} + x_{2 bar(2)} x_{3 bar(2)}\right) & 0 & 0 & 0\\0 & 0 & \kappa \left(x_{0 bar(0)} + x_{2 bar(0)}\right) & 0 & 0 & 0 & 0 & 0 & 0 & \kappa \left(x_{0 bar(0)} x_{1 bar(0)} + x_{2 bar(0)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(0)} x_{1 bar(1)} + x_{2 bar(0)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(0)} x_{1 bar(2)} + x_{2 bar(0)} x_{3 bar(2)}\right)\\0 & 0 & \kappa \left(x_{0 bar(1)} + x_{2 bar(1)}\right) & 0 & 0 & 0 & 0 & 0 & 0 & \kappa \left(x_{0 bar(1)} x_{1 bar(0)} + x_{2 bar(1)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(1)} x_{1 bar(1)} + x_{2 bar(1)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(1)} x_{1 bar(2)} + x_{2 bar(1)} x_{3 bar(2)}\right)\\0 & 0 & \kappa \left(x_{0 bar(2)} + x_{2 bar(2)}\right) & 0 & 0 & 0 & 0 & 0 & 0 & \kappa \left(x_{0 bar(2)} x_{1 bar(0)} + x_{2 bar(2)} x_{3 bar(0)}\right) & \kappa \left(x_{0 bar(2)} x_{1 bar(1)} + x_{2 bar(2)} x_{3 bar(1)}\right) & \kappa \left(x_{0 bar(2)} x_{1 bar(2)} + x_{2 bar(2)} x_{3 bar(2)}\right)\end{array}\right]$$ +Symbol Name Mapping: +qi0_bar: + -> {} + -> Matrix([[qi0_bar(0)], [qi0_bar(1)], [qi0_bar(2)]]) +qi1_bar: + -> {} + -> Matrix([[qi1_bar(0)], [qi1_bar(1)], [qi1_bar(2)]]) +qi: + -> {} + -> Matrix([[qi(0)], [qi(1)], [qi(2)], [qi(3)], [qi(4)], [qi(5)], [qi(6)], [qi(7)], [qi(8)], [qi(9)], [qi(10)], [qi(11)]]) +qj0_bar: + -> {} + -> Matrix([[qj0_bar(0)], [qj0_bar(1)], [qj0_bar(2)]]) +qj1_bar: + -> {} + -> Matrix([[qj1_bar(0)], [qj1_bar(1)], [qj1_bar(2)]]) +qj: + -> {} + -> Matrix([[qj(0)], [qj(1)], [qj(2)], [qj(3)], [qj(4)], [qj(5)], [qj(6)], [qj(7)], [qj(8)], [qj(9)], [qj(10)], [qj(11)]]) +*****************************************************************************************************************************/ +/* Sub Exprs */ +auto x0 = qi(0) - qj(0); +auto x1 = qi(1) - qj(1); +auto x2 = qi(2) - qj(2); +/* Simplified Expr */ +R(0) = qi(3)*qi0_bar(0) + qi(4)*qi0_bar(1) + qi(5)*qi0_bar(2) - qj(3)*qj0_bar(0) - qj(4)*qj0_bar(1) - qj(5)*qj0_bar(2) + x0; +R(1) = qi(6)*qi0_bar(0) + qi(7)*qi0_bar(1) + qi(8)*qi0_bar(2) - qj(6)*qj0_bar(0) - qj(7)*qj0_bar(1) - qj(8)*qj0_bar(2) + x1; +R(2) = qi(10)*qi0_bar(1) + qi(11)*qi0_bar(2) + qi(9)*qi0_bar(0) - qj(10)*qj0_bar(1) - qj(11)*qj0_bar(2) - qj(9)*qj0_bar(0) + x2; +R(3) = qi(3)*qi1_bar(0) + qi(4)*qi1_bar(1) + qi(5)*qi1_bar(2) - qj(3)*qj1_bar(0) - qj(4)*qj1_bar(1) - qj(5)*qj1_bar(2) + x0; +R(4) = qi(6)*qi1_bar(0) + qi(7)*qi1_bar(1) + qi(8)*qi1_bar(2) - qj(6)*qj1_bar(0) - qj(7)*qj1_bar(1) - qj(8)*qj1_bar(2) + x1; +R(5) = qi(10)*qi1_bar(1) + qi(11)*qi1_bar(2) + qi(9)*qi1_bar(0) - qj(10)*qj1_bar(1) - qj(11)*qj1_bar(2) - qj(9)*qj1_bar(0) + x2; +} +template +__host__ __device__ void JaxisT_Gaxis(Eigen::Vector& R, const Eigen::Vector& Gaxis, const Eigen::Vector& qi0_bar, const Eigen::Vector& qi1_bar, const Eigen::Vector& qj0_bar, const Eigen::Vector& qj1_bar) +{ +/***************************************************************************************************************************** +Function generated by SymEigen.py +Author: MuGdxy +GitHub: https://github.com/MuGdxy/SymEigen +E-Mail: lxy819469559@gmail.com +****************************************************************************************************************************** +Symbol Name Mapping: +Gaxis: + -> {} + -> Matrix([[Gaxis(0)], [Gaxis(1)], [Gaxis(2)], [Gaxis(3)], [Gaxis(4)], [Gaxis(5)]]) +qi0_bar: + -> {} + -> Matrix([[qi0_bar(0)], [qi0_bar(1)], [qi0_bar(2)]]) +qi1_bar: + -> {} + -> Matrix([[qi1_bar(0)], [qi1_bar(1)], [qi1_bar(2)]]) +qj0_bar: + -> {} + -> Matrix([[qj0_bar(0)], [qj0_bar(1)], [qj0_bar(2)]]) +qj1_bar: + -> {} + -> Matrix([[qj1_bar(0)], [qj1_bar(1)], [qj1_bar(2)]]) +*****************************************************************************************************************************/ +/* Sub Exprs */ +auto x0 = Gaxis(0) + Gaxis(3); +auto x1 = Gaxis(1) + Gaxis(4); +auto x2 = Gaxis(2) + Gaxis(5); +/* Simplified Expr */ +R(0) = x0; +R(1) = x1; +R(2) = x2; +R(3) = Gaxis(0)*qi0_bar(0) + Gaxis(3)*qi1_bar(0); +R(4) = Gaxis(0)*qi0_bar(1) + Gaxis(3)*qi1_bar(1); +R(5) = Gaxis(0)*qi0_bar(2) + Gaxis(3)*qi1_bar(2); +R(6) = Gaxis(1)*qi0_bar(0) + Gaxis(4)*qi1_bar(0); +R(7) = Gaxis(1)*qi0_bar(1) + Gaxis(4)*qi1_bar(1); +R(8) = Gaxis(1)*qi0_bar(2) + Gaxis(4)*qi1_bar(2); +R(9) = Gaxis(2)*qi0_bar(0) + Gaxis(5)*qi1_bar(0); +R(10) = Gaxis(2)*qi0_bar(1) + Gaxis(5)*qi1_bar(1); +R(11) = Gaxis(2)*qi0_bar(2) + Gaxis(5)*qi1_bar(2); +R(12) = -x0; +R(13) = -x1; +R(14) = -x2; +R(15) = -Gaxis(0)*qj0_bar(0) - Gaxis(3)*qj1_bar(0); +R(16) = -Gaxis(0)*qj0_bar(1) - Gaxis(3)*qj1_bar(1); +R(17) = -Gaxis(0)*qj0_bar(2) - Gaxis(3)*qj1_bar(2); +R(18) = -Gaxis(1)*qj0_bar(0) - Gaxis(4)*qj1_bar(0); +R(19) = -Gaxis(1)*qj0_bar(1) - Gaxis(4)*qj1_bar(1); +R(20) = -Gaxis(1)*qj0_bar(2) - Gaxis(4)*qj1_bar(2); +R(21) = -Gaxis(2)*qj0_bar(0) - Gaxis(5)*qj1_bar(0); +R(22) = -Gaxis(2)*qj0_bar(1) - Gaxis(5)*qj1_bar(1); +R(23) = -Gaxis(2)*qj0_bar(2) - Gaxis(5)*qj1_bar(2); +} +template +__host__ __device__ void JaxisT_Haxis_Jaxis(Eigen::Matrix& R, const Eigen::Matrix& Haxis, const Eigen::Vector& qi0_bar, const Eigen::Vector& qi1_bar, const Eigen::Vector& qj0_bar, const Eigen::Vector& qj1_bar) +{ +/***************************************************************************************************************************** +Function generated by SymEigen.py +Author: MuGdxy +GitHub: https://github.com/MuGdxy/SymEigen +E-Mail: lxy819469559@gmail.com +****************************************************************************************************************************** +Symbol Name Mapping: +Haxis: + -> {} + -> Matrix([[Haxis(0,0), Haxis(0,1), Haxis(0,2), Haxis(0,3), Haxis(0,4), Haxis(0,5)], [Haxis(1,0), Haxis(1,1), Haxis(1,2), Haxis(1,3), Haxis(1,4), Haxis(1,5)], [Haxis(2,0), Haxis(2,1), Haxis(2,2), Haxis(2,3), Haxis(2,4), Haxis(2,5)], [Haxis(3,0), Haxis(3,1), Haxis(3,2), Haxis(3,3), Haxis(3,4), Haxis(3,5)], [Haxis(4,0), Haxis(4,1), Haxis(4,2), Haxis(4,3), Haxis(4,4), Haxis(4,5)], [Haxis(5,0), Haxis(5,1), Haxis(5,2), Haxis(5,3), Haxis(5,4), Haxis(5,5)]]) +qi0_bar: + -> {} + -> Matrix([[qi0_bar(0)], [qi0_bar(1)], [qi0_bar(2)]]) +qi1_bar: + -> {} + -> Matrix([[qi1_bar(0)], [qi1_bar(1)], [qi1_bar(2)]]) +qj0_bar: + -> {} + -> Matrix([[qj0_bar(0)], [qj0_bar(1)], [qj0_bar(2)]]) +qj1_bar: + -> {} + -> Matrix([[qj1_bar(0)], [qj1_bar(1)], [qj1_bar(2)]]) +*****************************************************************************************************************************/ +/* Sub Exprs */ +auto x0 = Haxis(0,0) + Haxis(3,0); +auto x1 = Haxis(0,3) + Haxis(3,3); +auto x2 = x0 + x1; +auto x3 = Haxis(0,1) + Haxis(3,1); +auto x4 = Haxis(0,4) + Haxis(3,4); +auto x5 = x3 + x4; +auto x6 = Haxis(0,2) + Haxis(3,2); +auto x7 = Haxis(0,5) + Haxis(3,5); +auto x8 = x6 + x7; +auto x9 = -x2; +auto x10 = -x5; +auto x11 = -x8; +auto x12 = Haxis(1,0) + Haxis(4,0); +auto x13 = Haxis(1,3) + Haxis(4,3); +auto x14 = x12 + x13; +auto x15 = Haxis(1,1) + Haxis(4,1); +auto x16 = Haxis(1,4) + Haxis(4,4); +auto x17 = x15 + x16; +auto x18 = Haxis(1,2) + Haxis(4,2); +auto x19 = Haxis(1,5) + Haxis(4,5); +auto x20 = x18 + x19; +auto x21 = -x14; +auto x22 = -x17; +auto x23 = -x20; +auto x24 = Haxis(2,0) + Haxis(5,0); +auto x25 = Haxis(2,3) + Haxis(5,3); +auto x26 = x24 + x25; +auto x27 = Haxis(2,1) + Haxis(5,1); +auto x28 = Haxis(2,4) + Haxis(5,4); +auto x29 = x27 + x28; +auto x30 = Haxis(2,2) + Haxis(5,2); +auto x31 = Haxis(2,5) + Haxis(5,5); +auto x32 = x30 + x31; +auto x33 = -x26; +auto x34 = -x29; +auto x35 = -x32; +auto x36 = Haxis(0,0)*qi0_bar(0) + Haxis(3,0)*qi1_bar(0); +auto x37 = Haxis(0,3)*qi0_bar(0) + Haxis(3,3)*qi1_bar(0); +auto x38 = x36 + x37; +auto x39 = Haxis(0,1)*qi0_bar(0) + Haxis(3,1)*qi1_bar(0); +auto x40 = Haxis(0,4)*qi0_bar(0) + Haxis(3,4)*qi1_bar(0); +auto x41 = x39 + x40; +auto x42 = Haxis(0,2)*qi0_bar(0) + Haxis(3,2)*qi1_bar(0); +auto x43 = Haxis(0,5)*qi0_bar(0) + Haxis(3,5)*qi1_bar(0); +auto x44 = x42 + x43; +auto x45 = Haxis(0,0)*qi0_bar(1) + Haxis(3,0)*qi1_bar(1); +auto x46 = Haxis(0,3)*qi0_bar(1) + Haxis(3,3)*qi1_bar(1); +auto x47 = x45 + x46; +auto x48 = Haxis(0,1)*qi0_bar(1) + Haxis(3,1)*qi1_bar(1); +auto x49 = Haxis(0,4)*qi0_bar(1) + Haxis(3,4)*qi1_bar(1); +auto x50 = x48 + x49; +auto x51 = Haxis(0,2)*qi0_bar(1) + Haxis(3,2)*qi1_bar(1); +auto x52 = Haxis(0,5)*qi0_bar(1) + Haxis(3,5)*qi1_bar(1); +auto x53 = x51 + x52; +auto x54 = Haxis(0,0)*qi0_bar(2) + Haxis(3,0)*qi1_bar(2); +auto x55 = Haxis(0,3)*qi0_bar(2) + Haxis(3,3)*qi1_bar(2); +auto x56 = x54 + x55; +auto x57 = Haxis(0,1)*qi0_bar(2) + Haxis(3,1)*qi1_bar(2); +auto x58 = Haxis(0,4)*qi0_bar(2) + Haxis(3,4)*qi1_bar(2); +auto x59 = x57 + x58; +auto x60 = Haxis(0,2)*qi0_bar(2) + Haxis(3,2)*qi1_bar(2); +auto x61 = Haxis(0,5)*qi0_bar(2) + Haxis(3,5)*qi1_bar(2); +auto x62 = x60 + x61; +auto x63 = Haxis(1,0)*qi0_bar(0) + Haxis(4,0)*qi1_bar(0); +auto x64 = Haxis(1,3)*qi0_bar(0) + Haxis(4,3)*qi1_bar(0); +auto x65 = x63 + x64; +auto x66 = Haxis(1,1)*qi0_bar(0) + Haxis(4,1)*qi1_bar(0); +auto x67 = Haxis(1,4)*qi0_bar(0) + Haxis(4,4)*qi1_bar(0); +auto x68 = x66 + x67; +auto x69 = Haxis(1,2)*qi0_bar(0) + Haxis(4,2)*qi1_bar(0); +auto x70 = Haxis(1,5)*qi0_bar(0) + Haxis(4,5)*qi1_bar(0); +auto x71 = x69 + x70; +auto x72 = Haxis(1,0)*qi0_bar(1) + Haxis(4,0)*qi1_bar(1); +auto x73 = Haxis(1,3)*qi0_bar(1) + Haxis(4,3)*qi1_bar(1); +auto x74 = x72 + x73; +auto x75 = Haxis(1,1)*qi0_bar(1) + Haxis(4,1)*qi1_bar(1); +auto x76 = Haxis(1,4)*qi0_bar(1) + Haxis(4,4)*qi1_bar(1); +auto x77 = x75 + x76; +auto x78 = Haxis(1,2)*qi0_bar(1) + Haxis(4,2)*qi1_bar(1); +auto x79 = Haxis(1,5)*qi0_bar(1) + Haxis(4,5)*qi1_bar(1); +auto x80 = x78 + x79; +auto x81 = Haxis(1,0)*qi0_bar(2) + Haxis(4,0)*qi1_bar(2); +auto x82 = Haxis(1,3)*qi0_bar(2) + Haxis(4,3)*qi1_bar(2); +auto x83 = x81 + x82; +auto x84 = Haxis(1,1)*qi0_bar(2) + Haxis(4,1)*qi1_bar(2); +auto x85 = Haxis(1,4)*qi0_bar(2) + Haxis(4,4)*qi1_bar(2); +auto x86 = x84 + x85; +auto x87 = Haxis(1,2)*qi0_bar(2) + Haxis(4,2)*qi1_bar(2); +auto x88 = Haxis(1,5)*qi0_bar(2) + Haxis(4,5)*qi1_bar(2); +auto x89 = x87 + x88; +auto x90 = Haxis(2,0)*qi0_bar(0) + Haxis(5,0)*qi1_bar(0); +auto x91 = Haxis(2,3)*qi0_bar(0) + Haxis(5,3)*qi1_bar(0); +auto x92 = x90 + x91; +auto x93 = Haxis(2,1)*qi0_bar(0) + Haxis(5,1)*qi1_bar(0); +auto x94 = Haxis(2,4)*qi0_bar(0) + Haxis(5,4)*qi1_bar(0); +auto x95 = x93 + x94; +auto x96 = Haxis(2,2)*qi0_bar(0) + Haxis(5,2)*qi1_bar(0); +auto x97 = Haxis(2,5)*qi0_bar(0) + Haxis(5,5)*qi1_bar(0); +auto x98 = x96 + x97; +auto x99 = Haxis(2,0)*qi0_bar(1) + Haxis(5,0)*qi1_bar(1); +auto x100 = Haxis(2,3)*qi0_bar(1) + Haxis(5,3)*qi1_bar(1); +auto x101 = x100 + x99; +auto x102 = Haxis(2,1)*qi0_bar(1) + Haxis(5,1)*qi1_bar(1); +auto x103 = Haxis(2,4)*qi0_bar(1) + Haxis(5,4)*qi1_bar(1); +auto x104 = x102 + x103; +auto x105 = Haxis(2,2)*qi0_bar(1) + Haxis(5,2)*qi1_bar(1); +auto x106 = Haxis(2,5)*qi0_bar(1) + Haxis(5,5)*qi1_bar(1); +auto x107 = x105 + x106; +auto x108 = Haxis(2,0)*qi0_bar(2) + Haxis(5,0)*qi1_bar(2); +auto x109 = Haxis(2,3)*qi0_bar(2) + Haxis(5,3)*qi1_bar(2); +auto x110 = x108 + x109; +auto x111 = Haxis(2,1)*qi0_bar(2) + Haxis(5,1)*qi1_bar(2); +auto x112 = Haxis(2,4)*qi0_bar(2) + Haxis(5,4)*qi1_bar(2); +auto x113 = x111 + x112; +auto x114 = Haxis(2,2)*qi0_bar(2) + Haxis(5,2)*qi1_bar(2); +auto x115 = Haxis(2,5)*qi0_bar(2) + Haxis(5,5)*qi1_bar(2); +auto x116 = x114 + x115; +auto x117 = -x0; +auto x118 = -x1; +auto x119 = -x3; +auto x120 = -x4; +auto x121 = -x6; +auto x122 = -x7; +auto x123 = -x12; +auto x124 = -x13; +auto x125 = -x15; +auto x126 = -x16; +auto x127 = -x18; +auto x128 = -x19; +auto x129 = -x24; +auto x130 = -x25; +auto x131 = -x27; +auto x132 = -x28; +auto x133 = -x30; +auto x134 = -x31; +auto x135 = Haxis(0,0)*qj0_bar(0) + Haxis(3,0)*qj1_bar(0); +auto x136 = Haxis(0,3)*qj0_bar(0) + Haxis(3,3)*qj1_bar(0); +auto x137 = x135 + x136; +auto x138 = Haxis(0,1)*qj0_bar(0) + Haxis(3,1)*qj1_bar(0); +auto x139 = Haxis(0,4)*qj0_bar(0) + Haxis(3,4)*qj1_bar(0); +auto x140 = x138 + x139; +auto x141 = Haxis(0,2)*qj0_bar(0) + Haxis(3,2)*qj1_bar(0); +auto x142 = Haxis(0,5)*qj0_bar(0) + Haxis(3,5)*qj1_bar(0); +auto x143 = x141 + x142; +auto x144 = -x135; +auto x145 = -x136; +auto x146 = -x138; +auto x147 = -x139; +auto x148 = -x141; +auto x149 = -x142; +auto x150 = Haxis(0,0)*qj0_bar(1) + Haxis(3,0)*qj1_bar(1); +auto x151 = Haxis(0,3)*qj0_bar(1) + Haxis(3,3)*qj1_bar(1); +auto x152 = x150 + x151; +auto x153 = Haxis(0,1)*qj0_bar(1) + Haxis(3,1)*qj1_bar(1); +auto x154 = Haxis(0,4)*qj0_bar(1) + Haxis(3,4)*qj1_bar(1); +auto x155 = x153 + x154; +auto x156 = Haxis(0,2)*qj0_bar(1) + Haxis(3,2)*qj1_bar(1); +auto x157 = Haxis(0,5)*qj0_bar(1) + Haxis(3,5)*qj1_bar(1); +auto x158 = x156 + x157; +auto x159 = -x150; +auto x160 = -x151; +auto x161 = -x153; +auto x162 = -x154; +auto x163 = -x156; +auto x164 = -x157; +auto x165 = Haxis(0,0)*qj0_bar(2) + Haxis(3,0)*qj1_bar(2); +auto x166 = Haxis(0,3)*qj0_bar(2) + Haxis(3,3)*qj1_bar(2); +auto x167 = x165 + x166; +auto x168 = Haxis(0,1)*qj0_bar(2) + Haxis(3,1)*qj1_bar(2); +auto x169 = Haxis(0,4)*qj0_bar(2) + Haxis(3,4)*qj1_bar(2); +auto x170 = x168 + x169; +auto x171 = Haxis(0,2)*qj0_bar(2) + Haxis(3,2)*qj1_bar(2); +auto x172 = Haxis(0,5)*qj0_bar(2) + Haxis(3,5)*qj1_bar(2); +auto x173 = x171 + x172; +auto x174 = -x165; +auto x175 = -x166; +auto x176 = -x168; +auto x177 = -x169; +auto x178 = -x171; +auto x179 = -x172; +auto x180 = Haxis(1,0)*qj0_bar(0) + Haxis(4,0)*qj1_bar(0); +auto x181 = Haxis(1,3)*qj0_bar(0) + Haxis(4,3)*qj1_bar(0); +auto x182 = x180 + x181; +auto x183 = Haxis(1,1)*qj0_bar(0) + Haxis(4,1)*qj1_bar(0); +auto x184 = Haxis(1,4)*qj0_bar(0) + Haxis(4,4)*qj1_bar(0); +auto x185 = x183 + x184; +auto x186 = Haxis(1,2)*qj0_bar(0) + Haxis(4,2)*qj1_bar(0); +auto x187 = Haxis(1,5)*qj0_bar(0) + Haxis(4,5)*qj1_bar(0); +auto x188 = x186 + x187; +auto x189 = -x180; +auto x190 = -x181; +auto x191 = -x183; +auto x192 = -x184; +auto x193 = -x186; +auto x194 = -x187; +auto x195 = Haxis(1,0)*qj0_bar(1) + Haxis(4,0)*qj1_bar(1); +auto x196 = Haxis(1,3)*qj0_bar(1) + Haxis(4,3)*qj1_bar(1); +auto x197 = x195 + x196; +auto x198 = Haxis(1,1)*qj0_bar(1) + Haxis(4,1)*qj1_bar(1); +auto x199 = Haxis(1,4)*qj0_bar(1) + Haxis(4,4)*qj1_bar(1); +auto x200 = x198 + x199; +auto x201 = Haxis(1,2)*qj0_bar(1) + Haxis(4,2)*qj1_bar(1); +auto x202 = Haxis(1,5)*qj0_bar(1) + Haxis(4,5)*qj1_bar(1); +auto x203 = x201 + x202; +auto x204 = -x195; +auto x205 = -x196; +auto x206 = -x198; +auto x207 = -x199; +auto x208 = -x201; +auto x209 = -x202; +auto x210 = Haxis(1,0)*qj0_bar(2) + Haxis(4,0)*qj1_bar(2); +auto x211 = Haxis(1,3)*qj0_bar(2) + Haxis(4,3)*qj1_bar(2); +auto x212 = x210 + x211; +auto x213 = Haxis(1,1)*qj0_bar(2) + Haxis(4,1)*qj1_bar(2); +auto x214 = Haxis(1,4)*qj0_bar(2) + Haxis(4,4)*qj1_bar(2); +auto x215 = x213 + x214; +auto x216 = Haxis(1,2)*qj0_bar(2) + Haxis(4,2)*qj1_bar(2); +auto x217 = Haxis(1,5)*qj0_bar(2) + Haxis(4,5)*qj1_bar(2); +auto x218 = x216 + x217; +auto x219 = -x210; +auto x220 = -x211; +auto x221 = -x213; +auto x222 = -x214; +auto x223 = -x216; +auto x224 = -x217; +auto x225 = Haxis(2,0)*qj0_bar(0) + Haxis(5,0)*qj1_bar(0); +auto x226 = Haxis(2,3)*qj0_bar(0) + Haxis(5,3)*qj1_bar(0); +auto x227 = x225 + x226; +auto x228 = Haxis(2,1)*qj0_bar(0) + Haxis(5,1)*qj1_bar(0); +auto x229 = Haxis(2,4)*qj0_bar(0) + Haxis(5,4)*qj1_bar(0); +auto x230 = x228 + x229; +auto x231 = Haxis(2,2)*qj0_bar(0) + Haxis(5,2)*qj1_bar(0); +auto x232 = Haxis(2,5)*qj0_bar(0) + Haxis(5,5)*qj1_bar(0); +auto x233 = x231 + x232; +auto x234 = -x225; +auto x235 = -x226; +auto x236 = -x228; +auto x237 = -x229; +auto x238 = -x231; +auto x239 = -x232; +auto x240 = Haxis(2,0)*qj0_bar(1) + Haxis(5,0)*qj1_bar(1); +auto x241 = Haxis(2,3)*qj0_bar(1) + Haxis(5,3)*qj1_bar(1); +auto x242 = x240 + x241; +auto x243 = Haxis(2,1)*qj0_bar(1) + Haxis(5,1)*qj1_bar(1); +auto x244 = Haxis(2,4)*qj0_bar(1) + Haxis(5,4)*qj1_bar(1); +auto x245 = x243 + x244; +auto x246 = Haxis(2,2)*qj0_bar(1) + Haxis(5,2)*qj1_bar(1); +auto x247 = Haxis(2,5)*qj0_bar(1) + Haxis(5,5)*qj1_bar(1); +auto x248 = x246 + x247; +auto x249 = -x240; +auto x250 = -x241; +auto x251 = -x243; +auto x252 = -x244; +auto x253 = -x246; +auto x254 = -x247; +auto x255 = Haxis(2,0)*qj0_bar(2) + Haxis(5,0)*qj1_bar(2); +auto x256 = Haxis(2,3)*qj0_bar(2) + Haxis(5,3)*qj1_bar(2); +auto x257 = x255 + x256; +auto x258 = Haxis(2,1)*qj0_bar(2) + Haxis(5,1)*qj1_bar(2); +auto x259 = Haxis(2,4)*qj0_bar(2) + Haxis(5,4)*qj1_bar(2); +auto x260 = x258 + x259; +auto x261 = Haxis(2,2)*qj0_bar(2) + Haxis(5,2)*qj1_bar(2); +auto x262 = Haxis(2,5)*qj0_bar(2) + Haxis(5,5)*qj1_bar(2); +auto x263 = x261 + x262; +auto x264 = -x255; +auto x265 = -x256; +auto x266 = -x258; +auto x267 = -x259; +auto x268 = -x261; +auto x269 = -x262; +/* Simplified Expr */ +R(0,0) = x2; +R(0,1) = x5; +R(0,2) = x8; +R(0,3) = qi0_bar(0)*x0 + qi1_bar(0)*x1; +R(0,4) = qi0_bar(1)*x0 + qi1_bar(1)*x1; +R(0,5) = qi0_bar(2)*x0 + qi1_bar(2)*x1; +R(0,6) = qi0_bar(0)*x3 + qi1_bar(0)*x4; +R(0,7) = qi0_bar(1)*x3 + qi1_bar(1)*x4; +R(0,8) = qi0_bar(2)*x3 + qi1_bar(2)*x4; +R(0,9) = qi0_bar(0)*x6 + qi1_bar(0)*x7; +R(0,10) = qi0_bar(1)*x6 + qi1_bar(1)*x7; +R(0,11) = qi0_bar(2)*x6 + qi1_bar(2)*x7; +R(0,12) = x9; +R(0,13) = x10; +R(0,14) = x11; +R(0,15) = -qj0_bar(0)*x0 - qj1_bar(0)*x1; +R(0,16) = -qj0_bar(1)*x0 - qj1_bar(1)*x1; +R(0,17) = -qj0_bar(2)*x0 - qj1_bar(2)*x1; +R(0,18) = -qj0_bar(0)*x3 - qj1_bar(0)*x4; +R(0,19) = -qj0_bar(1)*x3 - qj1_bar(1)*x4; +R(0,20) = -qj0_bar(2)*x3 - qj1_bar(2)*x4; +R(0,21) = -qj0_bar(0)*x6 - qj1_bar(0)*x7; +R(0,22) = -qj0_bar(1)*x6 - qj1_bar(1)*x7; +R(0,23) = -qj0_bar(2)*x6 - qj1_bar(2)*x7; +R(1,0) = x14; +R(1,1) = x17; +R(1,2) = x20; +R(1,3) = qi0_bar(0)*x12 + qi1_bar(0)*x13; +R(1,4) = qi0_bar(1)*x12 + qi1_bar(1)*x13; +R(1,5) = qi0_bar(2)*x12 + qi1_bar(2)*x13; +R(1,6) = qi0_bar(0)*x15 + qi1_bar(0)*x16; +R(1,7) = qi0_bar(1)*x15 + qi1_bar(1)*x16; +R(1,8) = qi0_bar(2)*x15 + qi1_bar(2)*x16; +R(1,9) = qi0_bar(0)*x18 + qi1_bar(0)*x19; +R(1,10) = qi0_bar(1)*x18 + qi1_bar(1)*x19; +R(1,11) = qi0_bar(2)*x18 + qi1_bar(2)*x19; +R(1,12) = x21; +R(1,13) = x22; +R(1,14) = x23; +R(1,15) = -qj0_bar(0)*x12 - qj1_bar(0)*x13; +R(1,16) = -qj0_bar(1)*x12 - qj1_bar(1)*x13; +R(1,17) = -qj0_bar(2)*x12 - qj1_bar(2)*x13; +R(1,18) = -qj0_bar(0)*x15 - qj1_bar(0)*x16; +R(1,19) = -qj0_bar(1)*x15 - qj1_bar(1)*x16; +R(1,20) = -qj0_bar(2)*x15 - qj1_bar(2)*x16; +R(1,21) = -qj0_bar(0)*x18 - qj1_bar(0)*x19; +R(1,22) = -qj0_bar(1)*x18 - qj1_bar(1)*x19; +R(1,23) = -qj0_bar(2)*x18 - qj1_bar(2)*x19; +R(2,0) = x26; +R(2,1) = x29; +R(2,2) = x32; +R(2,3) = qi0_bar(0)*x24 + qi1_bar(0)*x25; +R(2,4) = qi0_bar(1)*x24 + qi1_bar(1)*x25; +R(2,5) = qi0_bar(2)*x24 + qi1_bar(2)*x25; +R(2,6) = qi0_bar(0)*x27 + qi1_bar(0)*x28; +R(2,7) = qi0_bar(1)*x27 + qi1_bar(1)*x28; +R(2,8) = qi0_bar(2)*x27 + qi1_bar(2)*x28; +R(2,9) = qi0_bar(0)*x30 + qi1_bar(0)*x31; +R(2,10) = qi0_bar(1)*x30 + qi1_bar(1)*x31; +R(2,11) = qi0_bar(2)*x30 + qi1_bar(2)*x31; +R(2,12) = x33; +R(2,13) = x34; +R(2,14) = x35; +R(2,15) = -qj0_bar(0)*x24 - qj1_bar(0)*x25; +R(2,16) = -qj0_bar(1)*x24 - qj1_bar(1)*x25; +R(2,17) = -qj0_bar(2)*x24 - qj1_bar(2)*x25; +R(2,18) = -qj0_bar(0)*x27 - qj1_bar(0)*x28; +R(2,19) = -qj0_bar(1)*x27 - qj1_bar(1)*x28; +R(2,20) = -qj0_bar(2)*x27 - qj1_bar(2)*x28; +R(2,21) = -qj0_bar(0)*x30 - qj1_bar(0)*x31; +R(2,22) = -qj0_bar(1)*x30 - qj1_bar(1)*x31; +R(2,23) = -qj0_bar(2)*x30 - qj1_bar(2)*x31; +R(3,0) = x38; +R(3,1) = x41; +R(3,2) = x44; +R(3,3) = qi0_bar(0)*x36 + qi1_bar(0)*x37; +R(3,4) = qi0_bar(1)*x36 + qi1_bar(1)*x37; +R(3,5) = qi0_bar(2)*x36 + qi1_bar(2)*x37; +R(3,6) = qi0_bar(0)*x39 + qi1_bar(0)*x40; +R(3,7) = qi0_bar(1)*x39 + qi1_bar(1)*x40; +R(3,8) = qi0_bar(2)*x39 + qi1_bar(2)*x40; +R(3,9) = qi0_bar(0)*x42 + qi1_bar(0)*x43; +R(3,10) = qi0_bar(1)*x42 + qi1_bar(1)*x43; +R(3,11) = qi0_bar(2)*x42 + qi1_bar(2)*x43; +R(3,12) = -x38; +R(3,13) = -x41; +R(3,14) = -x44; +R(3,15) = -qj0_bar(0)*x36 - qj1_bar(0)*x37; +R(3,16) = -qj0_bar(1)*x36 - qj1_bar(1)*x37; +R(3,17) = -qj0_bar(2)*x36 - qj1_bar(2)*x37; +R(3,18) = -qj0_bar(0)*x39 - qj1_bar(0)*x40; +R(3,19) = -qj0_bar(1)*x39 - qj1_bar(1)*x40; +R(3,20) = -qj0_bar(2)*x39 - qj1_bar(2)*x40; +R(3,21) = -qj0_bar(0)*x42 - qj1_bar(0)*x43; +R(3,22) = -qj0_bar(1)*x42 - qj1_bar(1)*x43; +R(3,23) = -qj0_bar(2)*x42 - qj1_bar(2)*x43; +R(4,0) = x47; +R(4,1) = x50; +R(4,2) = x53; +R(4,3) = qi0_bar(0)*x45 + qi1_bar(0)*x46; +R(4,4) = qi0_bar(1)*x45 + qi1_bar(1)*x46; +R(4,5) = qi0_bar(2)*x45 + qi1_bar(2)*x46; +R(4,6) = qi0_bar(0)*x48 + qi1_bar(0)*x49; +R(4,7) = qi0_bar(1)*x48 + qi1_bar(1)*x49; +R(4,8) = qi0_bar(2)*x48 + qi1_bar(2)*x49; +R(4,9) = qi0_bar(0)*x51 + qi1_bar(0)*x52; +R(4,10) = qi0_bar(1)*x51 + qi1_bar(1)*x52; +R(4,11) = qi0_bar(2)*x51 + qi1_bar(2)*x52; +R(4,12) = -x47; +R(4,13) = -x50; +R(4,14) = -x53; +R(4,15) = -qj0_bar(0)*x45 - qj1_bar(0)*x46; +R(4,16) = -qj0_bar(1)*x45 - qj1_bar(1)*x46; +R(4,17) = -qj0_bar(2)*x45 - qj1_bar(2)*x46; +R(4,18) = -qj0_bar(0)*x48 - qj1_bar(0)*x49; +R(4,19) = -qj0_bar(1)*x48 - qj1_bar(1)*x49; +R(4,20) = -qj0_bar(2)*x48 - qj1_bar(2)*x49; +R(4,21) = -qj0_bar(0)*x51 - qj1_bar(0)*x52; +R(4,22) = -qj0_bar(1)*x51 - qj1_bar(1)*x52; +R(4,23) = -qj0_bar(2)*x51 - qj1_bar(2)*x52; +R(5,0) = x56; +R(5,1) = x59; +R(5,2) = x62; +R(5,3) = qi0_bar(0)*x54 + qi1_bar(0)*x55; +R(5,4) = qi0_bar(1)*x54 + qi1_bar(1)*x55; +R(5,5) = qi0_bar(2)*x54 + qi1_bar(2)*x55; +R(5,6) = qi0_bar(0)*x57 + qi1_bar(0)*x58; +R(5,7) = qi0_bar(1)*x57 + qi1_bar(1)*x58; +R(5,8) = qi0_bar(2)*x57 + qi1_bar(2)*x58; +R(5,9) = qi0_bar(0)*x60 + qi1_bar(0)*x61; +R(5,10) = qi0_bar(1)*x60 + qi1_bar(1)*x61; +R(5,11) = qi0_bar(2)*x60 + qi1_bar(2)*x61; +R(5,12) = -x56; +R(5,13) = -x59; +R(5,14) = -x62; +R(5,15) = -qj0_bar(0)*x54 - qj1_bar(0)*x55; +R(5,16) = -qj0_bar(1)*x54 - qj1_bar(1)*x55; +R(5,17) = -qj0_bar(2)*x54 - qj1_bar(2)*x55; +R(5,18) = -qj0_bar(0)*x57 - qj1_bar(0)*x58; +R(5,19) = -qj0_bar(1)*x57 - qj1_bar(1)*x58; +R(5,20) = -qj0_bar(2)*x57 - qj1_bar(2)*x58; +R(5,21) = -qj0_bar(0)*x60 - qj1_bar(0)*x61; +R(5,22) = -qj0_bar(1)*x60 - qj1_bar(1)*x61; +R(5,23) = -qj0_bar(2)*x60 - qj1_bar(2)*x61; +R(6,0) = x65; +R(6,1) = x68; +R(6,2) = x71; +R(6,3) = qi0_bar(0)*x63 + qi1_bar(0)*x64; +R(6,4) = qi0_bar(1)*x63 + qi1_bar(1)*x64; +R(6,5) = qi0_bar(2)*x63 + qi1_bar(2)*x64; +R(6,6) = qi0_bar(0)*x66 + qi1_bar(0)*x67; +R(6,7) = qi0_bar(1)*x66 + qi1_bar(1)*x67; +R(6,8) = qi0_bar(2)*x66 + qi1_bar(2)*x67; +R(6,9) = qi0_bar(0)*x69 + qi1_bar(0)*x70; +R(6,10) = qi0_bar(1)*x69 + qi1_bar(1)*x70; +R(6,11) = qi0_bar(2)*x69 + qi1_bar(2)*x70; +R(6,12) = -x65; +R(6,13) = -x68; +R(6,14) = -x71; +R(6,15) = -qj0_bar(0)*x63 - qj1_bar(0)*x64; +R(6,16) = -qj0_bar(1)*x63 - qj1_bar(1)*x64; +R(6,17) = -qj0_bar(2)*x63 - qj1_bar(2)*x64; +R(6,18) = -qj0_bar(0)*x66 - qj1_bar(0)*x67; +R(6,19) = -qj0_bar(1)*x66 - qj1_bar(1)*x67; +R(6,20) = -qj0_bar(2)*x66 - qj1_bar(2)*x67; +R(6,21) = -qj0_bar(0)*x69 - qj1_bar(0)*x70; +R(6,22) = -qj0_bar(1)*x69 - qj1_bar(1)*x70; +R(6,23) = -qj0_bar(2)*x69 - qj1_bar(2)*x70; +R(7,0) = x74; +R(7,1) = x77; +R(7,2) = x80; +R(7,3) = qi0_bar(0)*x72 + qi1_bar(0)*x73; +R(7,4) = qi0_bar(1)*x72 + qi1_bar(1)*x73; +R(7,5) = qi0_bar(2)*x72 + qi1_bar(2)*x73; +R(7,6) = qi0_bar(0)*x75 + qi1_bar(0)*x76; +R(7,7) = qi0_bar(1)*x75 + qi1_bar(1)*x76; +R(7,8) = qi0_bar(2)*x75 + qi1_bar(2)*x76; +R(7,9) = qi0_bar(0)*x78 + qi1_bar(0)*x79; +R(7,10) = qi0_bar(1)*x78 + qi1_bar(1)*x79; +R(7,11) = qi0_bar(2)*x78 + qi1_bar(2)*x79; +R(7,12) = -x74; +R(7,13) = -x77; +R(7,14) = -x80; +R(7,15) = -qj0_bar(0)*x72 - qj1_bar(0)*x73; +R(7,16) = -qj0_bar(1)*x72 - qj1_bar(1)*x73; +R(7,17) = -qj0_bar(2)*x72 - qj1_bar(2)*x73; +R(7,18) = -qj0_bar(0)*x75 - qj1_bar(0)*x76; +R(7,19) = -qj0_bar(1)*x75 - qj1_bar(1)*x76; +R(7,20) = -qj0_bar(2)*x75 - qj1_bar(2)*x76; +R(7,21) = -qj0_bar(0)*x78 - qj1_bar(0)*x79; +R(7,22) = -qj0_bar(1)*x78 - qj1_bar(1)*x79; +R(7,23) = -qj0_bar(2)*x78 - qj1_bar(2)*x79; +R(8,0) = x83; +R(8,1) = x86; +R(8,2) = x89; +R(8,3) = qi0_bar(0)*x81 + qi1_bar(0)*x82; +R(8,4) = qi0_bar(1)*x81 + qi1_bar(1)*x82; +R(8,5) = qi0_bar(2)*x81 + qi1_bar(2)*x82; +R(8,6) = qi0_bar(0)*x84 + qi1_bar(0)*x85; +R(8,7) = qi0_bar(1)*x84 + qi1_bar(1)*x85; +R(8,8) = qi0_bar(2)*x84 + qi1_bar(2)*x85; +R(8,9) = qi0_bar(0)*x87 + qi1_bar(0)*x88; +R(8,10) = qi0_bar(1)*x87 + qi1_bar(1)*x88; +R(8,11) = qi0_bar(2)*x87 + qi1_bar(2)*x88; +R(8,12) = -x83; +R(8,13) = -x86; +R(8,14) = -x89; +R(8,15) = -qj0_bar(0)*x81 - qj1_bar(0)*x82; +R(8,16) = -qj0_bar(1)*x81 - qj1_bar(1)*x82; +R(8,17) = -qj0_bar(2)*x81 - qj1_bar(2)*x82; +R(8,18) = -qj0_bar(0)*x84 - qj1_bar(0)*x85; +R(8,19) = -qj0_bar(1)*x84 - qj1_bar(1)*x85; +R(8,20) = -qj0_bar(2)*x84 - qj1_bar(2)*x85; +R(8,21) = -qj0_bar(0)*x87 - qj1_bar(0)*x88; +R(8,22) = -qj0_bar(1)*x87 - qj1_bar(1)*x88; +R(8,23) = -qj0_bar(2)*x87 - qj1_bar(2)*x88; +R(9,0) = x92; +R(9,1) = x95; +R(9,2) = x98; +R(9,3) = qi0_bar(0)*x90 + qi1_bar(0)*x91; +R(9,4) = qi0_bar(1)*x90 + qi1_bar(1)*x91; +R(9,5) = qi0_bar(2)*x90 + qi1_bar(2)*x91; +R(9,6) = qi0_bar(0)*x93 + qi1_bar(0)*x94; +R(9,7) = qi0_bar(1)*x93 + qi1_bar(1)*x94; +R(9,8) = qi0_bar(2)*x93 + qi1_bar(2)*x94; +R(9,9) = qi0_bar(0)*x96 + qi1_bar(0)*x97; +R(9,10) = qi0_bar(1)*x96 + qi1_bar(1)*x97; +R(9,11) = qi0_bar(2)*x96 + qi1_bar(2)*x97; +R(9,12) = -x92; +R(9,13) = -x95; +R(9,14) = -x98; +R(9,15) = -qj0_bar(0)*x90 - qj1_bar(0)*x91; +R(9,16) = -qj0_bar(1)*x90 - qj1_bar(1)*x91; +R(9,17) = -qj0_bar(2)*x90 - qj1_bar(2)*x91; +R(9,18) = -qj0_bar(0)*x93 - qj1_bar(0)*x94; +R(9,19) = -qj0_bar(1)*x93 - qj1_bar(1)*x94; +R(9,20) = -qj0_bar(2)*x93 - qj1_bar(2)*x94; +R(9,21) = -qj0_bar(0)*x96 - qj1_bar(0)*x97; +R(9,22) = -qj0_bar(1)*x96 - qj1_bar(1)*x97; +R(9,23) = -qj0_bar(2)*x96 - qj1_bar(2)*x97; +R(10,0) = x101; +R(10,1) = x104; +R(10,2) = x107; +R(10,3) = qi0_bar(0)*x99 + qi1_bar(0)*x100; +R(10,4) = qi0_bar(1)*x99 + qi1_bar(1)*x100; +R(10,5) = qi0_bar(2)*x99 + qi1_bar(2)*x100; +R(10,6) = qi0_bar(0)*x102 + qi1_bar(0)*x103; +R(10,7) = qi0_bar(1)*x102 + qi1_bar(1)*x103; +R(10,8) = qi0_bar(2)*x102 + qi1_bar(2)*x103; +R(10,9) = qi0_bar(0)*x105 + qi1_bar(0)*x106; +R(10,10) = qi0_bar(1)*x105 + qi1_bar(1)*x106; +R(10,11) = qi0_bar(2)*x105 + qi1_bar(2)*x106; +R(10,12) = -x101; +R(10,13) = -x104; +R(10,14) = -x107; +R(10,15) = -qj0_bar(0)*x99 - qj1_bar(0)*x100; +R(10,16) = -qj0_bar(1)*x99 - qj1_bar(1)*x100; +R(10,17) = -qj0_bar(2)*x99 - qj1_bar(2)*x100; +R(10,18) = -qj0_bar(0)*x102 - qj1_bar(0)*x103; +R(10,19) = -qj0_bar(1)*x102 - qj1_bar(1)*x103; +R(10,20) = -qj0_bar(2)*x102 - qj1_bar(2)*x103; +R(10,21) = -qj0_bar(0)*x105 - qj1_bar(0)*x106; +R(10,22) = -qj0_bar(1)*x105 - qj1_bar(1)*x106; +R(10,23) = -qj0_bar(2)*x105 - qj1_bar(2)*x106; +R(11,0) = x110; +R(11,1) = x113; +R(11,2) = x116; +R(11,3) = qi0_bar(0)*x108 + qi1_bar(0)*x109; +R(11,4) = qi0_bar(1)*x108 + qi1_bar(1)*x109; +R(11,5) = qi0_bar(2)*x108 + qi1_bar(2)*x109; +R(11,6) = qi0_bar(0)*x111 + qi1_bar(0)*x112; +R(11,7) = qi0_bar(1)*x111 + qi1_bar(1)*x112; +R(11,8) = qi0_bar(2)*x111 + qi1_bar(2)*x112; +R(11,9) = qi0_bar(0)*x114 + qi1_bar(0)*x115; +R(11,10) = qi0_bar(1)*x114 + qi1_bar(1)*x115; +R(11,11) = qi0_bar(2)*x114 + qi1_bar(2)*x115; +R(11,12) = -x110; +R(11,13) = -x113; +R(11,14) = -x116; +R(11,15) = -qj0_bar(0)*x108 - qj1_bar(0)*x109; +R(11,16) = -qj0_bar(1)*x108 - qj1_bar(1)*x109; +R(11,17) = -qj0_bar(2)*x108 - qj1_bar(2)*x109; +R(11,18) = -qj0_bar(0)*x111 - qj1_bar(0)*x112; +R(11,19) = -qj0_bar(1)*x111 - qj1_bar(1)*x112; +R(11,20) = -qj0_bar(2)*x111 - qj1_bar(2)*x112; +R(11,21) = -qj0_bar(0)*x114 - qj1_bar(0)*x115; +R(11,22) = -qj0_bar(1)*x114 - qj1_bar(1)*x115; +R(11,23) = -qj0_bar(2)*x114 - qj1_bar(2)*x115; +R(12,0) = x9; +R(12,1) = x10; +R(12,2) = x11; +R(12,3) = qi0_bar(0)*x117 + qi1_bar(0)*x118; +R(12,4) = qi0_bar(1)*x117 + qi1_bar(1)*x118; +R(12,5) = qi0_bar(2)*x117 + qi1_bar(2)*x118; +R(12,6) = qi0_bar(0)*x119 + qi1_bar(0)*x120; +R(12,7) = qi0_bar(1)*x119 + qi1_bar(1)*x120; +R(12,8) = qi0_bar(2)*x119 + qi1_bar(2)*x120; +R(12,9) = qi0_bar(0)*x121 + qi1_bar(0)*x122; +R(12,10) = qi0_bar(1)*x121 + qi1_bar(1)*x122; +R(12,11) = qi0_bar(2)*x121 + qi1_bar(2)*x122; +R(12,12) = x2; +R(12,13) = x5; +R(12,14) = x8; +R(12,15) = -qj0_bar(0)*x117 - qj1_bar(0)*x118; +R(12,16) = -qj0_bar(1)*x117 - qj1_bar(1)*x118; +R(12,17) = -qj0_bar(2)*x117 - qj1_bar(2)*x118; +R(12,18) = -qj0_bar(0)*x119 - qj1_bar(0)*x120; +R(12,19) = -qj0_bar(1)*x119 - qj1_bar(1)*x120; +R(12,20) = -qj0_bar(2)*x119 - qj1_bar(2)*x120; +R(12,21) = -qj0_bar(0)*x121 - qj1_bar(0)*x122; +R(12,22) = -qj0_bar(1)*x121 - qj1_bar(1)*x122; +R(12,23) = -qj0_bar(2)*x121 - qj1_bar(2)*x122; +R(13,0) = x21; +R(13,1) = x22; +R(13,2) = x23; +R(13,3) = qi0_bar(0)*x123 + qi1_bar(0)*x124; +R(13,4) = qi0_bar(1)*x123 + qi1_bar(1)*x124; +R(13,5) = qi0_bar(2)*x123 + qi1_bar(2)*x124; +R(13,6) = qi0_bar(0)*x125 + qi1_bar(0)*x126; +R(13,7) = qi0_bar(1)*x125 + qi1_bar(1)*x126; +R(13,8) = qi0_bar(2)*x125 + qi1_bar(2)*x126; +R(13,9) = qi0_bar(0)*x127 + qi1_bar(0)*x128; +R(13,10) = qi0_bar(1)*x127 + qi1_bar(1)*x128; +R(13,11) = qi0_bar(2)*x127 + qi1_bar(2)*x128; +R(13,12) = x14; +R(13,13) = x17; +R(13,14) = x20; +R(13,15) = -qj0_bar(0)*x123 - qj1_bar(0)*x124; +R(13,16) = -qj0_bar(1)*x123 - qj1_bar(1)*x124; +R(13,17) = -qj0_bar(2)*x123 - qj1_bar(2)*x124; +R(13,18) = -qj0_bar(0)*x125 - qj1_bar(0)*x126; +R(13,19) = -qj0_bar(1)*x125 - qj1_bar(1)*x126; +R(13,20) = -qj0_bar(2)*x125 - qj1_bar(2)*x126; +R(13,21) = -qj0_bar(0)*x127 - qj1_bar(0)*x128; +R(13,22) = -qj0_bar(1)*x127 - qj1_bar(1)*x128; +R(13,23) = -qj0_bar(2)*x127 - qj1_bar(2)*x128; +R(14,0) = x33; +R(14,1) = x34; +R(14,2) = x35; +R(14,3) = qi0_bar(0)*x129 + qi1_bar(0)*x130; +R(14,4) = qi0_bar(1)*x129 + qi1_bar(1)*x130; +R(14,5) = qi0_bar(2)*x129 + qi1_bar(2)*x130; +R(14,6) = qi0_bar(0)*x131 + qi1_bar(0)*x132; +R(14,7) = qi0_bar(1)*x131 + qi1_bar(1)*x132; +R(14,8) = qi0_bar(2)*x131 + qi1_bar(2)*x132; +R(14,9) = qi0_bar(0)*x133 + qi1_bar(0)*x134; +R(14,10) = qi0_bar(1)*x133 + qi1_bar(1)*x134; +R(14,11) = qi0_bar(2)*x133 + qi1_bar(2)*x134; +R(14,12) = x26; +R(14,13) = x29; +R(14,14) = x32; +R(14,15) = -qj0_bar(0)*x129 - qj1_bar(0)*x130; +R(14,16) = -qj0_bar(1)*x129 - qj1_bar(1)*x130; +R(14,17) = -qj0_bar(2)*x129 - qj1_bar(2)*x130; +R(14,18) = -qj0_bar(0)*x131 - qj1_bar(0)*x132; +R(14,19) = -qj0_bar(1)*x131 - qj1_bar(1)*x132; +R(14,20) = -qj0_bar(2)*x131 - qj1_bar(2)*x132; +R(14,21) = -qj0_bar(0)*x133 - qj1_bar(0)*x134; +R(14,22) = -qj0_bar(1)*x133 - qj1_bar(1)*x134; +R(14,23) = -qj0_bar(2)*x133 - qj1_bar(2)*x134; +R(15,0) = -x137; +R(15,1) = -x140; +R(15,2) = -x143; +R(15,3) = qi0_bar(0)*x144 + qi1_bar(0)*x145; +R(15,4) = qi0_bar(1)*x144 + qi1_bar(1)*x145; +R(15,5) = qi0_bar(2)*x144 + qi1_bar(2)*x145; +R(15,6) = qi0_bar(0)*x146 + qi1_bar(0)*x147; +R(15,7) = qi0_bar(1)*x146 + qi1_bar(1)*x147; +R(15,8) = qi0_bar(2)*x146 + qi1_bar(2)*x147; +R(15,9) = qi0_bar(0)*x148 + qi1_bar(0)*x149; +R(15,10) = qi0_bar(1)*x148 + qi1_bar(1)*x149; +R(15,11) = qi0_bar(2)*x148 + qi1_bar(2)*x149; +R(15,12) = x137; +R(15,13) = x140; +R(15,14) = x143; +R(15,15) = -qj0_bar(0)*x144 - qj1_bar(0)*x145; +R(15,16) = -qj0_bar(1)*x144 - qj1_bar(1)*x145; +R(15,17) = -qj0_bar(2)*x144 - qj1_bar(2)*x145; +R(15,18) = -qj0_bar(0)*x146 - qj1_bar(0)*x147; +R(15,19) = -qj0_bar(1)*x146 - qj1_bar(1)*x147; +R(15,20) = -qj0_bar(2)*x146 - qj1_bar(2)*x147; +R(15,21) = -qj0_bar(0)*x148 - qj1_bar(0)*x149; +R(15,22) = -qj0_bar(1)*x148 - qj1_bar(1)*x149; +R(15,23) = -qj0_bar(2)*x148 - qj1_bar(2)*x149; +R(16,0) = -x152; +R(16,1) = -x155; +R(16,2) = -x158; +R(16,3) = qi0_bar(0)*x159 + qi1_bar(0)*x160; +R(16,4) = qi0_bar(1)*x159 + qi1_bar(1)*x160; +R(16,5) = qi0_bar(2)*x159 + qi1_bar(2)*x160; +R(16,6) = qi0_bar(0)*x161 + qi1_bar(0)*x162; +R(16,7) = qi0_bar(1)*x161 + qi1_bar(1)*x162; +R(16,8) = qi0_bar(2)*x161 + qi1_bar(2)*x162; +R(16,9) = qi0_bar(0)*x163 + qi1_bar(0)*x164; +R(16,10) = qi0_bar(1)*x163 + qi1_bar(1)*x164; +R(16,11) = qi0_bar(2)*x163 + qi1_bar(2)*x164; +R(16,12) = x152; +R(16,13) = x155; +R(16,14) = x158; +R(16,15) = -qj0_bar(0)*x159 - qj1_bar(0)*x160; +R(16,16) = -qj0_bar(1)*x159 - qj1_bar(1)*x160; +R(16,17) = -qj0_bar(2)*x159 - qj1_bar(2)*x160; +R(16,18) = -qj0_bar(0)*x161 - qj1_bar(0)*x162; +R(16,19) = -qj0_bar(1)*x161 - qj1_bar(1)*x162; +R(16,20) = -qj0_bar(2)*x161 - qj1_bar(2)*x162; +R(16,21) = -qj0_bar(0)*x163 - qj1_bar(0)*x164; +R(16,22) = -qj0_bar(1)*x163 - qj1_bar(1)*x164; +R(16,23) = -qj0_bar(2)*x163 - qj1_bar(2)*x164; +R(17,0) = -x167; +R(17,1) = -x170; +R(17,2) = -x173; +R(17,3) = qi0_bar(0)*x174 + qi1_bar(0)*x175; +R(17,4) = qi0_bar(1)*x174 + qi1_bar(1)*x175; +R(17,5) = qi0_bar(2)*x174 + qi1_bar(2)*x175; +R(17,6) = qi0_bar(0)*x176 + qi1_bar(0)*x177; +R(17,7) = qi0_bar(1)*x176 + qi1_bar(1)*x177; +R(17,8) = qi0_bar(2)*x176 + qi1_bar(2)*x177; +R(17,9) = qi0_bar(0)*x178 + qi1_bar(0)*x179; +R(17,10) = qi0_bar(1)*x178 + qi1_bar(1)*x179; +R(17,11) = qi0_bar(2)*x178 + qi1_bar(2)*x179; +R(17,12) = x167; +R(17,13) = x170; +R(17,14) = x173; +R(17,15) = -qj0_bar(0)*x174 - qj1_bar(0)*x175; +R(17,16) = -qj0_bar(1)*x174 - qj1_bar(1)*x175; +R(17,17) = -qj0_bar(2)*x174 - qj1_bar(2)*x175; +R(17,18) = -qj0_bar(0)*x176 - qj1_bar(0)*x177; +R(17,19) = -qj0_bar(1)*x176 - qj1_bar(1)*x177; +R(17,20) = -qj0_bar(2)*x176 - qj1_bar(2)*x177; +R(17,21) = -qj0_bar(0)*x178 - qj1_bar(0)*x179; +R(17,22) = -qj0_bar(1)*x178 - qj1_bar(1)*x179; +R(17,23) = -qj0_bar(2)*x178 - qj1_bar(2)*x179; +R(18,0) = -x182; +R(18,1) = -x185; +R(18,2) = -x188; +R(18,3) = qi0_bar(0)*x189 + qi1_bar(0)*x190; +R(18,4) = qi0_bar(1)*x189 + qi1_bar(1)*x190; +R(18,5) = qi0_bar(2)*x189 + qi1_bar(2)*x190; +R(18,6) = qi0_bar(0)*x191 + qi1_bar(0)*x192; +R(18,7) = qi0_bar(1)*x191 + qi1_bar(1)*x192; +R(18,8) = qi0_bar(2)*x191 + qi1_bar(2)*x192; +R(18,9) = qi0_bar(0)*x193 + qi1_bar(0)*x194; +R(18,10) = qi0_bar(1)*x193 + qi1_bar(1)*x194; +R(18,11) = qi0_bar(2)*x193 + qi1_bar(2)*x194; +R(18,12) = x182; +R(18,13) = x185; +R(18,14) = x188; +R(18,15) = -qj0_bar(0)*x189 - qj1_bar(0)*x190; +R(18,16) = -qj0_bar(1)*x189 - qj1_bar(1)*x190; +R(18,17) = -qj0_bar(2)*x189 - qj1_bar(2)*x190; +R(18,18) = -qj0_bar(0)*x191 - qj1_bar(0)*x192; +R(18,19) = -qj0_bar(1)*x191 - qj1_bar(1)*x192; +R(18,20) = -qj0_bar(2)*x191 - qj1_bar(2)*x192; +R(18,21) = -qj0_bar(0)*x193 - qj1_bar(0)*x194; +R(18,22) = -qj0_bar(1)*x193 - qj1_bar(1)*x194; +R(18,23) = -qj0_bar(2)*x193 - qj1_bar(2)*x194; +R(19,0) = -x197; +R(19,1) = -x200; +R(19,2) = -x203; +R(19,3) = qi0_bar(0)*x204 + qi1_bar(0)*x205; +R(19,4) = qi0_bar(1)*x204 + qi1_bar(1)*x205; +R(19,5) = qi0_bar(2)*x204 + qi1_bar(2)*x205; +R(19,6) = qi0_bar(0)*x206 + qi1_bar(0)*x207; +R(19,7) = qi0_bar(1)*x206 + qi1_bar(1)*x207; +R(19,8) = qi0_bar(2)*x206 + qi1_bar(2)*x207; +R(19,9) = qi0_bar(0)*x208 + qi1_bar(0)*x209; +R(19,10) = qi0_bar(1)*x208 + qi1_bar(1)*x209; +R(19,11) = qi0_bar(2)*x208 + qi1_bar(2)*x209; +R(19,12) = x197; +R(19,13) = x200; +R(19,14) = x203; +R(19,15) = -qj0_bar(0)*x204 - qj1_bar(0)*x205; +R(19,16) = -qj0_bar(1)*x204 - qj1_bar(1)*x205; +R(19,17) = -qj0_bar(2)*x204 - qj1_bar(2)*x205; +R(19,18) = -qj0_bar(0)*x206 - qj1_bar(0)*x207; +R(19,19) = -qj0_bar(1)*x206 - qj1_bar(1)*x207; +R(19,20) = -qj0_bar(2)*x206 - qj1_bar(2)*x207; +R(19,21) = -qj0_bar(0)*x208 - qj1_bar(0)*x209; +R(19,22) = -qj0_bar(1)*x208 - qj1_bar(1)*x209; +R(19,23) = -qj0_bar(2)*x208 - qj1_bar(2)*x209; +R(20,0) = -x212; +R(20,1) = -x215; +R(20,2) = -x218; +R(20,3) = qi0_bar(0)*x219 + qi1_bar(0)*x220; +R(20,4) = qi0_bar(1)*x219 + qi1_bar(1)*x220; +R(20,5) = qi0_bar(2)*x219 + qi1_bar(2)*x220; +R(20,6) = qi0_bar(0)*x221 + qi1_bar(0)*x222; +R(20,7) = qi0_bar(1)*x221 + qi1_bar(1)*x222; +R(20,8) = qi0_bar(2)*x221 + qi1_bar(2)*x222; +R(20,9) = qi0_bar(0)*x223 + qi1_bar(0)*x224; +R(20,10) = qi0_bar(1)*x223 + qi1_bar(1)*x224; +R(20,11) = qi0_bar(2)*x223 + qi1_bar(2)*x224; +R(20,12) = x212; +R(20,13) = x215; +R(20,14) = x218; +R(20,15) = -qj0_bar(0)*x219 - qj1_bar(0)*x220; +R(20,16) = -qj0_bar(1)*x219 - qj1_bar(1)*x220; +R(20,17) = -qj0_bar(2)*x219 - qj1_bar(2)*x220; +R(20,18) = -qj0_bar(0)*x221 - qj1_bar(0)*x222; +R(20,19) = -qj0_bar(1)*x221 - qj1_bar(1)*x222; +R(20,20) = -qj0_bar(2)*x221 - qj1_bar(2)*x222; +R(20,21) = -qj0_bar(0)*x223 - qj1_bar(0)*x224; +R(20,22) = -qj0_bar(1)*x223 - qj1_bar(1)*x224; +R(20,23) = -qj0_bar(2)*x223 - qj1_bar(2)*x224; +R(21,0) = -x227; +R(21,1) = -x230; +R(21,2) = -x233; +R(21,3) = qi0_bar(0)*x234 + qi1_bar(0)*x235; +R(21,4) = qi0_bar(1)*x234 + qi1_bar(1)*x235; +R(21,5) = qi0_bar(2)*x234 + qi1_bar(2)*x235; +R(21,6) = qi0_bar(0)*x236 + qi1_bar(0)*x237; +R(21,7) = qi0_bar(1)*x236 + qi1_bar(1)*x237; +R(21,8) = qi0_bar(2)*x236 + qi1_bar(2)*x237; +R(21,9) = qi0_bar(0)*x238 + qi1_bar(0)*x239; +R(21,10) = qi0_bar(1)*x238 + qi1_bar(1)*x239; +R(21,11) = qi0_bar(2)*x238 + qi1_bar(2)*x239; +R(21,12) = x227; +R(21,13) = x230; +R(21,14) = x233; +R(21,15) = -qj0_bar(0)*x234 - qj1_bar(0)*x235; +R(21,16) = -qj0_bar(1)*x234 - qj1_bar(1)*x235; +R(21,17) = -qj0_bar(2)*x234 - qj1_bar(2)*x235; +R(21,18) = -qj0_bar(0)*x236 - qj1_bar(0)*x237; +R(21,19) = -qj0_bar(1)*x236 - qj1_bar(1)*x237; +R(21,20) = -qj0_bar(2)*x236 - qj1_bar(2)*x237; +R(21,21) = -qj0_bar(0)*x238 - qj1_bar(0)*x239; +R(21,22) = -qj0_bar(1)*x238 - qj1_bar(1)*x239; +R(21,23) = -qj0_bar(2)*x238 - qj1_bar(2)*x239; +R(22,0) = -x242; +R(22,1) = -x245; +R(22,2) = -x248; +R(22,3) = qi0_bar(0)*x249 + qi1_bar(0)*x250; +R(22,4) = qi0_bar(1)*x249 + qi1_bar(1)*x250; +R(22,5) = qi0_bar(2)*x249 + qi1_bar(2)*x250; +R(22,6) = qi0_bar(0)*x251 + qi1_bar(0)*x252; +R(22,7) = qi0_bar(1)*x251 + qi1_bar(1)*x252; +R(22,8) = qi0_bar(2)*x251 + qi1_bar(2)*x252; +R(22,9) = qi0_bar(0)*x253 + qi1_bar(0)*x254; +R(22,10) = qi0_bar(1)*x253 + qi1_bar(1)*x254; +R(22,11) = qi0_bar(2)*x253 + qi1_bar(2)*x254; +R(22,12) = x242; +R(22,13) = x245; +R(22,14) = x248; +R(22,15) = -qj0_bar(0)*x249 - qj1_bar(0)*x250; +R(22,16) = -qj0_bar(1)*x249 - qj1_bar(1)*x250; +R(22,17) = -qj0_bar(2)*x249 - qj1_bar(2)*x250; +R(22,18) = -qj0_bar(0)*x251 - qj1_bar(0)*x252; +R(22,19) = -qj0_bar(1)*x251 - qj1_bar(1)*x252; +R(22,20) = -qj0_bar(2)*x251 - qj1_bar(2)*x252; +R(22,21) = -qj0_bar(0)*x253 - qj1_bar(0)*x254; +R(22,22) = -qj0_bar(1)*x253 - qj1_bar(1)*x254; +R(22,23) = -qj0_bar(2)*x253 - qj1_bar(2)*x254; +R(23,0) = -x257; +R(23,1) = -x260; +R(23,2) = -x263; +R(23,3) = qi0_bar(0)*x264 + qi1_bar(0)*x265; +R(23,4) = qi0_bar(1)*x264 + qi1_bar(1)*x265; +R(23,5) = qi0_bar(2)*x264 + qi1_bar(2)*x265; +R(23,6) = qi0_bar(0)*x266 + qi1_bar(0)*x267; +R(23,7) = qi0_bar(1)*x266 + qi1_bar(1)*x267; +R(23,8) = qi0_bar(2)*x266 + qi1_bar(2)*x267; +R(23,9) = qi0_bar(0)*x268 + qi1_bar(0)*x269; +R(23,10) = qi0_bar(1)*x268 + qi1_bar(1)*x269; +R(23,11) = qi0_bar(2)*x268 + qi1_bar(2)*x269; +R(23,12) = x257; +R(23,13) = x260; +R(23,14) = x263; +R(23,15) = -qj0_bar(0)*x264 - qj1_bar(0)*x265; +R(23,16) = -qj0_bar(1)*x264 - qj1_bar(1)*x265; +R(23,17) = -qj0_bar(2)*x264 - qj1_bar(2)*x265; +R(23,18) = -qj0_bar(0)*x266 - qj1_bar(0)*x267; +R(23,19) = -qj0_bar(1)*x266 - qj1_bar(1)*x267; +R(23,20) = -qj0_bar(2)*x266 - qj1_bar(2)*x267; +R(23,21) = -qj0_bar(0)*x268 - qj1_bar(0)*x269; +R(23,22) = -qj0_bar(1)*x268 - qj1_bar(1)*x269; +R(23,23) = -qj0_bar(2)*x268 - qj1_bar(2)*x269; +} + // Revolute Joint Energy and Derivatives + +template +__host__ __device__ void Eaxis(T& R, const T& kappa, const Eigen::Vector& Faxis) +{ +/***************************************************************************************************************************** +Function generated by SymEigen.py +Author: MuGdxy +GitHub: https://github.com/MuGdxy/SymEigen +E-Mail: lxy819469559@gmail.com +****************************************************************************************************************************** Symbol Name Mapping: kappa: -> {} -> Matrix([[kappa]]) -x0_bar: +Faxis: + -> {} + -> Matrix([[Faxis(0)], [Faxis(1)], [Faxis(2)], [Faxis(3)], [Faxis(4)], [Faxis(5)]]) +*****************************************************************************************************************************/ +/* Sub Exprs */ +/* Simplified Expr */ +R = 0.5*kappa*(std::pow(Faxis(0), 2) + std::pow(Faxis(1), 2) + std::pow(Faxis(2), 2) + std::pow(Faxis(3), 2) + std::pow(Faxis(4), 2) + std::pow(Faxis(5), 2)); +} +template +__host__ __device__ void dEaxisdFaxis(Eigen::Vector& R, const T& kappa, const Eigen::Vector& Faxis) +{ +/***************************************************************************************************************************** +Function generated by SymEigen.py +Author: MuGdxy +GitHub: https://github.com/MuGdxy/SymEigen +E-Mail: lxy819469559@gmail.com +****************************************************************************************************************************** +Symbol Name Mapping: +kappa: -> {} - -> Matrix([[x0_bar(0)], [x0_bar(1)], [x0_bar(2)]]) -x1_bar: + -> Matrix([[kappa]]) +Faxis: -> {} - -> Matrix([[x1_bar(0)], [x1_bar(1)], [x1_bar(2)]]) -x2_bar: + -> Matrix([[Faxis(0)], [Faxis(1)], [Faxis(2)], [Faxis(3)], [Faxis(4)], [Faxis(5)]]) +*****************************************************************************************************************************/ +/* Sub Exprs */ +auto x0 = 1.0*kappa; +/* Simplified Expr */ +R(0) = Faxis(0)*x0; +R(1) = Faxis(1)*x0; +R(2) = Faxis(2)*x0; +R(3) = Faxis(3)*x0; +R(4) = Faxis(4)*x0; +R(5) = Faxis(5)*x0; +} +template +__host__ __device__ void ddEaxisddFaxis(Eigen::Matrix& R, const T& kappa, const Eigen::Vector& Faxis) +{ +/***************************************************************************************************************************** +Function generated by SymEigen.py +Author: MuGdxy +GitHub: https://github.com/MuGdxy/SymEigen +E-Mail: lxy819469559@gmail.com +****************************************************************************************************************************** +Symbol Name Mapping: +kappa: -> {} - -> Matrix([[x2_bar(0)], [x2_bar(1)], [x2_bar(2)]]) -x3_bar: + -> Matrix([[kappa]]) +Faxis: -> {} - -> Matrix([[x3_bar(0)], [x3_bar(1)], [x3_bar(2)]]) + -> Matrix([[Faxis(0)], [Faxis(1)], [Faxis(2)], [Faxis(3)], [Faxis(4)], [Faxis(5)]]) *****************************************************************************************************************************/ /* Sub Exprs */ -auto x0 = 2*kappa; -auto x1 = kappa*(x1_bar(0) + x3_bar(0)); -auto x2 = kappa*(x1_bar(1) + x3_bar(1)); -auto x3 = kappa*(x1_bar(2) + x3_bar(2)); -auto x4 = kappa*(x0_bar(0) + x2_bar(0)); -auto x5 = kappa*(x0_bar(0)*x1_bar(0) + x2_bar(0)*x3_bar(0)); -auto x6 = kappa*(x0_bar(0)*x1_bar(1) + x2_bar(0)*x3_bar(1)); -auto x7 = kappa*(x0_bar(0)*x1_bar(2) + x2_bar(0)*x3_bar(2)); -auto x8 = kappa*(x0_bar(1) + x2_bar(1)); -auto x9 = kappa*(x0_bar(1)*x1_bar(0) + x2_bar(1)*x3_bar(0)); -auto x10 = kappa*(x0_bar(1)*x1_bar(1) + x2_bar(1)*x3_bar(1)); -auto x11 = kappa*(x0_bar(1)*x1_bar(2) + x2_bar(1)*x3_bar(2)); -auto x12 = kappa*(x0_bar(2) + x2_bar(2)); -auto x13 = kappa*(x0_bar(2)*x1_bar(0) + x2_bar(2)*x3_bar(0)); -auto x14 = kappa*(x0_bar(2)*x1_bar(1) + x2_bar(2)*x3_bar(1)); -auto x15 = kappa*(x0_bar(2)*x1_bar(2) + x2_bar(2)*x3_bar(2)); +auto x0 = 1.0*kappa; /* Simplified Expr */ R(0,0) = x0; R(0,1) = 0; R(0,2) = 0; -R(0,3) = x1; -R(0,4) = x2; -R(0,5) = x3; -R(0,6) = 0; -R(0,7) = 0; -R(0,8) = 0; -R(0,9) = 0; -R(0,10) = 0; -R(0,11) = 0; +R(0,3) = 0; +R(0,4) = 0; +R(0,5) = 0; R(1,0) = 0; R(1,1) = x0; R(1,2) = 0; R(1,3) = 0; R(1,4) = 0; R(1,5) = 0; -R(1,6) = x1; -R(1,7) = x2; -R(1,8) = x3; -R(1,9) = 0; -R(1,10) = 0; -R(1,11) = 0; R(2,0) = 0; R(2,1) = 0; R(2,2) = x0; R(2,3) = 0; R(2,4) = 0; R(2,5) = 0; -R(2,6) = 0; -R(2,7) = 0; -R(2,8) = 0; -R(2,9) = x1; -R(2,10) = x2; -R(2,11) = x3; -R(3,0) = x4; +R(3,0) = 0; R(3,1) = 0; R(3,2) = 0; -R(3,3) = x5; -R(3,4) = x6; -R(3,5) = x7; -R(3,6) = 0; -R(3,7) = 0; -R(3,8) = 0; -R(3,9) = 0; -R(3,10) = 0; -R(3,11) = 0; -R(4,0) = x8; +R(3,3) = x0; +R(3,4) = 0; +R(3,5) = 0; +R(4,0) = 0; R(4,1) = 0; R(4,2) = 0; -R(4,3) = x9; -R(4,4) = x10; -R(4,5) = x11; -R(4,6) = 0; -R(4,7) = 0; -R(4,8) = 0; -R(4,9) = 0; -R(4,10) = 0; -R(4,11) = 0; -R(5,0) = x12; +R(4,3) = 0; +R(4,4) = x0; +R(4,5) = 0; +R(5,0) = 0; R(5,1) = 0; R(5,2) = 0; -R(5,3) = x13; -R(5,4) = x14; -R(5,5) = x15; -R(5,6) = 0; -R(5,7) = 0; -R(5,8) = 0; -R(5,9) = 0; -R(5,10) = 0; -R(5,11) = 0; -R(6,0) = 0; -R(6,1) = x4; -R(6,2) = 0; -R(6,3) = 0; -R(6,4) = 0; -R(6,5) = 0; -R(6,6) = x5; -R(6,7) = x6; -R(6,8) = x7; -R(6,9) = 0; -R(6,10) = 0; -R(6,11) = 0; -R(7,0) = 0; -R(7,1) = x8; -R(7,2) = 0; -R(7,3) = 0; -R(7,4) = 0; -R(7,5) = 0; -R(7,6) = x9; -R(7,7) = x10; -R(7,8) = x11; -R(7,9) = 0; -R(7,10) = 0; -R(7,11) = 0; -R(8,0) = 0; -R(8,1) = x12; -R(8,2) = 0; -R(8,3) = 0; -R(8,4) = 0; -R(8,5) = 0; -R(8,6) = x13; -R(8,7) = x14; -R(8,8) = x15; -R(8,9) = 0; -R(8,10) = 0; -R(8,11) = 0; -R(9,0) = 0; -R(9,1) = 0; -R(9,2) = x4; -R(9,3) = 0; -R(9,4) = 0; -R(9,5) = 0; -R(9,6) = 0; -R(9,7) = 0; -R(9,8) = 0; -R(9,9) = x5; -R(9,10) = x6; -R(9,11) = x7; -R(10,0) = 0; -R(10,1) = 0; -R(10,2) = x8; -R(10,3) = 0; -R(10,4) = 0; -R(10,5) = 0; -R(10,6) = 0; -R(10,7) = 0; -R(10,8) = 0; -R(10,9) = x9; -R(10,10) = x10; -R(10,11) = x11; -R(11,0) = 0; -R(11,1) = 0; -R(11,2) = x12; -R(11,3) = 0; -R(11,4) = 0; -R(11,5) = 0; -R(11,6) = 0; -R(11,7) = 0; -R(11,8) = 0; -R(11,9) = x13; -R(11,10) = x14; -R(11,11) = x15; +R(5,3) = 0; +R(5,4) = 0; +R(5,5) = x0; } + +// -------------------------------------------------------------------------