From 7bb20d10a9b370b93b0bd5e560e743ad552e1917 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Mon, 23 Feb 2026 13:55:52 -0600 Subject: [PATCH 1/9] collision checking: add functions to return l2 distance between sphere and other primitives, and compute the signed distance between spheres and the environments --- src/impl/vamp/collision/sphere_capsule.hh | 23 ++ src/impl/vamp/collision/sphere_cuboid.hh | 31 ++ src/impl/vamp/collision/sphere_heightfield.hh | 11 + src/impl/vamp/collision/validity.hh | 354 ++++++++++++++++++ 4 files changed, 419 insertions(+) diff --git a/src/impl/vamp/collision/sphere_capsule.hh b/src/impl/vamp/collision/sphere_capsule.hh index 7ee241ca..c5927ed8 100644 --- a/src/impl/vamp/collision/sphere_capsule.hh +++ b/src/impl/vamp/collision/sphere_capsule.hh @@ -27,6 +27,17 @@ namespace vamp::collision return sphere_capsule(c, s.x, s.y, s.z, s.r); } + template + inline constexpr auto sphere_capsule_l2(const Capsule &c, const Sphere &s) noexcept -> DataT + { + auto dot = dot_3(s.x - c.x1, s.y - c.y1, s.z - c.z1, c.xv, c.yv, c.zv); + auto cdf = (dot * c.rdv).clamp(0.F, 1.F); + + auto sum = sql2_3(s.x, s.y, s.z, c.x1 + c.xv * cdf, c.y1 + c.yv * cdf, c.z1 + c.zv * cdf).sqrt(); + auto rs = s.r + c.r; + return sum - rs; + } + template inline constexpr auto sphere_z_aligned_capsule( const Capsule &c, @@ -49,4 +60,16 @@ namespace vamp::collision { return sphere_z_aligned_capsule(c, s.x, s.y, s.z, s.r); } + + template + inline constexpr auto sphere_z_aligned_capsule_l2(const Capsule &c, const Sphere &s) noexcept + -> DataT + { + auto dot = (s.z - c.z1) * c.zv; + auto cdf = (dot * c.rdv).clamp(0.F, 1.F); + + auto sum = sql2_3(s.x, s.y, s.z, c.x1, c.y1, c.z1 + c.zv * cdf).sqrt(); + auto rs = s.r + c.r; + return sum - rs; + } } // namespace vamp::collision diff --git a/src/impl/vamp/collision/sphere_cuboid.hh b/src/impl/vamp/collision/sphere_cuboid.hh index 0ba6a045..9d608a86 100644 --- a/src/impl/vamp/collision/sphere_cuboid.hh +++ b/src/impl/vamp/collision/sphere_cuboid.hh @@ -31,6 +31,21 @@ namespace vamp::collision return sphere_cuboid(c, s.x, s.y, s.z, s.r * s.r); } + template + inline constexpr auto sphere_cuboid_l2(const Cuboid &c, const Sphere &s) noexcept -> DataT + { + auto xs = s.x - c.x; + auto ys = s.y - c.y; + auto zs = s.z - c.z; + + auto a1 = (dot_3(c.axis_1_x, c.axis_1_y, c.axis_1_z, xs, ys, zs).abs() - c.axis_1_r).max(0.); + auto a2 = (dot_3(c.axis_2_x, c.axis_2_y, c.axis_2_z, xs, ys, zs).abs() - c.axis_2_r).max(0.); + auto a3 = (dot_3(c.axis_3_x, c.axis_3_y, c.axis_3_z, xs, ys, zs).abs() - c.axis_3_r).max(0.); + + auto sum = dot_3(a1, a2, a3, a1, a2, a3).sqrt(); + return sum - s.r; + } + template inline constexpr auto sphere_z_aligned_cuboid( const Cuboid &c, @@ -57,4 +72,20 @@ namespace vamp::collision { return sphere_z_aligned_cuboid(c, s.x, s.y, s.z, s.r * s.r); } + + template + inline constexpr auto sphere_z_aligned_cuboid_l2(const Cuboid &c, const Sphere &s) noexcept + -> DataT + { + auto xs = s.x - c.x; + auto ys = s.y - c.y; + auto zs = s.z - c.z; + + auto a1 = (dot_2(c.axis_1_x, c.axis_1_y, xs, ys).abs() - c.axis_1_r).max(0.); + auto a2 = (dot_2(c.axis_2_x, c.axis_2_y, xs, ys).abs() - c.axis_2_r).max(0.); + auto a3 = (zs.abs() - c.axis_3_r).max(0.); + + auto sum = dot_3(a1, a2, a3, a1, a2, a3).sqrt(); + return sum - s.r; + } } // namespace vamp::collision diff --git a/src/impl/vamp/collision/sphere_heightfield.hh b/src/impl/vamp/collision/sphere_heightfield.hh index 1b3a5470..3b3f2684 100644 --- a/src/impl/vamp/collision/sphere_heightfield.hh +++ b/src/impl/vamp/collision/sphere_heightfield.hh @@ -28,4 +28,15 @@ namespace vamp::collision return z - r - zhs; } + + template + inline constexpr auto sphere_heightfield_l2( + const HeightField &a, + const DataT &x, + const DataT &y, + const DataT &z, + const DataT &r) noexcept -> DataT + { + return sphere_heightfield(a, x, y, z, r); + } } // namespace vamp::collision diff --git a/src/impl/vamp/collision/validity.hh b/src/impl/vamp/collision/validity.hh index b555c13e..7ad1b7d8 100644 --- a/src/impl/vamp/collision/validity.hh +++ b/src/impl/vamp/collision/validity.hh @@ -149,6 +149,360 @@ namespace vamp return false; } + template + inline constexpr auto sphere_environment_signed_distance( + const collision::Environment &e, // + ArgT1 sx_, + ArgT2 sy_, + ArgT3 sz_, + ArgT4 sr_) noexcept -> DataT + { + // TODO: Figure out a way to avoid needing to upcast floats to vectors + auto sx = static_cast(sx_); + auto sy = static_cast(sy_); + auto sz = static_cast(sz_); + auto sr = static_cast(sr_); + + const auto s = collision::Sphere(sx, sy, sz, sr); + + auto min_dist = DataT(1000.0f); // Large value + + for (const auto &es : e.spheres) + { + auto dist = collision::sphere_sphere_l2(es, s); + min_dist = min_dist.blend(dist, dist < min_dist); + } + + for (const auto &ec : e.capsules) + { + auto dist = collision::sphere_capsule_l2(ec, s); + min_dist = min_dist.blend(dist, dist < min_dist); + } + + for (const auto &ec : e.z_aligned_capsules) + { + auto dist = collision::sphere_z_aligned_capsule_l2(ec, s); + min_dist = min_dist.blend(dist, dist < min_dist); + } + + for (const auto &ec : e.cuboids) + { + auto dist = collision::sphere_cuboid_l2(ec, s); + min_dist = min_dist.blend(dist, dist < min_dist); + } + + for (const auto &ec : e.z_aligned_cuboids) + { + auto dist = collision::sphere_z_aligned_cuboid_l2(ec, s); + min_dist = min_dist.blend(dist, dist < min_dist); + } + + for (const auto &eh : e.heightfields) + { + auto dist = collision::sphere_heightfield_l2(eh, sx, sy, sz, sr); + min_dist = min_dist.blend(dist, dist < min_dist); + } + + // Skip pointclouds for now as SDF is not supported + + return min_dist; + } + + template + inline constexpr auto sphere_environment_signed_distance_and_gradient( + const collision::Environment &e, // + ArgT1 sx_, + ArgT2 sy_, + ArgT3 sz_, + ArgT4 sr_) noexcept -> std::pair> + { + auto sx = static_cast(sx_); + auto sy = static_cast(sy_); + auto sz = static_cast(sz_); + auto sr = static_cast(sr_); + + const auto s = collision::Sphere(sx, sy, sz, sr); + + auto min_dist = DataT(1000.0f); + auto gx = DataT(0.0f); + auto gy = DataT(0.0f); + auto gz = DataT(0.0f); + + auto update = [&](const DataT& dist, const DataT& nx, const DataT& ny, const DataT& nz) { + auto mask = dist < min_dist; + min_dist = min_dist.blend(dist, mask); + gx = gx.blend(nx, mask); + gy = gy.blend(ny, mask); + gz = gz.blend(nz, mask); + }; + + auto normalize_and_update = [&](const DataT& dist, const DataT& dx, const DataT& dy, const DataT& dz) { + // Normalized gradient vector pointing FROM obstacle TO sphere center + // dist = ||d|| - (r1 + r2) + // grad = d / ||d|| + auto len = collision::sqrt(collision::dot_3(dx, dy, dz, dx, dy, dz)); + // Handle zero length case to avoid NaN? Usually implies penetration or center overlap. + // For now assume non-zero. + auto inv_len = DataT(1.0f) / len; + update(dist, dx * inv_len, dy * inv_len, dz * inv_len); + }; + + // Spheres + for (const auto &es : e.spheres) + { + // Vectors: s (query) - es (obstacle) + auto dx = s.x - es.x; + auto dy = s.y - es.y; + auto dz = s.z - es.z; + auto dist_sq = collision::dot_3(dx, dy, dz, dx, dy, dz); + + // Pruning: if dist_sq >= (min_dist + s.r + es.r)^2, we can skip. + // We use a conservative check for all lanes. + // Note: min_dist decreases, so we start eagerly. + auto combined_r = s.r + es.r; + auto threshold = min_dist + combined_r; + + // Check if we can safely skip this sphere for all lanes + // Skip if all: dist_sq >= threshold^2 AND threshold > 0 + // We use blend/mask logic or just operate. + // Since "all()" might be expensive or not available, we can just proceed to calculate dist + // but defer gradient. + + // However, strictly calculating sqrt is what we want to avoid. + // Let's rely on the mask after distance calc to skip gradient, + // which is the main cost besides sqrt. + + auto len = collision::sqrt(dist_sq); + auto dist = len - combined_r; + + // Check if ANY lane improves min_dist + auto mask = dist < min_dist; + if (mask.any()) + { + auto inv_len = DataT(1.0f) / len; + update(dist, dx * inv_len, dy * inv_len, dz * inv_len); + } + } + + // Capsules + for (const auto &ec : e.capsules) + { + // From sphere_capsule_l2 + auto dot = collision::dot_3(s.x - ec.x1, s.y - ec.y1, s.z - ec.z1, ec.xv, ec.yv, ec.zv); + auto cdf = (dot * ec.rdv).clamp(0.F, 1.F); + + // Closest point on capsule segment + auto cx = ec.x1 + ec.xv * cdf; + auto cy = ec.y1 + ec.yv * cdf; + auto cz = ec.z1 + ec.zv * cdf; + + auto dx = s.x - cx; + auto dy = s.y - cy; + auto dz = s.z - cz; + + auto dist_sq = collision::dot_3(dx, dy, dz, dx, dy, dz); + + auto combined_r = s.r + ec.r; + // Pruning could happen here based on dist_sq but we already did some math. + // The segment projection is cheap enough. Sqrt is the cost. + + auto len = collision::sqrt(dist_sq); + auto dist = len - combined_r; + + auto mask = dist < min_dist; + if (mask.any()) + { + auto inv_len = DataT(1.0f) / len; + update(dist, dx * inv_len, dy * inv_len, dz * inv_len); + } + } + + // Z-Aligned Capsules + for (const auto &ec : e.z_aligned_capsules) + { + auto dot = (s.z - ec.z1) * ec.zv; + auto cdf = (dot * ec.rdv).clamp(0.F, 1.F); + + auto cx = ec.x1; + auto cy = ec.y1; + auto cz = ec.z1 + ec.zv * cdf; + + auto dx = s.x - cx; + auto dy = s.y - cy; + auto dz = s.z - cz; + + auto dist_sq = collision::dot_3(dx, dy, dz, dx, dy, dz); + auto combined_r = s.r + ec.r; + + auto len = collision::sqrt(dist_sq); + auto dist = len - combined_r; + + auto mask = dist < min_dist; + if (mask.any()) + { + auto inv_len = DataT(1.0f) / len; + update(dist, dx * inv_len, dy * inv_len, dz * inv_len); + } + } + + // Cuboids + for (const auto &ec : e.cuboids) + { + auto xs = s.x - ec.x; + auto ys = s.y - ec.y; + auto zs = s.z - ec.z; + + // Bounding Sphere Pruning + // Conservative radius of cuboid = length of half-diagonal + auto diag_sq = ec.axis_1_r * ec.axis_1_r + ec.axis_2_r * ec.axis_2_r + ec.axis_3_r * ec.axis_3_r; + auto R = collision::sqrt(diag_sq); + auto center_dist_sq = collision::dot_3(xs, ys, zs, xs, ys, zs); + auto thresh = min_dist + s.r + R; + + // If strictly outside bounding sphere for all lanes, skip. + // Condition: center_dist > thresh. (Using squared check) + // Safety: thresh must be positive. min_dist usually > 0. + auto skip_mask = (center_dist_sq >= thresh * thresh) & (thresh > 0.0f); + if (skip_mask.all()) + { + continue; + } + + // Project query point onto local axes + auto p1 = collision::dot_3(ec.axis_1_x, ec.axis_1_y, ec.axis_1_z, xs, ys, zs); + auto p2 = collision::dot_3(ec.axis_2_x, ec.axis_2_y, ec.axis_2_z, xs, ys, zs); + auto p3 = collision::dot_3(ec.axis_3_x, ec.axis_3_y, ec.axis_3_z, xs, ys, zs); + + auto a1 = (p1.abs() - ec.axis_1_r).max(0.); + auto a2 = (p2.abs() - ec.axis_2_r).max(0.); + auto a3 = (p3.abs() - ec.axis_3_r).max(0.); + + auto sum = collision::dot_3(a1, a2, a3, a1, a2, a3).sqrt(); + auto dist = sum - s.r; + + auto mask = dist < min_dist; + if (mask.any()) + { + // Gradient in local frame (unnormalized) + // grad_v = sum( 2 * ai * grad(ai) ) + // grad(ai) = sign(pi) * axis_i if ai > 0 else 0 + // The gradient direction of sqrt(v) is same as v. + // So we want vector: sum( ai * sign(pi) * axis_i ) + + // Sign of projections + auto s1 = DataT(-1.0f).blend(DataT(1.0f), p1 >= 0.0f); + auto s2 = DataT(-1.0f).blend(DataT(1.0f), p2 >= 0.0f); + auto s3 = DataT(-1.0f).blend(DataT(1.0f), p3 >= 0.0f); + + // Components. Only needed if mask is true? + // We compute for vector width, masked writes happen in update. + // We can save some ops if we really wanted to blend 0, but scalar broadcast is cheap. + + auto c1 = a1 * s1; + auto c2 = a2 * s2; + auto c3 = a3 * s3; + + // Transform back to world + auto gx_local = c1 * ec.axis_1_x + c2 * ec.axis_2_x + c3 * ec.axis_3_x; + auto gy_local = c1 * ec.axis_1_y + c2 * ec.axis_2_y + c3 * ec.axis_3_y; + auto gz_local = c1 * ec.axis_1_z + c2 * ec.axis_2_z + c3 * ec.axis_3_z; + + // Normalize + auto g_len = collision::sqrt(collision::dot_3(gx_local, gy_local, gz_local, gx_local, gy_local, gz_local)); + + // Beware divide by zero. + auto non_zero = g_len > 0.0001f; + auto inv_g_len = DataT(1.0f) / g_len; + + auto nx = gx_local * inv_g_len; + auto ny = gy_local * inv_g_len; + auto nz = gz_local * inv_g_len; + + // Blend zero if length is small + nx = DataT(0.0f).blend(nx, non_zero); + ny = DataT(0.0f).blend(ny, non_zero); + nz = DataT(0.0f).blend(nz, non_zero); + + update(dist, nx, ny, nz); + } + } + + for (const auto &ec : e.z_aligned_cuboids) + { + auto xs = s.x - ec.x; + auto ys = s.y - ec.y; + auto zs = s.z - ec.z; // axis 3 is Z + + // Bounding Sphere Pruning + auto diag_sq = ec.axis_1_r * ec.axis_1_r + ec.axis_2_r * ec.axis_2_r + ec.axis_3_r * ec.axis_3_r; + auto R = collision::sqrt(diag_sq); + auto center_dist_sq = collision::dot_3(xs, ys, zs, xs, ys, zs); + auto thresh = min_dist + s.r + R; + + auto skip_mask = (center_dist_sq >= thresh * thresh) & (thresh > 0.0f); + if (skip_mask.all()) + { + continue; + } + + // Project + auto p1 = collision::dot_2(ec.axis_1_x, ec.axis_1_y, xs, ys); + auto p2 = collision::dot_2(ec.axis_2_x, ec.axis_2_y, xs, ys); + auto p3 = zs; + + auto a1 = (p1.abs() - ec.axis_1_r).max(0.); + auto a2 = (p2.abs() - ec.axis_2_r).max(0.); + auto a3 = (p3.abs() - ec.axis_3_r).max(0.); + + auto sum = collision::dot_3(a1, a2, a3, a1, a2, a3).sqrt(); + auto dist = sum - s.r; + + auto mask = dist < min_dist; + if (mask.any()) + { + auto s1 = DataT(-1.0f).blend(DataT(1.0f), p1 >= 0.0f); + auto s2 = DataT(-1.0f).blend(DataT(1.0f), p2 >= 0.0f); + auto s3 = DataT(-1.0f).blend(DataT(1.0f), p3 >= 0.0f); + + auto c1 = a1 * s1; + auto c2 = a2 * s2; + auto c3 = a3 * s3; + + // box axes: axis1, axis2, (0,0,1) + auto gx_local = c1 * ec.axis_1_x + c2 * ec.axis_2_x; + auto gy_local = c1 * ec.axis_1_y + c2 * ec.axis_2_y; + auto gz_local = c3; + + auto g_len = collision::sqrt(collision::dot_3(gx_local, gy_local, gz_local, gx_local, gy_local, gz_local)); + auto non_zero = g_len > 0.0001f; + auto inv_g_len = DataT(1.0f) / g_len; + + auto nx = DataT(0.0f).blend(gx_local * inv_g_len, non_zero); + auto ny = DataT(0.0f).blend(gy_local * inv_g_len, non_zero); + auto nz = DataT(0.0f).blend(gz_local * inv_g_len, non_zero); + + update(dist, nx, ny, nz); + } + } + + // Heightfields - gradient approx up (0,0,1) for now? + // Or analytical? Heightfield is specialized. + // For now use (0,0,1) as a placeholder if active, or just skip grad update? + // Let's implement basic gradient logic later if needed. + // For now just update distance (gradient 0) + for (const auto &eh : e.heightfields) + { + auto dist = collision::sphere_heightfield_l2(eh, sx, sy, sz, sr); + auto mask = dist < min_dist; + if (mask.any()) + { + update(dist, DataT(0.0f), DataT(0.0f), DataT(1.0f)); // dummy gradient up + } + } + + return {min_dist, {gx, gy, gz}}; + } + template inline auto sphere_environment_get_collisions( const collision::Environment &e, // From c42898eef1edc3441e63e4fa83aa58e9198ae78e Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Mon, 23 Feb 2026 14:45:31 -0600 Subject: [PATCH 2/9] sdf: add `sdf_gradient` and `d_collision_d_q` to the robots --- src/impl/vamp/robots/baxter.hh | 4245 ++++++++++++++++++++++++++++++-- src/impl/vamp/robots/fetch.hh | 3303 +++++++++++++++++++++++-- src/impl/vamp/robots/panda.hh | 2680 +++++++++++++++++++- src/impl/vamp/robots/sphere.hh | 20 + src/impl/vamp/robots/ur5.hh | 1778 ++++++++++++- 5 files changed, 11471 insertions(+), 555 deletions(-) diff --git a/src/impl/vamp/robots/baxter.hh b/src/impl/vamp/robots/baxter.hh index 5e9476ca..16b496ce 100644 --- a/src/impl/vamp/robots/baxter.hh +++ b/src/impl/vamp/robots/baxter.hh @@ -1433,205 +1433,155 @@ namespace vamp::robots output.first.emplace_back( sphere_environment_get_collisions(environment, y[96], y[97], y[98], y[99])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[100], y[101], y[102], y[103])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[100], y[101], y[102], y[103])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[104], y[105], y[106], y[107])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[104], y[105], y[106], y[107])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[108], y[109], y[110], y[111])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[108], y[109], y[110], y[111])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[112], y[113], y[114], y[115])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[112], y[113], y[114], y[115])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[116], y[117], y[118], y[119])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[116], y[117], y[118], y[119])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[120], y[121], y[122], y[123])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[120], y[121], y[122], y[123])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[124], y[125], y[126], y[127])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[124], y[125], y[126], y[127])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[128], y[129], y[130], y[131])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[128], y[129], y[130], y[131])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[132], y[133], y[134], y[135])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[132], y[133], y[134], y[135])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[136], y[137], y[138], y[139])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[136], y[137], y[138], y[139])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[140], y[141], y[142], y[143])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[140], y[141], y[142], y[143])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[144], y[145], y[146], y[147])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[144], y[145], y[146], y[147])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[148], y[149], y[150], y[151])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[148], y[149], y[150], y[151])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[152], y[153], y[154], y[155])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[152], y[153], y[154], y[155])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[156], y[157], y[158], y[159])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[156], y[157], y[158], y[159])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[160], y[161], y[162], y[163])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[160], y[161], y[162], y[163])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[164], y[165], y[166], y[167])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[164], y[165], y[166], y[167])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[168], y[169], y[170], y[171])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[168], y[169], y[170], y[171])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[172], y[173], y[174], y[175])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[172], y[173], y[174], y[175])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[176], y[177], y[178], y[179])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[176], y[177], y[178], y[179])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[180], y[181], y[182], y[183])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[180], y[181], y[182], y[183])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[184], y[185], y[186], y[187])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[184], y[185], y[186], y[187])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[188], y[189], y[190], y[191])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[188], y[189], y[190], y[191])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[192], y[193], y[194], y[195])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[192], y[193], y[194], y[195])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[196], y[197], y[198], y[199])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[196], y[197], y[198], y[199])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[200], y[201], y[202], y[203])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[200], y[201], y[202], y[203])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[204], y[205], y[206], y[207])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[204], y[205], y[206], y[207])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[208], y[209], y[210], y[211])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[208], y[209], y[210], y[211])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[212], y[213], y[214], y[215])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[212], y[213], y[214], y[215])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[216], y[217], y[218], y[219])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[216], y[217], y[218], y[219])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[220], y[221], y[222], y[223])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[220], y[221], y[222], y[223])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[224], y[225], y[226], y[227])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[224], y[225], y[226], y[227])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[228], y[229], y[230], y[231])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[228], y[229], y[230], y[231])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[232], y[233], y[234], y[235])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[232], y[233], y[234], y[235])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[236], y[237], y[238], y[239])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[236], y[237], y[238], y[239])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[240], y[241], y[242], y[243])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[240], y[241], y[242], y[243])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[244], y[245], y[246], y[247])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[244], y[245], y[246], y[247])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[248], y[249], y[250], y[251])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[248], y[249], y[250], y[251])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[252], y[253], y[254], y[255])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[252], y[253], y[254], y[255])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[256], y[257], y[258], y[259])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[256], y[257], y[258], y[259])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[260], y[261], y[262], y[263])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[260], y[261], y[262], y[263])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[264], y[265], y[266], y[267])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[264], y[265], y[266], y[267])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[268], y[269], y[270], y[271])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[268], y[269], y[270], y[271])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[272], y[273], y[274], y[275])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[272], y[273], y[274], y[275])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[276], y[277], y[278], y[279])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[276], y[277], y[278], y[279])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[280], y[281], y[282], y[283])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[280], y[281], y[282], y[283])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[284], y[285], y[286], y[287])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[284], y[285], y[286], y[287])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[288], y[289], y[290], y[291])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[288], y[289], y[290], y[291])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[292], y[293], y[294], y[295])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[292], y[293], y[294], y[295])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[296], y[297], y[298], y[299])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[296], y[297], y[298], y[299])); if (sphere_sphere_self_collision( y[0], y[1], y[2], y[3], y[40], y[41], y[42], y[43])) @@ -41448,6 +41398,4001 @@ namespace vamp::robots return to_isometry(y.data()); } + + template + inline static auto sdf_gradient( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &x) noexcept + -> std::pair, FloatVector> + { + std::array, 51> v; + std::array, 432> y; + + v[0] = cos(x[0]); + v[1] = sin(x[0]); + v[2] = 0.707105482511236 * v[0] + -0.707108079859474 * v[1]; + y[24] = 0.0640272398484633 + 0.069 * v[2]; + v[3] = 0.707108079859474 * v[0] + 0.707105482511236 * v[1]; + y[25] = 0.259027384507773 + 0.069 * v[3]; + v[4] = cos(x[1]); + v[1] = -v[1]; + v[5] = 0.707105482511236 * v[1] + -0.707108079859474 * v[0]; + v[6] = sin(x[1]); + v[7] = 4.89663865010925e-12 * v[6]; + v[8] = v[2] * v[4] + v[5] * v[7]; + v[9] = cos(x[2]); + v[10] = sin(x[2]); + v[11] = 4.89663865010925e-12 * v[9] + -4.89658313895802e-12 * v[10]; + v[12] = -v[6]; + v[13] = 4.89663865010925e-12 * v[4]; + v[2] = v[2] * v[12] + v[5] * v[13]; + v[14] = 5.55111512312578e-17 * v[9] + v[10]; + v[15] = v[8] * v[11] + v[2] * v[9] + v[5] * v[14]; + v[16] = v[8] + -4.89658313895802e-12 * v[2] + 4.89663865010925e-12 * v[5]; + y[36] = y[24] + 0.102 * v[8]; + y[28] = -0.02 * v[15] + 0.22 * v[16] + y[36]; + v[1] = 0.707108079859474 * v[1] + 0.707105482511236 * v[0]; + v[7] = v[3] * v[4] + v[1] * v[7]; + v[13] = v[3] * v[12] + v[1] * v[13]; + v[12] = v[7] * v[11] + v[13] * v[9] + v[1] * v[14]; + v[3] = v[7] + -4.89658313895802e-12 * v[13] + 4.89663865010925e-12 * v[1]; + y[37] = y[25] + 0.102 * v[7]; + y[29] = -0.02 * v[12] + 0.22 * v[3] + y[37]; + v[6] = -1. * v[6]; + v[4] = -1. * v[4]; + v[14] = v[6] * v[11] + v[4] * v[9] + 4.89663865010925e-12 * v[14]; + v[11] = v[6] + 2.39770700697438e-23 + -4.89658313895802e-12 * v[4]; + y[38] = 0.399976 + 0.102 * v[6]; + y[30] = -0.02 * v[14] + 0.22 * v[11] + y[38]; + y[32] = -0.01 * v[15] + 0.11 * v[16] + y[36]; + y[33] = -0.01 * v[12] + 0.11 * v[3] + y[37]; + y[34] = -0.01 * v[14] + 0.11 * v[11] + y[38]; + y[40] = y[36] + 0.069 * v[15] + 0.26242 * v[16]; + y[41] = y[37] + 0.069 * v[12] + 0.26242 * v[3]; + y[42] = y[38] + 0.069 * v[14] + 0.26242 * v[11]; + v[0] = cos(x[3]); + v[17] = sin(x[3]); + v[18] = 4.89663865010925e-12 * v[0] + v[17]; + v[10] = -v[10]; + v[19] = 4.89663865010925e-12 * v[10] + -4.89658313895802e-12 * v[9]; + v[9] = 5.55111512312578e-17 * v[10] + v[9]; + v[2] = v[8] * v[19] + v[2] * v[10] + v[5] * v[9]; + v[8] = 5.55111512312578e-17 * v[0] + 4.89663865010925e-12 * v[17]; + v[5] = v[0] + -4.89658313895802e-12 * v[17]; + v[20] = v[15] * v[18] + v[2] * v[8] + v[16] * v[5]; + y[44] = y[40] + 0.10359 * v[20]; + v[13] = v[7] * v[19] + v[13] * v[10] + v[1] * v[9]; + v[7] = v[12] * v[18] + v[13] * v[8] + v[3] * v[5]; + y[45] = y[41] + 0.10359 * v[7]; + v[9] = v[6] * v[19] + v[4] * v[10] + 4.89663865010925e-12 * v[9]; + v[5] = v[14] * v[18] + v[9] * v[8] + v[11] * v[5]; + y[46] = y[42] + 0.10359 * v[5]; + v[17] = -v[17]; + v[8] = 4.89663865010925e-12 * v[17] + v[0]; + v[18] = 5.55111512312578e-17 * v[17] + 4.89663865010925e-12 * v[0]; + v[17] = v[17] + -4.89658313895802e-12 * v[0]; + v[0] = v[15] * v[8] + v[2] * v[18] + v[16] * v[17]; + v[2] = -4.89658313895802e-12 * v[15] + v[2]; + v[19] = v[20] + -4.89658313895802e-12 * v[0] + 4.89663865010925e-12 * v[2]; + y[48] = 0.22 * v[19] + y[44]; + v[10] = v[12] * v[8] + v[13] * v[18] + v[3] * v[17]; + v[13] = -4.89658313895802e-12 * v[12] + v[13]; + v[4] = v[7] + -4.89658313895802e-12 * v[10] + 4.89663865010925e-12 * v[13]; + y[49] = 0.22 * v[4] + y[45]; + v[17] = v[14] * v[8] + v[9] * v[18] + v[11] * v[17]; + v[9] = -4.89658313895802e-12 * v[14] + v[9]; + v[18] = v[5] + -4.89658313895802e-12 * v[17] + 4.89663865010925e-12 * v[9]; + y[50] = 0.22 * v[18] + y[46]; + y[52] = 0.11 * v[19] + y[44]; + y[53] = 0.11 * v[4] + y[45]; + y[54] = 0.11 * v[18] + y[46]; + v[8] = cos(x[4]); + v[6] = sin(x[4]); + v[1] = 4.89663865010925e-12 * v[8] + -4.89658313895802e-12 * v[6]; + v[21] = 5.55111512312578e-17 * v[8] + v[6]; + v[22] = v[20] * v[1] + v[0] * v[8] + v[2] * v[21]; + v[6] = -v[6]; + v[23] = 4.89663865010925e-12 * v[6] + -4.89658313895802e-12 * v[8]; + v[24] = 5.55111512312578e-17 * v[6] + v[8]; + v[2] = v[20] * v[23] + v[0] * v[6] + v[2] * v[24]; + v[0] = -4.89658313895802e-12 * v[22] + v[2]; + y[328] = y[44] + 0.01 * v[22] + 0.2707 * v[19]; + y[56] = 0.03 * v[0] + y[328]; + v[20] = v[7] * v[1] + v[10] * v[8] + v[13] * v[21]; + v[13] = v[7] * v[23] + v[10] * v[6] + v[13] * v[24]; + v[10] = -4.89658313895802e-12 * v[20] + v[13]; + y[329] = y[45] + 0.01 * v[20] + 0.2707 * v[4]; + y[57] = 0.03 * v[10] + y[329]; + v[21] = v[5] * v[1] + v[17] * v[8] + v[9] * v[21]; + v[24] = v[5] * v[23] + v[17] * v[6] + v[9] * v[24]; + v[23] = -4.89658313895802e-12 * v[21] + v[24]; + y[330] = y[46] + 0.01 * v[21] + 0.2707 * v[18]; + y[58] = 0.03 * v[23] + y[330]; + y[60] = -0.03 * v[0] + y[328]; + y[61] = -0.03 * v[10] + y[329]; + y[62] = -0.03 * v[23] + y[330]; + v[6] = cos(x[5]); + v[9] = sin(x[5]); + v[17] = 4.89663865010925e-12 * v[6] + v[9]; + v[5] = 5.55111512312578e-17 * v[6] + 4.89663865010925e-12 * v[9]; + v[1] = v[6] + -4.89658313895802e-12 * v[9]; + v[8] = v[22] * v[17] + v[2] * v[5] + v[19] * v[1]; + v[9] = -v[9]; + v[7] = 4.89663865010925e-12 * v[9] + v[6]; + v[25] = 5.55111512312578e-17 * v[9] + 4.89663865010925e-12 * v[6]; + v[9] = v[9] + -4.89658313895802e-12 * v[6]; + v[2] = v[22] * v[7] + v[2] * v[25] + v[19] * v[9]; + v[22] = v[8] + -4.89658313895802e-12 * v[2] + 4.89663865010925e-12 * v[0]; + v[6] = y[328] + 0.115975 * v[8]; + y[64] = 0.02 * v[22] + v[6]; + v[26] = v[20] * v[17] + v[13] * v[5] + v[4] * v[1]; + v[13] = v[20] * v[7] + v[13] * v[25] + v[4] * v[9]; + v[20] = v[26] + -4.89658313895802e-12 * v[13] + 4.89663865010925e-12 * v[10]; + v[27] = y[329] + 0.115975 * v[26]; + y[65] = 0.02 * v[20] + v[27]; + v[1] = v[21] * v[17] + v[24] * v[5] + v[18] * v[1]; + v[9] = v[21] * v[7] + v[24] * v[25] + v[18] * v[9]; + v[25] = v[1] + -4.89658313895802e-12 * v[9] + 4.89663865010925e-12 * v[23]; + v[7] = y[330] + 0.115975 * v[1]; + y[66] = 0.02 * v[25] + v[7]; + y[68] = -0.04 * v[22] + v[6]; + y[69] = -0.04 * v[20] + v[27]; + y[70] = -0.04 * v[25] + v[7]; + v[24] = cos(x[6]); + v[21] = sin(x[6]); + v[5] = 4.89663865010925e-12 * v[24] + -4.89658313895802e-12 * v[21]; + v[17] = 5.55111512312578e-17 * v[24] + v[21]; + v[28] = v[8] * v[5] + v[2] * v[24] + v[0] * v[17]; + y[72] = 0.01 * v[28] + 0.09355 * v[22] + v[6]; + v[29] = v[26] * v[5] + v[13] * v[24] + v[10] * v[17]; + y[73] = 0.01 * v[29] + 0.09355 * v[20] + v[27]; + v[17] = v[1] * v[5] + v[9] * v[24] + v[23] * v[17]; + y[74] = 0.01 * v[17] + 0.09355 * v[25] + v[7]; + v[21] = -v[21]; + v[5] = 4.89663865010925e-12 * v[21] + -4.89658313895802e-12 * v[24]; + v[24] = 5.55111512312578e-17 * v[21] + v[24]; + v[2] = v[8] * v[5] + v[2] * v[21] + v[0] * v[24]; + y[76] = 0.02 * v[2] + 0.13855 * v[22] + v[6]; + v[13] = v[26] * v[5] + v[13] * v[21] + v[10] * v[24]; + y[77] = 0.02 * v[13] + 0.13855 * v[20] + v[27]; + v[24] = v[1] * v[5] + v[9] * v[21] + v[23] * v[24]; + y[78] = 0.02 * v[24] + 0.13855 * v[25] + v[7]; + y[80] = -0.02 * v[2] + 0.13855 * v[22] + v[6]; + y[81] = -0.02 * v[13] + 0.13855 * v[20] + v[27]; + y[82] = -0.02 * v[24] + 0.13855 * v[25] + v[7]; + y[84] = -0.005 * v[28] + 0.081333 * v[2] + 0.16655 * v[22] + v[6]; + y[85] = -0.005 * v[29] + 0.081333 * v[13] + 0.16655 * v[20] + v[27]; + y[86] = -0.005 * v[17] + 0.081333 * v[24] + 0.16655 * v[25] + v[7]; + y[88] = -0.005 * v[28] + 0.057333 * v[2] + 0.16655 * v[22] + v[6]; + y[89] = -0.005 * v[29] + 0.057333 * v[13] + 0.16655 * v[20] + v[27]; + y[90] = -0.005 * v[17] + 0.057333 * v[24] + 0.16655 * v[25] + v[7]; + y[92] = 0.086583 * v[2] + 0.18855 * v[22] + v[6]; + y[93] = 0.086583 * v[13] + 0.18855 * v[20] + v[27]; + y[94] = 0.086583 * v[24] + 0.18855 * v[25] + v[7]; + y[96] = 0.086583 * v[2] + 0.20855 * v[22] + v[6]; + y[97] = 0.086583 * v[13] + 0.20855 * v[20] + v[27]; + y[98] = 0.086583 * v[24] + 0.20855 * v[25] + v[7]; + y[100] = 0.086583 * v[2] + 0.22855 * v[22] + v[6]; + y[101] = 0.086583 * v[13] + 0.22855 * v[20] + v[27]; + y[102] = 0.086583 * v[24] + 0.22855 * v[25] + v[7]; + y[104] = 0.01 * v[28] + 0.082083 * v[2] + 0.26625 * v[22] + v[6]; + y[105] = 0.01 * v[29] + 0.082083 * v[13] + 0.26625 * v[20] + v[27]; + y[106] = 0.01 * v[17] + 0.082083 * v[24] + 0.26625 * v[25] + v[7]; + y[108] = -0.01 * v[28] + 0.082083 * v[2] + 0.26625 * v[22] + v[6]; + y[109] = -0.01 * v[29] + 0.082083 * v[13] + 0.26625 * v[20] + v[27]; + y[110] = -0.01 * v[17] + 0.082083 * v[24] + 0.26625 * v[25] + v[7]; + y[112] = -0.01 * v[28] + 0.082083 * v[2] + 0.24625 * v[22] + v[6]; + y[113] = -0.01 * v[29] + 0.082083 * v[13] + 0.24625 * v[20] + v[27]; + y[114] = -0.01 * v[17] + 0.082083 * v[24] + 0.24625 * v[25] + v[7]; + y[116] = 0.01 * v[28] + 0.082083 * v[2] + 0.24625 * v[22] + v[6]; + y[117] = 0.01 * v[29] + 0.082083 * v[13] + 0.24625 * v[20] + v[27]; + y[118] = 0.01 * v[17] + 0.082083 * v[24] + 0.24625 * v[25] + v[7]; + y[120] = 0.005 * v[28] + -0.059333 * v[2] + 0.16655 * v[22] + v[6]; + y[121] = 0.005 * v[29] + -0.059333 * v[13] + 0.16655 * v[20] + v[27]; + y[122] = 0.005 * v[17] + -0.059333 * v[24] + 0.16655 * v[25] + v[7]; + y[124] = 0.005 * v[28] + -0.079333 * v[2] + 0.16655 * v[22] + v[6]; + y[125] = 0.005 * v[29] + -0.079333 * v[13] + 0.16655 * v[20] + v[27]; + y[126] = 0.005 * v[17] + -0.079333 * v[24] + 0.16655 * v[25] + v[7]; + y[128] = -0.086583 * v[2] + 0.18855 * v[22] + v[6]; + y[129] = -0.086583 * v[13] + 0.18855 * v[20] + v[27]; + y[130] = -0.086583 * v[24] + 0.18855 * v[25] + v[7]; + y[132] = -0.086583 * v[2] + 0.20855 * v[22] + v[6]; + y[133] = -0.086583 * v[13] + 0.20855 * v[20] + v[27]; + y[134] = -0.086583 * v[24] + 0.20855 * v[25] + v[7]; + y[136] = -0.086583 * v[2] + 0.22855 * v[22] + v[6]; + y[137] = -0.086583 * v[13] + 0.22855 * v[20] + v[27]; + y[138] = -0.086583 * v[24] + 0.22855 * v[25] + v[7]; + y[140] = 0.01 * v[28] + -0.082083 * v[2] + 0.26625 * v[22] + v[6]; + y[141] = 0.01 * v[29] + -0.082083 * v[13] + 0.26625 * v[20] + v[27]; + y[142] = 0.01 * v[17] + -0.082083 * v[24] + 0.26625 * v[25] + v[7]; + y[144] = -0.01 * v[28] + -0.082083 * v[2] + 0.26625 * v[22] + v[6]; + y[145] = -0.01 * v[29] + -0.082083 * v[13] + 0.26625 * v[20] + v[27]; + y[146] = -0.01 * v[17] + -0.082083 * v[24] + 0.26625 * v[25] + v[7]; + y[148] = -0.01 * v[28] + -0.082083 * v[2] + 0.24625 * v[22] + v[6]; + y[149] = -0.01 * v[29] + -0.082083 * v[13] + 0.24625 * v[20] + v[27]; + y[150] = -0.01 * v[17] + -0.082083 * v[24] + 0.24625 * v[25] + v[7]; + y[152] = 0.01 * v[28] + -0.082083 * v[2] + 0.24625 * v[22] + v[6]; + y[153] = 0.01 * v[29] + -0.082083 * v[13] + 0.24625 * v[20] + v[27]; + y[154] = 0.01 * v[17] + -0.082083 * v[24] + 0.24625 * v[25] + v[7]; + v[5] = cos(x[7]); + v[21] = sin(x[7]); + v[9] = 0.707105482511236 * v[5] + 0.707108079859474 * v[21]; + y[168] = 0.0640272398484633 + 0.069 * v[9]; + v[1] = -0.707108079859474 * v[5] + 0.707105482511236 * v[21]; + y[169] = -0.259027384507773 + 0.069 * v[1]; + v[23] = cos(x[8]); + v[21] = -v[21]; + v[26] = 0.707105482511236 * v[21] + 0.707108079859474 * v[5]; + v[10] = sin(x[8]); + v[8] = 4.89663865010925e-12 * v[10]; + v[0] = v[9] * v[23] + v[26] * v[8]; + v[30] = cos(x[9]); + v[31] = sin(x[9]); + v[32] = 4.89663865010925e-12 * v[30] + -4.89658313895802e-12 * v[31]; + v[33] = -v[10]; + v[34] = 4.89663865010925e-12 * v[23]; + v[9] = v[9] * v[33] + v[26] * v[34]; + v[35] = 5.55111512312578e-17 * v[30] + v[31]; + v[36] = v[0] * v[32] + v[9] * v[30] + v[26] * v[35]; + v[37] = v[0] + -4.89658313895802e-12 * v[9] + 4.89663865010925e-12 * v[26]; + y[180] = y[168] + 0.102 * v[0]; + y[172] = -0.02 * v[36] + 0.22 * v[37] + y[180]; + v[21] = -0.707108079859474 * v[21] + 0.707105482511236 * v[5]; + v[8] = v[1] * v[23] + v[21] * v[8]; + v[34] = v[1] * v[33] + v[21] * v[34]; + v[33] = v[8] * v[32] + v[34] * v[30] + v[21] * v[35]; + v[1] = v[8] + -4.89658313895802e-12 * v[34] + 4.89663865010925e-12 * v[21]; + y[181] = y[169] + 0.102 * v[8]; + y[173] = -0.02 * v[33] + 0.22 * v[1] + y[181]; + v[10] = -1. * v[10]; + v[23] = -1. * v[23]; + v[35] = v[10] * v[32] + v[23] * v[30] + 4.89663865010925e-12 * v[35]; + v[32] = v[10] + 2.39770700697438e-23 + -4.89658313895802e-12 * v[23]; + y[182] = 0.399976 + 0.102 * v[10]; + y[174] = -0.02 * v[35] + 0.22 * v[32] + y[182]; + y[176] = -0.01 * v[36] + 0.11 * v[37] + y[180]; + y[177] = -0.01 * v[33] + 0.11 * v[1] + y[181]; + y[178] = -0.01 * v[35] + 0.11 * v[32] + y[182]; + y[184] = y[180] + 0.069 * v[36] + 0.26242 * v[37]; + y[185] = y[181] + 0.069 * v[33] + 0.26242 * v[1]; + y[186] = y[182] + 0.069 * v[35] + 0.26242 * v[32]; + v[5] = cos(x[10]); + v[38] = sin(x[10]); + v[39] = 4.89663865010925e-12 * v[5] + v[38]; + v[31] = -v[31]; + v[40] = 4.89663865010925e-12 * v[31] + -4.89658313895802e-12 * v[30]; + v[30] = 5.55111512312578e-17 * v[31] + v[30]; + v[9] = v[0] * v[40] + v[9] * v[31] + v[26] * v[30]; + v[0] = 5.55111512312578e-17 * v[5] + 4.89663865010925e-12 * v[38]; + v[26] = v[5] + -4.89658313895802e-12 * v[38]; + v[41] = v[36] * v[39] + v[9] * v[0] + v[37] * v[26]; + y[188] = y[184] + 0.10359 * v[41]; + v[34] = v[8] * v[40] + v[34] * v[31] + v[21] * v[30]; + v[8] = v[33] * v[39] + v[34] * v[0] + v[1] * v[26]; + y[189] = y[185] + 0.10359 * v[8]; + v[30] = v[10] * v[40] + v[23] * v[31] + 4.89663865010925e-12 * v[30]; + v[26] = v[35] * v[39] + v[30] * v[0] + v[32] * v[26]; + y[190] = y[186] + 0.10359 * v[26]; + v[38] = -v[38]; + v[0] = 4.89663865010925e-12 * v[38] + v[5]; + v[39] = 5.55111512312578e-17 * v[38] + 4.89663865010925e-12 * v[5]; + v[38] = v[38] + -4.89658313895802e-12 * v[5]; + v[5] = v[36] * v[0] + v[9] * v[39] + v[37] * v[38]; + v[9] = -4.89658313895802e-12 * v[36] + v[9]; + v[40] = v[41] + -4.89658313895802e-12 * v[5] + 4.89663865010925e-12 * v[9]; + y[192] = 0.22 * v[40] + y[188]; + v[31] = v[33] * v[0] + v[34] * v[39] + v[1] * v[38]; + v[34] = -4.89658313895802e-12 * v[33] + v[34]; + v[23] = v[8] + -4.89658313895802e-12 * v[31] + 4.89663865010925e-12 * v[34]; + y[193] = 0.22 * v[23] + y[189]; + v[38] = v[35] * v[0] + v[30] * v[39] + v[32] * v[38]; + v[30] = -4.89658313895802e-12 * v[35] + v[30]; + v[39] = v[26] + -4.89658313895802e-12 * v[38] + 4.89663865010925e-12 * v[30]; + y[194] = 0.22 * v[39] + y[190]; + y[196] = 0.11 * v[40] + y[188]; + y[197] = 0.11 * v[23] + y[189]; + y[198] = 0.11 * v[39] + y[190]; + v[0] = cos(x[11]); + v[10] = sin(x[11]); + v[21] = 4.89663865010925e-12 * v[0] + -4.89658313895802e-12 * v[10]; + v[42] = 5.55111512312578e-17 * v[0] + v[10]; + v[43] = v[41] * v[21] + v[5] * v[0] + v[9] * v[42]; + v[10] = -v[10]; + v[44] = 4.89663865010925e-12 * v[10] + -4.89658313895802e-12 * v[0]; + v[45] = 5.55111512312578e-17 * v[10] + v[0]; + v[9] = v[41] * v[44] + v[5] * v[10] + v[9] * v[45]; + v[5] = -4.89658313895802e-12 * v[43] + v[9]; + y[392] = y[188] + 0.01 * v[43] + 0.2707 * v[40]; + y[200] = 0.03 * v[5] + y[392]; + v[41] = v[8] * v[21] + v[31] * v[0] + v[34] * v[42]; + v[34] = v[8] * v[44] + v[31] * v[10] + v[34] * v[45]; + v[31] = -4.89658313895802e-12 * v[41] + v[34]; + y[393] = y[189] + 0.01 * v[41] + 0.2707 * v[23]; + y[201] = 0.03 * v[31] + y[393]; + v[42] = v[26] * v[21] + v[38] * v[0] + v[30] * v[42]; + v[45] = v[26] * v[44] + v[38] * v[10] + v[30] * v[45]; + v[44] = -4.89658313895802e-12 * v[42] + v[45]; + y[394] = y[190] + 0.01 * v[42] + 0.2707 * v[39]; + y[202] = 0.03 * v[44] + y[394]; + y[204] = -0.03 * v[5] + y[392]; + y[205] = -0.03 * v[31] + y[393]; + y[206] = -0.03 * v[44] + y[394]; + v[10] = cos(x[12]); + v[30] = sin(x[12]); + v[38] = 4.89663865010925e-12 * v[10] + v[30]; + v[26] = 5.55111512312578e-17 * v[10] + 4.89663865010925e-12 * v[30]; + v[21] = v[10] + -4.89658313895802e-12 * v[30]; + v[0] = v[43] * v[38] + v[9] * v[26] + v[40] * v[21]; + v[30] = -v[30]; + v[8] = 4.89663865010925e-12 * v[30] + v[10]; + v[46] = 5.55111512312578e-17 * v[30] + 4.89663865010925e-12 * v[10]; + v[30] = v[30] + -4.89658313895802e-12 * v[10]; + v[9] = v[43] * v[8] + v[9] * v[46] + v[40] * v[30]; + v[43] = v[0] + -4.89658313895802e-12 * v[9] + 4.89663865010925e-12 * v[5]; + v[10] = y[392] + 0.115975 * v[0]; + y[208] = 0.02 * v[43] + v[10]; + v[47] = v[41] * v[38] + v[34] * v[26] + v[23] * v[21]; + v[34] = v[41] * v[8] + v[34] * v[46] + v[23] * v[30]; + v[41] = v[47] + -4.89658313895802e-12 * v[34] + 4.89663865010925e-12 * v[31]; + v[48] = y[393] + 0.115975 * v[47]; + y[209] = 0.02 * v[41] + v[48]; + v[21] = v[42] * v[38] + v[45] * v[26] + v[39] * v[21]; + v[30] = v[42] * v[8] + v[45] * v[46] + v[39] * v[30]; + v[46] = v[21] + -4.89658313895802e-12 * v[30] + 4.89663865010925e-12 * v[44]; + v[8] = y[394] + 0.115975 * v[21]; + y[210] = 0.02 * v[46] + v[8]; + y[212] = -0.04 * v[43] + v[10]; + y[213] = -0.04 * v[41] + v[48]; + y[214] = -0.04 * v[46] + v[8]; + v[45] = cos(x[13]); + v[42] = sin(x[13]); + v[26] = 4.89663865010925e-12 * v[45] + -4.89658313895802e-12 * v[42]; + v[38] = 5.55111512312578e-17 * v[45] + v[42]; + v[49] = v[0] * v[26] + v[9] * v[45] + v[5] * v[38]; + y[216] = 0.01 * v[49] + 0.09355 * v[43] + v[10]; + v[50] = v[47] * v[26] + v[34] * v[45] + v[31] * v[38]; + y[217] = 0.01 * v[50] + 0.09355 * v[41] + v[48]; + v[38] = v[21] * v[26] + v[30] * v[45] + v[44] * v[38]; + y[218] = 0.01 * v[38] + 0.09355 * v[46] + v[8]; + v[42] = -v[42]; + v[26] = 4.89663865010925e-12 * v[42] + -4.89658313895802e-12 * v[45]; + v[45] = 5.55111512312578e-17 * v[42] + v[45]; + v[9] = v[0] * v[26] + v[9] * v[42] + v[5] * v[45]; + y[220] = 0.02 * v[9] + 0.13855 * v[43] + v[10]; + v[34] = v[47] * v[26] + v[34] * v[42] + v[31] * v[45]; + y[221] = 0.02 * v[34] + 0.13855 * v[41] + v[48]; + v[45] = v[21] * v[26] + v[30] * v[42] + v[44] * v[45]; + y[222] = 0.02 * v[45] + 0.13855 * v[46] + v[8]; + y[224] = -0.02 * v[9] + 0.13855 * v[43] + v[10]; + y[225] = -0.02 * v[34] + 0.13855 * v[41] + v[48]; + y[226] = -0.02 * v[45] + 0.13855 * v[46] + v[8]; + y[228] = -0.005 * v[49] + 0.081333 * v[9] + 0.16655 * v[43] + v[10]; + y[229] = -0.005 * v[50] + 0.081333 * v[34] + 0.16655 * v[41] + v[48]; + y[230] = -0.005 * v[38] + 0.081333 * v[45] + 0.16655 * v[46] + v[8]; + y[232] = -0.005 * v[49] + 0.057333 * v[9] + 0.16655 * v[43] + v[10]; + y[233] = -0.005 * v[50] + 0.057333 * v[34] + 0.16655 * v[41] + v[48]; + y[234] = -0.005 * v[38] + 0.057333 * v[45] + 0.16655 * v[46] + v[8]; + y[236] = 0.086583 * v[9] + 0.18855 * v[43] + v[10]; + y[237] = 0.086583 * v[34] + 0.18855 * v[41] + v[48]; + y[238] = 0.086583 * v[45] + 0.18855 * v[46] + v[8]; + y[240] = 0.086583 * v[9] + 0.20855 * v[43] + v[10]; + y[241] = 0.086583 * v[34] + 0.20855 * v[41] + v[48]; + y[242] = 0.086583 * v[45] + 0.20855 * v[46] + v[8]; + y[244] = 0.086583 * v[9] + 0.22855 * v[43] + v[10]; + y[245] = 0.086583 * v[34] + 0.22855 * v[41] + v[48]; + y[246] = 0.086583 * v[45] + 0.22855 * v[46] + v[8]; + y[248] = 0.01 * v[49] + 0.082083 * v[9] + 0.26625 * v[43] + v[10]; + y[249] = 0.01 * v[50] + 0.082083 * v[34] + 0.26625 * v[41] + v[48]; + y[250] = 0.01 * v[38] + 0.082083 * v[45] + 0.26625 * v[46] + v[8]; + y[252] = -0.01 * v[49] + 0.082083 * v[9] + 0.26625 * v[43] + v[10]; + y[253] = -0.01 * v[50] + 0.082083 * v[34] + 0.26625 * v[41] + v[48]; + y[254] = -0.01 * v[38] + 0.082083 * v[45] + 0.26625 * v[46] + v[8]; + y[256] = -0.01 * v[49] + 0.082083 * v[9] + 0.24625 * v[43] + v[10]; + y[257] = -0.01 * v[50] + 0.082083 * v[34] + 0.24625 * v[41] + v[48]; + y[258] = -0.01 * v[38] + 0.082083 * v[45] + 0.24625 * v[46] + v[8]; + y[260] = 0.01 * v[49] + 0.082083 * v[9] + 0.24625 * v[43] + v[10]; + y[261] = 0.01 * v[50] + 0.082083 * v[34] + 0.24625 * v[41] + v[48]; + y[262] = 0.01 * v[38] + 0.082083 * v[45] + 0.24625 * v[46] + v[8]; + y[264] = 0.005 * v[49] + -0.059333 * v[9] + 0.16655 * v[43] + v[10]; + y[265] = 0.005 * v[50] + -0.059333 * v[34] + 0.16655 * v[41] + v[48]; + y[266] = 0.005 * v[38] + -0.059333 * v[45] + 0.16655 * v[46] + v[8]; + y[268] = 0.005 * v[49] + -0.079333 * v[9] + 0.16655 * v[43] + v[10]; + y[269] = 0.005 * v[50] + -0.079333 * v[34] + 0.16655 * v[41] + v[48]; + y[270] = 0.005 * v[38] + -0.079333 * v[45] + 0.16655 * v[46] + v[8]; + y[272] = -0.086583 * v[9] + 0.18855 * v[43] + v[10]; + y[273] = -0.086583 * v[34] + 0.18855 * v[41] + v[48]; + y[274] = -0.086583 * v[45] + 0.18855 * v[46] + v[8]; + y[276] = -0.086583 * v[9] + 0.20855 * v[43] + v[10]; + y[277] = -0.086583 * v[34] + 0.20855 * v[41] + v[48]; + y[278] = -0.086583 * v[45] + 0.20855 * v[46] + v[8]; + y[280] = -0.086583 * v[9] + 0.22855 * v[43] + v[10]; + y[281] = -0.086583 * v[34] + 0.22855 * v[41] + v[48]; + y[282] = -0.086583 * v[45] + 0.22855 * v[46] + v[8]; + y[284] = 0.01 * v[49] + -0.082083 * v[9] + 0.26625 * v[43] + v[10]; + y[285] = 0.01 * v[50] + -0.082083 * v[34] + 0.26625 * v[41] + v[48]; + y[286] = 0.01 * v[38] + -0.082083 * v[45] + 0.26625 * v[46] + v[8]; + y[288] = -0.01 * v[49] + -0.082083 * v[9] + 0.26625 * v[43] + v[10]; + y[289] = -0.01 * v[50] + -0.082083 * v[34] + 0.26625 * v[41] + v[48]; + y[290] = -0.01 * v[38] + -0.082083 * v[45] + 0.26625 * v[46] + v[8]; + y[292] = -0.01 * v[49] + -0.082083 * v[9] + 0.24625 * v[43] + v[10]; + y[293] = -0.01 * v[50] + -0.082083 * v[34] + 0.24625 * v[41] + v[48]; + y[294] = -0.01 * v[38] + -0.082083 * v[45] + 0.24625 * v[46] + v[8]; + y[296] = 0.01 * v[49] + -0.082083 * v[9] + 0.24625 * v[43] + v[10]; + y[297] = 0.01 * v[50] + -0.082083 * v[34] + 0.24625 * v[41] + v[48]; + y[298] = 0.01 * v[38] + -0.082083 * v[45] + 0.24625 * v[46] + v[8]; + y[316] = -0.00999999977648258 * v[15] + 0.109999999403954 * v[16] + y[36]; + y[317] = -0.00999999977648258 * v[12] + 0.109999999403954 * v[3] + y[37]; + y[318] = -0.00999999977648258 * v[14] + 0.109999999403954 * v[11] + y[38]; + y[324] = 0.109999999403954 * v[19] + y[44]; + y[325] = 0.109999999403954 * v[4] + y[45]; + y[326] = 0.109999999403954 * v[18] + y[46]; + y[332] = -0.0149999987334013 * v[22] + v[6]; + y[333] = -0.0149999987334013 * v[20] + v[27]; + y[334] = -0.0149999987334013 * v[25] + v[7]; + y[336] = 0.00999999977648258 * v[28] + 0.0935499966144562 * v[22] + v[6]; + y[337] = 0.00999999977648258 * v[29] + 0.0935499966144562 * v[20] + v[27]; + y[338] = 0.00999999977648258 * v[17] + 0.0935499966144562 * v[25] + v[7]; + y[340] = 0.138549998402596 * v[22] + v[6]; + y[341] = 0.138549998402596 * v[20] + v[27]; + y[342] = 0.138549998402596 * v[25] + v[7]; + y[344] = + -0.00499999988824129 * v[28] + 0.0693330019712448 * v[2] + 0.166549995541573 * v[22] + v[6]; + y[345] = + -0.00499999988824129 * v[29] + 0.0693330019712448 * v[13] + 0.166549995541573 * v[20] + v[27]; + y[346] = + -0.00499999988824129 * v[17] + 0.0693330019712448 * v[24] + 0.166549995541573 * v[25] + v[7]; + y[348] = 0.0865830034017563 * v[2] + 0.208550006151199 * v[22] + v[6]; + y[349] = 0.0865830034017563 * v[13] + 0.208550006151199 * v[20] + v[27]; + y[350] = 0.0865830034017563 * v[24] + 0.208550006151199 * v[25] + v[7]; + y[352] = 0.0820830017328262 * v[2] + 0.256249994039536 * v[22] + v[6]; + y[353] = 0.0820830017328262 * v[13] + 0.256249994039536 * v[20] + v[27]; + y[354] = 0.0820830017328262 * v[24] + 0.256249994039536 * v[25] + v[7]; + y[356] = + 0.00499999988824129 * v[28] + -0.0693330019712448 * v[2] + 0.166549995541573 * v[22] + v[6]; + y[357] = + 0.00499999988824129 * v[29] + -0.0693330019712448 * v[13] + 0.166549995541573 * v[20] + v[27]; + y[358] = + 0.00499999988824129 * v[17] + -0.0693330019712448 * v[24] + 0.166549995541573 * v[25] + v[7]; + y[360] = -0.0865830034017563 * v[2] + 0.208550006151199 * v[22] + v[6]; + y[361] = -0.0865830034017563 * v[13] + 0.208550006151199 * v[20] + v[27]; + y[362] = -0.0865830034017563 * v[24] + 0.208550006151199 * v[25] + v[7]; + y[364] = -0.0820830017328262 * v[2] + 0.256249994039536 * v[22] + v[6]; + y[365] = -0.0820830017328262 * v[13] + 0.256249994039536 * v[20] + v[27]; + y[366] = -0.0820830017328262 * v[24] + 0.256249994039536 * v[25] + v[7]; + y[380] = -0.00999999977648258 * v[36] + 0.109999999403954 * v[37] + y[180]; + y[381] = -0.00999999977648258 * v[33] + 0.109999999403954 * v[1] + y[181]; + y[382] = -0.00999999977648258 * v[35] + 0.109999999403954 * v[32] + y[182]; + y[388] = 0.109999999403954 * v[40] + y[188]; + y[389] = 0.109999999403954 * v[23] + y[189]; + y[390] = 0.109999999403954 * v[39] + y[190]; + y[396] = -0.0149999987334013 * v[43] + v[10]; + y[397] = -0.0149999987334013 * v[41] + v[48]; + y[398] = -0.0149999987334013 * v[46] + v[8]; + y[400] = 0.00999999977648258 * v[49] + 0.0935499966144562 * v[43] + v[10]; + y[401] = 0.00999999977648258 * v[50] + 0.0935499966144562 * v[41] + v[48]; + y[402] = 0.00999999977648258 * v[38] + 0.0935499966144562 * v[46] + v[8]; + y[404] = 0.138549998402596 * v[43] + v[10]; + y[405] = 0.138549998402596 * v[41] + v[48]; + y[406] = 0.138549998402596 * v[46] + v[8]; + y[408] = + -0.00499999988824129 * v[49] + 0.0693330019712448 * v[9] + 0.166549995541573 * v[43] + v[10]; + y[409] = + -0.00499999988824129 * v[50] + 0.0693330019712448 * v[34] + 0.166549995541573 * v[41] + v[48]; + y[410] = + -0.00499999988824129 * v[38] + 0.0693330019712448 * v[45] + 0.166549995541573 * v[46] + v[8]; + y[412] = 0.0865830034017563 * v[9] + 0.208550006151199 * v[43] + v[10]; + y[413] = 0.0865830034017563 * v[34] + 0.208550006151199 * v[41] + v[48]; + y[414] = 0.0865830034017563 * v[45] + 0.208550006151199 * v[46] + v[8]; + y[416] = 0.0820830017328262 * v[9] + 0.256249994039536 * v[43] + v[10]; + y[417] = 0.0820830017328262 * v[34] + 0.256249994039536 * v[41] + v[48]; + y[418] = 0.0820830017328262 * v[45] + 0.256249994039536 * v[46] + v[8]; + y[420] = + 0.00499999988824129 * v[49] + -0.0693330019712448 * v[9] + 0.166549995541573 * v[43] + v[10]; + y[421] = + 0.00499999988824129 * v[50] + -0.0693330019712448 * v[34] + 0.166549995541573 * v[41] + v[48]; + y[422] = + 0.00499999988824129 * v[38] + -0.0693330019712448 * v[45] + 0.166549995541573 * v[46] + v[8]; + y[424] = -0.0865830034017563 * v[9] + 0.208550006151199 * v[43] + v[10]; + y[425] = -0.0865830034017563 * v[34] + 0.208550006151199 * v[41] + v[48]; + y[426] = -0.0865830034017563 * v[45] + 0.208550006151199 * v[46] + v[8]; + y[428] = -0.0820830017328262 * v[9] + 0.256249994039536 * v[43] + v[10]; + y[429] = -0.0820830017328262 * v[34] + 0.256249994039536 * v[41] + v[48]; + y[430] = -0.0820830017328262 * v[45] + 0.256249994039536 * v[46] + v[8]; + // variable duplicates: 10 + y[312] = y[24]; + y[313] = y[25]; + y[320] = y[40]; + y[321] = y[41]; + y[322] = y[42]; + y[376] = y[168]; + y[377] = y[169]; + y[384] = y[184]; + y[385] = y[185]; + y[386] = y[186]; + // dependent variables without operations + y[0] = -0.025; + y[1] = -0.1; + y[2] = 0.1; + y[3] = 0.25; + y[4] = -0.025; + y[5] = 0.1; + y[6] = 0.1; + y[7] = 0.25; + y[8] = -0.065; + y[9] = 0.; + y[10] = 0.4; + y[11] = 0.230000004172325; + y[12] = 0.04; + y[13] = 0.; + y[14] = 0.686; + y[15] = 0.200000002980232; + y[16] = 0.0640272398484633; + y[17] = 0.259027384507773; + y[18] = 0.379626; + y[19] = 0.100000001490116; + y[20] = 0.0640272398484633; + y[21] = 0.259027384507773; + y[22] = 0.229626; + y[23] = 0.100000001490116; + y[26] = 0.399976; + y[27] = 0.100000001490116; + y[31] = 0.0799999982118607; + y[35] = 0.0799999982118607; + y[39] = 0.0799999982118607; + y[43] = 0.100000001490116; + y[47] = 0.0799999982118607; + y[51] = 0.0799999982118607; + y[55] = 0.0799999982118607; + y[59] = 0.0700000002980232; + y[63] = 0.0700000002980232; + y[67] = 0.0700000002980232; + y[71] = 0.0799999982118607; + y[75] = 0.0500000007450581; + y[79] = 0.0399999991059303; + y[83] = 0.0399999991059303; + y[87] = 0.0149999996647239; + y[91] = 0.0149999996647239; + y[95] = 0.0120000001043081; + y[99] = 0.0120000001043081; + y[103] = 0.0120000001043081; + y[107] = 0.0140000004321337; + y[111] = 0.0140000004321337; + y[115] = 0.0140000004321337; + y[119] = 0.0140000004321337; + y[123] = 0.0149999996647239; + y[127] = 0.0149999996647239; + y[131] = 0.0120000001043081; + y[135] = 0.0120000001043081; + y[139] = 0.0120000001043081; + y[143] = 0.0140000004321337; + y[147] = 0.0140000004321337; + y[151] = 0.0140000004321337; + y[155] = 0.0140000004321337; + y[156] = 0.; + y[157] = 0.; + y[158] = -0.6; + y[159] = 0.5; + y[160] = 0.0640272398484633; + y[161] = -0.259027384507773; + y[162] = 0.379626; + y[163] = 0.100000001490116; + y[164] = 0.0640272398484633; + y[165] = -0.259027384507773; + y[166] = 0.229626; + y[167] = 0.100000001490116; + y[170] = 0.399976; + y[171] = 0.100000001490116; + y[175] = 0.0799999982118607; + y[179] = 0.0799999982118607; + y[183] = 0.0799999982118607; + y[187] = 0.100000001490116; + y[191] = 0.0799999982118607; + y[195] = 0.0799999982118607; + y[199] = 0.0799999982118607; + y[203] = 0.0700000002980232; + y[207] = 0.0700000002980232; + y[211] = 0.0700000002980232; + y[215] = 0.0799999982118607; + y[219] = 0.0500000007450581; + y[223] = 0.0399999991059303; + y[227] = 0.0399999991059303; + y[231] = 0.0149999996647239; + y[235] = 0.0149999996647239; + y[239] = 0.0120000001043081; + y[243] = 0.0120000001043081; + y[247] = 0.0120000001043081; + y[251] = 0.0140000004321337; + y[255] = 0.0140000004321337; + y[259] = 0.0140000004321337; + y[263] = 0.0140000004321337; + y[267] = 0.0149999996647239; + y[271] = 0.0149999996647239; + y[275] = 0.0120000001043081; + y[279] = 0.0120000001043081; + y[283] = 0.0120000001043081; + y[287] = 0.0140000004321337; + y[291] = 0.0140000004321337; + y[295] = 0.0140000004321337; + y[299] = 0.0140000004321337; + y[300] = -0.0413404628634453; + y[301] = 0.; + y[302] = 0.222553476691246; + y[303] = 0.409016877412796; + y[304] = 0.0399999991059303; + y[305] = 0.; + y[306] = 0.685999989509583; + y[307] = 0.200000002980232; + y[308] = 0.0640272398484633; + y[309] = 0.259027384507773; + y[310] = 0.304625997019768; + y[311] = 0.174999997019768; + y[314] = 0.399976; + y[315] = 0.100000001490116; + y[319] = 0.190453603863716; + y[323] = 0.100000001490116; + y[327] = 0.189999997615814; + y[331] = 0.100000001490116; + y[335] = 0.104999996721745; + y[339] = 0.0500000007450581; + y[343] = 0.0599999986588955; + y[347] = 0.0269999988377094; + y[351] = 0.0320000015199184; + y[355] = 0.0281421355903149; + y[359] = 0.025000000372529; + y[363] = 0.0320000015199184; + y[367] = 0.0281421355903149; + y[368] = 0.; + y[369] = 0.; + y[370] = -0.600000023841858; + y[371] = 0.5; + y[372] = 0.0640272398484633; + y[373] = -0.259027384507773; + y[374] = 0.304625997019768; + y[375] = 0.174999997019768; + y[378] = 0.399976; + y[379] = 0.100000001490116; + y[383] = 0.190453603863716; + y[387] = 0.100000001490116; + y[391] = 0.189999997615814; + y[395] = 0.100000001490116; + y[399] = 0.104999996721745; + y[403] = 0.0500000007450581; + y[407] = 0.0599999986588955; + y[411] = 0.0269999988377094; + y[415] = 0.0320000015199184; + y[419] = 0.0281421355903149; + y[423] = 0.025000000372529; + y[427] = 0.0320000015199184; + y[431] = 0.0281421355903149; + + FloatVector dists; + FloatVector grads; + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[0], y[1], y[2], y[3]); + dists[0] = res.first; + grads[0] = res.second[0]; + grads[1] = res.second[1]; + grads[2] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[4], y[5], y[6], y[7]); + dists[1] = res.first; + grads[3] = res.second[0]; + grads[4] = res.second[1]; + grads[5] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[8], y[9], y[10], y[11]); + dists[2] = res.first; + grads[6] = res.second[0]; + grads[7] = res.second[1]; + grads[8] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[12], y[13], y[14], y[15]); + dists[3] = res.first; + grads[9] = res.second[0]; + grads[10] = res.second[1]; + grads[11] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[16], y[17], y[18], y[19]); + dists[4] = res.first; + grads[12] = res.second[0]; + grads[13] = res.second[1]; + grads[14] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[20], y[21], y[22], y[23]); + dists[5] = res.first; + grads[15] = res.second[0]; + grads[16] = res.second[1]; + grads[17] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[24], y[25], y[26], y[27]); + dists[6] = res.first; + grads[18] = res.second[0]; + grads[19] = res.second[1]; + grads[20] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[28], y[29], y[30], y[31]); + dists[7] = res.first; + grads[21] = res.second[0]; + grads[22] = res.second[1]; + grads[23] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[32], y[33], y[34], y[35]); + dists[8] = res.first; + grads[24] = res.second[0]; + grads[25] = res.second[1]; + grads[26] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[36], y[37], y[38], y[39]); + dists[9] = res.first; + grads[27] = res.second[0]; + grads[28] = res.second[1]; + grads[29] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[40], y[41], y[42], y[43]); + dists[10] = res.first; + grads[30] = res.second[0]; + grads[31] = res.second[1]; + grads[32] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[44], y[45], y[46], y[47]); + dists[11] = res.first; + grads[33] = res.second[0]; + grads[34] = res.second[1]; + grads[35] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[48], y[49], y[50], y[51]); + dists[12] = res.first; + grads[36] = res.second[0]; + grads[37] = res.second[1]; + grads[38] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[52], y[53], y[54], y[55]); + dists[13] = res.first; + grads[39] = res.second[0]; + grads[40] = res.second[1]; + grads[41] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[56], y[57], y[58], y[59]); + dists[14] = res.first; + grads[42] = res.second[0]; + grads[43] = res.second[1]; + grads[44] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[60], y[61], y[62], y[63]); + dists[15] = res.first; + grads[45] = res.second[0]; + grads[46] = res.second[1]; + grads[47] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[64], y[65], y[66], y[67]); + dists[16] = res.first; + grads[48] = res.second[0]; + grads[49] = res.second[1]; + grads[50] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[68], y[69], y[70], y[71]); + dists[17] = res.first; + grads[51] = res.second[0]; + grads[52] = res.second[1]; + grads[53] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[72], y[73], y[74], y[75]); + dists[18] = res.first; + grads[54] = res.second[0]; + grads[55] = res.second[1]; + grads[56] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[76], y[77], y[78], y[79]); + dists[19] = res.first; + grads[57] = res.second[0]; + grads[58] = res.second[1]; + grads[59] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[80], y[81], y[82], y[83]); + dists[20] = res.first; + grads[60] = res.second[0]; + grads[61] = res.second[1]; + grads[62] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[84], y[85], y[86], y[87]); + dists[21] = res.first; + grads[63] = res.second[0]; + grads[64] = res.second[1]; + grads[65] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[88], y[89], y[90], y[91]); + dists[22] = res.first; + grads[66] = res.second[0]; + grads[67] = res.second[1]; + grads[68] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[92], y[93], y[94], y[95]); + dists[23] = res.first; + grads[69] = res.second[0]; + grads[70] = res.second[1]; + grads[71] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[96], y[97], y[98], y[99]); + dists[24] = res.first; + grads[72] = res.second[0]; + grads[73] = res.second[1]; + grads[74] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[100], y[101], y[102], y[103]); + dists[25] = res.first; + grads[75] = res.second[0]; + grads[76] = res.second[1]; + grads[77] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[104], y[105], y[106], y[107]); + dists[26] = res.first; + grads[78] = res.second[0]; + grads[79] = res.second[1]; + grads[80] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[108], y[109], y[110], y[111]); + dists[27] = res.first; + grads[81] = res.second[0]; + grads[82] = res.second[1]; + grads[83] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[112], y[113], y[114], y[115]); + dists[28] = res.first; + grads[84] = res.second[0]; + grads[85] = res.second[1]; + grads[86] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[116], y[117], y[118], y[119]); + dists[29] = res.first; + grads[87] = res.second[0]; + grads[88] = res.second[1]; + grads[89] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[120], y[121], y[122], y[123]); + dists[30] = res.first; + grads[90] = res.second[0]; + grads[91] = res.second[1]; + grads[92] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[124], y[125], y[126], y[127]); + dists[31] = res.first; + grads[93] = res.second[0]; + grads[94] = res.second[1]; + grads[95] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[128], y[129], y[130], y[131]); + dists[32] = res.first; + grads[96] = res.second[0]; + grads[97] = res.second[1]; + grads[98] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[132], y[133], y[134], y[135]); + dists[33] = res.first; + grads[99] = res.second[0]; + grads[100] = res.second[1]; + grads[101] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[136], y[137], y[138], y[139]); + dists[34] = res.first; + grads[102] = res.second[0]; + grads[103] = res.second[1]; + grads[104] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[140], y[141], y[142], y[143]); + dists[35] = res.first; + grads[105] = res.second[0]; + grads[106] = res.second[1]; + grads[107] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[144], y[145], y[146], y[147]); + dists[36] = res.first; + grads[108] = res.second[0]; + grads[109] = res.second[1]; + grads[110] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[148], y[149], y[150], y[151]); + dists[37] = res.first; + grads[111] = res.second[0]; + grads[112] = res.second[1]; + grads[113] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[152], y[153], y[154], y[155]); + dists[38] = res.first; + grads[114] = res.second[0]; + grads[115] = res.second[1]; + grads[116] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[156], y[157], y[158], y[159]); + dists[39] = res.first; + grads[117] = res.second[0]; + grads[118] = res.second[1]; + grads[119] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[160], y[161], y[162], y[163]); + dists[40] = res.first; + grads[120] = res.second[0]; + grads[121] = res.second[1]; + grads[122] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[164], y[165], y[166], y[167]); + dists[41] = res.first; + grads[123] = res.second[0]; + grads[124] = res.second[1]; + grads[125] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[168], y[169], y[170], y[171]); + dists[42] = res.first; + grads[126] = res.second[0]; + grads[127] = res.second[1]; + grads[128] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[172], y[173], y[174], y[175]); + dists[43] = res.first; + grads[129] = res.second[0]; + grads[130] = res.second[1]; + grads[131] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[176], y[177], y[178], y[179]); + dists[44] = res.first; + grads[132] = res.second[0]; + grads[133] = res.second[1]; + grads[134] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[180], y[181], y[182], y[183]); + dists[45] = res.first; + grads[135] = res.second[0]; + grads[136] = res.second[1]; + grads[137] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[184], y[185], y[186], y[187]); + dists[46] = res.first; + grads[138] = res.second[0]; + grads[139] = res.second[1]; + grads[140] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[188], y[189], y[190], y[191]); + dists[47] = res.first; + grads[141] = res.second[0]; + grads[142] = res.second[1]; + grads[143] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[192], y[193], y[194], y[195]); + dists[48] = res.first; + grads[144] = res.second[0]; + grads[145] = res.second[1]; + grads[146] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[196], y[197], y[198], y[199]); + dists[49] = res.first; + grads[147] = res.second[0]; + grads[148] = res.second[1]; + grads[149] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[200], y[201], y[202], y[203]); + dists[50] = res.first; + grads[150] = res.second[0]; + grads[151] = res.second[1]; + grads[152] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[204], y[205], y[206], y[207]); + dists[51] = res.first; + grads[153] = res.second[0]; + grads[154] = res.second[1]; + grads[155] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[208], y[209], y[210], y[211]); + dists[52] = res.first; + grads[156] = res.second[0]; + grads[157] = res.second[1]; + grads[158] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[212], y[213], y[214], y[215]); + dists[53] = res.first; + grads[159] = res.second[0]; + grads[160] = res.second[1]; + grads[161] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[216], y[217], y[218], y[219]); + dists[54] = res.first; + grads[162] = res.second[0]; + grads[163] = res.second[1]; + grads[164] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[220], y[221], y[222], y[223]); + dists[55] = res.first; + grads[165] = res.second[0]; + grads[166] = res.second[1]; + grads[167] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[224], y[225], y[226], y[227]); + dists[56] = res.first; + grads[168] = res.second[0]; + grads[169] = res.second[1]; + grads[170] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[228], y[229], y[230], y[231]); + dists[57] = res.first; + grads[171] = res.second[0]; + grads[172] = res.second[1]; + grads[173] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[232], y[233], y[234], y[235]); + dists[58] = res.first; + grads[174] = res.second[0]; + grads[175] = res.second[1]; + grads[176] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[236], y[237], y[238], y[239]); + dists[59] = res.first; + grads[177] = res.second[0]; + grads[178] = res.second[1]; + grads[179] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[240], y[241], y[242], y[243]); + dists[60] = res.first; + grads[180] = res.second[0]; + grads[181] = res.second[1]; + grads[182] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[244], y[245], y[246], y[247]); + dists[61] = res.first; + grads[183] = res.second[0]; + grads[184] = res.second[1]; + grads[185] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[248], y[249], y[250], y[251]); + dists[62] = res.first; + grads[186] = res.second[0]; + grads[187] = res.second[1]; + grads[188] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[252], y[253], y[254], y[255]); + dists[63] = res.first; + grads[189] = res.second[0]; + grads[190] = res.second[1]; + grads[191] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[256], y[257], y[258], y[259]); + dists[64] = res.first; + grads[192] = res.second[0]; + grads[193] = res.second[1]; + grads[194] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[260], y[261], y[262], y[263]); + dists[65] = res.first; + grads[195] = res.second[0]; + grads[196] = res.second[1]; + grads[197] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[264], y[265], y[266], y[267]); + dists[66] = res.first; + grads[198] = res.second[0]; + grads[199] = res.second[1]; + grads[200] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[268], y[269], y[270], y[271]); + dists[67] = res.first; + grads[201] = res.second[0]; + grads[202] = res.second[1]; + grads[203] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[272], y[273], y[274], y[275]); + dists[68] = res.first; + grads[204] = res.second[0]; + grads[205] = res.second[1]; + grads[206] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[276], y[277], y[278], y[279]); + dists[69] = res.first; + grads[207] = res.second[0]; + grads[208] = res.second[1]; + grads[209] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[280], y[281], y[282], y[283]); + dists[70] = res.first; + grads[210] = res.second[0]; + grads[211] = res.second[1]; + grads[212] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[284], y[285], y[286], y[287]); + dists[71] = res.first; + grads[213] = res.second[0]; + grads[214] = res.second[1]; + grads[215] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[288], y[289], y[290], y[291]); + dists[72] = res.first; + grads[216] = res.second[0]; + grads[217] = res.second[1]; + grads[218] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[292], y[293], y[294], y[295]); + dists[73] = res.first; + grads[219] = res.second[0]; + grads[220] = res.second[1]; + grads[221] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[296], y[297], y[298], y[299]); + dists[74] = res.first; + grads[222] = res.second[0]; + grads[223] = res.second[1]; + grads[224] = res.second[2]; + } + + return {dists, grads}; + } + + template + inline static void d_collision_d_q( + const ConfigurationBlock &q_in, + const FloatVector &gradients, + ConfigurationBlock &out) noexcept + { + // Generated code expects a single input array 'x' containing + // [q_0, ..., q_n, gx_0, gy_0, gz_0, ..., gx_m, gy_m, gz_m] + std::array, 14 + 75 * 3 + 33 * 3> x; + + // Copy q + for (std::size_t i = 0; i < 14; ++i) + { + x[i] = q_in[i]; + } + + // Copy gradients + for (std::size_t i = 0; i < 75 * 3; ++i) + { + x[14 + i] = gradients[i]; + } + + std::array, 174> v; + std::array, 14> y; + + v[0] = cos(x[0]); + v[1] = sin(x[0]); + v[2] = 0.707108079859474 * v[0] + 0.707105482511236 * v[1]; + v[3] = 0.259027384507773 + 0.069 * v[2]; + v[4] = 0.707105482511236 * v[0] + -0.707108079859474 * v[1]; + v[5] = 0.0640272398484633 + 0.069 * v[4]; + v[6] = cos(x[1]); + v[1] = -v[1]; + v[7] = 0.707108079859474 * v[1] + 0.707105482511236 * v[0]; + v[8] = sin(x[1]); + v[9] = 4.89663865010925e-12 * v[8]; + v[10] = v[2] * v[6] + v[7] * v[9]; + v[11] = v[3] + 0.102 * v[10]; + v[1] = 0.707105482511236 * v[1] + -0.707108079859474 * v[0]; + v[9] = v[4] * v[6] + v[1] * v[9]; + v[0] = v[5] + 0.102 * v[9]; + v[12] = cos(x[2]); + v[13] = sin(x[2]); + v[14] = 4.89663865010925e-12 * v[12] + -4.89658313895802e-12 * v[13]; + v[15] = -v[8]; + v[16] = 4.89663865010925e-12 * v[6]; + v[4] = v[4] * v[15] + v[1] * v[16]; + v[17] = 5.55111512312578e-17 * v[12] + v[13]; + v[18] = v[9] * v[14] + v[4] * v[12] + v[1] * v[17]; + v[19] = v[9] + -4.89658313895802e-12 * v[4] + 4.89663865010925e-12 * v[1]; + v[20] = -0.02 * v[18] + 0.22 * v[19]; + v[16] = v[2] * v[15] + v[7] * v[16]; + v[15] = v[10] * v[14] + v[16] * v[12] + v[7] * v[17]; + v[2] = v[10] + -4.89658313895802e-12 * v[16] + 4.89663865010925e-12 * v[7]; + v[21] = -0.02 * v[15] + 0.22 * v[2]; + v[22] = v[20] * x[36] - v[21] * x[35]; + v[23] = -0.01 * v[18] + 0.11 * v[19]; + v[24] = -0.01 * v[15] + 0.11 * v[2]; + v[25] = v[23] * x[39] - v[24] * x[38]; + v[26] = v[11] + 0.069 * v[15] + 0.26242 * v[2]; + v[27] = v[0] + 0.069 * v[18] + 0.26242 * v[19]; + v[28] = cos(x[3]); + v[29] = sin(x[3]); + v[30] = 4.89663865010925e-12 * v[28] + v[29]; + v[13] = -v[13]; + v[31] = 4.89663865010925e-12 * v[13] + -4.89658313895802e-12 * v[12]; + v[32] = 5.55111512312578e-17 * v[13] + v[12]; + v[16] = v[10] * v[31] + v[16] * v[13] + v[7] * v[32]; + v[10] = 5.55111512312578e-17 * v[28] + 4.89663865010925e-12 * v[29]; + v[7] = v[28] + -4.89658313895802e-12 * v[29]; + v[33] = v[15] * v[30] + v[16] * v[10] + v[2] * v[7]; + v[34] = v[26] + 0.10359 * v[33]; + v[4] = v[9] * v[31] + v[4] * v[13] + v[1] * v[32]; + v[9] = v[18] * v[30] + v[4] * v[10] + v[19] * v[7]; + v[1] = v[27] + 0.10359 * v[9]; + v[29] = -v[29]; + v[35] = 4.89663865010925e-12 * v[29] + v[28]; + v[36] = 5.55111512312578e-17 * v[29] + 4.89663865010925e-12 * v[28]; + v[29] = v[29] + -4.89658313895802e-12 * v[28]; + v[28] = v[18] * v[35] + v[4] * v[36] + v[19] * v[29]; + v[4] = -4.89658313895802e-12 * v[18] + v[4]; + v[37] = v[9] + -4.89658313895802e-12 * v[28] + 4.89663865010925e-12 * v[4]; + v[38] = 0.22 * v[37]; + v[39] = v[15] * v[35] + v[16] * v[36] + v[2] * v[29]; + v[16] = -4.89658313895802e-12 * v[15] + v[16]; + v[40] = v[33] + -4.89658313895802e-12 * v[39] + 4.89663865010925e-12 * v[16]; + v[41] = 0.22 * v[40]; + v[42] = v[38] * x[51] - v[41] * x[50]; + v[43] = 0.11 * v[37]; + v[44] = 0.11 * v[40]; + v[45] = v[43] * x[54] - v[44] * x[53]; + v[46] = cos(x[4]); + v[47] = sin(x[4]); + v[48] = 4.89663865010925e-12 * v[46] + -4.89658313895802e-12 * v[47]; + v[49] = 5.55111512312578e-17 * v[46] + v[47]; + v[50] = v[33] * v[48] + v[39] * v[46] + v[16] * v[49]; + v[51] = v[34] + 0.01 * v[50] + 0.2707 * v[40]; + v[52] = v[9] * v[48] + v[28] * v[46] + v[4] * v[49]; + v[53] = v[1] + 0.01 * v[52] + 0.2707 * v[37]; + v[47] = -v[47]; + v[54] = 4.89663865010925e-12 * v[47] + -4.89658313895802e-12 * v[46]; + v[55] = 5.55111512312578e-17 * v[47] + v[46]; + v[4] = v[9] * v[54] + v[28] * v[47] + v[4] * v[55]; + v[28] = -4.89658313895802e-12 * v[52] + v[4]; + v[9] = 0.03 * v[28]; + v[16] = v[33] * v[54] + v[39] * v[47] + v[16] * v[55]; + v[39] = -4.89658313895802e-12 * v[50] + v[16]; + v[33] = 0.03 * v[39]; + v[56] = v[9] * x[57] - v[33] * x[56]; + v[57] = -0.03 * v[28]; + v[58] = -0.03 * v[39]; + v[59] = v[57] * x[60] - v[58] * x[59]; + v[60] = cos(x[5]); + v[61] = sin(x[5]); + v[62] = 4.89663865010925e-12 * v[60] + v[61]; + v[63] = 5.55111512312578e-17 * v[60] + 4.89663865010925e-12 * v[61]; + v[64] = v[60] + -4.89658313895802e-12 * v[61]; + v[65] = v[50] * v[62] + v[16] * v[63] + v[40] * v[64]; + v[66] = v[51] + 0.115975 * v[65]; + v[67] = v[52] * v[62] + v[4] * v[63] + v[37] * v[64]; + v[68] = v[53] + 0.115975 * v[67]; + v[61] = -v[61]; + v[69] = 4.89663865010925e-12 * v[61] + v[60]; + v[70] = 5.55111512312578e-17 * v[61] + 4.89663865010925e-12 * v[60]; + v[61] = v[61] + -4.89658313895802e-12 * v[60]; + v[4] = v[52] * v[69] + v[4] * v[70] + v[37] * v[61]; + v[52] = v[67] + -4.89658313895802e-12 * v[4] + 4.89663865010925e-12 * v[28]; + v[60] = 0.02 * v[52]; + v[16] = v[50] * v[69] + v[16] * v[70] + v[40] * v[61]; + v[50] = v[65] + -4.89658313895802e-12 * v[16] + 4.89663865010925e-12 * v[39]; + v[71] = 0.02 * v[50]; + v[72] = v[60] * x[63] - v[71] * x[62]; + v[73] = -0.04 * v[52]; + v[74] = -0.04 * v[50]; + v[75] = v[73] * x[66] - v[74] * x[65]; + v[76] = cos(x[6]); + v[77] = sin(x[6]); + v[78] = 4.89663865010925e-12 * v[76] + -4.89658313895802e-12 * v[77]; + v[79] = 5.55111512312578e-17 * v[76] + v[77]; + v[80] = v[67] * v[78] + v[4] * v[76] + v[28] * v[79]; + v[81] = 0.01 * v[80] + 0.09355 * v[52]; + v[82] = v[65] * v[78] + v[16] * v[76] + v[39] * v[79]; + v[83] = 0.01 * v[82] + 0.09355 * v[50]; + v[84] = v[81] * x[69] - v[83] * x[68]; + v[77] = -v[77]; + v[85] = 4.89663865010925e-12 * v[77] + -4.89658313895802e-12 * v[76]; + v[86] = 5.55111512312578e-17 * v[77] + v[76]; + v[4] = v[67] * v[85] + v[4] * v[77] + v[28] * v[86]; + v[67] = 0.02 * v[4] + 0.13855 * v[52]; + v[16] = v[65] * v[85] + v[16] * v[77] + v[39] * v[86]; + v[65] = 0.02 * v[16] + 0.13855 * v[50]; + v[39] = v[67] * x[72] - v[65] * x[71]; + v[28] = -0.02 * v[4] + 0.13855 * v[52]; + v[87] = -0.02 * v[16] + 0.13855 * v[50]; + v[88] = v[28] * x[75] - v[87] * x[74]; + v[89] = -0.005 * v[80] + 0.081333 * v[4] + 0.16655 * v[52]; + v[90] = -0.005 * v[82] + 0.081333 * v[16] + 0.16655 * v[50]; + v[91] = v[89] * x[78] - v[90] * x[77]; + v[92] = -0.005 * v[80] + 0.057333 * v[4] + 0.16655 * v[52]; + v[93] = -0.005 * v[82] + 0.057333 * v[16] + 0.16655 * v[50]; + v[94] = v[92] * x[81] - v[93] * x[80]; + v[95] = 0.086583 * v[4] + 0.18855 * v[52]; + v[96] = 0.086583 * v[16] + 0.18855 * v[50]; + v[97] = v[95] * x[84] - v[96] * x[83]; + v[98] = 0.086583 * v[4] + 0.20855 * v[52]; + v[99] = 0.086583 * v[16] + 0.20855 * v[50]; + v[100] = v[98] * x[87] - v[99] * x[86]; + v[101] = 0.086583 * v[4] + 0.22855 * v[52]; + v[102] = 0.086583 * v[16] + 0.22855 * v[50]; + v[103] = v[101] * x[90] - v[102] * x[89]; + v[104] = 0.01 * v[80] + 0.082083 * v[4] + 0.26625 * v[52]; + v[105] = 0.01 * v[82] + 0.082083 * v[16] + 0.26625 * v[50]; + v[106] = v[104] * x[93] - v[105] * x[92]; + v[107] = -0.01 * v[80] + 0.082083 * v[4] + 0.26625 * v[52]; + v[108] = -0.01 * v[82] + 0.082083 * v[16] + 0.26625 * v[50]; + v[109] = v[107] * x[96] - v[108] * x[95]; + v[110] = -0.01 * v[80] + 0.082083 * v[4] + 0.24625 * v[52]; + v[111] = -0.01 * v[82] + 0.082083 * v[16] + 0.24625 * v[50]; + v[112] = v[110] * x[99] - v[111] * x[98]; + v[113] = 0.01 * v[80] + 0.082083 * v[4] + 0.24625 * v[52]; + v[114] = 0.01 * v[82] + 0.082083 * v[16] + 0.24625 * v[50]; + v[115] = v[113] * x[102] - v[114] * x[101]; + v[116] = 0.005 * v[80] + -0.059333 * v[4] + 0.16655 * v[52]; + v[117] = 0.005 * v[82] + -0.059333 * v[16] + 0.16655 * v[50]; + v[118] = v[116] * x[105] - v[117] * x[104]; + v[119] = 0.005 * v[80] + -0.079333 * v[4] + 0.16655 * v[52]; + v[120] = 0.005 * v[82] + -0.079333 * v[16] + 0.16655 * v[50]; + v[121] = v[119] * x[108] - v[120] * x[107]; + v[122] = -0.086583 * v[4] + 0.18855 * v[52]; + v[123] = -0.086583 * v[16] + 0.18855 * v[50]; + v[124] = v[122] * x[111] - v[123] * x[110]; + v[125] = -0.086583 * v[4] + 0.20855 * v[52]; + v[126] = -0.086583 * v[16] + 0.20855 * v[50]; + v[127] = v[125] * x[114] - v[126] * x[113]; + v[128] = -0.086583 * v[4] + 0.22855 * v[52]; + v[129] = -0.086583 * v[16] + 0.22855 * v[50]; + v[130] = v[128] * x[117] - v[129] * x[116]; + v[131] = 0.01 * v[80] + -0.082083 * v[4] + 0.26625 * v[52]; + v[132] = 0.01 * v[82] + -0.082083 * v[16] + 0.26625 * v[50]; + v[133] = v[131] * x[120] - v[132] * x[119]; + v[134] = -0.01 * v[80] + -0.082083 * v[4] + 0.26625 * v[52]; + v[135] = -0.01 * v[82] + -0.082083 * v[16] + 0.26625 * v[50]; + v[136] = v[134] * x[123] - v[135] * x[122]; + v[137] = -0.01 * v[80] + -0.082083 * v[4] + 0.24625 * v[52]; + v[138] = -0.01 * v[82] + -0.082083 * v[16] + 0.24625 * v[50]; + v[139] = v[137] * x[126] - v[138] * x[125]; + v[140] = 0.01 * v[80] + -0.082083 * v[4] + 0.24625 * v[52]; + v[141] = 0.01 * v[82] + -0.082083 * v[16] + 0.24625 * v[50]; + v[142] = v[140] * x[129] - v[141] * x[128]; + v[19] = -0.00999999977648258 * v[18] + 0.109999999403954 * v[19]; + v[2] = -0.00999999977648258 * v[15] + 0.109999999403954 * v[2]; + v[15] = v[19] * x[252] - v[2] * x[251]; + v[37] = 0.109999999403954 * v[37]; + v[40] = 0.109999999403954 * v[40]; + v[18] = v[37] * x[258] - v[40] * x[257]; + v[143] = -0.0149999987334013 * v[52]; + v[144] = -0.0149999987334013 * v[50]; + v[145] = v[143] * x[264] - v[144] * x[263]; + v[146] = 0.00999999977648258 * v[80] + 0.0935499966144562 * v[52]; + v[147] = 0.00999999977648258 * v[82] + 0.0935499966144562 * v[50]; + v[148] = v[146] * x[267] - v[147] * x[266]; + v[149] = 0.138549998402596 * v[52]; + v[150] = 0.138549998402596 * v[50]; + v[151] = v[149] * x[270] - v[150] * x[269]; + v[152] = -0.00499999988824129 * v[80] + 0.0693330019712448 * v[4] + 0.166549995541573 * v[52]; + v[153] = -0.00499999988824129 * v[82] + 0.0693330019712448 * v[16] + 0.166549995541573 * v[50]; + v[154] = v[152] * x[273] - v[153] * x[272]; + v[155] = 0.0865830034017563 * v[4] + 0.208550006151199 * v[52]; + v[156] = 0.0865830034017563 * v[16] + 0.208550006151199 * v[50]; + v[157] = v[155] * x[276] - v[156] * x[275]; + v[158] = 0.0820830017328262 * v[4] + 0.256249994039536 * v[52]; + v[159] = 0.0820830017328262 * v[16] + 0.256249994039536 * v[50]; + v[160] = v[158] * x[279] - v[159] * x[278]; + v[80] = 0.00499999988824129 * v[80] + -0.0693330019712448 * v[4] + 0.166549995541573 * v[52]; + v[82] = 0.00499999988824129 * v[82] + -0.0693330019712448 * v[16] + 0.166549995541573 * v[50]; + v[161] = v[80] * x[282] - v[82] * x[281]; + v[162] = -0.0865830034017563 * v[4] + 0.208550006151199 * v[52]; + v[163] = -0.0865830034017563 * v[16] + 0.208550006151199 * v[50]; + v[164] = v[162] * x[285] - v[163] * x[284]; + v[4] = -0.0820830017328262 * v[4] + 0.256249994039536 * v[52]; + v[16] = -0.0820830017328262 * v[16] + 0.256249994039536 * v[50]; + v[50] = v[4] * x[288] - v[16] * x[287]; + y[0] = + (0.259027384507773 - v[3]) * x[32] + (-0.0640272398484633 - (0. - v[5])) * x[33] + + (0.259027384507773 - v[11]) * x[35] + (-0.0640272398484633 - (0. - v[0])) * x[36] + v[22] + + (0.259027384507773 - v[11]) * x[38] + (-0.0640272398484633 - (0. - v[0])) * x[39] + v[25] + + (0.259027384507773 - v[11]) * x[41] + (-0.0640272398484633 - (0. - v[0])) * x[42] + + (0.259027384507773 - v[26]) * x[44] + (-0.0640272398484633 - (0. - v[27])) * x[45] + + (0.259027384507773 - v[34]) * x[47] + (-0.0640272398484633 - (0. - v[1])) * x[48] + + (0.259027384507773 - v[34]) * x[50] + (-0.0640272398484633 - (0. - v[1])) * x[51] + v[42] + + (0.259027384507773 - v[34]) * x[53] + (-0.0640272398484633 - (0. - v[1])) * x[54] + v[45] + + (0.259027384507773 - v[51]) * x[56] + (-0.0640272398484633 - (0. - v[53])) * x[57] + v[56] + + (0.259027384507773 - v[51]) * x[59] + (-0.0640272398484633 - (0. - v[53])) * x[60] + v[59] + + (0.259027384507773 - v[66]) * x[62] + (-0.0640272398484633 - (0. - v[68])) * x[63] + v[72] + + (0.259027384507773 - v[66]) * x[65] + (-0.0640272398484633 - (0. - v[68])) * x[66] + v[75] + + (0.259027384507773 - v[66]) * x[68] + (-0.0640272398484633 - (0. - v[68])) * x[69] + v[84] + + (0.259027384507773 - v[66]) * x[71] + (-0.0640272398484633 - (0. - v[68])) * x[72] + v[39] + + (0.259027384507773 - v[66]) * x[74] + (-0.0640272398484633 - (0. - v[68])) * x[75] + v[88] + + (0.259027384507773 - v[66]) * x[77] + (-0.0640272398484633 - (0. - v[68])) * x[78] + v[91] + + (0.259027384507773 - v[66]) * x[80] + (-0.0640272398484633 - (0. - v[68])) * x[81] + v[94] + + (0.259027384507773 - v[66]) * x[83] + (-0.0640272398484633 - (0. - v[68])) * x[84] + v[97] + + (0.259027384507773 - v[66]) * x[86] + (-0.0640272398484633 - (0. - v[68])) * x[87] + v[100] + + (0.259027384507773 - v[66]) * x[89] + (-0.0640272398484633 - (0. - v[68])) * x[90] + v[103] + + (0.259027384507773 - v[66]) * x[92] + (-0.0640272398484633 - (0. - v[68])) * x[93] + v[106] + + (0.259027384507773 - v[66]) * x[95] + (-0.0640272398484633 - (0. - v[68])) * x[96] + v[109] + + (0.259027384507773 - v[66]) * x[98] + (-0.0640272398484633 - (0. - v[68])) * x[99] + v[112] + + (0.259027384507773 - v[66]) * x[101] + (-0.0640272398484633 - (0. - v[68])) * x[102] + + v[115] + (0.259027384507773 - v[66]) * x[104] + + (-0.0640272398484633 - (0. - v[68])) * x[105] + v[118] + + (0.259027384507773 - v[66]) * x[107] + (-0.0640272398484633 - (0. - v[68])) * x[108] + + v[121] + (0.259027384507773 - v[66]) * x[110] + + (-0.0640272398484633 - (0. - v[68])) * x[111] + v[124] + + (0.259027384507773 - v[66]) * x[113] + (-0.0640272398484633 - (0. - v[68])) * x[114] + + v[127] + (0.259027384507773 - v[66]) * x[116] + + (-0.0640272398484633 - (0. - v[68])) * x[117] + v[130] + + (0.259027384507773 - v[66]) * x[119] + (-0.0640272398484633 - (0. - v[68])) * x[120] + + v[133] + (0.259027384507773 - v[66]) * x[122] + + (-0.0640272398484633 - (0. - v[68])) * x[123] + v[136] + + (0.259027384507773 - v[66]) * x[125] + (-0.0640272398484633 - (0. - v[68])) * x[126] + + v[139] + (0.259027384507773 - v[66]) * x[128] + + (-0.0640272398484633 - (0. - v[68])) * x[129] + v[142] + (0.259027384507773 - v[3]) * x[248] + + (-0.0640272398484633 - (0. - v[5])) * x[249] + (0.259027384507773 - v[11]) * x[251] + + (-0.0640272398484633 - (0. - v[0])) * x[252] + v[15] + (0.259027384507773 - v[26]) * x[254] + + (-0.0640272398484633 - (0. - v[27])) * x[255] + (0.259027384507773 - v[34]) * x[257] + + (-0.0640272398484633 - (0. - v[1])) * x[258] + v[18] + (0.259027384507773 - v[51]) * x[260] + + (-0.0640272398484633 - (0. - v[53])) * x[261] + (0.259027384507773 - v[66]) * x[263] + + (-0.0640272398484633 - (0. - v[68])) * x[264] + v[145] + + (0.259027384507773 - v[66]) * x[266] + (-0.0640272398484633 - (0. - v[68])) * x[267] + + v[148] + (0.259027384507773 - v[66]) * x[269] + + (-0.0640272398484633 - (0. - v[68])) * x[270] + v[151] + + (0.259027384507773 - v[66]) * x[272] + (-0.0640272398484633 - (0. - v[68])) * x[273] + + v[154] + (0.259027384507773 - v[66]) * x[275] + + (-0.0640272398484633 - (0. - v[68])) * x[276] + v[157] + + (0.259027384507773 - v[66]) * x[278] + (-0.0640272398484633 - (0. - v[68])) * x[279] + + v[160] + (0.259027384507773 - v[66]) * x[281] + + (-0.0640272398484633 - (0. - v[68])) * x[282] + v[161] + + (0.259027384507773 - v[66]) * x[284] + (-0.0640272398484633 - (0. - v[68])) * x[285] + + v[164] + (0.259027384507773 - v[66]) * x[287] + + (-0.0640272398484633 - (0. - v[68])) * x[288] + v[50]; + v[52] = cos(x[0]); + v[165] = sin(x[0]); + v[166] = 0.707108079859474 * v[52] + 0.707105482511236 * v[165]; + v[167] = 0.259027384507773 + 0.069 * v[166]; + v[168] = -v[165]; + v[169] = 0.707108079859474 * v[168] + 0.707105482511236 * v[52]; + v[170] = 4.89663865010925e-12 * v[167] - 0.399976 * v[169]; + v[168] = 0.707105482511236 * v[168] + -0.707108079859474 * v[52]; + v[165] = 0.707105482511236 * v[52] + -0.707108079859474 * v[165]; + v[52] = 0.0640272398484633 + 0.069 * v[165]; + v[171] = 0.399976 * v[168] - 4.89663865010925e-12 * v[52]; + v[172] = v[52] * v[169] - v[167] * v[168]; + v[8] = -1. * v[8]; + v[173] = 0.399976 + 0.102 * v[8]; + v[6] = -1. * v[6]; + v[17] = v[8] * v[14] + v[6] * v[12] + 4.89663865010925e-12 * v[17]; + v[14] = v[8] + 2.39770700697438e-23 + -4.89658313895802e-12 * v[6]; + v[12] = -0.02 * v[17] + 0.22 * v[14]; + v[21] = v[21] * x[37] - v[12] * x[36]; + v[12] = v[12] * x[35] - v[20] * x[37]; + v[20] = -0.01 * v[17] + 0.11 * v[14]; + v[24] = v[24] * x[40] - v[20] * x[39]; + v[20] = v[20] * x[38] - v[23] * x[40]; + v[23] = v[173] + 0.069 * v[17] + 0.26242 * v[14]; + v[6] = v[8] * v[31] + v[6] * v[13] + 4.89663865010925e-12 * v[32]; + v[7] = v[17] * v[30] + v[6] * v[10] + v[14] * v[7]; + v[10] = v[23] + 0.10359 * v[7]; + v[29] = v[17] * v[35] + v[6] * v[36] + v[14] * v[29]; + v[6] = -4.89658313895802e-12 * v[17] + v[6]; + v[36] = v[7] + -4.89658313895802e-12 * v[29] + 4.89663865010925e-12 * v[6]; + v[35] = 0.22 * v[36]; + v[41] = v[41] * x[52] - v[35] * x[51]; + v[35] = v[35] * x[50] - v[38] * x[52]; + v[38] = 0.11 * v[36]; + v[44] = v[44] * x[55] - v[38] * x[54]; + v[38] = v[38] * x[53] - v[43] * x[55]; + v[49] = v[7] * v[48] + v[29] * v[46] + v[6] * v[49]; + v[48] = v[10] + 0.01 * v[49] + 0.2707 * v[36]; + v[6] = v[7] * v[54] + v[29] * v[47] + v[6] * v[55]; + v[29] = -4.89658313895802e-12 * v[49] + v[6]; + v[7] = 0.03 * v[29]; + v[33] = v[33] * x[58] - v[7] * x[57]; + v[7] = v[7] * x[56] - v[9] * x[58]; + v[9] = -0.03 * v[29]; + v[58] = v[58] * x[61] - v[9] * x[60]; + v[9] = v[9] * x[59] - v[57] * x[61]; + v[64] = v[49] * v[62] + v[6] * v[63] + v[36] * v[64]; + v[63] = v[48] + 0.115975 * v[64]; + v[6] = v[49] * v[69] + v[6] * v[70] + v[36] * v[61]; + v[49] = v[64] + -4.89658313895802e-12 * v[6] + 4.89663865010925e-12 * v[29]; + v[61] = 0.02 * v[49]; + v[71] = v[71] * x[64] - v[61] * x[63]; + v[61] = v[61] * x[62] - v[60] * x[64]; + v[60] = -0.04 * v[49]; + v[74] = v[74] * x[67] - v[60] * x[66]; + v[60] = v[60] * x[65] - v[73] * x[67]; + v[79] = v[64] * v[78] + v[6] * v[76] + v[29] * v[79]; + v[78] = 0.01 * v[79] + 0.09355 * v[49]; + v[83] = v[83] * x[70] - v[78] * x[69]; + v[78] = v[78] * x[68] - v[81] * x[70]; + v[6] = v[64] * v[85] + v[6] * v[77] + v[29] * v[86]; + v[64] = 0.02 * v[6] + 0.13855 * v[49]; + v[65] = v[65] * x[73] - v[64] * x[72]; + v[64] = v[64] * x[71] - v[67] * x[73]; + v[67] = -0.02 * v[6] + 0.13855 * v[49]; + v[87] = v[87] * x[76] - v[67] * x[75]; + v[67] = v[67] * x[74] - v[28] * x[76]; + v[28] = -0.005 * v[79] + 0.081333 * v[6] + 0.16655 * v[49]; + v[90] = v[90] * x[79] - v[28] * x[78]; + v[28] = v[28] * x[77] - v[89] * x[79]; + v[89] = -0.005 * v[79] + 0.057333 * v[6] + 0.16655 * v[49]; + v[93] = v[93] * x[82] - v[89] * x[81]; + v[89] = v[89] * x[80] - v[92] * x[82]; + v[92] = 0.086583 * v[6] + 0.18855 * v[49]; + v[96] = v[96] * x[85] - v[92] * x[84]; + v[92] = v[92] * x[83] - v[95] * x[85]; + v[95] = 0.086583 * v[6] + 0.20855 * v[49]; + v[99] = v[99] * x[88] - v[95] * x[87]; + v[95] = v[95] * x[86] - v[98] * x[88]; + v[98] = 0.086583 * v[6] + 0.22855 * v[49]; + v[102] = v[102] * x[91] - v[98] * x[90]; + v[98] = v[98] * x[89] - v[101] * x[91]; + v[101] = 0.01 * v[79] + 0.082083 * v[6] + 0.26625 * v[49]; + v[105] = v[105] * x[94] - v[101] * x[93]; + v[101] = v[101] * x[92] - v[104] * x[94]; + v[104] = -0.01 * v[79] + 0.082083 * v[6] + 0.26625 * v[49]; + v[108] = v[108] * x[97] - v[104] * x[96]; + v[104] = v[104] * x[95] - v[107] * x[97]; + v[107] = -0.01 * v[79] + 0.082083 * v[6] + 0.24625 * v[49]; + v[111] = v[111] * x[100] - v[107] * x[99]; + v[107] = v[107] * x[98] - v[110] * x[100]; + v[110] = 0.01 * v[79] + 0.082083 * v[6] + 0.24625 * v[49]; + v[114] = v[114] * x[103] - v[110] * x[102]; + v[110] = v[110] * x[101] - v[113] * x[103]; + v[113] = 0.005 * v[79] + -0.059333 * v[6] + 0.16655 * v[49]; + v[117] = v[117] * x[106] - v[113] * x[105]; + v[113] = v[113] * x[104] - v[116] * x[106]; + v[116] = 0.005 * v[79] + -0.079333 * v[6] + 0.16655 * v[49]; + v[120] = v[120] * x[109] - v[116] * x[108]; + v[116] = v[116] * x[107] - v[119] * x[109]; + v[119] = -0.086583 * v[6] + 0.18855 * v[49]; + v[123] = v[123] * x[112] - v[119] * x[111]; + v[119] = v[119] * x[110] - v[122] * x[112]; + v[122] = -0.086583 * v[6] + 0.20855 * v[49]; + v[126] = v[126] * x[115] - v[122] * x[114]; + v[122] = v[122] * x[113] - v[125] * x[115]; + v[125] = -0.086583 * v[6] + 0.22855 * v[49]; + v[129] = v[129] * x[118] - v[125] * x[117]; + v[125] = v[125] * x[116] - v[128] * x[118]; + v[128] = 0.01 * v[79] + -0.082083 * v[6] + 0.26625 * v[49]; + v[132] = v[132] * x[121] - v[128] * x[120]; + v[128] = v[128] * x[119] - v[131] * x[121]; + v[131] = -0.01 * v[79] + -0.082083 * v[6] + 0.26625 * v[49]; + v[135] = v[135] * x[124] - v[131] * x[123]; + v[131] = v[131] * x[122] - v[134] * x[124]; + v[134] = -0.01 * v[79] + -0.082083 * v[6] + 0.24625 * v[49]; + v[138] = v[138] * x[127] - v[134] * x[126]; + v[134] = v[134] * x[125] - v[137] * x[127]; + v[137] = 0.01 * v[79] + -0.082083 * v[6] + 0.24625 * v[49]; + v[141] = v[141] * x[130] - v[137] * x[129]; + v[137] = v[137] * x[128] - v[140] * x[130]; + v[14] = -0.00999999977648258 * v[17] + 0.109999999403954 * v[14]; + v[2] = v[2] * x[253] - v[14] * x[252]; + v[14] = v[14] * x[251] - v[19] * x[253]; + v[36] = 0.109999999403954 * v[36]; + v[40] = v[40] * x[259] - v[36] * x[258]; + v[36] = v[36] * x[257] - v[37] * x[259]; + v[37] = -0.0149999987334013 * v[49]; + v[144] = v[144] * x[265] - v[37] * x[264]; + v[37] = v[37] * x[263] - v[143] * x[265]; + v[143] = 0.00999999977648258 * v[79] + 0.0935499966144562 * v[49]; + v[147] = v[147] * x[268] - v[143] * x[267]; + v[143] = v[143] * x[266] - v[146] * x[268]; + v[146] = 0.138549998402596 * v[49]; + v[150] = v[150] * x[271] - v[146] * x[270]; + v[146] = v[146] * x[269] - v[149] * x[271]; + v[149] = -0.00499999988824129 * v[79] + 0.0693330019712448 * v[6] + 0.166549995541573 * v[49]; + v[153] = v[153] * x[274] - v[149] * x[273]; + v[149] = v[149] * x[272] - v[152] * x[274]; + v[152] = 0.0865830034017563 * v[6] + 0.208550006151199 * v[49]; + v[156] = v[156] * x[277] - v[152] * x[276]; + v[152] = v[152] * x[275] - v[155] * x[277]; + v[155] = 0.0820830017328262 * v[6] + 0.256249994039536 * v[49]; + v[159] = v[159] * x[280] - v[155] * x[279]; + v[155] = v[155] * x[278] - v[158] * x[280]; + v[79] = 0.00499999988824129 * v[79] + -0.0693330019712448 * v[6] + 0.166549995541573 * v[49]; + v[82] = v[82] * x[283] - v[79] * x[282]; + v[79] = v[79] * x[281] - v[80] * x[283]; + v[80] = -0.0865830034017563 * v[6] + 0.208550006151199 * v[49]; + v[163] = v[163] * x[286] - v[80] * x[285]; + v[80] = v[80] * x[284] - v[162] * x[286]; + v[6] = -0.0820830017328262 * v[6] + 0.256249994039536 * v[49]; + v[16] = v[16] * x[289] - v[6] * x[288]; + v[6] = v[6] * x[287] - v[4] * x[289]; + y[1] = (v[170] - (4.89663865010925e-12 * v[3] - 0.399976 * v[169])) * x[32] + + (v[171] - (0.399976 * v[168] - 4.89663865010925e-12 * v[5])) * x[33] + + (v[172] - (v[5] * v[169] - v[3] * v[168])) * x[34] + + (v[170] - (4.89663865010925e-12 * v[11] - v[173] * v[169])) * x[35] + + (v[171] - (v[173] * v[168] - 4.89663865010925e-12 * v[0])) * x[36] + + (v[172] - (v[0] * v[169] - v[11] * v[168])) * x[37] + v[168] * v[21] + v[169] * v[12] + + 4.89663865010925e-12 * v[22] + + (v[170] - (4.89663865010925e-12 * v[11] - v[173] * v[169])) * x[38] + + (v[171] - (v[173] * v[168] - 4.89663865010925e-12 * v[0])) * x[39] + + (v[172] - (v[0] * v[169] - v[11] * v[168])) * x[40] + v[168] * v[24] + v[169] * v[20] + + 4.89663865010925e-12 * v[25] + + (v[170] - (4.89663865010925e-12 * v[11] - v[173] * v[169])) * x[41] + + (v[171] - (v[173] * v[168] - 4.89663865010925e-12 * v[0])) * x[42] + + (v[172] - (v[0] * v[169] - v[11] * v[168])) * x[43] + + (v[170] - (4.89663865010925e-12 * v[26] - v[23] * v[169])) * x[44] + + (v[171] - (v[23] * v[168] - 4.89663865010925e-12 * v[27])) * x[45] + + (v[172] - (v[27] * v[169] - v[26] * v[168])) * x[46] + + (v[170] - (4.89663865010925e-12 * v[34] - v[10] * v[169])) * x[47] + + (v[171] - (v[10] * v[168] - 4.89663865010925e-12 * v[1])) * x[48] + + (v[172] - (v[1] * v[169] - v[34] * v[168])) * x[49] + + (v[170] - (4.89663865010925e-12 * v[34] - v[10] * v[169])) * x[50] + + (v[171] - (v[10] * v[168] - 4.89663865010925e-12 * v[1])) * x[51] + + (v[172] - (v[1] * v[169] - v[34] * v[168])) * x[52] + v[168] * v[41] + v[169] * v[35] + + 4.89663865010925e-12 * v[42] + + (v[170] - (4.89663865010925e-12 * v[34] - v[10] * v[169])) * x[53] + + (v[171] - (v[10] * v[168] - 4.89663865010925e-12 * v[1])) * x[54] + + (v[172] - (v[1] * v[169] - v[34] * v[168])) * x[55] + v[168] * v[44] + v[169] * v[38] + + 4.89663865010925e-12 * v[45] + + (v[170] - (4.89663865010925e-12 * v[51] - v[48] * v[169])) * x[56] + + (v[171] - (v[48] * v[168] - 4.89663865010925e-12 * v[53])) * x[57] + + (v[172] - (v[53] * v[169] - v[51] * v[168])) * x[58] + v[168] * v[33] + v[169] * v[7] + + 4.89663865010925e-12 * v[56] + + (v[170] - (4.89663865010925e-12 * v[51] - v[48] * v[169])) * x[59] + + (v[171] - (v[48] * v[168] - 4.89663865010925e-12 * v[53])) * x[60] + + (v[172] - (v[53] * v[169] - v[51] * v[168])) * x[61] + v[168] * v[58] + v[169] * v[9] + + 4.89663865010925e-12 * v[59] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[62] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[63] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[64] + v[168] * v[71] + v[169] * v[61] + + 4.89663865010925e-12 * v[72] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[65] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[66] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[67] + v[168] * v[74] + v[169] * v[60] + + 4.89663865010925e-12 * v[75] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[68] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[69] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[70] + v[168] * v[83] + v[169] * v[78] + + 4.89663865010925e-12 * v[84] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[71] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[72] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[73] + v[168] * v[65] + v[169] * v[64] + + 4.89663865010925e-12 * v[39] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[74] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[75] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[76] + v[168] * v[87] + v[169] * v[67] + + 4.89663865010925e-12 * v[88] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[77] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[78] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[79] + v[168] * v[90] + v[169] * v[28] + + 4.89663865010925e-12 * v[91] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[80] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[81] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[82] + v[168] * v[93] + v[169] * v[89] + + 4.89663865010925e-12 * v[94] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[83] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[84] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[85] + v[168] * v[96] + v[169] * v[92] + + 4.89663865010925e-12 * v[97] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[86] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[87] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[88] + v[168] * v[99] + v[169] * v[95] + + 4.89663865010925e-12 * v[100] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[89] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[90] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[91] + v[168] * v[102] + v[169] * v[98] + + 4.89663865010925e-12 * v[103] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[92] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[93] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[94] + v[168] * v[105] + v[169] * v[101] + + 4.89663865010925e-12 * v[106] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[95] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[96] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[97] + v[168] * v[108] + v[169] * v[104] + + 4.89663865010925e-12 * v[109] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[98] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[99] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[100] + v[168] * v[111] + v[169] * v[107] + + 4.89663865010925e-12 * v[112] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[101] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[102] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[103] + v[168] * v[114] + v[169] * v[110] + + 4.89663865010925e-12 * v[115] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[104] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[105] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[106] + v[168] * v[117] + v[169] * v[113] + + 4.89663865010925e-12 * v[118] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[107] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[108] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[109] + v[168] * v[120] + v[169] * v[116] + + 4.89663865010925e-12 * v[121] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[110] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[111] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[112] + v[168] * v[123] + v[169] * v[119] + + 4.89663865010925e-12 * v[124] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[113] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[114] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[115] + v[168] * v[126] + v[169] * v[122] + + 4.89663865010925e-12 * v[127] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[116] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[117] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[118] + v[168] * v[129] + v[169] * v[125] + + 4.89663865010925e-12 * v[130] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[119] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[120] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[121] + v[168] * v[132] + v[169] * v[128] + + 4.89663865010925e-12 * v[133] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[122] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[123] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[124] + v[168] * v[135] + v[169] * v[131] + + 4.89663865010925e-12 * v[136] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[125] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[126] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[127] + v[168] * v[138] + v[169] * v[134] + + 4.89663865010925e-12 * v[139] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[128] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[129] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[130] + v[168] * v[141] + v[169] * v[137] + + 4.89663865010925e-12 * v[142] + + (v[170] - (4.89663865010925e-12 * v[3] - 0.399976 * v[169])) * x[248] + + (v[171] - (0.399976 * v[168] - 4.89663865010925e-12 * v[5])) * x[249] + + (v[172] - (v[5] * v[169] - v[3] * v[168])) * x[250] + + (v[170] - (4.89663865010925e-12 * v[11] - v[173] * v[169])) * x[251] + + (v[171] - (v[173] * v[168] - 4.89663865010925e-12 * v[0])) * x[252] + + (v[172] - (v[0] * v[169] - v[11] * v[168])) * x[253] + v[168] * v[2] + v[169] * v[14] + + 4.89663865010925e-12 * v[15] + + (v[170] - (4.89663865010925e-12 * v[26] - v[23] * v[169])) * x[254] + + (v[171] - (v[23] * v[168] - 4.89663865010925e-12 * v[27])) * x[255] + + (v[172] - (v[27] * v[169] - v[26] * v[168])) * x[256] + + (v[170] - (4.89663865010925e-12 * v[34] - v[10] * v[169])) * x[257] + + (v[171] - (v[10] * v[168] - 4.89663865010925e-12 * v[1])) * x[258] + + (v[172] - (v[1] * v[169] - v[34] * v[168])) * x[259] + v[168] * v[40] + v[169] * v[36] + + 4.89663865010925e-12 * v[18] + + (v[170] - (4.89663865010925e-12 * v[51] - v[48] * v[169])) * x[260] + + (v[171] - (v[48] * v[168] - 4.89663865010925e-12 * v[53])) * x[261] + + (v[172] - (v[53] * v[169] - v[51] * v[168])) * x[262] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[263] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[264] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[265] + v[168] * v[144] + v[169] * v[37] + + 4.89663865010925e-12 * v[145] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[266] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[267] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[268] + v[168] * v[147] + v[169] * v[143] + + 4.89663865010925e-12 * v[148] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[269] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[270] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[271] + v[168] * v[150] + v[169] * v[146] + + 4.89663865010925e-12 * v[151] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[272] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[273] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[274] + v[168] * v[153] + v[169] * v[149] + + 4.89663865010925e-12 * v[154] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[275] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[276] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[277] + v[168] * v[156] + v[169] * v[152] + + 4.89663865010925e-12 * v[157] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[278] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[279] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[280] + v[168] * v[159] + v[169] * v[155] + + 4.89663865010925e-12 * v[160] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[281] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[282] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[283] + v[168] * v[82] + v[169] * v[79] + + 4.89663865010925e-12 * v[161] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[284] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[285] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[286] + v[168] * v[163] + v[169] * v[80] + + 4.89663865010925e-12 * v[164] + + (v[170] - (4.89663865010925e-12 * v[66] - v[63] * v[169])) * x[287] + + (v[171] - (v[63] * v[168] - 4.89663865010925e-12 * v[68])) * x[288] + + (v[172] - (v[68] * v[169] - v[66] * v[168])) * x[289] + v[168] * v[16] + v[169] * v[6] + + 4.89663865010925e-12 * v[50]; + v[172] = cos(x[1]); + v[171] = sin(x[1]); + v[170] = 4.89663865010925e-12 * v[171]; + v[5] = v[166] * v[172] + v[169] * v[170]; + v[167] = v[167] + 0.102 * v[5]; + v[3] = -1. * v[171]; + v[4] = -1. * v[172]; + v[49] = v[3] + 2.39770700697438e-23 + -4.89658313895802e-12 * v[4]; + v[162] = 0.399976 + 0.102 * v[3]; + v[171] = -v[171]; + v[158] = 4.89663865010925e-12 * v[172]; + v[166] = v[166] * v[171] + v[169] * v[158]; + v[19] = v[5] + -4.89658313895802e-12 * v[166] + 4.89663865010925e-12 * v[169]; + v[17] = v[167] * v[49] - v[162] * v[19]; + v[170] = v[165] * v[172] + v[168] * v[170]; + v[158] = v[165] * v[171] + v[168] * v[158]; + v[171] = v[170] + -4.89658313895802e-12 * v[158] + 4.89663865010925e-12 * v[168]; + v[52] = v[52] + 0.102 * v[170]; + v[165] = v[162] * v[171] - v[52] * v[49]; + v[172] = v[52] * v[19] - v[167] * v[171]; + y[2] = (v[17] - (v[11] * v[49] - v[173] * v[19])) * x[35] + + (v[165] - (v[173] * v[171] - v[0] * v[49])) * x[36] + + (v[172] - (v[0] * v[19] - v[11] * v[171])) * x[37] + v[171] * v[21] + v[19] * v[12] + + v[49] * v[22] + (v[17] - (v[11] * v[49] - v[173] * v[19])) * x[38] + + (v[165] - (v[173] * v[171] - v[0] * v[49])) * x[39] + + (v[172] - (v[0] * v[19] - v[11] * v[171])) * x[40] + v[171] * v[24] + v[19] * v[20] + + v[49] * v[25] + (v[17] - (v[11] * v[49] - v[173] * v[19])) * x[41] + + (v[165] - (v[173] * v[171] - v[0] * v[49])) * x[42] + + (v[172] - (v[0] * v[19] - v[11] * v[171])) * x[43] + + (v[17] - (v[26] * v[49] - v[23] * v[19])) * x[44] + + (v[165] - (v[23] * v[171] - v[27] * v[49])) * x[45] + + (v[172] - (v[27] * v[19] - v[26] * v[171])) * x[46] + + (v[17] - (v[34] * v[49] - v[10] * v[19])) * x[47] + + (v[165] - (v[10] * v[171] - v[1] * v[49])) * x[48] + + (v[172] - (v[1] * v[19] - v[34] * v[171])) * x[49] + + (v[17] - (v[34] * v[49] - v[10] * v[19])) * x[50] + + (v[165] - (v[10] * v[171] - v[1] * v[49])) * x[51] + + (v[172] - (v[1] * v[19] - v[34] * v[171])) * x[52] + v[171] * v[41] + v[19] * v[35] + + v[49] * v[42] + (v[17] - (v[34] * v[49] - v[10] * v[19])) * x[53] + + (v[165] - (v[10] * v[171] - v[1] * v[49])) * x[54] + + (v[172] - (v[1] * v[19] - v[34] * v[171])) * x[55] + v[171] * v[44] + v[19] * v[38] + + v[49] * v[45] + (v[17] - (v[51] * v[49] - v[48] * v[19])) * x[56] + + (v[165] - (v[48] * v[171] - v[53] * v[49])) * x[57] + + (v[172] - (v[53] * v[19] - v[51] * v[171])) * x[58] + v[171] * v[33] + v[19] * v[7] + + v[49] * v[56] + (v[17] - (v[51] * v[49] - v[48] * v[19])) * x[59] + + (v[165] - (v[48] * v[171] - v[53] * v[49])) * x[60] + + (v[172] - (v[53] * v[19] - v[51] * v[171])) * x[61] + v[171] * v[58] + v[19] * v[9] + + v[49] * v[59] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[62] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[63] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[64] + v[171] * v[71] + v[19] * v[61] + + v[49] * v[72] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[65] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[66] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[67] + v[171] * v[74] + v[19] * v[60] + + v[49] * v[75] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[68] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[69] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[70] + v[171] * v[83] + v[19] * v[78] + + v[49] * v[84] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[71] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[72] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[73] + v[171] * v[65] + v[19] * v[64] + + v[49] * v[39] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[74] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[75] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[76] + v[171] * v[87] + v[19] * v[67] + + v[49] * v[88] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[77] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[78] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[79] + v[171] * v[90] + v[19] * v[28] + + v[49] * v[91] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[80] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[81] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[82] + v[171] * v[93] + v[19] * v[89] + + v[49] * v[94] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[83] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[84] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[85] + v[171] * v[96] + v[19] * v[92] + + v[49] * v[97] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[86] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[87] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[88] + v[171] * v[99] + v[19] * v[95] + + v[49] * v[100] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[89] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[90] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[91] + v[171] * v[102] + v[19] * v[98] + + v[49] * v[103] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[92] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[93] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[94] + v[171] * v[105] + v[19] * v[101] + + v[49] * v[106] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[95] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[96] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[97] + v[171] * v[108] + v[19] * v[104] + + v[49] * v[109] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[98] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[99] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[100] + v[171] * v[111] + v[19] * v[107] + + v[49] * v[112] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[101] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[102] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[103] + v[171] * v[114] + v[19] * v[110] + + v[49] * v[115] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[104] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[105] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[106] + v[171] * v[117] + v[19] * v[113] + + v[49] * v[118] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[107] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[108] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[109] + v[171] * v[120] + v[19] * v[116] + + v[49] * v[121] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[110] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[111] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[112] + v[171] * v[123] + v[19] * v[119] + + v[49] * v[124] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[113] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[114] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[115] + v[171] * v[126] + v[19] * v[122] + + v[49] * v[127] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[116] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[117] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[118] + v[171] * v[129] + v[19] * v[125] + + v[49] * v[130] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[119] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[120] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[121] + v[171] * v[132] + v[19] * v[128] + + v[49] * v[133] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[122] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[123] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[124] + v[171] * v[135] + v[19] * v[131] + + v[49] * v[136] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[125] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[126] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[127] + v[171] * v[138] + v[19] * v[134] + + v[49] * v[139] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[128] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[129] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[130] + v[171] * v[141] + v[19] * v[137] + + v[49] * v[142] + (v[17] - (v[11] * v[49] - v[173] * v[19])) * x[251] + + (v[165] - (v[173] * v[171] - v[0] * v[49])) * x[252] + + (v[172] - (v[0] * v[19] - v[11] * v[171])) * x[253] + v[171] * v[2] + v[19] * v[14] + + v[49] * v[15] + (v[17] - (v[26] * v[49] - v[23] * v[19])) * x[254] + + (v[165] - (v[23] * v[171] - v[27] * v[49])) * x[255] + + (v[172] - (v[27] * v[19] - v[26] * v[171])) * x[256] + + (v[17] - (v[34] * v[49] - v[10] * v[19])) * x[257] + + (v[165] - (v[10] * v[171] - v[1] * v[49])) * x[258] + + (v[172] - (v[1] * v[19] - v[34] * v[171])) * x[259] + v[171] * v[40] + v[19] * v[36] + + v[49] * v[18] + (v[17] - (v[51] * v[49] - v[48] * v[19])) * x[260] + + (v[165] - (v[48] * v[171] - v[53] * v[49])) * x[261] + + (v[172] - (v[53] * v[19] - v[51] * v[171])) * x[262] + + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[263] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[264] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[265] + v[171] * v[144] + v[19] * v[37] + + v[49] * v[145] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[266] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[267] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[268] + v[171] * v[147] + v[19] * v[143] + + v[49] * v[148] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[269] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[270] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[271] + v[171] * v[150] + v[19] * v[146] + + v[49] * v[151] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[272] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[273] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[274] + v[171] * v[153] + v[19] * v[149] + + v[49] * v[154] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[275] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[276] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[277] + v[171] * v[156] + v[19] * v[152] + + v[49] * v[157] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[278] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[279] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[280] + v[171] * v[159] + v[19] * v[155] + + v[49] * v[160] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[281] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[282] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[283] + v[171] * v[82] + v[19] * v[79] + + v[49] * v[161] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[284] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[285] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[286] + v[171] * v[163] + v[19] * v[80] + + v[49] * v[164] + (v[17] - (v[66] * v[49] - v[63] * v[19])) * x[287] + + (v[165] - (v[63] * v[171] - v[68] * v[49])) * x[288] + + (v[172] - (v[68] * v[19] - v[66] * v[171])) * x[289] + v[171] * v[16] + v[19] * v[6] + + v[49] * v[50]; + v[172] = cos(x[2]); + v[165] = sin(x[2]); + v[17] = 4.89663865010925e-12 * v[172] + -4.89658313895802e-12 * v[165]; + v[14] = 5.55111512312578e-17 * v[172] + v[165]; + v[2] = v[5] * v[17] + v[166] * v[172] + v[169] * v[14]; + v[167] = v[167] + 0.069 * v[2] + 0.26242 * v[19]; + v[20] = v[3] * v[17] + v[4] * v[172] + 4.89663865010925e-12 * v[14]; + v[165] = -v[165]; + v[24] = 4.89663865010925e-12 * v[165] + -4.89658313895802e-12 * v[172]; + v[12] = 5.55111512312578e-17 * v[165] + v[172]; + v[4] = v[3] * v[24] + v[4] * v[165] + 4.89663865010925e-12 * v[12]; + v[3] = -4.89658313895802e-12 * v[20] + v[4]; + v[162] = v[162] + 0.069 * v[20] + 0.26242 * v[49]; + v[166] = v[5] * v[24] + v[166] * v[165] + v[169] * v[12]; + v[5] = -4.89658313895802e-12 * v[2] + v[166]; + v[169] = v[167] * v[3] - v[162] * v[5]; + v[14] = v[170] * v[17] + v[158] * v[172] + v[168] * v[14]; + v[12] = v[170] * v[24] + v[158] * v[165] + v[168] * v[12]; + v[24] = -4.89658313895802e-12 * v[14] + v[12]; + v[52] = v[52] + 0.069 * v[14] + 0.26242 * v[171]; + v[165] = v[162] * v[24] - v[52] * v[3]; + v[158] = v[52] * v[5] - v[167] * v[24]; + y[3] = (v[169] - (v[26] * v[3] - v[23] * v[5])) * x[44] + + (v[165] - (v[23] * v[24] - v[27] * v[3])) * x[45] + + (v[158] - (v[27] * v[5] - v[26] * v[24])) * x[46] + + (v[169] - (v[34] * v[3] - v[10] * v[5])) * x[47] + + (v[165] - (v[10] * v[24] - v[1] * v[3])) * x[48] + + (v[158] - (v[1] * v[5] - v[34] * v[24])) * x[49] + + (v[169] - (v[34] * v[3] - v[10] * v[5])) * x[50] + + (v[165] - (v[10] * v[24] - v[1] * v[3])) * x[51] + + (v[158] - (v[1] * v[5] - v[34] * v[24])) * x[52] + v[24] * v[41] + v[5] * v[35] + + v[3] * v[42] + (v[169] - (v[34] * v[3] - v[10] * v[5])) * x[53] + + (v[165] - (v[10] * v[24] - v[1] * v[3])) * x[54] + + (v[158] - (v[1] * v[5] - v[34] * v[24])) * x[55] + v[24] * v[44] + v[5] * v[38] + + v[3] * v[45] + (v[169] - (v[51] * v[3] - v[48] * v[5])) * x[56] + + (v[165] - (v[48] * v[24] - v[53] * v[3])) * x[57] + + (v[158] - (v[53] * v[5] - v[51] * v[24])) * x[58] + v[24] * v[33] + v[5] * v[7] + + v[3] * v[56] + (v[169] - (v[51] * v[3] - v[48] * v[5])) * x[59] + + (v[165] - (v[48] * v[24] - v[53] * v[3])) * x[60] + + (v[158] - (v[53] * v[5] - v[51] * v[24])) * x[61] + v[24] * v[58] + v[5] * v[9] + + v[3] * v[59] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[62] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[63] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[64] + v[24] * v[71] + v[5] * v[61] + + v[3] * v[72] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[65] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[66] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[67] + v[24] * v[74] + v[5] * v[60] + + v[3] * v[75] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[68] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[69] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[70] + v[24] * v[83] + v[5] * v[78] + + v[3] * v[84] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[71] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[72] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[73] + v[24] * v[65] + v[5] * v[64] + + v[3] * v[39] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[74] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[75] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[76] + v[24] * v[87] + v[5] * v[67] + + v[3] * v[88] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[77] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[78] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[79] + v[24] * v[90] + v[5] * v[28] + + v[3] * v[91] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[80] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[81] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[82] + v[24] * v[93] + v[5] * v[89] + + v[3] * v[94] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[83] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[84] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[85] + v[24] * v[96] + v[5] * v[92] + + v[3] * v[97] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[86] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[87] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[88] + v[24] * v[99] + v[5] * v[95] + + v[3] * v[100] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[89] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[90] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[91] + v[24] * v[102] + v[5] * v[98] + + v[3] * v[103] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[92] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[93] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[94] + v[24] * v[105] + v[5] * v[101] + + v[3] * v[106] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[95] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[96] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[97] + v[24] * v[108] + v[5] * v[104] + + v[3] * v[109] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[98] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[99] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[100] + v[24] * v[111] + v[5] * v[107] + + v[3] * v[112] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[101] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[102] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[103] + v[24] * v[114] + v[5] * v[110] + + v[3] * v[115] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[104] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[105] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[106] + v[24] * v[117] + v[5] * v[113] + + v[3] * v[118] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[107] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[108] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[109] + v[24] * v[120] + v[5] * v[116] + + v[3] * v[121] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[110] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[111] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[112] + v[24] * v[123] + v[5] * v[119] + + v[3] * v[124] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[113] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[114] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[115] + v[24] * v[126] + v[5] * v[122] + + v[3] * v[127] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[116] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[117] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[118] + v[24] * v[129] + v[5] * v[125] + + v[3] * v[130] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[119] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[120] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[121] + v[24] * v[132] + v[5] * v[128] + + v[3] * v[133] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[122] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[123] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[124] + v[24] * v[135] + v[5] * v[131] + + v[3] * v[136] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[125] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[126] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[127] + v[24] * v[138] + v[5] * v[134] + + v[3] * v[139] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[128] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[129] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[130] + v[24] * v[141] + v[5] * v[137] + + v[3] * v[142] + (v[169] - (v[26] * v[3] - v[23] * v[5])) * x[254] + + (v[165] - (v[23] * v[24] - v[27] * v[3])) * x[255] + + (v[158] - (v[27] * v[5] - v[26] * v[24])) * x[256] + + (v[169] - (v[34] * v[3] - v[10] * v[5])) * x[257] + + (v[165] - (v[10] * v[24] - v[1] * v[3])) * x[258] + + (v[158] - (v[1] * v[5] - v[34] * v[24])) * x[259] + v[24] * v[40] + v[5] * v[36] + + v[3] * v[18] + (v[169] - (v[51] * v[3] - v[48] * v[5])) * x[260] + + (v[165] - (v[48] * v[24] - v[53] * v[3])) * x[261] + + (v[158] - (v[53] * v[5] - v[51] * v[24])) * x[262] + + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[263] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[264] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[265] + v[24] * v[144] + v[5] * v[37] + + v[3] * v[145] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[266] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[267] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[268] + v[24] * v[147] + v[5] * v[143] + + v[3] * v[148] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[269] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[270] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[271] + v[24] * v[150] + v[5] * v[146] + + v[3] * v[151] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[272] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[273] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[274] + v[24] * v[153] + v[5] * v[149] + + v[3] * v[154] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[275] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[276] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[277] + v[24] * v[156] + v[5] * v[152] + + v[3] * v[157] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[278] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[279] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[280] + v[24] * v[159] + v[5] * v[155] + + v[3] * v[160] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[281] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[282] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[283] + v[24] * v[82] + v[5] * v[79] + + v[3] * v[161] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[284] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[285] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[286] + v[24] * v[163] + v[5] * v[80] + + v[3] * v[164] + (v[169] - (v[66] * v[3] - v[63] * v[5])) * x[287] + + (v[165] - (v[63] * v[24] - v[68] * v[3])) * x[288] + + (v[158] - (v[68] * v[5] - v[66] * v[24])) * x[289] + v[24] * v[16] + v[5] * v[6] + + v[3] * v[50]; + v[158] = cos(x[3]); + v[165] = sin(x[3]); + v[169] = 4.89663865010925e-12 * v[158] + v[165]; + v[23] = 5.55111512312578e-17 * v[158] + 4.89663865010925e-12 * v[165]; + v[27] = v[158] + -4.89658313895802e-12 * v[165]; + v[26] = v[2] * v[169] + v[166] * v[23] + v[19] * v[27]; + v[167] = v[167] + 0.10359 * v[26]; + v[170] = v[20] * v[169] + v[4] * v[23] + v[49] * v[27]; + v[165] = -v[165]; + v[168] = 4.89663865010925e-12 * v[165] + v[158]; + v[17] = 5.55111512312578e-17 * v[165] + 4.89663865010925e-12 * v[158]; + v[165] = v[165] + -4.89658313895802e-12 * v[158]; + v[4] = v[20] * v[168] + v[4] * v[17] + v[49] * v[165]; + v[20] = v[170] + -4.89658313895802e-12 * v[4] + 4.89663865010925e-12 * v[3]; + v[162] = v[162] + 0.10359 * v[170]; + v[166] = v[2] * v[168] + v[166] * v[17] + v[19] * v[165]; + v[2] = v[26] + -4.89658313895802e-12 * v[166] + 4.89663865010925e-12 * v[5]; + v[19] = v[167] * v[20] - v[162] * v[2]; + v[27] = v[14] * v[169] + v[12] * v[23] + v[171] * v[27]; + v[165] = v[14] * v[168] + v[12] * v[17] + v[171] * v[165]; + v[17] = v[27] + -4.89658313895802e-12 * v[165] + 4.89663865010925e-12 * v[24]; + v[52] = v[52] + 0.10359 * v[27]; + v[168] = v[162] * v[17] - v[52] * v[20]; + v[12] = v[52] * v[2] - v[167] * v[17]; + y[4] = (v[19] - (v[34] * v[20] - v[10] * v[2])) * x[47] + + (v[168] - (v[10] * v[17] - v[1] * v[20])) * x[48] + + (v[12] - (v[1] * v[2] - v[34] * v[17])) * x[49] + + (v[19] - (v[34] * v[20] - v[10] * v[2])) * x[50] + + (v[168] - (v[10] * v[17] - v[1] * v[20])) * x[51] + + (v[12] - (v[1] * v[2] - v[34] * v[17])) * x[52] + v[17] * v[41] + v[2] * v[35] + + v[20] * v[42] + (v[19] - (v[34] * v[20] - v[10] * v[2])) * x[53] + + (v[168] - (v[10] * v[17] - v[1] * v[20])) * x[54] + + (v[12] - (v[1] * v[2] - v[34] * v[17])) * x[55] + v[17] * v[44] + v[2] * v[38] + + v[20] * v[45] + (v[19] - (v[51] * v[20] - v[48] * v[2])) * x[56] + + (v[168] - (v[48] * v[17] - v[53] * v[20])) * x[57] + + (v[12] - (v[53] * v[2] - v[51] * v[17])) * x[58] + v[17] * v[33] + v[2] * v[7] + + v[20] * v[56] + (v[19] - (v[51] * v[20] - v[48] * v[2])) * x[59] + + (v[168] - (v[48] * v[17] - v[53] * v[20])) * x[60] + + (v[12] - (v[53] * v[2] - v[51] * v[17])) * x[61] + v[17] * v[58] + v[2] * v[9] + + v[20] * v[59] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[62] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[63] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[64] + v[17] * v[71] + v[2] * v[61] + + v[20] * v[72] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[65] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[66] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[67] + v[17] * v[74] + v[2] * v[60] + + v[20] * v[75] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[68] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[69] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[70] + v[17] * v[83] + v[2] * v[78] + + v[20] * v[84] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[71] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[72] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[73] + v[17] * v[65] + v[2] * v[64] + + v[20] * v[39] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[74] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[75] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[76] + v[17] * v[87] + v[2] * v[67] + + v[20] * v[88] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[77] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[78] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[79] + v[17] * v[90] + v[2] * v[28] + + v[20] * v[91] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[80] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[81] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[82] + v[17] * v[93] + v[2] * v[89] + + v[20] * v[94] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[83] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[84] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[85] + v[17] * v[96] + v[2] * v[92] + + v[20] * v[97] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[86] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[87] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[88] + v[17] * v[99] + v[2] * v[95] + + v[20] * v[100] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[89] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[90] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[91] + v[17] * v[102] + v[2] * v[98] + + v[20] * v[103] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[92] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[93] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[94] + v[17] * v[105] + v[2] * v[101] + + v[20] * v[106] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[95] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[96] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[97] + v[17] * v[108] + v[2] * v[104] + + v[20] * v[109] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[98] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[99] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[100] + v[17] * v[111] + v[2] * v[107] + + v[20] * v[112] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[101] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[102] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[103] + v[17] * v[114] + v[2] * v[110] + + v[20] * v[115] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[104] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[105] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[106] + v[17] * v[117] + v[2] * v[113] + + v[20] * v[118] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[107] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[108] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[109] + v[17] * v[120] + v[2] * v[116] + + v[20] * v[121] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[110] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[111] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[112] + v[17] * v[123] + v[2] * v[119] + + v[20] * v[124] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[113] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[114] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[115] + v[17] * v[126] + v[2] * v[122] + + v[20] * v[127] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[116] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[117] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[118] + v[17] * v[129] + v[2] * v[125] + + v[20] * v[130] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[119] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[120] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[121] + v[17] * v[132] + v[2] * v[128] + + v[20] * v[133] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[122] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[123] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[124] + v[17] * v[135] + v[2] * v[131] + + v[20] * v[136] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[125] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[126] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[127] + v[17] * v[138] + v[2] * v[134] + + v[20] * v[139] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[128] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[129] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[130] + v[17] * v[141] + v[2] * v[137] + + v[20] * v[142] + (v[19] - (v[34] * v[20] - v[10] * v[2])) * x[257] + + (v[168] - (v[10] * v[17] - v[1] * v[20])) * x[258] + + (v[12] - (v[1] * v[2] - v[34] * v[17])) * x[259] + v[17] * v[40] + v[2] * v[36] + + v[20] * v[18] + (v[19] - (v[51] * v[20] - v[48] * v[2])) * x[260] + + (v[168] - (v[48] * v[17] - v[53] * v[20])) * x[261] + + (v[12] - (v[53] * v[2] - v[51] * v[17])) * x[262] + + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[263] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[264] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[265] + v[17] * v[144] + v[2] * v[37] + + v[20] * v[145] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[266] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[267] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[268] + v[17] * v[147] + v[2] * v[143] + + v[20] * v[148] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[269] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[270] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[271] + v[17] * v[150] + v[2] * v[146] + + v[20] * v[151] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[272] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[273] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[274] + v[17] * v[153] + v[2] * v[149] + + v[20] * v[154] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[275] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[276] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[277] + v[17] * v[156] + v[2] * v[152] + + v[20] * v[157] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[278] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[279] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[280] + v[17] * v[159] + v[2] * v[155] + + v[20] * v[160] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[281] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[282] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[283] + v[17] * v[82] + v[2] * v[79] + + v[20] * v[161] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[284] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[285] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[286] + v[17] * v[163] + v[2] * v[80] + + v[20] * v[164] + (v[19] - (v[66] * v[20] - v[63] * v[2])) * x[287] + + (v[168] - (v[63] * v[17] - v[68] * v[20])) * x[288] + + (v[12] - (v[68] * v[2] - v[66] * v[17])) * x[289] + v[17] * v[16] + v[2] * v[6] + + v[20] * v[50]; + v[12] = cos(x[4]); + v[168] = sin(x[4]); + v[19] = 4.89663865010925e-12 * v[12] + -4.89658313895802e-12 * v[168]; + v[36] = 5.55111512312578e-17 * v[12] + v[168]; + v[40] = v[26] * v[19] + v[166] * v[12] + v[5] * v[36]; + v[167] = v[167] + 0.01 * v[40] + 0.2707 * v[2]; + v[38] = v[170] * v[19] + v[4] * v[12] + v[3] * v[36]; + v[168] = -v[168]; + v[44] = 4.89663865010925e-12 * v[168] + -4.89658313895802e-12 * v[12]; + v[35] = 5.55111512312578e-17 * v[168] + v[12]; + v[4] = v[170] * v[44] + v[4] * v[168] + v[3] * v[35]; + v[170] = -4.89658313895802e-12 * v[38] + v[4]; + v[162] = v[162] + 0.01 * v[38] + 0.2707 * v[20]; + v[166] = v[26] * v[44] + v[166] * v[168] + v[5] * v[35]; + v[26] = -4.89658313895802e-12 * v[40] + v[166]; + v[5] = v[167] * v[170] - v[162] * v[26]; + v[36] = v[27] * v[19] + v[165] * v[12] + v[24] * v[36]; + v[35] = v[27] * v[44] + v[165] * v[168] + v[24] * v[35]; + v[44] = -4.89658313895802e-12 * v[36] + v[35]; + v[52] = v[52] + 0.01 * v[36] + 0.2707 * v[17]; + v[168] = v[162] * v[44] - v[52] * v[170]; + v[165] = v[52] * v[26] - v[167] * v[44]; + y[5] = (v[5] - (v[51] * v[170] - v[48] * v[26])) * x[56] + + (v[168] - (v[48] * v[44] - v[53] * v[170])) * x[57] + + (v[165] - (v[53] * v[26] - v[51] * v[44])) * x[58] + v[44] * v[33] + v[26] * v[7] + + v[170] * v[56] + (v[5] - (v[51] * v[170] - v[48] * v[26])) * x[59] + + (v[168] - (v[48] * v[44] - v[53] * v[170])) * x[60] + + (v[165] - (v[53] * v[26] - v[51] * v[44])) * x[61] + v[44] * v[58] + v[26] * v[9] + + v[170] * v[59] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[62] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[63] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[64] + v[44] * v[71] + v[26] * v[61] + + v[170] * v[72] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[65] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[66] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[67] + v[44] * v[74] + v[26] * v[60] + + v[170] * v[75] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[68] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[69] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[70] + v[44] * v[83] + v[26] * v[78] + + v[170] * v[84] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[71] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[72] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[73] + v[44] * v[65] + v[26] * v[64] + + v[170] * v[39] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[74] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[75] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[76] + v[44] * v[87] + v[26] * v[67] + + v[170] * v[88] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[77] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[78] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[79] + v[44] * v[90] + v[26] * v[28] + + v[170] * v[91] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[80] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[81] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[82] + v[44] * v[93] + v[26] * v[89] + + v[170] * v[94] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[83] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[84] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[85] + v[44] * v[96] + v[26] * v[92] + + v[170] * v[97] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[86] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[87] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[88] + v[44] * v[99] + v[26] * v[95] + + v[170] * v[100] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[89] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[90] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[91] + v[44] * v[102] + v[26] * v[98] + + v[170] * v[103] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[92] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[93] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[94] + v[44] * v[105] + v[26] * v[101] + + v[170] * v[106] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[95] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[96] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[97] + v[44] * v[108] + v[26] * v[104] + + v[170] * v[109] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[98] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[99] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[100] + v[44] * v[111] + v[26] * v[107] + + v[170] * v[112] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[101] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[102] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[103] + v[44] * v[114] + v[26] * v[110] + + v[170] * v[115] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[104] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[105] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[106] + v[44] * v[117] + v[26] * v[113] + + v[170] * v[118] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[107] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[108] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[109] + v[44] * v[120] + v[26] * v[116] + + v[170] * v[121] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[110] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[111] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[112] + v[44] * v[123] + v[26] * v[119] + + v[170] * v[124] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[113] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[114] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[115] + v[44] * v[126] + v[26] * v[122] + + v[170] * v[127] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[116] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[117] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[118] + v[44] * v[129] + v[26] * v[125] + + v[170] * v[130] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[119] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[120] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[121] + v[44] * v[132] + v[26] * v[128] + + v[170] * v[133] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[122] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[123] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[124] + v[44] * v[135] + v[26] * v[131] + + v[170] * v[136] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[125] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[126] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[127] + v[44] * v[138] + v[26] * v[134] + + v[170] * v[139] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[128] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[129] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[130] + v[44] * v[141] + v[26] * v[137] + + v[170] * v[142] + (v[5] - (v[51] * v[170] - v[48] * v[26])) * x[260] + + (v[168] - (v[48] * v[44] - v[53] * v[170])) * x[261] + + (v[165] - (v[53] * v[26] - v[51] * v[44])) * x[262] + + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[263] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[264] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[265] + v[44] * v[144] + v[26] * v[37] + + v[170] * v[145] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[266] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[267] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[268] + v[44] * v[147] + v[26] * v[143] + + v[170] * v[148] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[269] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[270] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[271] + v[44] * v[150] + v[26] * v[146] + + v[170] * v[151] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[272] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[273] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[274] + v[44] * v[153] + v[26] * v[149] + + v[170] * v[154] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[275] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[276] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[277] + v[44] * v[156] + v[26] * v[152] + + v[170] * v[157] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[278] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[279] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[280] + v[44] * v[159] + v[26] * v[155] + + v[170] * v[160] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[281] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[282] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[283] + v[44] * v[82] + v[26] * v[79] + + v[170] * v[161] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[284] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[285] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[286] + v[44] * v[163] + v[26] * v[80] + + v[170] * v[164] + (v[5] - (v[66] * v[170] - v[63] * v[26])) * x[287] + + (v[168] - (v[63] * v[44] - v[68] * v[170])) * x[288] + + (v[165] - (v[68] * v[26] - v[66] * v[44])) * x[289] + v[44] * v[16] + v[26] * v[6] + + v[170] * v[50]; + v[165] = cos(x[5]); + v[168] = sin(x[5]); + v[5] = 4.89663865010925e-12 * v[165] + v[168]; + v[9] = 5.55111512312578e-17 * v[165] + 4.89663865010925e-12 * v[168]; + v[58] = v[165] + -4.89658313895802e-12 * v[168]; + v[7] = v[40] * v[5] + v[166] * v[9] + v[2] * v[58]; + v[167] = v[167] + 0.115975 * v[7]; + v[33] = v[38] * v[5] + v[4] * v[9] + v[20] * v[58]; + v[168] = -v[168]; + v[48] = 4.89663865010925e-12 * v[168] + v[165]; + v[59] = 5.55111512312578e-17 * v[168] + 4.89663865010925e-12 * v[165]; + v[168] = v[168] + -4.89658313895802e-12 * v[165]; + v[170] = v[33] + -4.89658313895802e-12 * (v[38] * v[48] + v[4] * v[59] + v[20] * v[168]) + + 4.89663865010925e-12 * v[170]; + v[33] = v[162] + 0.115975 * v[33]; + v[7] = v[7] + -4.89658313895802e-12 * (v[40] * v[48] + v[166] * v[59] + v[2] * v[168]) + + 4.89663865010925e-12 * v[26]; + v[26] = v[167] * v[170] - v[33] * v[7]; + v[58] = v[36] * v[5] + v[35] * v[9] + v[17] * v[58]; + v[168] = v[58] + -4.89658313895802e-12 * (v[36] * v[48] + v[35] * v[59] + v[17] * v[168]) + + 4.89663865010925e-12 * v[44]; + v[58] = v[52] + 0.115975 * v[58]; + v[33] = v[33] * v[168] - v[58] * v[170]; + v[58] = v[58] * v[7] - v[167] * v[168]; + y[6] = (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[62] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[63] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[64] + v[168] * v[71] + v[7] * v[61] + + v[170] * v[72] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[65] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[66] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[67] + v[168] * v[74] + v[7] * v[60] + + v[170] * v[75] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[68] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[69] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[70] + v[168] * v[83] + v[7] * v[78] + + v[170] * v[84] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[71] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[72] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[73] + v[168] * v[65] + v[7] * v[64] + + v[170] * v[39] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[74] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[75] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[76] + v[168] * v[87] + v[7] * v[67] + + v[170] * v[88] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[77] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[78] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[79] + v[168] * v[90] + v[7] * v[28] + + v[170] * v[91] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[80] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[81] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[82] + v[168] * v[93] + v[7] * v[89] + + v[170] * v[94] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[83] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[84] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[85] + v[168] * v[96] + v[7] * v[92] + + v[170] * v[97] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[86] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[87] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[88] + v[168] * v[99] + v[7] * v[95] + + v[170] * v[100] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[89] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[90] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[91] + v[168] * v[102] + v[7] * v[98] + + v[170] * v[103] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[92] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[93] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[94] + v[168] * v[105] + v[7] * v[101] + + v[170] * v[106] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[95] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[96] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[97] + v[168] * v[108] + v[7] * v[104] + + v[170] * v[109] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[98] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[99] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[100] + v[168] * v[111] + v[7] * v[107] + + v[170] * v[112] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[101] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[102] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[103] + v[168] * v[114] + v[7] * v[110] + + v[170] * v[115] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[104] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[105] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[106] + v[168] * v[117] + v[7] * v[113] + + v[170] * v[118] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[107] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[108] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[109] + v[168] * v[120] + v[7] * v[116] + + v[170] * v[121] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[110] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[111] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[112] + v[168] * v[123] + v[7] * v[119] + + v[170] * v[124] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[113] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[114] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[115] + v[168] * v[126] + v[7] * v[122] + + v[170] * v[127] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[116] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[117] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[118] + v[168] * v[129] + v[7] * v[125] + + v[170] * v[130] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[119] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[120] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[121] + v[168] * v[132] + v[7] * v[128] + + v[170] * v[133] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[122] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[123] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[124] + v[168] * v[135] + v[7] * v[131] + + v[170] * v[136] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[125] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[126] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[127] + v[168] * v[138] + v[7] * v[134] + + v[170] * v[139] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[128] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[129] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[130] + v[168] * v[141] + v[7] * v[137] + + v[170] * v[142] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[263] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[264] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[265] + v[168] * v[144] + v[7] * v[37] + + v[170] * v[145] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[266] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[267] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[268] + v[168] * v[147] + v[7] * v[143] + + v[170] * v[148] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[269] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[270] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[271] + v[168] * v[150] + v[7] * v[146] + + v[170] * v[151] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[272] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[273] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[274] + v[168] * v[153] + v[7] * v[149] + + v[170] * v[154] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[275] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[276] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[277] + v[168] * v[156] + v[7] * v[152] + + v[170] * v[157] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[278] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[279] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[280] + v[168] * v[159] + v[7] * v[155] + + v[170] * v[160] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[281] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[282] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[283] + v[168] * v[82] + v[7] * v[79] + + v[170] * v[161] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[284] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[285] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[286] + v[168] * v[163] + v[7] * v[80] + + v[170] * v[164] + (v[26] - (v[66] * v[170] - v[63] * v[7])) * x[287] + + (v[33] - (v[63] * v[168] - v[68] * v[170])) * x[288] + + (v[58] - (v[68] * v[7] - v[66] * v[168])) * x[289] + v[168] * v[16] + v[7] * v[6] + + v[170] * v[50]; + v[58] = cos(x[7]); + v[33] = sin(x[7]); + v[168] = -0.707108079859474 * v[58] + 0.707105482511236 * v[33]; + v[26] = -0.259027384507773 + 0.069 * v[168]; + v[7] = 0.707105482511236 * v[58] + 0.707108079859474 * v[33]; + v[170] = 0.0640272398484633 + 0.069 * v[7]; + v[6] = cos(x[8]); + v[33] = -v[33]; + v[16] = -0.707108079859474 * v[33] + 0.707105482511236 * v[58]; + v[80] = sin(x[8]); + v[163] = 4.89663865010925e-12 * v[80]; + v[79] = v[168] * v[6] + v[16] * v[163]; + v[82] = v[26] + 0.102 * v[79]; + v[33] = 0.707105482511236 * v[33] + 0.707108079859474 * v[58]; + v[163] = v[7] * v[6] + v[33] * v[163]; + v[58] = v[170] + 0.102 * v[163]; + v[155] = cos(x[9]); + v[159] = sin(x[9]); + v[152] = 4.89663865010925e-12 * v[155] + -4.89658313895802e-12 * v[159]; + v[156] = -v[80]; + v[149] = 4.89663865010925e-12 * v[6]; + v[7] = v[7] * v[156] + v[33] * v[149]; + v[153] = 5.55111512312578e-17 * v[155] + v[159]; + v[146] = v[163] * v[152] + v[7] * v[155] + v[33] * v[153]; + v[150] = v[163] + -4.89658313895802e-12 * v[7] + 4.89663865010925e-12 * v[33]; + v[143] = -0.02 * v[146] + 0.22 * v[150]; + v[149] = v[168] * v[156] + v[16] * v[149]; + v[156] = v[79] * v[152] + v[149] * v[155] + v[16] * v[153]; + v[168] = v[79] + -4.89658313895802e-12 * v[149] + 4.89663865010925e-12 * v[16]; + v[147] = -0.02 * v[156] + 0.22 * v[168]; + v[37] = v[143] * x[144] - v[147] * x[143]; + v[144] = -0.01 * v[146] + 0.11 * v[150]; + v[137] = -0.01 * v[156] + 0.11 * v[168]; + v[141] = v[144] * x[147] - v[137] * x[146]; + v[134] = v[82] + 0.069 * v[156] + 0.26242 * v[168]; + v[138] = v[58] + 0.069 * v[146] + 0.26242 * v[150]; + v[131] = cos(x[10]); + v[135] = sin(x[10]); + v[128] = 4.89663865010925e-12 * v[131] + v[135]; + v[159] = -v[159]; + v[132] = 4.89663865010925e-12 * v[159] + -4.89658313895802e-12 * v[155]; + v[125] = 5.55111512312578e-17 * v[159] + v[155]; + v[149] = v[79] * v[132] + v[149] * v[159] + v[16] * v[125]; + v[79] = 5.55111512312578e-17 * v[131] + 4.89663865010925e-12 * v[135]; + v[16] = v[131] + -4.89658313895802e-12 * v[135]; + v[129] = v[156] * v[128] + v[149] * v[79] + v[168] * v[16]; + v[122] = v[134] + 0.10359 * v[129]; + v[7] = v[163] * v[132] + v[7] * v[159] + v[33] * v[125]; + v[163] = v[146] * v[128] + v[7] * v[79] + v[150] * v[16]; + v[33] = v[138] + 0.10359 * v[163]; + v[135] = -v[135]; + v[126] = 4.89663865010925e-12 * v[135] + v[131]; + v[119] = 5.55111512312578e-17 * v[135] + 4.89663865010925e-12 * v[131]; + v[135] = v[135] + -4.89658313895802e-12 * v[131]; + v[131] = v[146] * v[126] + v[7] * v[119] + v[150] * v[135]; + v[7] = -4.89658313895802e-12 * v[146] + v[7]; + v[123] = v[163] + -4.89658313895802e-12 * v[131] + 4.89663865010925e-12 * v[7]; + v[116] = 0.22 * v[123]; + v[120] = v[156] * v[126] + v[149] * v[119] + v[168] * v[135]; + v[149] = -4.89658313895802e-12 * v[156] + v[149]; + v[113] = v[129] + -4.89658313895802e-12 * v[120] + 4.89663865010925e-12 * v[149]; + v[117] = 0.22 * v[113]; + v[110] = v[116] * x[159] - v[117] * x[158]; + v[114] = 0.11 * v[123]; + v[107] = 0.11 * v[113]; + v[111] = v[114] * x[162] - v[107] * x[161]; + v[104] = cos(x[11]); + v[108] = sin(x[11]); + v[101] = 4.89663865010925e-12 * v[104] + -4.89658313895802e-12 * v[108]; + v[105] = 5.55111512312578e-17 * v[104] + v[108]; + v[98] = v[129] * v[101] + v[120] * v[104] + v[149] * v[105]; + v[102] = v[122] + 0.01 * v[98] + 0.2707 * v[113]; + v[95] = v[163] * v[101] + v[131] * v[104] + v[7] * v[105]; + v[99] = v[33] + 0.01 * v[95] + 0.2707 * v[123]; + v[108] = -v[108]; + v[92] = 4.89663865010925e-12 * v[108] + -4.89658313895802e-12 * v[104]; + v[96] = 5.55111512312578e-17 * v[108] + v[104]; + v[7] = v[163] * v[92] + v[131] * v[108] + v[7] * v[96]; + v[131] = -4.89658313895802e-12 * v[95] + v[7]; + v[163] = 0.03 * v[131]; + v[149] = v[129] * v[92] + v[120] * v[108] + v[149] * v[96]; + v[120] = -4.89658313895802e-12 * v[98] + v[149]; + v[129] = 0.03 * v[120]; + v[89] = v[163] * x[165] - v[129] * x[164]; + v[93] = -0.03 * v[131]; + v[28] = -0.03 * v[120]; + v[90] = v[93] * x[168] - v[28] * x[167]; + v[67] = cos(x[12]); + v[87] = sin(x[12]); + v[64] = 4.89663865010925e-12 * v[67] + v[87]; + v[65] = 5.55111512312578e-17 * v[67] + 4.89663865010925e-12 * v[87]; + v[78] = v[67] + -4.89658313895802e-12 * v[87]; + v[83] = v[98] * v[64] + v[149] * v[65] + v[113] * v[78]; + v[60] = v[102] + 0.115975 * v[83]; + v[74] = v[95] * v[64] + v[7] * v[65] + v[123] * v[78]; + v[61] = v[99] + 0.115975 * v[74]; + v[87] = -v[87]; + v[71] = 4.89663865010925e-12 * v[87] + v[67]; + v[63] = 5.55111512312578e-17 * v[87] + 4.89663865010925e-12 * v[67]; + v[87] = v[87] + -4.89658313895802e-12 * v[67]; + v[7] = v[95] * v[71] + v[7] * v[63] + v[123] * v[87]; + v[95] = v[74] + -4.89658313895802e-12 * v[7] + 4.89663865010925e-12 * v[131]; + v[67] = 0.02 * v[95]; + v[149] = v[98] * v[71] + v[149] * v[63] + v[113] * v[87]; + v[98] = v[83] + -4.89658313895802e-12 * v[149] + 4.89663865010925e-12 * v[120]; + v[50] = 0.02 * v[98]; + v[164] = v[67] * x[171] - v[50] * x[170]; + v[161] = -0.04 * v[95]; + v[160] = -0.04 * v[98]; + v[157] = v[161] * x[174] - v[160] * x[173]; + v[154] = cos(x[13]); + v[151] = sin(x[13]); + v[148] = 4.89663865010925e-12 * v[154] + -4.89658313895802e-12 * v[151]; + v[145] = 5.55111512312578e-17 * v[154] + v[151]; + v[142] = v[74] * v[148] + v[7] * v[154] + v[131] * v[145]; + v[139] = 0.01 * v[142] + 0.09355 * v[95]; + v[136] = v[83] * v[148] + v[149] * v[154] + v[120] * v[145]; + v[133] = 0.01 * v[136] + 0.09355 * v[98]; + v[130] = v[139] * x[177] - v[133] * x[176]; + v[151] = -v[151]; + v[127] = 4.89663865010925e-12 * v[151] + -4.89658313895802e-12 * v[154]; + v[124] = 5.55111512312578e-17 * v[151] + v[154]; + v[7] = v[74] * v[127] + v[7] * v[151] + v[131] * v[124]; + v[74] = 0.02 * v[7] + 0.13855 * v[95]; + v[149] = v[83] * v[127] + v[149] * v[151] + v[120] * v[124]; + v[83] = 0.02 * v[149] + 0.13855 * v[98]; + v[120] = v[74] * x[180] - v[83] * x[179]; + v[131] = -0.02 * v[7] + 0.13855 * v[95]; + v[121] = -0.02 * v[149] + 0.13855 * v[98]; + v[118] = v[131] * x[183] - v[121] * x[182]; + v[115] = -0.005 * v[142] + 0.081333 * v[7] + 0.16655 * v[95]; + v[112] = -0.005 * v[136] + 0.081333 * v[149] + 0.16655 * v[98]; + v[109] = v[115] * x[186] - v[112] * x[185]; + v[106] = -0.005 * v[142] + 0.057333 * v[7] + 0.16655 * v[95]; + v[103] = -0.005 * v[136] + 0.057333 * v[149] + 0.16655 * v[98]; + v[100] = v[106] * x[189] - v[103] * x[188]; + v[97] = 0.086583 * v[7] + 0.18855 * v[95]; + v[94] = 0.086583 * v[149] + 0.18855 * v[98]; + v[91] = v[97] * x[192] - v[94] * x[191]; + v[88] = 0.086583 * v[7] + 0.20855 * v[95]; + v[39] = 0.086583 * v[149] + 0.20855 * v[98]; + v[84] = v[88] * x[195] - v[39] * x[194]; + v[75] = 0.086583 * v[7] + 0.22855 * v[95]; + v[72] = 0.086583 * v[149] + 0.22855 * v[98]; + v[68] = v[75] * x[198] - v[72] * x[197]; + v[66] = 0.01 * v[142] + 0.082083 * v[7] + 0.26625 * v[95]; + v[167] = 0.01 * v[136] + 0.082083 * v[149] + 0.26625 * v[98]; + v[52] = v[66] * x[201] - v[167] * x[200]; + v[59] = -0.01 * v[142] + 0.082083 * v[7] + 0.26625 * v[95]; + v[48] = -0.01 * v[136] + 0.082083 * v[149] + 0.26625 * v[98]; + v[44] = v[59] * x[204] - v[48] * x[203]; + v[35] = -0.01 * v[142] + 0.082083 * v[7] + 0.24625 * v[95]; + v[36] = -0.01 * v[136] + 0.082083 * v[149] + 0.24625 * v[98]; + v[17] = v[35] * x[207] - v[36] * x[206]; + v[9] = 0.01 * v[142] + 0.082083 * v[7] + 0.24625 * v[95]; + v[5] = 0.01 * v[136] + 0.082083 * v[149] + 0.24625 * v[98]; + v[166] = v[9] * x[210] - v[5] * x[209]; + v[40] = 0.005 * v[142] + -0.059333 * v[7] + 0.16655 * v[95]; + v[2] = 0.005 * v[136] + -0.059333 * v[149] + 0.16655 * v[98]; + v[162] = v[40] * x[213] - v[2] * x[212]; + v[4] = 0.005 * v[142] + -0.079333 * v[7] + 0.16655 * v[95]; + v[38] = 0.005 * v[136] + -0.079333 * v[149] + 0.16655 * v[98]; + v[20] = v[4] * x[216] - v[38] * x[215]; + v[165] = -0.086583 * v[7] + 0.18855 * v[95]; + v[56] = -0.086583 * v[149] + 0.18855 * v[98]; + v[53] = v[165] * x[219] - v[56] * x[218]; + v[51] = -0.086583 * v[7] + 0.20855 * v[95]; + v[27] = -0.086583 * v[149] + 0.20855 * v[98]; + v[24] = v[51] * x[222] - v[27] * x[221]; + v[19] = -0.086583 * v[7] + 0.22855 * v[95]; + v[12] = -0.086583 * v[149] + 0.22855 * v[98]; + v[3] = v[19] * x[225] - v[12] * x[224]; + v[41] = 0.01 * v[142] + -0.082083 * v[7] + 0.26625 * v[95]; + v[10] = 0.01 * v[136] + -0.082083 * v[149] + 0.26625 * v[98]; + v[18] = v[41] * x[228] - v[10] * x[227]; + v[45] = -0.01 * v[142] + -0.082083 * v[7] + 0.26625 * v[95]; + v[42] = -0.01 * v[136] + -0.082083 * v[149] + 0.26625 * v[98]; + v[1] = v[45] * x[231] - v[42] * x[230]; + v[34] = -0.01 * v[142] + -0.082083 * v[7] + 0.24625 * v[95]; + v[14] = -0.01 * v[136] + -0.082083 * v[149] + 0.24625 * v[98]; + v[171] = v[34] * x[234] - v[14] * x[233]; + v[23] = 0.01 * v[142] + -0.082083 * v[7] + 0.24625 * v[95]; + v[169] = 0.01 * v[136] + -0.082083 * v[149] + 0.24625 * v[98]; + v[49] = v[23] * x[237] - v[169] * x[236]; + v[150] = -0.00999999977648258 * v[146] + 0.109999999403954 * v[150]; + v[168] = -0.00999999977648258 * v[156] + 0.109999999403954 * v[168]; + v[156] = v[150] * x[300] - v[168] * x[299]; + v[123] = 0.109999999403954 * v[123]; + v[113] = 0.109999999403954 * v[113]; + v[146] = v[123] * x[306] - v[113] * x[305]; + v[158] = -0.0149999987334013 * v[95]; + v[172] = -0.0149999987334013 * v[98]; + v[21] = v[158] * x[312] - v[172] * x[311]; + v[173] = 0.00999999977648258 * v[142] + 0.0935499966144562 * v[95]; + v[15] = 0.00999999977648258 * v[136] + 0.0935499966144562 * v[98]; + v[25] = v[173] * x[315] - v[15] * x[314]; + v[22] = 0.138549998402596 * v[95]; + v[0] = 0.138549998402596 * v[98]; + v[11] = v[22] * x[318] - v[0] * x[317]; + v[140] = -0.00499999988824129 * v[142] + 0.0693330019712448 * v[7] + 0.166549995541573 * v[95]; + v[29] = -0.00499999988824129 * v[136] + 0.0693330019712448 * v[149] + 0.166549995541573 * v[98]; + v[86] = v[140] * x[321] - v[29] * x[320]; + v[85] = 0.0865830034017563 * v[7] + 0.208550006151199 * v[95]; + v[77] = 0.0865830034017563 * v[149] + 0.208550006151199 * v[98]; + v[81] = v[85] * x[324] - v[77] * x[323]; + v[76] = 0.0820830017328262 * v[7] + 0.256249994039536 * v[95]; + v[73] = 0.0820830017328262 * v[149] + 0.256249994039536 * v[98]; + v[70] = v[76] * x[327] - v[73] * x[326]; + v[142] = 0.00499999988824129 * v[142] + -0.0693330019712448 * v[7] + 0.166549995541573 * v[95]; + v[136] = 0.00499999988824129 * v[136] + -0.0693330019712448 * v[149] + 0.166549995541573 * v[98]; + v[69] = v[142] * x[330] - v[136] * x[329]; + v[62] = -0.0865830034017563 * v[7] + 0.208550006151199 * v[95]; + v[57] = -0.0865830034017563 * v[149] + 0.208550006151199 * v[98]; + v[55] = v[62] * x[333] - v[57] * x[332]; + v[7] = -0.0820830017328262 * v[7] + 0.256249994039536 * v[95]; + v[149] = -0.0820830017328262 * v[149] + 0.256249994039536 * v[98]; + v[98] = v[7] * x[336] - v[149] * x[335]; + y[7] = (-0.259027384507773 - v[26]) * x[140] + (-0.0640272398484633 - (0. - v[170])) * x[141] + + (-0.259027384507773 - v[82]) * x[143] + (-0.0640272398484633 - (0. - v[58])) * x[144] + + v[37] + (-0.259027384507773 - v[82]) * x[146] + + (-0.0640272398484633 - (0. - v[58])) * x[147] + v[141] + + (-0.259027384507773 - v[82]) * x[149] + (-0.0640272398484633 - (0. - v[58])) * x[150] + + (-0.259027384507773 - v[134]) * x[152] + (-0.0640272398484633 - (0. - v[138])) * x[153] + + (-0.259027384507773 - v[122]) * x[155] + (-0.0640272398484633 - (0. - v[33])) * x[156] + + (-0.259027384507773 - v[122]) * x[158] + (-0.0640272398484633 - (0. - v[33])) * x[159] + + v[110] + (-0.259027384507773 - v[122]) * x[161] + + (-0.0640272398484633 - (0. - v[33])) * x[162] + v[111] + + (-0.259027384507773 - v[102]) * x[164] + (-0.0640272398484633 - (0. - v[99])) * x[165] + + v[89] + (-0.259027384507773 - v[102]) * x[167] + + (-0.0640272398484633 - (0. - v[99])) * x[168] + v[90] + + (-0.259027384507773 - v[60]) * x[170] + (-0.0640272398484633 - (0. - v[61])) * x[171] + + v[164] + (-0.259027384507773 - v[60]) * x[173] + + (-0.0640272398484633 - (0. - v[61])) * x[174] + v[157] + + (-0.259027384507773 - v[60]) * x[176] + (-0.0640272398484633 - (0. - v[61])) * x[177] + + v[130] + (-0.259027384507773 - v[60]) * x[179] + + (-0.0640272398484633 - (0. - v[61])) * x[180] + v[120] + + (-0.259027384507773 - v[60]) * x[182] + (-0.0640272398484633 - (0. - v[61])) * x[183] + + v[118] + (-0.259027384507773 - v[60]) * x[185] + + (-0.0640272398484633 - (0. - v[61])) * x[186] + v[109] + + (-0.259027384507773 - v[60]) * x[188] + (-0.0640272398484633 - (0. - v[61])) * x[189] + + v[100] + (-0.259027384507773 - v[60]) * x[191] + + (-0.0640272398484633 - (0. - v[61])) * x[192] + v[91] + + (-0.259027384507773 - v[60]) * x[194] + (-0.0640272398484633 - (0. - v[61])) * x[195] + + v[84] + (-0.259027384507773 - v[60]) * x[197] + + (-0.0640272398484633 - (0. - v[61])) * x[198] + v[68] + + (-0.259027384507773 - v[60]) * x[200] + (-0.0640272398484633 - (0. - v[61])) * x[201] + + v[52] + (-0.259027384507773 - v[60]) * x[203] + + (-0.0640272398484633 - (0. - v[61])) * x[204] + v[44] + + (-0.259027384507773 - v[60]) * x[206] + (-0.0640272398484633 - (0. - v[61])) * x[207] + + v[17] + (-0.259027384507773 - v[60]) * x[209] + + (-0.0640272398484633 - (0. - v[61])) * x[210] + v[166] + + (-0.259027384507773 - v[60]) * x[212] + (-0.0640272398484633 - (0. - v[61])) * x[213] + + v[162] + (-0.259027384507773 - v[60]) * x[215] + + (-0.0640272398484633 - (0. - v[61])) * x[216] + v[20] + + (-0.259027384507773 - v[60]) * x[218] + (-0.0640272398484633 - (0. - v[61])) * x[219] + + v[53] + (-0.259027384507773 - v[60]) * x[221] + + (-0.0640272398484633 - (0. - v[61])) * x[222] + v[24] + + (-0.259027384507773 - v[60]) * x[224] + (-0.0640272398484633 - (0. - v[61])) * x[225] + + v[3] + (-0.259027384507773 - v[60]) * x[227] + + (-0.0640272398484633 - (0. - v[61])) * x[228] + v[18] + + (-0.259027384507773 - v[60]) * x[230] + (-0.0640272398484633 - (0. - v[61])) * x[231] + + v[1] + (-0.259027384507773 - v[60]) * x[233] + + (-0.0640272398484633 - (0. - v[61])) * x[234] + v[171] + + (-0.259027384507773 - v[60]) * x[236] + (-0.0640272398484633 - (0. - v[61])) * x[237] + + v[49] + (-0.259027384507773 - v[26]) * x[296] + + (-0.0640272398484633 - (0. - v[170])) * x[297] + (-0.259027384507773 - v[82]) * x[299] + + (-0.0640272398484633 - (0. - v[58])) * x[300] + v[156] + + (-0.259027384507773 - v[134]) * x[302] + (-0.0640272398484633 - (0. - v[138])) * x[303] + + (-0.259027384507773 - v[122]) * x[305] + (-0.0640272398484633 - (0. - v[33])) * x[306] + + v[146] + (-0.259027384507773 - v[102]) * x[308] + + (-0.0640272398484633 - (0. - v[99])) * x[309] + (-0.259027384507773 - v[60]) * x[311] + + (-0.0640272398484633 - (0. - v[61])) * x[312] + v[21] + + (-0.259027384507773 - v[60]) * x[314] + (-0.0640272398484633 - (0. - v[61])) * x[315] + + v[25] + (-0.259027384507773 - v[60]) * x[317] + + (-0.0640272398484633 - (0. - v[61])) * x[318] + v[11] + + (-0.259027384507773 - v[60]) * x[320] + (-0.0640272398484633 - (0. - v[61])) * x[321] + + v[86] + (-0.259027384507773 - v[60]) * x[323] + + (-0.0640272398484633 - (0. - v[61])) * x[324] + v[81] + + (-0.259027384507773 - v[60]) * x[326] + (-0.0640272398484633 - (0. - v[61])) * x[327] + + v[70] + (-0.259027384507773 - v[60]) * x[329] + + (-0.0640272398484633 - (0. - v[61])) * x[330] + v[69] + + (-0.259027384507773 - v[60]) * x[332] + (-0.0640272398484633 - (0. - v[61])) * x[333] + + v[55] + (-0.259027384507773 - v[60]) * x[335] + + (-0.0640272398484633 - (0. - v[61])) * x[336] + v[98]; + v[95] = cos(x[7]); + v[54] = sin(x[7]); + v[47] = -0.707108079859474 * v[95] + 0.707105482511236 * v[54]; + v[46] = -0.259027384507773 + 0.069 * v[47]; + v[43] = -v[54]; + v[30] = -0.707108079859474 * v[43] + 0.707105482511236 * v[95]; + v[8] = 4.89663865010925e-12 * v[46] - 0.399976 * v[30]; + v[43] = 0.707105482511236 * v[43] + 0.707108079859474 * v[95]; + v[54] = 0.707105482511236 * v[95] + 0.707108079859474 * v[54]; + v[95] = 0.0640272398484633 + 0.069 * v[54]; + v[32] = 0.399976 * v[43] - 4.89663865010925e-12 * v[95]; + v[31] = v[95] * v[30] - v[46] * v[43]; + v[80] = -1. * v[80]; + v[13] = 0.399976 + 0.102 * v[80]; + v[6] = -1. * v[6]; + v[153] = v[80] * v[152] + v[6] * v[155] + 4.89663865010925e-12 * v[153]; + v[152] = v[80] + 2.39770700697438e-23 + -4.89658313895802e-12 * v[6]; + v[155] = -0.02 * v[153] + 0.22 * v[152]; + v[147] = v[147] * x[145] - v[155] * x[144]; + v[155] = v[155] * x[143] - v[143] * x[145]; + v[143] = -0.01 * v[153] + 0.11 * v[152]; + v[137] = v[137] * x[148] - v[143] * x[147]; + v[143] = v[143] * x[146] - v[144] * x[148]; + v[144] = v[13] + 0.069 * v[153] + 0.26242 * v[152]; + v[6] = v[80] * v[132] + v[6] * v[159] + 4.89663865010925e-12 * v[125]; + v[16] = v[153] * v[128] + v[6] * v[79] + v[152] * v[16]; + v[79] = v[144] + 0.10359 * v[16]; + v[135] = v[153] * v[126] + v[6] * v[119] + v[152] * v[135]; + v[6] = -4.89658313895802e-12 * v[153] + v[6]; + v[119] = v[16] + -4.89658313895802e-12 * v[135] + 4.89663865010925e-12 * v[6]; + v[126] = 0.22 * v[119]; + v[117] = v[117] * x[160] - v[126] * x[159]; + v[126] = v[126] * x[158] - v[116] * x[160]; + v[116] = 0.11 * v[119]; + v[107] = v[107] * x[163] - v[116] * x[162]; + v[116] = v[116] * x[161] - v[114] * x[163]; + v[105] = v[16] * v[101] + v[135] * v[104] + v[6] * v[105]; + v[101] = v[79] + 0.01 * v[105] + 0.2707 * v[119]; + v[6] = v[16] * v[92] + v[135] * v[108] + v[6] * v[96]; + v[135] = -4.89658313895802e-12 * v[105] + v[6]; + v[16] = 0.03 * v[135]; + v[129] = v[129] * x[166] - v[16] * x[165]; + v[16] = v[16] * x[164] - v[163] * x[166]; + v[163] = -0.03 * v[135]; + v[28] = v[28] * x[169] - v[163] * x[168]; + v[163] = v[163] * x[167] - v[93] * x[169]; + v[78] = v[105] * v[64] + v[6] * v[65] + v[119] * v[78]; + v[65] = v[101] + 0.115975 * v[78]; + v[6] = v[105] * v[71] + v[6] * v[63] + v[119] * v[87]; + v[105] = v[78] + -4.89658313895802e-12 * v[6] + 4.89663865010925e-12 * v[135]; + v[87] = 0.02 * v[105]; + v[50] = v[50] * x[172] - v[87] * x[171]; + v[87] = v[87] * x[170] - v[67] * x[172]; + v[67] = -0.04 * v[105]; + v[160] = v[160] * x[175] - v[67] * x[174]; + v[67] = v[67] * x[173] - v[161] * x[175]; + v[145] = v[78] * v[148] + v[6] * v[154] + v[135] * v[145]; + v[148] = 0.01 * v[145] + 0.09355 * v[105]; + v[133] = v[133] * x[178] - v[148] * x[177]; + v[148] = v[148] * x[176] - v[139] * x[178]; + v[6] = v[78] * v[127] + v[6] * v[151] + v[135] * v[124]; + v[78] = 0.02 * v[6] + 0.13855 * v[105]; + v[83] = v[83] * x[181] - v[78] * x[180]; + v[78] = v[78] * x[179] - v[74] * x[181]; + v[74] = -0.02 * v[6] + 0.13855 * v[105]; + v[121] = v[121] * x[184] - v[74] * x[183]; + v[74] = v[74] * x[182] - v[131] * x[184]; + v[131] = -0.005 * v[145] + 0.081333 * v[6] + 0.16655 * v[105]; + v[112] = v[112] * x[187] - v[131] * x[186]; + v[131] = v[131] * x[185] - v[115] * x[187]; + v[115] = -0.005 * v[145] + 0.057333 * v[6] + 0.16655 * v[105]; + v[103] = v[103] * x[190] - v[115] * x[189]; + v[115] = v[115] * x[188] - v[106] * x[190]; + v[106] = 0.086583 * v[6] + 0.18855 * v[105]; + v[94] = v[94] * x[193] - v[106] * x[192]; + v[106] = v[106] * x[191] - v[97] * x[193]; + v[97] = 0.086583 * v[6] + 0.20855 * v[105]; + v[39] = v[39] * x[196] - v[97] * x[195]; + v[97] = v[97] * x[194] - v[88] * x[196]; + v[88] = 0.086583 * v[6] + 0.22855 * v[105]; + v[72] = v[72] * x[199] - v[88] * x[198]; + v[88] = v[88] * x[197] - v[75] * x[199]; + v[75] = 0.01 * v[145] + 0.082083 * v[6] + 0.26625 * v[105]; + v[167] = v[167] * x[202] - v[75] * x[201]; + v[75] = v[75] * x[200] - v[66] * x[202]; + v[66] = -0.01 * v[145] + 0.082083 * v[6] + 0.26625 * v[105]; + v[48] = v[48] * x[205] - v[66] * x[204]; + v[66] = v[66] * x[203] - v[59] * x[205]; + v[59] = -0.01 * v[145] + 0.082083 * v[6] + 0.24625 * v[105]; + v[36] = v[36] * x[208] - v[59] * x[207]; + v[59] = v[59] * x[206] - v[35] * x[208]; + v[35] = 0.01 * v[145] + 0.082083 * v[6] + 0.24625 * v[105]; + v[5] = v[5] * x[211] - v[35] * x[210]; + v[35] = v[35] * x[209] - v[9] * x[211]; + v[9] = 0.005 * v[145] + -0.059333 * v[6] + 0.16655 * v[105]; + v[2] = v[2] * x[214] - v[9] * x[213]; + v[9] = v[9] * x[212] - v[40] * x[214]; + v[40] = 0.005 * v[145] + -0.079333 * v[6] + 0.16655 * v[105]; + v[38] = v[38] * x[217] - v[40] * x[216]; + v[40] = v[40] * x[215] - v[4] * x[217]; + v[4] = -0.086583 * v[6] + 0.18855 * v[105]; + v[56] = v[56] * x[220] - v[4] * x[219]; + v[4] = v[4] * x[218] - v[165] * x[220]; + v[165] = -0.086583 * v[6] + 0.20855 * v[105]; + v[27] = v[27] * x[223] - v[165] * x[222]; + v[165] = v[165] * x[221] - v[51] * x[223]; + v[51] = -0.086583 * v[6] + 0.22855 * v[105]; + v[12] = v[12] * x[226] - v[51] * x[225]; + v[51] = v[51] * x[224] - v[19] * x[226]; + v[19] = 0.01 * v[145] + -0.082083 * v[6] + 0.26625 * v[105]; + v[10] = v[10] * x[229] - v[19] * x[228]; + v[19] = v[19] * x[227] - v[41] * x[229]; + v[41] = -0.01 * v[145] + -0.082083 * v[6] + 0.26625 * v[105]; + v[42] = v[42] * x[232] - v[41] * x[231]; + v[41] = v[41] * x[230] - v[45] * x[232]; + v[45] = -0.01 * v[145] + -0.082083 * v[6] + 0.24625 * v[105]; + v[14] = v[14] * x[235] - v[45] * x[234]; + v[45] = v[45] * x[233] - v[34] * x[235]; + v[34] = 0.01 * v[145] + -0.082083 * v[6] + 0.24625 * v[105]; + v[169] = v[169] * x[238] - v[34] * x[237]; + v[34] = v[34] * x[236] - v[23] * x[238]; + v[152] = -0.00999999977648258 * v[153] + 0.109999999403954 * v[152]; + v[168] = v[168] * x[301] - v[152] * x[300]; + v[152] = v[152] * x[299] - v[150] * x[301]; + v[119] = 0.109999999403954 * v[119]; + v[113] = v[113] * x[307] - v[119] * x[306]; + v[119] = v[119] * x[305] - v[123] * x[307]; + v[123] = -0.0149999987334013 * v[105]; + v[172] = v[172] * x[313] - v[123] * x[312]; + v[123] = v[123] * x[311] - v[158] * x[313]; + v[158] = 0.00999999977648258 * v[145] + 0.0935499966144562 * v[105]; + v[15] = v[15] * x[316] - v[158] * x[315]; + v[158] = v[158] * x[314] - v[173] * x[316]; + v[173] = 0.138549998402596 * v[105]; + v[0] = v[0] * x[319] - v[173] * x[318]; + v[173] = v[173] * x[317] - v[22] * x[319]; + v[22] = -0.00499999988824129 * v[145] + 0.0693330019712448 * v[6] + 0.166549995541573 * v[105]; + v[29] = v[29] * x[322] - v[22] * x[321]; + v[22] = v[22] * x[320] - v[140] * x[322]; + v[140] = 0.0865830034017563 * v[6] + 0.208550006151199 * v[105]; + v[77] = v[77] * x[325] - v[140] * x[324]; + v[140] = v[140] * x[323] - v[85] * x[325]; + v[85] = 0.0820830017328262 * v[6] + 0.256249994039536 * v[105]; + v[73] = v[73] * x[328] - v[85] * x[327]; + v[85] = v[85] * x[326] - v[76] * x[328]; + v[145] = 0.00499999988824129 * v[145] + -0.0693330019712448 * v[6] + 0.166549995541573 * v[105]; + v[136] = v[136] * x[331] - v[145] * x[330]; + v[145] = v[145] * x[329] - v[142] * x[331]; + v[142] = -0.0865830034017563 * v[6] + 0.208550006151199 * v[105]; + v[57] = v[57] * x[334] - v[142] * x[333]; + v[142] = v[142] * x[332] - v[62] * x[334]; + v[6] = -0.0820830017328262 * v[6] + 0.256249994039536 * v[105]; + v[149] = v[149] * x[337] - v[6] * x[336]; + v[6] = v[6] * x[335] - v[7] * x[337]; + y[8] = (v[8] - (4.89663865010925e-12 * v[26] - 0.399976 * v[30])) * x[140] + + (v[32] - (0.399976 * v[43] - 4.89663865010925e-12 * v[170])) * x[141] + + (v[31] - (v[170] * v[30] - v[26] * v[43])) * x[142] + + (v[8] - (4.89663865010925e-12 * v[82] - v[13] * v[30])) * x[143] + + (v[32] - (v[13] * v[43] - 4.89663865010925e-12 * v[58])) * x[144] + + (v[31] - (v[58] * v[30] - v[82] * v[43])) * x[145] + v[43] * v[147] + v[30] * v[155] + + 4.89663865010925e-12 * v[37] + + (v[8] - (4.89663865010925e-12 * v[82] - v[13] * v[30])) * x[146] + + (v[32] - (v[13] * v[43] - 4.89663865010925e-12 * v[58])) * x[147] + + (v[31] - (v[58] * v[30] - v[82] * v[43])) * x[148] + v[43] * v[137] + v[30] * v[143] + + 4.89663865010925e-12 * v[141] + + (v[8] - (4.89663865010925e-12 * v[82] - v[13] * v[30])) * x[149] + + (v[32] - (v[13] * v[43] - 4.89663865010925e-12 * v[58])) * x[150] + + (v[31] - (v[58] * v[30] - v[82] * v[43])) * x[151] + + (v[8] - (4.89663865010925e-12 * v[134] - v[144] * v[30])) * x[152] + + (v[32] - (v[144] * v[43] - 4.89663865010925e-12 * v[138])) * x[153] + + (v[31] - (v[138] * v[30] - v[134] * v[43])) * x[154] + + (v[8] - (4.89663865010925e-12 * v[122] - v[79] * v[30])) * x[155] + + (v[32] - (v[79] * v[43] - 4.89663865010925e-12 * v[33])) * x[156] + + (v[31] - (v[33] * v[30] - v[122] * v[43])) * x[157] + + (v[8] - (4.89663865010925e-12 * v[122] - v[79] * v[30])) * x[158] + + (v[32] - (v[79] * v[43] - 4.89663865010925e-12 * v[33])) * x[159] + + (v[31] - (v[33] * v[30] - v[122] * v[43])) * x[160] + v[43] * v[117] + v[30] * v[126] + + 4.89663865010925e-12 * v[110] + + (v[8] - (4.89663865010925e-12 * v[122] - v[79] * v[30])) * x[161] + + (v[32] - (v[79] * v[43] - 4.89663865010925e-12 * v[33])) * x[162] + + (v[31] - (v[33] * v[30] - v[122] * v[43])) * x[163] + v[43] * v[107] + v[30] * v[116] + + 4.89663865010925e-12 * v[111] + + (v[8] - (4.89663865010925e-12 * v[102] - v[101] * v[30])) * x[164] + + (v[32] - (v[101] * v[43] - 4.89663865010925e-12 * v[99])) * x[165] + + (v[31] - (v[99] * v[30] - v[102] * v[43])) * x[166] + v[43] * v[129] + v[30] * v[16] + + 4.89663865010925e-12 * v[89] + + (v[8] - (4.89663865010925e-12 * v[102] - v[101] * v[30])) * x[167] + + (v[32] - (v[101] * v[43] - 4.89663865010925e-12 * v[99])) * x[168] + + (v[31] - (v[99] * v[30] - v[102] * v[43])) * x[169] + v[43] * v[28] + v[30] * v[163] + + 4.89663865010925e-12 * v[90] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[170] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[171] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[172] + v[43] * v[50] + v[30] * v[87] + + 4.89663865010925e-12 * v[164] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[173] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[174] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[175] + v[43] * v[160] + v[30] * v[67] + + 4.89663865010925e-12 * v[157] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[176] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[177] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[178] + v[43] * v[133] + v[30] * v[148] + + 4.89663865010925e-12 * v[130] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[179] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[180] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[181] + v[43] * v[83] + v[30] * v[78] + + 4.89663865010925e-12 * v[120] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[182] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[183] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[184] + v[43] * v[121] + v[30] * v[74] + + 4.89663865010925e-12 * v[118] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[185] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[186] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[187] + v[43] * v[112] + v[30] * v[131] + + 4.89663865010925e-12 * v[109] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[188] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[189] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[190] + v[43] * v[103] + v[30] * v[115] + + 4.89663865010925e-12 * v[100] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[191] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[192] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[193] + v[43] * v[94] + v[30] * v[106] + + 4.89663865010925e-12 * v[91] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[194] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[195] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[196] + v[43] * v[39] + v[30] * v[97] + + 4.89663865010925e-12 * v[84] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[197] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[198] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[199] + v[43] * v[72] + v[30] * v[88] + + 4.89663865010925e-12 * v[68] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[200] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[201] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[202] + v[43] * v[167] + v[30] * v[75] + + 4.89663865010925e-12 * v[52] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[203] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[204] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[205] + v[43] * v[48] + v[30] * v[66] + + 4.89663865010925e-12 * v[44] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[206] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[207] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[208] + v[43] * v[36] + v[30] * v[59] + + 4.89663865010925e-12 * v[17] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[209] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[210] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[211] + v[43] * v[5] + v[30] * v[35] + + 4.89663865010925e-12 * v[166] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[212] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[213] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[214] + v[43] * v[2] + v[30] * v[9] + + 4.89663865010925e-12 * v[162] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[215] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[216] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[217] + v[43] * v[38] + v[30] * v[40] + + 4.89663865010925e-12 * v[20] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[218] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[219] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[220] + v[43] * v[56] + v[30] * v[4] + + 4.89663865010925e-12 * v[53] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[221] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[222] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[223] + v[43] * v[27] + v[30] * v[165] + + 4.89663865010925e-12 * v[24] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[224] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[225] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[226] + v[43] * v[12] + v[30] * v[51] + + 4.89663865010925e-12 * v[3] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[227] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[228] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[229] + v[43] * v[10] + v[30] * v[19] + + 4.89663865010925e-12 * v[18] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[230] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[231] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[232] + v[43] * v[42] + v[30] * v[41] + + 4.89663865010925e-12 * v[1] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[233] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[234] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[235] + v[43] * v[14] + v[30] * v[45] + + 4.89663865010925e-12 * v[171] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[236] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[237] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[238] + v[43] * v[169] + v[30] * v[34] + + 4.89663865010925e-12 * v[49] + + (v[8] - (4.89663865010925e-12 * v[26] - 0.399976 * v[30])) * x[296] + + (v[32] - (0.399976 * v[43] - 4.89663865010925e-12 * v[170])) * x[297] + + (v[31] - (v[170] * v[30] - v[26] * v[43])) * x[298] + + (v[8] - (4.89663865010925e-12 * v[82] - v[13] * v[30])) * x[299] + + (v[32] - (v[13] * v[43] - 4.89663865010925e-12 * v[58])) * x[300] + + (v[31] - (v[58] * v[30] - v[82] * v[43])) * x[301] + v[43] * v[168] + v[30] * v[152] + + 4.89663865010925e-12 * v[156] + + (v[8] - (4.89663865010925e-12 * v[134] - v[144] * v[30])) * x[302] + + (v[32] - (v[144] * v[43] - 4.89663865010925e-12 * v[138])) * x[303] + + (v[31] - (v[138] * v[30] - v[134] * v[43])) * x[304] + + (v[8] - (4.89663865010925e-12 * v[122] - v[79] * v[30])) * x[305] + + (v[32] - (v[79] * v[43] - 4.89663865010925e-12 * v[33])) * x[306] + + (v[31] - (v[33] * v[30] - v[122] * v[43])) * x[307] + v[43] * v[113] + v[30] * v[119] + + 4.89663865010925e-12 * v[146] + + (v[8] - (4.89663865010925e-12 * v[102] - v[101] * v[30])) * x[308] + + (v[32] - (v[101] * v[43] - 4.89663865010925e-12 * v[99])) * x[309] + + (v[31] - (v[99] * v[30] - v[102] * v[43])) * x[310] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[311] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[312] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[313] + v[43] * v[172] + v[30] * v[123] + + 4.89663865010925e-12 * v[21] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[314] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[315] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[316] + v[43] * v[15] + v[30] * v[158] + + 4.89663865010925e-12 * v[25] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[317] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[318] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[319] + v[43] * v[0] + v[30] * v[173] + + 4.89663865010925e-12 * v[11] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[320] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[321] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[322] + v[43] * v[29] + v[30] * v[22] + + 4.89663865010925e-12 * v[86] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[323] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[324] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[325] + v[43] * v[77] + v[30] * v[140] + + 4.89663865010925e-12 * v[81] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[326] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[327] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[328] + v[43] * v[73] + v[30] * v[85] + + 4.89663865010925e-12 * v[70] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[329] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[330] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[331] + v[43] * v[136] + v[30] * v[145] + + 4.89663865010925e-12 * v[69] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[332] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[333] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[334] + v[43] * v[57] + v[30] * v[142] + + 4.89663865010925e-12 * v[55] + + (v[8] - (4.89663865010925e-12 * v[60] - v[65] * v[30])) * x[335] + + (v[32] - (v[65] * v[43] - 4.89663865010925e-12 * v[61])) * x[336] + + (v[31] - (v[61] * v[30] - v[60] * v[43])) * x[337] + v[43] * v[149] + v[30] * v[6] + + 4.89663865010925e-12 * v[98]; + v[31] = cos(x[8]); + v[32] = sin(x[8]); + v[8] = 4.89663865010925e-12 * v[32]; + v[170] = v[47] * v[31] + v[30] * v[8]; + v[46] = v[46] + 0.102 * v[170]; + v[26] = -1. * v[32]; + v[7] = -1. * v[31]; + v[105] = v[26] + 2.39770700697438e-23 + -4.89658313895802e-12 * v[7]; + v[62] = 0.399976 + 0.102 * v[26]; + v[32] = -v[32]; + v[76] = 4.89663865010925e-12 * v[31]; + v[47] = v[47] * v[32] + v[30] * v[76]; + v[150] = v[170] + -4.89658313895802e-12 * v[47] + 4.89663865010925e-12 * v[30]; + v[153] = v[46] * v[105] - v[62] * v[150]; + v[8] = v[54] * v[31] + v[43] * v[8]; + v[76] = v[54] * v[32] + v[43] * v[76]; + v[32] = v[8] + -4.89658313895802e-12 * v[76] + 4.89663865010925e-12 * v[43]; + v[95] = v[95] + 0.102 * v[8]; + v[54] = v[62] * v[32] - v[95] * v[105]; + v[31] = v[95] * v[150] - v[46] * v[32]; + y[9] = (v[153] - (v[82] * v[105] - v[13] * v[150])) * x[143] + + (v[54] - (v[13] * v[32] - v[58] * v[105])) * x[144] + + (v[31] - (v[58] * v[150] - v[82] * v[32])) * x[145] + v[32] * v[147] + v[150] * v[155] + + v[105] * v[37] + (v[153] - (v[82] * v[105] - v[13] * v[150])) * x[146] + + (v[54] - (v[13] * v[32] - v[58] * v[105])) * x[147] + + (v[31] - (v[58] * v[150] - v[82] * v[32])) * x[148] + v[32] * v[137] + v[150] * v[143] + + v[105] * v[141] + (v[153] - (v[82] * v[105] - v[13] * v[150])) * x[149] + + (v[54] - (v[13] * v[32] - v[58] * v[105])) * x[150] + + (v[31] - (v[58] * v[150] - v[82] * v[32])) * x[151] + + (v[153] - (v[134] * v[105] - v[144] * v[150])) * x[152] + + (v[54] - (v[144] * v[32] - v[138] * v[105])) * x[153] + + (v[31] - (v[138] * v[150] - v[134] * v[32])) * x[154] + + (v[153] - (v[122] * v[105] - v[79] * v[150])) * x[155] + + (v[54] - (v[79] * v[32] - v[33] * v[105])) * x[156] + + (v[31] - (v[33] * v[150] - v[122] * v[32])) * x[157] + + (v[153] - (v[122] * v[105] - v[79] * v[150])) * x[158] + + (v[54] - (v[79] * v[32] - v[33] * v[105])) * x[159] + + (v[31] - (v[33] * v[150] - v[122] * v[32])) * x[160] + v[32] * v[117] + v[150] * v[126] + + v[105] * v[110] + (v[153] - (v[122] * v[105] - v[79] * v[150])) * x[161] + + (v[54] - (v[79] * v[32] - v[33] * v[105])) * x[162] + + (v[31] - (v[33] * v[150] - v[122] * v[32])) * x[163] + v[32] * v[107] + v[150] * v[116] + + v[105] * v[111] + (v[153] - (v[102] * v[105] - v[101] * v[150])) * x[164] + + (v[54] - (v[101] * v[32] - v[99] * v[105])) * x[165] + + (v[31] - (v[99] * v[150] - v[102] * v[32])) * x[166] + v[32] * v[129] + v[150] * v[16] + + v[105] * v[89] + (v[153] - (v[102] * v[105] - v[101] * v[150])) * x[167] + + (v[54] - (v[101] * v[32] - v[99] * v[105])) * x[168] + + (v[31] - (v[99] * v[150] - v[102] * v[32])) * x[169] + v[32] * v[28] + v[150] * v[163] + + v[105] * v[90] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[170] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[171] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[172] + v[32] * v[50] + v[150] * v[87] + + v[105] * v[164] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[173] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[174] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[175] + v[32] * v[160] + v[150] * v[67] + + v[105] * v[157] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[176] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[177] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[178] + v[32] * v[133] + v[150] * v[148] + + v[105] * v[130] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[179] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[180] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[181] + v[32] * v[83] + v[150] * v[78] + + v[105] * v[120] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[182] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[183] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[184] + v[32] * v[121] + v[150] * v[74] + + v[105] * v[118] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[185] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[186] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[187] + v[32] * v[112] + v[150] * v[131] + + v[105] * v[109] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[188] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[189] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[190] + v[32] * v[103] + v[150] * v[115] + + v[105] * v[100] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[191] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[192] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[193] + v[32] * v[94] + v[150] * v[106] + + v[105] * v[91] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[194] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[195] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[196] + v[32] * v[39] + v[150] * v[97] + + v[105] * v[84] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[197] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[198] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[199] + v[32] * v[72] + v[150] * v[88] + + v[105] * v[68] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[200] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[201] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[202] + v[32] * v[167] + v[150] * v[75] + + v[105] * v[52] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[203] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[204] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[205] + v[32] * v[48] + v[150] * v[66] + + v[105] * v[44] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[206] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[207] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[208] + v[32] * v[36] + v[150] * v[59] + + v[105] * v[17] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[209] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[210] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[211] + v[32] * v[5] + v[150] * v[35] + + v[105] * v[166] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[212] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[213] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[214] + v[32] * v[2] + v[150] * v[9] + + v[105] * v[162] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[215] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[216] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[217] + v[32] * v[38] + v[150] * v[40] + + v[105] * v[20] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[218] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[219] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[220] + v[32] * v[56] + v[150] * v[4] + + v[105] * v[53] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[221] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[222] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[223] + v[32] * v[27] + v[150] * v[165] + + v[105] * v[24] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[224] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[225] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[226] + v[32] * v[12] + v[150] * v[51] + + v[105] * v[3] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[227] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[228] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[229] + v[32] * v[10] + v[150] * v[19] + + v[105] * v[18] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[230] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[231] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[232] + v[32] * v[42] + v[150] * v[41] + + v[105] * v[1] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[233] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[234] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[235] + v[32] * v[14] + v[150] * v[45] + + v[105] * v[171] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[236] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[237] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[238] + v[32] * v[169] + v[150] * v[34] + + v[105] * v[49] + (v[153] - (v[82] * v[105] - v[13] * v[150])) * x[299] + + (v[54] - (v[13] * v[32] - v[58] * v[105])) * x[300] + + (v[31] - (v[58] * v[150] - v[82] * v[32])) * x[301] + v[32] * v[168] + v[150] * v[152] + + v[105] * v[156] + (v[153] - (v[134] * v[105] - v[144] * v[150])) * x[302] + + (v[54] - (v[144] * v[32] - v[138] * v[105])) * x[303] + + (v[31] - (v[138] * v[150] - v[134] * v[32])) * x[304] + + (v[153] - (v[122] * v[105] - v[79] * v[150])) * x[305] + + (v[54] - (v[79] * v[32] - v[33] * v[105])) * x[306] + + (v[31] - (v[33] * v[150] - v[122] * v[32])) * x[307] + v[32] * v[113] + v[150] * v[119] + + v[105] * v[146] + (v[153] - (v[102] * v[105] - v[101] * v[150])) * x[308] + + (v[54] - (v[101] * v[32] - v[99] * v[105])) * x[309] + + (v[31] - (v[99] * v[150] - v[102] * v[32])) * x[310] + + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[311] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[312] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[313] + v[32] * v[172] + v[150] * v[123] + + v[105] * v[21] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[314] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[315] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[316] + v[32] * v[15] + v[150] * v[158] + + v[105] * v[25] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[317] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[318] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[319] + v[32] * v[0] + v[150] * v[173] + + v[105] * v[11] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[320] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[321] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[322] + v[32] * v[29] + v[150] * v[22] + + v[105] * v[86] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[323] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[324] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[325] + v[32] * v[77] + v[150] * v[140] + + v[105] * v[81] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[326] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[327] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[328] + v[32] * v[73] + v[150] * v[85] + + v[105] * v[70] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[329] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[330] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[331] + v[32] * v[136] + v[150] * v[145] + + v[105] * v[69] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[332] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[333] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[334] + v[32] * v[57] + v[150] * v[142] + + v[105] * v[55] + (v[153] - (v[60] * v[105] - v[65] * v[150])) * x[335] + + (v[54] - (v[65] * v[32] - v[61] * v[105])) * x[336] + + (v[31] - (v[61] * v[150] - v[60] * v[32])) * x[337] + v[32] * v[149] + v[150] * v[6] + + v[105] * v[98]; + v[31] = cos(x[9]); + v[54] = sin(x[9]); + v[153] = 4.89663865010925e-12 * v[31] + -4.89658313895802e-12 * v[54]; + v[152] = 5.55111512312578e-17 * v[31] + v[54]; + v[168] = v[170] * v[153] + v[47] * v[31] + v[30] * v[152]; + v[46] = v[46] + 0.069 * v[168] + 0.26242 * v[150]; + v[143] = v[26] * v[153] + v[7] * v[31] + 4.89663865010925e-12 * v[152]; + v[54] = -v[54]; + v[137] = 4.89663865010925e-12 * v[54] + -4.89658313895802e-12 * v[31]; + v[155] = 5.55111512312578e-17 * v[54] + v[31]; + v[7] = v[26] * v[137] + v[7] * v[54] + 4.89663865010925e-12 * v[155]; + v[26] = -4.89658313895802e-12 * v[143] + v[7]; + v[62] = v[62] + 0.069 * v[143] + 0.26242 * v[105]; + v[47] = v[170] * v[137] + v[47] * v[54] + v[30] * v[155]; + v[170] = -4.89658313895802e-12 * v[168] + v[47]; + v[30] = v[46] * v[26] - v[62] * v[170]; + v[152] = v[8] * v[153] + v[76] * v[31] + v[43] * v[152]; + v[155] = v[8] * v[137] + v[76] * v[54] + v[43] * v[155]; + v[137] = -4.89658313895802e-12 * v[152] + v[155]; + v[95] = v[95] + 0.069 * v[152] + 0.26242 * v[32]; + v[54] = v[62] * v[137] - v[95] * v[26]; + v[76] = v[95] * v[170] - v[46] * v[137]; + y[10] = (v[30] - (v[134] * v[26] - v[144] * v[170])) * x[152] + + (v[54] - (v[144] * v[137] - v[138] * v[26])) * x[153] + + (v[76] - (v[138] * v[170] - v[134] * v[137])) * x[154] + + (v[30] - (v[122] * v[26] - v[79] * v[170])) * x[155] + + (v[54] - (v[79] * v[137] - v[33] * v[26])) * x[156] + + (v[76] - (v[33] * v[170] - v[122] * v[137])) * x[157] + + (v[30] - (v[122] * v[26] - v[79] * v[170])) * x[158] + + (v[54] - (v[79] * v[137] - v[33] * v[26])) * x[159] + + (v[76] - (v[33] * v[170] - v[122] * v[137])) * x[160] + v[137] * v[117] + + v[170] * v[126] + v[26] * v[110] + (v[30] - (v[122] * v[26] - v[79] * v[170])) * x[161] + + (v[54] - (v[79] * v[137] - v[33] * v[26])) * x[162] + + (v[76] - (v[33] * v[170] - v[122] * v[137])) * x[163] + v[137] * v[107] + + v[170] * v[116] + v[26] * v[111] + (v[30] - (v[102] * v[26] - v[101] * v[170])) * x[164] + + (v[54] - (v[101] * v[137] - v[99] * v[26])) * x[165] + + (v[76] - (v[99] * v[170] - v[102] * v[137])) * x[166] + v[137] * v[129] + v[170] * v[16] + + v[26] * v[89] + (v[30] - (v[102] * v[26] - v[101] * v[170])) * x[167] + + (v[54] - (v[101] * v[137] - v[99] * v[26])) * x[168] + + (v[76] - (v[99] * v[170] - v[102] * v[137])) * x[169] + v[137] * v[28] + v[170] * v[163] + + v[26] * v[90] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[170] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[171] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[172] + v[137] * v[50] + v[170] * v[87] + + v[26] * v[164] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[173] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[174] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[175] + v[137] * v[160] + v[170] * v[67] + + v[26] * v[157] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[176] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[177] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[178] + v[137] * v[133] + v[170] * v[148] + + v[26] * v[130] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[179] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[180] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[181] + v[137] * v[83] + v[170] * v[78] + + v[26] * v[120] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[182] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[183] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[184] + v[137] * v[121] + v[170] * v[74] + + v[26] * v[118] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[185] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[186] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[187] + v[137] * v[112] + v[170] * v[131] + + v[26] * v[109] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[188] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[189] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[190] + v[137] * v[103] + v[170] * v[115] + + v[26] * v[100] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[191] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[192] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[193] + v[137] * v[94] + v[170] * v[106] + + v[26] * v[91] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[194] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[195] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[196] + v[137] * v[39] + v[170] * v[97] + + v[26] * v[84] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[197] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[198] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[199] + v[137] * v[72] + v[170] * v[88] + + v[26] * v[68] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[200] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[201] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[202] + v[137] * v[167] + v[170] * v[75] + + v[26] * v[52] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[203] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[204] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[205] + v[137] * v[48] + v[170] * v[66] + + v[26] * v[44] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[206] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[207] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[208] + v[137] * v[36] + v[170] * v[59] + + v[26] * v[17] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[209] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[210] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[211] + v[137] * v[5] + v[170] * v[35] + + v[26] * v[166] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[212] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[213] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[214] + v[137] * v[2] + v[170] * v[9] + + v[26] * v[162] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[215] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[216] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[217] + v[137] * v[38] + v[170] * v[40] + + v[26] * v[20] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[218] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[219] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[220] + v[137] * v[56] + v[170] * v[4] + + v[26] * v[53] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[221] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[222] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[223] + v[137] * v[27] + v[170] * v[165] + + v[26] * v[24] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[224] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[225] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[226] + v[137] * v[12] + v[170] * v[51] + + v[26] * v[3] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[227] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[228] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[229] + v[137] * v[10] + v[170] * v[19] + + v[26] * v[18] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[230] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[231] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[232] + v[137] * v[42] + v[170] * v[41] + + v[26] * v[1] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[233] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[234] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[235] + v[137] * v[14] + v[170] * v[45] + + v[26] * v[171] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[236] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[237] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[238] + v[137] * v[169] + v[170] * v[34] + + v[26] * v[49] + (v[30] - (v[134] * v[26] - v[144] * v[170])) * x[302] + + (v[54] - (v[144] * v[137] - v[138] * v[26])) * x[303] + + (v[76] - (v[138] * v[170] - v[134] * v[137])) * x[304] + + (v[30] - (v[122] * v[26] - v[79] * v[170])) * x[305] + + (v[54] - (v[79] * v[137] - v[33] * v[26])) * x[306] + + (v[76] - (v[33] * v[170] - v[122] * v[137])) * x[307] + v[137] * v[113] + + v[170] * v[119] + v[26] * v[146] + (v[30] - (v[102] * v[26] - v[101] * v[170])) * x[308] + + (v[54] - (v[101] * v[137] - v[99] * v[26])) * x[309] + + (v[76] - (v[99] * v[170] - v[102] * v[137])) * x[310] + + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[311] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[312] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[313] + v[137] * v[172] + v[170] * v[123] + + v[26] * v[21] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[314] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[315] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[316] + v[137] * v[15] + v[170] * v[158] + + v[26] * v[25] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[317] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[318] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[319] + v[137] * v[0] + v[170] * v[173] + + v[26] * v[11] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[320] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[321] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[322] + v[137] * v[29] + v[170] * v[22] + + v[26] * v[86] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[323] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[324] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[325] + v[137] * v[77] + v[170] * v[140] + + v[26] * v[81] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[326] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[327] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[328] + v[137] * v[73] + v[170] * v[85] + + v[26] * v[70] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[329] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[330] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[331] + v[137] * v[136] + v[170] * v[145] + + v[26] * v[69] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[332] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[333] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[334] + v[137] * v[57] + v[170] * v[142] + + v[26] * v[55] + (v[30] - (v[60] * v[26] - v[65] * v[170])) * x[335] + + (v[54] - (v[65] * v[137] - v[61] * v[26])) * x[336] + + (v[76] - (v[61] * v[170] - v[60] * v[137])) * x[337] + v[137] * v[149] + v[170] * v[6] + + v[26] * v[98]; + v[76] = cos(x[10]); + v[54] = sin(x[10]); + v[30] = 4.89663865010925e-12 * v[76] + v[54]; + v[144] = 5.55111512312578e-17 * v[76] + 4.89663865010925e-12 * v[54]; + v[138] = v[76] + -4.89658313895802e-12 * v[54]; + v[134] = v[168] * v[30] + v[47] * v[144] + v[150] * v[138]; + v[46] = v[46] + 0.10359 * v[134]; + v[8] = v[143] * v[30] + v[7] * v[144] + v[105] * v[138]; + v[54] = -v[54]; + v[43] = 4.89663865010925e-12 * v[54] + v[76]; + v[153] = 5.55111512312578e-17 * v[54] + 4.89663865010925e-12 * v[76]; + v[54] = v[54] + -4.89658313895802e-12 * v[76]; + v[7] = v[143] * v[43] + v[7] * v[153] + v[105] * v[54]; + v[143] = v[8] + -4.89658313895802e-12 * v[7] + 4.89663865010925e-12 * v[26]; + v[62] = v[62] + 0.10359 * v[8]; + v[47] = v[168] * v[43] + v[47] * v[153] + v[150] * v[54]; + v[168] = v[134] + -4.89658313895802e-12 * v[47] + 4.89663865010925e-12 * v[170]; + v[150] = v[46] * v[143] - v[62] * v[168]; + v[138] = v[152] * v[30] + v[155] * v[144] + v[32] * v[138]; + v[54] = v[152] * v[43] + v[155] * v[153] + v[32] * v[54]; + v[153] = v[138] + -4.89658313895802e-12 * v[54] + 4.89663865010925e-12 * v[137]; + v[95] = v[95] + 0.10359 * v[138]; + v[43] = v[62] * v[153] - v[95] * v[143]; + v[155] = v[95] * v[168] - v[46] * v[153]; + y[11] = + (v[150] - (v[122] * v[143] - v[79] * v[168])) * x[155] + + (v[43] - (v[79] * v[153] - v[33] * v[143])) * x[156] + + (v[155] - (v[33] * v[168] - v[122] * v[153])) * x[157] + + (v[150] - (v[122] * v[143] - v[79] * v[168])) * x[158] + + (v[43] - (v[79] * v[153] - v[33] * v[143])) * x[159] + + (v[155] - (v[33] * v[168] - v[122] * v[153])) * x[160] + v[153] * v[117] + v[168] * v[126] + + v[143] * v[110] + (v[150] - (v[122] * v[143] - v[79] * v[168])) * x[161] + + (v[43] - (v[79] * v[153] - v[33] * v[143])) * x[162] + + (v[155] - (v[33] * v[168] - v[122] * v[153])) * x[163] + v[153] * v[107] + v[168] * v[116] + + v[143] * v[111] + (v[150] - (v[102] * v[143] - v[101] * v[168])) * x[164] + + (v[43] - (v[101] * v[153] - v[99] * v[143])) * x[165] + + (v[155] - (v[99] * v[168] - v[102] * v[153])) * x[166] + v[153] * v[129] + v[168] * v[16] + + v[143] * v[89] + (v[150] - (v[102] * v[143] - v[101] * v[168])) * x[167] + + (v[43] - (v[101] * v[153] - v[99] * v[143])) * x[168] + + (v[155] - (v[99] * v[168] - v[102] * v[153])) * x[169] + v[153] * v[28] + v[168] * v[163] + + v[143] * v[90] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[170] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[171] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[172] + v[153] * v[50] + v[168] * v[87] + + v[143] * v[164] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[173] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[174] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[175] + v[153] * v[160] + v[168] * v[67] + + v[143] * v[157] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[176] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[177] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[178] + v[153] * v[133] + v[168] * v[148] + + v[143] * v[130] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[179] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[180] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[181] + v[153] * v[83] + v[168] * v[78] + + v[143] * v[120] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[182] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[183] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[184] + v[153] * v[121] + v[168] * v[74] + + v[143] * v[118] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[185] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[186] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[187] + v[153] * v[112] + v[168] * v[131] + + v[143] * v[109] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[188] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[189] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[190] + v[153] * v[103] + v[168] * v[115] + + v[143] * v[100] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[191] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[192] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[193] + v[153] * v[94] + v[168] * v[106] + + v[143] * v[91] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[194] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[195] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[196] + v[153] * v[39] + v[168] * v[97] + + v[143] * v[84] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[197] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[198] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[199] + v[153] * v[72] + v[168] * v[88] + + v[143] * v[68] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[200] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[201] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[202] + v[153] * v[167] + v[168] * v[75] + + v[143] * v[52] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[203] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[204] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[205] + v[153] * v[48] + v[168] * v[66] + + v[143] * v[44] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[206] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[207] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[208] + v[153] * v[36] + v[168] * v[59] + + v[143] * v[17] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[209] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[210] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[211] + v[153] * v[5] + v[168] * v[35] + + v[143] * v[166] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[212] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[213] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[214] + v[153] * v[2] + v[168] * v[9] + + v[143] * v[162] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[215] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[216] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[217] + v[153] * v[38] + v[168] * v[40] + + v[143] * v[20] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[218] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[219] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[220] + v[153] * v[56] + v[168] * v[4] + + v[143] * v[53] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[221] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[222] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[223] + v[153] * v[27] + v[168] * v[165] + + v[143] * v[24] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[224] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[225] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[226] + v[153] * v[12] + v[168] * v[51] + + v[143] * v[3] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[227] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[228] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[229] + v[153] * v[10] + v[168] * v[19] + + v[143] * v[18] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[230] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[231] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[232] + v[153] * v[42] + v[168] * v[41] + + v[143] * v[1] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[233] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[234] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[235] + v[153] * v[14] + v[168] * v[45] + + v[143] * v[171] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[236] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[237] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[238] + v[153] * v[169] + v[168] * v[34] + + v[143] * v[49] + (v[150] - (v[122] * v[143] - v[79] * v[168])) * x[305] + + (v[43] - (v[79] * v[153] - v[33] * v[143])) * x[306] + + (v[155] - (v[33] * v[168] - v[122] * v[153])) * x[307] + v[153] * v[113] + v[168] * v[119] + + v[143] * v[146] + (v[150] - (v[102] * v[143] - v[101] * v[168])) * x[308] + + (v[43] - (v[101] * v[153] - v[99] * v[143])) * x[309] + + (v[155] - (v[99] * v[168] - v[102] * v[153])) * x[310] + + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[311] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[312] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[313] + v[153] * v[172] + v[168] * v[123] + + v[143] * v[21] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[314] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[315] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[316] + v[153] * v[15] + v[168] * v[158] + + v[143] * v[25] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[317] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[318] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[319] + v[153] * v[0] + v[168] * v[173] + + v[143] * v[11] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[320] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[321] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[322] + v[153] * v[29] + v[168] * v[22] + + v[143] * v[86] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[323] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[324] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[325] + v[153] * v[77] + v[168] * v[140] + + v[143] * v[81] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[326] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[327] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[328] + v[153] * v[73] + v[168] * v[85] + + v[143] * v[70] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[329] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[330] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[331] + v[153] * v[136] + v[168] * v[145] + + v[143] * v[69] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[332] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[333] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[334] + v[153] * v[57] + v[168] * v[142] + + v[143] * v[55] + (v[150] - (v[60] * v[143] - v[65] * v[168])) * x[335] + + (v[43] - (v[65] * v[153] - v[61] * v[143])) * x[336] + + (v[155] - (v[61] * v[168] - v[60] * v[153])) * x[337] + v[153] * v[149] + v[168] * v[6] + + v[143] * v[98]; + v[155] = cos(x[11]); + v[43] = sin(x[11]); + v[150] = 4.89663865010925e-12 * v[155] + -4.89658313895802e-12 * v[43]; + v[119] = 5.55111512312578e-17 * v[155] + v[43]; + v[113] = v[134] * v[150] + v[47] * v[155] + v[170] * v[119]; + v[46] = v[46] + 0.01 * v[113] + 0.2707 * v[168]; + v[116] = v[8] * v[150] + v[7] * v[155] + v[26] * v[119]; + v[43] = -v[43]; + v[107] = 4.89663865010925e-12 * v[43] + -4.89658313895802e-12 * v[155]; + v[126] = 5.55111512312578e-17 * v[43] + v[155]; + v[7] = v[8] * v[107] + v[7] * v[43] + v[26] * v[126]; + v[8] = -4.89658313895802e-12 * v[116] + v[7]; + v[62] = v[62] + 0.01 * v[116] + 0.2707 * v[143]; + v[47] = v[134] * v[107] + v[47] * v[43] + v[170] * v[126]; + v[134] = -4.89658313895802e-12 * v[113] + v[47]; + v[170] = v[46] * v[8] - v[62] * v[134]; + v[119] = v[138] * v[150] + v[54] * v[155] + v[137] * v[119]; + v[126] = v[138] * v[107] + v[54] * v[43] + v[137] * v[126]; + v[107] = -4.89658313895802e-12 * v[119] + v[126]; + v[95] = v[95] + 0.01 * v[119] + 0.2707 * v[153]; + v[43] = v[62] * v[107] - v[95] * v[8]; + v[54] = v[95] * v[134] - v[46] * v[107]; + y[12] = (v[170] - (v[102] * v[8] - v[101] * v[134])) * x[164] + + (v[43] - (v[101] * v[107] - v[99] * v[8])) * x[165] + + (v[54] - (v[99] * v[134] - v[102] * v[107])) * x[166] + v[107] * v[129] + v[134] * v[16] + + v[8] * v[89] + (v[170] - (v[102] * v[8] - v[101] * v[134])) * x[167] + + (v[43] - (v[101] * v[107] - v[99] * v[8])) * x[168] + + (v[54] - (v[99] * v[134] - v[102] * v[107])) * x[169] + v[107] * v[28] + v[134] * v[163] + + v[8] * v[90] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[170] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[171] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[172] + v[107] * v[50] + v[134] * v[87] + + v[8] * v[164] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[173] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[174] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[175] + v[107] * v[160] + v[134] * v[67] + + v[8] * v[157] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[176] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[177] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[178] + v[107] * v[133] + v[134] * v[148] + + v[8] * v[130] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[179] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[180] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[181] + v[107] * v[83] + v[134] * v[78] + + v[8] * v[120] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[182] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[183] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[184] + v[107] * v[121] + v[134] * v[74] + + v[8] * v[118] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[185] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[186] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[187] + v[107] * v[112] + v[134] * v[131] + + v[8] * v[109] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[188] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[189] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[190] + v[107] * v[103] + v[134] * v[115] + + v[8] * v[100] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[191] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[192] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[193] + v[107] * v[94] + v[134] * v[106] + + v[8] * v[91] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[194] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[195] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[196] + v[107] * v[39] + v[134] * v[97] + + v[8] * v[84] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[197] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[198] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[199] + v[107] * v[72] + v[134] * v[88] + + v[8] * v[68] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[200] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[201] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[202] + v[107] * v[167] + v[134] * v[75] + + v[8] * v[52] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[203] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[204] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[205] + v[107] * v[48] + v[134] * v[66] + + v[8] * v[44] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[206] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[207] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[208] + v[107] * v[36] + v[134] * v[59] + + v[8] * v[17] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[209] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[210] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[211] + v[107] * v[5] + v[134] * v[35] + + v[8] * v[166] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[212] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[213] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[214] + v[107] * v[2] + v[134] * v[9] + + v[8] * v[162] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[215] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[216] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[217] + v[107] * v[38] + v[134] * v[40] + + v[8] * v[20] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[218] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[219] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[220] + v[107] * v[56] + v[134] * v[4] + + v[8] * v[53] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[221] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[222] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[223] + v[107] * v[27] + v[134] * v[165] + + v[8] * v[24] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[224] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[225] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[226] + v[107] * v[12] + v[134] * v[51] + + v[8] * v[3] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[227] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[228] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[229] + v[107] * v[10] + v[134] * v[19] + + v[8] * v[18] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[230] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[231] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[232] + v[107] * v[42] + v[134] * v[41] + + v[8] * v[1] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[233] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[234] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[235] + v[107] * v[14] + v[134] * v[45] + + v[8] * v[171] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[236] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[237] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[238] + v[107] * v[169] + v[134] * v[34] + + v[8] * v[49] + (v[170] - (v[102] * v[8] - v[101] * v[134])) * x[308] + + (v[43] - (v[101] * v[107] - v[99] * v[8])) * x[309] + + (v[54] - (v[99] * v[134] - v[102] * v[107])) * x[310] + + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[311] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[312] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[313] + v[107] * v[172] + v[134] * v[123] + + v[8] * v[21] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[314] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[315] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[316] + v[107] * v[15] + v[134] * v[158] + + v[8] * v[25] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[317] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[318] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[319] + v[107] * v[0] + v[134] * v[173] + + v[8] * v[11] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[320] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[321] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[322] + v[107] * v[29] + v[134] * v[22] + + v[8] * v[86] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[323] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[324] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[325] + v[107] * v[77] + v[134] * v[140] + + v[8] * v[81] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[326] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[327] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[328] + v[107] * v[73] + v[134] * v[85] + + v[8] * v[70] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[329] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[330] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[331] + v[107] * v[136] + v[134] * v[145] + + v[8] * v[69] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[332] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[333] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[334] + v[107] * v[57] + v[134] * v[142] + + v[8] * v[55] + (v[170] - (v[60] * v[8] - v[65] * v[134])) * x[335] + + (v[43] - (v[65] * v[107] - v[61] * v[8])) * x[336] + + (v[54] - (v[61] * v[134] - v[60] * v[107])) * x[337] + v[107] * v[149] + v[134] * v[6] + + v[8] * v[98]; + v[54] = cos(x[12]); + v[43] = sin(x[12]); + v[170] = 4.89663865010925e-12 * v[54] + v[43]; + v[163] = 5.55111512312578e-17 * v[54] + 4.89663865010925e-12 * v[43]; + v[28] = v[54] + -4.89658313895802e-12 * v[43]; + v[16] = v[113] * v[170] + v[47] * v[163] + v[168] * v[28]; + v[46] = v[46] + 0.115975 * v[16]; + v[129] = v[116] * v[170] + v[7] * v[163] + v[143] * v[28]; + v[43] = -v[43]; + v[101] = 4.89663865010925e-12 * v[43] + v[54]; + v[90] = 5.55111512312578e-17 * v[43] + 4.89663865010925e-12 * v[54]; + v[43] = v[43] + -4.89658313895802e-12 * v[54]; + v[8] = v[129] + -4.89658313895802e-12 * (v[116] * v[101] + v[7] * v[90] + v[143] * v[43]) + + 4.89663865010925e-12 * v[8]; + v[129] = v[62] + 0.115975 * v[129]; + v[16] = v[16] + -4.89658313895802e-12 * (v[113] * v[101] + v[47] * v[90] + v[168] * v[43]) + + 4.89663865010925e-12 * v[134]; + v[134] = v[46] * v[8] - v[129] * v[16]; + v[28] = v[119] * v[170] + v[126] * v[163] + v[153] * v[28]; + v[43] = v[28] + -4.89658313895802e-12 * (v[119] * v[101] + v[126] * v[90] + v[153] * v[43]) + + 4.89663865010925e-12 * v[107]; + v[28] = v[95] + 0.115975 * v[28]; + v[129] = v[129] * v[43] - v[28] * v[8]; + v[28] = v[28] * v[16] - v[46] * v[43]; + y[13] = (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[170] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[171] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[172] + v[43] * v[50] + v[16] * v[87] + + v[8] * v[164] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[173] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[174] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[175] + v[43] * v[160] + v[16] * v[67] + + v[8] * v[157] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[176] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[177] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[178] + v[43] * v[133] + v[16] * v[148] + + v[8] * v[130] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[179] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[180] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[181] + v[43] * v[83] + v[16] * v[78] + + v[8] * v[120] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[182] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[183] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[184] + v[43] * v[121] + v[16] * v[74] + + v[8] * v[118] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[185] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[186] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[187] + v[43] * v[112] + v[16] * v[131] + + v[8] * v[109] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[188] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[189] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[190] + v[43] * v[103] + v[16] * v[115] + + v[8] * v[100] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[191] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[192] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[193] + v[43] * v[94] + v[16] * v[106] + + v[8] * v[91] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[194] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[195] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[196] + v[43] * v[39] + v[16] * v[97] + + v[8] * v[84] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[197] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[198] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[199] + v[43] * v[72] + v[16] * v[88] + + v[8] * v[68] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[200] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[201] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[202] + v[43] * v[167] + v[16] * v[75] + + v[8] * v[52] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[203] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[204] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[205] + v[43] * v[48] + v[16] * v[66] + + v[8] * v[44] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[206] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[207] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[208] + v[43] * v[36] + v[16] * v[59] + + v[8] * v[17] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[209] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[210] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[211] + v[43] * v[5] + v[16] * v[35] + + v[8] * v[166] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[212] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[213] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[214] + v[43] * v[2] + v[16] * v[9] + + v[8] * v[162] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[215] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[216] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[217] + v[43] * v[38] + v[16] * v[40] + + v[8] * v[20] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[218] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[219] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[220] + v[43] * v[56] + v[16] * v[4] + + v[8] * v[53] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[221] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[222] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[223] + v[43] * v[27] + v[16] * v[165] + + v[8] * v[24] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[224] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[225] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[226] + v[43] * v[12] + v[16] * v[51] + + v[8] * v[3] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[227] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[228] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[229] + v[43] * v[10] + v[16] * v[19] + + v[8] * v[18] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[230] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[231] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[232] + v[43] * v[42] + v[16] * v[41] + + v[8] * v[1] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[233] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[234] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[235] + v[43] * v[14] + v[16] * v[45] + + v[8] * v[171] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[236] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[237] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[238] + v[43] * v[169] + v[16] * v[34] + + v[8] * v[49] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[311] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[312] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[313] + v[43] * v[172] + v[16] * v[123] + + v[8] * v[21] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[314] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[315] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[316] + v[43] * v[15] + v[16] * v[158] + + v[8] * v[25] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[317] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[318] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[319] + v[43] * v[0] + v[16] * v[173] + + v[8] * v[11] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[320] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[321] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[322] + v[43] * v[29] + v[16] * v[22] + + v[8] * v[86] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[323] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[324] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[325] + v[43] * v[77] + v[16] * v[140] + + v[8] * v[81] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[326] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[327] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[328] + v[43] * v[73] + v[16] * v[85] + + v[8] * v[70] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[329] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[330] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[331] + v[43] * v[136] + v[16] * v[145] + + v[8] * v[69] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[332] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[333] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[334] + v[43] * v[57] + v[16] * v[142] + + v[8] * v[55] + (v[134] - (v[60] * v[8] - v[65] * v[16])) * x[335] + + (v[129] - (v[65] * v[43] - v[61] * v[8])) * x[336] + + (v[28] - (v[61] * v[16] - v[60] * v[43])) * x[337] + v[43] * v[149] + v[16] * v[6] + + v[8] * v[98]; + + // Copy result to output + for (std::size_t i = 0; i < 14; ++i) + { + out[i] = y[i]; + } + } }; } // namespace vamp::robots diff --git a/src/impl/vamp/robots/fetch.hh b/src/impl/vamp/robots/fetch.hh index b7c4eb98..7721c1cf 100644 --- a/src/impl/vamp/robots/fetch.hh +++ b/src/impl/vamp/robots/fetch.hh @@ -1318,349 +1318,263 @@ namespace vamp::robots output.first.emplace_back( sphere_environment_get_collisions(environment, y[96], y[97], y[98], y[99])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[100], y[101], y[102], y[103])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[100], y[101], y[102], y[103])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[104], y[105], y[106], y[107])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[104], y[105], y[106], y[107])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[108], y[109], y[110], y[111])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[108], y[109], y[110], y[111])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[112], y[113], y[114], y[115])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[112], y[113], y[114], y[115])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[116], y[117], y[118], y[119])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[116], y[117], y[118], y[119])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[120], y[121], y[122], y[123])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[120], y[121], y[122], y[123])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[124], y[125], y[126], y[127])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[124], y[125], y[126], y[127])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[128], y[129], y[130], y[131])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[128], y[129], y[130], y[131])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[132], y[133], y[134], y[135])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[132], y[133], y[134], y[135])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[136], y[137], y[138], y[139])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[136], y[137], y[138], y[139])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[140], y[141], y[142], y[143])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[140], y[141], y[142], y[143])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[144], y[145], y[146], y[147])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[144], y[145], y[146], y[147])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[148], y[149], y[150], y[151])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[148], y[149], y[150], y[151])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[152], y[153], y[154], y[155])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[152], y[153], y[154], y[155])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[156], y[157], y[158], y[159])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[156], y[157], y[158], y[159])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[160], y[161], y[162], y[163])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[160], y[161], y[162], y[163])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[164], y[165], y[166], y[167])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[164], y[165], y[166], y[167])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[168], y[169], y[170], y[171])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[168], y[169], y[170], y[171])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[172], y[173], y[174], y[175])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[172], y[173], y[174], y[175])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[176], y[177], y[178], y[179])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[176], y[177], y[178], y[179])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[180], y[181], y[182], y[183])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[180], y[181], y[182], y[183])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[184], y[185], y[186], y[187])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[184], y[185], y[186], y[187])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[188], y[189], y[190], y[191])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[188], y[189], y[190], y[191])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[192], y[193], y[194], y[195])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[192], y[193], y[194], y[195])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[196], y[197], y[198], y[199])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[196], y[197], y[198], y[199])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[200], y[201], y[202], y[203])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[200], y[201], y[202], y[203])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[204], y[205], y[206], y[207])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[204], y[205], y[206], y[207])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[208], y[209], y[210], y[211])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[208], y[209], y[210], y[211])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[212], y[213], y[214], y[215])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[212], y[213], y[214], y[215])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[216], y[217], y[218], y[219])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[216], y[217], y[218], y[219])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[220], y[221], y[222], y[223])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[220], y[221], y[222], y[223])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[224], y[225], y[226], y[227])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[224], y[225], y[226], y[227])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[228], y[229], y[230], y[231])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[228], y[229], y[230], y[231])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[232], y[233], y[234], y[235])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[232], y[233], y[234], y[235])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[236], y[237], y[238], y[239])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[236], y[237], y[238], y[239])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[240], y[241], y[242], y[243])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[240], y[241], y[242], y[243])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[244], y[245], y[246], y[247])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[244], y[245], y[246], y[247])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[248], y[249], y[250], y[251])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[248], y[249], y[250], y[251])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[252], y[253], y[254], y[255])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[252], y[253], y[254], y[255])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[256], y[257], y[258], y[259])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[256], y[257], y[258], y[259])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[260], y[261], y[262], y[263])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[260], y[261], y[262], y[263])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[264], y[265], y[266], y[267])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[264], y[265], y[266], y[267])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[268], y[269], y[270], y[271])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[268], y[269], y[270], y[271])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[272], y[273], y[274], y[275])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[272], y[273], y[274], y[275])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[276], y[277], y[278], y[279])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[276], y[277], y[278], y[279])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[280], y[281], y[282], y[283])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[280], y[281], y[282], y[283])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[284], y[285], y[286], y[287])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[284], y[285], y[286], y[287])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[288], y[289], y[290], y[291])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[288], y[289], y[290], y[291])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[292], y[293], y[294], y[295])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[292], y[293], y[294], y[295])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[296], y[297], y[298], y[299])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[296], y[297], y[298], y[299])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[300], y[301], y[302], y[303])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[300], y[301], y[302], y[303])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[304], y[305], y[306], y[307])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[304], y[305], y[306], y[307])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[308], y[309], y[310], y[311])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[308], y[309], y[310], y[311])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[312], y[313], y[314], y[315])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[312], y[313], y[314], y[315])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[316], y[317], y[318], y[319])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[316], y[317], y[318], y[319])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[320], y[321], y[322], y[323])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[320], y[321], y[322], y[323])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[324], y[325], y[326], y[327])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[324], y[325], y[326], y[327])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[328], y[329], y[330], y[331])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[328], y[329], y[330], y[331])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[332], y[333], y[334], y[335])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[332], y[333], y[334], y[335])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[336], y[337], y[338], y[339])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[336], y[337], y[338], y[339])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[340], y[341], y[342], y[343])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[340], y[341], y[342], y[343])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[344], y[345], y[346], y[347])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[344], y[345], y[346], y[347])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[348], y[349], y[350], y[351])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[348], y[349], y[350], y[351])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[352], y[353], y[354], y[355])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[352], y[353], y[354], y[355])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[356], y[357], y[358], y[359])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[356], y[357], y[358], y[359])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[360], y[361], y[362], y[363])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[360], y[361], y[362], y[363])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[364], y[365], y[366], y[367])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[364], y[365], y[366], y[367])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[368], y[369], y[370], y[371])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[368], y[369], y[370], y[371])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[372], y[373], y[374], y[375])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[372], y[373], y[374], y[375])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[376], y[377], y[378], y[379])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[376], y[377], y[378], y[379])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[380], y[381], y[382], y[383])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[380], y[381], y[382], y[383])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[384], y[385], y[386], y[387])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[384], y[385], y[386], y[387])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[388], y[389], y[390], y[391])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[388], y[389], y[390], y[391])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[392], y[393], y[394], y[395])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[392], y[393], y[394], y[395])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[396], y[397], y[398], y[399])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[396], y[397], y[398], y[399])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[400], y[401], y[402], y[403])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[400], y[401], y[402], y[403])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[404], y[405], y[406], y[407])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[404], y[405], y[406], y[407])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[408], y[409], y[410], y[411])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[408], y[409], y[410], y[411])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[412], y[413], y[414], y[415])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[412], y[413], y[414], y[415])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[416], y[417], y[418], y[419])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[416], y[417], y[418], y[419])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[420], y[421], y[422], y[423])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[420], y[421], y[422], y[423])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[424], y[425], y[426], y[427])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[424], y[425], y[426], y[427])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[428], y[429], y[430], y[431])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[428], y[429], y[430], y[431])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[432], y[433], y[434], y[435])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[432], y[433], y[434], y[435])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[436], y[437], y[438], y[439])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[436], y[437], y[438], y[439])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[440], y[441], y[442], y[443])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[440], y[441], y[442], y[443])); if (sphere_sphere_self_collision( y[0], y[1], y[2], y[3], y[292], y[293], y[294], y[295])) @@ -51688,6 +51602,2879 @@ namespace vamp::robots return to_isometry(y.data()); } + + template + inline static auto sdf_gradient( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &x) noexcept + -> std::pair, FloatVector> + { + std::array, 50> v; + std::array, 504> y; + + v[0] = 0.37743 + x[0]; + y[82] = 0.15 + v[0]; + y[86] = 0.15 + v[0]; + y[90] = 0.3 + v[0]; + y[94] = 0.45 + v[0]; + y[98] = 0.45 + v[0]; + y[102] = 0.3 + v[0]; + y[106] = 0.663001417713939 + v[0]; + y[110] = 0.661001417713939 + v[0]; + y[114] = 0.661001417713939 + v[0]; + y[118] = 0.661001417713939 + v[0]; + y[122] = 0.661001417713939 + v[0]; + y[126] = 0.661001417713939 + v[0]; + y[130] = 0.633001417713939 + v[0]; + y[134] = 0.633001417713939 + v[0]; + y[138] = 0.633001417713939 + v[0]; + y[142] = 0.633001417713939 + v[0]; + y[146] = 0.688001417713939 + v[0]; + y[150] = 0.688001417713939 + v[0]; + y[154] = 0.688001417713939 + v[0]; + y[158] = 0.688001417713939 + v[0]; + y[162] = 0.678001417713939 + v[0]; + y[166] = 0.660501417713939 + v[0]; + y[170] = 0.643001417713939 + v[0]; + y[174] = 0.633001417713939 + v[0]; + y[178] = 0.633001417713939 + v[0]; + y[182] = 0.633001417713939 + v[0]; + y[186] = 0.633001417713939 + v[0]; + y[190] = 0.688001417713939 + v[0]; + y[194] = 0.688001417713939 + v[0]; + y[198] = 0.688001417713939 + v[0]; + y[202] = 0.688001417713939 + v[0]; + y[206] = 0.678001417713939 + v[0]; + y[210] = 0.660501417713939 + v[0]; + y[214] = 0.643001417713939 + v[0]; + y[218] = 0.34858 + v[0]; + v[1] = cos(x[1]); + v[2] = sin(x[1]); + v[3] = -v[2]; + y[220] = 0.03265 + 0.025 * v[1] + -0.015 * v[3]; + y[221] = 0.025 * v[2] + -0.015 * v[1]; + y[222] = 0.035 + y[218]; + y[224] = 0.03265 + 0.05 * v[1] + -0.03 * v[3]; + y[225] = 0.05 * v[2] + -0.03 * v[1]; + y[226] = 0.06 + y[218]; + y[228] = 0.03265 + 0.12 * v[1] + -0.03 * v[3]; + y[229] = 0.12 * v[2] + -0.03 * v[1]; + y[230] = 0.06 + y[218]; + v[4] = cos(x[2]); + v[5] = v[1] * v[4]; + v[6] = sin(x[2]); + v[7] = v[1] * v[6]; + v[8] = 0.03265 + 0.117 * v[1]; + y[232] = 0.025 * v[5] + 0.04 * v[3] + 0.025 * v[7] + v[8]; + v[9] = v[2] * v[4]; + v[10] = v[2] * v[6]; + v[11] = 0.117 * v[2]; + y[233] = 0.025 * v[9] + 0.04 * v[1] + 0.025 * v[10] + v[11]; + v[6] = -v[6]; + v[12] = 0.0599999999999999 + y[218]; + y[234] = 0.025 * v[6] + 0.025 * v[4] + v[12]; + y[236] = -0.025 * v[5] + 0.04 * v[3] + -0.025 * v[7] + v[8]; + y[237] = -0.025 * v[9] + 0.04 * v[1] + -0.025 * v[10] + v[11]; + y[238] = -0.025 * v[6] + -0.025 * v[4] + v[12]; + y[240] = 0.025 * v[5] + 0.04 * v[3] + -0.025 * v[7] + v[8]; + y[241] = 0.025 * v[9] + 0.04 * v[1] + -0.025 * v[10] + v[11]; + y[242] = 0.025 * v[6] + -0.025 * v[4] + v[12]; + y[244] = -0.025 * v[5] + 0.04 * v[3] + 0.025 * v[7] + v[8]; + y[245] = -0.025 * v[9] + 0.04 * v[1] + 0.025 * v[10] + v[11]; + y[246] = -0.025 * v[6] + 0.025 * v[4] + v[12]; + y[248] = 0.08 * v[5] + v[8]; + y[249] = 0.08 * v[9] + v[11]; + y[250] = 0.08 * v[6] + v[12]; + y[252] = 0.11 * v[5] + v[8]; + y[253] = 0.11 * v[9] + v[11]; + y[254] = 0.11 * v[6] + v[12]; + y[256] = 0.14 * v[5] + v[8]; + y[257] = 0.14 * v[9] + v[11]; + y[258] = 0.14 * v[6] + v[12]; + v[13] = v[8] + 0.219 * v[5]; + y[260] = -0.02 * v[5] + v[13]; + v[14] = v[11] + 0.219 * v[9]; + y[261] = -0.02 * v[9] + v[14]; + v[15] = v[12] + 0.219 * v[6]; + y[262] = -0.02 * v[6] + v[15]; + y[264] = 0.03 * v[5] + v[13]; + y[265] = 0.03 * v[9] + v[14]; + y[266] = 0.03 * v[6] + v[15]; + y[268] = 0.08 * v[5] + v[13]; + y[269] = 0.08 * v[9] + v[14]; + y[270] = 0.08 * v[6] + v[15]; + v[16] = cos(x[3]); + v[17] = sin(x[3]); + v[18] = v[3] * v[16] + v[7] * v[17]; + v[19] = -v[17]; + v[20] = v[3] * v[19] + v[7] * v[16]; + y[272] = 0.11 * v[5] + -0.045 * v[18] + 0.02 * v[20] + v[13]; + v[21] = v[1] * v[16] + v[10] * v[17]; + v[19] = v[1] * v[19] + v[10] * v[16]; + y[273] = 0.11 * v[9] + -0.045 * v[21] + 0.02 * v[19] + v[14]; + v[17] = v[4] * v[17]; + v[16] = v[4] * v[16]; + y[274] = 0.11 * v[6] + -0.045 * v[17] + 0.02 * v[16] + v[15]; + y[276] = 0.11 * v[5] + -0.045 * v[18] + -0.02 * v[20] + v[13]; + y[277] = 0.11 * v[9] + -0.045 * v[21] + -0.02 * v[19] + v[14]; + y[278] = 0.11 * v[6] + -0.045 * v[17] + -0.02 * v[16] + v[15]; + y[280] = 0.155 * v[5] + -0.045 * v[18] + 0.02 * v[20] + v[13]; + y[281] = 0.155 * v[9] + -0.045 * v[21] + 0.02 * v[19] + v[14]; + y[282] = 0.155 * v[6] + -0.045 * v[17] + 0.02 * v[16] + v[15]; + y[284] = 0.155 * v[5] + -0.045 * v[18] + -0.02 * v[20] + v[13]; + y[285] = 0.155 * v[9] + -0.045 * v[21] + -0.02 * v[19] + v[14]; + y[286] = 0.155 * v[6] + -0.045 * v[17] + -0.02 * v[16] + v[15]; + y[288] = 0.13 * v[5] + v[13]; + y[289] = 0.13 * v[9] + v[14]; + y[290] = 0.13 * v[6] + v[15]; + v[22] = cos(x[4]); + v[23] = sin(x[4]); + v[24] = -v[23]; + v[25] = v[5] * v[22] + v[20] * v[24]; + v[26] = v[5] * v[23] + v[20] * v[22]; + v[27] = v[13] + 0.133 * v[5]; + y[292] = 0.02 * v[25] + 0.045 * v[18] + 0.02 * v[26] + v[27]; + v[28] = v[9] * v[22] + v[19] * v[24]; + v[29] = v[9] * v[23] + v[19] * v[22]; + v[30] = v[14] + 0.133 * v[9]; + y[293] = 0.02 * v[28] + 0.045 * v[21] + 0.02 * v[29] + v[30]; + v[24] = v[6] * v[22] + v[16] * v[24]; + v[23] = v[6] * v[23] + v[16] * v[22]; + v[22] = v[15] + 0.133 * v[6]; + y[294] = 0.02 * v[24] + 0.045 * v[17] + 0.02 * v[23] + v[22]; + y[296] = 0.02 * v[25] + 0.045 * v[18] + -0.02 * v[26] + v[27]; + y[297] = 0.02 * v[28] + 0.045 * v[21] + -0.02 * v[29] + v[30]; + y[298] = 0.02 * v[24] + 0.045 * v[17] + -0.02 * v[23] + v[22]; + y[300] = -0.02 * v[25] + 0.045 * v[18] + 0.02 * v[26] + v[27]; + y[301] = -0.02 * v[28] + 0.045 * v[21] + 0.02 * v[29] + v[30]; + y[302] = -0.02 * v[24] + 0.045 * v[17] + 0.02 * v[23] + v[22]; + y[304] = -0.02 * v[25] + 0.045 * v[18] + -0.02 * v[26] + v[27]; + y[305] = -0.02 * v[28] + 0.045 * v[21] + -0.02 * v[29] + v[30]; + y[306] = -0.02 * v[24] + 0.045 * v[17] + -0.02 * v[23] + v[22]; + y[308] = 0.08 * v[25] + v[27]; + y[309] = 0.08 * v[28] + v[30]; + y[310] = 0.08 * v[24] + v[22]; + y[312] = 0.14 * v[25] + v[27]; + y[313] = 0.14 * v[28] + v[30]; + y[314] = 0.14 * v[24] + v[22]; + y[316] = v[27] + 0.197 * v[25]; + y[317] = v[30] + 0.197 * v[28]; + y[318] = v[22] + 0.197 * v[24]; + v[31] = cos(x[5]); + v[32] = sin(x[5]); + v[33] = v[18] * v[31] + v[26] * v[32]; + v[34] = -v[32]; + v[35] = v[18] * v[34] + v[26] * v[31]; + y[320] = 0.05 * v[25] + -0.06 * v[33] + 0.02 * v[35] + y[316]; + v[36] = v[21] * v[31] + v[29] * v[32]; + v[37] = v[21] * v[34] + v[29] * v[31]; + y[321] = 0.05 * v[28] + -0.06 * v[36] + 0.02 * v[37] + y[317]; + v[32] = v[17] * v[31] + v[23] * v[32]; + v[34] = v[17] * v[34] + v[23] * v[31]; + y[322] = 0.05 * v[24] + -0.06 * v[32] + 0.02 * v[34] + y[318]; + y[324] = 0.05 * v[25] + -0.06 * v[33] + -0.02 * v[35] + y[316]; + y[325] = 0.05 * v[28] + -0.06 * v[36] + -0.02 * v[37] + y[317]; + y[326] = 0.05 * v[24] + -0.06 * v[32] + -0.02 * v[34] + y[318]; + y[328] = 0.1 * v[25] + -0.06 * v[33] + 0.02 * v[35] + y[316]; + y[329] = 0.1 * v[28] + -0.06 * v[36] + 0.02 * v[37] + y[317]; + y[330] = 0.1 * v[24] + -0.06 * v[32] + 0.02 * v[34] + y[318]; + y[332] = 0.1 * v[25] + -0.06 * v[33] + -0.02 * v[35] + y[316]; + y[333] = 0.1 * v[28] + -0.06 * v[36] + -0.02 * v[37] + y[317]; + y[334] = 0.1 * v[24] + -0.06 * v[32] + -0.02 * v[34] + y[318]; + y[336] = 0.15 * v[25] + -0.06 * v[33] + 0.02 * v[35] + y[316]; + y[337] = 0.15 * v[28] + -0.06 * v[36] + 0.02 * v[37] + y[317]; + y[338] = 0.15 * v[24] + -0.06 * v[32] + 0.02 * v[34] + y[318]; + y[340] = 0.15 * v[25] + -0.06 * v[33] + -0.02 * v[35] + y[316]; + y[341] = 0.15 * v[28] + -0.06 * v[36] + -0.02 * v[37] + y[317]; + y[342] = 0.15 * v[24] + -0.06 * v[32] + -0.02 * v[34] + y[318]; + y[344] = y[316] + 0.1245 * v[25]; + y[345] = y[317] + 0.1245 * v[28]; + y[346] = y[318] + 0.1245 * v[24]; + v[31] = cos(x[6]); + v[38] = sin(x[6]); + v[39] = -v[38]; + v[40] = v[25] * v[31] + v[35] * v[39]; + y[348] = 0.06 * v[40] + y[344]; + v[41] = v[28] * v[31] + v[37] * v[39]; + y[349] = 0.06 * v[41] + y[345]; + v[39] = v[24] * v[31] + v[34] * v[39]; + y[350] = 0.06 * v[39] + y[346]; + v[42] = v[25] * v[38] + v[35] * v[31]; + y[352] = 0.02 * v[40] + 0.045 * v[33] + 0.02 * v[42] + y[344]; + v[43] = v[28] * v[38] + v[37] * v[31]; + y[353] = 0.02 * v[41] + 0.045 * v[36] + 0.02 * v[43] + y[345]; + v[38] = v[24] * v[38] + v[34] * v[31]; + y[354] = 0.02 * v[39] + 0.045 * v[32] + 0.02 * v[38] + y[346]; + y[356] = 0.02 * v[40] + 0.045 * v[33] + -0.02 * v[42] + y[344]; + y[357] = 0.02 * v[41] + 0.045 * v[36] + -0.02 * v[43] + y[345]; + y[358] = 0.02 * v[39] + 0.045 * v[32] + -0.02 * v[38] + y[346]; + y[360] = -0.02 * v[40] + 0.045 * v[33] + 0.02 * v[42] + y[344]; + y[361] = -0.02 * v[41] + 0.045 * v[36] + 0.02 * v[43] + y[345]; + y[362] = -0.02 * v[39] + 0.045 * v[32] + 0.02 * v[38] + y[346]; + y[364] = -0.02 * v[40] + 0.045 * v[33] + -0.02 * v[42] + y[344]; + y[365] = -0.02 * v[41] + 0.045 * v[36] + -0.02 * v[43] + y[345]; + y[366] = -0.02 * v[39] + 0.045 * v[32] + -0.02 * v[38] + y[346]; + y[372] = y[344] + 0.1385 * v[40]; + y[368] = -0.03 * v[40] + y[372]; + y[373] = y[345] + 0.1385 * v[41]; + y[369] = -0.03 * v[41] + y[373]; + y[374] = y[346] + 0.1385 * v[39]; + y[370] = -0.03 * v[39] + y[374]; + v[31] = cos(x[7]); + v[44] = sin(x[7]); + v[45] = v[33] * v[31] + v[42] * v[44]; + y[376] = 0.09645 * v[40] + 0.02 * v[45] + y[372]; + v[46] = v[36] * v[31] + v[43] * v[44]; + y[377] = 0.09645 * v[41] + 0.02 * v[46] + y[373]; + v[47] = v[32] * v[31] + v[38] * v[44]; + y[378] = 0.09645 * v[39] + 0.02 * v[47] + y[374]; + y[380] = 0.09645 * v[40] + -0.02 * v[45] + y[372]; + y[381] = 0.09645 * v[41] + -0.02 * v[46] + y[373]; + y[382] = 0.09645 * v[39] + -0.02 * v[47] + y[374]; + y[384] = 0.06645 * v[40] + 0.02 * v[45] + y[372]; + y[385] = 0.06645 * v[41] + 0.02 * v[46] + y[373]; + y[386] = 0.06645 * v[39] + 0.02 * v[47] + y[374]; + y[388] = 0.06645 * v[40] + -0.02 * v[45] + y[372]; + y[389] = 0.06645 * v[41] + -0.02 * v[46] + y[373]; + y[390] = 0.06645 * v[39] + -0.02 * v[47] + y[374]; + v[44] = -v[44]; + v[48] = v[33] * v[44] + v[42] * v[31]; + y[392] = 0.18345 * v[40] + -0.056925 * v[45] + -0.005 * v[48] + y[372]; + v[49] = v[36] * v[44] + v[43] * v[31]; + y[393] = 0.18345 * v[41] + -0.056925 * v[46] + -0.005 * v[49] + y[373]; + v[44] = v[32] * v[44] + v[38] * v[31]; + y[394] = 0.18345 * v[39] + -0.056925 * v[47] + -0.005 * v[44] + y[374]; + y[396] = 0.18345 * v[40] + -0.056925 * v[45] + 0.005 * v[48] + y[372]; + y[397] = 0.18345 * v[41] + -0.056925 * v[46] + 0.005 * v[49] + y[373]; + y[398] = 0.18345 * v[39] + -0.056925 * v[47] + 0.005 * v[44] + y[374]; + y[400] = 0.16645 * v[40] + -0.056925 * v[45] + -0.005 * v[48] + y[372]; + y[401] = 0.16645 * v[41] + -0.056925 * v[46] + -0.005 * v[49] + y[373]; + y[402] = 0.16645 * v[39] + -0.056925 * v[47] + -0.005 * v[44] + y[374]; + y[404] = 0.16645 * v[40] + -0.056925 * v[45] + 0.005 * v[48] + y[372]; + y[405] = 0.16645 * v[41] + -0.056925 * v[46] + 0.005 * v[49] + y[373]; + y[406] = 0.16645 * v[39] + -0.056925 * v[47] + 0.005 * v[44] + y[374]; + y[408] = 0.14945 * v[40] + -0.056925 * v[45] + -0.005 * v[48] + y[372]; + y[409] = 0.14945 * v[41] + -0.056925 * v[46] + -0.005 * v[49] + y[373]; + y[410] = 0.14945 * v[39] + -0.056925 * v[47] + -0.005 * v[44] + y[374]; + y[412] = 0.14945 * v[40] + -0.056925 * v[45] + 0.005 * v[48] + y[372]; + y[413] = 0.14945 * v[41] + -0.056925 * v[46] + 0.005 * v[49] + y[373]; + y[414] = 0.14945 * v[39] + -0.056925 * v[47] + 0.005 * v[44] + y[374]; + y[416] = 0.18345 * v[40] + 0.056925 * v[45] + -0.005 * v[48] + y[372]; + y[417] = 0.18345 * v[41] + 0.056925 * v[46] + -0.005 * v[49] + y[373]; + y[418] = 0.18345 * v[39] + 0.056925 * v[47] + -0.005 * v[44] + y[374]; + y[420] = 0.18345 * v[40] + 0.056925 * v[45] + 0.005 * v[48] + y[372]; + y[421] = 0.18345 * v[41] + 0.056925 * v[46] + 0.005 * v[49] + y[373]; + y[422] = 0.18345 * v[39] + 0.056925 * v[47] + 0.005 * v[44] + y[374]; + y[424] = 0.16645 * v[40] + 0.056925 * v[45] + -0.005 * v[48] + y[372]; + y[425] = 0.16645 * v[41] + 0.056925 * v[46] + -0.005 * v[49] + y[373]; + y[426] = 0.16645 * v[39] + 0.056925 * v[47] + -0.005 * v[44] + y[374]; + y[428] = 0.16645 * v[40] + 0.056925 * v[45] + 0.005 * v[48] + y[372]; + y[429] = 0.16645 * v[41] + 0.056925 * v[46] + 0.005 * v[49] + y[373]; + y[430] = 0.16645 * v[39] + 0.056925 * v[47] + 0.005 * v[44] + y[374]; + y[432] = 0.14945 * v[40] + 0.056925 * v[45] + -0.005 * v[48] + y[372]; + y[433] = 0.14945 * v[41] + 0.056925 * v[46] + -0.005 * v[49] + y[373]; + y[434] = 0.14945 * v[39] + 0.056925 * v[47] + -0.005 * v[44] + y[374]; + y[436] = 0.14945 * v[40] + 0.056925 * v[45] + 0.005 * v[48] + y[372]; + y[437] = 0.14945 * v[41] + 0.056925 * v[46] + 0.005 * v[49] + y[373]; + y[438] = 0.14945 * v[39] + 0.056925 * v[47] + 0.005 * v[44] + y[374]; + y[442] = 0.24 + v[0]; + y[454] = 0.300000011920929 + v[0]; + y[458] = 0.662302553653717 + v[0]; + y[460] = 0.03265 + 0.0599999986588955 * v[1] + -0.0149999996647239 * v[3]; + y[461] = 0.0599999986588955 * v[2] + -0.0149999996647239 * v[1]; + y[462] = 0.0299999993294477 + y[218]; + y[464] = + 0.0631452798843384 * v[5] + 0.018631448969245 * v[3] + 1.73472347597681e-18 * v[7] + v[8]; + y[465] = + 0.0631452798843384 * v[9] + 0.018631448969245 * v[1] + 1.73472347597681e-18 * v[10] + v[11]; + y[466] = 0.0631452798843384 * v[6] + 1.73472347597681e-18 * v[4] + v[12]; + y[468] = 0.056335523724556 * v[5] + -0.0196291357278824 * v[18] + -1.73472347597681e-18 * v[20] + + v[13]; + y[469] = 0.056335523724556 * v[9] + -0.0196291357278824 * v[21] + -1.73472347597681e-18 * v[19] + + v[14]; + y[470] = 0.056335523724556 * v[6] + -0.0196291357278824 * v[17] + -1.73472347597681e-18 * v[16] + + v[15]; + y[472] = 0.0710262209177017 * v[25] + 0.0193988755345345 * v[18] + 1.73472347597681e-18 * v[26] + + v[27]; + y[473] = 0.0710262209177017 * v[28] + 0.0193988755345345 * v[21] + 1.73472347597681e-18 * v[29] + + v[30]; + y[474] = 0.0710262209177017 * v[24] + 0.0193988755345345 * v[17] + 1.73472347597681e-18 * v[23] + + v[22]; + y[476] = 0.0643894299864769 * v[25] + -0.0257557742297649 * v[33] + + -1.73472347597681e-18 * v[35] + y[316]; + y[477] = 0.0643894299864769 * v[28] + -0.0257557742297649 * v[36] + + -1.73472347597681e-18 * v[37] + y[317]; + y[478] = 0.0643894299864769 * v[24] + -0.0257557742297649 * v[32] + + -1.73472347597681e-18 * v[34] + y[318]; + y[480] = 0.0294021144509315 * v[40] + 0.0172113105654716 * v[33] + 4.33680868994202e-18 * v[42] + + y[344]; + y[481] = 0.0294021144509315 * v[41] + 0.0172113105654716 * v[36] + 4.33680868994202e-18 * v[43] + + y[345]; + y[482] = 0.0294021144509315 * v[39] + 0.0172113105654716 * v[32] + 4.33680868994202e-18 * v[38] + + y[346]; + y[484] = -0.0149999996647239 * v[40] + y[372]; + y[485] = -0.0149999996647239 * v[41] + y[373]; + y[486] = -0.0149999996647239 * v[39] + y[374]; + y[488] = 0.0814500004053116 * v[40] + y[372]; + y[489] = 0.0814500004053116 * v[41] + y[373]; + y[490] = 0.0814500004053116 * v[39] + y[374]; + y[492] = 0.166449993848801 * v[40] + -0.0569249987602234 * v[45] + y[372]; + y[493] = 0.166449993848801 * v[41] + -0.0569249987602234 * v[46] + y[373]; + y[494] = 0.166449993848801 * v[39] + -0.0569249987602234 * v[47] + y[374]; + y[496] = 0.166449993848801 * v[40] + 0.0569249987602234 * v[45] + y[372]; + y[497] = 0.166449993848801 * v[41] + 0.0569249987602234 * v[46] + y[373]; + y[498] = 0.166449993848801 * v[39] + 0.0569249987602234 * v[47] + y[374]; + y[502] = 0.239999994635582 + v[0]; + // dependent variables without operations + y[0] = -0.12; + y[1] = 0.; + y[2] = 0.182; + y[3] = 0.239999994635582; + y[4] = 0.225; + y[5] = 0.; + y[6] = 0.31; + y[7] = 0.0659999996423721; + y[8] = 0.08; + y[9] = -0.06; + y[10] = 0.16; + y[11] = 0.219999998807907; + y[12] = 0.215; + y[13] = -0.07; + y[14] = 0.31; + y[15] = 0.0659999996423721; + y[16] = 0.185; + y[17] = -0.135; + y[18] = 0.31; + y[19] = 0.0659999996423721; + y[20] = 0.13; + y[21] = -0.185; + y[22] = 0.31; + y[23] = 0.0659999996423721; + y[24] = 0.065; + y[25] = -0.2; + y[26] = 0.31; + y[27] = 0.0659999996423721; + y[28] = 0.01; + y[29] = -0.2; + y[30] = 0.31; + y[31] = 0.0659999996423721; + y[32] = 0.08; + y[33] = 0.06; + y[34] = 0.16; + y[35] = 0.219999998807907; + y[36] = 0.215; + y[37] = 0.07; + y[38] = 0.31; + y[39] = 0.0659999996423721; + y[40] = 0.185; + y[41] = 0.135; + y[42] = 0.31; + y[43] = 0.0659999996423721; + y[44] = 0.13; + y[45] = 0.185; + y[46] = 0.31; + y[47] = 0.0659999996423721; + y[48] = 0.065; + y[49] = 0.2; + y[50] = 0.31; + y[51] = 0.0659999996423721; + y[52] = 0.01; + y[53] = 0.2; + y[54] = 0.31; + y[55] = 0.0659999996423721; + y[56] = -0.186875; + y[57] = -0.07; + y[58] = 0.727425; + y[59] = 0.119999997317791; + y[60] = -0.186875; + y[61] = 0.07; + y[62] = 0.727425; + y[63] = 0.119999997317791; + y[64] = -0.186875; + y[65] = -0.07; + y[66] = 0.577425; + y[67] = 0.119999997317791; + y[68] = -0.186875; + y[69] = 0.07; + y[70] = 0.577425; + y[71] = 0.119999997317791; + y[72] = -0.186875; + y[73] = 0.07; + y[74] = 0.447425; + y[75] = 0.119999997317791; + y[76] = -0.186875; + y[77] = -0.07; + y[78] = 0.447425; + y[79] = 0.119999997317791; + y[80] = -0.186875; + y[81] = -0.05; + y[83] = 0.150000005960464; + y[84] = -0.186875; + y[85] = 0.05; + y[87] = 0.150000005960464; + y[88] = -0.186875; + y[89] = 0.05; + y[91] = 0.150000005960464; + y[92] = -0.186875; + y[93] = 0.05; + y[95] = 0.150000005960464; + y[96] = -0.186875; + y[97] = -0.05; + y[99] = 0.150000005960464; + y[100] = -0.186875; + y[101] = -0.05; + y[103] = 0.150000005960464; + y[104] = -0.03375; + y[105] = 0.; + y[107] = 0.150000005960464; + y[108] = 0.11125; + y[109] = 0.; + y[111] = 0.0500000007450581; + y[112] = 0.11125; + y[113] = -0.0425; + y[115] = 0.0500000007450581; + y[116] = 0.11125; + y[117] = 0.0425; + y[119] = 0.0500000007450581; + y[120] = 0.11125; + y[121] = 0.085; + y[123] = 0.0500000007450581; + y[124] = 0.11125; + y[125] = -0.085; + y[127] = 0.0500000007450581; + y[128] = 0.02875; + y[129] = -0.115; + y[131] = 0.0299999993294477; + y[132] = 0.05425; + y[133] = -0.115; + y[135] = 0.0299999993294477; + y[136] = 0.07975; + y[137] = -0.115; + y[139] = 0.0299999993294477; + y[140] = 0.10525; + y[141] = -0.115; + y[143] = 0.0299999993294477; + y[144] = 0.02875; + y[145] = -0.115; + y[147] = 0.0299999993294477; + y[148] = 0.05425; + y[149] = -0.115; + y[151] = 0.0299999993294477; + y[152] = 0.07975; + y[153] = -0.115; + y[155] = 0.0299999993294477; + y[156] = 0.10525; + y[157] = -0.115; + y[159] = 0.0299999993294477; + y[160] = 0.12625; + y[161] = -0.115; + y[163] = 0.0299999993294477; + y[164] = 0.13425; + y[165] = -0.115; + y[167] = 0.0299999993294477; + y[168] = 0.12625; + y[169] = -0.115; + y[171] = 0.0299999993294477; + y[172] = 0.02875; + y[173] = 0.115; + y[175] = 0.0299999993294477; + y[176] = 0.05425; + y[177] = 0.115; + y[179] = 0.0299999993294477; + y[180] = 0.07975; + y[181] = 0.115; + y[183] = 0.0299999993294477; + y[184] = 0.10525; + y[185] = 0.115; + y[187] = 0.0299999993294477; + y[188] = 0.02875; + y[189] = 0.115; + y[191] = 0.0299999993294477; + y[192] = 0.05425; + y[193] = 0.115; + y[195] = 0.0299999993294477; + y[196] = 0.07975; + y[197] = 0.115; + y[199] = 0.0299999993294477; + y[200] = 0.10525; + y[201] = 0.115; + y[203] = 0.0299999993294477; + y[204] = 0.12625; + y[205] = 0.115; + y[207] = 0.0299999993294477; + y[208] = 0.13425; + y[209] = 0.115; + y[211] = 0.0299999993294477; + y[212] = 0.12625; + y[213] = 0.115; + y[215] = 0.0299999993294477; + y[216] = 0.03265; + y[217] = 0.; + y[219] = 0.0549999997019768; + y[223] = 0.0549999997019768; + y[227] = 0.0549999997019768; + y[231] = 0.0549999997019768; + y[235] = 0.0399999991059303; + y[239] = 0.0399999991059303; + y[243] = 0.0399999991059303; + y[247] = 0.0399999991059303; + y[251] = 0.0549999997019768; + y[255] = 0.0549999997019768; + y[259] = 0.0549999997019768; + y[263] = 0.0549999997019768; + y[267] = 0.0549999997019768; + y[271] = 0.0549999997019768; + y[275] = 0.0299999993294477; + y[279] = 0.0299999993294477; + y[283] = 0.0299999993294477; + y[287] = 0.0299999993294477; + y[291] = 0.0549999997019768; + y[295] = 0.0299999993294477; + y[299] = 0.0299999993294477; + y[303] = 0.0299999993294477; + y[307] = 0.0299999993294477; + y[311] = 0.0549999997019768; + y[315] = 0.0549999997019768; + y[319] = 0.0549999997019768; + y[323] = 0.0299999993294477; + y[327] = 0.0299999993294477; + y[331] = 0.0299999993294477; + y[335] = 0.0299999993294477; + y[339] = 0.0299999993294477; + y[343] = 0.0299999993294477; + y[347] = 0.0549999997019768; + y[351] = 0.0549999997019768; + y[355] = 0.0299999993294477; + y[359] = 0.0299999993294477; + y[363] = 0.0299999993294477; + y[367] = 0.0299999993294477; + y[371] = 0.0549999997019768; + y[375] = 0.0549999997019768; + y[379] = 0.0500000007450581; + y[383] = 0.0500000007450581; + y[387] = 0.0500000007450581; + y[391] = 0.0500000007450581; + y[395] = 0.0120000001043081; + y[399] = 0.0120000001043081; + y[403] = 0.0120000001043081; + y[407] = 0.0120000001043081; + y[411] = 0.0120000001043081; + y[415] = 0.0120000001043081; + y[419] = 0.0120000001043081; + y[423] = 0.0120000001043081; + y[427] = 0.0120000001043081; + y[431] = 0.0120000001043081; + y[435] = 0.0120000001043081; + y[439] = 0.0120000001043081; + y[440] = 0.013125; + y[441] = 0.; + y[443] = 0.0700000002980232; + y[444] = -0.0201118849217892; + y[445] = -5.20417042793042e-17; + y[446] = 0.188239961862564; + y[447] = 0.340082824230194; + y[448] = -0.186875000596046; + y[449] = 2.77555756156289e-17; + y[450] = 0.587424993515015; + y[451] = 0.276524752378464; + y[452] = -0.186875001490116; + y[453] = 0.; + y[455] = 0.308113902807236; + y[456] = 0.0132126799225807; + y[457] = -2.42861286636753e-17; + y[459] = 0.196967884898186; + y[463] = 0.123738631606102; + y[467] = 0.134080842137337; + y[471] = 0.133818879723549; + y[475] = 0.12664982676506; + y[479] = 0.124349541962147; + y[483] = 0.0901064053177834; + y[487] = 0.0700000002980232; + y[491] = 0.0750000029802322; + y[495] = 0.0297200456261635; + y[499] = 0.0297200456261635; + y[500] = 0.0131250014901161; + y[501] = 0.; + y[503] = 0.0700000002980232; + + FloatVector dists; + FloatVector grads; + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[0], y[1], y[2], y[3]); + dists[0] = res.first; + grads[0] = res.second[0]; + grads[1] = res.second[1]; + grads[2] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[4], y[5], y[6], y[7]); + dists[1] = res.first; + grads[3] = res.second[0]; + grads[4] = res.second[1]; + grads[5] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[8], y[9], y[10], y[11]); + dists[2] = res.first; + grads[6] = res.second[0]; + grads[7] = res.second[1]; + grads[8] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[12], y[13], y[14], y[15]); + dists[3] = res.first; + grads[9] = res.second[0]; + grads[10] = res.second[1]; + grads[11] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[16], y[17], y[18], y[19]); + dists[4] = res.first; + grads[12] = res.second[0]; + grads[13] = res.second[1]; + grads[14] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[20], y[21], y[22], y[23]); + dists[5] = res.first; + grads[15] = res.second[0]; + grads[16] = res.second[1]; + grads[17] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[24], y[25], y[26], y[27]); + dists[6] = res.first; + grads[18] = res.second[0]; + grads[19] = res.second[1]; + grads[20] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[28], y[29], y[30], y[31]); + dists[7] = res.first; + grads[21] = res.second[0]; + grads[22] = res.second[1]; + grads[23] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[32], y[33], y[34], y[35]); + dists[8] = res.first; + grads[24] = res.second[0]; + grads[25] = res.second[1]; + grads[26] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[36], y[37], y[38], y[39]); + dists[9] = res.first; + grads[27] = res.second[0]; + grads[28] = res.second[1]; + grads[29] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[40], y[41], y[42], y[43]); + dists[10] = res.first; + grads[30] = res.second[0]; + grads[31] = res.second[1]; + grads[32] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[44], y[45], y[46], y[47]); + dists[11] = res.first; + grads[33] = res.second[0]; + grads[34] = res.second[1]; + grads[35] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[48], y[49], y[50], y[51]); + dists[12] = res.first; + grads[36] = res.second[0]; + grads[37] = res.second[1]; + grads[38] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[52], y[53], y[54], y[55]); + dists[13] = res.first; + grads[39] = res.second[0]; + grads[40] = res.second[1]; + grads[41] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[56], y[57], y[58], y[59]); + dists[14] = res.first; + grads[42] = res.second[0]; + grads[43] = res.second[1]; + grads[44] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[60], y[61], y[62], y[63]); + dists[15] = res.first; + grads[45] = res.second[0]; + grads[46] = res.second[1]; + grads[47] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[64], y[65], y[66], y[67]); + dists[16] = res.first; + grads[48] = res.second[0]; + grads[49] = res.second[1]; + grads[50] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[68], y[69], y[70], y[71]); + dists[17] = res.first; + grads[51] = res.second[0]; + grads[52] = res.second[1]; + grads[53] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[72], y[73], y[74], y[75]); + dists[18] = res.first; + grads[54] = res.second[0]; + grads[55] = res.second[1]; + grads[56] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[76], y[77], y[78], y[79]); + dists[19] = res.first; + grads[57] = res.second[0]; + grads[58] = res.second[1]; + grads[59] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[80], y[81], y[82], y[83]); + dists[20] = res.first; + grads[60] = res.second[0]; + grads[61] = res.second[1]; + grads[62] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[84], y[85], y[86], y[87]); + dists[21] = res.first; + grads[63] = res.second[0]; + grads[64] = res.second[1]; + grads[65] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[88], y[89], y[90], y[91]); + dists[22] = res.first; + grads[66] = res.second[0]; + grads[67] = res.second[1]; + grads[68] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[92], y[93], y[94], y[95]); + dists[23] = res.first; + grads[69] = res.second[0]; + grads[70] = res.second[1]; + grads[71] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[96], y[97], y[98], y[99]); + dists[24] = res.first; + grads[72] = res.second[0]; + grads[73] = res.second[1]; + grads[74] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[100], y[101], y[102], y[103]); + dists[25] = res.first; + grads[75] = res.second[0]; + grads[76] = res.second[1]; + grads[77] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[104], y[105], y[106], y[107]); + dists[26] = res.first; + grads[78] = res.second[0]; + grads[79] = res.second[1]; + grads[80] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[108], y[109], y[110], y[111]); + dists[27] = res.first; + grads[81] = res.second[0]; + grads[82] = res.second[1]; + grads[83] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[112], y[113], y[114], y[115]); + dists[28] = res.first; + grads[84] = res.second[0]; + grads[85] = res.second[1]; + grads[86] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[116], y[117], y[118], y[119]); + dists[29] = res.first; + grads[87] = res.second[0]; + grads[88] = res.second[1]; + grads[89] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[120], y[121], y[122], y[123]); + dists[30] = res.first; + grads[90] = res.second[0]; + grads[91] = res.second[1]; + grads[92] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[124], y[125], y[126], y[127]); + dists[31] = res.first; + grads[93] = res.second[0]; + grads[94] = res.second[1]; + grads[95] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[128], y[129], y[130], y[131]); + dists[32] = res.first; + grads[96] = res.second[0]; + grads[97] = res.second[1]; + grads[98] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[132], y[133], y[134], y[135]); + dists[33] = res.first; + grads[99] = res.second[0]; + grads[100] = res.second[1]; + grads[101] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[136], y[137], y[138], y[139]); + dists[34] = res.first; + grads[102] = res.second[0]; + grads[103] = res.second[1]; + grads[104] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[140], y[141], y[142], y[143]); + dists[35] = res.first; + grads[105] = res.second[0]; + grads[106] = res.second[1]; + grads[107] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[144], y[145], y[146], y[147]); + dists[36] = res.first; + grads[108] = res.second[0]; + grads[109] = res.second[1]; + grads[110] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[148], y[149], y[150], y[151]); + dists[37] = res.first; + grads[111] = res.second[0]; + grads[112] = res.second[1]; + grads[113] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[152], y[153], y[154], y[155]); + dists[38] = res.first; + grads[114] = res.second[0]; + grads[115] = res.second[1]; + grads[116] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[156], y[157], y[158], y[159]); + dists[39] = res.first; + grads[117] = res.second[0]; + grads[118] = res.second[1]; + grads[119] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[160], y[161], y[162], y[163]); + dists[40] = res.first; + grads[120] = res.second[0]; + grads[121] = res.second[1]; + grads[122] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[164], y[165], y[166], y[167]); + dists[41] = res.first; + grads[123] = res.second[0]; + grads[124] = res.second[1]; + grads[125] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[168], y[169], y[170], y[171]); + dists[42] = res.first; + grads[126] = res.second[0]; + grads[127] = res.second[1]; + grads[128] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[172], y[173], y[174], y[175]); + dists[43] = res.first; + grads[129] = res.second[0]; + grads[130] = res.second[1]; + grads[131] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[176], y[177], y[178], y[179]); + dists[44] = res.first; + grads[132] = res.second[0]; + grads[133] = res.second[1]; + grads[134] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[180], y[181], y[182], y[183]); + dists[45] = res.first; + grads[135] = res.second[0]; + grads[136] = res.second[1]; + grads[137] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[184], y[185], y[186], y[187]); + dists[46] = res.first; + grads[138] = res.second[0]; + grads[139] = res.second[1]; + grads[140] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[188], y[189], y[190], y[191]); + dists[47] = res.first; + grads[141] = res.second[0]; + grads[142] = res.second[1]; + grads[143] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[192], y[193], y[194], y[195]); + dists[48] = res.first; + grads[144] = res.second[0]; + grads[145] = res.second[1]; + grads[146] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[196], y[197], y[198], y[199]); + dists[49] = res.first; + grads[147] = res.second[0]; + grads[148] = res.second[1]; + grads[149] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[200], y[201], y[202], y[203]); + dists[50] = res.first; + grads[150] = res.second[0]; + grads[151] = res.second[1]; + grads[152] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[204], y[205], y[206], y[207]); + dists[51] = res.first; + grads[153] = res.second[0]; + grads[154] = res.second[1]; + grads[155] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[208], y[209], y[210], y[211]); + dists[52] = res.first; + grads[156] = res.second[0]; + grads[157] = res.second[1]; + grads[158] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[212], y[213], y[214], y[215]); + dists[53] = res.first; + grads[159] = res.second[0]; + grads[160] = res.second[1]; + grads[161] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[216], y[217], y[218], y[219]); + dists[54] = res.first; + grads[162] = res.second[0]; + grads[163] = res.second[1]; + grads[164] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[220], y[221], y[222], y[223]); + dists[55] = res.first; + grads[165] = res.second[0]; + grads[166] = res.second[1]; + grads[167] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[224], y[225], y[226], y[227]); + dists[56] = res.first; + grads[168] = res.second[0]; + grads[169] = res.second[1]; + grads[170] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[228], y[229], y[230], y[231]); + dists[57] = res.first; + grads[171] = res.second[0]; + grads[172] = res.second[1]; + grads[173] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[232], y[233], y[234], y[235]); + dists[58] = res.first; + grads[174] = res.second[0]; + grads[175] = res.second[1]; + grads[176] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[236], y[237], y[238], y[239]); + dists[59] = res.first; + grads[177] = res.second[0]; + grads[178] = res.second[1]; + grads[179] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[240], y[241], y[242], y[243]); + dists[60] = res.first; + grads[180] = res.second[0]; + grads[181] = res.second[1]; + grads[182] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[244], y[245], y[246], y[247]); + dists[61] = res.first; + grads[183] = res.second[0]; + grads[184] = res.second[1]; + grads[185] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[248], y[249], y[250], y[251]); + dists[62] = res.first; + grads[186] = res.second[0]; + grads[187] = res.second[1]; + grads[188] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[252], y[253], y[254], y[255]); + dists[63] = res.first; + grads[189] = res.second[0]; + grads[190] = res.second[1]; + grads[191] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[256], y[257], y[258], y[259]); + dists[64] = res.first; + grads[192] = res.second[0]; + grads[193] = res.second[1]; + grads[194] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[260], y[261], y[262], y[263]); + dists[65] = res.first; + grads[195] = res.second[0]; + grads[196] = res.second[1]; + grads[197] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[264], y[265], y[266], y[267]); + dists[66] = res.first; + grads[198] = res.second[0]; + grads[199] = res.second[1]; + grads[200] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[268], y[269], y[270], y[271]); + dists[67] = res.first; + grads[201] = res.second[0]; + grads[202] = res.second[1]; + grads[203] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[272], y[273], y[274], y[275]); + dists[68] = res.first; + grads[204] = res.second[0]; + grads[205] = res.second[1]; + grads[206] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[276], y[277], y[278], y[279]); + dists[69] = res.first; + grads[207] = res.second[0]; + grads[208] = res.second[1]; + grads[209] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[280], y[281], y[282], y[283]); + dists[70] = res.first; + grads[210] = res.second[0]; + grads[211] = res.second[1]; + grads[212] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[284], y[285], y[286], y[287]); + dists[71] = res.first; + grads[213] = res.second[0]; + grads[214] = res.second[1]; + grads[215] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[288], y[289], y[290], y[291]); + dists[72] = res.first; + grads[216] = res.second[0]; + grads[217] = res.second[1]; + grads[218] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[292], y[293], y[294], y[295]); + dists[73] = res.first; + grads[219] = res.second[0]; + grads[220] = res.second[1]; + grads[221] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[296], y[297], y[298], y[299]); + dists[74] = res.first; + grads[222] = res.second[0]; + grads[223] = res.second[1]; + grads[224] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[300], y[301], y[302], y[303]); + dists[75] = res.first; + grads[225] = res.second[0]; + grads[226] = res.second[1]; + grads[227] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[304], y[305], y[306], y[307]); + dists[76] = res.first; + grads[228] = res.second[0]; + grads[229] = res.second[1]; + grads[230] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[308], y[309], y[310], y[311]); + dists[77] = res.first; + grads[231] = res.second[0]; + grads[232] = res.second[1]; + grads[233] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[312], y[313], y[314], y[315]); + dists[78] = res.first; + grads[234] = res.second[0]; + grads[235] = res.second[1]; + grads[236] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[316], y[317], y[318], y[319]); + dists[79] = res.first; + grads[237] = res.second[0]; + grads[238] = res.second[1]; + grads[239] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[320], y[321], y[322], y[323]); + dists[80] = res.first; + grads[240] = res.second[0]; + grads[241] = res.second[1]; + grads[242] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[324], y[325], y[326], y[327]); + dists[81] = res.first; + grads[243] = res.second[0]; + grads[244] = res.second[1]; + grads[245] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[328], y[329], y[330], y[331]); + dists[82] = res.first; + grads[246] = res.second[0]; + grads[247] = res.second[1]; + grads[248] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[332], y[333], y[334], y[335]); + dists[83] = res.first; + grads[249] = res.second[0]; + grads[250] = res.second[1]; + grads[251] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[336], y[337], y[338], y[339]); + dists[84] = res.first; + grads[252] = res.second[0]; + grads[253] = res.second[1]; + grads[254] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[340], y[341], y[342], y[343]); + dists[85] = res.first; + grads[255] = res.second[0]; + grads[256] = res.second[1]; + grads[257] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[344], y[345], y[346], y[347]); + dists[86] = res.first; + grads[258] = res.second[0]; + grads[259] = res.second[1]; + grads[260] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[348], y[349], y[350], y[351]); + dists[87] = res.first; + grads[261] = res.second[0]; + grads[262] = res.second[1]; + grads[263] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[352], y[353], y[354], y[355]); + dists[88] = res.first; + grads[264] = res.second[0]; + grads[265] = res.second[1]; + grads[266] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[356], y[357], y[358], y[359]); + dists[89] = res.first; + grads[267] = res.second[0]; + grads[268] = res.second[1]; + grads[269] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[360], y[361], y[362], y[363]); + dists[90] = res.first; + grads[270] = res.second[0]; + grads[271] = res.second[1]; + grads[272] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[364], y[365], y[366], y[367]); + dists[91] = res.first; + grads[273] = res.second[0]; + grads[274] = res.second[1]; + grads[275] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[368], y[369], y[370], y[371]); + dists[92] = res.first; + grads[276] = res.second[0]; + grads[277] = res.second[1]; + grads[278] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[372], y[373], y[374], y[375]); + dists[93] = res.first; + grads[279] = res.second[0]; + grads[280] = res.second[1]; + grads[281] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[376], y[377], y[378], y[379]); + dists[94] = res.first; + grads[282] = res.second[0]; + grads[283] = res.second[1]; + grads[284] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[380], y[381], y[382], y[383]); + dists[95] = res.first; + grads[285] = res.second[0]; + grads[286] = res.second[1]; + grads[287] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[384], y[385], y[386], y[387]); + dists[96] = res.first; + grads[288] = res.second[0]; + grads[289] = res.second[1]; + grads[290] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[388], y[389], y[390], y[391]); + dists[97] = res.first; + grads[291] = res.second[0]; + grads[292] = res.second[1]; + grads[293] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[392], y[393], y[394], y[395]); + dists[98] = res.first; + grads[294] = res.second[0]; + grads[295] = res.second[1]; + grads[296] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[396], y[397], y[398], y[399]); + dists[99] = res.first; + grads[297] = res.second[0]; + grads[298] = res.second[1]; + grads[299] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[400], y[401], y[402], y[403]); + dists[100] = res.first; + grads[300] = res.second[0]; + grads[301] = res.second[1]; + grads[302] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[404], y[405], y[406], y[407]); + dists[101] = res.first; + grads[303] = res.second[0]; + grads[304] = res.second[1]; + grads[305] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[408], y[409], y[410], y[411]); + dists[102] = res.first; + grads[306] = res.second[0]; + grads[307] = res.second[1]; + grads[308] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[412], y[413], y[414], y[415]); + dists[103] = res.first; + grads[309] = res.second[0]; + grads[310] = res.second[1]; + grads[311] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[416], y[417], y[418], y[419]); + dists[104] = res.first; + grads[312] = res.second[0]; + grads[313] = res.second[1]; + grads[314] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[420], y[421], y[422], y[423]); + dists[105] = res.first; + grads[315] = res.second[0]; + grads[316] = res.second[1]; + grads[317] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[424], y[425], y[426], y[427]); + dists[106] = res.first; + grads[318] = res.second[0]; + grads[319] = res.second[1]; + grads[320] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[428], y[429], y[430], y[431]); + dists[107] = res.first; + grads[321] = res.second[0]; + grads[322] = res.second[1]; + grads[323] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[432], y[433], y[434], y[435]); + dists[108] = res.first; + grads[324] = res.second[0]; + grads[325] = res.second[1]; + grads[326] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[436], y[437], y[438], y[439]); + dists[109] = res.first; + grads[327] = res.second[0]; + grads[328] = res.second[1]; + grads[329] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[440], y[441], y[442], y[443]); + dists[110] = res.first; + grads[330] = res.second[0]; + grads[331] = res.second[1]; + grads[332] = res.second[2]; + } + + return {dists, grads}; + } + + template + inline static void d_collision_d_q( + const ConfigurationBlock &q_in, + const FloatVector &gradients, + ConfigurationBlock &out) noexcept + { + // Generated code expects a single input array 'x' containing + // [q_0, ..., q_n, gx_0, gy_0, gz_0, ..., gx_m, gy_m, gz_m] + std::array, 8 + 111 * 3 + 15 * 3> x; + + // Copy q + for (std::size_t i = 0; i < 8; ++i) + { + x[i] = q_in[i]; + } + + // Copy gradients + for (std::size_t i = 0; i < 111 * 3; ++i) + { + x[8 + i] = gradients[i]; + } + + std::array, 214> v; + std::array, 8> y; + + y[0] = x[70] + x[73] + x[76] + x[79] + x[82] + x[85] + x[88] + x[91] + x[94] + x[97] + x[100] + + x[103] + x[106] + x[109] + x[112] + x[115] + x[118] + x[121] + x[124] + x[127] + x[130] + + x[133] + x[136] + x[139] + x[142] + x[145] + x[148] + x[151] + x[154] + x[157] + x[160] + + x[163] + x[166] + x[169] + x[172] + x[175] + x[178] + x[181] + x[184] + x[187] + x[190] + + x[193] + x[196] + x[199] + x[202] + x[205] + x[208] + x[211] + x[214] + x[217] + x[220] + + x[223] + x[226] + x[229] + x[232] + x[235] + x[238] + x[241] + x[244] + x[247] + x[250] + + x[253] + x[256] + x[259] + x[262] + x[265] + x[268] + x[271] + x[274] + x[277] + x[280] + + x[283] + x[286] + x[289] + x[292] + x[295] + x[298] + x[301] + x[304] + x[307] + x[310] + + x[313] + x[316] + x[319] + x[322] + x[325] + x[328] + x[331] + x[334] + x[337] + x[340] + + x[349] + x[352] + x[355] + x[358] + x[361] + x[364] + x[367] + x[370] + x[373] + x[376] + + x[379] + x[382] + x[385]; + v[0] = cos(x[1]); + v[1] = sin(x[1]); + v[2] = -v[1]; + v[3] = 0.117 * v[1]; + v[4] = 0.03265 + 0.117 * v[0]; + v[5] = cos(x[2]); + v[6] = v[0] * v[5]; + v[7] = sin(x[2]); + v[8] = v[0] * v[7]; + v[9] = 0.025 * v[6] + 0.04 * v[2] + 0.025 * v[8]; + v[10] = v[1] * v[5]; + v[11] = v[1] * v[7]; + v[12] = 0.025 * v[10] + 0.04 * v[0] + 0.025 * v[11]; + v[13] = -0.025 * v[6] + 0.04 * v[2] + -0.025 * v[8]; + v[14] = -0.025 * v[10] + 0.04 * v[0] + -0.025 * v[11]; + v[15] = 0.025 * v[6] + 0.04 * v[2] + -0.025 * v[8]; + v[16] = 0.025 * v[10] + 0.04 * v[0] + -0.025 * v[11]; + v[17] = -0.025 * v[6] + 0.04 * v[2] + 0.025 * v[8]; + v[18] = -0.025 * v[10] + 0.04 * v[0] + 0.025 * v[11]; + v[19] = 0.08 * v[6]; + v[20] = 0.08 * v[10]; + v[21] = 0.11 * v[6]; + v[22] = 0.11 * v[10]; + v[23] = 0.14 * v[6]; + v[24] = 0.14 * v[10]; + v[25] = v[3] + 0.219 * v[10]; + v[26] = v[4] + 0.219 * v[6]; + v[27] = -0.02 * v[6]; + v[28] = -0.02 * v[10]; + v[29] = v[27] * x[204] - v[28] * x[203]; + v[30] = 0.03 * v[6]; + v[31] = 0.03 * v[10]; + v[32] = v[30] * x[207] - v[31] * x[206]; + v[33] = 0.08 * v[6]; + v[34] = 0.08 * v[10]; + v[35] = v[33] * x[210] - v[34] * x[209]; + v[36] = cos(x[3]); + v[37] = sin(x[3]); + v[38] = v[2] * v[36] + v[8] * v[37]; + v[39] = -v[37]; + v[40] = v[2] * v[39] + v[8] * v[36]; + v[41] = 0.11 * v[6] + -0.045 * v[38] + 0.02 * v[40]; + v[42] = v[0] * v[36] + v[11] * v[37]; + v[39] = v[0] * v[39] + v[11] * v[36]; + v[43] = 0.11 * v[10] + -0.045 * v[42] + 0.02 * v[39]; + v[44] = v[41] * x[213] - v[43] * x[212]; + v[45] = 0.11 * v[6] + -0.045 * v[38] + -0.02 * v[40]; + v[46] = 0.11 * v[10] + -0.045 * v[42] + -0.02 * v[39]; + v[47] = v[45] * x[216] - v[46] * x[215]; + v[48] = 0.155 * v[6] + -0.045 * v[38] + 0.02 * v[40]; + v[49] = 0.155 * v[10] + -0.045 * v[42] + 0.02 * v[39]; + v[50] = v[48] * x[219] - v[49] * x[218]; + v[51] = 0.155 * v[6] + -0.045 * v[38] + -0.02 * v[40]; + v[52] = 0.155 * v[10] + -0.045 * v[42] + -0.02 * v[39]; + v[53] = v[51] * x[222] - v[52] * x[221]; + v[54] = 0.13 * v[6]; + v[55] = 0.13 * v[10]; + v[56] = v[54] * x[225] - v[55] * x[224]; + v[57] = v[25] + 0.133 * v[10]; + v[58] = v[26] + 0.133 * v[6]; + v[59] = cos(x[4]); + v[60] = sin(x[4]); + v[61] = -v[60]; + v[62] = v[6] * v[59] + v[40] * v[61]; + v[63] = v[6] * v[60] + v[40] * v[59]; + v[64] = 0.02 * v[62] + 0.045 * v[38] + 0.02 * v[63]; + v[65] = v[10] * v[59] + v[39] * v[61]; + v[66] = v[10] * v[60] + v[39] * v[59]; + v[67] = 0.02 * v[65] + 0.045 * v[42] + 0.02 * v[66]; + v[68] = v[64] * x[228] - v[67] * x[227]; + v[69] = 0.02 * v[62] + 0.045 * v[38] + -0.02 * v[63]; + v[70] = 0.02 * v[65] + 0.045 * v[42] + -0.02 * v[66]; + v[71] = v[69] * x[231] - v[70] * x[230]; + v[72] = -0.02 * v[62] + 0.045 * v[38] + 0.02 * v[63]; + v[73] = -0.02 * v[65] + 0.045 * v[42] + 0.02 * v[66]; + v[74] = v[72] * x[234] - v[73] * x[233]; + v[75] = -0.02 * v[62] + 0.045 * v[38] + -0.02 * v[63]; + v[76] = -0.02 * v[65] + 0.045 * v[42] + -0.02 * v[66]; + v[77] = v[75] * x[237] - v[76] * x[236]; + v[78] = 0.08 * v[62]; + v[79] = 0.08 * v[65]; + v[80] = v[78] * x[240] - v[79] * x[239]; + v[81] = 0.14 * v[62]; + v[82] = 0.14 * v[65]; + v[83] = v[81] * x[243] - v[82] * x[242]; + v[84] = v[57] + 0.197 * v[65]; + v[85] = v[58] + 0.197 * v[62]; + v[86] = cos(x[5]); + v[87] = sin(x[5]); + v[88] = v[38] * v[86] + v[63] * v[87]; + v[89] = -v[87]; + v[90] = v[38] * v[89] + v[63] * v[86]; + v[91] = 0.05 * v[62] + -0.06 * v[88] + 0.02 * v[90]; + v[92] = v[42] * v[86] + v[66] * v[87]; + v[93] = v[42] * v[89] + v[66] * v[86]; + v[94] = 0.05 * v[65] + -0.06 * v[92] + 0.02 * v[93]; + v[95] = v[91] * x[249] - v[94] * x[248]; + v[96] = 0.05 * v[62] + -0.06 * v[88] + -0.02 * v[90]; + v[97] = 0.05 * v[65] + -0.06 * v[92] + -0.02 * v[93]; + v[98] = v[96] * x[252] - v[97] * x[251]; + v[99] = 0.1 * v[62] + -0.06 * v[88] + 0.02 * v[90]; + v[100] = 0.1 * v[65] + -0.06 * v[92] + 0.02 * v[93]; + v[101] = v[99] * x[255] - v[100] * x[254]; + v[102] = 0.1 * v[62] + -0.06 * v[88] + -0.02 * v[90]; + v[103] = 0.1 * v[65] + -0.06 * v[92] + -0.02 * v[93]; + v[104] = v[102] * x[258] - v[103] * x[257]; + v[105] = 0.15 * v[62] + -0.06 * v[88] + 0.02 * v[90]; + v[106] = 0.15 * v[65] + -0.06 * v[92] + 0.02 * v[93]; + v[107] = v[105] * x[261] - v[106] * x[260]; + v[108] = 0.15 * v[62] + -0.06 * v[88] + -0.02 * v[90]; + v[109] = 0.15 * v[65] + -0.06 * v[92] + -0.02 * v[93]; + v[110] = v[108] * x[264] - v[109] * x[263]; + v[111] = v[84] + 0.1245 * v[65]; + v[112] = v[85] + 0.1245 * v[62]; + v[113] = cos(x[6]); + v[114] = sin(x[6]); + v[115] = -v[114]; + v[116] = v[62] * v[113] + v[90] * v[115]; + v[117] = 0.06 * v[116]; + v[118] = v[65] * v[113] + v[93] * v[115]; + v[119] = 0.06 * v[118]; + v[120] = v[117] * x[270] - v[119] * x[269]; + v[121] = v[62] * v[114] + v[90] * v[113]; + v[122] = 0.02 * v[116] + 0.045 * v[88] + 0.02 * v[121]; + v[123] = v[65] * v[114] + v[93] * v[113]; + v[124] = 0.02 * v[118] + 0.045 * v[92] + 0.02 * v[123]; + v[125] = v[122] * x[273] - v[124] * x[272]; + v[126] = 0.02 * v[116] + 0.045 * v[88] + -0.02 * v[121]; + v[127] = 0.02 * v[118] + 0.045 * v[92] + -0.02 * v[123]; + v[128] = v[126] * x[276] - v[127] * x[275]; + v[129] = -0.02 * v[116] + 0.045 * v[88] + 0.02 * v[121]; + v[130] = -0.02 * v[118] + 0.045 * v[92] + 0.02 * v[123]; + v[131] = v[129] * x[279] - v[130] * x[278]; + v[132] = -0.02 * v[116] + 0.045 * v[88] + -0.02 * v[121]; + v[133] = -0.02 * v[118] + 0.045 * v[92] + -0.02 * v[123]; + v[134] = v[132] * x[282] - v[133] * x[281]; + v[135] = v[111] + 0.1385 * v[118]; + v[136] = v[112] + 0.1385 * v[116]; + v[137] = -0.03 * v[116]; + v[138] = -0.03 * v[118]; + v[139] = v[137] * x[285] - v[138] * x[284]; + v[140] = cos(x[7]); + v[141] = sin(x[7]); + v[142] = v[88] * v[140] + v[121] * v[141]; + v[143] = 0.09645 * v[116] + 0.02 * v[142]; + v[144] = v[92] * v[140] + v[123] * v[141]; + v[145] = 0.09645 * v[118] + 0.02 * v[144]; + v[146] = v[143] * x[291] - v[145] * x[290]; + v[147] = 0.09645 * v[116] + -0.02 * v[142]; + v[148] = 0.09645 * v[118] + -0.02 * v[144]; + v[149] = v[147] * x[294] - v[148] * x[293]; + v[150] = 0.06645 * v[116] + 0.02 * v[142]; + v[151] = 0.06645 * v[118] + 0.02 * v[144]; + v[152] = v[150] * x[297] - v[151] * x[296]; + v[153] = 0.06645 * v[116] + -0.02 * v[142]; + v[154] = 0.06645 * v[118] + -0.02 * v[144]; + v[155] = v[153] * x[300] - v[154] * x[299]; + v[156] = -v[141]; + v[157] = v[88] * v[156] + v[121] * v[140]; + v[158] = 0.18345 * v[116] + -0.056925 * v[142] + -0.005 * v[157]; + v[159] = v[92] * v[156] + v[123] * v[140]; + v[160] = 0.18345 * v[118] + -0.056925 * v[144] + -0.005 * v[159]; + v[161] = v[158] * x[303] - v[160] * x[302]; + v[162] = 0.18345 * v[116] + -0.056925 * v[142] + 0.005 * v[157]; + v[163] = 0.18345 * v[118] + -0.056925 * v[144] + 0.005 * v[159]; + v[164] = v[162] * x[306] - v[163] * x[305]; + v[165] = 0.16645 * v[116] + -0.056925 * v[142] + -0.005 * v[157]; + v[166] = 0.16645 * v[118] + -0.056925 * v[144] + -0.005 * v[159]; + v[167] = v[165] * x[309] - v[166] * x[308]; + v[168] = 0.16645 * v[116] + -0.056925 * v[142] + 0.005 * v[157]; + v[169] = 0.16645 * v[118] + -0.056925 * v[144] + 0.005 * v[159]; + v[170] = v[168] * x[312] - v[169] * x[311]; + v[171] = 0.14945 * v[116] + -0.056925 * v[142] + -0.005 * v[157]; + v[172] = 0.14945 * v[118] + -0.056925 * v[144] + -0.005 * v[159]; + v[173] = v[171] * x[315] - v[172] * x[314]; + v[174] = 0.14945 * v[116] + -0.056925 * v[142] + 0.005 * v[157]; + v[175] = 0.14945 * v[118] + -0.056925 * v[144] + 0.005 * v[159]; + v[176] = v[174] * x[318] - v[175] * x[317]; + v[177] = 0.18345 * v[116] + 0.056925 * v[142] + -0.005 * v[157]; + v[178] = 0.18345 * v[118] + 0.056925 * v[144] + -0.005 * v[159]; + v[179] = v[177] * x[321] - v[178] * x[320]; + v[180] = 0.18345 * v[116] + 0.056925 * v[142] + 0.005 * v[157]; + v[181] = 0.18345 * v[118] + 0.056925 * v[144] + 0.005 * v[159]; + v[182] = v[180] * x[324] - v[181] * x[323]; + v[183] = 0.16645 * v[116] + 0.056925 * v[142] + -0.005 * v[157]; + v[184] = 0.16645 * v[118] + 0.056925 * v[144] + -0.005 * v[159]; + v[185] = v[183] * x[327] - v[184] * x[326]; + v[186] = 0.16645 * v[116] + 0.056925 * v[142] + 0.005 * v[157]; + v[187] = 0.16645 * v[118] + 0.056925 * v[144] + 0.005 * v[159]; + v[188] = v[186] * x[330] - v[187] * x[329]; + v[189] = 0.14945 * v[116] + 0.056925 * v[142] + -0.005 * v[157]; + v[190] = 0.14945 * v[118] + 0.056925 * v[144] + -0.005 * v[159]; + v[191] = v[189] * x[333] - v[190] * x[332]; + v[157] = 0.14945 * v[116] + 0.056925 * v[142] + 0.005 * v[157]; + v[159] = 0.14945 * v[118] + 0.056925 * v[144] + 0.005 * v[159]; + v[192] = v[157] * x[336] - v[159] * x[335]; + v[8] = 0.0631452798843384 * v[6] + 0.018631448969245 * v[2] + 1.73472347597681e-18 * v[8]; + v[11] = 0.0631452798843384 * v[10] + 0.018631448969245 * v[0] + 1.73472347597681e-18 * v[11]; + v[40] = 0.056335523724556 * v[6] + -0.0196291357278824 * v[38] + -1.73472347597681e-18 * v[40]; + v[39] = 0.056335523724556 * v[10] + -0.0196291357278824 * v[42] + -1.73472347597681e-18 * v[39]; + v[10] = v[40] * x[360] - v[39] * x[359]; + v[63] = 0.0710262209177017 * v[62] + 0.0193988755345345 * v[38] + 1.73472347597681e-18 * v[63]; + v[66] = 0.0710262209177017 * v[65] + 0.0193988755345345 * v[42] + 1.73472347597681e-18 * v[66]; + v[42] = v[63] * x[363] - v[66] * x[362]; + v[90] = 0.0643894299864769 * v[62] + -0.0257557742297649 * v[88] + -1.73472347597681e-18 * v[90]; + v[93] = 0.0643894299864769 * v[65] + -0.0257557742297649 * v[92] + -1.73472347597681e-18 * v[93]; + v[65] = v[90] * x[366] - v[93] * x[365]; + v[121] = 0.0294021144509315 * v[116] + 0.0172113105654716 * v[88] + 4.33680868994202e-18 * v[121]; + v[123] = 0.0294021144509315 * v[118] + 0.0172113105654716 * v[92] + 4.33680868994202e-18 * v[123]; + v[92] = v[121] * x[369] - v[123] * x[368]; + v[88] = -0.0149999996647239 * v[116]; + v[62] = -0.0149999996647239 * v[118]; + v[38] = v[88] * x[372] - v[62] * x[371]; + v[6] = 0.0814500004053116 * v[116]; + v[193] = 0.0814500004053116 * v[118]; + v[194] = v[6] * x[375] - v[193] * x[374]; + v[195] = 0.166449993848801 * v[116] + -0.0569249987602234 * v[142]; + v[196] = 0.166449993848801 * v[118] + -0.0569249987602234 * v[144]; + v[197] = v[195] * x[378] - v[196] * x[377]; + v[142] = 0.166449993848801 * v[116] + 0.0569249987602234 * v[142]; + v[144] = 0.166449993848801 * v[118] + 0.0569249987602234 * v[144]; + v[118] = v[142] * x[381] - v[144] * x[380]; + y[1] = + (0.025 * v[0] + -0.015 * v[2]) * x[174] - (0.025 * v[1] + -0.015 * v[0]) * x[173] + + (0.05 * v[0] + -0.03 * v[2]) * x[177] - (0.05 * v[1] + -0.03 * v[0]) * x[176] + + (0.12 * v[0] + -0.03 * v[2]) * x[180] - (0.12 * v[1] + -0.03 * v[0]) * x[179] + + (0. - v[3]) * x[182] + (-0.03265 - (0. - v[4])) * x[183] + v[9] * x[183] - v[12] * x[182] + + (0. - v[3]) * x[185] + (-0.03265 - (0. - v[4])) * x[186] + v[13] * x[186] - v[14] * x[185] + + (0. - v[3]) * x[188] + (-0.03265 - (0. - v[4])) * x[189] + v[15] * x[189] - v[16] * x[188] + + (0. - v[3]) * x[191] + (-0.03265 - (0. - v[4])) * x[192] + v[17] * x[192] - v[18] * x[191] + + (0. - v[3]) * x[194] + (-0.03265 - (0. - v[4])) * x[195] + v[19] * x[195] - v[20] * x[194] + + (0. - v[3]) * x[197] + (-0.03265 - (0. - v[4])) * x[198] + v[21] * x[198] - v[22] * x[197] + + (0. - v[3]) * x[200] + (-0.03265 - (0. - v[4])) * x[201] + v[23] * x[201] - v[24] * x[200] + + (0. - v[25]) * x[203] + (-0.03265 - (0. - v[26])) * x[204] + v[29] + (0. - v[25]) * x[206] + + (-0.03265 - (0. - v[26])) * x[207] + v[32] + (0. - v[25]) * x[209] + + (-0.03265 - (0. - v[26])) * x[210] + v[35] + (0. - v[25]) * x[212] + + (-0.03265 - (0. - v[26])) * x[213] + v[44] + (0. - v[25]) * x[215] + + (-0.03265 - (0. - v[26])) * x[216] + v[47] + (0. - v[25]) * x[218] + + (-0.03265 - (0. - v[26])) * x[219] + v[50] + (0. - v[25]) * x[221] + + (-0.03265 - (0. - v[26])) * x[222] + v[53] + (0. - v[25]) * x[224] + + (-0.03265 - (0. - v[26])) * x[225] + v[56] + (0. - v[57]) * x[227] + + (-0.03265 - (0. - v[58])) * x[228] + v[68] + (0. - v[57]) * x[230] + + (-0.03265 - (0. - v[58])) * x[231] + v[71] + (0. - v[57]) * x[233] + + (-0.03265 - (0. - v[58])) * x[234] + v[74] + (0. - v[57]) * x[236] + + (-0.03265 - (0. - v[58])) * x[237] + v[77] + (0. - v[57]) * x[239] + + (-0.03265 - (0. - v[58])) * x[240] + v[80] + (0. - v[57]) * x[242] + + (-0.03265 - (0. - v[58])) * x[243] + v[83] + (0. - v[84]) * x[245] + + (-0.03265 - (0. - v[85])) * x[246] + (0. - v[84]) * x[248] + + (-0.03265 - (0. - v[85])) * x[249] + v[95] + (0. - v[84]) * x[251] + + (-0.03265 - (0. - v[85])) * x[252] + v[98] + (0. - v[84]) * x[254] + + (-0.03265 - (0. - v[85])) * x[255] + v[101] + (0. - v[84]) * x[257] + + (-0.03265 - (0. - v[85])) * x[258] + v[104] + (0. - v[84]) * x[260] + + (-0.03265 - (0. - v[85])) * x[261] + v[107] + (0. - v[84]) * x[263] + + (-0.03265 - (0. - v[85])) * x[264] + v[110] + (0. - v[111]) * x[266] + + (-0.03265 - (0. - v[112])) * x[267] + (0. - v[111]) * x[269] + + (-0.03265 - (0. - v[112])) * x[270] + v[120] + (0. - v[111]) * x[272] + + (-0.03265 - (0. - v[112])) * x[273] + v[125] + (0. - v[111]) * x[275] + + (-0.03265 - (0. - v[112])) * x[276] + v[128] + (0. - v[111]) * x[278] + + (-0.03265 - (0. - v[112])) * x[279] + v[131] + (0. - v[111]) * x[281] + + (-0.03265 - (0. - v[112])) * x[282] + v[134] + (0. - v[135]) * x[284] + + (-0.03265 - (0. - v[136])) * x[285] + v[139] + (0. - v[135]) * x[287] + + (-0.03265 - (0. - v[136])) * x[288] + (0. - v[135]) * x[290] + + (-0.03265 - (0. - v[136])) * x[291] + v[146] + (0. - v[135]) * x[293] + + (-0.03265 - (0. - v[136])) * x[294] + v[149] + (0. - v[135]) * x[296] + + (-0.03265 - (0. - v[136])) * x[297] + v[152] + (0. - v[135]) * x[299] + + (-0.03265 - (0. - v[136])) * x[300] + v[155] + (0. - v[135]) * x[302] + + (-0.03265 - (0. - v[136])) * x[303] + v[161] + (0. - v[135]) * x[305] + + (-0.03265 - (0. - v[136])) * x[306] + v[164] + (0. - v[135]) * x[308] + + (-0.03265 - (0. - v[136])) * x[309] + v[167] + (0. - v[135]) * x[311] + + (-0.03265 - (0. - v[136])) * x[312] + v[170] + (0. - v[135]) * x[314] + + (-0.03265 - (0. - v[136])) * x[315] + v[173] + (0. - v[135]) * x[317] + + (-0.03265 - (0. - v[136])) * x[318] + v[176] + (0. - v[135]) * x[320] + + (-0.03265 - (0. - v[136])) * x[321] + v[179] + (0. - v[135]) * x[323] + + (-0.03265 - (0. - v[136])) * x[324] + v[182] + (0. - v[135]) * x[326] + + (-0.03265 - (0. - v[136])) * x[327] + v[185] + (0. - v[135]) * x[329] + + (-0.03265 - (0. - v[136])) * x[330] + v[188] + (0. - v[135]) * x[332] + + (-0.03265 - (0. - v[136])) * x[333] + v[191] + (0. - v[135]) * x[335] + + (-0.03265 - (0. - v[136])) * x[336] + v[192] + + (0.0599999986588955 * v[0] + -0.0149999996647239 * v[2]) * x[354] - + (0.0599999986588955 * v[1] + -0.0149999996647239 * v[0]) * x[353] + (0. - v[3]) * x[356] + + (-0.03265 - (0. - v[4])) * x[357] + v[8] * x[357] - v[11] * x[356] + (0. - v[25]) * x[359] + + (-0.03265 - (0. - v[26])) * x[360] + v[10] + (0. - v[57]) * x[362] + + (-0.03265 - (0. - v[58])) * x[363] + v[42] + (0. - v[84]) * x[365] + + (-0.03265 - (0. - v[85])) * x[366] + v[65] + (0. - v[111]) * x[368] + + (-0.03265 - (0. - v[112])) * x[369] + v[92] + (0. - v[135]) * x[371] + + (-0.03265 - (0. - v[136])) * x[372] + v[38] + (0. - v[135]) * x[374] + + (-0.03265 - (0. - v[136])) * x[375] + v[194] + (0. - v[135]) * x[377] + + (-0.03265 - (0. - v[136])) * x[378] + v[197] + (0. - v[135]) * x[380] + + (-0.03265 - (0. - v[136])) * x[381] + v[118]; + v[2] = 0.0599999999999999 + 0.34858 + 0.37743 + x[0]; + v[1] = cos(x[1]); + v[0] = 0. - v[2] * v[1]; + v[116] = 0.0599999999999999 + 0.34858 + 0.37743 + x[0]; + v[198] = sin(x[1]); + v[199] = -v[198]; + v[200] = v[2] * v[199]; + v[201] = 0.03265 + 0.117 * v[1]; + v[202] = 0.117 * v[198]; + v[203] = v[201] * v[1] - v[202] * v[199]; + v[7] = -v[7]; + v[204] = 0.025 * v[7] + 0.025 * v[5]; + v[205] = -0.025 * v[7] + -0.025 * v[5]; + v[206] = 0.025 * v[7] + -0.025 * v[5]; + v[207] = -0.025 * v[7] + 0.025 * v[5]; + v[208] = 0.08 * v[7]; + v[209] = 0.11 * v[7]; + v[210] = 0.14 * v[7]; + v[211] = v[116] + 0.219 * v[7]; + v[212] = -0.02 * v[7]; + v[28] = v[28] * x[205] - v[212] * x[204]; + v[212] = v[212] * x[203] - v[27] * x[205]; + v[27] = 0.03 * v[7]; + v[31] = v[31] * x[208] - v[27] * x[207]; + v[27] = v[27] * x[206] - v[30] * x[208]; + v[30] = 0.08 * v[7]; + v[34] = v[34] * x[211] - v[30] * x[210]; + v[30] = v[30] * x[209] - v[33] * x[211]; + v[37] = v[5] * v[37]; + v[36] = v[5] * v[36]; + v[33] = 0.11 * v[7] + -0.045 * v[37] + 0.02 * v[36]; + v[43] = v[43] * x[214] - v[33] * x[213]; + v[33] = v[33] * x[212] - v[41] * x[214]; + v[41] = 0.11 * v[7] + -0.045 * v[37] + -0.02 * v[36]; + v[46] = v[46] * x[217] - v[41] * x[216]; + v[41] = v[41] * x[215] - v[45] * x[217]; + v[45] = 0.155 * v[7] + -0.045 * v[37] + 0.02 * v[36]; + v[49] = v[49] * x[220] - v[45] * x[219]; + v[45] = v[45] * x[218] - v[48] * x[220]; + v[48] = 0.155 * v[7] + -0.045 * v[37] + -0.02 * v[36]; + v[52] = v[52] * x[223] - v[48] * x[222]; + v[48] = v[48] * x[221] - v[51] * x[223]; + v[51] = 0.13 * v[7]; + v[55] = v[55] * x[226] - v[51] * x[225]; + v[51] = v[51] * x[224] - v[54] * x[226]; + v[54] = v[211] + 0.133 * v[7]; + v[61] = v[7] * v[59] + v[36] * v[61]; + v[60] = v[7] * v[60] + v[36] * v[59]; + v[59] = 0.02 * v[61] + 0.045 * v[37] + 0.02 * v[60]; + v[67] = v[67] * x[229] - v[59] * x[228]; + v[59] = v[59] * x[227] - v[64] * x[229]; + v[64] = 0.02 * v[61] + 0.045 * v[37] + -0.02 * v[60]; + v[70] = v[70] * x[232] - v[64] * x[231]; + v[64] = v[64] * x[230] - v[69] * x[232]; + v[69] = -0.02 * v[61] + 0.045 * v[37] + 0.02 * v[60]; + v[73] = v[73] * x[235] - v[69] * x[234]; + v[69] = v[69] * x[233] - v[72] * x[235]; + v[72] = -0.02 * v[61] + 0.045 * v[37] + -0.02 * v[60]; + v[76] = v[76] * x[238] - v[72] * x[237]; + v[72] = v[72] * x[236] - v[75] * x[238]; + v[75] = 0.08 * v[61]; + v[79] = v[79] * x[241] - v[75] * x[240]; + v[75] = v[75] * x[239] - v[78] * x[241]; + v[78] = 0.14 * v[61]; + v[82] = v[82] * x[244] - v[78] * x[243]; + v[78] = v[78] * x[242] - v[81] * x[244]; + v[81] = v[54] + 0.197 * v[61]; + v[87] = v[37] * v[86] + v[60] * v[87]; + v[89] = v[37] * v[89] + v[60] * v[86]; + v[86] = 0.05 * v[61] + -0.06 * v[87] + 0.02 * v[89]; + v[94] = v[94] * x[250] - v[86] * x[249]; + v[86] = v[86] * x[248] - v[91] * x[250]; + v[91] = 0.05 * v[61] + -0.06 * v[87] + -0.02 * v[89]; + v[97] = v[97] * x[253] - v[91] * x[252]; + v[91] = v[91] * x[251] - v[96] * x[253]; + v[96] = 0.1 * v[61] + -0.06 * v[87] + 0.02 * v[89]; + v[100] = v[100] * x[256] - v[96] * x[255]; + v[96] = v[96] * x[254] - v[99] * x[256]; + v[99] = 0.1 * v[61] + -0.06 * v[87] + -0.02 * v[89]; + v[103] = v[103] * x[259] - v[99] * x[258]; + v[99] = v[99] * x[257] - v[102] * x[259]; + v[102] = 0.15 * v[61] + -0.06 * v[87] + 0.02 * v[89]; + v[106] = v[106] * x[262] - v[102] * x[261]; + v[102] = v[102] * x[260] - v[105] * x[262]; + v[105] = 0.15 * v[61] + -0.06 * v[87] + -0.02 * v[89]; + v[109] = v[109] * x[265] - v[105] * x[264]; + v[105] = v[105] * x[263] - v[108] * x[265]; + v[108] = v[81] + 0.1245 * v[61]; + v[115] = v[61] * v[113] + v[89] * v[115]; + v[213] = 0.06 * v[115]; + v[119] = v[119] * x[271] - v[213] * x[270]; + v[213] = v[213] * x[269] - v[117] * x[271]; + v[114] = v[61] * v[114] + v[89] * v[113]; + v[113] = 0.02 * v[115] + 0.045 * v[87] + 0.02 * v[114]; + v[124] = v[124] * x[274] - v[113] * x[273]; + v[113] = v[113] * x[272] - v[122] * x[274]; + v[122] = 0.02 * v[115] + 0.045 * v[87] + -0.02 * v[114]; + v[127] = v[127] * x[277] - v[122] * x[276]; + v[122] = v[122] * x[275] - v[126] * x[277]; + v[126] = -0.02 * v[115] + 0.045 * v[87] + 0.02 * v[114]; + v[130] = v[130] * x[280] - v[126] * x[279]; + v[126] = v[126] * x[278] - v[129] * x[280]; + v[129] = -0.02 * v[115] + 0.045 * v[87] + -0.02 * v[114]; + v[133] = v[133] * x[283] - v[129] * x[282]; + v[129] = v[129] * x[281] - v[132] * x[283]; + v[132] = v[108] + 0.1385 * v[115]; + v[117] = -0.03 * v[115]; + v[138] = v[138] * x[286] - v[117] * x[285]; + v[117] = v[117] * x[284] - v[137] * x[286]; + v[141] = v[87] * v[140] + v[114] * v[141]; + v[137] = 0.09645 * v[115] + 0.02 * v[141]; + v[145] = v[145] * x[292] - v[137] * x[291]; + v[137] = v[137] * x[290] - v[143] * x[292]; + v[143] = 0.09645 * v[115] + -0.02 * v[141]; + v[148] = v[148] * x[295] - v[143] * x[294]; + v[143] = v[143] * x[293] - v[147] * x[295]; + v[147] = 0.06645 * v[115] + 0.02 * v[141]; + v[151] = v[151] * x[298] - v[147] * x[297]; + v[147] = v[147] * x[296] - v[150] * x[298]; + v[150] = 0.06645 * v[115] + -0.02 * v[141]; + v[154] = v[154] * x[301] - v[150] * x[300]; + v[150] = v[150] * x[299] - v[153] * x[301]; + v[156] = v[87] * v[156] + v[114] * v[140]; + v[140] = 0.18345 * v[115] + -0.056925 * v[141] + -0.005 * v[156]; + v[160] = v[160] * x[304] - v[140] * x[303]; + v[140] = v[140] * x[302] - v[158] * x[304]; + v[158] = 0.18345 * v[115] + -0.056925 * v[141] + 0.005 * v[156]; + v[163] = v[163] * x[307] - v[158] * x[306]; + v[158] = v[158] * x[305] - v[162] * x[307]; + v[162] = 0.16645 * v[115] + -0.056925 * v[141] + -0.005 * v[156]; + v[166] = v[166] * x[310] - v[162] * x[309]; + v[162] = v[162] * x[308] - v[165] * x[310]; + v[165] = 0.16645 * v[115] + -0.056925 * v[141] + 0.005 * v[156]; + v[169] = v[169] * x[313] - v[165] * x[312]; + v[165] = v[165] * x[311] - v[168] * x[313]; + v[168] = 0.14945 * v[115] + -0.056925 * v[141] + -0.005 * v[156]; + v[172] = v[172] * x[316] - v[168] * x[315]; + v[168] = v[168] * x[314] - v[171] * x[316]; + v[171] = 0.14945 * v[115] + -0.056925 * v[141] + 0.005 * v[156]; + v[175] = v[175] * x[319] - v[171] * x[318]; + v[171] = v[171] * x[317] - v[174] * x[319]; + v[174] = 0.18345 * v[115] + 0.056925 * v[141] + -0.005 * v[156]; + v[178] = v[178] * x[322] - v[174] * x[321]; + v[174] = v[174] * x[320] - v[177] * x[322]; + v[177] = 0.18345 * v[115] + 0.056925 * v[141] + 0.005 * v[156]; + v[181] = v[181] * x[325] - v[177] * x[324]; + v[177] = v[177] * x[323] - v[180] * x[325]; + v[180] = 0.16645 * v[115] + 0.056925 * v[141] + -0.005 * v[156]; + v[184] = v[184] * x[328] - v[180] * x[327]; + v[180] = v[180] * x[326] - v[183] * x[328]; + v[183] = 0.16645 * v[115] + 0.056925 * v[141] + 0.005 * v[156]; + v[187] = v[187] * x[331] - v[183] * x[330]; + v[183] = v[183] * x[329] - v[186] * x[331]; + v[186] = 0.14945 * v[115] + 0.056925 * v[141] + -0.005 * v[156]; + v[190] = v[190] * x[334] - v[186] * x[333]; + v[186] = v[186] * x[332] - v[189] * x[334]; + v[156] = 0.14945 * v[115] + 0.056925 * v[141] + 0.005 * v[156]; + v[159] = v[159] * x[337] - v[156] * x[336]; + v[156] = v[156] * x[335] - v[157] * x[337]; + v[5] = 0.0631452798843384 * v[7] + 1.73472347597681e-18 * v[5]; + v[36] = 0.056335523724556 * v[7] + -0.0196291357278824 * v[37] + -1.73472347597681e-18 * v[36]; + v[39] = v[39] * x[361] - v[36] * x[360]; + v[36] = v[36] * x[359] - v[40] * x[361]; + v[60] = 0.0710262209177017 * v[61] + 0.0193988755345345 * v[37] + 1.73472347597681e-18 * v[60]; + v[66] = v[66] * x[364] - v[60] * x[363]; + v[60] = v[60] * x[362] - v[63] * x[364]; + v[89] = 0.0643894299864769 * v[61] + -0.0257557742297649 * v[87] + -1.73472347597681e-18 * v[89]; + v[93] = v[93] * x[367] - v[89] * x[366]; + v[89] = v[89] * x[365] - v[90] * x[367]; + v[114] = 0.0294021144509315 * v[115] + 0.0172113105654716 * v[87] + 4.33680868994202e-18 * v[114]; + v[123] = v[123] * x[370] - v[114] * x[369]; + v[114] = v[114] * x[368] - v[121] * x[370]; + v[121] = -0.0149999996647239 * v[115]; + v[62] = v[62] * x[373] - v[121] * x[372]; + v[121] = v[121] * x[371] - v[88] * x[373]; + v[88] = 0.0814500004053116 * v[115]; + v[193] = v[193] * x[376] - v[88] * x[375]; + v[88] = v[88] * x[374] - v[6] * x[376]; + v[6] = 0.166449993848801 * v[115] + -0.0569249987602234 * v[141]; + v[196] = v[196] * x[379] - v[6] * x[378]; + v[6] = v[6] * x[377] - v[195] * x[379]; + v[141] = 0.166449993848801 * v[115] + 0.0569249987602234 * v[141]; + v[144] = v[144] * x[382] - v[141] * x[381]; + v[141] = v[141] * x[380] - v[142] * x[382]; + y[2] = (v[0] - (0. - v[116] * v[1])) * x[182] + (v[200] - v[116] * v[199]) * x[183] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[184] + + v[199] * (v[12] * x[184] - v[204] * x[183]) + v[1] * (v[204] * x[182] - v[9] * x[184]) + + (v[0] - (0. - v[116] * v[1])) * x[185] + (v[200] - v[116] * v[199]) * x[186] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[187] + + v[199] * (v[14] * x[187] - v[205] * x[186]) + v[1] * (v[205] * x[185] - v[13] * x[187]) + + (v[0] - (0. - v[116] * v[1])) * x[188] + (v[200] - v[116] * v[199]) * x[189] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[190] + + v[199] * (v[16] * x[190] - v[206] * x[189]) + v[1] * (v[206] * x[188] - v[15] * x[190]) + + (v[0] - (0. - v[116] * v[1])) * x[191] + (v[200] - v[116] * v[199]) * x[192] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[193] + + v[199] * (v[18] * x[193] - v[207] * x[192]) + v[1] * (v[207] * x[191] - v[17] * x[193]) + + (v[0] - (0. - v[116] * v[1])) * x[194] + (v[200] - v[116] * v[199]) * x[195] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[196] + + v[199] * (v[20] * x[196] - v[208] * x[195]) + v[1] * (v[208] * x[194] - v[19] * x[196]) + + (v[0] - (0. - v[116] * v[1])) * x[197] + (v[200] - v[116] * v[199]) * x[198] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[199] + + v[199] * (v[22] * x[199] - v[209] * x[198]) + v[1] * (v[209] * x[197] - v[21] * x[199]) + + (v[0] - (0. - v[116] * v[1])) * x[200] + (v[200] - v[116] * v[199]) * x[201] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[202] + + v[199] * (v[24] * x[202] - v[210] * x[201]) + v[1] * (v[210] * x[200] - v[23] * x[202]) + + (v[0] - (0. - v[211] * v[1])) * x[203] + (v[200] - v[211] * v[199]) * x[204] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[205] + v[199] * v[28] + v[1] * v[212] + + (v[0] - (0. - v[211] * v[1])) * x[206] + (v[200] - v[211] * v[199]) * x[207] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[208] + v[199] * v[31] + v[1] * v[27] + + (v[0] - (0. - v[211] * v[1])) * x[209] + (v[200] - v[211] * v[199]) * x[210] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[211] + v[199] * v[34] + v[1] * v[30] + + (v[0] - (0. - v[211] * v[1])) * x[212] + (v[200] - v[211] * v[199]) * x[213] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[214] + v[199] * v[43] + v[1] * v[33] + + (v[0] - (0. - v[211] * v[1])) * x[215] + (v[200] - v[211] * v[199]) * x[216] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[217] + v[199] * v[46] + v[1] * v[41] + + (v[0] - (0. - v[211] * v[1])) * x[218] + (v[200] - v[211] * v[199]) * x[219] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[220] + v[199] * v[49] + v[1] * v[45] + + (v[0] - (0. - v[211] * v[1])) * x[221] + (v[200] - v[211] * v[199]) * x[222] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[223] + v[199] * v[52] + v[1] * v[48] + + (v[0] - (0. - v[211] * v[1])) * x[224] + (v[200] - v[211] * v[199]) * x[225] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[226] + v[199] * v[55] + v[1] * v[51] + + (v[0] - (0. - v[54] * v[1])) * x[227] + (v[200] - v[54] * v[199]) * x[228] + + (v[203] - (v[58] * v[1] - v[57] * v[199])) * x[229] + v[199] * v[67] + v[1] * v[59] + + (v[0] - (0. - v[54] * v[1])) * x[230] + (v[200] - v[54] * v[199]) * x[231] + + (v[203] - (v[58] * v[1] - v[57] * v[199])) * x[232] + v[199] * v[70] + v[1] * v[64] + + (v[0] - (0. - v[54] * v[1])) * x[233] + (v[200] - v[54] * v[199]) * x[234] + + (v[203] - (v[58] * v[1] - v[57] * v[199])) * x[235] + v[199] * v[73] + v[1] * v[69] + + (v[0] - (0. - v[54] * v[1])) * x[236] + (v[200] - v[54] * v[199]) * x[237] + + (v[203] - (v[58] * v[1] - v[57] * v[199])) * x[238] + v[199] * v[76] + v[1] * v[72] + + (v[0] - (0. - v[54] * v[1])) * x[239] + (v[200] - v[54] * v[199]) * x[240] + + (v[203] - (v[58] * v[1] - v[57] * v[199])) * x[241] + v[199] * v[79] + v[1] * v[75] + + (v[0] - (0. - v[54] * v[1])) * x[242] + (v[200] - v[54] * v[199]) * x[243] + + (v[203] - (v[58] * v[1] - v[57] * v[199])) * x[244] + v[199] * v[82] + v[1] * v[78] + + (v[0] - (0. - v[81] * v[1])) * x[245] + (v[200] - v[81] * v[199]) * x[246] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[247] + + (v[0] - (0. - v[81] * v[1])) * x[248] + (v[200] - v[81] * v[199]) * x[249] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[250] + v[199] * v[94] + v[1] * v[86] + + (v[0] - (0. - v[81] * v[1])) * x[251] + (v[200] - v[81] * v[199]) * x[252] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[253] + v[199] * v[97] + v[1] * v[91] + + (v[0] - (0. - v[81] * v[1])) * x[254] + (v[200] - v[81] * v[199]) * x[255] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[256] + v[199] * v[100] + v[1] * v[96] + + (v[0] - (0. - v[81] * v[1])) * x[257] + (v[200] - v[81] * v[199]) * x[258] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[259] + v[199] * v[103] + v[1] * v[99] + + (v[0] - (0. - v[81] * v[1])) * x[260] + (v[200] - v[81] * v[199]) * x[261] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[262] + v[199] * v[106] + v[1] * v[102] + + (v[0] - (0. - v[81] * v[1])) * x[263] + (v[200] - v[81] * v[199]) * x[264] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[265] + v[199] * v[109] + v[1] * v[105] + + (v[0] - (0. - v[108] * v[1])) * x[266] + (v[200] - v[108] * v[199]) * x[267] + + (v[203] - (v[112] * v[1] - v[111] * v[199])) * x[268] + + (v[0] - (0. - v[108] * v[1])) * x[269] + (v[200] - v[108] * v[199]) * x[270] + + (v[203] - (v[112] * v[1] - v[111] * v[199])) * x[271] + v[199] * v[119] + v[1] * v[213] + + (v[0] - (0. - v[108] * v[1])) * x[272] + (v[200] - v[108] * v[199]) * x[273] + + (v[203] - (v[112] * v[1] - v[111] * v[199])) * x[274] + v[199] * v[124] + v[1] * v[113] + + (v[0] - (0. - v[108] * v[1])) * x[275] + (v[200] - v[108] * v[199]) * x[276] + + (v[203] - (v[112] * v[1] - v[111] * v[199])) * x[277] + v[199] * v[127] + v[1] * v[122] + + (v[0] - (0. - v[108] * v[1])) * x[278] + (v[200] - v[108] * v[199]) * x[279] + + (v[203] - (v[112] * v[1] - v[111] * v[199])) * x[280] + v[199] * v[130] + v[1] * v[126] + + (v[0] - (0. - v[108] * v[1])) * x[281] + (v[200] - v[108] * v[199]) * x[282] + + (v[203] - (v[112] * v[1] - v[111] * v[199])) * x[283] + v[199] * v[133] + v[1] * v[129] + + (v[0] - (0. - v[132] * v[1])) * x[284] + (v[200] - v[132] * v[199]) * x[285] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[286] + v[199] * v[138] + v[1] * v[117] + + (v[0] - (0. - v[132] * v[1])) * x[287] + (v[200] - v[132] * v[199]) * x[288] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[289] + + (v[0] - (0. - v[132] * v[1])) * x[290] + (v[200] - v[132] * v[199]) * x[291] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[292] + v[199] * v[145] + v[1] * v[137] + + (v[0] - (0. - v[132] * v[1])) * x[293] + (v[200] - v[132] * v[199]) * x[294] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[295] + v[199] * v[148] + v[1] * v[143] + + (v[0] - (0. - v[132] * v[1])) * x[296] + (v[200] - v[132] * v[199]) * x[297] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[298] + v[199] * v[151] + v[1] * v[147] + + (v[0] - (0. - v[132] * v[1])) * x[299] + (v[200] - v[132] * v[199]) * x[300] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[301] + v[199] * v[154] + v[1] * v[150] + + (v[0] - (0. - v[132] * v[1])) * x[302] + (v[200] - v[132] * v[199]) * x[303] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[304] + v[199] * v[160] + v[1] * v[140] + + (v[0] - (0. - v[132] * v[1])) * x[305] + (v[200] - v[132] * v[199]) * x[306] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[307] + v[199] * v[163] + v[1] * v[158] + + (v[0] - (0. - v[132] * v[1])) * x[308] + (v[200] - v[132] * v[199]) * x[309] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[310] + v[199] * v[166] + v[1] * v[162] + + (v[0] - (0. - v[132] * v[1])) * x[311] + (v[200] - v[132] * v[199]) * x[312] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[313] + v[199] * v[169] + v[1] * v[165] + + (v[0] - (0. - v[132] * v[1])) * x[314] + (v[200] - v[132] * v[199]) * x[315] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[316] + v[199] * v[172] + v[1] * v[168] + + (v[0] - (0. - v[132] * v[1])) * x[317] + (v[200] - v[132] * v[199]) * x[318] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[319] + v[199] * v[175] + v[1] * v[171] + + (v[0] - (0. - v[132] * v[1])) * x[320] + (v[200] - v[132] * v[199]) * x[321] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[322] + v[199] * v[178] + v[1] * v[174] + + (v[0] - (0. - v[132] * v[1])) * x[323] + (v[200] - v[132] * v[199]) * x[324] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[325] + v[199] * v[181] + v[1] * v[177] + + (v[0] - (0. - v[132] * v[1])) * x[326] + (v[200] - v[132] * v[199]) * x[327] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[328] + v[199] * v[184] + v[1] * v[180] + + (v[0] - (0. - v[132] * v[1])) * x[329] + (v[200] - v[132] * v[199]) * x[330] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[331] + v[199] * v[187] + v[1] * v[183] + + (v[0] - (0. - v[132] * v[1])) * x[332] + (v[200] - v[132] * v[199]) * x[333] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[334] + v[199] * v[190] + v[1] * v[186] + + (v[0] - (0. - v[132] * v[1])) * x[335] + (v[200] - v[132] * v[199]) * x[336] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[337] + v[199] * v[159] + v[1] * v[156] + + (v[0] - (0. - v[116] * v[1])) * x[356] + (v[200] - v[116] * v[199]) * x[357] + + (v[203] - (v[4] * v[1] - v[3] * v[199])) * x[358] + + v[199] * (v[11] * x[358] - v[5] * x[357]) + v[1] * (v[5] * x[356] - v[8] * x[358]) + + (v[0] - (0. - v[211] * v[1])) * x[359] + (v[200] - v[211] * v[199]) * x[360] + + (v[203] - (v[26] * v[1] - v[25] * v[199])) * x[361] + v[199] * v[39] + v[1] * v[36] + + (v[0] - (0. - v[54] * v[1])) * x[362] + (v[200] - v[54] * v[199]) * x[363] + + (v[203] - (v[58] * v[1] - v[57] * v[199])) * x[364] + v[199] * v[66] + v[1] * v[60] + + (v[0] - (0. - v[81] * v[1])) * x[365] + (v[200] - v[81] * v[199]) * x[366] + + (v[203] - (v[85] * v[1] - v[84] * v[199])) * x[367] + v[199] * v[93] + v[1] * v[89] + + (v[0] - (0. - v[108] * v[1])) * x[368] + (v[200] - v[108] * v[199]) * x[369] + + (v[203] - (v[112] * v[1] - v[111] * v[199])) * x[370] + v[199] * v[123] + v[1] * v[114] + + (v[0] - (0. - v[132] * v[1])) * x[371] + (v[200] - v[132] * v[199]) * x[372] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[373] + v[199] * v[62] + v[1] * v[121] + + (v[0] - (0. - v[132] * v[1])) * x[374] + (v[200] - v[132] * v[199]) * x[375] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[376] + v[199] * v[193] + v[1] * v[88] + + (v[0] - (0. - v[132] * v[1])) * x[377] + (v[200] - v[132] * v[199]) * x[378] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[379] + v[199] * v[196] + v[1] * v[6] + + (v[0] - (0. - v[132] * v[1])) * x[380] + (v[200] - v[132] * v[199]) * x[381] + + (v[203] - (v[136] * v[1] - v[135] * v[199])) * x[382] + v[199] * v[144] + v[1] * v[141]; + v[5] = cos(x[2]); + v[210] = v[198] * v[5]; + v[202] = v[202] + 0.219 * v[210]; + v[209] = sin(x[2]); + v[208] = -v[209]; + v[2] = v[2] + 0.219 * v[208]; + v[207] = v[202] * v[208] - v[2] * v[210]; + v[206] = v[1] * v[5]; + v[201] = v[201] + 0.219 * v[206]; + v[205] = v[2] * v[206] - v[201] * v[208]; + v[204] = v[201] * v[210] - v[202] * v[206]; + y[3] = + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[203] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[204] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[205] + v[206] * v[28] + v[210] * v[212] + + v[208] * v[29] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[206] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[207] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[208] + v[206] * v[31] + v[210] * v[27] + + v[208] * v[32] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[209] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[210] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[211] + v[206] * v[34] + v[210] * v[30] + + v[208] * v[35] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[212] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[213] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[214] + v[206] * v[43] + v[210] * v[33] + + v[208] * v[44] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[215] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[216] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[217] + v[206] * v[46] + v[210] * v[41] + + v[208] * v[47] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[218] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[219] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[220] + v[206] * v[49] + v[210] * v[45] + + v[208] * v[50] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[221] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[222] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[223] + v[206] * v[52] + v[210] * v[48] + + v[208] * v[53] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[224] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[225] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[226] + v[206] * v[55] + v[210] * v[51] + + v[208] * v[56] + (v[207] - (v[57] * v[208] - v[54] * v[210])) * x[227] + + (v[205] - (v[54] * v[206] - v[58] * v[208])) * x[228] + + (v[204] - (v[58] * v[210] - v[57] * v[206])) * x[229] + v[206] * v[67] + v[210] * v[59] + + v[208] * v[68] + (v[207] - (v[57] * v[208] - v[54] * v[210])) * x[230] + + (v[205] - (v[54] * v[206] - v[58] * v[208])) * x[231] + + (v[204] - (v[58] * v[210] - v[57] * v[206])) * x[232] + v[206] * v[70] + v[210] * v[64] + + v[208] * v[71] + (v[207] - (v[57] * v[208] - v[54] * v[210])) * x[233] + + (v[205] - (v[54] * v[206] - v[58] * v[208])) * x[234] + + (v[204] - (v[58] * v[210] - v[57] * v[206])) * x[235] + v[206] * v[73] + v[210] * v[69] + + v[208] * v[74] + (v[207] - (v[57] * v[208] - v[54] * v[210])) * x[236] + + (v[205] - (v[54] * v[206] - v[58] * v[208])) * x[237] + + (v[204] - (v[58] * v[210] - v[57] * v[206])) * x[238] + v[206] * v[76] + v[210] * v[72] + + v[208] * v[77] + (v[207] - (v[57] * v[208] - v[54] * v[210])) * x[239] + + (v[205] - (v[54] * v[206] - v[58] * v[208])) * x[240] + + (v[204] - (v[58] * v[210] - v[57] * v[206])) * x[241] + v[206] * v[79] + v[210] * v[75] + + v[208] * v[80] + (v[207] - (v[57] * v[208] - v[54] * v[210])) * x[242] + + (v[205] - (v[54] * v[206] - v[58] * v[208])) * x[243] + + (v[204] - (v[58] * v[210] - v[57] * v[206])) * x[244] + v[206] * v[82] + v[210] * v[78] + + v[208] * v[83] + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[245] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[246] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[247] + + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[248] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[249] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[250] + v[206] * v[94] + v[210] * v[86] + + v[208] * v[95] + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[251] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[252] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[253] + v[206] * v[97] + v[210] * v[91] + + v[208] * v[98] + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[254] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[255] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[256] + v[206] * v[100] + v[210] * v[96] + + v[208] * v[101] + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[257] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[258] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[259] + v[206] * v[103] + v[210] * v[99] + + v[208] * v[104] + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[260] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[261] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[262] + v[206] * v[106] + v[210] * v[102] + + v[208] * v[107] + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[263] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[264] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[265] + v[206] * v[109] + v[210] * v[105] + + v[208] * v[110] + (v[207] - (v[111] * v[208] - v[108] * v[210])) * x[266] + + (v[205] - (v[108] * v[206] - v[112] * v[208])) * x[267] + + (v[204] - (v[112] * v[210] - v[111] * v[206])) * x[268] + + (v[207] - (v[111] * v[208] - v[108] * v[210])) * x[269] + + (v[205] - (v[108] * v[206] - v[112] * v[208])) * x[270] + + (v[204] - (v[112] * v[210] - v[111] * v[206])) * x[271] + v[206] * v[119] + v[210] * v[213] + + v[208] * v[120] + (v[207] - (v[111] * v[208] - v[108] * v[210])) * x[272] + + (v[205] - (v[108] * v[206] - v[112] * v[208])) * x[273] + + (v[204] - (v[112] * v[210] - v[111] * v[206])) * x[274] + v[206] * v[124] + v[210] * v[113] + + v[208] * v[125] + (v[207] - (v[111] * v[208] - v[108] * v[210])) * x[275] + + (v[205] - (v[108] * v[206] - v[112] * v[208])) * x[276] + + (v[204] - (v[112] * v[210] - v[111] * v[206])) * x[277] + v[206] * v[127] + v[210] * v[122] + + v[208] * v[128] + (v[207] - (v[111] * v[208] - v[108] * v[210])) * x[278] + + (v[205] - (v[108] * v[206] - v[112] * v[208])) * x[279] + + (v[204] - (v[112] * v[210] - v[111] * v[206])) * x[280] + v[206] * v[130] + v[210] * v[126] + + v[208] * v[131] + (v[207] - (v[111] * v[208] - v[108] * v[210])) * x[281] + + (v[205] - (v[108] * v[206] - v[112] * v[208])) * x[282] + + (v[204] - (v[112] * v[210] - v[111] * v[206])) * x[283] + v[206] * v[133] + v[210] * v[129] + + v[208] * v[134] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[284] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[285] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[286] + v[206] * v[138] + v[210] * v[117] + + v[208] * v[139] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[287] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[288] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[289] + + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[290] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[291] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[292] + v[206] * v[145] + v[210] * v[137] + + v[208] * v[146] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[293] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[294] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[295] + v[206] * v[148] + v[210] * v[143] + + v[208] * v[149] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[296] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[297] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[298] + v[206] * v[151] + v[210] * v[147] + + v[208] * v[152] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[299] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[300] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[301] + v[206] * v[154] + v[210] * v[150] + + v[208] * v[155] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[302] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[303] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[304] + v[206] * v[160] + v[210] * v[140] + + v[208] * v[161] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[305] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[306] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[307] + v[206] * v[163] + v[210] * v[158] + + v[208] * v[164] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[308] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[309] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[310] + v[206] * v[166] + v[210] * v[162] + + v[208] * v[167] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[311] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[312] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[313] + v[206] * v[169] + v[210] * v[165] + + v[208] * v[170] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[314] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[315] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[316] + v[206] * v[172] + v[210] * v[168] + + v[208] * v[173] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[317] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[318] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[319] + v[206] * v[175] + v[210] * v[171] + + v[208] * v[176] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[320] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[321] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[322] + v[206] * v[178] + v[210] * v[174] + + v[208] * v[179] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[323] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[324] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[325] + v[206] * v[181] + v[210] * v[177] + + v[208] * v[182] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[326] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[327] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[328] + v[206] * v[184] + v[210] * v[180] + + v[208] * v[185] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[329] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[330] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[331] + v[206] * v[187] + v[210] * v[183] + + v[208] * v[188] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[332] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[333] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[334] + v[206] * v[190] + v[210] * v[186] + + v[208] * v[191] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[335] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[336] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[337] + v[206] * v[159] + v[210] * v[156] + + v[208] * v[192] + (v[207] - (v[25] * v[208] - v[211] * v[210])) * x[359] + + (v[205] - (v[211] * v[206] - v[26] * v[208])) * x[360] + + (v[204] - (v[26] * v[210] - v[25] * v[206])) * x[361] + v[206] * v[39] + v[210] * v[36] + + v[208] * v[10] + (v[207] - (v[57] * v[208] - v[54] * v[210])) * x[362] + + (v[205] - (v[54] * v[206] - v[58] * v[208])) * x[363] + + (v[204] - (v[58] * v[210] - v[57] * v[206])) * x[364] + v[206] * v[66] + v[210] * v[60] + + v[208] * v[42] + (v[207] - (v[84] * v[208] - v[81] * v[210])) * x[365] + + (v[205] - (v[81] * v[206] - v[85] * v[208])) * x[366] + + (v[204] - (v[85] * v[210] - v[84] * v[206])) * x[367] + v[206] * v[93] + v[210] * v[89] + + v[208] * v[65] + (v[207] - (v[111] * v[208] - v[108] * v[210])) * x[368] + + (v[205] - (v[108] * v[206] - v[112] * v[208])) * x[369] + + (v[204] - (v[112] * v[210] - v[111] * v[206])) * x[370] + v[206] * v[123] + v[210] * v[114] + + v[208] * v[92] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[371] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[372] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[373] + v[206] * v[62] + v[210] * v[121] + + v[208] * v[38] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[374] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[375] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[376] + v[206] * v[193] + v[210] * v[88] + + v[208] * v[194] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[377] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[378] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[379] + v[206] * v[196] + v[210] * v[6] + + v[208] * v[197] + (v[207] - (v[135] * v[208] - v[132] * v[210])) * x[380] + + (v[205] - (v[132] * v[206] - v[136] * v[208])) * x[381] + + (v[204] - (v[136] * v[210] - v[135] * v[206])) * x[382] + v[206] * v[144] + v[210] * v[141] + + v[208] * v[118]; + v[202] = v[202] + 0.133 * v[210]; + v[204] = sin(x[3]); + v[205] = v[5] * v[204]; + v[2] = v[2] + 0.133 * v[208]; + v[207] = cos(x[3]); + v[198] = v[198] * v[209]; + v[36] = v[1] * v[207] + v[198] * v[204]; + v[39] = v[202] * v[205] - v[2] * v[36]; + v[209] = v[1] * v[209]; + v[51] = v[199] * v[207] + v[209] * v[204]; + v[201] = v[201] + 0.133 * v[206]; + v[55] = v[2] * v[51] - v[201] * v[205]; + v[48] = v[201] * v[36] - v[202] * v[51]; + y[4] = (v[39] - (v[57] * v[205] - v[54] * v[36])) * x[227] + + (v[55] - (v[54] * v[51] - v[58] * v[205])) * x[228] + + (v[48] - (v[58] * v[36] - v[57] * v[51])) * x[229] + v[51] * v[67] + v[36] * v[59] + + v[205] * v[68] + (v[39] - (v[57] * v[205] - v[54] * v[36])) * x[230] + + (v[55] - (v[54] * v[51] - v[58] * v[205])) * x[231] + + (v[48] - (v[58] * v[36] - v[57] * v[51])) * x[232] + v[51] * v[70] + v[36] * v[64] + + v[205] * v[71] + (v[39] - (v[57] * v[205] - v[54] * v[36])) * x[233] + + (v[55] - (v[54] * v[51] - v[58] * v[205])) * x[234] + + (v[48] - (v[58] * v[36] - v[57] * v[51])) * x[235] + v[51] * v[73] + v[36] * v[69] + + v[205] * v[74] + (v[39] - (v[57] * v[205] - v[54] * v[36])) * x[236] + + (v[55] - (v[54] * v[51] - v[58] * v[205])) * x[237] + + (v[48] - (v[58] * v[36] - v[57] * v[51])) * x[238] + v[51] * v[76] + v[36] * v[72] + + v[205] * v[77] + (v[39] - (v[57] * v[205] - v[54] * v[36])) * x[239] + + (v[55] - (v[54] * v[51] - v[58] * v[205])) * x[240] + + (v[48] - (v[58] * v[36] - v[57] * v[51])) * x[241] + v[51] * v[79] + v[36] * v[75] + + v[205] * v[80] + (v[39] - (v[57] * v[205] - v[54] * v[36])) * x[242] + + (v[55] - (v[54] * v[51] - v[58] * v[205])) * x[243] + + (v[48] - (v[58] * v[36] - v[57] * v[51])) * x[244] + v[51] * v[82] + v[36] * v[78] + + v[205] * v[83] + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[245] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[246] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[247] + + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[248] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[249] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[250] + v[51] * v[94] + v[36] * v[86] + + v[205] * v[95] + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[251] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[252] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[253] + v[51] * v[97] + v[36] * v[91] + + v[205] * v[98] + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[254] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[255] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[256] + v[51] * v[100] + v[36] * v[96] + + v[205] * v[101] + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[257] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[258] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[259] + v[51] * v[103] + v[36] * v[99] + + v[205] * v[104] + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[260] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[261] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[262] + v[51] * v[106] + v[36] * v[102] + + v[205] * v[107] + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[263] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[264] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[265] + v[51] * v[109] + v[36] * v[105] + + v[205] * v[110] + (v[39] - (v[111] * v[205] - v[108] * v[36])) * x[266] + + (v[55] - (v[108] * v[51] - v[112] * v[205])) * x[267] + + (v[48] - (v[112] * v[36] - v[111] * v[51])) * x[268] + + (v[39] - (v[111] * v[205] - v[108] * v[36])) * x[269] + + (v[55] - (v[108] * v[51] - v[112] * v[205])) * x[270] + + (v[48] - (v[112] * v[36] - v[111] * v[51])) * x[271] + v[51] * v[119] + v[36] * v[213] + + v[205] * v[120] + (v[39] - (v[111] * v[205] - v[108] * v[36])) * x[272] + + (v[55] - (v[108] * v[51] - v[112] * v[205])) * x[273] + + (v[48] - (v[112] * v[36] - v[111] * v[51])) * x[274] + v[51] * v[124] + v[36] * v[113] + + v[205] * v[125] + (v[39] - (v[111] * v[205] - v[108] * v[36])) * x[275] + + (v[55] - (v[108] * v[51] - v[112] * v[205])) * x[276] + + (v[48] - (v[112] * v[36] - v[111] * v[51])) * x[277] + v[51] * v[127] + v[36] * v[122] + + v[205] * v[128] + (v[39] - (v[111] * v[205] - v[108] * v[36])) * x[278] + + (v[55] - (v[108] * v[51] - v[112] * v[205])) * x[279] + + (v[48] - (v[112] * v[36] - v[111] * v[51])) * x[280] + v[51] * v[130] + v[36] * v[126] + + v[205] * v[131] + (v[39] - (v[111] * v[205] - v[108] * v[36])) * x[281] + + (v[55] - (v[108] * v[51] - v[112] * v[205])) * x[282] + + (v[48] - (v[112] * v[36] - v[111] * v[51])) * x[283] + v[51] * v[133] + v[36] * v[129] + + v[205] * v[134] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[284] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[285] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[286] + v[51] * v[138] + v[36] * v[117] + + v[205] * v[139] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[287] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[288] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[289] + + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[290] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[291] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[292] + v[51] * v[145] + v[36] * v[137] + + v[205] * v[146] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[293] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[294] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[295] + v[51] * v[148] + v[36] * v[143] + + v[205] * v[149] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[296] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[297] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[298] + v[51] * v[151] + v[36] * v[147] + + v[205] * v[152] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[299] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[300] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[301] + v[51] * v[154] + v[36] * v[150] + + v[205] * v[155] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[302] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[303] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[304] + v[51] * v[160] + v[36] * v[140] + + v[205] * v[161] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[305] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[306] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[307] + v[51] * v[163] + v[36] * v[158] + + v[205] * v[164] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[308] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[309] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[310] + v[51] * v[166] + v[36] * v[162] + + v[205] * v[167] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[311] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[312] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[313] + v[51] * v[169] + v[36] * v[165] + + v[205] * v[170] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[314] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[315] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[316] + v[51] * v[172] + v[36] * v[168] + + v[205] * v[173] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[317] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[318] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[319] + v[51] * v[175] + v[36] * v[171] + + v[205] * v[176] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[320] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[321] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[322] + v[51] * v[178] + v[36] * v[174] + + v[205] * v[179] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[323] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[324] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[325] + v[51] * v[181] + v[36] * v[177] + + v[205] * v[182] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[326] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[327] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[328] + v[51] * v[184] + v[36] * v[180] + + v[205] * v[185] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[329] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[330] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[331] + v[51] * v[187] + v[36] * v[183] + + v[205] * v[188] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[332] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[333] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[334] + v[51] * v[190] + v[36] * v[186] + + v[205] * v[191] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[335] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[336] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[337] + v[51] * v[159] + v[36] * v[156] + + v[205] * v[192] + (v[39] - (v[57] * v[205] - v[54] * v[36])) * x[362] + + (v[55] - (v[54] * v[51] - v[58] * v[205])) * x[363] + + (v[48] - (v[58] * v[36] - v[57] * v[51])) * x[364] + v[51] * v[66] + v[36] * v[60] + + v[205] * v[42] + (v[39] - (v[84] * v[205] - v[81] * v[36])) * x[365] + + (v[55] - (v[81] * v[51] - v[85] * v[205])) * x[366] + + (v[48] - (v[85] * v[36] - v[84] * v[51])) * x[367] + v[51] * v[93] + v[36] * v[89] + + v[205] * v[65] + (v[39] - (v[111] * v[205] - v[108] * v[36])) * x[368] + + (v[55] - (v[108] * v[51] - v[112] * v[205])) * x[369] + + (v[48] - (v[112] * v[36] - v[111] * v[51])) * x[370] + v[51] * v[123] + v[36] * v[114] + + v[205] * v[92] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[371] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[372] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[373] + v[51] * v[62] + v[36] * v[121] + + v[205] * v[38] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[374] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[375] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[376] + v[51] * v[193] + v[36] * v[88] + + v[205] * v[194] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[377] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[378] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[379] + v[51] * v[196] + v[36] * v[6] + + v[205] * v[197] + (v[39] - (v[135] * v[205] - v[132] * v[36])) * x[380] + + (v[55] - (v[132] * v[51] - v[136] * v[205])) * x[381] + + (v[48] - (v[136] * v[36] - v[135] * v[51])) * x[382] + v[51] * v[144] + v[36] * v[141] + + v[205] * v[118]; + v[48] = cos(x[4]); + v[204] = -v[204]; + v[198] = v[1] * v[204] + v[198] * v[207]; + v[1] = sin(x[4]); + v[55] = -v[1]; + v[39] = v[210] * v[48] + v[198] * v[55]; + v[202] = v[202] + 0.197 * v[39]; + v[5] = v[5] * v[207]; + v[60] = v[208] * v[48] + v[5] * v[55]; + v[2] = v[2] + 0.197 * v[60]; + v[66] = v[202] * v[60] - v[2] * v[39]; + v[204] = v[199] * v[204] + v[209] * v[207]; + v[55] = v[206] * v[48] + v[204] * v[55]; + v[201] = v[201] + 0.197 * v[55]; + v[209] = v[2] * v[55] - v[201] * v[60]; + v[207] = v[201] * v[39] - v[202] * v[55]; + y[5] = (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[245] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[246] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[247] + + (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[248] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[249] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[250] + v[55] * v[94] + v[39] * v[86] + + v[60] * v[95] + (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[251] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[252] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[253] + v[55] * v[97] + v[39] * v[91] + + v[60] * v[98] + (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[254] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[255] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[256] + v[55] * v[100] + v[39] * v[96] + + v[60] * v[101] + (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[257] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[258] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[259] + v[55] * v[103] + v[39] * v[99] + + v[60] * v[104] + (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[260] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[261] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[262] + v[55] * v[106] + v[39] * v[102] + + v[60] * v[107] + (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[263] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[264] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[265] + v[55] * v[109] + v[39] * v[105] + + v[60] * v[110] + (v[66] - (v[111] * v[60] - v[108] * v[39])) * x[266] + + (v[209] - (v[108] * v[55] - v[112] * v[60])) * x[267] + + (v[207] - (v[112] * v[39] - v[111] * v[55])) * x[268] + + (v[66] - (v[111] * v[60] - v[108] * v[39])) * x[269] + + (v[209] - (v[108] * v[55] - v[112] * v[60])) * x[270] + + (v[207] - (v[112] * v[39] - v[111] * v[55])) * x[271] + v[55] * v[119] + v[39] * v[213] + + v[60] * v[120] + (v[66] - (v[111] * v[60] - v[108] * v[39])) * x[272] + + (v[209] - (v[108] * v[55] - v[112] * v[60])) * x[273] + + (v[207] - (v[112] * v[39] - v[111] * v[55])) * x[274] + v[55] * v[124] + v[39] * v[113] + + v[60] * v[125] + (v[66] - (v[111] * v[60] - v[108] * v[39])) * x[275] + + (v[209] - (v[108] * v[55] - v[112] * v[60])) * x[276] + + (v[207] - (v[112] * v[39] - v[111] * v[55])) * x[277] + v[55] * v[127] + v[39] * v[122] + + v[60] * v[128] + (v[66] - (v[111] * v[60] - v[108] * v[39])) * x[278] + + (v[209] - (v[108] * v[55] - v[112] * v[60])) * x[279] + + (v[207] - (v[112] * v[39] - v[111] * v[55])) * x[280] + v[55] * v[130] + v[39] * v[126] + + v[60] * v[131] + (v[66] - (v[111] * v[60] - v[108] * v[39])) * x[281] + + (v[209] - (v[108] * v[55] - v[112] * v[60])) * x[282] + + (v[207] - (v[112] * v[39] - v[111] * v[55])) * x[283] + v[55] * v[133] + v[39] * v[129] + + v[60] * v[134] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[284] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[285] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[286] + v[55] * v[138] + v[39] * v[117] + + v[60] * v[139] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[287] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[288] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[289] + + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[290] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[291] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[292] + v[55] * v[145] + v[39] * v[137] + + v[60] * v[146] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[293] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[294] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[295] + v[55] * v[148] + v[39] * v[143] + + v[60] * v[149] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[296] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[297] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[298] + v[55] * v[151] + v[39] * v[147] + + v[60] * v[152] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[299] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[300] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[301] + v[55] * v[154] + v[39] * v[150] + + v[60] * v[155] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[302] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[303] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[304] + v[55] * v[160] + v[39] * v[140] + + v[60] * v[161] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[305] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[306] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[307] + v[55] * v[163] + v[39] * v[158] + + v[60] * v[164] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[308] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[309] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[310] + v[55] * v[166] + v[39] * v[162] + + v[60] * v[167] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[311] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[312] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[313] + v[55] * v[169] + v[39] * v[165] + + v[60] * v[170] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[314] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[315] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[316] + v[55] * v[172] + v[39] * v[168] + + v[60] * v[173] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[317] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[318] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[319] + v[55] * v[175] + v[39] * v[171] + + v[60] * v[176] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[320] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[321] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[322] + v[55] * v[178] + v[39] * v[174] + + v[60] * v[179] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[323] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[324] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[325] + v[55] * v[181] + v[39] * v[177] + + v[60] * v[182] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[326] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[327] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[328] + v[55] * v[184] + v[39] * v[180] + + v[60] * v[185] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[329] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[330] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[331] + v[55] * v[187] + v[39] * v[183] + + v[60] * v[188] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[332] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[333] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[334] + v[55] * v[190] + v[39] * v[186] + + v[60] * v[191] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[335] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[336] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[337] + v[55] * v[159] + v[39] * v[156] + + v[60] * v[192] + (v[66] - (v[84] * v[60] - v[81] * v[39])) * x[365] + + (v[209] - (v[81] * v[55] - v[85] * v[60])) * x[366] + + (v[207] - (v[85] * v[39] - v[84] * v[55])) * x[367] + v[55] * v[93] + v[39] * v[89] + + v[60] * v[65] + (v[66] - (v[111] * v[60] - v[108] * v[39])) * x[368] + + (v[209] - (v[108] * v[55] - v[112] * v[60])) * x[369] + + (v[207] - (v[112] * v[39] - v[111] * v[55])) * x[370] + v[55] * v[123] + v[39] * v[114] + + v[60] * v[92] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[371] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[372] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[373] + v[55] * v[62] + v[39] * v[121] + + v[60] * v[38] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[374] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[375] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[376] + v[55] * v[193] + v[39] * v[88] + + v[60] * v[194] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[377] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[378] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[379] + v[55] * v[196] + v[39] * v[6] + + v[60] * v[197] + (v[66] - (v[135] * v[60] - v[132] * v[39])) * x[380] + + (v[209] - (v[132] * v[55] - v[136] * v[60])) * x[381] + + (v[207] - (v[136] * v[39] - v[135] * v[55])) * x[382] + v[55] * v[144] + v[39] * v[141] + + v[60] * v[118]; + v[202] = v[202] + 0.1245 * v[39]; + v[207] = cos(x[5]); + v[5] = v[208] * v[1] + v[5] * v[48]; + v[208] = sin(x[5]); + v[209] = v[205] * v[207] + v[5] * v[208]; + v[2] = v[2] + 0.1245 * v[60]; + v[198] = v[210] * v[1] + v[198] * v[48]; + v[210] = v[36] * v[207] + v[198] * v[208]; + v[66] = v[202] * v[209] - v[2] * v[210]; + v[204] = v[206] * v[1] + v[204] * v[48]; + v[1] = v[51] * v[207] + v[204] * v[208]; + v[201] = v[201] + 0.1245 * v[55]; + v[48] = v[2] * v[1] - v[201] * v[209]; + v[206] = v[201] * v[210] - v[202] * v[1]; + y[6] = (v[66] - (v[111] * v[209] - v[108] * v[210])) * x[266] + + (v[48] - (v[108] * v[1] - v[112] * v[209])) * x[267] + + (v[206] - (v[112] * v[210] - v[111] * v[1])) * x[268] + + (v[66] - (v[111] * v[209] - v[108] * v[210])) * x[269] + + (v[48] - (v[108] * v[1] - v[112] * v[209])) * x[270] + + (v[206] - (v[112] * v[210] - v[111] * v[1])) * x[271] + v[1] * v[119] + v[210] * v[213] + + v[209] * v[120] + (v[66] - (v[111] * v[209] - v[108] * v[210])) * x[272] + + (v[48] - (v[108] * v[1] - v[112] * v[209])) * x[273] + + (v[206] - (v[112] * v[210] - v[111] * v[1])) * x[274] + v[1] * v[124] + v[210] * v[113] + + v[209] * v[125] + (v[66] - (v[111] * v[209] - v[108] * v[210])) * x[275] + + (v[48] - (v[108] * v[1] - v[112] * v[209])) * x[276] + + (v[206] - (v[112] * v[210] - v[111] * v[1])) * x[277] + v[1] * v[127] + v[210] * v[122] + + v[209] * v[128] + (v[66] - (v[111] * v[209] - v[108] * v[210])) * x[278] + + (v[48] - (v[108] * v[1] - v[112] * v[209])) * x[279] + + (v[206] - (v[112] * v[210] - v[111] * v[1])) * x[280] + v[1] * v[130] + v[210] * v[126] + + v[209] * v[131] + (v[66] - (v[111] * v[209] - v[108] * v[210])) * x[281] + + (v[48] - (v[108] * v[1] - v[112] * v[209])) * x[282] + + (v[206] - (v[112] * v[210] - v[111] * v[1])) * x[283] + v[1] * v[133] + v[210] * v[129] + + v[209] * v[134] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[284] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[285] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[286] + v[1] * v[138] + v[210] * v[117] + + v[209] * v[139] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[287] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[288] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[289] + + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[290] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[291] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[292] + v[1] * v[145] + v[210] * v[137] + + v[209] * v[146] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[293] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[294] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[295] + v[1] * v[148] + v[210] * v[143] + + v[209] * v[149] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[296] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[297] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[298] + v[1] * v[151] + v[210] * v[147] + + v[209] * v[152] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[299] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[300] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[301] + v[1] * v[154] + v[210] * v[150] + + v[209] * v[155] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[302] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[303] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[304] + v[1] * v[160] + v[210] * v[140] + + v[209] * v[161] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[305] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[306] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[307] + v[1] * v[163] + v[210] * v[158] + + v[209] * v[164] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[308] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[309] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[310] + v[1] * v[166] + v[210] * v[162] + + v[209] * v[167] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[311] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[312] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[313] + v[1] * v[169] + v[210] * v[165] + + v[209] * v[170] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[314] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[315] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[316] + v[1] * v[172] + v[210] * v[168] + + v[209] * v[173] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[317] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[318] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[319] + v[1] * v[175] + v[210] * v[171] + + v[209] * v[176] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[320] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[321] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[322] + v[1] * v[178] + v[210] * v[174] + + v[209] * v[179] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[323] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[324] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[325] + v[1] * v[181] + v[210] * v[177] + + v[209] * v[182] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[326] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[327] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[328] + v[1] * v[184] + v[210] * v[180] + + v[209] * v[185] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[329] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[330] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[331] + v[1] * v[187] + v[210] * v[183] + + v[209] * v[188] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[332] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[333] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[334] + v[1] * v[190] + v[210] * v[186] + + v[209] * v[191] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[335] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[336] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[337] + v[1] * v[159] + v[210] * v[156] + + v[209] * v[192] + (v[66] - (v[111] * v[209] - v[108] * v[210])) * x[368] + + (v[48] - (v[108] * v[1] - v[112] * v[209])) * x[369] + + (v[206] - (v[112] * v[210] - v[111] * v[1])) * x[370] + v[1] * v[123] + v[210] * v[114] + + v[209] * v[92] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[371] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[372] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[373] + v[1] * v[62] + v[210] * v[121] + + v[209] * v[38] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[374] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[375] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[376] + v[1] * v[193] + v[210] * v[88] + + v[209] * v[194] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[377] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[378] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[379] + v[1] * v[196] + v[210] * v[6] + + v[209] * v[197] + (v[66] - (v[135] * v[209] - v[132] * v[210])) * x[380] + + (v[48] - (v[132] * v[1] - v[136] * v[209])) * x[381] + + (v[206] - (v[136] * v[210] - v[135] * v[1])) * x[382] + v[1] * v[144] + v[210] * v[141] + + v[209] * v[118]; + v[206] = cos(x[6]); + v[208] = -v[208]; + v[48] = -sin(x[6]); + v[198] = v[39] * v[206] + (v[36] * v[208] + v[198] * v[207]) * v[48]; + v[202] = v[202] + 0.1385 * v[198]; + v[5] = v[60] * v[206] + (v[205] * v[208] + v[5] * v[207]) * v[48]; + v[2] = v[2] + 0.1385 * v[5]; + v[60] = v[202] * v[5] - v[2] * v[198]; + v[48] = v[55] * v[206] + (v[51] * v[208] + v[204] * v[207]) * v[48]; + v[201] = v[201] + 0.1385 * v[48]; + v[2] = v[2] * v[48] - v[201] * v[5]; + v[201] = v[201] * v[198] - v[202] * v[48]; + y[7] = (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[284] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[285] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[286] + v[48] * v[138] + v[198] * v[117] + + v[5] * v[139] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[287] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[288] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[289] + + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[290] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[291] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[292] + v[48] * v[145] + v[198] * v[137] + + v[5] * v[146] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[293] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[294] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[295] + v[48] * v[148] + v[198] * v[143] + + v[5] * v[149] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[296] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[297] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[298] + v[48] * v[151] + v[198] * v[147] + + v[5] * v[152] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[299] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[300] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[301] + v[48] * v[154] + v[198] * v[150] + + v[5] * v[155] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[302] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[303] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[304] + v[48] * v[160] + v[198] * v[140] + + v[5] * v[161] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[305] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[306] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[307] + v[48] * v[163] + v[198] * v[158] + + v[5] * v[164] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[308] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[309] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[310] + v[48] * v[166] + v[198] * v[162] + + v[5] * v[167] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[311] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[312] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[313] + v[48] * v[169] + v[198] * v[165] + + v[5] * v[170] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[314] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[315] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[316] + v[48] * v[172] + v[198] * v[168] + + v[5] * v[173] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[317] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[318] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[319] + v[48] * v[175] + v[198] * v[171] + + v[5] * v[176] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[320] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[321] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[322] + v[48] * v[178] + v[198] * v[174] + + v[5] * v[179] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[323] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[324] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[325] + v[48] * v[181] + v[198] * v[177] + + v[5] * v[182] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[326] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[327] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[328] + v[48] * v[184] + v[198] * v[180] + + v[5] * v[185] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[329] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[330] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[331] + v[48] * v[187] + v[198] * v[183] + + v[5] * v[188] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[332] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[333] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[334] + v[48] * v[190] + v[198] * v[186] + + v[5] * v[191] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[335] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[336] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[337] + v[48] * v[159] + v[198] * v[156] + + v[5] * v[192] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[371] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[372] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[373] + v[48] * v[62] + v[198] * v[121] + + v[5] * v[38] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[374] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[375] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[376] + v[48] * v[193] + v[198] * v[88] + + v[5] * v[194] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[377] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[378] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[379] + v[48] * v[196] + v[198] * v[6] + + v[5] * v[197] + (v[60] - (v[135] * v[5] - v[132] * v[198])) * x[380] + + (v[2] - (v[132] * v[48] - v[136] * v[5])) * x[381] + + (v[201] - (v[136] * v[198] - v[135] * v[48])) * x[382] + v[48] * v[144] + v[198] * v[141] + + v[5] * v[118]; + + // Copy result to output + for (std::size_t i = 0; i < 8; ++i) + { + out[i] = y[i]; + } + } }; } // namespace vamp::robots diff --git a/src/impl/vamp/robots/panda.hh b/src/impl/vamp/robots/panda.hh index 0c51e3f9..3192c73d 100644 --- a/src/impl/vamp/robots/panda.hh +++ b/src/impl/vamp/robots/panda.hh @@ -944,141 +944,107 @@ namespace vamp::robots output.first.emplace_back( sphere_environment_get_collisions(environment, y[96], y[97], y[98], y[99])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[100], y[101], y[102], y[103])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[100], y[101], y[102], y[103])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[104], y[105], y[106], y[107])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[104], y[105], y[106], y[107])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[108], y[109], y[110], y[111])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[108], y[109], y[110], y[111])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[112], y[113], y[114], y[115])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[112], y[113], y[114], y[115])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[116], y[117], y[118], y[119])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[116], y[117], y[118], y[119])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[120], y[121], y[122], y[123])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[120], y[121], y[122], y[123])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[124], y[125], y[126], y[127])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[124], y[125], y[126], y[127])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[128], y[129], y[130], y[131])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[128], y[129], y[130], y[131])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[132], y[133], y[134], y[135])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[132], y[133], y[134], y[135])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[136], y[137], y[138], y[139])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[136], y[137], y[138], y[139])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[140], y[141], y[142], y[143])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[140], y[141], y[142], y[143])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[144], y[145], y[146], y[147])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[144], y[145], y[146], y[147])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[148], y[149], y[150], y[151])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[148], y[149], y[150], y[151])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[152], y[153], y[154], y[155])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[152], y[153], y[154], y[155])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[156], y[157], y[158], y[159])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[156], y[157], y[158], y[159])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[160], y[161], y[162], y[163])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[160], y[161], y[162], y[163])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[164], y[165], y[166], y[167])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[164], y[165], y[166], y[167])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[168], y[169], y[170], y[171])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[168], y[169], y[170], y[171])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[172], y[173], y[174], y[175])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[172], y[173], y[174], y[175])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[176], y[177], y[178], y[179])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[176], y[177], y[178], y[179])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[180], y[181], y[182], y[183])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[180], y[181], y[182], y[183])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[184], y[185], y[186], y[187])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[184], y[185], y[186], y[187])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[188], y[189], y[190], y[191])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[188], y[189], y[190], y[191])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[192], y[193], y[194], y[195])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[192], y[193], y[194], y[195])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[196], y[197], y[198], y[199])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[196], y[197], y[198], y[199])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[200], y[201], y[202], y[203])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[200], y[201], y[202], y[203])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[204], y[205], y[206], y[207])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[204], y[205], y[206], y[207])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[208], y[209], y[210], y[211])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[208], y[209], y[210], y[211])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[212], y[213], y[214], y[215])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[212], y[213], y[214], y[215])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[216], y[217], y[218], y[219])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[216], y[217], y[218], y[219])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[220], y[221], y[222], y[223])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[220], y[221], y[222], y[223])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[224], y[225], y[226], y[227])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[224], y[225], y[226], y[227])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[228], y[229], y[230], y[231])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[228], y[229], y[230], y[231])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[232], y[233], y[234], y[235])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[232], y[233], y[234], y[235])); if (sphere_sphere_self_collision( y[0], y[1], y[2], y[3], y[68], y[69], y[70], y[71])) @@ -15550,6 +15516,2516 @@ namespace vamp::robots return to_isometry(y.data()); } + + template + inline static auto sdf_gradient( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &x) noexcept + -> std::pair, FloatVector> + { + std::array, 57> v; + std::array, 280> y; + + v[0] = sin(x[0]); + v[1] = -v[0]; + y[4] = -0.08 * v[1]; + v[2] = cos(x[0]); + y[5] = -0.08 * v[2]; + y[8] = -0.03 * v[1]; + y[9] = -0.03 * v[2]; + y[20] = 0.03 * v[1]; + y[21] = 0.03 * v[2]; + y[24] = 0.08 * v[1]; + y[25] = 0.08 * v[2]; + v[3] = sin(x[1]); + v[4] = -v[3]; + v[5] = cos(x[1]); + v[6] = 4.89663865010925e-12 * v[5]; + v[7] = v[2] * v[4] + v[1] * v[6]; + y[28] = -0.12 * v[7]; + v[6] = v[0] * v[4] + v[2] * v[6]; + y[29] = -0.12 * v[6]; + v[4] = -1. * v[5]; + y[30] = 0.333 + -0.12 * v[4]; + y[32] = -0.17 * v[7]; + y[33] = -0.17 * v[6]; + y[34] = 0.333 + -0.17 * v[4]; + v[8] = -1. * v[7] + 4.89663865010925e-12 * v[1]; + v[9] = -0.316 * v[7]; + y[36] = -0.1 * v[8] + v[9]; + v[10] = -1. * v[6] + 4.89663865010925e-12 * v[2]; + v[11] = -0.316 * v[6]; + y[37] = -0.1 * v[10] + v[11]; + v[12] = 2.39770700697438e-23 + -1. * v[4]; + v[13] = 0.333 + -0.316 * v[4]; + y[38] = -0.1 * v[12] + v[13]; + y[40] = -0.06 * v[8] + v[9]; + y[41] = -0.06 * v[10] + v[11]; + y[42] = -0.06 * v[12] + v[13]; + v[14] = 4.89663865010925e-12 * v[3]; + v[15] = v[2] * v[5] + v[1] * v[14]; + v[16] = cos(x[2]); + v[17] = sin(x[2]); + v[18] = 4.89663865010925e-12 * v[17]; + v[19] = v[15] * v[16] + v[7] * v[18] + v[1] * v[17]; + v[20] = -v[17]; + v[21] = 4.89663865010925e-12 * v[16]; + v[15] = v[15] * v[20] + v[7] * v[21] + v[1] * v[16]; + y[44] = 0.08 * v[19] + 0.06 * v[15] + v[9]; + v[14] = v[0] * v[5] + v[2] * v[14]; + v[5] = v[14] * v[16] + v[6] * v[18] + v[2] * v[17]; + v[14] = v[14] * v[20] + v[6] * v[21] + v[2] * v[16]; + y[45] = 0.08 * v[5] + 0.06 * v[14] + v[11]; + v[3] = -1. * v[3]; + v[18] = v[3] * v[16] + v[4] * v[18] + 4.89663865010925e-12 * v[17]; + v[3] = v[3] * v[20] + v[4] * v[21] + 4.89663865010925e-12 * v[16]; + y[46] = 0.08 * v[18] + 0.06 * v[3] + v[13]; + y[48] = 0.08 * v[19] + 0.02 * v[15] + v[9]; + y[49] = 0.08 * v[5] + 0.02 * v[14] + v[11]; + y[50] = 0.08 * v[18] + 0.02 * v[3] + v[13]; + v[21] = cos(x[3]); + v[20] = sin(x[3]); + v[16] = 4.89663865010925e-12 * v[20]; + v[17] = v[19] * v[21] + v[15] * v[16] + v[8] * v[20]; + v[0] = -v[20]; + v[22] = 4.89663865010925e-12 * v[21]; + v[23] = v[19] * v[0] + v[15] * v[22] + v[8] * v[21]; + v[24] = v[9] + 0.0825 * v[19]; + y[52] = -0.08 * v[17] + 0.095 * v[23] + v[24]; + v[25] = v[5] * v[21] + v[14] * v[16] + v[10] * v[20]; + v[26] = v[5] * v[0] + v[14] * v[22] + v[10] * v[21]; + v[27] = v[11] + 0.0825 * v[5]; + y[53] = -0.08 * v[25] + 0.095 * v[26] + v[27]; + v[16] = v[18] * v[21] + v[3] * v[16] + v[12] * v[20]; + v[22] = v[18] * v[0] + v[3] * v[22] + v[12] * v[21]; + v[0] = v[13] + 0.0825 * v[18]; + y[54] = -0.08 * v[16] + 0.095 * v[22] + v[0]; + v[21] = -1. * v[15] + 4.89663865010925e-12 * v[8]; + y[56] = 0.02 * v[21] + v[24]; + v[20] = -1. * v[14] + 4.89663865010925e-12 * v[10]; + y[57] = 0.02 * v[20] + v[27]; + v[28] = -1. * v[3] + 4.89663865010925e-12 * v[12]; + y[58] = 0.02 * v[28] + v[0]; + y[60] = 0.06 * v[21] + v[24]; + y[61] = 0.06 * v[20] + v[27]; + y[62] = 0.06 * v[28] + v[0]; + y[64] = -0.08 * v[17] + 0.06 * v[23] + v[24]; + y[65] = -0.08 * v[25] + 0.06 * v[26] + v[27]; + y[66] = -0.08 * v[16] + 0.06 * v[22] + v[0]; + v[29] = sin(x[4]); + v[30] = -v[29]; + v[31] = cos(x[4]); + v[32] = 4.89663865010925e-12 * v[31]; + v[33] = -1. * v[31]; + v[34] = v[17] * v[30] + v[23] * v[32] + v[21] * v[33]; + y[116] = v[24] + -0.0825 * v[17] + 0.384 * v[23]; + y[68] = 0.055 * v[34] + y[116]; + v[35] = v[25] * v[30] + v[26] * v[32] + v[20] * v[33]; + y[117] = v[27] + -0.0825 * v[25] + 0.384 * v[26]; + y[69] = 0.055 * v[35] + y[117]; + v[33] = v[16] * v[30] + v[22] * v[32] + v[28] * v[33]; + y[118] = v[0] + -0.0825 * v[16] + 0.384 * v[22]; + y[70] = 0.055 * v[33] + y[118]; + y[72] = 0.075 * v[34] + y[116]; + y[73] = 0.075 * v[35] + y[117]; + y[74] = 0.075 * v[33] + y[118]; + v[32] = v[23] + 4.89663865010925e-12 * v[21]; + y[76] = -0.22 * v[32] + y[116]; + v[30] = v[26] + 4.89663865010925e-12 * v[20]; + y[77] = -0.22 * v[30] + y[117]; + v[36] = v[22] + 4.89663865010925e-12 * v[28]; + y[78] = -0.22 * v[36] + y[118]; + y[80] = 0.05 * v[34] + -0.18 * v[32] + y[116]; + y[81] = 0.05 * v[35] + -0.18 * v[30] + y[117]; + y[82] = 0.05 * v[33] + -0.18 * v[36] + y[118]; + v[37] = 4.89663865010925e-12 * v[29]; + v[29] = -1. * v[29]; + v[38] = v[17] * v[31] + v[23] * v[37] + v[21] * v[29]; + y[84] = 0.01 * v[38] + 0.08 * v[34] + -0.14 * v[32] + y[116]; + v[39] = v[25] * v[31] + v[26] * v[37] + v[20] * v[29]; + y[85] = 0.01 * v[39] + 0.08 * v[35] + -0.14 * v[30] + y[117]; + v[29] = v[16] * v[31] + v[22] * v[37] + v[28] * v[29]; + y[86] = 0.01 * v[29] + 0.08 * v[33] + -0.14 * v[36] + y[118]; + y[88] = 0.01 * v[38] + 0.085 * v[34] + -0.11 * v[32] + y[116]; + y[89] = 0.01 * v[39] + 0.085 * v[35] + -0.11 * v[30] + y[117]; + y[90] = 0.01 * v[29] + 0.085 * v[33] + -0.11 * v[36] + y[118]; + y[92] = 0.01 * v[38] + 0.09 * v[34] + -0.08 * v[32] + y[116]; + y[93] = 0.01 * v[39] + 0.09 * v[35] + -0.08 * v[30] + y[117]; + y[94] = 0.01 * v[29] + 0.09 * v[33] + -0.08 * v[36] + y[118]; + y[96] = 0.01 * v[38] + 0.095 * v[34] + -0.05 * v[32] + y[116]; + y[97] = 0.01 * v[39] + 0.095 * v[35] + -0.05 * v[30] + y[117]; + y[98] = 0.01 * v[29] + 0.095 * v[33] + -0.05 * v[36] + y[118]; + y[100] = -0.01 * v[38] + 0.08 * v[34] + -0.14 * v[32] + y[116]; + y[101] = -0.01 * v[39] + 0.08 * v[35] + -0.14 * v[30] + y[117]; + y[102] = -0.01 * v[29] + 0.08 * v[33] + -0.14 * v[36] + y[118]; + y[104] = -0.01 * v[38] + 0.085 * v[34] + -0.11 * v[32] + y[116]; + y[105] = -0.01 * v[39] + 0.085 * v[35] + -0.11 * v[30] + y[117]; + y[106] = -0.01 * v[29] + 0.085 * v[33] + -0.11 * v[36] + y[118]; + y[108] = -0.01 * v[38] + 0.09 * v[34] + -0.08 * v[32] + y[116]; + y[109] = -0.01 * v[39] + 0.09 * v[35] + -0.08 * v[30] + y[117]; + y[110] = -0.01 * v[29] + 0.09 * v[33] + -0.08 * v[36] + y[118]; + y[112] = -0.01 * v[38] + 0.095 * v[34] + -0.05 * v[32] + y[116]; + y[113] = -0.01 * v[39] + 0.095 * v[35] + -0.05 * v[30] + y[117]; + y[114] = -0.01 * v[29] + 0.095 * v[33] + -0.05 * v[36] + y[118]; + v[37] = cos(x[5]); + v[31] = sin(x[5]); + v[40] = 4.89663865010925e-12 * v[31]; + v[41] = v[38] * v[37] + v[34] * v[40] + v[32] * v[31]; + v[42] = -v[31]; + v[43] = 4.89663865010925e-12 * v[37]; + v[38] = v[38] * v[42] + v[34] * v[43] + v[32] * v[37]; + y[120] = 0.08 * v[41] + -0.01 * v[38] + y[116]; + v[44] = v[39] * v[37] + v[35] * v[40] + v[30] * v[31]; + v[39] = v[39] * v[42] + v[35] * v[43] + v[30] * v[37]; + y[121] = 0.08 * v[44] + -0.01 * v[39] + y[117]; + v[40] = v[29] * v[37] + v[33] * v[40] + v[36] * v[31]; + v[43] = v[29] * v[42] + v[33] * v[43] + v[36] * v[37]; + y[122] = 0.08 * v[40] + -0.01 * v[43] + y[118]; + y[124] = 0.08 * v[41] + 0.035 * v[38] + y[116]; + y[125] = 0.08 * v[44] + 0.035 * v[39] + y[117]; + y[126] = 0.08 * v[40] + 0.035 * v[43] + y[118]; + v[42] = -1. * v[34] + 4.89663865010925e-12 * v[32]; + v[37] = -1. * v[38] + 4.89663865010925e-12 * v[42]; + v[29] = y[116] + 0.088 * v[41]; + y[128] = 0.07 * v[37] + v[29]; + v[31] = -1. * v[35] + 4.89663865010925e-12 * v[30]; + v[45] = -1. * v[39] + 4.89663865010925e-12 * v[31]; + v[46] = y[117] + 0.088 * v[44]; + y[129] = 0.07 * v[45] + v[46]; + v[47] = -1. * v[33] + 4.89663865010925e-12 * v[36]; + v[48] = -1. * v[43] + 4.89663865010925e-12 * v[47]; + v[49] = y[118] + 0.088 * v[40]; + y[130] = 0.07 * v[48] + v[49]; + v[50] = cos(x[6]); + v[51] = sin(x[6]); + v[52] = 4.89663865010925e-12 * v[51]; + v[53] = v[41] * v[50] + v[38] * v[52] + v[42] * v[51]; + v[54] = -v[51]; + v[55] = 4.89663865010925e-12 * v[50]; + v[42] = v[41] * v[54] + v[38] * v[55] + v[42] * v[50]; + y[132] = 0.02 * v[53] + 0.04 * v[42] + 0.08 * v[37] + v[29]; + v[56] = v[44] * v[50] + v[39] * v[52] + v[31] * v[51]; + v[31] = v[44] * v[54] + v[39] * v[55] + v[31] * v[50]; + y[133] = 0.02 * v[56] + 0.04 * v[31] + 0.08 * v[45] + v[46]; + v[52] = v[40] * v[50] + v[43] * v[52] + v[47] * v[51]; + v[55] = v[40] * v[54] + v[43] * v[55] + v[47] * v[50]; + y[134] = 0.02 * v[52] + 0.04 * v[55] + 0.08 * v[48] + v[49]; + y[136] = 0.04 * v[53] + 0.02 * v[42] + 0.08 * v[37] + v[29]; + y[137] = 0.04 * v[56] + 0.02 * v[31] + 0.08 * v[45] + v[46]; + y[138] = 0.04 * v[52] + 0.02 * v[55] + 0.08 * v[48] + v[49]; + y[140] = 0.04 * v[53] + 0.06 * v[42] + 0.085 * v[37] + v[29]; + y[141] = 0.04 * v[56] + 0.06 * v[31] + 0.085 * v[45] + v[46]; + y[142] = 0.04 * v[52] + 0.06 * v[55] + 0.085 * v[48] + v[49]; + y[144] = 0.06 * v[53] + 0.04 * v[42] + 0.085 * v[37] + v[29]; + y[145] = 0.06 * v[56] + 0.04 * v[31] + 0.085 * v[45] + v[46]; + y[146] = 0.06 * v[52] + 0.04 * v[55] + 0.085 * v[48] + v[49]; + y[148] = -0.0530330085889673 * v[53] + -0.0530330085890148 * v[42] + 0.117 * v[37] + v[29]; + y[149] = -0.0530330085889673 * v[56] + -0.0530330085890148 * v[31] + 0.117 * v[45] + v[46]; + y[150] = -0.0530330085889673 * v[52] + -0.0530330085890148 * v[55] + 0.117 * v[48] + v[49]; + y[152] = -0.0318198051533804 * v[53] + -0.0318198051534089 * v[42] + 0.117 * v[37] + v[29]; + y[153] = -0.0318198051533804 * v[56] + -0.0318198051534089 * v[31] + 0.117 * v[45] + v[46]; + y[154] = -0.0318198051533804 * v[52] + -0.0318198051534089 * v[55] + 0.117 * v[48] + v[49]; + y[156] = -0.0106066017177935 * v[53] + -0.010606601717803 * v[42] + 0.117 * v[37] + v[29]; + y[157] = -0.0106066017177935 * v[56] + -0.010606601717803 * v[31] + 0.117 * v[45] + v[46]; + y[158] = -0.0106066017177935 * v[52] + -0.010606601717803 * v[55] + 0.117 * v[48] + v[49]; + y[160] = 0.0106066017177935 * v[53] + 0.010606601717803 * v[42] + 0.117 * v[37] + v[29]; + y[161] = 0.0106066017177935 * v[56] + 0.010606601717803 * v[31] + 0.117 * v[45] + v[46]; + y[162] = 0.0106066017177935 * v[52] + 0.010606601717803 * v[55] + 0.117 * v[48] + v[49]; + y[164] = 0.0318198051533804 * v[53] + 0.0318198051534089 * v[42] + 0.117 * v[37] + v[29]; + y[165] = 0.0318198051533804 * v[56] + 0.0318198051534089 * v[31] + 0.117 * v[45] + v[46]; + y[166] = 0.0318198051533804 * v[52] + 0.0318198051534089 * v[55] + 0.117 * v[48] + v[49]; + y[168] = 0.0530330085889673 * v[53] + 0.0530330085890148 * v[42] + 0.117 * v[37] + v[29]; + y[169] = 0.0530330085889673 * v[56] + 0.0530330085890148 * v[31] + 0.117 * v[45] + v[46]; + y[170] = 0.0530330085889673 * v[52] + 0.0530330085890148 * v[55] + 0.117 * v[48] + v[49]; + y[172] = -0.0530330085889673 * v[53] + -0.0530330085890148 * v[42] + 0.137 * v[37] + v[29]; + y[173] = -0.0530330085889673 * v[56] + -0.0530330085890148 * v[31] + 0.137 * v[45] + v[46]; + y[174] = -0.0530330085889673 * v[52] + -0.0530330085890148 * v[55] + 0.137 * v[48] + v[49]; + y[176] = -0.0318198051533804 * v[53] + -0.0318198051534089 * v[42] + 0.137 * v[37] + v[29]; + y[177] = -0.0318198051533804 * v[56] + -0.0318198051534089 * v[31] + 0.137 * v[45] + v[46]; + y[178] = -0.0318198051533804 * v[52] + -0.0318198051534089 * v[55] + 0.137 * v[48] + v[49]; + y[180] = -0.0106066017177935 * v[53] + -0.010606601717803 * v[42] + 0.137 * v[37] + v[29]; + y[181] = -0.0106066017177935 * v[56] + -0.010606601717803 * v[31] + 0.137 * v[45] + v[46]; + y[182] = -0.0106066017177935 * v[52] + -0.010606601717803 * v[55] + 0.137 * v[48] + v[49]; + y[184] = 0.0106066017177935 * v[53] + 0.010606601717803 * v[42] + 0.137 * v[37] + v[29]; + y[185] = 0.0106066017177935 * v[56] + 0.010606601717803 * v[31] + 0.137 * v[45] + v[46]; + y[186] = 0.0106066017177935 * v[52] + 0.010606601717803 * v[55] + 0.137 * v[48] + v[49]; + y[188] = 0.0318198051533804 * v[53] + 0.0318198051534089 * v[42] + 0.137 * v[37] + v[29]; + y[189] = 0.0318198051533804 * v[56] + 0.0318198051534089 * v[31] + 0.137 * v[45] + v[46]; + y[190] = 0.0318198051533804 * v[52] + 0.0318198051534089 * v[55] + 0.137 * v[48] + v[49]; + y[192] = 0.0530330085889673 * v[53] + 0.0530330085890148 * v[42] + 0.137 * v[37] + v[29]; + y[193] = 0.0530330085889673 * v[56] + 0.0530330085890148 * v[31] + 0.137 * v[45] + v[46]; + y[194] = 0.0530330085889673 * v[52] + 0.0530330085890148 * v[55] + 0.137 * v[48] + v[49]; + y[196] = -0.0530330085889673 * v[53] + -0.0530330085890148 * v[42] + 0.157 * v[37] + v[29]; + y[197] = -0.0530330085889673 * v[56] + -0.0530330085890148 * v[31] + 0.157 * v[45] + v[46]; + y[198] = -0.0530330085889673 * v[52] + -0.0530330085890148 * v[55] + 0.157 * v[48] + v[49]; + y[200] = -0.0318198051533804 * v[53] + -0.0318198051534089 * v[42] + 0.157 * v[37] + v[29]; + y[201] = -0.0318198051533804 * v[56] + -0.0318198051534089 * v[31] + 0.157 * v[45] + v[46]; + y[202] = -0.0318198051533804 * v[52] + -0.0318198051534089 * v[55] + 0.157 * v[48] + v[49]; + y[204] = -0.0106066017177935 * v[53] + -0.010606601717803 * v[42] + 0.157 * v[37] + v[29]; + y[205] = -0.0106066017177935 * v[56] + -0.010606601717803 * v[31] + 0.157 * v[45] + v[46]; + y[206] = -0.0106066017177935 * v[52] + -0.010606601717803 * v[55] + 0.157 * v[48] + v[49]; + y[208] = 0.0106066017177935 * v[53] + 0.010606601717803 * v[42] + 0.157 * v[37] + v[29]; + y[209] = 0.0106066017177935 * v[56] + 0.010606601717803 * v[31] + 0.157 * v[45] + v[46]; + y[210] = 0.0106066017177935 * v[52] + 0.010606601717803 * v[55] + 0.157 * v[48] + v[49]; + y[212] = 0.0318198051533804 * v[53] + 0.0318198051534089 * v[42] + 0.157 * v[37] + v[29]; + y[213] = 0.0318198051533804 * v[56] + 0.0318198051534089 * v[31] + 0.157 * v[45] + v[46]; + y[214] = 0.0318198051533804 * v[52] + 0.0318198051534089 * v[55] + 0.157 * v[48] + v[49]; + y[216] = 0.0530330085889673 * v[53] + 0.0530330085890148 * v[42] + 0.157 * v[37] + v[29]; + y[217] = 0.0530330085889673 * v[56] + 0.0530330085890148 * v[31] + 0.157 * v[45] + v[46]; + y[218] = 0.0530330085889673 * v[52] + 0.0530330085890148 * v[55] + 0.157 * v[48] + v[49]; + y[220] = 0.0565685424948984 * v[53] + 0.0565685424949492 * v[42] + 0.1874 * v[37] + v[29]; + y[221] = 0.0565685424948984 * v[56] + 0.0565685424949492 * v[31] + 0.1874 * v[45] + v[46]; + y[222] = 0.0565685424948984 * v[52] + 0.0565685424949492 * v[55] + 0.1874 * v[48] + v[49]; + y[224] = 0.0516187950265948 * v[53] + 0.0516187950266411 * v[42] + 0.2094 * v[37] + v[29]; + y[225] = 0.0516187950265948 * v[56] + 0.0516187950266411 * v[31] + 0.2094 * v[45] + v[46]; + y[226] = 0.0516187950265948 * v[52] + 0.0516187950266411 * v[55] + 0.2094 * v[48] + v[49]; + y[228] = -0.0565685424948984 * v[53] + -0.0565685424949492 * v[42] + 0.1874 * v[37] + v[29]; + y[229] = -0.0565685424948984 * v[56] + -0.0565685424949492 * v[31] + 0.1874 * v[45] + v[46]; + y[230] = -0.0565685424948984 * v[52] + -0.0565685424949492 * v[55] + 0.1874 * v[48] + v[49]; + y[232] = -0.0516187950265948 * v[53] + -0.0516187950266411 * v[42] + 0.2094 * v[37] + v[29]; + y[233] = -0.0516187950265948 * v[56] + -0.0516187950266411 * v[31] + 0.2094 * v[45] + v[46]; + y[234] = -0.0516187950265948 * v[52] + -0.0516187950266411 * v[55] + 0.2094 * v[48] + v[49]; + y[240] = -0.0399999991059303 * v[1]; + y[241] = -0.0399999991059303 * v[2]; + y[244] = -0.0850000008940697 * v[7] + 0.0399999991059303 * v[1]; + y[245] = -0.0850000008940697 * v[6] + 0.0399999991059303 * v[2]; + y[246] = 0.333 + 1.95865541626434e-13 + -0.0850000008940697 * v[4]; + y[248] = + 0.0385857857763767 * v[19] + 0.0289393402636051 * v[15] + -0.0517677664756775 * v[8] + v[9]; + y[249] = + 0.0385857857763767 * v[5] + 0.0289393402636051 * v[14] + -0.0517677664756775 * v[10] + v[11]; + y[250] = + 0.0385857857763767 * v[18] + 0.0289393402636051 * v[3] + -0.0517677664756775 * v[12] + v[13]; + y[252] = + -0.041449997574091 * v[17] + 0.0492218732833862 * v[23] + 0.0289125014096498 * v[21] + v[24]; + y[253] = + -0.041449997574091 * v[25] + 0.0492218732833862 * v[26] + 0.0289125014096498 * v[20] + v[27]; + y[254] = + -0.041449997574091 * v[16] + 0.0492218732833862 * v[22] + 0.0289125014096498 * v[28] + v[0]; + y[256] = 0.0375000014901161 * v[34] + -0.109999999403954 * v[32] + y[116]; + y[257] = 0.0375000014901161 * v[35] + -0.109999999403954 * v[30] + y[117]; + y[258] = 0.0375000014901161 * v[33] + -0.109999999403954 * v[36] + y[118]; + y[260] = 0.0424309633672237 * v[41] + 0.0144476993009448 * v[38] + y[116]; + y[261] = 0.0424309633672237 * v[44] + 0.0144476993009448 * v[39] + y[117]; + y[262] = 0.0424309633672237 * v[40] + 0.0144476993009448 * v[43] + y[118]; + y[264] = + 0.0153005504980683 * v[53] + 0.0153005504980683 * v[42] + 0.0745901614427567 * v[37] + v[29]; + y[265] = + 0.0153005504980683 * v[56] + 0.0153005504980683 * v[31] + 0.0745901614427567 * v[45] + v[46]; + y[266] = + 0.0153005504980683 * v[52] + 0.0153005504980683 * v[55] + 0.0745901614427567 * v[48] + v[49]; + y[268] = -5.20417042793042e-18 * v[53] + -2.16840434497101e-18 * v[42] + + 0.129201397299767 * v[37] + v[29]; + y[269] = -5.20417042793042e-18 * v[56] + -2.16840434497101e-18 * v[31] + + 0.129201397299767 * v[45] + v[46]; + y[270] = -5.20417042793042e-18 * v[52] + -2.16840434497101e-18 * v[55] + + 0.129201397299767 * v[48] + v[49]; + y[272] = + 0.0540936700999737 * v[53] + 0.0540936700999737 * v[42] + 0.198400005698204 * v[37] + v[29]; + y[273] = + 0.0540936700999737 * v[56] + 0.0540936700999737 * v[31] + 0.198400005698204 * v[45] + v[46]; + y[274] = + 0.0540936700999737 * v[52] + 0.0540936700999737 * v[55] + 0.198400005698204 * v[48] + v[49]; + y[276] = + -0.0540936700999737 * v[53] + -0.0540936700999737 * v[42] + 0.198400005698204 * v[37] + v[29]; + y[277] = + -0.0540936700999737 * v[56] + -0.0540936700999737 * v[31] + 0.198400005698204 * v[45] + v[46]; + y[278] = + -0.0540936700999737 * v[52] + -0.0540936700999737 * v[55] + 0.198400005698204 * v[48] + v[49]; + // dependent variables without operations + y[0] = 0.; + y[1] = 0.; + y[2] = 0.05; + y[3] = 0.0799999982118607; + y[6] = 0.333; + y[7] = 0.0599999986588955; + y[10] = 0.333; + y[11] = 0.0599999986588955; + y[12] = 0.; + y[13] = 0.; + y[14] = 0.213; + y[15] = 0.0599999986588955; + y[16] = 0.; + y[17] = 0.; + y[18] = 0.163; + y[19] = 0.0599999986588955; + y[22] = 0.333000000000147; + y[23] = 0.0599999986588955; + y[26] = 0.333000000000392; + y[27] = 0.0599999986588955; + y[31] = 0.0599999986588955; + y[35] = 0.0599999986588955; + y[39] = 0.0599999986588955; + y[43] = 0.0500000007450581; + y[47] = 0.0549999997019768; + y[51] = 0.0549999997019768; + y[55] = 0.0599999986588955; + y[59] = 0.0549999997019768; + y[63] = 0.0549999997019768; + y[67] = 0.0549999997019768; + y[71] = 0.0599999986588955; + y[75] = 0.0599999986588955; + y[79] = 0.0599999986588955; + y[83] = 0.0500000007450581; + y[87] = 0.025000000372529; + y[91] = 0.025000000372529; + y[95] = 0.025000000372529; + y[99] = 0.025000000372529; + y[103] = 0.025000000372529; + y[107] = 0.025000000372529; + y[111] = 0.025000000372529; + y[115] = 0.025000000372529; + y[119] = 0.0500000007450581; + y[123] = 0.0500000007450581; + y[127] = 0.0520000010728836; + y[131] = 0.0500000007450581; + y[135] = 0.025000000372529; + y[139] = 0.025000000372529; + y[143] = 0.0199999995529652; + y[147] = 0.0199999995529652; + y[151] = 0.0280000008642673; + y[155] = 0.0280000008642673; + y[159] = 0.0280000008642673; + y[163] = 0.0280000008642673; + y[167] = 0.0280000008642673; + y[171] = 0.0280000008642673; + y[175] = 0.0260000005364418; + y[179] = 0.0260000005364418; + y[183] = 0.0260000005364418; + y[187] = 0.0260000005364418; + y[191] = 0.0260000005364418; + y[195] = 0.0260000005364418; + y[199] = 0.0240000002086163; + y[203] = 0.0240000002086163; + y[207] = 0.0240000002086163; + y[211] = 0.0240000002086163; + y[215] = 0.0240000002086163; + y[219] = 0.0240000002086163; + y[223] = 0.0120000001043081; + y[227] = 0.0120000001043081; + y[231] = 0.0120000001043081; + y[235] = 0.0120000001043081; + y[236] = 0.; + y[237] = 0.; + y[238] = 0.0500000007450581; + y[239] = 0.0799999982118607; + y[242] = 0.24799999910593; + y[243] = 0.153941467404366; + y[247] = 0.153941467404366; + y[251] = 0.128210678696632; + y[255] = 0.126465573906898; + y[259] = 0.176216393709183; + y[263] = 0.0948232412338257; + y[267] = 0.0721197500824928; + y[271] = 0.103986009955406; + y[275] = 0.0235433969646692; + y[279] = 0.0235433969646692; + + FloatVector dists; + FloatVector grads; + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[0], y[1], y[2], y[3]); + dists[0] = res.first; + grads[0] = res.second[0]; + grads[1] = res.second[1]; + grads[2] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[4], y[5], y[6], y[7]); + dists[1] = res.first; + grads[3] = res.second[0]; + grads[4] = res.second[1]; + grads[5] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[8], y[9], y[10], y[11]); + dists[2] = res.first; + grads[6] = res.second[0]; + grads[7] = res.second[1]; + grads[8] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[12], y[13], y[14], y[15]); + dists[3] = res.first; + grads[9] = res.second[0]; + grads[10] = res.second[1]; + grads[11] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[16], y[17], y[18], y[19]); + dists[4] = res.first; + grads[12] = res.second[0]; + grads[13] = res.second[1]; + grads[14] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[20], y[21], y[22], y[23]); + dists[5] = res.first; + grads[15] = res.second[0]; + grads[16] = res.second[1]; + grads[17] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[24], y[25], y[26], y[27]); + dists[6] = res.first; + grads[18] = res.second[0]; + grads[19] = res.second[1]; + grads[20] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[28], y[29], y[30], y[31]); + dists[7] = res.first; + grads[21] = res.second[0]; + grads[22] = res.second[1]; + grads[23] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[32], y[33], y[34], y[35]); + dists[8] = res.first; + grads[24] = res.second[0]; + grads[25] = res.second[1]; + grads[26] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[36], y[37], y[38], y[39]); + dists[9] = res.first; + grads[27] = res.second[0]; + grads[28] = res.second[1]; + grads[29] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[40], y[41], y[42], y[43]); + dists[10] = res.first; + grads[30] = res.second[0]; + grads[31] = res.second[1]; + grads[32] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[44], y[45], y[46], y[47]); + dists[11] = res.first; + grads[33] = res.second[0]; + grads[34] = res.second[1]; + grads[35] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[48], y[49], y[50], y[51]); + dists[12] = res.first; + grads[36] = res.second[0]; + grads[37] = res.second[1]; + grads[38] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[52], y[53], y[54], y[55]); + dists[13] = res.first; + grads[39] = res.second[0]; + grads[40] = res.second[1]; + grads[41] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[56], y[57], y[58], y[59]); + dists[14] = res.first; + grads[42] = res.second[0]; + grads[43] = res.second[1]; + grads[44] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[60], y[61], y[62], y[63]); + dists[15] = res.first; + grads[45] = res.second[0]; + grads[46] = res.second[1]; + grads[47] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[64], y[65], y[66], y[67]); + dists[16] = res.first; + grads[48] = res.second[0]; + grads[49] = res.second[1]; + grads[50] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[68], y[69], y[70], y[71]); + dists[17] = res.first; + grads[51] = res.second[0]; + grads[52] = res.second[1]; + grads[53] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[72], y[73], y[74], y[75]); + dists[18] = res.first; + grads[54] = res.second[0]; + grads[55] = res.second[1]; + grads[56] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[76], y[77], y[78], y[79]); + dists[19] = res.first; + grads[57] = res.second[0]; + grads[58] = res.second[1]; + grads[59] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[80], y[81], y[82], y[83]); + dists[20] = res.first; + grads[60] = res.second[0]; + grads[61] = res.second[1]; + grads[62] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[84], y[85], y[86], y[87]); + dists[21] = res.first; + grads[63] = res.second[0]; + grads[64] = res.second[1]; + grads[65] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[88], y[89], y[90], y[91]); + dists[22] = res.first; + grads[66] = res.second[0]; + grads[67] = res.second[1]; + grads[68] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[92], y[93], y[94], y[95]); + dists[23] = res.first; + grads[69] = res.second[0]; + grads[70] = res.second[1]; + grads[71] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[96], y[97], y[98], y[99]); + dists[24] = res.first; + grads[72] = res.second[0]; + grads[73] = res.second[1]; + grads[74] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[100], y[101], y[102], y[103]); + dists[25] = res.first; + grads[75] = res.second[0]; + grads[76] = res.second[1]; + grads[77] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[104], y[105], y[106], y[107]); + dists[26] = res.first; + grads[78] = res.second[0]; + grads[79] = res.second[1]; + grads[80] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[108], y[109], y[110], y[111]); + dists[27] = res.first; + grads[81] = res.second[0]; + grads[82] = res.second[1]; + grads[83] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[112], y[113], y[114], y[115]); + dists[28] = res.first; + grads[84] = res.second[0]; + grads[85] = res.second[1]; + grads[86] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[116], y[117], y[118], y[119]); + dists[29] = res.first; + grads[87] = res.second[0]; + grads[88] = res.second[1]; + grads[89] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[120], y[121], y[122], y[123]); + dists[30] = res.first; + grads[90] = res.second[0]; + grads[91] = res.second[1]; + grads[92] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[124], y[125], y[126], y[127]); + dists[31] = res.first; + grads[93] = res.second[0]; + grads[94] = res.second[1]; + grads[95] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[128], y[129], y[130], y[131]); + dists[32] = res.first; + grads[96] = res.second[0]; + grads[97] = res.second[1]; + grads[98] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[132], y[133], y[134], y[135]); + dists[33] = res.first; + grads[99] = res.second[0]; + grads[100] = res.second[1]; + grads[101] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[136], y[137], y[138], y[139]); + dists[34] = res.first; + grads[102] = res.second[0]; + grads[103] = res.second[1]; + grads[104] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[140], y[141], y[142], y[143]); + dists[35] = res.first; + grads[105] = res.second[0]; + grads[106] = res.second[1]; + grads[107] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[144], y[145], y[146], y[147]); + dists[36] = res.first; + grads[108] = res.second[0]; + grads[109] = res.second[1]; + grads[110] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[148], y[149], y[150], y[151]); + dists[37] = res.first; + grads[111] = res.second[0]; + grads[112] = res.second[1]; + grads[113] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[152], y[153], y[154], y[155]); + dists[38] = res.first; + grads[114] = res.second[0]; + grads[115] = res.second[1]; + grads[116] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[156], y[157], y[158], y[159]); + dists[39] = res.first; + grads[117] = res.second[0]; + grads[118] = res.second[1]; + grads[119] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[160], y[161], y[162], y[163]); + dists[40] = res.first; + grads[120] = res.second[0]; + grads[121] = res.second[1]; + grads[122] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[164], y[165], y[166], y[167]); + dists[41] = res.first; + grads[123] = res.second[0]; + grads[124] = res.second[1]; + grads[125] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[168], y[169], y[170], y[171]); + dists[42] = res.first; + grads[126] = res.second[0]; + grads[127] = res.second[1]; + grads[128] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[172], y[173], y[174], y[175]); + dists[43] = res.first; + grads[129] = res.second[0]; + grads[130] = res.second[1]; + grads[131] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[176], y[177], y[178], y[179]); + dists[44] = res.first; + grads[132] = res.second[0]; + grads[133] = res.second[1]; + grads[134] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[180], y[181], y[182], y[183]); + dists[45] = res.first; + grads[135] = res.second[0]; + grads[136] = res.second[1]; + grads[137] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[184], y[185], y[186], y[187]); + dists[46] = res.first; + grads[138] = res.second[0]; + grads[139] = res.second[1]; + grads[140] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[188], y[189], y[190], y[191]); + dists[47] = res.first; + grads[141] = res.second[0]; + grads[142] = res.second[1]; + grads[143] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[192], y[193], y[194], y[195]); + dists[48] = res.first; + grads[144] = res.second[0]; + grads[145] = res.second[1]; + grads[146] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[196], y[197], y[198], y[199]); + dists[49] = res.first; + grads[147] = res.second[0]; + grads[148] = res.second[1]; + grads[149] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[200], y[201], y[202], y[203]); + dists[50] = res.first; + grads[150] = res.second[0]; + grads[151] = res.second[1]; + grads[152] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[204], y[205], y[206], y[207]); + dists[51] = res.first; + grads[153] = res.second[0]; + grads[154] = res.second[1]; + grads[155] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[208], y[209], y[210], y[211]); + dists[52] = res.first; + grads[156] = res.second[0]; + grads[157] = res.second[1]; + grads[158] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[212], y[213], y[214], y[215]); + dists[53] = res.first; + grads[159] = res.second[0]; + grads[160] = res.second[1]; + grads[161] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[216], y[217], y[218], y[219]); + dists[54] = res.first; + grads[162] = res.second[0]; + grads[163] = res.second[1]; + grads[164] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[220], y[221], y[222], y[223]); + dists[55] = res.first; + grads[165] = res.second[0]; + grads[166] = res.second[1]; + grads[167] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[224], y[225], y[226], y[227]); + dists[56] = res.first; + grads[168] = res.second[0]; + grads[169] = res.second[1]; + grads[170] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[228], y[229], y[230], y[231]); + dists[57] = res.first; + grads[171] = res.second[0]; + grads[172] = res.second[1]; + grads[173] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[232], y[233], y[234], y[235]); + dists[58] = res.first; + grads[174] = res.second[0]; + grads[175] = res.second[1]; + grads[176] = res.second[2]; + } + + return {dists, grads}; + } + + template + inline static void d_collision_d_q( + const ConfigurationBlock &q_in, + const FloatVector &gradients, + ConfigurationBlock &out) noexcept + { + // Generated code expects a single input array 'x' containing + // [q_0, ..., q_n, gx_0, gy_0, gz_0, ..., gx_m, gy_m, gz_m] + std::array, 7 + 59 * 3 + 11 * 3> x; + + // Copy q + for (std::size_t i = 0; i < 7; ++i) + { + x[i] = q_in[i]; + } + + // Copy gradients + for (std::size_t i = 0; i < 59 * 3; ++i) + { + x[7 + i] = gradients[i]; + } + + std::array, 232> v; + std::array, 7> y; + + v[0] = sin(x[0]); + v[1] = -v[0]; + v[2] = cos(x[0]); + v[3] = 0.03 * v[1]; + v[4] = 0.03 * v[2]; + v[5] = v[3] * x[23] - v[4] * x[22]; + v[6] = 0.08 * v[1]; + v[7] = 0.08 * v[2]; + v[8] = v[6] * x[26] - v[7] * x[25]; + v[9] = sin(x[1]); + v[10] = -v[9]; + v[11] = cos(x[1]); + v[12] = 4.89663865010925e-12 * v[11]; + v[13] = v[2] * v[10] + v[1] * v[12]; + v[14] = -0.12 * v[13]; + v[12] = v[0] * v[10] + v[2] * v[12]; + v[10] = -0.12 * v[12]; + v[15] = v[14] * x[29] - v[10] * x[28]; + v[16] = -0.17 * v[13]; + v[17] = -0.17 * v[12]; + v[18] = v[16] * x[32] - v[17] * x[31]; + v[19] = -0.316 * v[12]; + v[20] = -0.316 * v[13]; + v[21] = -1. * v[13] + 4.89663865010925e-12 * v[1]; + v[22] = -0.1 * v[21]; + v[23] = -1. * v[12] + 4.89663865010925e-12 * v[2]; + v[24] = -0.1 * v[23]; + v[25] = v[22] * x[35] - v[24] * x[34]; + v[26] = -0.06 * v[21]; + v[27] = -0.06 * v[23]; + v[28] = v[26] * x[38] - v[27] * x[37]; + v[29] = 4.89663865010925e-12 * v[9]; + v[30] = v[2] * v[11] + v[1] * v[29]; + v[31] = cos(x[2]); + v[32] = sin(x[2]); + v[33] = 4.89663865010925e-12 * v[32]; + v[34] = v[30] * v[31] + v[13] * v[33] + v[1] * v[32]; + v[35] = -v[32]; + v[36] = 4.89663865010925e-12 * v[31]; + v[30] = v[30] * v[35] + v[13] * v[36] + v[1] * v[31]; + v[37] = 0.08 * v[34] + 0.06 * v[30]; + v[29] = v[0] * v[11] + v[2] * v[29]; + v[0] = v[29] * v[31] + v[12] * v[33] + v[2] * v[32]; + v[29] = v[29] * v[35] + v[12] * v[36] + v[2] * v[31]; + v[38] = 0.08 * v[0] + 0.06 * v[29]; + v[39] = v[37] * x[41] - v[38] * x[40]; + v[40] = 0.08 * v[34] + 0.02 * v[30]; + v[41] = 0.08 * v[0] + 0.02 * v[29]; + v[42] = v[40] * x[44] - v[41] * x[43]; + v[43] = v[19] + 0.0825 * v[0]; + v[44] = v[20] + 0.0825 * v[34]; + v[45] = cos(x[3]); + v[46] = sin(x[3]); + v[47] = 4.89663865010925e-12 * v[46]; + v[48] = v[34] * v[45] + v[30] * v[47] + v[21] * v[46]; + v[49] = -v[46]; + v[50] = 4.89663865010925e-12 * v[45]; + v[51] = v[34] * v[49] + v[30] * v[50] + v[21] * v[45]; + v[52] = -0.08 * v[48] + 0.095 * v[51]; + v[53] = v[0] * v[45] + v[29] * v[47] + v[23] * v[46]; + v[54] = v[0] * v[49] + v[29] * v[50] + v[23] * v[45]; + v[55] = -0.08 * v[53] + 0.095 * v[54]; + v[56] = v[52] * x[47] - v[55] * x[46]; + v[57] = -1. * v[30] + 4.89663865010925e-12 * v[21]; + v[58] = 0.02 * v[57]; + v[59] = -1. * v[29] + 4.89663865010925e-12 * v[23]; + v[60] = 0.02 * v[59]; + v[61] = v[58] * x[50] - v[60] * x[49]; + v[62] = 0.06 * v[57]; + v[63] = 0.06 * v[59]; + v[64] = v[62] * x[53] - v[63] * x[52]; + v[65] = -0.08 * v[48] + 0.06 * v[51]; + v[66] = -0.08 * v[53] + 0.06 * v[54]; + v[67] = v[65] * x[56] - v[66] * x[55]; + v[68] = v[43] + -0.0825 * v[53] + 0.384 * v[54]; + v[69] = v[44] + -0.0825 * v[48] + 0.384 * v[51]; + v[70] = sin(x[4]); + v[71] = -v[70]; + v[72] = cos(x[4]); + v[73] = 4.89663865010925e-12 * v[72]; + v[74] = -1. * v[72]; + v[75] = v[48] * v[71] + v[51] * v[73] + v[57] * v[74]; + v[76] = 0.055 * v[75]; + v[77] = v[53] * v[71] + v[54] * v[73] + v[59] * v[74]; + v[78] = 0.055 * v[77]; + v[79] = v[76] * x[59] - v[78] * x[58]; + v[80] = 0.075 * v[75]; + v[81] = 0.075 * v[77]; + v[82] = v[80] * x[62] - v[81] * x[61]; + v[83] = v[51] + 4.89663865010925e-12 * v[57]; + v[84] = -0.22 * v[83]; + v[85] = v[54] + 4.89663865010925e-12 * v[59]; + v[86] = -0.22 * v[85]; + v[87] = v[84] * x[65] - v[86] * x[64]; + v[88] = 0.05 * v[75] + -0.18 * v[83]; + v[89] = 0.05 * v[77] + -0.18 * v[85]; + v[90] = v[88] * x[68] - v[89] * x[67]; + v[91] = 4.89663865010925e-12 * v[70]; + v[70] = -1. * v[70]; + v[92] = v[48] * v[72] + v[51] * v[91] + v[57] * v[70]; + v[93] = 0.01 * v[92] + 0.08 * v[75] + -0.14 * v[83]; + v[94] = v[53] * v[72] + v[54] * v[91] + v[59] * v[70]; + v[95] = 0.01 * v[94] + 0.08 * v[77] + -0.14 * v[85]; + v[96] = v[93] * x[71] - v[95] * x[70]; + v[97] = 0.01 * v[92] + 0.085 * v[75] + -0.11 * v[83]; + v[98] = 0.01 * v[94] + 0.085 * v[77] + -0.11 * v[85]; + v[99] = v[97] * x[74] - v[98] * x[73]; + v[100] = 0.01 * v[92] + 0.09 * v[75] + -0.08 * v[83]; + v[101] = 0.01 * v[94] + 0.09 * v[77] + -0.08 * v[85]; + v[102] = v[100] * x[77] - v[101] * x[76]; + v[103] = 0.01 * v[92] + 0.095 * v[75] + -0.05 * v[83]; + v[104] = 0.01 * v[94] + 0.095 * v[77] + -0.05 * v[85]; + v[105] = v[103] * x[80] - v[104] * x[79]; + v[106] = -0.01 * v[92] + 0.08 * v[75] + -0.14 * v[83]; + v[107] = -0.01 * v[94] + 0.08 * v[77] + -0.14 * v[85]; + v[108] = v[106] * x[83] - v[107] * x[82]; + v[109] = -0.01 * v[92] + 0.085 * v[75] + -0.11 * v[83]; + v[110] = -0.01 * v[94] + 0.085 * v[77] + -0.11 * v[85]; + v[111] = v[109] * x[86] - v[110] * x[85]; + v[112] = -0.01 * v[92] + 0.09 * v[75] + -0.08 * v[83]; + v[113] = -0.01 * v[94] + 0.09 * v[77] + -0.08 * v[85]; + v[114] = v[112] * x[89] - v[113] * x[88]; + v[115] = -0.01 * v[92] + 0.095 * v[75] + -0.05 * v[83]; + v[116] = -0.01 * v[94] + 0.095 * v[77] + -0.05 * v[85]; + v[117] = v[115] * x[92] - v[116] * x[91]; + v[118] = cos(x[5]); + v[119] = sin(x[5]); + v[120] = 4.89663865010925e-12 * v[119]; + v[121] = v[92] * v[118] + v[75] * v[120] + v[83] * v[119]; + v[122] = -v[119]; + v[123] = 4.89663865010925e-12 * v[118]; + v[92] = v[92] * v[122] + v[75] * v[123] + v[83] * v[118]; + v[124] = 0.08 * v[121] + -0.01 * v[92]; + v[125] = v[94] * v[118] + v[77] * v[120] + v[85] * v[119]; + v[94] = v[94] * v[122] + v[77] * v[123] + v[85] * v[118]; + v[126] = 0.08 * v[125] + -0.01 * v[94]; + v[127] = v[124] * x[98] - v[126] * x[97]; + v[128] = 0.08 * v[121] + 0.035 * v[92]; + v[129] = 0.08 * v[125] + 0.035 * v[94]; + v[130] = v[128] * x[101] - v[129] * x[100]; + v[131] = v[68] + 0.088 * v[125]; + v[132] = v[69] + 0.088 * v[121]; + v[133] = -1. * v[75] + 4.89663865010925e-12 * v[83]; + v[134] = -1. * v[92] + 4.89663865010925e-12 * v[133]; + v[135] = 0.07 * v[134]; + v[136] = -1. * v[77] + 4.89663865010925e-12 * v[85]; + v[137] = -1. * v[94] + 4.89663865010925e-12 * v[136]; + v[138] = 0.07 * v[137]; + v[139] = v[135] * x[104] - v[138] * x[103]; + v[140] = cos(x[6]); + v[141] = sin(x[6]); + v[142] = 4.89663865010925e-12 * v[141]; + v[143] = v[121] * v[140] + v[92] * v[142] + v[133] * v[141]; + v[144] = -v[141]; + v[145] = 4.89663865010925e-12 * v[140]; + v[133] = v[121] * v[144] + v[92] * v[145] + v[133] * v[140]; + v[146] = 0.02 * v[143] + 0.04 * v[133] + 0.08 * v[134]; + v[147] = v[125] * v[140] + v[94] * v[142] + v[136] * v[141]; + v[136] = v[125] * v[144] + v[94] * v[145] + v[136] * v[140]; + v[148] = 0.02 * v[147] + 0.04 * v[136] + 0.08 * v[137]; + v[149] = v[146] * x[107] - v[148] * x[106]; + v[150] = 0.04 * v[143] + 0.02 * v[133] + 0.08 * v[134]; + v[151] = 0.04 * v[147] + 0.02 * v[136] + 0.08 * v[137]; + v[152] = v[150] * x[110] - v[151] * x[109]; + v[153] = 0.04 * v[143] + 0.06 * v[133] + 0.085 * v[134]; + v[154] = 0.04 * v[147] + 0.06 * v[136] + 0.085 * v[137]; + v[155] = v[153] * x[113] - v[154] * x[112]; + v[156] = 0.06 * v[143] + 0.04 * v[133] + 0.085 * v[134]; + v[157] = 0.06 * v[147] + 0.04 * v[136] + 0.085 * v[137]; + v[158] = v[156] * x[116] - v[157] * x[115]; + v[159] = -0.0530330085889673 * v[143] + -0.0530330085890148 * v[133] + 0.117 * v[134]; + v[160] = -0.0530330085889673 * v[147] + -0.0530330085890148 * v[136] + 0.117 * v[137]; + v[161] = v[159] * x[119] - v[160] * x[118]; + v[162] = -0.0318198051533804 * v[143] + -0.0318198051534089 * v[133] + 0.117 * v[134]; + v[163] = -0.0318198051533804 * v[147] + -0.0318198051534089 * v[136] + 0.117 * v[137]; + v[164] = v[162] * x[122] - v[163] * x[121]; + v[165] = -0.0106066017177935 * v[143] + -0.010606601717803 * v[133] + 0.117 * v[134]; + v[166] = -0.0106066017177935 * v[147] + -0.010606601717803 * v[136] + 0.117 * v[137]; + v[167] = v[165] * x[125] - v[166] * x[124]; + v[168] = 0.0106066017177935 * v[143] + 0.010606601717803 * v[133] + 0.117 * v[134]; + v[169] = 0.0106066017177935 * v[147] + 0.010606601717803 * v[136] + 0.117 * v[137]; + v[170] = v[168] * x[128] - v[169] * x[127]; + v[171] = 0.0318198051533804 * v[143] + 0.0318198051534089 * v[133] + 0.117 * v[134]; + v[172] = 0.0318198051533804 * v[147] + 0.0318198051534089 * v[136] + 0.117 * v[137]; + v[173] = v[171] * x[131] - v[172] * x[130]; + v[174] = 0.0530330085889673 * v[143] + 0.0530330085890148 * v[133] + 0.117 * v[134]; + v[175] = 0.0530330085889673 * v[147] + 0.0530330085890148 * v[136] + 0.117 * v[137]; + v[176] = v[174] * x[134] - v[175] * x[133]; + v[177] = -0.0530330085889673 * v[143] + -0.0530330085890148 * v[133] + 0.137 * v[134]; + v[178] = -0.0530330085889673 * v[147] + -0.0530330085890148 * v[136] + 0.137 * v[137]; + v[179] = v[177] * x[137] - v[178] * x[136]; + v[180] = -0.0318198051533804 * v[143] + -0.0318198051534089 * v[133] + 0.137 * v[134]; + v[181] = -0.0318198051533804 * v[147] + -0.0318198051534089 * v[136] + 0.137 * v[137]; + v[182] = v[180] * x[140] - v[181] * x[139]; + v[183] = -0.0106066017177935 * v[143] + -0.010606601717803 * v[133] + 0.137 * v[134]; + v[184] = -0.0106066017177935 * v[147] + -0.010606601717803 * v[136] + 0.137 * v[137]; + v[185] = v[183] * x[143] - v[184] * x[142]; + v[186] = 0.0106066017177935 * v[143] + 0.010606601717803 * v[133] + 0.137 * v[134]; + v[187] = 0.0106066017177935 * v[147] + 0.010606601717803 * v[136] + 0.137 * v[137]; + v[188] = v[186] * x[146] - v[187] * x[145]; + v[189] = 0.0318198051533804 * v[143] + 0.0318198051534089 * v[133] + 0.137 * v[134]; + v[190] = 0.0318198051533804 * v[147] + 0.0318198051534089 * v[136] + 0.137 * v[137]; + v[191] = v[189] * x[149] - v[190] * x[148]; + v[192] = 0.0530330085889673 * v[143] + 0.0530330085890148 * v[133] + 0.137 * v[134]; + v[193] = 0.0530330085889673 * v[147] + 0.0530330085890148 * v[136] + 0.137 * v[137]; + v[194] = v[192] * x[152] - v[193] * x[151]; + v[195] = -0.0530330085889673 * v[143] + -0.0530330085890148 * v[133] + 0.157 * v[134]; + v[196] = -0.0530330085889673 * v[147] + -0.0530330085890148 * v[136] + 0.157 * v[137]; + v[197] = v[195] * x[155] - v[196] * x[154]; + v[198] = -0.0318198051533804 * v[143] + -0.0318198051534089 * v[133] + 0.157 * v[134]; + v[199] = -0.0318198051533804 * v[147] + -0.0318198051534089 * v[136] + 0.157 * v[137]; + v[200] = v[198] * x[158] - v[199] * x[157]; + v[201] = -0.0106066017177935 * v[143] + -0.010606601717803 * v[133] + 0.157 * v[134]; + v[202] = -0.0106066017177935 * v[147] + -0.010606601717803 * v[136] + 0.157 * v[137]; + v[203] = v[201] * x[161] - v[202] * x[160]; + v[204] = 0.0106066017177935 * v[143] + 0.010606601717803 * v[133] + 0.157 * v[134]; + v[205] = 0.0106066017177935 * v[147] + 0.010606601717803 * v[136] + 0.157 * v[137]; + v[206] = v[204] * x[164] - v[205] * x[163]; + v[207] = 0.0318198051533804 * v[143] + 0.0318198051534089 * v[133] + 0.157 * v[134]; + v[208] = 0.0318198051533804 * v[147] + 0.0318198051534089 * v[136] + 0.157 * v[137]; + v[209] = v[207] * x[167] - v[208] * x[166]; + v[210] = 0.0530330085889673 * v[143] + 0.0530330085890148 * v[133] + 0.157 * v[134]; + v[211] = 0.0530330085889673 * v[147] + 0.0530330085890148 * v[136] + 0.157 * v[137]; + v[212] = v[210] * x[170] - v[211] * x[169]; + v[213] = 0.0565685424948984 * v[143] + 0.0565685424949492 * v[133] + 0.1874 * v[134]; + v[214] = 0.0565685424948984 * v[147] + 0.0565685424949492 * v[136] + 0.1874 * v[137]; + v[215] = v[213] * x[173] - v[214] * x[172]; + v[216] = 0.0516187950265948 * v[143] + 0.0516187950266411 * v[133] + 0.2094 * v[134]; + v[217] = 0.0516187950265948 * v[147] + 0.0516187950266411 * v[136] + 0.2094 * v[137]; + v[218] = v[216] * x[176] - v[217] * x[175]; + v[219] = -0.0565685424948984 * v[143] + -0.0565685424949492 * v[133] + 0.1874 * v[134]; + v[220] = -0.0565685424948984 * v[147] + -0.0565685424949492 * v[136] + 0.1874 * v[137]; + v[221] = v[219] * x[179] - v[220] * x[178]; + v[222] = -0.0516187950265948 * v[143] + -0.0516187950266411 * v[133] + 0.2094 * v[134]; + v[223] = -0.0516187950265948 * v[147] + -0.0516187950266411 * v[136] + 0.2094 * v[137]; + v[224] = v[222] * x[182] - v[223] * x[181]; + v[13] = -0.0850000008940697 * v[13] + 0.0399999991059303 * v[1]; + v[12] = -0.0850000008940697 * v[12] + 0.0399999991059303 * v[2]; + v[225] = v[13] * x[191] - v[12] * x[190]; + v[30] = 0.0385857857763767 * v[34] + 0.0289393402636051 * v[30] + -0.0517677664756775 * v[21]; + v[29] = 0.0385857857763767 * v[0] + 0.0289393402636051 * v[29] + -0.0517677664756775 * v[23]; + v[0] = v[30] * x[194] - v[29] * x[193]; + v[57] = -0.041449997574091 * v[48] + 0.0492218732833862 * v[51] + 0.0289125014096498 * v[57]; + v[59] = -0.041449997574091 * v[53] + 0.0492218732833862 * v[54] + 0.0289125014096498 * v[59]; + v[54] = v[57] * x[197] - v[59] * x[196]; + v[83] = 0.0375000014901161 * v[75] + -0.109999999403954 * v[83]; + v[85] = 0.0375000014901161 * v[77] + -0.109999999403954 * v[85]; + v[77] = v[83] * x[200] - v[85] * x[199]; + v[92] = 0.0424309633672237 * v[121] + 0.0144476993009448 * v[92]; + v[94] = 0.0424309633672237 * v[125] + 0.0144476993009448 * v[94]; + v[125] = v[92] * x[203] - v[94] * x[202]; + v[121] = 0.0153005504980683 * v[143] + 0.0153005504980683 * v[133] + 0.0745901614427567 * v[134]; + v[75] = 0.0153005504980683 * v[147] + 0.0153005504980683 * v[136] + 0.0745901614427567 * v[137]; + v[53] = v[121] * x[206] - v[75] * x[205]; + v[51] = + -5.20417042793042e-18 * v[143] + -2.16840434497101e-18 * v[133] + 0.129201397299767 * v[134]; + v[48] = + -5.20417042793042e-18 * v[147] + -2.16840434497101e-18 * v[136] + 0.129201397299767 * v[137]; + v[23] = v[51] * x[209] - v[48] * x[208]; + v[34] = 0.0540936700999737 * v[143] + 0.0540936700999737 * v[133] + 0.198400005698204 * v[134]; + v[21] = 0.0540936700999737 * v[147] + 0.0540936700999737 * v[136] + 0.198400005698204 * v[137]; + v[226] = v[34] * x[212] - v[21] * x[211]; + v[133] = -0.0540936700999737 * v[143] + -0.0540936700999737 * v[133] + 0.198400005698204 * v[134]; + v[136] = -0.0540936700999737 * v[147] + -0.0540936700999737 * v[136] + 0.198400005698204 * v[137]; + v[147] = v[133] * x[215] - v[136] * x[214]; + y[0] = + -0.08 * v[1] * x[11] - -0.08 * v[2] * x[10] + -0.03 * v[1] * x[14] - -0.03 * v[2] * x[13] + + v[5] + v[8] + v[15] + v[18] + (0. - v[19]) * x[34] + (0. - (0. - v[20])) * x[35] + v[25] + + (0. - v[19]) * x[37] + (0. - (0. - v[20])) * x[38] + v[28] + (0. - v[19]) * x[40] + + (0. - (0. - v[20])) * x[41] + v[39] + (0. - v[19]) * x[43] + (0. - (0. - v[20])) * x[44] + + v[42] + (0. - v[43]) * x[46] + (0. - (0. - v[44])) * x[47] + v[56] + (0. - v[43]) * x[49] + + (0. - (0. - v[44])) * x[50] + v[61] + (0. - v[43]) * x[52] + (0. - (0. - v[44])) * x[53] + + v[64] + (0. - v[43]) * x[55] + (0. - (0. - v[44])) * x[56] + v[67] + (0. - v[68]) * x[58] + + (0. - (0. - v[69])) * x[59] + v[79] + (0. - v[68]) * x[61] + (0. - (0. - v[69])) * x[62] + + v[82] + (0. - v[68]) * x[64] + (0. - (0. - v[69])) * x[65] + v[87] + (0. - v[68]) * x[67] + + (0. - (0. - v[69])) * x[68] + v[90] + (0. - v[68]) * x[70] + (0. - (0. - v[69])) * x[71] + + v[96] + (0. - v[68]) * x[73] + (0. - (0. - v[69])) * x[74] + v[99] + (0. - v[68]) * x[76] + + (0. - (0. - v[69])) * x[77] + v[102] + (0. - v[68]) * x[79] + (0. - (0. - v[69])) * x[80] + + v[105] + (0. - v[68]) * x[82] + (0. - (0. - v[69])) * x[83] + v[108] + (0. - v[68]) * x[85] + + (0. - (0. - v[69])) * x[86] + v[111] + (0. - v[68]) * x[88] + (0. - (0. - v[69])) * x[89] + + v[114] + (0. - v[68]) * x[91] + (0. - (0. - v[69])) * x[92] + v[117] + (0. - v[68]) * x[94] + + (0. - (0. - v[69])) * x[95] + (0. - v[68]) * x[97] + (0. - (0. - v[69])) * x[98] + v[127] + + (0. - v[68]) * x[100] + (0. - (0. - v[69])) * x[101] + v[130] + (0. - v[131]) * x[103] + + (0. - (0. - v[132])) * x[104] + v[139] + (0. - v[131]) * x[106] + + (0. - (0. - v[132])) * x[107] + v[149] + (0. - v[131]) * x[109] + + (0. - (0. - v[132])) * x[110] + v[152] + (0. - v[131]) * x[112] + + (0. - (0. - v[132])) * x[113] + v[155] + (0. - v[131]) * x[115] + + (0. - (0. - v[132])) * x[116] + v[158] + (0. - v[131]) * x[118] + + (0. - (0. - v[132])) * x[119] + v[161] + (0. - v[131]) * x[121] + + (0. - (0. - v[132])) * x[122] + v[164] + (0. - v[131]) * x[124] + + (0. - (0. - v[132])) * x[125] + v[167] + (0. - v[131]) * x[127] + + (0. - (0. - v[132])) * x[128] + v[170] + (0. - v[131]) * x[130] + + (0. - (0. - v[132])) * x[131] + v[173] + (0. - v[131]) * x[133] + + (0. - (0. - v[132])) * x[134] + v[176] + (0. - v[131]) * x[136] + + (0. - (0. - v[132])) * x[137] + v[179] + (0. - v[131]) * x[139] + + (0. - (0. - v[132])) * x[140] + v[182] + (0. - v[131]) * x[142] + + (0. - (0. - v[132])) * x[143] + v[185] + (0. - v[131]) * x[145] + + (0. - (0. - v[132])) * x[146] + v[188] + (0. - v[131]) * x[148] + + (0. - (0. - v[132])) * x[149] + v[191] + (0. - v[131]) * x[151] + + (0. - (0. - v[132])) * x[152] + v[194] + (0. - v[131]) * x[154] + + (0. - (0. - v[132])) * x[155] + v[197] + (0. - v[131]) * x[157] + + (0. - (0. - v[132])) * x[158] + v[200] + (0. - v[131]) * x[160] + + (0. - (0. - v[132])) * x[161] + v[203] + (0. - v[131]) * x[163] + + (0. - (0. - v[132])) * x[164] + v[206] + (0. - v[131]) * x[166] + + (0. - (0. - v[132])) * x[167] + v[209] + (0. - v[131]) * x[169] + + (0. - (0. - v[132])) * x[170] + v[212] + (0. - v[131]) * x[172] + + (0. - (0. - v[132])) * x[173] + v[215] + (0. - v[131]) * x[175] + + (0. - (0. - v[132])) * x[176] + v[218] + (0. - v[131]) * x[178] + + (0. - (0. - v[132])) * x[179] + v[221] + (0. - v[131]) * x[181] + + (0. - (0. - v[132])) * x[182] + v[224] + -0.0399999991059303 * v[1] * x[188] - + -0.0399999991059303 * v[2] * x[187] + v[225] + (0. - v[19]) * x[193] + + (0. - (0. - v[20])) * x[194] + v[0] + (0. - v[43]) * x[196] + (0. - (0. - v[44])) * x[197] + + v[54] + (0. - v[68]) * x[199] + (0. - (0. - v[69])) * x[200] + v[77] + (0. - v[68]) * x[202] + + (0. - (0. - v[69])) * x[203] + v[125] + (0. - v[131]) * x[205] + + (0. - (0. - v[132])) * x[206] + v[53] + (0. - v[131]) * x[208] + + (0. - (0. - v[132])) * x[209] + v[23] + (0. - v[131]) * x[211] + + (0. - (0. - v[132])) * x[212] + v[226] + (0. - v[131]) * x[214] + + (0. - (0. - v[132])) * x[215] + v[147]; + v[2] = cos(x[0]); + v[1] = 0. - 0.333 * v[2]; + v[137] = sin(x[0]); + v[143] = -v[137]; + v[134] = 0.333 * v[143]; + v[11] = -1. * v[11]; + v[227] = -0.12 * v[11]; + v[228] = -0.17 * v[11]; + v[229] = 0.333 + -0.316 * v[11]; + v[230] = 2.39770700697438e-23 + -1. * v[11]; + v[231] = -0.1 * v[230]; + v[24] = v[24] * x[36] - v[231] * x[35]; + v[231] = v[231] * x[34] - v[22] * x[36]; + v[22] = -0.06 * v[230]; + v[27] = v[27] * x[39] - v[22] * x[38]; + v[22] = v[22] * x[37] - v[26] * x[39]; + v[9] = -1. * v[9]; + v[33] = v[9] * v[31] + v[11] * v[33] + 4.89663865010925e-12 * v[32]; + v[9] = v[9] * v[35] + v[11] * v[36] + 4.89663865010925e-12 * v[31]; + v[36] = 0.08 * v[33] + 0.06 * v[9]; + v[38] = v[38] * x[42] - v[36] * x[41]; + v[36] = v[36] * x[40] - v[37] * x[42]; + v[37] = 0.08 * v[33] + 0.02 * v[9]; + v[41] = v[41] * x[45] - v[37] * x[44]; + v[37] = v[37] * x[43] - v[40] * x[45]; + v[40] = v[229] + 0.0825 * v[33]; + v[47] = v[33] * v[45] + v[9] * v[47] + v[230] * v[46]; + v[50] = v[33] * v[49] + v[9] * v[50] + v[230] * v[45]; + v[49] = -0.08 * v[47] + 0.095 * v[50]; + v[55] = v[55] * x[48] - v[49] * x[47]; + v[49] = v[49] * x[46] - v[52] * x[48]; + v[52] = -1. * v[9] + 4.89663865010925e-12 * v[230]; + v[45] = 0.02 * v[52]; + v[60] = v[60] * x[51] - v[45] * x[50]; + v[45] = v[45] * x[49] - v[58] * x[51]; + v[58] = 0.06 * v[52]; + v[63] = v[63] * x[54] - v[58] * x[53]; + v[58] = v[58] * x[52] - v[62] * x[54]; + v[62] = -0.08 * v[47] + 0.06 * v[50]; + v[66] = v[66] * x[57] - v[62] * x[56]; + v[62] = v[62] * x[55] - v[65] * x[57]; + v[65] = v[40] + -0.0825 * v[47] + 0.384 * v[50]; + v[74] = v[47] * v[71] + v[50] * v[73] + v[52] * v[74]; + v[73] = 0.055 * v[74]; + v[78] = v[78] * x[60] - v[73] * x[59]; + v[73] = v[73] * x[58] - v[76] * x[60]; + v[76] = 0.075 * v[74]; + v[81] = v[81] * x[63] - v[76] * x[62]; + v[76] = v[76] * x[61] - v[80] * x[63]; + v[80] = v[50] + 4.89663865010925e-12 * v[52]; + v[71] = -0.22 * v[80]; + v[86] = v[86] * x[66] - v[71] * x[65]; + v[71] = v[71] * x[64] - v[84] * x[66]; + v[84] = 0.05 * v[74] + -0.18 * v[80]; + v[89] = v[89] * x[69] - v[84] * x[68]; + v[84] = v[84] * x[67] - v[88] * x[69]; + v[70] = v[47] * v[72] + v[50] * v[91] + v[52] * v[70]; + v[91] = 0.01 * v[70] + 0.08 * v[74] + -0.14 * v[80]; + v[95] = v[95] * x[72] - v[91] * x[71]; + v[91] = v[91] * x[70] - v[93] * x[72]; + v[93] = 0.01 * v[70] + 0.085 * v[74] + -0.11 * v[80]; + v[98] = v[98] * x[75] - v[93] * x[74]; + v[93] = v[93] * x[73] - v[97] * x[75]; + v[97] = 0.01 * v[70] + 0.09 * v[74] + -0.08 * v[80]; + v[101] = v[101] * x[78] - v[97] * x[77]; + v[97] = v[97] * x[76] - v[100] * x[78]; + v[100] = 0.01 * v[70] + 0.095 * v[74] + -0.05 * v[80]; + v[104] = v[104] * x[81] - v[100] * x[80]; + v[100] = v[100] * x[79] - v[103] * x[81]; + v[103] = -0.01 * v[70] + 0.08 * v[74] + -0.14 * v[80]; + v[107] = v[107] * x[84] - v[103] * x[83]; + v[103] = v[103] * x[82] - v[106] * x[84]; + v[106] = -0.01 * v[70] + 0.085 * v[74] + -0.11 * v[80]; + v[110] = v[110] * x[87] - v[106] * x[86]; + v[106] = v[106] * x[85] - v[109] * x[87]; + v[109] = -0.01 * v[70] + 0.09 * v[74] + -0.08 * v[80]; + v[113] = v[113] * x[90] - v[109] * x[89]; + v[109] = v[109] * x[88] - v[112] * x[90]; + v[112] = -0.01 * v[70] + 0.095 * v[74] + -0.05 * v[80]; + v[116] = v[116] * x[93] - v[112] * x[92]; + v[112] = v[112] * x[91] - v[115] * x[93]; + v[120] = v[70] * v[118] + v[74] * v[120] + v[80] * v[119]; + v[70] = v[70] * v[122] + v[74] * v[123] + v[80] * v[118]; + v[123] = 0.08 * v[120] + -0.01 * v[70]; + v[126] = v[126] * x[99] - v[123] * x[98]; + v[123] = v[123] * x[97] - v[124] * x[99]; + v[124] = 0.08 * v[120] + 0.035 * v[70]; + v[129] = v[129] * x[102] - v[124] * x[101]; + v[124] = v[124] * x[100] - v[128] * x[102]; + v[128] = v[65] + 0.088 * v[120]; + v[122] = -1. * v[74] + 4.89663865010925e-12 * v[80]; + v[118] = -1. * v[70] + 4.89663865010925e-12 * v[122]; + v[119] = 0.07 * v[118]; + v[138] = v[138] * x[105] - v[119] * x[104]; + v[119] = v[119] * x[103] - v[135] * x[105]; + v[142] = v[120] * v[140] + v[70] * v[142] + v[122] * v[141]; + v[122] = v[120] * v[144] + v[70] * v[145] + v[122] * v[140]; + v[145] = 0.02 * v[142] + 0.04 * v[122] + 0.08 * v[118]; + v[148] = v[148] * x[108] - v[145] * x[107]; + v[145] = v[145] * x[106] - v[146] * x[108]; + v[146] = 0.04 * v[142] + 0.02 * v[122] + 0.08 * v[118]; + v[151] = v[151] * x[111] - v[146] * x[110]; + v[146] = v[146] * x[109] - v[150] * x[111]; + v[150] = 0.04 * v[142] + 0.06 * v[122] + 0.085 * v[118]; + v[154] = v[154] * x[114] - v[150] * x[113]; + v[150] = v[150] * x[112] - v[153] * x[114]; + v[153] = 0.06 * v[142] + 0.04 * v[122] + 0.085 * v[118]; + v[157] = v[157] * x[117] - v[153] * x[116]; + v[153] = v[153] * x[115] - v[156] * x[117]; + v[156] = -0.0530330085889673 * v[142] + -0.0530330085890148 * v[122] + 0.117 * v[118]; + v[160] = v[160] * x[120] - v[156] * x[119]; + v[156] = v[156] * x[118] - v[159] * x[120]; + v[159] = -0.0318198051533804 * v[142] + -0.0318198051534089 * v[122] + 0.117 * v[118]; + v[163] = v[163] * x[123] - v[159] * x[122]; + v[159] = v[159] * x[121] - v[162] * x[123]; + v[162] = -0.0106066017177935 * v[142] + -0.010606601717803 * v[122] + 0.117 * v[118]; + v[166] = v[166] * x[126] - v[162] * x[125]; + v[162] = v[162] * x[124] - v[165] * x[126]; + v[165] = 0.0106066017177935 * v[142] + 0.010606601717803 * v[122] + 0.117 * v[118]; + v[169] = v[169] * x[129] - v[165] * x[128]; + v[165] = v[165] * x[127] - v[168] * x[129]; + v[168] = 0.0318198051533804 * v[142] + 0.0318198051534089 * v[122] + 0.117 * v[118]; + v[172] = v[172] * x[132] - v[168] * x[131]; + v[168] = v[168] * x[130] - v[171] * x[132]; + v[171] = 0.0530330085889673 * v[142] + 0.0530330085890148 * v[122] + 0.117 * v[118]; + v[175] = v[175] * x[135] - v[171] * x[134]; + v[171] = v[171] * x[133] - v[174] * x[135]; + v[174] = -0.0530330085889673 * v[142] + -0.0530330085890148 * v[122] + 0.137 * v[118]; + v[178] = v[178] * x[138] - v[174] * x[137]; + v[174] = v[174] * x[136] - v[177] * x[138]; + v[177] = -0.0318198051533804 * v[142] + -0.0318198051534089 * v[122] + 0.137 * v[118]; + v[181] = v[181] * x[141] - v[177] * x[140]; + v[177] = v[177] * x[139] - v[180] * x[141]; + v[180] = -0.0106066017177935 * v[142] + -0.010606601717803 * v[122] + 0.137 * v[118]; + v[184] = v[184] * x[144] - v[180] * x[143]; + v[180] = v[180] * x[142] - v[183] * x[144]; + v[183] = 0.0106066017177935 * v[142] + 0.010606601717803 * v[122] + 0.137 * v[118]; + v[187] = v[187] * x[147] - v[183] * x[146]; + v[183] = v[183] * x[145] - v[186] * x[147]; + v[186] = 0.0318198051533804 * v[142] + 0.0318198051534089 * v[122] + 0.137 * v[118]; + v[190] = v[190] * x[150] - v[186] * x[149]; + v[186] = v[186] * x[148] - v[189] * x[150]; + v[189] = 0.0530330085889673 * v[142] + 0.0530330085890148 * v[122] + 0.137 * v[118]; + v[193] = v[193] * x[153] - v[189] * x[152]; + v[189] = v[189] * x[151] - v[192] * x[153]; + v[192] = -0.0530330085889673 * v[142] + -0.0530330085890148 * v[122] + 0.157 * v[118]; + v[196] = v[196] * x[156] - v[192] * x[155]; + v[192] = v[192] * x[154] - v[195] * x[156]; + v[195] = -0.0318198051533804 * v[142] + -0.0318198051534089 * v[122] + 0.157 * v[118]; + v[199] = v[199] * x[159] - v[195] * x[158]; + v[195] = v[195] * x[157] - v[198] * x[159]; + v[198] = -0.0106066017177935 * v[142] + -0.010606601717803 * v[122] + 0.157 * v[118]; + v[202] = v[202] * x[162] - v[198] * x[161]; + v[198] = v[198] * x[160] - v[201] * x[162]; + v[201] = 0.0106066017177935 * v[142] + 0.010606601717803 * v[122] + 0.157 * v[118]; + v[205] = v[205] * x[165] - v[201] * x[164]; + v[201] = v[201] * x[163] - v[204] * x[165]; + v[204] = 0.0318198051533804 * v[142] + 0.0318198051534089 * v[122] + 0.157 * v[118]; + v[208] = v[208] * x[168] - v[204] * x[167]; + v[204] = v[204] * x[166] - v[207] * x[168]; + v[207] = 0.0530330085889673 * v[142] + 0.0530330085890148 * v[122] + 0.157 * v[118]; + v[211] = v[211] * x[171] - v[207] * x[170]; + v[207] = v[207] * x[169] - v[210] * x[171]; + v[210] = 0.0565685424948984 * v[142] + 0.0565685424949492 * v[122] + 0.1874 * v[118]; + v[214] = v[214] * x[174] - v[210] * x[173]; + v[210] = v[210] * x[172] - v[213] * x[174]; + v[213] = 0.0516187950265948 * v[142] + 0.0516187950266411 * v[122] + 0.2094 * v[118]; + v[217] = v[217] * x[177] - v[213] * x[176]; + v[213] = v[213] * x[175] - v[216] * x[177]; + v[216] = -0.0565685424948984 * v[142] + -0.0565685424949492 * v[122] + 0.1874 * v[118]; + v[220] = v[220] * x[180] - v[216] * x[179]; + v[216] = v[216] * x[178] - v[219] * x[180]; + v[219] = -0.0516187950265948 * v[142] + -0.0516187950266411 * v[122] + 0.2094 * v[118]; + v[223] = v[223] * x[183] - v[219] * x[182]; + v[219] = v[219] * x[181] - v[222] * x[183]; + v[11] = 1.95865541626434e-13 + -0.0850000008940697 * v[11]; + v[9] = 0.0385857857763767 * v[33] + 0.0289393402636051 * v[9] + -0.0517677664756775 * v[230]; + v[29] = v[29] * x[195] - v[9] * x[194]; + v[9] = v[9] * x[193] - v[30] * x[195]; + v[52] = -0.041449997574091 * v[47] + 0.0492218732833862 * v[50] + 0.0289125014096498 * v[52]; + v[59] = v[59] * x[198] - v[52] * x[197]; + v[52] = v[52] * x[196] - v[57] * x[198]; + v[80] = 0.0375000014901161 * v[74] + -0.109999999403954 * v[80]; + v[85] = v[85] * x[201] - v[80] * x[200]; + v[80] = v[80] * x[199] - v[83] * x[201]; + v[70] = 0.0424309633672237 * v[120] + 0.0144476993009448 * v[70]; + v[94] = v[94] * x[204] - v[70] * x[203]; + v[70] = v[70] * x[202] - v[92] * x[204]; + v[92] = 0.0153005504980683 * v[142] + 0.0153005504980683 * v[122] + 0.0745901614427567 * v[118]; + v[75] = v[75] * x[207] - v[92] * x[206]; + v[92] = v[92] * x[205] - v[121] * x[207]; + v[121] = + -5.20417042793042e-18 * v[142] + -2.16840434497101e-18 * v[122] + 0.129201397299767 * v[118]; + v[48] = v[48] * x[210] - v[121] * x[209]; + v[121] = v[121] * x[208] - v[51] * x[210]; + v[51] = 0.0540936700999737 * v[142] + 0.0540936700999737 * v[122] + 0.198400005698204 * v[118]; + v[21] = v[21] * x[213] - v[51] * x[212]; + v[51] = v[51] * x[211] - v[34] * x[213]; + v[122] = -0.0540936700999737 * v[142] + -0.0540936700999737 * v[122] + 0.198400005698204 * v[118]; + v[136] = v[136] * x[216] - v[122] * x[215]; + v[122] = v[122] * x[214] - v[133] * x[216]; + y[1] = (v[1] - (0. - 0.333 * v[2])) * x[22] + (v[134] - 0.333 * v[143]) * x[23] + + v[143] * (v[4] * x[24] - 1.46899159503278e-13 * x[23]) + + v[2] * (1.46899159503278e-13 * x[22] - v[3] * x[24]) + 4.89663865010925e-12 * v[5] + + (v[1] - (0. - 0.333 * v[2])) * x[25] + (v[134] - 0.333 * v[143]) * x[26] + + v[143] * (v[7] * x[27] - 3.9173109200874e-13 * x[26]) + + v[2] * (3.9173109200874e-13 * x[25] - v[6] * x[27]) + 4.89663865010925e-12 * v[8] + + (v[1] - (0. - 0.333 * v[2])) * x[28] + (v[134] - 0.333 * v[143]) * x[29] + + v[143] * (v[10] * x[30] - v[227] * x[29]) + v[2] * (v[227] * x[28] - v[14] * x[30]) + + 4.89663865010925e-12 * v[15] + (v[1] - (0. - 0.333 * v[2])) * x[31] + + (v[134] - 0.333 * v[143]) * x[32] + v[143] * (v[17] * x[33] - v[228] * x[32]) + + v[2] * (v[228] * x[31] - v[16] * x[33]) + 4.89663865010925e-12 * v[18] + + (v[1] - (4.89663865010925e-12 * v[19] - v[229] * v[2])) * x[34] + + (v[134] - (v[229] * v[143] - 4.89663865010925e-12 * v[20])) * x[35] + + (0. - (v[20] * v[2] - v[19] * v[143])) * x[36] + v[143] * v[24] + v[2] * v[231] + + 4.89663865010925e-12 * v[25] + + (v[1] - (4.89663865010925e-12 * v[19] - v[229] * v[2])) * x[37] + + (v[134] - (v[229] * v[143] - 4.89663865010925e-12 * v[20])) * x[38] + + (0. - (v[20] * v[2] - v[19] * v[143])) * x[39] + v[143] * v[27] + v[2] * v[22] + + 4.89663865010925e-12 * v[28] + + (v[1] - (4.89663865010925e-12 * v[19] - v[229] * v[2])) * x[40] + + (v[134] - (v[229] * v[143] - 4.89663865010925e-12 * v[20])) * x[41] + + (0. - (v[20] * v[2] - v[19] * v[143])) * x[42] + v[143] * v[38] + v[2] * v[36] + + 4.89663865010925e-12 * v[39] + + (v[1] - (4.89663865010925e-12 * v[19] - v[229] * v[2])) * x[43] + + (v[134] - (v[229] * v[143] - 4.89663865010925e-12 * v[20])) * x[44] + + (0. - (v[20] * v[2] - v[19] * v[143])) * x[45] + v[143] * v[41] + v[2] * v[37] + + 4.89663865010925e-12 * v[42] + + (v[1] - (4.89663865010925e-12 * v[43] - v[40] * v[2])) * x[46] + + (v[134] - (v[40] * v[143] - 4.89663865010925e-12 * v[44])) * x[47] + + (0. - (v[44] * v[2] - v[43] * v[143])) * x[48] + v[143] * v[55] + v[2] * v[49] + + 4.89663865010925e-12 * v[56] + + (v[1] - (4.89663865010925e-12 * v[43] - v[40] * v[2])) * x[49] + + (v[134] - (v[40] * v[143] - 4.89663865010925e-12 * v[44])) * x[50] + + (0. - (v[44] * v[2] - v[43] * v[143])) * x[51] + v[143] * v[60] + v[2] * v[45] + + 4.89663865010925e-12 * v[61] + + (v[1] - (4.89663865010925e-12 * v[43] - v[40] * v[2])) * x[52] + + (v[134] - (v[40] * v[143] - 4.89663865010925e-12 * v[44])) * x[53] + + (0. - (v[44] * v[2] - v[43] * v[143])) * x[54] + v[143] * v[63] + v[2] * v[58] + + 4.89663865010925e-12 * v[64] + + (v[1] - (4.89663865010925e-12 * v[43] - v[40] * v[2])) * x[55] + + (v[134] - (v[40] * v[143] - 4.89663865010925e-12 * v[44])) * x[56] + + (0. - (v[44] * v[2] - v[43] * v[143])) * x[57] + v[143] * v[66] + v[2] * v[62] + + 4.89663865010925e-12 * v[67] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[58] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[59] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[60] + v[143] * v[78] + v[2] * v[73] + + 4.89663865010925e-12 * v[79] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[61] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[62] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[63] + v[143] * v[81] + v[2] * v[76] + + 4.89663865010925e-12 * v[82] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[64] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[65] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[66] + v[143] * v[86] + v[2] * v[71] + + 4.89663865010925e-12 * v[87] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[67] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[68] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[69] + v[143] * v[89] + v[2] * v[84] + + 4.89663865010925e-12 * v[90] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[70] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[71] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[72] + v[143] * v[95] + v[2] * v[91] + + 4.89663865010925e-12 * v[96] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[73] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[74] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[75] + v[143] * v[98] + v[2] * v[93] + + 4.89663865010925e-12 * v[99] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[76] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[77] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[78] + v[143] * v[101] + v[2] * v[97] + + 4.89663865010925e-12 * v[102] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[79] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[80] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[81] + v[143] * v[104] + v[2] * v[100] + + 4.89663865010925e-12 * v[105] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[82] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[83] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[84] + v[143] * v[107] + v[2] * v[103] + + 4.89663865010925e-12 * v[108] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[85] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[86] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[87] + v[143] * v[110] + v[2] * v[106] + + 4.89663865010925e-12 * v[111] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[88] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[89] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[90] + v[143] * v[113] + v[2] * v[109] + + 4.89663865010925e-12 * v[114] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[91] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[92] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[93] + v[143] * v[116] + v[2] * v[112] + + 4.89663865010925e-12 * v[117] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[94] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[95] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[96] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[97] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[98] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[99] + v[143] * v[126] + v[2] * v[123] + + 4.89663865010925e-12 * v[127] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[100] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[101] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[102] + v[143] * v[129] + v[2] * v[124] + + 4.89663865010925e-12 * v[130] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[103] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[104] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[105] + v[143] * v[138] + v[2] * v[119] + + 4.89663865010925e-12 * v[139] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[106] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[107] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[108] + v[143] * v[148] + v[2] * v[145] + + 4.89663865010925e-12 * v[149] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[109] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[110] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[111] + v[143] * v[151] + v[2] * v[146] + + 4.89663865010925e-12 * v[152] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[112] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[113] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[114] + v[143] * v[154] + v[2] * v[150] + + 4.89663865010925e-12 * v[155] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[115] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[116] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[117] + v[143] * v[157] + v[2] * v[153] + + 4.89663865010925e-12 * v[158] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[118] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[119] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[120] + v[143] * v[160] + v[2] * v[156] + + 4.89663865010925e-12 * v[161] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[121] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[122] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[123] + v[143] * v[163] + v[2] * v[159] + + 4.89663865010925e-12 * v[164] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[124] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[125] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[126] + v[143] * v[166] + v[2] * v[162] + + 4.89663865010925e-12 * v[167] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[127] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[128] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[129] + v[143] * v[169] + v[2] * v[165] + + 4.89663865010925e-12 * v[170] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[130] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[131] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[132] + v[143] * v[172] + v[2] * v[168] + + 4.89663865010925e-12 * v[173] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[133] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[134] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[135] + v[143] * v[175] + v[2] * v[171] + + 4.89663865010925e-12 * v[176] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[136] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[137] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[138] + v[143] * v[178] + v[2] * v[174] + + 4.89663865010925e-12 * v[179] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[139] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[140] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[141] + v[143] * v[181] + v[2] * v[177] + + 4.89663865010925e-12 * v[182] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[142] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[143] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[144] + v[143] * v[184] + v[2] * v[180] + + 4.89663865010925e-12 * v[185] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[145] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[146] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[147] + v[143] * v[187] + v[2] * v[183] + + 4.89663865010925e-12 * v[188] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[148] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[149] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[150] + v[143] * v[190] + v[2] * v[186] + + 4.89663865010925e-12 * v[191] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[151] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[152] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[153] + v[143] * v[193] + v[2] * v[189] + + 4.89663865010925e-12 * v[194] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[154] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[155] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[156] + v[143] * v[196] + v[2] * v[192] + + 4.89663865010925e-12 * v[197] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[157] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[158] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[159] + v[143] * v[199] + v[2] * v[195] + + 4.89663865010925e-12 * v[200] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[160] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[161] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[162] + v[143] * v[202] + v[2] * v[198] + + 4.89663865010925e-12 * v[203] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[163] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[164] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[165] + v[143] * v[205] + v[2] * v[201] + + 4.89663865010925e-12 * v[206] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[166] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[167] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[168] + v[143] * v[208] + v[2] * v[204] + + 4.89663865010925e-12 * v[209] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[169] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[170] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[171] + v[143] * v[211] + v[2] * v[207] + + 4.89663865010925e-12 * v[212] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[172] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[173] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[174] + v[143] * v[214] + v[2] * v[210] + + 4.89663865010925e-12 * v[215] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[175] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[176] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[177] + v[143] * v[217] + v[2] * v[213] + + 4.89663865010925e-12 * v[218] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[178] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[179] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[180] + v[143] * v[220] + v[2] * v[216] + + 4.89663865010925e-12 * v[221] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[181] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[182] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[183] + v[143] * v[223] + v[2] * v[219] + + 4.89663865010925e-12 * v[224] + (v[1] - (0. - 0.333 * v[2])) * x[190] + + (v[134] - 0.333 * v[143]) * x[191] + v[143] * (v[12] * x[192] - v[11] * x[191]) + + v[2] * (v[11] * x[190] - v[13] * x[192]) + 4.89663865010925e-12 * v[225] + + (v[1] - (4.89663865010925e-12 * v[19] - v[229] * v[2])) * x[193] + + (v[134] - (v[229] * v[143] - 4.89663865010925e-12 * v[20])) * x[194] + + (0. - (v[20] * v[2] - v[19] * v[143])) * x[195] + v[143] * v[29] + v[2] * v[9] + + 4.89663865010925e-12 * v[0] + + (v[1] - (4.89663865010925e-12 * v[43] - v[40] * v[2])) * x[196] + + (v[134] - (v[40] * v[143] - 4.89663865010925e-12 * v[44])) * x[197] + + (0. - (v[44] * v[2] - v[43] * v[143])) * x[198] + v[143] * v[59] + v[2] * v[52] + + 4.89663865010925e-12 * v[54] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[199] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[200] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[201] + v[143] * v[85] + v[2] * v[80] + + 4.89663865010925e-12 * v[77] + + (v[1] - (4.89663865010925e-12 * v[68] - v[65] * v[2])) * x[202] + + (v[134] - (v[65] * v[143] - 4.89663865010925e-12 * v[69])) * x[203] + + (0. - (v[69] * v[2] - v[68] * v[143])) * x[204] + v[143] * v[94] + v[2] * v[70] + + 4.89663865010925e-12 * v[125] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[205] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[206] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[207] + v[143] * v[75] + v[2] * v[92] + + 4.89663865010925e-12 * v[53] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[208] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[209] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[210] + v[143] * v[48] + v[2] * v[121] + + 4.89663865010925e-12 * v[23] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[211] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[212] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[213] + v[143] * v[21] + v[2] * v[51] + + 4.89663865010925e-12 * v[226] + + (v[1] - (4.89663865010925e-12 * v[131] - v[128] * v[2])) * x[214] + + (v[134] - (v[128] * v[143] - 4.89663865010925e-12 * v[132])) * x[215] + + (0. - (v[132] * v[2] - v[131] * v[143])) * x[216] + v[143] * v[136] + v[2] * v[122] + + 4.89663865010925e-12 * v[147]; + v[11] = sin(x[1]); + v[228] = -v[11]; + v[227] = cos(x[1]); + v[134] = 4.89663865010925e-12 * v[227]; + v[1] = v[137] * v[228] + v[2] * v[134]; + v[225] = -0.316 * v[1]; + v[12] = -1. * v[227]; + v[13] = 2.39770700697438e-23 + -1. * v[12]; + v[18] = 0.333 + -0.316 * v[12]; + v[17] = -1. * v[1] + 4.89663865010925e-12 * v[2]; + v[16] = v[225] * v[13] - v[18] * v[17]; + v[134] = v[2] * v[228] + v[143] * v[134]; + v[228] = -1. * v[134] + 4.89663865010925e-12 * v[143]; + v[15] = -0.316 * v[134]; + v[10] = v[18] * v[228] - v[15] * v[13]; + v[14] = v[15] * v[17] - v[225] * v[228]; + y[2] = (v[16] - (v[19] * v[13] - v[229] * v[17])) * x[34] + + (v[10] - (v[229] * v[228] - v[20] * v[13])) * x[35] + + (v[14] - (v[20] * v[17] - v[19] * v[228])) * x[36] + v[228] * v[24] + v[17] * v[231] + + v[13] * v[25] + (v[16] - (v[19] * v[13] - v[229] * v[17])) * x[37] + + (v[10] - (v[229] * v[228] - v[20] * v[13])) * x[38] + + (v[14] - (v[20] * v[17] - v[19] * v[228])) * x[39] + v[228] * v[27] + v[17] * v[22] + + v[13] * v[28] + (v[16] - (v[19] * v[13] - v[229] * v[17])) * x[40] + + (v[10] - (v[229] * v[228] - v[20] * v[13])) * x[41] + + (v[14] - (v[20] * v[17] - v[19] * v[228])) * x[42] + v[228] * v[38] + v[17] * v[36] + + v[13] * v[39] + (v[16] - (v[19] * v[13] - v[229] * v[17])) * x[43] + + (v[10] - (v[229] * v[228] - v[20] * v[13])) * x[44] + + (v[14] - (v[20] * v[17] - v[19] * v[228])) * x[45] + v[228] * v[41] + v[17] * v[37] + + v[13] * v[42] + (v[16] - (v[43] * v[13] - v[40] * v[17])) * x[46] + + (v[10] - (v[40] * v[228] - v[44] * v[13])) * x[47] + + (v[14] - (v[44] * v[17] - v[43] * v[228])) * x[48] + v[228] * v[55] + v[17] * v[49] + + v[13] * v[56] + (v[16] - (v[43] * v[13] - v[40] * v[17])) * x[49] + + (v[10] - (v[40] * v[228] - v[44] * v[13])) * x[50] + + (v[14] - (v[44] * v[17] - v[43] * v[228])) * x[51] + v[228] * v[60] + v[17] * v[45] + + v[13] * v[61] + (v[16] - (v[43] * v[13] - v[40] * v[17])) * x[52] + + (v[10] - (v[40] * v[228] - v[44] * v[13])) * x[53] + + (v[14] - (v[44] * v[17] - v[43] * v[228])) * x[54] + v[228] * v[63] + v[17] * v[58] + + v[13] * v[64] + (v[16] - (v[43] * v[13] - v[40] * v[17])) * x[55] + + (v[10] - (v[40] * v[228] - v[44] * v[13])) * x[56] + + (v[14] - (v[44] * v[17] - v[43] * v[228])) * x[57] + v[228] * v[66] + v[17] * v[62] + + v[13] * v[67] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[58] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[59] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[60] + v[228] * v[78] + v[17] * v[73] + + v[13] * v[79] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[61] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[62] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[63] + v[228] * v[81] + v[17] * v[76] + + v[13] * v[82] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[64] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[65] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[66] + v[228] * v[86] + v[17] * v[71] + + v[13] * v[87] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[67] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[68] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[69] + v[228] * v[89] + v[17] * v[84] + + v[13] * v[90] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[70] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[71] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[72] + v[228] * v[95] + v[17] * v[91] + + v[13] * v[96] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[73] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[74] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[75] + v[228] * v[98] + v[17] * v[93] + + v[13] * v[99] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[76] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[77] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[78] + v[228] * v[101] + v[17] * v[97] + + v[13] * v[102] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[79] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[80] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[81] + v[228] * v[104] + v[17] * v[100] + + v[13] * v[105] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[82] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[83] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[84] + v[228] * v[107] + v[17] * v[103] + + v[13] * v[108] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[85] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[86] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[87] + v[228] * v[110] + v[17] * v[106] + + v[13] * v[111] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[88] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[89] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[90] + v[228] * v[113] + v[17] * v[109] + + v[13] * v[114] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[91] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[92] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[93] + v[228] * v[116] + v[17] * v[112] + + v[13] * v[117] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[94] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[95] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[96] + + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[97] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[98] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[99] + v[228] * v[126] + v[17] * v[123] + + v[13] * v[127] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[100] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[101] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[102] + v[228] * v[129] + v[17] * v[124] + + v[13] * v[130] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[103] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[104] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[105] + v[228] * v[138] + v[17] * v[119] + + v[13] * v[139] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[106] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[107] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[108] + v[228] * v[148] + v[17] * v[145] + + v[13] * v[149] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[109] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[110] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[111] + v[228] * v[151] + v[17] * v[146] + + v[13] * v[152] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[112] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[113] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[114] + v[228] * v[154] + v[17] * v[150] + + v[13] * v[155] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[115] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[116] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[117] + v[228] * v[157] + v[17] * v[153] + + v[13] * v[158] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[118] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[119] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[120] + v[228] * v[160] + v[17] * v[156] + + v[13] * v[161] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[121] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[122] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[123] + v[228] * v[163] + v[17] * v[159] + + v[13] * v[164] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[124] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[125] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[126] + v[228] * v[166] + v[17] * v[162] + + v[13] * v[167] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[127] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[128] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[129] + v[228] * v[169] + v[17] * v[165] + + v[13] * v[170] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[130] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[131] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[132] + v[228] * v[172] + v[17] * v[168] + + v[13] * v[173] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[133] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[134] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[135] + v[228] * v[175] + v[17] * v[171] + + v[13] * v[176] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[136] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[137] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[138] + v[228] * v[178] + v[17] * v[174] + + v[13] * v[179] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[139] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[140] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[141] + v[228] * v[181] + v[17] * v[177] + + v[13] * v[182] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[142] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[143] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[144] + v[228] * v[184] + v[17] * v[180] + + v[13] * v[185] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[145] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[146] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[147] + v[228] * v[187] + v[17] * v[183] + + v[13] * v[188] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[148] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[149] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[150] + v[228] * v[190] + v[17] * v[186] + + v[13] * v[191] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[151] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[152] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[153] + v[228] * v[193] + v[17] * v[189] + + v[13] * v[194] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[154] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[155] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[156] + v[228] * v[196] + v[17] * v[192] + + v[13] * v[197] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[157] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[158] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[159] + v[228] * v[199] + v[17] * v[195] + + v[13] * v[200] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[160] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[161] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[162] + v[228] * v[202] + v[17] * v[198] + + v[13] * v[203] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[163] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[164] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[165] + v[228] * v[205] + v[17] * v[201] + + v[13] * v[206] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[166] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[167] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[168] + v[228] * v[208] + v[17] * v[204] + + v[13] * v[209] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[169] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[170] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[171] + v[228] * v[211] + v[17] * v[207] + + v[13] * v[212] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[172] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[173] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[174] + v[228] * v[214] + v[17] * v[210] + + v[13] * v[215] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[175] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[176] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[177] + v[228] * v[217] + v[17] * v[213] + + v[13] * v[218] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[178] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[179] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[180] + v[228] * v[220] + v[17] * v[216] + + v[13] * v[221] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[181] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[182] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[183] + v[228] * v[223] + v[17] * v[219] + + v[13] * v[224] + (v[16] - (v[19] * v[13] - v[229] * v[17])) * x[193] + + (v[10] - (v[229] * v[228] - v[20] * v[13])) * x[194] + + (v[14] - (v[20] * v[17] - v[19] * v[228])) * x[195] + v[228] * v[29] + v[17] * v[9] + + v[13] * v[0] + (v[16] - (v[43] * v[13] - v[40] * v[17])) * x[196] + + (v[10] - (v[40] * v[228] - v[44] * v[13])) * x[197] + + (v[14] - (v[44] * v[17] - v[43] * v[228])) * x[198] + v[228] * v[59] + v[17] * v[52] + + v[13] * v[54] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[199] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[200] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[201] + v[228] * v[85] + v[17] * v[80] + + v[13] * v[77] + (v[16] - (v[68] * v[13] - v[65] * v[17])) * x[202] + + (v[10] - (v[65] * v[228] - v[69] * v[13])) * x[203] + + (v[14] - (v[69] * v[17] - v[68] * v[228])) * x[204] + v[228] * v[94] + v[17] * v[70] + + v[13] * v[125] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[205] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[206] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[207] + v[228] * v[75] + v[17] * v[92] + + v[13] * v[53] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[208] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[209] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[210] + v[228] * v[48] + v[17] * v[121] + + v[13] * v[23] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[211] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[212] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[213] + v[228] * v[21] + v[17] * v[51] + + v[13] * v[226] + (v[16] - (v[131] * v[13] - v[128] * v[17])) * x[214] + + (v[10] - (v[128] * v[228] - v[132] * v[13])) * x[215] + + (v[14] - (v[132] * v[17] - v[131] * v[228])) * x[216] + v[228] * v[136] + v[17] * v[122] + + v[13] * v[147]; + v[14] = 4.89663865010925e-12 * v[11]; + v[137] = v[137] * v[227] + v[2] * v[14]; + v[10] = cos(x[2]); + v[16] = sin(x[2]); + v[9] = 4.89663865010925e-12 * v[16]; + v[29] = v[137] * v[10] + v[1] * v[9] + v[2] * v[16]; + v[225] = v[225] + 0.0825 * v[29]; + v[11] = -1. * v[11]; + v[37] = -v[16]; + v[41] = 4.89663865010925e-12 * v[10]; + v[36] = v[11] * v[37] + v[12] * v[41] + 4.89663865010925e-12 * v[10]; + v[38] = -1. * v[36] + 4.89663865010925e-12 * v[13]; + v[11] = v[11] * v[10] + v[12] * v[9] + 4.89663865010925e-12 * v[16]; + v[18] = v[18] + 0.0825 * v[11]; + v[137] = v[137] * v[37] + v[1] * v[41] + v[2] * v[10]; + v[1] = -1. * v[137] + 4.89663865010925e-12 * v[17]; + v[12] = v[225] * v[38] - v[18] * v[1]; + v[14] = v[2] * v[227] + v[143] * v[14]; + v[41] = v[14] * v[37] + v[134] * v[41] + v[143] * v[10]; + v[37] = -1. * v[41] + 4.89663865010925e-12 * v[228]; + v[14] = v[14] * v[10] + v[134] * v[9] + v[143] * v[16]; + v[15] = v[15] + 0.0825 * v[14]; + v[9] = v[18] * v[37] - v[15] * v[38]; + v[16] = v[15] * v[1] - v[225] * v[37]; + y[3] = (v[12] - (v[43] * v[38] - v[40] * v[1])) * x[46] + + (v[9] - (v[40] * v[37] - v[44] * v[38])) * x[47] + + (v[16] - (v[44] * v[1] - v[43] * v[37])) * x[48] + v[37] * v[55] + v[1] * v[49] + + v[38] * v[56] + (v[12] - (v[43] * v[38] - v[40] * v[1])) * x[49] + + (v[9] - (v[40] * v[37] - v[44] * v[38])) * x[50] + + (v[16] - (v[44] * v[1] - v[43] * v[37])) * x[51] + v[37] * v[60] + v[1] * v[45] + + v[38] * v[61] + (v[12] - (v[43] * v[38] - v[40] * v[1])) * x[52] + + (v[9] - (v[40] * v[37] - v[44] * v[38])) * x[53] + + (v[16] - (v[44] * v[1] - v[43] * v[37])) * x[54] + v[37] * v[63] + v[1] * v[58] + + v[38] * v[64] + (v[12] - (v[43] * v[38] - v[40] * v[1])) * x[55] + + (v[9] - (v[40] * v[37] - v[44] * v[38])) * x[56] + + (v[16] - (v[44] * v[1] - v[43] * v[37])) * x[57] + v[37] * v[66] + v[1] * v[62] + + v[38] * v[67] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[58] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[59] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[60] + v[37] * v[78] + v[1] * v[73] + + v[38] * v[79] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[61] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[62] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[63] + v[37] * v[81] + v[1] * v[76] + + v[38] * v[82] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[64] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[65] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[66] + v[37] * v[86] + v[1] * v[71] + + v[38] * v[87] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[67] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[68] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[69] + v[37] * v[89] + v[1] * v[84] + + v[38] * v[90] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[70] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[71] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[72] + v[37] * v[95] + v[1] * v[91] + + v[38] * v[96] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[73] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[74] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[75] + v[37] * v[98] + v[1] * v[93] + + v[38] * v[99] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[76] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[77] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[78] + v[37] * v[101] + v[1] * v[97] + + v[38] * v[102] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[79] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[80] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[81] + v[37] * v[104] + v[1] * v[100] + + v[38] * v[105] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[82] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[83] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[84] + v[37] * v[107] + v[1] * v[103] + + v[38] * v[108] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[85] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[86] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[87] + v[37] * v[110] + v[1] * v[106] + + v[38] * v[111] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[88] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[89] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[90] + v[37] * v[113] + v[1] * v[109] + + v[38] * v[114] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[91] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[92] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[93] + v[37] * v[116] + v[1] * v[112] + + v[38] * v[117] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[94] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[95] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[96] + + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[97] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[98] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[99] + v[37] * v[126] + v[1] * v[123] + + v[38] * v[127] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[100] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[101] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[102] + v[37] * v[129] + v[1] * v[124] + + v[38] * v[130] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[103] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[104] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[105] + v[37] * v[138] + v[1] * v[119] + + v[38] * v[139] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[106] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[107] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[108] + v[37] * v[148] + v[1] * v[145] + + v[38] * v[149] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[109] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[110] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[111] + v[37] * v[151] + v[1] * v[146] + + v[38] * v[152] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[112] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[113] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[114] + v[37] * v[154] + v[1] * v[150] + + v[38] * v[155] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[115] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[116] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[117] + v[37] * v[157] + v[1] * v[153] + + v[38] * v[158] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[118] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[119] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[120] + v[37] * v[160] + v[1] * v[156] + + v[38] * v[161] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[121] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[122] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[123] + v[37] * v[163] + v[1] * v[159] + + v[38] * v[164] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[124] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[125] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[126] + v[37] * v[166] + v[1] * v[162] + + v[38] * v[167] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[127] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[128] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[129] + v[37] * v[169] + v[1] * v[165] + + v[38] * v[170] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[130] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[131] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[132] + v[37] * v[172] + v[1] * v[168] + + v[38] * v[173] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[133] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[134] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[135] + v[37] * v[175] + v[1] * v[171] + + v[38] * v[176] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[136] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[137] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[138] + v[37] * v[178] + v[1] * v[174] + + v[38] * v[179] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[139] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[140] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[141] + v[37] * v[181] + v[1] * v[177] + + v[38] * v[182] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[142] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[143] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[144] + v[37] * v[184] + v[1] * v[180] + + v[38] * v[185] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[145] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[146] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[147] + v[37] * v[187] + v[1] * v[183] + + v[38] * v[188] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[148] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[149] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[150] + v[37] * v[190] + v[1] * v[186] + + v[38] * v[191] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[151] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[152] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[153] + v[37] * v[193] + v[1] * v[189] + + v[38] * v[194] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[154] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[155] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[156] + v[37] * v[196] + v[1] * v[192] + + v[38] * v[197] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[157] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[158] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[159] + v[37] * v[199] + v[1] * v[195] + + v[38] * v[200] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[160] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[161] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[162] + v[37] * v[202] + v[1] * v[198] + + v[38] * v[203] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[163] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[164] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[165] + v[37] * v[205] + v[1] * v[201] + + v[38] * v[206] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[166] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[167] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[168] + v[37] * v[208] + v[1] * v[204] + + v[38] * v[209] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[169] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[170] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[171] + v[37] * v[211] + v[1] * v[207] + + v[38] * v[212] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[172] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[173] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[174] + v[37] * v[214] + v[1] * v[210] + + v[38] * v[215] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[175] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[176] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[177] + v[37] * v[217] + v[1] * v[213] + + v[38] * v[218] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[178] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[179] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[180] + v[37] * v[220] + v[1] * v[216] + + v[38] * v[221] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[181] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[182] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[183] + v[37] * v[223] + v[1] * v[219] + + v[38] * v[224] + (v[12] - (v[43] * v[38] - v[40] * v[1])) * x[196] + + (v[9] - (v[40] * v[37] - v[44] * v[38])) * x[197] + + (v[16] - (v[44] * v[1] - v[43] * v[37])) * x[198] + v[37] * v[59] + v[1] * v[52] + + v[38] * v[54] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[199] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[200] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[201] + v[37] * v[85] + v[1] * v[80] + + v[38] * v[77] + (v[12] - (v[68] * v[38] - v[65] * v[1])) * x[202] + + (v[9] - (v[65] * v[37] - v[69] * v[38])) * x[203] + + (v[16] - (v[69] * v[1] - v[68] * v[37])) * x[204] + v[37] * v[94] + v[1] * v[70] + + v[38] * v[125] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[205] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[206] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[207] + v[37] * v[75] + v[1] * v[92] + + v[38] * v[53] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[208] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[209] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[210] + v[37] * v[48] + v[1] * v[121] + + v[38] * v[23] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[211] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[212] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[213] + v[37] * v[21] + v[1] * v[51] + + v[38] * v[226] + (v[12] - (v[131] * v[38] - v[128] * v[1])) * x[214] + + (v[9] - (v[128] * v[37] - v[132] * v[38])) * x[215] + + (v[16] - (v[132] * v[1] - v[131] * v[37])) * x[216] + v[37] * v[136] + v[1] * v[122] + + v[38] * v[147]; + v[16] = cos(x[3]); + v[9] = sin(x[3]); + v[12] = 4.89663865010925e-12 * v[9]; + v[52] = v[29] * v[16] + v[137] * v[12] + v[17] * v[9]; + v[59] = -v[9]; + v[62] = 4.89663865010925e-12 * v[16]; + v[137] = v[29] * v[59] + v[137] * v[62] + v[17] * v[16]; + v[225] = v[225] + -0.0825 * v[52] + 0.384 * v[137]; + v[29] = v[11] * v[59] + v[36] * v[62] + v[13] * v[16]; + v[17] = v[29] + 4.89663865010925e-12 * v[38]; + v[11] = v[11] * v[16] + v[36] * v[12] + v[13] * v[9]; + v[18] = v[18] + -0.0825 * v[11] + 0.384 * v[29]; + v[36] = v[137] + 4.89663865010925e-12 * v[1]; + v[13] = v[225] * v[17] - v[18] * v[36]; + v[62] = v[14] * v[59] + v[41] * v[62] + v[228] * v[16]; + v[59] = v[62] + 4.89663865010925e-12 * v[37]; + v[12] = v[14] * v[16] + v[41] * v[12] + v[228] * v[9]; + v[15] = v[15] + -0.0825 * v[12] + 0.384 * v[62]; + v[9] = v[18] * v[59] - v[15] * v[17]; + v[16] = v[15] * v[36] - v[225] * v[59]; + y[4] = (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[58] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[59] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[60] + v[59] * v[78] + v[36] * v[73] + + v[17] * v[79] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[61] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[62] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[63] + v[59] * v[81] + v[36] * v[76] + + v[17] * v[82] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[64] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[65] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[66] + v[59] * v[86] + v[36] * v[71] + + v[17] * v[87] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[67] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[68] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[69] + v[59] * v[89] + v[36] * v[84] + + v[17] * v[90] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[70] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[71] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[72] + v[59] * v[95] + v[36] * v[91] + + v[17] * v[96] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[73] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[74] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[75] + v[59] * v[98] + v[36] * v[93] + + v[17] * v[99] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[76] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[77] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[78] + v[59] * v[101] + v[36] * v[97] + + v[17] * v[102] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[79] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[80] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[81] + v[59] * v[104] + v[36] * v[100] + + v[17] * v[105] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[82] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[83] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[84] + v[59] * v[107] + v[36] * v[103] + + v[17] * v[108] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[85] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[86] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[87] + v[59] * v[110] + v[36] * v[106] + + v[17] * v[111] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[88] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[89] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[90] + v[59] * v[113] + v[36] * v[109] + + v[17] * v[114] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[91] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[92] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[93] + v[59] * v[116] + v[36] * v[112] + + v[17] * v[117] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[94] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[95] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[96] + + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[97] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[98] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[99] + v[59] * v[126] + v[36] * v[123] + + v[17] * v[127] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[100] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[101] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[102] + v[59] * v[129] + v[36] * v[124] + + v[17] * v[130] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[103] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[104] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[105] + v[59] * v[138] + v[36] * v[119] + + v[17] * v[139] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[106] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[107] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[108] + v[59] * v[148] + v[36] * v[145] + + v[17] * v[149] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[109] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[110] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[111] + v[59] * v[151] + v[36] * v[146] + + v[17] * v[152] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[112] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[113] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[114] + v[59] * v[154] + v[36] * v[150] + + v[17] * v[155] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[115] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[116] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[117] + v[59] * v[157] + v[36] * v[153] + + v[17] * v[158] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[118] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[119] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[120] + v[59] * v[160] + v[36] * v[156] + + v[17] * v[161] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[121] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[122] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[123] + v[59] * v[163] + v[36] * v[159] + + v[17] * v[164] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[124] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[125] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[126] + v[59] * v[166] + v[36] * v[162] + + v[17] * v[167] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[127] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[128] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[129] + v[59] * v[169] + v[36] * v[165] + + v[17] * v[170] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[130] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[131] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[132] + v[59] * v[172] + v[36] * v[168] + + v[17] * v[173] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[133] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[134] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[135] + v[59] * v[175] + v[36] * v[171] + + v[17] * v[176] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[136] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[137] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[138] + v[59] * v[178] + v[36] * v[174] + + v[17] * v[179] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[139] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[140] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[141] + v[59] * v[181] + v[36] * v[177] + + v[17] * v[182] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[142] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[143] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[144] + v[59] * v[184] + v[36] * v[180] + + v[17] * v[185] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[145] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[146] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[147] + v[59] * v[187] + v[36] * v[183] + + v[17] * v[188] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[148] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[149] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[150] + v[59] * v[190] + v[36] * v[186] + + v[17] * v[191] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[151] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[152] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[153] + v[59] * v[193] + v[36] * v[189] + + v[17] * v[194] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[154] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[155] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[156] + v[59] * v[196] + v[36] * v[192] + + v[17] * v[197] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[157] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[158] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[159] + v[59] * v[199] + v[36] * v[195] + + v[17] * v[200] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[160] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[161] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[162] + v[59] * v[202] + v[36] * v[198] + + v[17] * v[203] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[163] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[164] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[165] + v[59] * v[205] + v[36] * v[201] + + v[17] * v[206] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[166] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[167] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[168] + v[59] * v[208] + v[36] * v[204] + + v[17] * v[209] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[169] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[170] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[171] + v[59] * v[211] + v[36] * v[207] + + v[17] * v[212] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[172] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[173] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[174] + v[59] * v[214] + v[36] * v[210] + + v[17] * v[215] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[175] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[176] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[177] + v[59] * v[217] + v[36] * v[213] + + v[17] * v[218] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[178] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[179] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[180] + v[59] * v[220] + v[36] * v[216] + + v[17] * v[221] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[181] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[182] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[183] + v[59] * v[223] + v[36] * v[219] + + v[17] * v[224] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[199] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[200] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[201] + v[59] * v[85] + v[36] * v[80] + + v[17] * v[77] + (v[13] - (v[68] * v[17] - v[65] * v[36])) * x[202] + + (v[9] - (v[65] * v[59] - v[69] * v[17])) * x[203] + + (v[16] - (v[69] * v[36] - v[68] * v[59])) * x[204] + v[59] * v[94] + v[36] * v[70] + + v[17] * v[125] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[205] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[206] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[207] + v[59] * v[75] + v[36] * v[92] + + v[17] * v[53] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[208] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[209] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[210] + v[59] * v[48] + v[36] * v[121] + + v[17] * v[23] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[211] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[212] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[213] + v[59] * v[21] + v[36] * v[51] + + v[17] * v[226] + (v[13] - (v[131] * v[17] - v[128] * v[36])) * x[214] + + (v[9] - (v[128] * v[59] - v[132] * v[17])) * x[215] + + (v[16] - (v[132] * v[36] - v[131] * v[59])) * x[216] + v[59] * v[136] + v[36] * v[122] + + v[17] * v[147]; + v[16] = sin(x[4]); + v[9] = -v[16]; + v[13] = cos(x[4]); + v[80] = 4.89663865010925e-12 * v[13]; + v[85] = -1. * v[13]; + v[112] = v[11] * v[9] + v[29] * v[80] + v[38] * v[85]; + v[116] = -1. * v[112] + 4.89663865010925e-12 * v[17]; + v[109] = v[52] * v[9] + v[137] * v[80] + v[1] * v[85]; + v[113] = -1. * v[109] + 4.89663865010925e-12 * v[36]; + v[106] = v[225] * v[116] - v[18] * v[113]; + v[85] = v[12] * v[9] + v[62] * v[80] + v[37] * v[85]; + v[80] = -1. * v[85] + 4.89663865010925e-12 * v[59]; + v[9] = v[18] * v[80] - v[15] * v[116]; + v[110] = v[15] * v[113] - v[225] * v[80]; + y[5] = (v[106] - (v[68] * v[116] - v[65] * v[113])) * x[94] + + (v[9] - (v[65] * v[80] - v[69] * v[116])) * x[95] + + (v[110] - (v[69] * v[113] - v[68] * v[80])) * x[96] + + (v[106] - (v[68] * v[116] - v[65] * v[113])) * x[97] + + (v[9] - (v[65] * v[80] - v[69] * v[116])) * x[98] + + (v[110] - (v[69] * v[113] - v[68] * v[80])) * x[99] + v[80] * v[126] + v[113] * v[123] + + v[116] * v[127] + (v[106] - (v[68] * v[116] - v[65] * v[113])) * x[100] + + (v[9] - (v[65] * v[80] - v[69] * v[116])) * x[101] + + (v[110] - (v[69] * v[113] - v[68] * v[80])) * x[102] + v[80] * v[129] + v[113] * v[124] + + v[116] * v[130] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[103] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[104] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[105] + v[80] * v[138] + v[113] * v[119] + + v[116] * v[139] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[106] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[107] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[108] + v[80] * v[148] + v[113] * v[145] + + v[116] * v[149] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[109] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[110] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[111] + v[80] * v[151] + v[113] * v[146] + + v[116] * v[152] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[112] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[113] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[114] + v[80] * v[154] + v[113] * v[150] + + v[116] * v[155] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[115] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[116] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[117] + v[80] * v[157] + v[113] * v[153] + + v[116] * v[158] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[118] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[119] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[120] + v[80] * v[160] + v[113] * v[156] + + v[116] * v[161] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[121] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[122] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[123] + v[80] * v[163] + v[113] * v[159] + + v[116] * v[164] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[124] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[125] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[126] + v[80] * v[166] + v[113] * v[162] + + v[116] * v[167] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[127] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[128] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[129] + v[80] * v[169] + v[113] * v[165] + + v[116] * v[170] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[130] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[131] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[132] + v[80] * v[172] + v[113] * v[168] + + v[116] * v[173] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[133] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[134] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[135] + v[80] * v[175] + v[113] * v[171] + + v[116] * v[176] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[136] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[137] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[138] + v[80] * v[178] + v[113] * v[174] + + v[116] * v[179] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[139] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[140] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[141] + v[80] * v[181] + v[113] * v[177] + + v[116] * v[182] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[142] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[143] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[144] + v[80] * v[184] + v[113] * v[180] + + v[116] * v[185] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[145] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[146] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[147] + v[80] * v[187] + v[113] * v[183] + + v[116] * v[188] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[148] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[149] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[150] + v[80] * v[190] + v[113] * v[186] + + v[116] * v[191] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[151] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[152] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[153] + v[80] * v[193] + v[113] * v[189] + + v[116] * v[194] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[154] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[155] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[156] + v[80] * v[196] + v[113] * v[192] + + v[116] * v[197] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[157] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[158] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[159] + v[80] * v[199] + v[113] * v[195] + + v[116] * v[200] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[160] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[161] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[162] + v[80] * v[202] + v[113] * v[198] + + v[116] * v[203] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[163] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[164] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[165] + v[80] * v[205] + v[113] * v[201] + + v[116] * v[206] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[166] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[167] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[168] + v[80] * v[208] + v[113] * v[204] + + v[116] * v[209] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[169] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[170] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[171] + v[80] * v[211] + v[113] * v[207] + + v[116] * v[212] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[172] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[173] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[174] + v[80] * v[214] + v[113] * v[210] + + v[116] * v[215] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[175] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[176] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[177] + v[80] * v[217] + v[113] * v[213] + + v[116] * v[218] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[178] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[179] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[180] + v[80] * v[220] + v[113] * v[216] + + v[116] * v[221] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[181] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[182] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[183] + v[80] * v[223] + v[113] * v[219] + + v[116] * v[224] + (v[106] - (v[68] * v[116] - v[65] * v[113])) * x[202] + + (v[9] - (v[65] * v[80] - v[69] * v[116])) * x[203] + + (v[110] - (v[69] * v[113] - v[68] * v[80])) * x[204] + v[80] * v[94] + v[113] * v[70] + + v[116] * v[125] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[205] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[206] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[207] + v[80] * v[75] + v[113] * v[92] + + v[116] * v[53] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[208] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[209] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[210] + v[80] * v[48] + v[113] * v[121] + + v[116] * v[23] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[211] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[212] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[213] + v[80] * v[21] + v[113] * v[51] + + v[116] * v[226] + (v[106] - (v[131] * v[116] - v[128] * v[113])) * x[214] + + (v[9] - (v[128] * v[80] - v[132] * v[116])) * x[215] + + (v[110] - (v[132] * v[113] - v[131] * v[80])) * x[216] + v[80] * v[136] + v[113] * v[122] + + v[116] * v[147]; + v[110] = 4.89663865010925e-12 * v[16]; + v[16] = -1. * v[16]; + v[137] = v[52] * v[13] + v[137] * v[110] + v[1] * v[16]; + v[52] = cos(x[5]); + v[1] = sin(x[5]); + v[9] = 4.89663865010925e-12 * v[1]; + v[225] = v[225] + 0.088 * (v[137] * v[52] + v[109] * v[9] + v[36] * v[1]); + v[11] = v[11] * v[13] + v[29] * v[110] + v[38] * v[16]; + v[29] = -v[1]; + v[38] = 4.89663865010925e-12 * v[52]; + v[116] = -1. * (v[11] * v[29] + v[112] * v[38] + v[17] * v[52]) + 4.89663865010925e-12 * v[116]; + v[11] = v[18] + 0.088 * (v[11] * v[52] + v[112] * v[9] + v[17] * v[1]); + v[137] = -1. * (v[137] * v[29] + v[109] * v[38] + v[36] * v[52]) + 4.89663865010925e-12 * v[113]; + v[113] = v[225] * v[116] - v[11] * v[137]; + v[16] = v[12] * v[13] + v[62] * v[110] + v[37] * v[16]; + v[38] = -1. * (v[16] * v[29] + v[85] * v[38] + v[59] * v[52]) + 4.89663865010925e-12 * v[80]; + v[16] = v[15] + 0.088 * (v[16] * v[52] + v[85] * v[9] + v[59] * v[1]); + v[11] = v[11] * v[38] - v[16] * v[116]; + v[16] = v[16] * v[137] - v[225] * v[38]; + y[6] = (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[103] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[104] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[105] + v[38] * v[138] + v[137] * v[119] + + v[116] * v[139] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[106] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[107] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[108] + v[38] * v[148] + v[137] * v[145] + + v[116] * v[149] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[109] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[110] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[111] + v[38] * v[151] + v[137] * v[146] + + v[116] * v[152] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[112] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[113] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[114] + v[38] * v[154] + v[137] * v[150] + + v[116] * v[155] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[115] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[116] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[117] + v[38] * v[157] + v[137] * v[153] + + v[116] * v[158] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[118] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[119] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[120] + v[38] * v[160] + v[137] * v[156] + + v[116] * v[161] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[121] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[122] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[123] + v[38] * v[163] + v[137] * v[159] + + v[116] * v[164] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[124] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[125] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[126] + v[38] * v[166] + v[137] * v[162] + + v[116] * v[167] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[127] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[128] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[129] + v[38] * v[169] + v[137] * v[165] + + v[116] * v[170] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[130] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[131] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[132] + v[38] * v[172] + v[137] * v[168] + + v[116] * v[173] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[133] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[134] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[135] + v[38] * v[175] + v[137] * v[171] + + v[116] * v[176] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[136] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[137] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[138] + v[38] * v[178] + v[137] * v[174] + + v[116] * v[179] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[139] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[140] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[141] + v[38] * v[181] + v[137] * v[177] + + v[116] * v[182] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[142] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[143] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[144] + v[38] * v[184] + v[137] * v[180] + + v[116] * v[185] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[145] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[146] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[147] + v[38] * v[187] + v[137] * v[183] + + v[116] * v[188] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[148] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[149] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[150] + v[38] * v[190] + v[137] * v[186] + + v[116] * v[191] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[151] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[152] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[153] + v[38] * v[193] + v[137] * v[189] + + v[116] * v[194] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[154] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[155] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[156] + v[38] * v[196] + v[137] * v[192] + + v[116] * v[197] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[157] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[158] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[159] + v[38] * v[199] + v[137] * v[195] + + v[116] * v[200] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[160] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[161] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[162] + v[38] * v[202] + v[137] * v[198] + + v[116] * v[203] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[163] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[164] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[165] + v[38] * v[205] + v[137] * v[201] + + v[116] * v[206] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[166] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[167] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[168] + v[38] * v[208] + v[137] * v[204] + + v[116] * v[209] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[169] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[170] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[171] + v[38] * v[211] + v[137] * v[207] + + v[116] * v[212] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[172] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[173] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[174] + v[38] * v[214] + v[137] * v[210] + + v[116] * v[215] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[175] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[176] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[177] + v[38] * v[217] + v[137] * v[213] + + v[116] * v[218] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[178] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[179] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[180] + v[38] * v[220] + v[137] * v[216] + + v[116] * v[221] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[181] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[182] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[183] + v[38] * v[223] + v[137] * v[219] + + v[116] * v[224] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[205] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[206] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[207] + v[38] * v[75] + v[137] * v[92] + + v[116] * v[53] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[208] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[209] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[210] + v[38] * v[48] + v[137] * v[121] + + v[116] * v[23] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[211] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[212] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[213] + v[38] * v[21] + v[137] * v[51] + + v[116] * v[226] + (v[113] - (v[131] * v[116] - v[128] * v[137])) * x[214] + + (v[11] - (v[128] * v[38] - v[132] * v[116])) * x[215] + + (v[16] - (v[132] * v[137] - v[131] * v[38])) * x[216] + v[38] * v[136] + v[137] * v[122] + + v[116] * v[147]; + + // Copy result to output + for (std::size_t i = 0; i < 7; ++i) + { + out[i] = y[i]; + } + } }; } // namespace vamp::robots diff --git a/src/impl/vamp/robots/sphere.hh b/src/impl/vamp/robots/sphere.hh index 76383995..744851ac 100644 --- a/src/impl/vamp/robots/sphere.hh +++ b/src/impl/vamp/robots/sphere.hh @@ -1,6 +1,7 @@ #pragma once #include +#include namespace vamp::robots { @@ -146,5 +147,24 @@ namespace vamp::robots tf.translation() = Eigen::Vector3f(q[0], q[1], q[2]); return tf; } + + template + inline static auto sdf_gradient( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &q) noexcept -> std::pair, FloatVector> + { + printf("sdf_gradient not implemented for sphere\n"); + return std::make_pair(FloatVector(), FloatVector()); + } + + template + inline static void d_collision_d_q( + const ConfigurationBlock &q_in, + const FloatVector &gradients, + ConfigurationBlock &out) noexcept + { + // Note: not tested + printf("d_collision_d_q not implemented for sphere\n"); + } }; } // namespace vamp::robots diff --git a/src/impl/vamp/robots/ur5.hh b/src/impl/vamp/robots/ur5.hh index 5cc815c7..62118722 100644 --- a/src/impl/vamp/robots/ur5.hh +++ b/src/impl/vamp/robots/ur5.hh @@ -853,65 +853,50 @@ namespace vamp::robots output.first.emplace_back( sphere_environment_get_collisions(environment, y[96], y[97], y[98], y[99])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[100], y[101], y[102], y[103])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[100], y[101], y[102], y[103])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[104], y[105], y[106], y[107])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[104], y[105], y[106], y[107])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[108], y[109], y[110], y[111])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[108], y[109], y[110], y[111])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[112], y[113], y[114], y[115])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[112], y[113], y[114], y[115])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[116], y[117], y[118], y[119])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[116], y[117], y[118], y[119])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[120], y[121], y[122], y[123])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[120], y[121], y[122], y[123])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[124], y[125], y[126], y[127])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[124], y[125], y[126], y[127])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[128], y[129], y[130], y[131])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[128], y[129], y[130], y[131])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[132], y[133], y[134], y[135])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[132], y[133], y[134], y[135])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[136], y[137], y[138], y[139])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[136], y[137], y[138], y[139])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[140], y[141], y[142], y[143])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[140], y[141], y[142], y[143])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[144], y[145], y[146], y[147])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[144], y[145], y[146], y[147])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[148], y[149], y[150], y[151])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[148], y[149], y[150], y[151])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[152], y[153], y[154], y[155])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[152], y[153], y[154], y[155])); - output.first.emplace_back( - sphere_environment_get_collisions( - environment, y[156], y[157], y[158], y[159])); + output.first.emplace_back(sphere_environment_get_collisions( + environment, y[156], y[157], y[158], y[159])); if (sphere_sphere_self_collision( y[0], y[1], y[2], y[3], y[8], y[9], y[10], y[11])) @@ -10021,6 +10006,1709 @@ namespace vamp::robots return to_isometry(y.data()); } + + template + inline static auto sdf_gradient( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &x) noexcept + -> std::pair, FloatVector> + { + std::array, 30> v; + std::array, 228> y; + + v[0] = cos(x[0]); + v[1] = sin(x[0]); + v[2] = 0.000796326710733264 * v[0] + -0.999999682931835 * v[1]; + v[3] = sin(x[1]); + v[4] = cos(x[1]); + v[5] = 1.79489656471077e-09 * v[3] + v[4]; + v[6] = v[2] * v[5]; + v[7] = -v[1]; + v[8] = 0.000796326710733264 * v[7] + -0.999999682931835 * v[0]; + y[24] = 0.13585 * v[8]; + y[8] = 0.105 * v[6] + y[24]; + v[1] = 0.999999682931835 * v[0] + 0.000796326710733264 * v[1]; + v[5] = v[1] * v[5]; + v[7] = 0.999999682931835 * v[7] + 0.000796326710733264 * v[0]; + y[25] = 0.13585 * v[7]; + y[9] = 0.105 * v[5] + y[25]; + v[0] = -1. * v[3] + 1.79489656471077e-09 * v[4]; + y[10] = 1.003559 + 0.105 * v[0]; + y[12] = 0.21 * v[6] + y[24]; + y[13] = 0.21 * v[5] + y[25]; + y[14] = 1.003559 + 0.21 * v[0]; + y[16] = 0.315 * v[6] + y[24]; + y[17] = 0.315 * v[5] + y[25]; + y[18] = 1.003559 + 0.315 * v[0]; + y[20] = 0.42 * v[6] + y[24]; + y[21] = 0.42 * v[5] + y[25]; + y[22] = 1.003559 + 0.42 * v[0]; + y[28] = y[24] + -0.1197 * v[8] + 0.425 * v[6]; + y[29] = y[25] + -0.1197 * v[7] + 0.425 * v[5]; + y[30] = 1.003559 + 0.425 * v[0]; + v[3] = -v[3]; + v[9] = 1.79489656471077e-09 * v[4] + v[3]; + v[2] = v[2] * v[9]; + v[10] = sin(x[2]); + v[11] = cos(x[2]); + v[12] = v[2] * v[10] + v[6] * v[11]; + y[32] = 0.1 * v[12] + y[28]; + v[9] = v[1] * v[9]; + v[1] = v[9] * v[10] + v[5] * v[11]; + y[33] = 0.1 * v[1] + y[29]; + v[3] = -1. * v[4] + 1.79489656471077e-09 * v[3]; + v[4] = v[3] * v[10] + v[0] * v[11]; + y[34] = 0.1 * v[4] + y[30]; + y[36] = 0.14 * v[12] + y[28]; + y[37] = 0.14 * v[1] + y[29]; + y[38] = 0.14 * v[4] + y[30]; + y[40] = 0.18 * v[12] + y[28]; + y[41] = 0.18 * v[1] + y[29]; + y[42] = 0.18 * v[4] + y[30]; + y[44] = 0.22 * v[12] + y[28]; + y[45] = 0.22 * v[1] + y[29]; + y[46] = 0.22 * v[4] + y[30]; + y[48] = 0.26 * v[12] + y[28]; + y[49] = 0.26 * v[1] + y[29]; + y[50] = 0.26 * v[4] + y[30]; + y[52] = 0.3 * v[12] + y[28]; + y[53] = 0.3 * v[1] + y[29]; + y[54] = 0.3 * v[4] + y[30]; + y[56] = 0.34 * v[12] + y[28]; + y[57] = 0.34 * v[1] + y[29]; + y[58] = 0.34 * v[4] + y[30]; + y[60] = 0.38 * v[12] + y[28]; + y[61] = 0.38 * v[1] + y[29]; + y[62] = 0.38 * v[4] + y[30]; + v[10] = -v[10]; + v[2] = v[2] * v[11] + v[6] * v[10]; + v[13] = sin(x[3]); + v[14] = cos(x[3]); + v[15] = 1.79489656471077e-09 * v[13] + v[14]; + v[16] = -1. * v[13] + 1.79489656471077e-09 * v[14]; + v[17] = v[2] * v[15] + v[12] * v[16]; + v[18] = y[28] + 0.39225 * v[12]; + y[64] = 0.09 * v[8] + 0.03 * v[17] + v[18]; + v[9] = v[9] * v[11] + v[5] * v[10]; + v[19] = v[9] * v[15] + v[1] * v[16]; + v[20] = y[29] + 0.39225 * v[1]; + y[65] = 0.09 * v[7] + 0.03 * v[19] + v[20]; + v[10] = v[3] * v[11] + v[0] * v[10]; + v[16] = v[10] * v[15] + v[4] * v[16]; + y[74] = y[30] + 0.39225 * v[4]; + y[66] = 0.03 * v[16] + y[74]; + y[68] = 0.09 * v[8] + -0.03 * v[17] + v[18]; + y[69] = 0.09 * v[7] + -0.03 * v[19] + v[20]; + y[70] = -0.03 * v[16] + y[74]; + y[72] = 0.09 * v[8] + v[18]; + y[73] = 0.09 * v[7] + v[20]; + v[13] = -v[13]; + v[15] = 1.79489656471077e-09 * v[14] + v[13]; + v[13] = -1. * v[14] + 1.79489656471077e-09 * v[13]; + v[2] = v[2] * v[15] + v[12] * v[13]; + v[14] = sin(x[4]); + v[3] = -v[14]; + v[11] = cos(x[4]); + v[21] = v[2] * v[3] + v[8] * v[11]; + v[22] = v[18] + 0.093 * v[8]; + y[76] = 0.03 * v[21] + 0.09 * v[17] + v[22]; + v[9] = v[9] * v[15] + v[1] * v[13]; + v[23] = v[9] * v[3] + v[7] * v[11]; + v[24] = v[20] + 0.093 * v[7]; + y[77] = 0.03 * v[23] + 0.09 * v[19] + v[24]; + v[13] = v[10] * v[15] + v[4] * v[13]; + v[3] = v[13] * v[3]; + y[78] = 0.03 * v[3] + 0.09 * v[16] + y[74]; + y[80] = -0.03 * v[21] + 0.09 * v[17] + v[22]; + y[81] = -0.03 * v[23] + 0.09 * v[19] + v[24]; + y[82] = -0.03 * v[3] + 0.09 * v[16] + y[74]; + y[84] = 0.09 * v[17] + v[22]; + y[85] = 0.09 * v[19] + v[24]; + y[86] = 0.09 * v[16] + y[74]; + v[15] = v[22] + 0.09465 * v[17]; + y[88] = 0.06 * v[21] + v[15]; + v[10] = v[24] + 0.09465 * v[19]; + y[89] = 0.06 * v[23] + v[10]; + v[25] = y[74] + 0.09465 * v[16]; + y[90] = 0.06 * v[3] + v[25]; + v[2] = v[2] * v[11] + v[8] * v[14]; + v[26] = cos(x[5]); + v[27] = sin(x[5]); + v[28] = -v[27]; + v[29] = v[2] * v[26] + v[17] * v[28]; + y[92] = 1.59265611381251e-05 * v[29] + 0.0973000063413347 * v[21] + v[15]; + v[9] = v[9] * v[11] + v[7] * v[14]; + v[14] = v[9] * v[26] + v[19] * v[28]; + y[93] = 1.59265611381251e-05 * v[14] + 0.0973000063413347 * v[23] + v[10]; + v[13] = v[13] * v[11]; + v[28] = v[13] * v[26] + v[16] * v[28]; + y[94] = 1.59265611381251e-05 * v[28] + 0.0973000063413347 * v[3] + v[25]; + v[2] = v[2] * v[27] + v[17] * v[26]; + y[96] = -4.77794169794995e-05 * v[29] + 0.177299961951912 * v[21] + 0.000547779602643996 * v[2] + + v[15]; + v[9] = v[9] * v[27] + v[19] * v[26]; + y[97] = -4.77794169794995e-05 * v[14] + 0.177299961951912 * v[23] + 0.000547779602643996 * v[9] + + v[10]; + v[13] = v[13] * v[27] + v[16] * v[26]; + y[98] = -4.77794169794995e-05 * v[28] + 0.177299961951912 * v[3] + 0.000547779602643996 * v[13] + + v[25]; + y[100] = + -1.592643044558e-05 * v[29] + 0.137299987317304 * v[21] + 0.000515926534214666 * v[2] + v[15]; + y[101] = + -1.592643044558e-05 * v[14] + 0.137299987317304 * v[23] + 0.000515926534214666 * v[9] + v[10]; + y[102] = + -1.592643044558e-05 * v[28] + 0.137299987317304 * v[3] + 0.000515926534214666 * v[13] + v[25]; + y[104] = + 0.0326288719300173 * v[29] + 0.206633483191718 * v[21] + 0.000571117947718783 * v[2] + v[15]; + y[105] = + 0.0326288719300173 * v[14] + 0.206633483191718 * v[23] + 0.000571117947718783 * v[9] + v[10]; + y[106] = + 0.0326288719300173 * v[28] + 0.206633483191718 * v[3] + 0.000571117947718783 * v[13] + v[25]; + y[108] = + 0.0471739661448351 * v[29] + 0.257142085198042 * v[21] + 0.000611330073064923 * v[2] + v[15]; + y[109] = + 0.0471739661448351 * v[14] + 0.257142085198042 * v[23] + 0.000611330073064923 * v[9] + v[10]; + y[110] = + 0.0471739661448351 * v[28] + 0.257142085198042 * v[3] + 0.000611330073064923 * v[13] + v[25]; + y[112] = + 0.0471938742614188 * v[29] + 0.232142101051412 * v[21] + 0.000591421905296592 * v[2] + v[15]; + y[113] = + 0.0471938742614188 * v[14] + 0.232142101051412 * v[23] + 0.000591421905296592 * v[9] + v[10]; + y[114] = + 0.0471938742614188 * v[28] + 0.232142101051412 * v[3] + 0.000591421905296592 * v[13] + v[25]; + y[116] = + 0.0305511319550535 * v[29] + 0.180116344904696 * v[21] + 0.000550002959776232 * v[2] + v[15]; + y[117] = + 0.0305511319550535 * v[14] + 0.180116344904696 * v[23] + 0.000550002959776232 * v[9] + v[10]; + y[118] = + 0.0305511319550535 * v[28] + 0.180116344904696 * v[3] + 0.000550002959776232 * v[13] + v[25]; + y[120] = + 0.0622118532100359 * v[29] + 0.218207593376579 * v[21] + 0.000580315961208078 * v[2] + v[15]; + y[121] = + 0.0622118532100359 * v[14] + 0.218207593376579 * v[23] + 0.000580315961208078 * v[9] + v[10]; + y[122] = + 0.0622118532100359 * v[28] + 0.218207593376579 * v[3] + 0.000580315961208078 * v[13] + v[25]; + y[124] = + 0.0622437061965698 * v[29] + 0.178207618741971 * v[21] + 0.000548462892778747 * v[2] + v[15]; + y[125] = + 0.0622437061965698 * v[14] + 0.178207618741971 * v[23] + 0.000548462892778747 * v[9] + v[10]; + y[126] = + 0.0622437061965698 * v[28] + 0.178207618741971 * v[3] + 0.000548462892778747 * v[13] + v[25]; + y[128] = + 0.0622277797033028 * v[29] + 0.198207606059275 * v[21] + 0.000564389426993413 * v[2] + v[15]; + y[129] = + 0.0622277797033028 * v[14] + 0.198207606059275 * v[23] + 0.000564389426993413 * v[9] + v[10]; + y[130] = + 0.0622277797033028 * v[28] + 0.198207606059275 * v[3] + 0.000564389426993413 * v[13] + v[25]; + y[132] = + -0.0327711073338182 * v[29] + 0.206581403542295 * v[21] + 0.000571117947718776 * v[2] + v[15]; + y[133] = + -0.0327711073338182 * v[14] + 0.206581403542295 * v[23] + 0.000571117947718776 * v[9] + v[10]; + y[134] = + -0.0327711073338182 * v[28] + 0.206581403542295 * v[3] + 0.000571117947718776 * v[13] + v[25]; + y[136] = + -0.0469176493992621 * v[29] + 0.257427526148899 * v[21] + 0.000611617044105126 * v[2] + v[15]; + y[137] = + -0.0469176493992621 * v[14] + 0.257427526148899 * v[23] + 0.000611617044105126 * v[9] + v[10]; + y[138] = + -0.0469176493992621 * v[28] + 0.257427526148899 * v[3] + 0.000611617044105126 * v[13] + v[25]; + y[140] = + -0.0468977412826784 * v[29] + 0.232427542002269 * v[21] + 0.000591708876336795 * v[2] + v[15]; + y[141] = + -0.0468977412826784 * v[14] + 0.232427542002269 * v[23] + 0.000591708876336795 * v[9] + v[10]; + y[142] = + -0.0468977412826784 * v[28] + 0.232427542002269 * v[3] + 0.000591708876336795 * v[13] + v[25]; + y[144] = + -0.0306511374916797 * v[29] + 0.180067607997177 * v[21] + 0.000550002959776232 * v[2] + v[15]; + y[145] = + -0.0306511374916797 * v[14] + 0.180067607997177 * v[23] + 0.000550002959776232 * v[9] + v[10]; + y[146] = + -0.0306511374916797 * v[28] + 0.180067607997177 * v[3] + 0.000550002959776232 * v[13] + v[25]; + y[148] = + -0.0623912961612096 * v[29] + 0.218440976014459 * v[21] + 0.000580580825254713 * v[2] + v[15]; + y[149] = + -0.0623912961612096 * v[14] + 0.218440976014459 * v[23] + 0.000580580825254713 * v[9] + v[10]; + y[150] = + -0.0623912961612096 * v[28] + 0.218440976014459 * v[3] + 0.000580580825254713 * v[13] + v[25]; + y[152] = + -0.0623594431746757 * v[29] + 0.178441001379851 * v[21] + 0.000548727756825382 * v[2] + v[15]; + y[153] = + -0.0623594431746757 * v[14] + 0.178441001379851 * v[23] + 0.000548727756825382 * v[9] + v[10]; + y[154] = + -0.0623594431746757 * v[28] + 0.178441001379851 * v[3] + 0.000548727756825382 * v[13] + v[25]; + y[156] = + -0.0623753696679426 * v[29] + 0.198440988697155 * v[21] + 0.000564654291040047 * v[2] + v[15]; + y[157] = + -0.0623753696679426 * v[14] + 0.198440988697155 * v[23] + 0.000564654291040047 * v[9] + v[10]; + y[158] = + -0.0623753696679426 * v[28] + 0.198440988697155 * v[3] + 0.000564654291040047 * v[13] + v[25]; + y[168] = 0.209999993443489 * v[6] + y[24]; + y[169] = 0.209999993443489 * v[5] + y[25]; + y[170] = 1.003559 + 0.209999993443489 * v[0]; + y[172] = 0.170000001788139 * v[12] + y[28]; + y[173] = 0.170000001788139 * v[1] + y[29]; + y[174] = 0.170000001788139 * v[4] + y[30]; + y[176] = 0.0900000035762787 * v[8] + v[18]; + y[177] = 0.0900000035762787 * v[7] + v[20]; + y[180] = 0.0900000035762787 * v[17] + v[22]; + y[181] = 0.0900000035762787 * v[19] + v[24]; + y[182] = 0.0900000035762787 * v[16] + y[74]; + y[184] = 0.0599999986588955 * v[21] + v[15]; + y[185] = 0.0599999986588955 * v[23] + v[10]; + y[186] = 0.0599999986588955 * v[3] + v[25]; + y[188] = 1.59265619004145e-05 * v[29] + 0.0973000079393387 * v[21] + v[15]; + y[189] = 1.59265619004145e-05 * v[14] + 0.0973000079393387 * v[23] + v[10]; + y[190] = 1.59265619004145e-05 * v[28] + 0.0973000079393387 * v[3] + v[25]; + y[192] = -3.18529237119947e-05 * v[29] + 0.157299980521202 * v[21] + 0.000531853060238063 * v[2] + + v[15]; + y[193] = -3.18529237119947e-05 * v[14] + 0.157299980521202 * v[23] + 0.000531853060238063 * v[9] + + v[10]; + y[194] = -3.18529237119947e-05 * v[28] + 0.157299980521202 * v[3] + 0.000531853060238063 * v[13] + + v[25]; + y[196] = + 0.0326288715004921 * v[29] + 0.206633478403091 * v[21] + 0.000571117969229817 * v[2] + v[15]; + y[197] = + 0.0326288715004921 * v[14] + 0.206633478403091 * v[23] + 0.000571117969229817 * v[9] + v[10]; + y[198] = + 0.0326288715004921 * v[28] + 0.206633478403091 * v[3] + 0.000571117969229817 * v[13] + v[25]; + y[200] = 0.047183919698 * v[29] + 0.244642093777657 * v[21] + 0.000601375999394804 * v[2] + v[15]; + y[201] = 0.047183919698 * v[14] + 0.244642093777657 * v[23] + 0.000601375999394804 * v[9] + v[10]; + y[202] = 0.047183919698 * v[28] + 0.244642093777657 * v[3] + 0.000601375999394804 * v[13] + v[25]; + y[204] = + 0.0305511318147182 * v[29] + 0.180116340517998 * v[21] + 0.000550002965610474 * v[2] + v[15]; + y[205] = + 0.0305511318147182 * v[14] + 0.180116340517998 * v[23] + 0.000550002965610474 * v[9] + v[10]; + y[206] = + 0.0305511318147182 * v[28] + 0.180116340517998 * v[3] + 0.000550002965610474 * v[13] + v[25]; + y[208] = + 0.0622277781367302 * v[29] + 0.198207601904869 * v[21] + 0.000564389454666525 * v[2] + v[15]; + y[209] = + 0.0622277781367302 * v[14] + 0.198207601904869 * v[23] + 0.000564389454666525 * v[9] + v[10]; + y[210] = + 0.0622277781367302 * v[28] + 0.198207601904869 * v[3] + 0.000564389454666525 * v[13] + v[25]; + y[212] = + -0.0327711068093777 * v[29] + 0.206581398844719 * v[21] + 0.000571117969229817 * v[2] + v[15]; + y[213] = + -0.0327711068093777 * v[14] + 0.206581398844719 * v[23] + 0.000571117969229817 * v[9] + v[10]; + y[214] = + -0.0327711068093777 * v[28] + 0.206581398844719 * v[3] + 0.000571117969229817 * v[13] + v[25]; + y[216] = + -0.0469076968729496 * v[29] + 0.244927540421486 * v[21] + 0.000601662963163108 * v[2] + v[15]; + y[217] = + -0.0469076968729496 * v[14] + 0.244927540421486 * v[23] + 0.000601662963163108 * v[9] + v[10]; + y[218] = + -0.0469076968729496 * v[28] + 0.244927540421486 * v[3] + 0.000601662963163108 * v[13] + v[25]; + y[220] = + -0.0306511372327805 * v[29] + 0.180067613720894 * v[21] + 0.000550002965610474 * v[2] + v[15]; + y[221] = + -0.0306511372327805 * v[14] + 0.180067613720894 * v[23] + 0.000550002965610474 * v[9] + v[10]; + y[222] = + -0.0306511372327805 * v[28] + 0.180067613720894 * v[3] + 0.000550002965610474 * v[13] + v[25]; + y[224] = + -0.062375370413065 * v[29] + 0.198440983891487 * v[21] + 0.000564654299523681 * v[2] + v[15]; + y[225] = + -0.062375370413065 * v[14] + 0.198440983891487 * v[23] + 0.000564654299523681 * v[9] + v[10]; + y[226] = + -0.062375370413065 * v[28] + 0.198440983891487 * v[3] + 0.000564654299523681 * v[13] + v[25]; + // variable duplicates: 1 + y[178] = y[74]; + // dependent variables without operations + y[0] = 0.; + y[1] = 0.; + y[2] = 0.9144; + y[3] = 0.0799999982118607; + y[4] = 0.; + y[5] = 0.; + y[6] = 1.003559; + y[7] = 0.0799999982118607; + y[11] = 0.0799999982118607; + y[15] = 0.0799999982118607; + y[19] = 0.0799999982118607; + y[23] = 0.0799999982118607; + y[26] = 1.003559; + y[27] = 0.0799999982118607; + y[31] = 0.0799999982118607; + y[35] = 0.0399999991059303; + y[39] = 0.0399999991059303; + y[43] = 0.0399999991059303; + y[47] = 0.0399999991059303; + y[51] = 0.0399999991059303; + y[55] = 0.0399999991059303; + y[59] = 0.0399999991059303; + y[63] = 0.0399999991059303; + y[67] = 0.0399999991059303; + y[71] = 0.0399999991059303; + y[75] = 0.0399999991059303; + y[79] = 0.0399999991059303; + y[83] = 0.0399999991059303; + y[87] = 0.0399999991059303; + y[91] = 0.0399999991059303; + y[95] = 0.0399999991059303; + y[99] = 0.0399999991059303; + y[103] = 0.0399999991059303; + y[107] = 0.0199999995529652; + y[111] = 0.0149999996647239; + y[115] = 0.0149999996647239; + y[119] = 0.0199999995529652; + y[123] = 0.0149999996647239; + y[127] = 0.0149999996647239; + y[131] = 0.0149999996647239; + y[135] = 0.0199999995529652; + y[139] = 0.0149999996647239; + y[143] = 0.0149999996647239; + y[147] = 0.0199999995529652; + y[151] = 0.0149999996647239; + y[155] = 0.0149999996647239; + y[159] = 0.0149999996647239; + y[160] = 0.; + y[161] = 0.; + y[162] = 0.914399981498718; + y[163] = 0.0799999982118607; + y[164] = 0.; + y[165] = 0.; + y[166] = 1.003559; + y[167] = 0.0799999982118607; + y[171] = 0.28999999165535; + y[175] = 0.25; + y[179] = 0.0700000002980232; + y[183] = 0.0700000002980232; + y[187] = 0.0399999991059303; + y[191] = 0.0399999991059303; + y[195] = 0.0599999986588955; + y[199] = 0.0199999995529652; + y[203] = 0.0274999998509884; + y[207] = 0.0199999995529652; + y[211] = 0.0350000001490116; + y[215] = 0.0199999995529652; + y[219] = 0.0274999998509884; + y[223] = 0.0199999995529652; + y[227] = 0.0350000001490116; + + FloatVector dists; + FloatVector grads; + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[0], y[1], y[2], y[3]); + dists[0] = res.first; + grads[0] = res.second[0]; + grads[1] = res.second[1]; + grads[2] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[4], y[5], y[6], y[7]); + dists[1] = res.first; + grads[3] = res.second[0]; + grads[4] = res.second[1]; + grads[5] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[8], y[9], y[10], y[11]); + dists[2] = res.first; + grads[6] = res.second[0]; + grads[7] = res.second[1]; + grads[8] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[12], y[13], y[14], y[15]); + dists[3] = res.first; + grads[9] = res.second[0]; + grads[10] = res.second[1]; + grads[11] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[16], y[17], y[18], y[19]); + dists[4] = res.first; + grads[12] = res.second[0]; + grads[13] = res.second[1]; + grads[14] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[20], y[21], y[22], y[23]); + dists[5] = res.first; + grads[15] = res.second[0]; + grads[16] = res.second[1]; + grads[17] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[24], y[25], y[26], y[27]); + dists[6] = res.first; + grads[18] = res.second[0]; + grads[19] = res.second[1]; + grads[20] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[28], y[29], y[30], y[31]); + dists[7] = res.first; + grads[21] = res.second[0]; + grads[22] = res.second[1]; + grads[23] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[32], y[33], y[34], y[35]); + dists[8] = res.first; + grads[24] = res.second[0]; + grads[25] = res.second[1]; + grads[26] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[36], y[37], y[38], y[39]); + dists[9] = res.first; + grads[27] = res.second[0]; + grads[28] = res.second[1]; + grads[29] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[40], y[41], y[42], y[43]); + dists[10] = res.first; + grads[30] = res.second[0]; + grads[31] = res.second[1]; + grads[32] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[44], y[45], y[46], y[47]); + dists[11] = res.first; + grads[33] = res.second[0]; + grads[34] = res.second[1]; + grads[35] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[48], y[49], y[50], y[51]); + dists[12] = res.first; + grads[36] = res.second[0]; + grads[37] = res.second[1]; + grads[38] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[52], y[53], y[54], y[55]); + dists[13] = res.first; + grads[39] = res.second[0]; + grads[40] = res.second[1]; + grads[41] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[56], y[57], y[58], y[59]); + dists[14] = res.first; + grads[42] = res.second[0]; + grads[43] = res.second[1]; + grads[44] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[60], y[61], y[62], y[63]); + dists[15] = res.first; + grads[45] = res.second[0]; + grads[46] = res.second[1]; + grads[47] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[64], y[65], y[66], y[67]); + dists[16] = res.first; + grads[48] = res.second[0]; + grads[49] = res.second[1]; + grads[50] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[68], y[69], y[70], y[71]); + dists[17] = res.first; + grads[51] = res.second[0]; + grads[52] = res.second[1]; + grads[53] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[72], y[73], y[74], y[75]); + dists[18] = res.first; + grads[54] = res.second[0]; + grads[55] = res.second[1]; + grads[56] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[76], y[77], y[78], y[79]); + dists[19] = res.first; + grads[57] = res.second[0]; + grads[58] = res.second[1]; + grads[59] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[80], y[81], y[82], y[83]); + dists[20] = res.first; + grads[60] = res.second[0]; + grads[61] = res.second[1]; + grads[62] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[84], y[85], y[86], y[87]); + dists[21] = res.first; + grads[63] = res.second[0]; + grads[64] = res.second[1]; + grads[65] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[88], y[89], y[90], y[91]); + dists[22] = res.first; + grads[66] = res.second[0]; + grads[67] = res.second[1]; + grads[68] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[92], y[93], y[94], y[95]); + dists[23] = res.first; + grads[69] = res.second[0]; + grads[70] = res.second[1]; + grads[71] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[96], y[97], y[98], y[99]); + dists[24] = res.first; + grads[72] = res.second[0]; + grads[73] = res.second[1]; + grads[74] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[100], y[101], y[102], y[103]); + dists[25] = res.first; + grads[75] = res.second[0]; + grads[76] = res.second[1]; + grads[77] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[104], y[105], y[106], y[107]); + dists[26] = res.first; + grads[78] = res.second[0]; + grads[79] = res.second[1]; + grads[80] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[108], y[109], y[110], y[111]); + dists[27] = res.first; + grads[81] = res.second[0]; + grads[82] = res.second[1]; + grads[83] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[112], y[113], y[114], y[115]); + dists[28] = res.first; + grads[84] = res.second[0]; + grads[85] = res.second[1]; + grads[86] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[116], y[117], y[118], y[119]); + dists[29] = res.first; + grads[87] = res.second[0]; + grads[88] = res.second[1]; + grads[89] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[120], y[121], y[122], y[123]); + dists[30] = res.first; + grads[90] = res.second[0]; + grads[91] = res.second[1]; + grads[92] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[124], y[125], y[126], y[127]); + dists[31] = res.first; + grads[93] = res.second[0]; + grads[94] = res.second[1]; + grads[95] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[128], y[129], y[130], y[131]); + dists[32] = res.first; + grads[96] = res.second[0]; + grads[97] = res.second[1]; + grads[98] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[132], y[133], y[134], y[135]); + dists[33] = res.first; + grads[99] = res.second[0]; + grads[100] = res.second[1]; + grads[101] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[136], y[137], y[138], y[139]); + dists[34] = res.first; + grads[102] = res.second[0]; + grads[103] = res.second[1]; + grads[104] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[140], y[141], y[142], y[143]); + dists[35] = res.first; + grads[105] = res.second[0]; + grads[106] = res.second[1]; + grads[107] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[144], y[145], y[146], y[147]); + dists[36] = res.first; + grads[108] = res.second[0]; + grads[109] = res.second[1]; + grads[110] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[148], y[149], y[150], y[151]); + dists[37] = res.first; + grads[111] = res.second[0]; + grads[112] = res.second[1]; + grads[113] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[152], y[153], y[154], y[155]); + dists[38] = res.first; + grads[114] = res.second[0]; + grads[115] = res.second[1]; + grads[116] = res.second[2]; + } + + { + auto res = vamp::sphere_environment_signed_distance_and_gradient( + environment, y[156], y[157], y[158], y[159]); + dists[39] = res.first; + grads[117] = res.second[0]; + grads[118] = res.second[1]; + grads[119] = res.second[2]; + } + + return {dists, grads}; + } + + template + inline static void d_collision_d_q( + const ConfigurationBlock &q_in, + const FloatVector &gradients, + ConfigurationBlock &out) noexcept + { + // Generated code expects a single input array 'x' containing + // [q_0, ..., q_n, gx_0, gy_0, gz_0, ..., gx_m, gy_m, gz_m] + std::array, 6 + 40 * 3 + 17 * 3> x; + + // Copy q + for (std::size_t i = 0; i < 6; ++i) + { + x[i] = q_in[i]; + } + + // Copy gradients + for (std::size_t i = 0; i < 40 * 3; ++i) + { + x[6 + i] = gradients[i]; + } + + std::array, 174> v; + std::array, 6> y; + + v[0] = sin(x[0]); + v[1] = -v[0]; + v[2] = cos(x[0]); + v[3] = 0.999999682931835 * v[1] + 0.000796326710733264 * v[2]; + v[4] = 0.13585 * v[3]; + v[1] = 0.000796326710733264 * v[1] + -0.999999682931835 * v[2]; + v[5] = 0.13585 * v[1]; + v[6] = 0.000796326710733264 * v[2] + -0.999999682931835 * v[0]; + v[7] = sin(x[1]); + v[8] = cos(x[1]); + v[9] = 1.79489656471077e-09 * v[7] + v[8]; + v[10] = v[6] * v[9]; + v[11] = 0.105 * v[10]; + v[2] = 0.999999682931835 * v[2] + 0.000796326710733264 * v[0]; + v[9] = v[2] * v[9]; + v[0] = 0.105 * v[9]; + v[12] = 0.21 * v[10]; + v[13] = 0.21 * v[9]; + v[14] = 0.315 * v[10]; + v[15] = 0.315 * v[9]; + v[16] = 0.42 * v[10]; + v[17] = 0.42 * v[9]; + v[18] = v[4] + -0.1197 * v[3] + 0.425 * v[9]; + v[19] = v[5] + -0.1197 * v[1] + 0.425 * v[10]; + v[20] = -v[7]; + v[21] = 1.79489656471077e-09 * v[8] + v[20]; + v[6] = v[6] * v[21]; + v[22] = sin(x[2]); + v[23] = cos(x[2]); + v[24] = v[6] * v[22] + v[10] * v[23]; + v[25] = 0.1 * v[24]; + v[21] = v[2] * v[21]; + v[2] = v[21] * v[22] + v[9] * v[23]; + v[26] = 0.1 * v[2]; + v[27] = 0.14 * v[24]; + v[28] = 0.14 * v[2]; + v[29] = 0.18 * v[24]; + v[30] = 0.18 * v[2]; + v[31] = 0.22 * v[24]; + v[32] = 0.22 * v[2]; + v[33] = 0.26 * v[24]; + v[34] = 0.26 * v[2]; + v[35] = 0.3 * v[24]; + v[36] = 0.3 * v[2]; + v[37] = 0.34 * v[24]; + v[38] = 0.34 * v[2]; + v[39] = 0.38 * v[24]; + v[40] = 0.38 * v[2]; + v[41] = v[18] + 0.39225 * v[2]; + v[42] = v[19] + 0.39225 * v[24]; + v[43] = -v[22]; + v[6] = v[6] * v[23] + v[10] * v[43]; + v[44] = sin(x[3]); + v[45] = cos(x[3]); + v[46] = 1.79489656471077e-09 * v[44] + v[45]; + v[47] = -1. * v[44] + 1.79489656471077e-09 * v[45]; + v[48] = v[6] * v[46] + v[24] * v[47]; + v[49] = 0.09 * v[1] + 0.03 * v[48]; + v[21] = v[21] * v[23] + v[9] * v[43]; + v[50] = v[21] * v[46] + v[2] * v[47]; + v[51] = 0.09 * v[3] + 0.03 * v[50]; + v[52] = 0.09 * v[1] + -0.03 * v[48]; + v[53] = 0.09 * v[3] + -0.03 * v[50]; + v[54] = 0.09 * v[1]; + v[55] = 0.09 * v[3]; + v[56] = v[41] + 0.093 * v[3]; + v[57] = v[42] + 0.093 * v[1]; + v[44] = -v[44]; + v[58] = 1.79489656471077e-09 * v[45] + v[44]; + v[44] = -1. * v[45] + 1.79489656471077e-09 * v[44]; + v[6] = v[6] * v[58] + v[24] * v[44]; + v[45] = sin(x[4]); + v[59] = -v[45]; + v[60] = cos(x[4]); + v[61] = v[6] * v[59] + v[1] * v[60]; + v[62] = 0.03 * v[61] + 0.09 * v[48]; + v[21] = v[21] * v[58] + v[2] * v[44]; + v[63] = v[21] * v[59] + v[3] * v[60]; + v[64] = 0.03 * v[63] + 0.09 * v[50]; + v[65] = v[62] * x[64] - v[64] * x[63]; + v[66] = -0.03 * v[61] + 0.09 * v[48]; + v[67] = -0.03 * v[63] + 0.09 * v[50]; + v[68] = v[66] * x[67] - v[67] * x[66]; + v[69] = 0.09 * v[48]; + v[70] = 0.09 * v[50]; + v[71] = v[69] * x[70] - v[70] * x[69]; + v[72] = v[56] + 0.09465 * v[50]; + v[73] = v[57] + 0.09465 * v[48]; + v[74] = 0.06 * v[61]; + v[75] = 0.06 * v[63]; + v[76] = v[74] * x[73] - v[75] * x[72]; + v[6] = v[6] * v[60] + v[1] * v[45]; + v[77] = cos(x[5]); + v[78] = sin(x[5]); + v[79] = -v[78]; + v[80] = v[6] * v[77] + v[48] * v[79]; + v[81] = 1.59265611381251e-05 * v[80] + 0.0973000063413347 * v[61]; + v[21] = v[21] * v[60] + v[3] * v[45]; + v[45] = v[21] * v[77] + v[50] * v[79]; + v[82] = 1.59265611381251e-05 * v[45] + 0.0973000063413347 * v[63]; + v[83] = v[81] * x[76] - v[82] * x[75]; + v[6] = v[6] * v[78] + v[48] * v[77]; + v[84] = -4.77794169794995e-05 * v[80] + 0.177299961951912 * v[61] + 0.000547779602643996 * v[6]; + v[21] = v[21] * v[78] + v[50] * v[77]; + v[85] = -4.77794169794995e-05 * v[45] + 0.177299961951912 * v[63] + 0.000547779602643996 * v[21]; + v[86] = v[84] * x[79] - v[85] * x[78]; + v[87] = -1.592643044558e-05 * v[80] + 0.137299987317304 * v[61] + 0.000515926534214666 * v[6]; + v[88] = -1.592643044558e-05 * v[45] + 0.137299987317304 * v[63] + 0.000515926534214666 * v[21]; + v[89] = v[87] * x[82] - v[88] * x[81]; + v[90] = 0.0326288719300173 * v[80] + 0.206633483191718 * v[61] + 0.000571117947718783 * v[6]; + v[91] = 0.0326288719300173 * v[45] + 0.206633483191718 * v[63] + 0.000571117947718783 * v[21]; + v[92] = v[90] * x[85] - v[91] * x[84]; + v[93] = 0.0471739661448351 * v[80] + 0.257142085198042 * v[61] + 0.000611330073064923 * v[6]; + v[94] = 0.0471739661448351 * v[45] + 0.257142085198042 * v[63] + 0.000611330073064923 * v[21]; + v[95] = v[93] * x[88] - v[94] * x[87]; + v[96] = 0.0471938742614188 * v[80] + 0.232142101051412 * v[61] + 0.000591421905296592 * v[6]; + v[97] = 0.0471938742614188 * v[45] + 0.232142101051412 * v[63] + 0.000591421905296592 * v[21]; + v[98] = v[96] * x[91] - v[97] * x[90]; + v[99] = 0.0305511319550535 * v[80] + 0.180116344904696 * v[61] + 0.000550002959776232 * v[6]; + v[100] = 0.0305511319550535 * v[45] + 0.180116344904696 * v[63] + 0.000550002959776232 * v[21]; + v[101] = v[99] * x[94] - v[100] * x[93]; + v[102] = 0.0622118532100359 * v[80] + 0.218207593376579 * v[61] + 0.000580315961208078 * v[6]; + v[103] = 0.0622118532100359 * v[45] + 0.218207593376579 * v[63] + 0.000580315961208078 * v[21]; + v[104] = v[102] * x[97] - v[103] * x[96]; + v[105] = 0.0622437061965698 * v[80] + 0.178207618741971 * v[61] + 0.000548462892778747 * v[6]; + v[106] = 0.0622437061965698 * v[45] + 0.178207618741971 * v[63] + 0.000548462892778747 * v[21]; + v[107] = v[105] * x[100] - v[106] * x[99]; + v[108] = 0.0622277797033028 * v[80] + 0.198207606059275 * v[61] + 0.000564389426993413 * v[6]; + v[109] = 0.0622277797033028 * v[45] + 0.198207606059275 * v[63] + 0.000564389426993413 * v[21]; + v[110] = v[108] * x[103] - v[109] * x[102]; + v[111] = -0.0327711073338182 * v[80] + 0.206581403542295 * v[61] + 0.000571117947718776 * v[6]; + v[112] = -0.0327711073338182 * v[45] + 0.206581403542295 * v[63] + 0.000571117947718776 * v[21]; + v[113] = v[111] * x[106] - v[112] * x[105]; + v[114] = -0.0469176493992621 * v[80] + 0.257427526148899 * v[61] + 0.000611617044105126 * v[6]; + v[115] = -0.0469176493992621 * v[45] + 0.257427526148899 * v[63] + 0.000611617044105126 * v[21]; + v[116] = v[114] * x[109] - v[115] * x[108]; + v[117] = -0.0468977412826784 * v[80] + 0.232427542002269 * v[61] + 0.000591708876336795 * v[6]; + v[118] = -0.0468977412826784 * v[45] + 0.232427542002269 * v[63] + 0.000591708876336795 * v[21]; + v[119] = v[117] * x[112] - v[118] * x[111]; + v[120] = -0.0306511374916797 * v[80] + 0.180067607997177 * v[61] + 0.000550002959776232 * v[6]; + v[121] = -0.0306511374916797 * v[45] + 0.180067607997177 * v[63] + 0.000550002959776232 * v[21]; + v[122] = v[120] * x[115] - v[121] * x[114]; + v[123] = -0.0623912961612096 * v[80] + 0.218440976014459 * v[61] + 0.000580580825254713 * v[6]; + v[124] = -0.0623912961612096 * v[45] + 0.218440976014459 * v[63] + 0.000580580825254713 * v[21]; + v[125] = v[123] * x[118] - v[124] * x[117]; + v[126] = -0.0623594431746757 * v[80] + 0.178441001379851 * v[61] + 0.000548727756825382 * v[6]; + v[127] = -0.0623594431746757 * v[45] + 0.178441001379851 * v[63] + 0.000548727756825382 * v[21]; + v[128] = v[126] * x[121] - v[127] * x[120]; + v[129] = -0.0623753696679426 * v[80] + 0.198440988697155 * v[61] + 0.000564654291040047 * v[6]; + v[130] = -0.0623753696679426 * v[45] + 0.198440988697155 * v[63] + 0.000564654291040047 * v[21]; + v[131] = v[129] * x[124] - v[130] * x[123]; + v[10] = 0.209999993443489 * v[10]; + v[9] = 0.209999993443489 * v[9]; + v[24] = 0.170000001788139 * v[24]; + v[2] = 0.170000001788139 * v[2]; + v[1] = 0.0900000035762787 * v[1]; + v[3] = 0.0900000035762787 * v[3]; + v[48] = 0.0900000035762787 * v[48]; + v[50] = 0.0900000035762787 * v[50]; + v[132] = v[48] * x[142] - v[50] * x[141]; + v[133] = 0.0599999986588955 * v[61]; + v[134] = 0.0599999986588955 * v[63]; + v[135] = v[133] * x[145] - v[134] * x[144]; + v[136] = 1.59265619004145e-05 * v[80] + 0.0973000079393387 * v[61]; + v[137] = 1.59265619004145e-05 * v[45] + 0.0973000079393387 * v[63]; + v[138] = v[136] * x[148] - v[137] * x[147]; + v[139] = -3.18529237119947e-05 * v[80] + 0.157299980521202 * v[61] + 0.000531853060238063 * v[6]; + v[140] = -3.18529237119947e-05 * v[45] + 0.157299980521202 * v[63] + 0.000531853060238063 * v[21]; + v[141] = v[139] * x[151] - v[140] * x[150]; + v[142] = 0.0326288715004921 * v[80] + 0.206633478403091 * v[61] + 0.000571117969229817 * v[6]; + v[143] = 0.0326288715004921 * v[45] + 0.206633478403091 * v[63] + 0.000571117969229817 * v[21]; + v[144] = v[142] * x[154] - v[143] * x[153]; + v[145] = 0.047183919698 * v[80] + 0.244642093777657 * v[61] + 0.000601375999394804 * v[6]; + v[146] = 0.047183919698 * v[45] + 0.244642093777657 * v[63] + 0.000601375999394804 * v[21]; + v[147] = v[145] * x[157] - v[146] * x[156]; + v[148] = 0.0305511318147182 * v[80] + 0.180116340517998 * v[61] + 0.000550002965610474 * v[6]; + v[149] = 0.0305511318147182 * v[45] + 0.180116340517998 * v[63] + 0.000550002965610474 * v[21]; + v[150] = v[148] * x[160] - v[149] * x[159]; + v[151] = 0.0622277781367302 * v[80] + 0.198207601904869 * v[61] + 0.000564389454666525 * v[6]; + v[152] = 0.0622277781367302 * v[45] + 0.198207601904869 * v[63] + 0.000564389454666525 * v[21]; + v[153] = v[151] * x[163] - v[152] * x[162]; + v[154] = -0.0327711068093777 * v[80] + 0.206581398844719 * v[61] + 0.000571117969229817 * v[6]; + v[155] = -0.0327711068093777 * v[45] + 0.206581398844719 * v[63] + 0.000571117969229817 * v[21]; + v[156] = v[154] * x[166] - v[155] * x[165]; + v[157] = -0.0469076968729496 * v[80] + 0.244927540421486 * v[61] + 0.000601662963163108 * v[6]; + v[158] = -0.0469076968729496 * v[45] + 0.244927540421486 * v[63] + 0.000601662963163108 * v[21]; + v[159] = v[157] * x[169] - v[158] * x[168]; + v[160] = -0.0306511372327805 * v[80] + 0.180067613720894 * v[61] + 0.000550002965610474 * v[6]; + v[161] = -0.0306511372327805 * v[45] + 0.180067613720894 * v[63] + 0.000550002965610474 * v[21]; + v[162] = v[160] * x[172] - v[161] * x[171]; + v[6] = -0.062375370413065 * v[80] + 0.198440983891487 * v[61] + 0.000564654299523681 * v[6]; + v[21] = -0.062375370413065 * v[45] + 0.198440983891487 * v[63] + 0.000564654299523681 * v[21]; + v[45] = v[6] * x[175] - v[21] * x[174]; + y[0] = + (0. - v[4]) * x[12] + (0. - (0. - v[5])) * x[13] + v[11] * x[13] - v[0] * x[12] + + (0. - v[4]) * x[15] + (0. - (0. - v[5])) * x[16] + v[12] * x[16] - v[13] * x[15] + + (0. - v[4]) * x[18] + (0. - (0. - v[5])) * x[19] + v[14] * x[19] - v[15] * x[18] + + (0. - v[4]) * x[21] + (0. - (0. - v[5])) * x[22] + v[16] * x[22] - v[17] * x[21] + + (0. - v[4]) * x[24] + (0. - (0. - v[5])) * x[25] + (0. - v[18]) * x[27] + + (0. - (0. - v[19])) * x[28] + (0. - v[18]) * x[30] + (0. - (0. - v[19])) * x[31] + + v[25] * x[31] - v[26] * x[30] + (0. - v[18]) * x[33] + (0. - (0. - v[19])) * x[34] + + v[27] * x[34] - v[28] * x[33] + (0. - v[18]) * x[36] + (0. - (0. - v[19])) * x[37] + + v[29] * x[37] - v[30] * x[36] + (0. - v[18]) * x[39] + (0. - (0. - v[19])) * x[40] + + v[31] * x[40] - v[32] * x[39] + (0. - v[18]) * x[42] + (0. - (0. - v[19])) * x[43] + + v[33] * x[43] - v[34] * x[42] + (0. - v[18]) * x[45] + (0. - (0. - v[19])) * x[46] + + v[35] * x[46] - v[36] * x[45] + (0. - v[18]) * x[48] + (0. - (0. - v[19])) * x[49] + + v[37] * x[49] - v[38] * x[48] + (0. - v[18]) * x[51] + (0. - (0. - v[19])) * x[52] + + v[39] * x[52] - v[40] * x[51] + (0. - v[41]) * x[54] + (0. - (0. - v[42])) * x[55] + + v[49] * x[55] - v[51] * x[54] + (0. - v[41]) * x[57] + (0. - (0. - v[42])) * x[58] + + v[52] * x[58] - v[53] * x[57] + (0. - v[41]) * x[60] + (0. - (0. - v[42])) * x[61] + + v[54] * x[61] - v[55] * x[60] + (0. - v[56]) * x[63] + (0. - (0. - v[57])) * x[64] + v[65] + + (0. - v[56]) * x[66] + (0. - (0. - v[57])) * x[67] + v[68] + (0. - v[56]) * x[69] + + (0. - (0. - v[57])) * x[70] + v[71] + (0. - v[72]) * x[72] + (0. - (0. - v[73])) * x[73] + + v[76] + (0. - v[72]) * x[75] + (0. - (0. - v[73])) * x[76] + v[83] + (0. - v[72]) * x[78] + + (0. - (0. - v[73])) * x[79] + v[86] + (0. - v[72]) * x[81] + (0. - (0. - v[73])) * x[82] + + v[89] + (0. - v[72]) * x[84] + (0. - (0. - v[73])) * x[85] + v[92] + (0. - v[72]) * x[87] + + (0. - (0. - v[73])) * x[88] + v[95] + (0. - v[72]) * x[90] + (0. - (0. - v[73])) * x[91] + + v[98] + (0. - v[72]) * x[93] + (0. - (0. - v[73])) * x[94] + v[101] + (0. - v[72]) * x[96] + + (0. - (0. - v[73])) * x[97] + v[104] + (0. - v[72]) * x[99] + (0. - (0. - v[73])) * x[100] + + v[107] + (0. - v[72]) * x[102] + (0. - (0. - v[73])) * x[103] + v[110] + + (0. - v[72]) * x[105] + (0. - (0. - v[73])) * x[106] + v[113] + (0. - v[72]) * x[108] + + (0. - (0. - v[73])) * x[109] + v[116] + (0. - v[72]) * x[111] + (0. - (0. - v[73])) * x[112] + + v[119] + (0. - v[72]) * x[114] + (0. - (0. - v[73])) * x[115] + v[122] + + (0. - v[72]) * x[117] + (0. - (0. - v[73])) * x[118] + v[125] + (0. - v[72]) * x[120] + + (0. - (0. - v[73])) * x[121] + v[128] + (0. - v[72]) * x[123] + (0. - (0. - v[73])) * x[124] + + v[131] + (0. - v[4]) * x[132] + (0. - (0. - v[5])) * x[133] + v[10] * x[133] - v[9] * x[132] + + (0. - v[18]) * x[135] + (0. - (0. - v[19])) * x[136] + v[24] * x[136] - v[2] * x[135] + + (0. - v[41]) * x[138] + (0. - (0. - v[42])) * x[139] + v[1] * x[139] - v[3] * x[138] + + (0. - v[56]) * x[141] + (0. - (0. - v[57])) * x[142] + v[132] + (0. - v[72]) * x[144] + + (0. - (0. - v[73])) * x[145] + v[135] + (0. - v[72]) * x[147] + (0. - (0. - v[73])) * x[148] + + v[138] + (0. - v[72]) * x[150] + (0. - (0. - v[73])) * x[151] + v[141] + + (0. - v[72]) * x[153] + (0. - (0. - v[73])) * x[154] + v[144] + (0. - v[72]) * x[156] + + (0. - (0. - v[73])) * x[157] + v[147] + (0. - v[72]) * x[159] + (0. - (0. - v[73])) * x[160] + + v[150] + (0. - v[72]) * x[162] + (0. - (0. - v[73])) * x[163] + v[153] + + (0. - v[72]) * x[165] + (0. - (0. - v[73])) * x[166] + v[156] + (0. - v[72]) * x[168] + + (0. - (0. - v[73])) * x[169] + v[159] + (0. - v[72]) * x[171] + (0. - (0. - v[73])) * x[172] + + v[162] + (0. - v[72]) * x[174] + (0. - (0. - v[73])) * x[175] + v[45]; + v[63] = sin(x[0]); + v[80] = -v[63]; + v[61] = cos(x[0]); + v[163] = 0.999999682931835 * v[80] + 0.000796326710733264 * v[61]; + v[164] = 0. - 1.003559 * v[163]; + v[80] = 0.000796326710733264 * v[80] + -0.999999682931835 * v[61]; + v[165] = 1.003559 * v[80]; + v[166] = 0.13585 * v[80]; + v[167] = 0.13585 * v[163]; + v[168] = v[166] * v[163] - v[167] * v[80]; + v[7] = -1. * v[7] + 1.79489656471077e-09 * v[8]; + v[169] = 0.105 * v[7]; + v[170] = 0.21 * v[7]; + v[171] = 0.315 * v[7]; + v[172] = 0.42 * v[7]; + v[173] = 1.003559 + 0.425 * v[7]; + v[20] = -1. * v[8] + 1.79489656471077e-09 * v[20]; + v[22] = v[20] * v[22] + v[7] * v[23]; + v[8] = 0.1 * v[22]; + v[26] = v[26] * x[32] - v[8] * x[31]; + v[8] = v[8] * x[30] - v[25] * x[32]; + v[25] = 0.14 * v[22]; + v[28] = v[28] * x[35] - v[25] * x[34]; + v[25] = v[25] * x[33] - v[27] * x[35]; + v[27] = 0.18 * v[22]; + v[30] = v[30] * x[38] - v[27] * x[37]; + v[27] = v[27] * x[36] - v[29] * x[38]; + v[29] = 0.22 * v[22]; + v[32] = v[32] * x[41] - v[29] * x[40]; + v[29] = v[29] * x[39] - v[31] * x[41]; + v[31] = 0.26 * v[22]; + v[34] = v[34] * x[44] - v[31] * x[43]; + v[31] = v[31] * x[42] - v[33] * x[44]; + v[33] = 0.3 * v[22]; + v[36] = v[36] * x[47] - v[33] * x[46]; + v[33] = v[33] * x[45] - v[35] * x[47]; + v[35] = 0.34 * v[22]; + v[38] = v[38] * x[50] - v[35] * x[49]; + v[35] = v[35] * x[48] - v[37] * x[50]; + v[37] = 0.38 * v[22]; + v[40] = v[40] * x[53] - v[37] * x[52]; + v[37] = v[37] * x[51] - v[39] * x[53]; + v[39] = v[173] + 0.39225 * v[22]; + v[20] = v[20] * v[23] + v[7] * v[43]; + v[47] = v[20] * v[46] + v[22] * v[47]; + v[46] = 0.03 * v[47]; + v[51] = v[51] * x[56] - v[46] * x[55]; + v[46] = v[46] * x[54] - v[49] * x[56]; + v[49] = -0.03 * v[47]; + v[53] = v[53] * x[59] - v[49] * x[58]; + v[49] = v[49] * x[57] - v[52] * x[59]; + v[55] = v[55] * x[62]; + v[54] = 0. - v[54] * x[62]; + v[20] = v[20] * v[58] + v[22] * v[44]; + v[59] = v[20] * v[59]; + v[44] = 0.03 * v[59] + 0.09 * v[47]; + v[64] = v[64] * x[65] - v[44] * x[64]; + v[44] = v[44] * x[63] - v[62] * x[65]; + v[62] = -0.03 * v[59] + 0.09 * v[47]; + v[67] = v[67] * x[68] - v[62] * x[67]; + v[62] = v[62] * x[66] - v[66] * x[68]; + v[66] = 0.09 * v[47]; + v[70] = v[70] * x[71] - v[66] * x[70]; + v[66] = v[66] * x[69] - v[69] * x[71]; + v[69] = v[39] + 0.09465 * v[47]; + v[58] = 0.06 * v[59]; + v[75] = v[75] * x[74] - v[58] * x[73]; + v[58] = v[58] * x[72] - v[74] * x[74]; + v[20] = v[20] * v[60]; + v[79] = v[20] * v[77] + v[47] * v[79]; + v[60] = 1.59265611381251e-05 * v[79] + 0.0973000063413347 * v[59]; + v[82] = v[82] * x[77] - v[60] * x[76]; + v[60] = v[60] * x[75] - v[81] * x[77]; + v[20] = v[20] * v[78] + v[47] * v[77]; + v[78] = -4.77794169794995e-05 * v[79] + 0.177299961951912 * v[59] + 0.000547779602643996 * v[20]; + v[85] = v[85] * x[80] - v[78] * x[79]; + v[78] = v[78] * x[78] - v[84] * x[80]; + v[84] = -1.592643044558e-05 * v[79] + 0.137299987317304 * v[59] + 0.000515926534214666 * v[20]; + v[88] = v[88] * x[83] - v[84] * x[82]; + v[84] = v[84] * x[81] - v[87] * x[83]; + v[87] = 0.0326288719300173 * v[79] + 0.206633483191718 * v[59] + 0.000571117947718783 * v[20]; + v[91] = v[91] * x[86] - v[87] * x[85]; + v[87] = v[87] * x[84] - v[90] * x[86]; + v[90] = 0.0471739661448351 * v[79] + 0.257142085198042 * v[59] + 0.000611330073064923 * v[20]; + v[94] = v[94] * x[89] - v[90] * x[88]; + v[90] = v[90] * x[87] - v[93] * x[89]; + v[93] = 0.0471938742614188 * v[79] + 0.232142101051412 * v[59] + 0.000591421905296592 * v[20]; + v[97] = v[97] * x[92] - v[93] * x[91]; + v[93] = v[93] * x[90] - v[96] * x[92]; + v[96] = 0.0305511319550535 * v[79] + 0.180116344904696 * v[59] + 0.000550002959776232 * v[20]; + v[100] = v[100] * x[95] - v[96] * x[94]; + v[96] = v[96] * x[93] - v[99] * x[95]; + v[99] = 0.0622118532100359 * v[79] + 0.218207593376579 * v[59] + 0.000580315961208078 * v[20]; + v[103] = v[103] * x[98] - v[99] * x[97]; + v[99] = v[99] * x[96] - v[102] * x[98]; + v[102] = 0.0622437061965698 * v[79] + 0.178207618741971 * v[59] + 0.000548462892778747 * v[20]; + v[106] = v[106] * x[101] - v[102] * x[100]; + v[102] = v[102] * x[99] - v[105] * x[101]; + v[105] = 0.0622277797033028 * v[79] + 0.198207606059275 * v[59] + 0.000564389426993413 * v[20]; + v[109] = v[109] * x[104] - v[105] * x[103]; + v[105] = v[105] * x[102] - v[108] * x[104]; + v[108] = -0.0327711073338182 * v[79] + 0.206581403542295 * v[59] + 0.000571117947718776 * v[20]; + v[112] = v[112] * x[107] - v[108] * x[106]; + v[108] = v[108] * x[105] - v[111] * x[107]; + v[111] = -0.0469176493992621 * v[79] + 0.257427526148899 * v[59] + 0.000611617044105126 * v[20]; + v[115] = v[115] * x[110] - v[111] * x[109]; + v[111] = v[111] * x[108] - v[114] * x[110]; + v[114] = -0.0468977412826784 * v[79] + 0.232427542002269 * v[59] + 0.000591708876336795 * v[20]; + v[118] = v[118] * x[113] - v[114] * x[112]; + v[114] = v[114] * x[111] - v[117] * x[113]; + v[117] = -0.0306511374916797 * v[79] + 0.180067607997177 * v[59] + 0.000550002959776232 * v[20]; + v[121] = v[121] * x[116] - v[117] * x[115]; + v[117] = v[117] * x[114] - v[120] * x[116]; + v[120] = -0.0623912961612096 * v[79] + 0.218440976014459 * v[59] + 0.000580580825254713 * v[20]; + v[124] = v[124] * x[119] - v[120] * x[118]; + v[120] = v[120] * x[117] - v[123] * x[119]; + v[123] = -0.0623594431746757 * v[79] + 0.178441001379851 * v[59] + 0.000548727756825382 * v[20]; + v[127] = v[127] * x[122] - v[123] * x[121]; + v[123] = v[123] * x[120] - v[126] * x[122]; + v[126] = -0.0623753696679426 * v[79] + 0.198440988697155 * v[59] + 0.000564654291040047 * v[20]; + v[130] = v[130] * x[125] - v[126] * x[124]; + v[126] = v[126] * x[123] - v[129] * x[125]; + v[7] = 0.209999993443489 * v[7]; + v[22] = 0.170000001788139 * v[22]; + v[2] = v[2] * x[137] - v[22] * x[136]; + v[22] = v[22] * x[135] - v[24] * x[137]; + v[3] = v[3] * x[140]; + v[1] = 0. - v[1] * x[140]; + v[47] = 0.0900000035762787 * v[47]; + v[50] = v[50] * x[143] - v[47] * x[142]; + v[47] = v[47] * x[141] - v[48] * x[143]; + v[48] = 0.0599999986588955 * v[59]; + v[134] = v[134] * x[146] - v[48] * x[145]; + v[48] = v[48] * x[144] - v[133] * x[146]; + v[133] = 1.59265619004145e-05 * v[79] + 0.0973000079393387 * v[59]; + v[137] = v[137] * x[149] - v[133] * x[148]; + v[133] = v[133] * x[147] - v[136] * x[149]; + v[136] = -3.18529237119947e-05 * v[79] + 0.157299980521202 * v[59] + 0.000531853060238063 * v[20]; + v[140] = v[140] * x[152] - v[136] * x[151]; + v[136] = v[136] * x[150] - v[139] * x[152]; + v[139] = 0.0326288715004921 * v[79] + 0.206633478403091 * v[59] + 0.000571117969229817 * v[20]; + v[143] = v[143] * x[155] - v[139] * x[154]; + v[139] = v[139] * x[153] - v[142] * x[155]; + v[142] = 0.047183919698 * v[79] + 0.244642093777657 * v[59] + 0.000601375999394804 * v[20]; + v[146] = v[146] * x[158] - v[142] * x[157]; + v[142] = v[142] * x[156] - v[145] * x[158]; + v[145] = 0.0305511318147182 * v[79] + 0.180116340517998 * v[59] + 0.000550002965610474 * v[20]; + v[149] = v[149] * x[161] - v[145] * x[160]; + v[145] = v[145] * x[159] - v[148] * x[161]; + v[148] = 0.0622277781367302 * v[79] + 0.198207601904869 * v[59] + 0.000564389454666525 * v[20]; + v[152] = v[152] * x[164] - v[148] * x[163]; + v[148] = v[148] * x[162] - v[151] * x[164]; + v[151] = -0.0327711068093777 * v[79] + 0.206581398844719 * v[59] + 0.000571117969229817 * v[20]; + v[155] = v[155] * x[167] - v[151] * x[166]; + v[151] = v[151] * x[165] - v[154] * x[167]; + v[154] = -0.0469076968729496 * v[79] + 0.244927540421486 * v[59] + 0.000601662963163108 * v[20]; + v[158] = v[158] * x[170] - v[154] * x[169]; + v[154] = v[154] * x[168] - v[157] * x[170]; + v[157] = -0.0306511372327805 * v[79] + 0.180067613720894 * v[59] + 0.000550002965610474 * v[20]; + v[161] = v[161] * x[173] - v[157] * x[172]; + v[157] = v[157] * x[171] - v[160] * x[173]; + v[20] = -0.062375370413065 * v[79] + 0.198440983891487 * v[59] + 0.000564654299523681 * v[20]; + v[21] = v[21] * x[176] - v[20] * x[175]; + v[20] = v[20] * x[174] - v[6] * x[176]; + y[1] = + (v[164] - (0. - 1.003559 * v[163])) * x[12] + (v[165] - 1.003559 * v[80]) * x[13] + + (v[168] - (v[5] * v[163] - v[4] * v[80])) * x[14] + v[80] * (v[0] * x[14] - v[169] * x[13]) + + v[163] * (v[169] * x[12] - v[11] * x[14]) + (v[164] - (0. - 1.003559 * v[163])) * x[15] + + (v[165] - 1.003559 * v[80]) * x[16] + (v[168] - (v[5] * v[163] - v[4] * v[80])) * x[17] + + v[80] * (v[13] * x[17] - v[170] * x[16]) + v[163] * (v[170] * x[15] - v[12] * x[17]) + + (v[164] - (0. - 1.003559 * v[163])) * x[18] + (v[165] - 1.003559 * v[80]) * x[19] + + (v[168] - (v[5] * v[163] - v[4] * v[80])) * x[20] + v[80] * (v[15] * x[20] - v[171] * x[19]) + + v[163] * (v[171] * x[18] - v[14] * x[20]) + (v[164] - (0. - 1.003559 * v[163])) * x[21] + + (v[165] - 1.003559 * v[80]) * x[22] + (v[168] - (v[5] * v[163] - v[4] * v[80])) * x[23] + + v[80] * (v[17] * x[23] - v[172] * x[22]) + v[163] * (v[172] * x[21] - v[16] * x[23]) + + (v[164] - (0. - 1.003559 * v[163])) * x[24] + (v[165] - 1.003559 * v[80]) * x[25] + + (v[168] - (v[5] * v[163] - v[4] * v[80])) * x[26] + + (v[164] - (0. - v[173] * v[163])) * x[27] + (v[165] - v[173] * v[80]) * x[28] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[29] + + (v[164] - (0. - v[173] * v[163])) * x[30] + (v[165] - v[173] * v[80]) * x[31] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[32] + v[80] * v[26] + v[163] * v[8] + + (v[164] - (0. - v[173] * v[163])) * x[33] + (v[165] - v[173] * v[80]) * x[34] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[35] + v[80] * v[28] + v[163] * v[25] + + (v[164] - (0. - v[173] * v[163])) * x[36] + (v[165] - v[173] * v[80]) * x[37] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[38] + v[80] * v[30] + v[163] * v[27] + + (v[164] - (0. - v[173] * v[163])) * x[39] + (v[165] - v[173] * v[80]) * x[40] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[41] + v[80] * v[32] + v[163] * v[29] + + (v[164] - (0. - v[173] * v[163])) * x[42] + (v[165] - v[173] * v[80]) * x[43] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[44] + v[80] * v[34] + v[163] * v[31] + + (v[164] - (0. - v[173] * v[163])) * x[45] + (v[165] - v[173] * v[80]) * x[46] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[47] + v[80] * v[36] + v[163] * v[33] + + (v[164] - (0. - v[173] * v[163])) * x[48] + (v[165] - v[173] * v[80]) * x[49] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[50] + v[80] * v[38] + v[163] * v[35] + + (v[164] - (0. - v[173] * v[163])) * x[51] + (v[165] - v[173] * v[80]) * x[52] + + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[53] + v[80] * v[40] + v[163] * v[37] + + (v[164] - (0. - v[39] * v[163])) * x[54] + (v[165] - v[39] * v[80]) * x[55] + + (v[168] - (v[42] * v[163] - v[41] * v[80])) * x[56] + v[80] * v[51] + v[163] * v[46] + + (v[164] - (0. - v[39] * v[163])) * x[57] + (v[165] - v[39] * v[80]) * x[58] + + (v[168] - (v[42] * v[163] - v[41] * v[80])) * x[59] + v[80] * v[53] + v[163] * v[49] + + (v[164] - (0. - v[39] * v[163])) * x[60] + (v[165] - v[39] * v[80]) * x[61] + + (v[168] - (v[42] * v[163] - v[41] * v[80])) * x[62] + v[80] * v[55] + v[163] * v[54] + + (v[164] - (0. - v[39] * v[163])) * x[63] + (v[165] - v[39] * v[80]) * x[64] + + (v[168] - (v[57] * v[163] - v[56] * v[80])) * x[65] + v[80] * v[64] + v[163] * v[44] + + (v[164] - (0. - v[39] * v[163])) * x[66] + (v[165] - v[39] * v[80]) * x[67] + + (v[168] - (v[57] * v[163] - v[56] * v[80])) * x[68] + v[80] * v[67] + v[163] * v[62] + + (v[164] - (0. - v[39] * v[163])) * x[69] + (v[165] - v[39] * v[80]) * x[70] + + (v[168] - (v[57] * v[163] - v[56] * v[80])) * x[71] + v[80] * v[70] + v[163] * v[66] + + (v[164] - (0. - v[69] * v[163])) * x[72] + (v[165] - v[69] * v[80]) * x[73] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[74] + v[80] * v[75] + v[163] * v[58] + + (v[164] - (0. - v[69] * v[163])) * x[75] + (v[165] - v[69] * v[80]) * x[76] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[77] + v[80] * v[82] + v[163] * v[60] + + (v[164] - (0. - v[69] * v[163])) * x[78] + (v[165] - v[69] * v[80]) * x[79] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[80] + v[80] * v[85] + v[163] * v[78] + + (v[164] - (0. - v[69] * v[163])) * x[81] + (v[165] - v[69] * v[80]) * x[82] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[83] + v[80] * v[88] + v[163] * v[84] + + (v[164] - (0. - v[69] * v[163])) * x[84] + (v[165] - v[69] * v[80]) * x[85] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[86] + v[80] * v[91] + v[163] * v[87] + + (v[164] - (0. - v[69] * v[163])) * x[87] + (v[165] - v[69] * v[80]) * x[88] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[89] + v[80] * v[94] + v[163] * v[90] + + (v[164] - (0. - v[69] * v[163])) * x[90] + (v[165] - v[69] * v[80]) * x[91] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[92] + v[80] * v[97] + v[163] * v[93] + + (v[164] - (0. - v[69] * v[163])) * x[93] + (v[165] - v[69] * v[80]) * x[94] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[95] + v[80] * v[100] + v[163] * v[96] + + (v[164] - (0. - v[69] * v[163])) * x[96] + (v[165] - v[69] * v[80]) * x[97] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[98] + v[80] * v[103] + v[163] * v[99] + + (v[164] - (0. - v[69] * v[163])) * x[99] + (v[165] - v[69] * v[80]) * x[100] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[101] + v[80] * v[106] + v[163] * v[102] + + (v[164] - (0. - v[69] * v[163])) * x[102] + (v[165] - v[69] * v[80]) * x[103] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[104] + v[80] * v[109] + v[163] * v[105] + + (v[164] - (0. - v[69] * v[163])) * x[105] + (v[165] - v[69] * v[80]) * x[106] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[107] + v[80] * v[112] + v[163] * v[108] + + (v[164] - (0. - v[69] * v[163])) * x[108] + (v[165] - v[69] * v[80]) * x[109] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[110] + v[80] * v[115] + v[163] * v[111] + + (v[164] - (0. - v[69] * v[163])) * x[111] + (v[165] - v[69] * v[80]) * x[112] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[113] + v[80] * v[118] + v[163] * v[114] + + (v[164] - (0. - v[69] * v[163])) * x[114] + (v[165] - v[69] * v[80]) * x[115] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[116] + v[80] * v[121] + v[163] * v[117] + + (v[164] - (0. - v[69] * v[163])) * x[117] + (v[165] - v[69] * v[80]) * x[118] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[119] + v[80] * v[124] + v[163] * v[120] + + (v[164] - (0. - v[69] * v[163])) * x[120] + (v[165] - v[69] * v[80]) * x[121] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[122] + v[80] * v[127] + v[163] * v[123] + + (v[164] - (0. - v[69] * v[163])) * x[123] + (v[165] - v[69] * v[80]) * x[124] + + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[125] + v[80] * v[130] + v[163] * v[126] + + (v[164] - (0. - 1.003559 * v[163])) * x[132] + (v[165] - 1.003559 * v[80]) * x[133] + + (v[168] - (v[5] * v[163] - v[4] * v[80])) * x[134] + v[80] * (v[9] * x[134] - v[7] * x[133]) + + v[163] * (v[7] * x[132] - v[10] * x[134]) + (v[164] - (0. - v[173] * v[163])) * x[135] + + (v[165] - v[173] * v[80]) * x[136] + (v[168] - (v[19] * v[163] - v[18] * v[80])) * x[137] + + v[80] * v[2] + v[163] * v[22] + (v[164] - (0. - v[39] * v[163])) * x[138] + + (v[165] - v[39] * v[80]) * x[139] + (v[168] - (v[42] * v[163] - v[41] * v[80])) * x[140] + + v[80] * v[3] + v[163] * v[1] + (v[164] - (0. - v[39] * v[163])) * x[141] + + (v[165] - v[39] * v[80]) * x[142] + (v[168] - (v[57] * v[163] - v[56] * v[80])) * x[143] + + v[80] * v[50] + v[163] * v[47] + (v[164] - (0. - v[69] * v[163])) * x[144] + + (v[165] - v[69] * v[80]) * x[145] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[146] + + v[80] * v[134] + v[163] * v[48] + (v[164] - (0. - v[69] * v[163])) * x[147] + + (v[165] - v[69] * v[80]) * x[148] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[149] + + v[80] * v[137] + v[163] * v[133] + (v[164] - (0. - v[69] * v[163])) * x[150] + + (v[165] - v[69] * v[80]) * x[151] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[152] + + v[80] * v[140] + v[163] * v[136] + (v[164] - (0. - v[69] * v[163])) * x[153] + + (v[165] - v[69] * v[80]) * x[154] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[155] + + v[80] * v[143] + v[163] * v[139] + (v[164] - (0. - v[69] * v[163])) * x[156] + + (v[165] - v[69] * v[80]) * x[157] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[158] + + v[80] * v[146] + v[163] * v[142] + (v[164] - (0. - v[69] * v[163])) * x[159] + + (v[165] - v[69] * v[80]) * x[160] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[161] + + v[80] * v[149] + v[163] * v[145] + (v[164] - (0. - v[69] * v[163])) * x[162] + + (v[165] - v[69] * v[80]) * x[163] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[164] + + v[80] * v[152] + v[163] * v[148] + (v[164] - (0. - v[69] * v[163])) * x[165] + + (v[165] - v[69] * v[80]) * x[166] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[167] + + v[80] * v[155] + v[163] * v[151] + (v[164] - (0. - v[69] * v[163])) * x[168] + + (v[165] - v[69] * v[80]) * x[169] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[170] + + v[80] * v[158] + v[163] * v[154] + (v[164] - (0. - v[69] * v[163])) * x[171] + + (v[165] - v[69] * v[80]) * x[172] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[173] + + v[80] * v[161] + v[163] * v[157] + (v[164] - (0. - v[69] * v[163])) * x[174] + + (v[165] - v[69] * v[80]) * x[175] + (v[168] - (v[73] * v[163] - v[72] * v[80])) * x[176] + + v[80] * v[21] + v[163] * v[20]; + v[7] = sin(x[1]); + v[172] = cos(x[1]); + v[171] = -1. * v[7] + 1.79489656471077e-09 * v[172]; + v[170] = 1.003559 + 0.425 * v[171]; + v[169] = 0. - v[170] * v[163]; + v[168] = v[170] * v[80]; + v[165] = 0.000796326710733264 * v[61] + -0.999999682931835 * v[63]; + v[164] = 1.79489656471077e-09 * v[7] + v[172]; + v[9] = v[165] * v[164]; + v[166] = v[166] + -0.1197 * v[80] + 0.425 * v[9]; + v[61] = 0.999999682931835 * v[61] + 0.000796326710733264 * v[63]; + v[164] = v[61] * v[164]; + v[167] = v[167] + -0.1197 * v[163] + 0.425 * v[164]; + v[63] = v[166] * v[163] - v[167] * v[80]; + y[2] = (v[169] - (0. - v[173] * v[163])) * x[27] + (v[168] - v[173] * v[80]) * x[28] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[29] + + (v[169] - (0. - v[173] * v[163])) * x[30] + (v[168] - v[173] * v[80]) * x[31] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[32] + v[80] * v[26] + v[163] * v[8] + + (v[169] - (0. - v[173] * v[163])) * x[33] + (v[168] - v[173] * v[80]) * x[34] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[35] + v[80] * v[28] + v[163] * v[25] + + (v[169] - (0. - v[173] * v[163])) * x[36] + (v[168] - v[173] * v[80]) * x[37] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[38] + v[80] * v[30] + v[163] * v[27] + + (v[169] - (0. - v[173] * v[163])) * x[39] + (v[168] - v[173] * v[80]) * x[40] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[41] + v[80] * v[32] + v[163] * v[29] + + (v[169] - (0. - v[173] * v[163])) * x[42] + (v[168] - v[173] * v[80]) * x[43] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[44] + v[80] * v[34] + v[163] * v[31] + + (v[169] - (0. - v[173] * v[163])) * x[45] + (v[168] - v[173] * v[80]) * x[46] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[47] + v[80] * v[36] + v[163] * v[33] + + (v[169] - (0. - v[173] * v[163])) * x[48] + (v[168] - v[173] * v[80]) * x[49] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[50] + v[80] * v[38] + v[163] * v[35] + + (v[169] - (0. - v[173] * v[163])) * x[51] + (v[168] - v[173] * v[80]) * x[52] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[53] + v[80] * v[40] + v[163] * v[37] + + (v[169] - (0. - v[39] * v[163])) * x[54] + (v[168] - v[39] * v[80]) * x[55] + + (v[63] - (v[42] * v[163] - v[41] * v[80])) * x[56] + v[80] * v[51] + v[163] * v[46] + + (v[169] - (0. - v[39] * v[163])) * x[57] + (v[168] - v[39] * v[80]) * x[58] + + (v[63] - (v[42] * v[163] - v[41] * v[80])) * x[59] + v[80] * v[53] + v[163] * v[49] + + (v[169] - (0. - v[39] * v[163])) * x[60] + (v[168] - v[39] * v[80]) * x[61] + + (v[63] - (v[42] * v[163] - v[41] * v[80])) * x[62] + v[80] * v[55] + v[163] * v[54] + + (v[169] - (0. - v[39] * v[163])) * x[63] + (v[168] - v[39] * v[80]) * x[64] + + (v[63] - (v[57] * v[163] - v[56] * v[80])) * x[65] + v[80] * v[64] + v[163] * v[44] + + (v[169] - (0. - v[39] * v[163])) * x[66] + (v[168] - v[39] * v[80]) * x[67] + + (v[63] - (v[57] * v[163] - v[56] * v[80])) * x[68] + v[80] * v[67] + v[163] * v[62] + + (v[169] - (0. - v[39] * v[163])) * x[69] + (v[168] - v[39] * v[80]) * x[70] + + (v[63] - (v[57] * v[163] - v[56] * v[80])) * x[71] + v[80] * v[70] + v[163] * v[66] + + (v[169] - (0. - v[69] * v[163])) * x[72] + (v[168] - v[69] * v[80]) * x[73] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[74] + v[80] * v[75] + v[163] * v[58] + + (v[169] - (0. - v[69] * v[163])) * x[75] + (v[168] - v[69] * v[80]) * x[76] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[77] + v[80] * v[82] + v[163] * v[60] + + (v[169] - (0. - v[69] * v[163])) * x[78] + (v[168] - v[69] * v[80]) * x[79] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[80] + v[80] * v[85] + v[163] * v[78] + + (v[169] - (0. - v[69] * v[163])) * x[81] + (v[168] - v[69] * v[80]) * x[82] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[83] + v[80] * v[88] + v[163] * v[84] + + (v[169] - (0. - v[69] * v[163])) * x[84] + (v[168] - v[69] * v[80]) * x[85] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[86] + v[80] * v[91] + v[163] * v[87] + + (v[169] - (0. - v[69] * v[163])) * x[87] + (v[168] - v[69] * v[80]) * x[88] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[89] + v[80] * v[94] + v[163] * v[90] + + (v[169] - (0. - v[69] * v[163])) * x[90] + (v[168] - v[69] * v[80]) * x[91] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[92] + v[80] * v[97] + v[163] * v[93] + + (v[169] - (0. - v[69] * v[163])) * x[93] + (v[168] - v[69] * v[80]) * x[94] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[95] + v[80] * v[100] + v[163] * v[96] + + (v[169] - (0. - v[69] * v[163])) * x[96] + (v[168] - v[69] * v[80]) * x[97] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[98] + v[80] * v[103] + v[163] * v[99] + + (v[169] - (0. - v[69] * v[163])) * x[99] + (v[168] - v[69] * v[80]) * x[100] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[101] + v[80] * v[106] + v[163] * v[102] + + (v[169] - (0. - v[69] * v[163])) * x[102] + (v[168] - v[69] * v[80]) * x[103] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[104] + v[80] * v[109] + v[163] * v[105] + + (v[169] - (0. - v[69] * v[163])) * x[105] + (v[168] - v[69] * v[80]) * x[106] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[107] + v[80] * v[112] + v[163] * v[108] + + (v[169] - (0. - v[69] * v[163])) * x[108] + (v[168] - v[69] * v[80]) * x[109] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[110] + v[80] * v[115] + v[163] * v[111] + + (v[169] - (0. - v[69] * v[163])) * x[111] + (v[168] - v[69] * v[80]) * x[112] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[113] + v[80] * v[118] + v[163] * v[114] + + (v[169] - (0. - v[69] * v[163])) * x[114] + (v[168] - v[69] * v[80]) * x[115] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[116] + v[80] * v[121] + v[163] * v[117] + + (v[169] - (0. - v[69] * v[163])) * x[117] + (v[168] - v[69] * v[80]) * x[118] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[119] + v[80] * v[124] + v[163] * v[120] + + (v[169] - (0. - v[69] * v[163])) * x[120] + (v[168] - v[69] * v[80]) * x[121] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[122] + v[80] * v[127] + v[163] * v[123] + + (v[169] - (0. - v[69] * v[163])) * x[123] + (v[168] - v[69] * v[80]) * x[124] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[125] + v[80] * v[130] + v[163] * v[126] + + (v[169] - (0. - v[173] * v[163])) * x[135] + (v[168] - v[173] * v[80]) * x[136] + + (v[63] - (v[19] * v[163] - v[18] * v[80])) * x[137] + v[80] * v[2] + v[163] * v[22] + + (v[169] - (0. - v[39] * v[163])) * x[138] + (v[168] - v[39] * v[80]) * x[139] + + (v[63] - (v[42] * v[163] - v[41] * v[80])) * x[140] + v[80] * v[3] + v[163] * v[1] + + (v[169] - (0. - v[39] * v[163])) * x[141] + (v[168] - v[39] * v[80]) * x[142] + + (v[63] - (v[57] * v[163] - v[56] * v[80])) * x[143] + v[80] * v[50] + v[163] * v[47] + + (v[169] - (0. - v[69] * v[163])) * x[144] + (v[168] - v[69] * v[80]) * x[145] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[146] + v[80] * v[134] + v[163] * v[48] + + (v[169] - (0. - v[69] * v[163])) * x[147] + (v[168] - v[69] * v[80]) * x[148] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[149] + v[80] * v[137] + v[163] * v[133] + + (v[169] - (0. - v[69] * v[163])) * x[150] + (v[168] - v[69] * v[80]) * x[151] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[152] + v[80] * v[140] + v[163] * v[136] + + (v[169] - (0. - v[69] * v[163])) * x[153] + (v[168] - v[69] * v[80]) * x[154] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[155] + v[80] * v[143] + v[163] * v[139] + + (v[169] - (0. - v[69] * v[163])) * x[156] + (v[168] - v[69] * v[80]) * x[157] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[158] + v[80] * v[146] + v[163] * v[142] + + (v[169] - (0. - v[69] * v[163])) * x[159] + (v[168] - v[69] * v[80]) * x[160] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[161] + v[80] * v[149] + v[163] * v[145] + + (v[169] - (0. - v[69] * v[163])) * x[162] + (v[168] - v[69] * v[80]) * x[163] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[164] + v[80] * v[152] + v[163] * v[148] + + (v[169] - (0. - v[69] * v[163])) * x[165] + (v[168] - v[69] * v[80]) * x[166] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[167] + v[80] * v[155] + v[163] * v[151] + + (v[169] - (0. - v[69] * v[163])) * x[168] + (v[168] - v[69] * v[80]) * x[169] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[170] + v[80] * v[158] + v[163] * v[154] + + (v[169] - (0. - v[69] * v[163])) * x[171] + (v[168] - v[69] * v[80]) * x[172] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[173] + v[80] * v[161] + v[163] * v[157] + + (v[169] - (0. - v[69] * v[163])) * x[174] + (v[168] - v[69] * v[80]) * x[175] + + (v[63] - (v[73] * v[163] - v[72] * v[80])) * x[176] + v[80] * v[21] + v[163] * v[20]; + v[7] = -v[7]; + v[63] = -1. * v[172] + 1.79489656471077e-09 * v[7]; + v[168] = sin(x[2]); + v[169] = cos(x[2]); + v[22] = v[63] * v[168] + v[171] * v[169]; + v[170] = v[170] + 0.39225 * v[22]; + v[2] = 0. - v[170] * v[163]; + v[37] = v[170] * v[80]; + v[7] = 1.79489656471077e-09 * v[172] + v[7]; + v[165] = v[165] * v[7]; + v[172] = v[165] * v[168] + v[9] * v[169]; + v[166] = v[166] + 0.39225 * v[172]; + v[7] = v[61] * v[7]; + v[61] = v[7] * v[168] + v[164] * v[169]; + v[167] = v[167] + 0.39225 * v[61]; + v[40] = v[166] * v[163] - v[167] * v[80]; + y[3] = (v[2] - (0. - v[39] * v[163])) * x[54] + (v[37] - v[39] * v[80]) * x[55] + + (v[40] - (v[42] * v[163] - v[41] * v[80])) * x[56] + v[80] * v[51] + v[163] * v[46] + + (v[2] - (0. - v[39] * v[163])) * x[57] + (v[37] - v[39] * v[80]) * x[58] + + (v[40] - (v[42] * v[163] - v[41] * v[80])) * x[59] + v[80] * v[53] + v[163] * v[49] + + (v[2] - (0. - v[39] * v[163])) * x[60] + (v[37] - v[39] * v[80]) * x[61] + + (v[40] - (v[42] * v[163] - v[41] * v[80])) * x[62] + v[80] * v[55] + v[163] * v[54] + + (v[2] - (0. - v[39] * v[163])) * x[63] + (v[37] - v[39] * v[80]) * x[64] + + (v[40] - (v[57] * v[163] - v[56] * v[80])) * x[65] + v[80] * v[64] + v[163] * v[44] + + (v[2] - (0. - v[39] * v[163])) * x[66] + (v[37] - v[39] * v[80]) * x[67] + + (v[40] - (v[57] * v[163] - v[56] * v[80])) * x[68] + v[80] * v[67] + v[163] * v[62] + + (v[2] - (0. - v[39] * v[163])) * x[69] + (v[37] - v[39] * v[80]) * x[70] + + (v[40] - (v[57] * v[163] - v[56] * v[80])) * x[71] + v[80] * v[70] + v[163] * v[66] + + (v[2] - (0. - v[69] * v[163])) * x[72] + (v[37] - v[69] * v[80]) * x[73] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[74] + v[80] * v[75] + v[163] * v[58] + + (v[2] - (0. - v[69] * v[163])) * x[75] + (v[37] - v[69] * v[80]) * x[76] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[77] + v[80] * v[82] + v[163] * v[60] + + (v[2] - (0. - v[69] * v[163])) * x[78] + (v[37] - v[69] * v[80]) * x[79] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[80] + v[80] * v[85] + v[163] * v[78] + + (v[2] - (0. - v[69] * v[163])) * x[81] + (v[37] - v[69] * v[80]) * x[82] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[83] + v[80] * v[88] + v[163] * v[84] + + (v[2] - (0. - v[69] * v[163])) * x[84] + (v[37] - v[69] * v[80]) * x[85] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[86] + v[80] * v[91] + v[163] * v[87] + + (v[2] - (0. - v[69] * v[163])) * x[87] + (v[37] - v[69] * v[80]) * x[88] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[89] + v[80] * v[94] + v[163] * v[90] + + (v[2] - (0. - v[69] * v[163])) * x[90] + (v[37] - v[69] * v[80]) * x[91] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[92] + v[80] * v[97] + v[163] * v[93] + + (v[2] - (0. - v[69] * v[163])) * x[93] + (v[37] - v[69] * v[80]) * x[94] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[95] + v[80] * v[100] + v[163] * v[96] + + (v[2] - (0. - v[69] * v[163])) * x[96] + (v[37] - v[69] * v[80]) * x[97] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[98] + v[80] * v[103] + v[163] * v[99] + + (v[2] - (0. - v[69] * v[163])) * x[99] + (v[37] - v[69] * v[80]) * x[100] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[101] + v[80] * v[106] + v[163] * v[102] + + (v[2] - (0. - v[69] * v[163])) * x[102] + (v[37] - v[69] * v[80]) * x[103] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[104] + v[80] * v[109] + v[163] * v[105] + + (v[2] - (0. - v[69] * v[163])) * x[105] + (v[37] - v[69] * v[80]) * x[106] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[107] + v[80] * v[112] + v[163] * v[108] + + (v[2] - (0. - v[69] * v[163])) * x[108] + (v[37] - v[69] * v[80]) * x[109] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[110] + v[80] * v[115] + v[163] * v[111] + + (v[2] - (0. - v[69] * v[163])) * x[111] + (v[37] - v[69] * v[80]) * x[112] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[113] + v[80] * v[118] + v[163] * v[114] + + (v[2] - (0. - v[69] * v[163])) * x[114] + (v[37] - v[69] * v[80]) * x[115] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[116] + v[80] * v[121] + v[163] * v[117] + + (v[2] - (0. - v[69] * v[163])) * x[117] + (v[37] - v[69] * v[80]) * x[118] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[119] + v[80] * v[124] + v[163] * v[120] + + (v[2] - (0. - v[69] * v[163])) * x[120] + (v[37] - v[69] * v[80]) * x[121] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[122] + v[80] * v[127] + v[163] * v[123] + + (v[2] - (0. - v[69] * v[163])) * x[123] + (v[37] - v[69] * v[80]) * x[124] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[125] + v[80] * v[130] + v[163] * v[126] + + (v[2] - (0. - v[39] * v[163])) * x[138] + (v[37] - v[39] * v[80]) * x[139] + + (v[40] - (v[42] * v[163] - v[41] * v[80])) * x[140] + v[80] * v[3] + v[163] * v[1] + + (v[2] - (0. - v[39] * v[163])) * x[141] + (v[37] - v[39] * v[80]) * x[142] + + (v[40] - (v[57] * v[163] - v[56] * v[80])) * x[143] + v[80] * v[50] + v[163] * v[47] + + (v[2] - (0. - v[69] * v[163])) * x[144] + (v[37] - v[69] * v[80]) * x[145] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[146] + v[80] * v[134] + v[163] * v[48] + + (v[2] - (0. - v[69] * v[163])) * x[147] + (v[37] - v[69] * v[80]) * x[148] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[149] + v[80] * v[137] + v[163] * v[133] + + (v[2] - (0. - v[69] * v[163])) * x[150] + (v[37] - v[69] * v[80]) * x[151] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[152] + v[80] * v[140] + v[163] * v[136] + + (v[2] - (0. - v[69] * v[163])) * x[153] + (v[37] - v[69] * v[80]) * x[154] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[155] + v[80] * v[143] + v[163] * v[139] + + (v[2] - (0. - v[69] * v[163])) * x[156] + (v[37] - v[69] * v[80]) * x[157] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[158] + v[80] * v[146] + v[163] * v[142] + + (v[2] - (0. - v[69] * v[163])) * x[159] + (v[37] - v[69] * v[80]) * x[160] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[161] + v[80] * v[149] + v[163] * v[145] + + (v[2] - (0. - v[69] * v[163])) * x[162] + (v[37] - v[69] * v[80]) * x[163] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[164] + v[80] * v[152] + v[163] * v[148] + + (v[2] - (0. - v[69] * v[163])) * x[165] + (v[37] - v[69] * v[80]) * x[166] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[167] + v[80] * v[155] + v[163] * v[151] + + (v[2] - (0. - v[69] * v[163])) * x[168] + (v[37] - v[69] * v[80]) * x[169] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[170] + v[80] * v[158] + v[163] * v[154] + + (v[2] - (0. - v[69] * v[163])) * x[171] + (v[37] - v[69] * v[80]) * x[172] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[173] + v[80] * v[161] + v[163] * v[157] + + (v[2] - (0. - v[69] * v[163])) * x[174] + (v[37] - v[69] * v[80]) * x[175] + + (v[40] - (v[73] * v[163] - v[72] * v[80])) * x[176] + v[80] * v[21] + v[163] * v[20]; + v[167] = v[167] + 0.093 * v[163]; + v[168] = -v[168]; + v[63] = v[63] * v[169] + v[171] * v[168]; + v[171] = sin(x[3]); + v[40] = cos(x[3]); + v[37] = 1.79489656471077e-09 * v[171] + v[40]; + v[2] = -1. * v[171] + 1.79489656471077e-09 * v[40]; + v[1] = v[63] * v[37] + v[22] * v[2]; + v[7] = v[7] * v[169] + v[164] * v[168]; + v[164] = v[7] * v[37] + v[61] * v[2]; + v[3] = v[167] * v[1] - v[170] * v[164]; + v[168] = v[165] * v[169] + v[9] * v[168]; + v[2] = v[168] * v[37] + v[172] * v[2]; + v[166] = v[166] + 0.093 * v[80]; + v[37] = v[170] * v[2] - v[166] * v[1]; + v[165] = v[166] * v[164] - v[167] * v[2]; + y[4] = (v[3] - (v[56] * v[1] - v[39] * v[164])) * x[63] + + (v[37] - (v[39] * v[2] - v[57] * v[1])) * x[64] + + (v[165] - (v[57] * v[164] - v[56] * v[2])) * x[65] + v[2] * v[64] + v[164] * v[44] + + v[1] * v[65] + (v[3] - (v[56] * v[1] - v[39] * v[164])) * x[66] + + (v[37] - (v[39] * v[2] - v[57] * v[1])) * x[67] + + (v[165] - (v[57] * v[164] - v[56] * v[2])) * x[68] + v[2] * v[67] + v[164] * v[62] + + v[1] * v[68] + (v[3] - (v[56] * v[1] - v[39] * v[164])) * x[69] + + (v[37] - (v[39] * v[2] - v[57] * v[1])) * x[70] + + (v[165] - (v[57] * v[164] - v[56] * v[2])) * x[71] + v[2] * v[70] + v[164] * v[66] + + v[1] * v[71] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[72] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[73] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[74] + v[2] * v[75] + v[164] * v[58] + + v[1] * v[76] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[75] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[76] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[77] + v[2] * v[82] + v[164] * v[60] + + v[1] * v[83] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[78] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[79] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[80] + v[2] * v[85] + v[164] * v[78] + + v[1] * v[86] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[81] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[82] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[83] + v[2] * v[88] + v[164] * v[84] + + v[1] * v[89] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[84] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[85] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[86] + v[2] * v[91] + v[164] * v[87] + + v[1] * v[92] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[87] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[88] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[89] + v[2] * v[94] + v[164] * v[90] + + v[1] * v[95] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[90] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[91] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[92] + v[2] * v[97] + v[164] * v[93] + + v[1] * v[98] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[93] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[94] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[95] + v[2] * v[100] + v[164] * v[96] + + v[1] * v[101] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[96] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[97] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[98] + v[2] * v[103] + v[164] * v[99] + + v[1] * v[104] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[99] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[100] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[101] + v[2] * v[106] + v[164] * v[102] + + v[1] * v[107] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[102] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[103] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[104] + v[2] * v[109] + v[164] * v[105] + + v[1] * v[110] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[105] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[106] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[107] + v[2] * v[112] + v[164] * v[108] + + v[1] * v[113] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[108] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[109] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[110] + v[2] * v[115] + v[164] * v[111] + + v[1] * v[116] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[111] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[112] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[113] + v[2] * v[118] + v[164] * v[114] + + v[1] * v[119] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[114] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[115] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[116] + v[2] * v[121] + v[164] * v[117] + + v[1] * v[122] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[117] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[118] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[119] + v[2] * v[124] + v[164] * v[120] + + v[1] * v[125] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[120] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[121] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[122] + v[2] * v[127] + v[164] * v[123] + + v[1] * v[128] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[123] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[124] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[125] + v[2] * v[130] + v[164] * v[126] + + v[1] * v[131] + (v[3] - (v[56] * v[1] - v[39] * v[164])) * x[141] + + (v[37] - (v[39] * v[2] - v[57] * v[1])) * x[142] + + (v[165] - (v[57] * v[164] - v[56] * v[2])) * x[143] + v[2] * v[50] + v[164] * v[47] + + v[1] * v[132] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[144] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[145] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[146] + v[2] * v[134] + v[164] * v[48] + + v[1] * v[135] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[147] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[148] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[149] + v[2] * v[137] + v[164] * v[133] + + v[1] * v[138] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[150] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[151] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[152] + v[2] * v[140] + v[164] * v[136] + + v[1] * v[141] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[153] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[154] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[155] + v[2] * v[143] + v[164] * v[139] + + v[1] * v[144] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[156] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[157] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[158] + v[2] * v[146] + v[164] * v[142] + + v[1] * v[147] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[159] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[160] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[161] + v[2] * v[149] + v[164] * v[145] + + v[1] * v[150] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[162] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[163] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[164] + v[2] * v[152] + v[164] * v[148] + + v[1] * v[153] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[165] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[166] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[167] + v[2] * v[155] + v[164] * v[151] + + v[1] * v[156] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[168] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[169] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[170] + v[2] * v[158] + v[164] * v[154] + + v[1] * v[159] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[171] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[172] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[173] + v[2] * v[161] + v[164] * v[157] + + v[1] * v[162] + (v[3] - (v[72] * v[1] - v[69] * v[164])) * x[174] + + (v[37] - (v[69] * v[2] - v[73] * v[1])) * x[175] + + (v[165] - (v[73] * v[164] - v[72] * v[2])) * x[176] + v[2] * v[21] + v[164] * v[20] + + v[1] * v[45]; + v[164] = v[167] + 0.09465 * v[164]; + v[171] = -v[171]; + v[167] = 1.79489656471077e-09 * v[40] + v[171]; + v[171] = -1. * v[40] + 1.79489656471077e-09 * v[171]; + v[40] = -sin(x[4]); + v[63] = (v[63] * v[167] + v[22] * v[171]) * v[40]; + v[1] = v[170] + 0.09465 * v[1]; + v[170] = cos(x[4]); + v[7] = (v[7] * v[167] + v[61] * v[171]) * v[40] + v[163] * v[170]; + v[61] = v[164] * v[63] - v[1] * v[7]; + v[170] = (v[168] * v[167] + v[172] * v[171]) * v[40] + v[80] * v[170]; + v[166] = v[166] + 0.09465 * v[2]; + v[1] = v[1] * v[170] - v[166] * v[63]; + v[166] = v[166] * v[7] - v[164] * v[170]; + y[5] = (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[72] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[73] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[74] + v[170] * v[75] + v[7] * v[58] + + v[63] * v[76] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[75] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[76] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[77] + v[170] * v[82] + v[7] * v[60] + + v[63] * v[83] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[78] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[79] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[80] + v[170] * v[85] + v[7] * v[78] + + v[63] * v[86] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[81] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[82] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[83] + v[170] * v[88] + v[7] * v[84] + + v[63] * v[89] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[84] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[85] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[86] + v[170] * v[91] + v[7] * v[87] + + v[63] * v[92] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[87] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[88] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[89] + v[170] * v[94] + v[7] * v[90] + + v[63] * v[95] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[90] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[91] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[92] + v[170] * v[97] + v[7] * v[93] + + v[63] * v[98] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[93] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[94] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[95] + v[170] * v[100] + v[7] * v[96] + + v[63] * v[101] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[96] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[97] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[98] + v[170] * v[103] + v[7] * v[99] + + v[63] * v[104] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[99] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[100] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[101] + v[170] * v[106] + v[7] * v[102] + + v[63] * v[107] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[102] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[103] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[104] + v[170] * v[109] + v[7] * v[105] + + v[63] * v[110] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[105] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[106] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[107] + v[170] * v[112] + v[7] * v[108] + + v[63] * v[113] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[108] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[109] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[110] + v[170] * v[115] + v[7] * v[111] + + v[63] * v[116] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[111] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[112] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[113] + v[170] * v[118] + v[7] * v[114] + + v[63] * v[119] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[114] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[115] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[116] + v[170] * v[121] + v[7] * v[117] + + v[63] * v[122] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[117] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[118] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[119] + v[170] * v[124] + v[7] * v[120] + + v[63] * v[125] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[120] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[121] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[122] + v[170] * v[127] + v[7] * v[123] + + v[63] * v[128] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[123] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[124] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[125] + v[170] * v[130] + v[7] * v[126] + + v[63] * v[131] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[144] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[145] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[146] + v[170] * v[134] + v[7] * v[48] + + v[63] * v[135] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[147] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[148] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[149] + v[170] * v[137] + v[7] * v[133] + + v[63] * v[138] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[150] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[151] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[152] + v[170] * v[140] + v[7] * v[136] + + v[63] * v[141] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[153] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[154] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[155] + v[170] * v[143] + v[7] * v[139] + + v[63] * v[144] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[156] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[157] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[158] + v[170] * v[146] + v[7] * v[142] + + v[63] * v[147] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[159] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[160] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[161] + v[170] * v[149] + v[7] * v[145] + + v[63] * v[150] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[162] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[163] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[164] + v[170] * v[152] + v[7] * v[148] + + v[63] * v[153] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[165] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[166] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[167] + v[170] * v[155] + v[7] * v[151] + + v[63] * v[156] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[168] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[169] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[170] + v[170] * v[158] + v[7] * v[154] + + v[63] * v[159] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[171] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[172] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[173] + v[170] * v[161] + v[7] * v[157] + + v[63] * v[162] + (v[61] - (v[72] * v[63] - v[69] * v[7])) * x[174] + + (v[1] - (v[69] * v[170] - v[73] * v[63])) * x[175] + + (v[166] - (v[73] * v[7] - v[72] * v[170])) * x[176] + v[170] * v[21] + v[7] * v[20] + + v[63] * v[45]; + + // Copy result to output + for (std::size_t i = 0; i < 6; ++i) + { + out[i] = y[i]; + } + } }; } // namespace vamp::robots From 59b0d287064ec1cb91d942bbabf0a10f276bdcef Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Mon, 23 Feb 2026 14:52:17 -0600 Subject: [PATCH 3/9] sdf: introduce new namespace `vamp::optimization` and `project.hh` to project the robot config to collision-free config --- src/impl/vamp/optimization/project.hh | 87 +++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 src/impl/vamp/optimization/project.hh diff --git a/src/impl/vamp/optimization/project.hh b/src/impl/vamp/optimization/project.hh new file mode 100644 index 00000000..f42d41cb --- /dev/null +++ b/src/impl/vamp/optimization/project.hh @@ -0,0 +1,87 @@ +#pragma once + +#include +#include +#include + +#include +#include + +namespace vamp::optimization +{ + template + inline auto project_to_valid( + const typename Robot::template ConfigurationBlock &c_in, + const vamp::collision::Environment> &env, + float alpha = 0.1f, + int max_iters = 100) -> typename Robot::template ConfigurationBlock + { + typename Robot::template ConfigurationBlock b = c_in; + float current_min_dist = -1e9f; + int iter = 0; + + while (current_min_dist < 0 && iter < max_iters) + { + auto res = Robot::sdf_gradient(env, b); + + auto dists_arr = res.first.to_array(); + current_min_dist = 1e9f; + for (auto d : dists_arr) + { + if (d < current_min_dist) + { + current_min_dist = d; + } + } + + if (current_min_dist >= 0) + { + break; + } + + typename Robot::template ConfigurationBlock dq_block; + Robot::d_collision_d_q(b, res.second, dq_block); + + std::array norms; + for (auto j = 0U; j < rake; j++) + { + float sum = 0; + for (auto i = 0U; i < Robot::dimension; i++) + { + sum += dq_block[i].element(j) * dq_block[i].element(j); + } + norms[j] = std::sqrt(sum); + } + auto max_norm = *std::max_element(norms.begin(), norms.end()); + std::array data; + if (max_norm > 1e-6f) + { + for (auto i = 0U; i < rake; ++i) + { + if (norms[i] > 1e-6f) + { + for (auto j = 0U; j < Robot::dimension; ++j) + { + data[j * rake + i] = dq_block[j].element(i) / norms[i] * alpha; + } + } + else + { + for (auto j = 0U; j < Robot::dimension; ++j) + { + data[j * rake + i] = 0; + } + } + } + auto dq_block_normalized = typename Robot::template ConfigurationBlock(data); + b = b + dq_block_normalized; + } + else + { + break; + } + iter++; + } + return b; + } +} // namespace vamp::optimization From b1250daab6aacdf6efe708f1abf4ae549f83798d Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Mon, 23 Feb 2026 16:13:40 -0600 Subject: [PATCH 4/9] sdf & python: Bind the projection method in python --- src/impl/vamp/bindings/robot_helper.hh | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index d6c4d303..4bb575ea 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -343,6 +344,23 @@ namespace vamp::binding return filtered; } + + inline static auto project_to_valid( + const Type &c_in, + const EnvironmentInput &environment, + float alpha = 0.1f, + int max_iters = 100) -> Type + { + auto projected_config = vamp::optimization::project_to_valid( + Input::template block(c_in), EnvironmentVector(environment), alpha, max_iters); + auto result_data = projected_config.to_array(); + std::array config_arr; + for (auto i = 0U; i < Robot::dimension; ++i) + { + config_arr[i] = result_data[i*rake]; + } + return Input::from(config_arr); + } }; template @@ -620,6 +638,14 @@ namespace vamp::binding "settings"_a, "rng"_a); + MF("project_to_valid", + project_to_valid, + "Projects a configuration to a valid one.", + "configuration"_a, + "environment"_a = vamp::collision::Environment(), + "alpha"_a = 0.1f, + "max_iter"_a = 100); + #define PLANNER(name, func, desc, ...) \ MF(name, func::single, desc, "start"_a, "goal"_a, "environment"_a, "settings"_a, "rng"_a); \ MF(name, func::multi, desc, "start"_a, "goal"_a, "environment"_a, "settings"_a, "rng"_a); From 8b9a58e9d93a3e7fb3481ab81f158dfd2cdc796f Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Mon, 23 Feb 2026 17:02:04 -0600 Subject: [PATCH 5/9] sdf: add sdf examples --- CMakeLists.txt | 11 ++- scripts/cpp/sdf_example.cc | 101 ++++++++++++++++++++ scripts/cpp/sdf_example_manual.cc | 154 ++++++++++++++++++++++++++++++ 3 files changed, 264 insertions(+), 2 deletions(-) create mode 100644 scripts/cpp/sdf_example.cc create mode 100644 scripts/cpp/sdf_example_manual.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 1217634c..780886c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -141,10 +141,17 @@ endif() if(VAMP_BUILD_CPP_DEMO) add_executable(vamp_rrtc_example scripts/cpp/rrtc_example.cc) target_link_libraries(vamp_rrtc_example PRIVATE vamp_cpp) - - # Disable strict warnings for demos to maintain compatibility + + add_executable(vamp_sdf_example scripts/cpp/sdf_example.cc) + target_link_libraries(vamp_sdf_example PRIVATE vamp_cpp) + + add_executable(vamp_sdf_example_manual scripts/cpp/sdf_example_manual.cc) + target_link_libraries(vamp_sdf_example_manual PRIVATE vamp_cpp) + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") target_compile_options(vamp_rrtc_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) + target_compile_options(vamp_sdf_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) + target_compile_options(vamp_sdf_example_manual PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) endif() endif() diff --git a/scripts/cpp/sdf_example.cc b/scripts/cpp/sdf_example.cc new file mode 100644 index 00000000..fce76ad8 --- /dev/null +++ b/scripts/cpp/sdf_example.cc @@ -0,0 +1,101 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using Robot = vamp::robots::Panda; +static constexpr const std::size_t rake = vamp::FloatVectorWidth; +using EnvironmentInput = vamp::collision::Environment; +using EnvironmentVector = vamp::collision::Environment>; + +// Spheres for the cage problem - (x, y, z) center coordinates with fixed, common radius defined below +static const std::vector> problem = { + {0.55, 0, 0.25}, + {0.35, 0.35, 0.25}, + {0, 0.55, 0.25}, + {-0.55, 0, 0.25}, + {-0.35, -0.35, 0.25}, + {0, -0.55, 0.25}, + {0.35, -0.35, 0.25}, + {-0.35, 0.35, 0.25}, + {0.35, 0.35, 0.8}, + {0, 0.55, 0.8}, + {-0.35, 0.35, 0.8}, + {-0.55, 0, 0.8}, + {-0.35, -0.35, 0.8}, + {0, -0.55, 0.8}, + {0.35, -0.35, 0.8}, + {0.55, 0, 0.8}, +}; + +// Radius for obstacle spheres +static constexpr float radius = 0.2; + +auto main(int, char **) -> int +{ + // Build sphere cage environment + EnvironmentInput environment; + for (const auto &sphere : problem) + { + environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius)); + } + + environment.sort(); + auto env_v = EnvironmentVector(environment); + + // Benchmark + vamp::rng::Halton sampler; + int n_samples = 1000; + int n_success = 0; + double total_time_ms = 0.0; + int total_iter = 0; + std::cout << "Starting Benchmark with " << n_samples << " samples..." << std::endl; + int i = 0; + while (i < n_samples) + { + auto q_random = sampler.next(); + // std::cout << "Sample " << i << ": " << q_random << std::endl; + Robot::ConfigurationBlock b; + for (auto k = 0U; k < Robot::dimension; ++k) + { + b[k] = q_random.broadcast(k); + } + auto valid = Robot::fkcc(env_v, b); + if (valid) + { + continue; + } + i++; + + // Project + auto start_t = std::chrono::steady_clock::now(); + + auto b_final = vamp::optimization::project_to_valid(b, env_v); + + auto end_t = std::chrono::steady_clock::now(); + auto dur = std::chrono::duration_cast(end_t - start_t); + valid = Robot::fkcc(env_v, b_final); + if (valid) + { + n_success++; + } + total_time_ms += dur.count() / 1e6; + } + + std::cout << "Benchmark Results:" << std::endl; + std::cout << "Total Samples: " << n_samples << std::endl; + std::cout << "Successful Projections: " << n_success << " (" + << (n_samples > 0 ? (100.0 * n_success / n_samples) : 0.0) << "%)" << std::endl; + if (n_success > 0) + { + std::cout << "Average Projection Time: " << (total_time_ms / n_samples) << " ms" << std::endl; + } + + return 0; +} diff --git a/scripts/cpp/sdf_example_manual.cc b/scripts/cpp/sdf_example_manual.cc new file mode 100644 index 00000000..a4038041 --- /dev/null +++ b/scripts/cpp/sdf_example_manual.cc @@ -0,0 +1,154 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +using Robot = vamp::robots::Panda; +static constexpr const std::size_t rake = vamp::FloatVectorWidth; +using EnvironmentInput = vamp::collision::Environment; +using EnvironmentVector = vamp::collision::Environment>; + +// Spheres for the cage problem - (x, y, z) center coordinates with fixed, common radius defined below +static const std::vector> problem = { + {0.55, 0, 0.25}, + {0.35, 0.35, 0.25}, + {0, 0.55, 0.25}, + {-0.55, 0, 0.25}, + {-0.35, -0.35, 0.25}, + {0, -0.55, 0.25}, + {0.35, -0.35, 0.25}, + {-0.35, 0.35, 0.25}, + {0.35, 0.35, 0.8}, + {0, 0.55, 0.8}, + {-0.35, 0.35, 0.8}, + {-0.55, 0, 0.8}, + {-0.35, -0.35, 0.8}, + {0, -0.55, 0.8}, + {0.35, -0.35, 0.8}, + {0.55, 0, 0.8}, +}; + +// Radius for obstacle spheres +static constexpr float radius = 0.2; + +auto main(int, char **) -> int +{ + // Build sphere cage environment + EnvironmentInput environment; + for (const auto &sphere : problem) + { + environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius)); + } + + environment.sort(); + auto env_v = EnvironmentVector(environment); + + // Benchmark + vamp::rng::Halton sampler; + int n_samples = 1000; + int n_success = 0; + double total_time_ms = 0.0; + int total_iter = 0; + std::cout << "Starting Benchmark with " << n_samples << " samples..." << std::endl; + int i = 0; + while (i < n_samples) + { + auto q_random = sampler.next(); + // std::cout << "Sample " << i << ": " << q_random << std::endl; + Robot::ConfigurationBlock b; + for (auto k = 0U; k < Robot::dimension; ++k) + { + b[k] = q_random.broadcast(k); + } + auto valid = Robot::fkcc(env_v, b); + if (valid) + { + continue; + } + i++; + + // Project + auto start_t = std::chrono::steady_clock::now(); + + float current_min_dist = -1e9f; + int iter = 0; + const int max_iters = 100; + float alpha = 0.1f; + + while (current_min_dist < 0 && iter < max_iters) + { + // Re-evaluate + auto res = Robot::sdf_gradient(env_v, b); + + auto dists_arr = res.first.to_array(); + current_min_dist = 1e9f; + for (auto d : dists_arr) + { + if (d < current_min_dist) + { + current_min_dist = d; + } + } + + if (current_min_dist >= 0) + { + break; + } + + Robot::ConfigurationBlock dq_block; + Robot::d_collision_d_q(b, res.second, dq_block); // b is already the block for q_new + std::vector dq(Robot::dimension); + for (auto k = 0U; k < Robot::dimension; ++k) + { + dq[k] = dq_block[k].element(0); + } + + float dq_norm = 0.0f; + for (float v : dq) + { + dq_norm += v * v; + } + dq_norm = std::sqrt(dq_norm); + + if (dq_norm > std::numeric_limits::epsilon()) + { + for (auto k = 0U; k < Robot::dimension; ++k) + { + b[k] = b[k] + alpha * (dq[k] / dq_norm); + } + } + else + { + break; + } + iter++; + } + auto end_t = std::chrono::steady_clock::now(); + auto dur = std::chrono::duration_cast(end_t - start_t); + auto b_final = b; + valid = Robot::fkcc(env_v, b_final); + if (valid) + { + n_success++; + } + total_time_ms += dur.count() / 1e6; + total_iter += iter; + } + + std::cout << "Benchmark Results:" << std::endl; + std::cout << "Total Samples: " << n_samples << std::endl; + std::cout << "Successful Projections: " << n_success << " (" + << (n_samples > 0 ? (100.0 * n_success / n_samples) : 0.0) << "%)" << std::endl; + if (n_success > 0) + { + std::cout << "Average Projection Time: " << (total_time_ms / n_samples) << " ms" << std::endl; + std::cout << "Average Iterations: " << (total_iter / n_samples) << std::endl; + } + + return 0; +} From e89b1e3bf674eafc15a9ed0d759b1ecc0affa442 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 26 Feb 2026 13:16:15 -0600 Subject: [PATCH 6/9] sdf: vibe code for capt-sphere min-distance computation --- src/impl/vamp/collision/capt.hh | 99 +++++++++++++++++++++++++++++ src/impl/vamp/collision/validity.hh | 18 +++++- 2 files changed, 116 insertions(+), 1 deletion(-) diff --git a/src/impl/vamp/collision/capt.hh b/src/impl/vamp/collision/capt.hh index 52af89a4..1f20eaf8 100644 --- a/src/impl/vamp/collision/capt.hh +++ b/src/impl/vamp/collision/capt.hh @@ -411,6 +411,51 @@ namespace vamp::collision return false; } + [[nodiscard]] float min_clearance(const Point& center, float r) const noexcept + { + // “Inflate” by point radius (same as collides) + const float R = r + r_point; + + // Optional fast lower-bound reject: if top AABB is far, distance to AABB is a lower bound + // This doesn't give exact min distance, but you can use it as an early return if you want: + // float lb2 = aabb_top.distsq_to(center); + // if (lb2 > some_current_best2) ... + + // Route to leaf (same as collides) + std::size_t test_idx = 0; + for (uint8_t i = 0, k = 0; i < nlog2; i++) + { + test_idx = 2 * test_idx + 1 + (center[k] >= tests[test_idx]); + k = (k + 1) % 3; + } + const std::size_t z = test_idx - tests.size(); + + // If leaf has no afforded points, you'd need a convention; in this build it should. + const uint32_t start = aff_starts[z]; + const uint32_t end = aff_starts[z + 1]; + + // Scan affordance blocks and track minimum squared distance + float best_d2 = std::numeric_limits::infinity(); + + const auto xc = FVectorT::fill(center[0]); + const auto yc = FVectorT::fill(center[1]); + const auto zc = FVectorT::fill(center[2]); + + for (uint32_t i = start; i < end; i++) + { + const auto d2v = sql2_3(affordances[0][i], affordances[1][i], affordances[2][i], xc, yc, zc); + + // You need a way to reduce SIMD lanes -> scalar min. + // Common pattern: + auto arr = d2v.to_array(); // or store to aligned array + for (float d2 : arr) best_d2 = std::min(best_d2, d2); + } + + // Convert to clearance (signed: negative means colliding) + const float best_d = std::sqrt(best_d2); + return best_d - R; + } + // Determine whether any of a set of spheres collides with a point in this tree. // // Templates @@ -505,6 +550,60 @@ namespace vamp::collision return false; } + auto min_clearance_simd(const std::array ¢ers, FVectorT radii) const noexcept -> FVectorT + { + // Pad radii by radius of points. + radii = radii + r_point; + + FVectorT these_tests = FVectorT::fill(tests[0]); + FVectorT cmp_results = centers[0].greater_equal(these_tests); + auto idxs = (cmp_results >> 31U).template as() + 1; + + // Search downward through the tree, parallel across each query lane + for (uint8_t i = 1, k = 1; i < nlog2; i++) + { + these_tests = FVectorT::gather(tests.data(), idxs); + cmp_results = centers[k].greater_equal(these_tests); + idxs = (idxs << 1U) + (cmp_results >> 31U).template as() + 1; + k = (k + 1) % 3; + } + + const IVectorT zs = idxs - tests.size(); + + // Convert the terminal test indices to reference indices for the affordance buffer + const auto *affdata = reinterpret_cast(aff_starts.data()); + const IVectorT starts_v = IVectorT::gather(affdata, zs); + const IVectorT ends_v = IVectorT::gather(affdata, zs + 1); + + const auto starts = starts_v.to_array(); + const auto ends = ends_v.to_array(); + + std::array dist_arr; + + for (uint8_t j = 0; j < FVectorT::num_scalars; j++) + { + const auto xc = centers[0].broadcast(j); + const auto yc = centers[1].broadcast(j); + const auto zc = centers[2].broadcast(j); + + float best_d2 = std::numeric_limits::infinity(); + + for (auto i = starts[j]; i < ends[j]; ++i) + { + const auto d2v = + sql2_3(affordances[0][i], affordances[1][i], affordances[2][i], xc, yc, zc); + auto arr = d2v.to_array(); + for (float d2 : arr) best_d2 = std::min(best_d2, d2); + } + + dist_arr[j] = best_d2; + } + + auto best_d2_vec = FVectorT(dist_arr); + auto best_d_vec = best_d2_vec.sqrt(); + return best_d_vec - radii; + } + auto is_valid() const noexcept -> bool { /// check relative sizing of tests / aff_starts diff --git a/src/impl/vamp/collision/validity.hh b/src/impl/vamp/collision/validity.hh index 7ad1b7d8..8cc6f82d 100644 --- a/src/impl/vamp/collision/validity.hh +++ b/src/impl/vamp/collision/validity.hh @@ -203,7 +203,12 @@ namespace vamp min_dist = min_dist.blend(dist, dist < min_dist); } - // Skip pointclouds for now as SDF is not supported + const std::array positions = {sx, sy, sz}; + for (const auto &pc : e.pointclouds) + { + auto dist = pc.min_clearance_simd(positions, sr); + min_dist = min_dist.blend(dist, dist < min_dist); + } return min_dist; } @@ -500,6 +505,17 @@ namespace vamp } } + const std::array positions = {sx, sy, sz}; + for (const auto &pc : e.pointclouds) + { + auto dist = pc.min_clearance_simd(positions, sr); + auto mask = dist < min_dist; + if (mask.any()) + { + update(dist, DataT(0.0f), DataT(0.0f), DataT(0.0f)); // dummy gradient + } + } + return {min_dist, {gx, gy, gz}}; } From 59ddc9c1c3e9fa16593a2f7a5f986c51ca4fb16a Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 26 Feb 2026 15:15:13 -0600 Subject: [PATCH 7/9] utils: add function to add multiple robots to Viser GUI. --- scripts/viser_utils.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/scripts/viser_utils.py b/scripts/viser_utils.py index 8c407fb9..fcce057e 100644 --- a/scripts/viser_utils.py +++ b/scripts/viser_utils.py @@ -19,6 +19,16 @@ def setup_viser_with_robot(robot_dir, robot_urdf_name): return server, robot +def add_new_robot_to_server(server, robot_dir, robot_urdf_name, **kwargs): + urdf = yourdfpy.URDF.load(str(robot_dir / robot_urdf_name)) + robot = ViserUrdf( + server, + urdf, + load_meshes = True, + load_collision_meshes = False, + **kwargs, + ) + return robot def add_point_cloud( server: viser.ViserServer, From 566b5f1c76803b6c3c6faf74e657ef7fbb94c9e2 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 26 Feb 2026 15:17:05 -0600 Subject: [PATCH 8/9] sdf: add a script to project a configuration in a spherical environment using Viser. --- scripts/sdf_example.py | 71 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 scripts/sdf_example.py diff --git a/scripts/sdf_example.py b/scripts/sdf_example.py new file mode 100644 index 00000000..c285bb65 --- /dev/null +++ b/scripts/sdf_example.py @@ -0,0 +1,71 @@ +import numpy as np +from viser_utils import setup_viser_with_robot, add_new_robot_to_server, add_spheres +from pathlib import Path + +import vamp +from fire import Fire + +# Starting configuration +a = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785] + +# Goal configuration +b = [2.35, 1.0, 0.0, -0.8, 0, 2.5, 0.785] + +# Problem specification: a list of sphere centers +problem = [ + [0.55, 0, 0.25], + [0.35, 0.35, 0.25], + [0, 0.55, 0.25], + [-0.55, 0, 0.25], + [-0.35, -0.35, 0.25], + [0, -0.55, 0.25], + [0.35, -0.35, 0.25], + [0.35, 0.35, 0.8], + [0, 0.55, 0.8], + [-0.35, 0.35, 0.8], + [-0.55, 0, 0.8], + [-0.35, -0.35, 0.8], + [0, -0.55, 0.8], + [0.35, -0.35, 0.8], + ] + + +def main( + obstacle_radius: float = 0.2, + attachment_radius: float = 0.07, + attachment_offset: float = 0.02, + planner: str = "rrtc", + **kwargs, + ): + + (vamp_module, planner_func, plan_settings, + simp_settings) = (vamp.configure_robot_and_planner_with_kwargs("panda", planner, **kwargs)) + + robot_dir = Path(__file__).parents[1] / "resources" / "panda" + server, robot_original = setup_viser_with_robot(robot_dir, "panda_spherized.urdf") + robot_projected = add_new_robot_to_server(server, robot_dir, "panda_spherized.urdf", root_node_name = "/robot_projected", mesh_color_override=(0, 255, 0, 0.5)) + + e = vamp.Environment() + for sphere in problem: + e.add_sphere(vamp.Sphere(sphere, obstacle_radius)) + + _problem_sphere_handles = add_spheres( + server, np.array(problem), np.array([obstacle_radius] * len(problem)) + ) + + sampler = vamp_module.halton() + # Add button to sample and project + sample_button = server.gui.add_button("Sample and Project") + @sample_button.on_click + def _(event): + q = sampler.next() + q_projected = vamp.panda.project_to_valid(q, e) + robot_original.update_cfg(q) + robot_projected.update_cfg(q_projected) + + while True: + continue + + +if __name__ == "__main__": + Fire(main) From 141908383b11c955d11a47bb3c54f01e75903ace Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 26 Feb 2026 15:18:31 -0600 Subject: [PATCH 9/9] format: run `yapf` on the script --- scripts/sdf_example.py | 11 +++++++++-- scripts/viser_utils.py | 1 + 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/scripts/sdf_example.py b/scripts/sdf_example.py index c285bb65..13a78623 100644 --- a/scripts/sdf_example.py +++ b/scripts/sdf_example.py @@ -43,7 +43,13 @@ def main( robot_dir = Path(__file__).parents[1] / "resources" / "panda" server, robot_original = setup_viser_with_robot(robot_dir, "panda_spherized.urdf") - robot_projected = add_new_robot_to_server(server, robot_dir, "panda_spherized.urdf", root_node_name = "/robot_projected", mesh_color_override=(0, 255, 0, 0.5)) + robot_projected = add_new_robot_to_server( + server, + robot_dir, + "panda_spherized.urdf", + root_node_name = "/robot_projected", + mesh_color_override = (0, 255, 0, 0.5) + ) e = vamp.Environment() for sphere in problem: @@ -56,13 +62,14 @@ def main( sampler = vamp_module.halton() # Add button to sample and project sample_button = server.gui.add_button("Sample and Project") + @sample_button.on_click def _(event): q = sampler.next() q_projected = vamp.panda.project_to_valid(q, e) robot_original.update_cfg(q) robot_projected.update_cfg(q_projected) - + while True: continue diff --git a/scripts/viser_utils.py b/scripts/viser_utils.py index fcce057e..8c3be584 100644 --- a/scripts/viser_utils.py +++ b/scripts/viser_utils.py @@ -19,6 +19,7 @@ def setup_viser_with_robot(robot_dir, robot_urdf_name): return server, robot + def add_new_robot_to_server(server, robot_dir, robot_urdf_name, **kwargs): urdf = yourdfpy.URDF.load(str(robot_dir / robot_urdf_name)) robot = ViserUrdf(