diff --git a/blast/blast b/blast/blast index a2d5d31..df9ba1a 100644 --- a/blast/blast +++ b/blast/blast @@ -5,8 +5,9 @@ #include #include #include -#include +#include #include +#include namespace blast { @@ -22,11 +23,11 @@ constexpr int32_t MAX_CAPSULES = BLAST_MAX_CAPSULES; constexpr int32_t MAX_CAPSULES = 7; #endif -using u8 = uint8_t; +using u8 = uint8_t; using u16 = uint16_t; using u32 = uint32_t; using u64 = uint64_t; -using i8 = int8_t; +using i8 = int8_t; using i16 = int16_t; using i32 = int32_t; using i64 = int64_t; @@ -90,11 +91,12 @@ using i64 = int64_t; #include "blast_containers.hpp" #include "blast_utilities.hpp" -#include "blast_task.hpp" -#include "blast_world.hpp" #include "blast_manipulator.hpp" +#include "blast_task.hpp" + #include "blast_optimization.hpp" #include "blast_trajectory.hpp" +#include "blast_world.hpp" #include "utilities/file_io.hpp" #include "utilities/is_close.hpp" diff --git a/blast/blast_optimization.hpp b/blast/blast_optimization.hpp index 2d2d3d1..1d54197 100644 --- a/blast/blast_optimization.hpp +++ b/blast/blast_optimization.hpp @@ -83,6 +83,8 @@ enum class OptimizationMethod : u32 { baseline, // point-based constraints, finite-difference gradients with_analytical_pva, // point-based, analytical gradients for position/velocity/acceleration with_analytical_dynamics, // point-based, analytical gradients for PVA + torque dynamics + broadphase, + double_broadphase, }; struct ConstraintSelection { @@ -117,11 +119,14 @@ struct Optimization { Objective objective; Matrix task; World world; - real trajectory_start_time = 0.0; - real success_tolerance = 0.01; // constraint violation after optimization that is still considered a success - int max_tries = 1; // Maximum number of tries in the optimization loop. - int max_eval = 1000; // Maximum number of function evaluations for a single NLopt call. - real max_time = 30.0; // Maximum time (seconds) for a single NLopt call. + + std::array, MAX_CAPSULES> time_bounding_volume_hierarchies; // should be std::array<> ? Never changes size + + real trajectory_start_time = 0.0; + real success_tolerance = 0.01; // constraint violation after optimization that is still considered a success + int max_tries = 1; // Maximum number of tries in the optimization loop. + int max_eval = 1000; // Maximum number of function evaluations for a single NLopt call. + real max_time = 30.0; // Maximum time (seconds) for a single NLopt call. void* custom_data; @@ -147,9 +152,17 @@ struct Optimization { inline void constraints_and_gradients_with_segments(const Array& x, Optimization& opt, Array& constraints, Matrix& grad); +inline void constraints_and_gradients_with_broadphase(const Array& x, Optimization& opt, Array& constraints, + Matrix& grad); +inline void constraints_and_gradients_with_double_broadphase(const Array& x, Optimization& opt, Array& constraints, + Matrix& grad); // inline void compute_constraints_with_segments(const Array& x, Optimization& opt, Array& constraints); inline void nlopt_constraints_with_segments(unsigned m, real* result, unsigned x_len, const real* x, real* grad, void* f_data); +inline void nlopt_constraints_with_broadphase(unsigned m, real* result, unsigned x_len, const real* x, real* grad, + void* f_data); +inline void nlopt_constraints_with_double_broadphase(unsigned m, real* result, unsigned x_len, const real* x, real* grad, + void* f_data); inline void compute_constraints(real* result, const Array& x, Optimization* opt); inline void nlopt_constraints(unsigned m, real* result, unsigned x_len, const real* x, real* grad, diff --git a/blast/blast_world.hpp b/blast/blast_world.hpp index b0f4e1f..f909fe2 100644 --- a/blast/blast_world.hpp +++ b/blast/blast_world.hpp @@ -13,51 +13,10 @@ struct DynamicBox; struct DynamicSphere; struct DynamicCapsule; struct DynamicDoor; +struct AxisAlignedBoundingBox; +template +struct BoundingVolumeHierarchy; -struct World { - std::vector boxes; - std::vector spheres; - std::vector capsules; - std::vector dynamic_boxes; - std::vector dynamic_spheres; - std::vector dynamic_capsules; - std::vector dynamic_doors; - u32 size = 0; - - host_fn void add_box(const Box& box); - host_fn void add_box(Vec3 center_point, Vec3 half_width, Mat3 rotation_matrix); - - host_fn void add_sphere(const Sphere& sphere); - host_fn void add_sphere(Vec3 center_point, real radius); - - host_fn void add_capsule(const Capsule& capsule); - host_fn void add_capsule(Vec3 point1, Vec3 point2, real radius); - - host_fn void add_dynamic_box(const DynamicBox& box); - host_fn void add_dynamic_box(const std::vector& new_boxes, u32 n_points, real start_time, real end_time); - - host_fn void add_dynamic_sphere(const DynamicSphere& sphere); - host_fn void add_dynamic_sphere(const std::vector& new_spheres, u32 n_points, real start_time, real end_time); - - host_fn void add_dynamic_capsule(const DynamicCapsule& capsule); - host_fn void add_dynamic_capsule(const std::vector& new_capsules, u32 n_points, real start_time, real end_time); -}; - -struct CollisionModel { - std::vector boxes; - std::vector spheres; - std::vector capsules; - u32 size = 0; - - host_fn void add_box(const Box& box); - host_fn void add_box(Vec3 center_point, Vec3 half_width, Mat3 rotation_matrix); - - host_fn void add_sphere(const Sphere& sphere); - host_fn void add_sphere(Vec3 center_point, real radius); - - host_fn void add_capsule(const Capsule& capsule); - host_fn void add_capsule(Vec3 point1, Vec3 point2, real radius); -}; struct PointCloud { Vec3 position; @@ -122,6 +81,146 @@ struct DynamicDoor { inline blast_fn Box lookup(real t) const; }; +enum class CollisionObjectType { + box, + sphere, + capsule, + aabb, + door, +}; +struct CollisionEntities { + CollisionObjectType other_object_type = CollisionObjectType::box; + union { + Box box{}; + Sphere sphere; + Capsule capsule; + }; + + int point_in_segment = 0; +}; + +struct AxisAlignedBoundingBox { + + Vec3 center{}; + Vec3 extents{}; + size_t children[2]; + CollisionObjectType child_type = CollisionObjectType::aabb; + const void* child_ptr = nullptr; + int point_in_segment = -1; + real dist = 0.0; +}; + +struct AABBPair { + int aabb_obj; + int aabb_cap; + real dist; +}; + +struct Compare { + const std::vector* leaves = nullptr; + + bool operator()(int id1, int id2) const { + return (*leaves)[id1].dist > (*leaves)[id2].dist; + } + + bool operator()(const AABBPair& a, const AABBPair& b) const { + return a.dist > b.dist; + } +}; +template +struct BoundingVolumeHierarchy { + + std::vector leaves{}; + + int root = -1; + int num_objects = 0; + real time = 0.0; + + struct PriorityQueue : public std::priority_queue, Compare> { + PriorityQueue(const std::vector* leaves_ptr = nullptr) : + std::priority_queue, Compare>(Compare{leaves_ptr}) {} + + void clear_and_reserve(size_t capacity) { + this->c.clear(); // Clears elements without deallocating vector capacity + this->c.reserve(capacity); // Ensures internal buffer is pre-allocated + } + void rebind(const std::vector* leaves_ptr) { + this->c.clear(); + this->comp = Compare{leaves_ptr}; + } + }; + PriorityQueue queue{&leaves}; + + BoundingVolumeHierarchy() : + queue(&leaves) {} + + // Ensure copy/move operations rebind the comparator's pointer to the local leaves vector + BoundingVolumeHierarchy(const BoundingVolumeHierarchy& other) : + leaves(other.leaves), + root(other.root), + num_objects(other.num_objects), + time(other.time), + queue(&leaves) {} + + BoundingVolumeHierarchy& operator=(const BoundingVolumeHierarchy& other) { + if (this != &other) { + leaves = other.leaves; + root = other.root; + num_objects = other.num_objects; + time = other.time; + queue.rebind(&leaves); + } + return *this; + } +}; + +struct World { + std::vector boxes; + std::vector spheres; + std::vector capsules; + std::vector dynamic_boxes; + std::vector dynamic_spheres; + std::vector dynamic_capsules; + std::vector dynamic_doors; + BoundingVolumeHierarchy static_bounding_volume_hierarchy; + BoundingVolumeHierarchy dynamic_bounding_volume_hierarchy; + u32 size = 0; + // ... + host_fn void add_box(const Box& box); + host_fn void add_box(Vec3 center_point, Vec3 half_width, Mat3 rotation_matrix); + + host_fn void add_sphere(const Sphere& sphere); + host_fn void add_sphere(Vec3 center_point, real radius); + + host_fn void add_capsule(const Capsule& capsule); + host_fn void add_capsule(Vec3 point1, Vec3 point2, real radius); + + host_fn void add_dynamic_box(const DynamicBox& box); + host_fn void add_dynamic_box(const std::vector& new_boxes, u32 n_points, real start_time, real end_time); + + host_fn void add_dynamic_sphere(const DynamicSphere& sphere); + host_fn void add_dynamic_sphere(const std::vector& new_spheres, u32 n_points, real start_time, real end_time); + + host_fn void add_dynamic_capsule(const DynamicCapsule& capsule); + host_fn void add_dynamic_capsule(const std::vector& new_capsules, u32 n_points, real start_time, real end_time); +}; + +struct CollisionModel { + std::vector boxes; + std::vector spheres; + std::vector capsules; + u32 size = 0; + + host_fn void add_box(const Box& box); + host_fn void add_box(Vec3 center_point, Vec3 half_width, Mat3 rotation_matrix); + + host_fn void add_sphere(const Sphere& sphere); + host_fn void add_sphere(Vec3 center_point, real radius); + + host_fn void add_capsule(const Capsule& capsule); + host_fn void add_capsule(Vec3 point1, Vec3 point2, real radius); +}; + /** * @struct CollisionModelCapsule * @brief Simple capsule primitive for collision checking. @@ -146,6 +245,7 @@ inline blast_fn real distance(const Capsule& capsule1, const Capsule& capsule2); inline blast_fn real distance(const Capsule& capsule, const Vec3& point); inline blast_fn real distance(const Box& box, const Vec3& point); inline blast_fn real distance(const Sphere& sphere, const Vec3& point); +inline blast_fn real distance(const AxisAlignedBoundingBox& aabb1, const AxisAlignedBoundingBox& aabb2); inline blast_fn Vec3 get_point(const Array& x, const Matrix& capsule_list); @@ -162,6 +262,7 @@ inline blast_fn Vec3 get_point(const Array& x, const Matrix& capsule_list); #include "world/dynamicsphere.hpp" #include "world/CoDO.hpp" +#include "world/broadphase.hpp" #include "world/scenes.hpp" diff --git a/blast/optimization/constraints.hpp b/blast/optimization/constraints.hpp index 39584f5..9c12744 100644 --- a/blast/optimization/constraints.hpp +++ b/blast/optimization/constraints.hpp @@ -66,24 +66,6 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt // constraints (p,v,a,tor) for each joint, for each segment // [p1, p2,..., v1, v2,..., a1, a2,..., t1, t2,...] - enum class CollisionObjectType { - box, - sphere, - capsule, - }; - struct CollisionEntities { - // object in the world - CollisionObjectType other_object_type = CollisionObjectType::box; - union { - Box box{}; - Sphere sphere; - Capsule capsule; - }; - - // which point in time to look up the capsule - int point_in_segment = 0; - }; - // basis: n_ctrl x n_points // Assert(constraints.is_alias); todo: fix for validation if (grad.size) { @@ -298,7 +280,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt // --- Dynamic tests --- todo: check & fix gradients int current_point = segment * n_points_per_segment + point_in_segment; - int max_point = n_segments * n_points_per_segment - 1; // todo: check -1 ? + int max_point = n_segments * n_points_per_segment - 1; real current_time = x.back() * ((real) current_point / (real) max_point) + opt.trajectory_start_time; // trajectory time * progression along trajectory count = 0; @@ -758,25 +740,1291 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt } } -inline blast_fn void nlopt_constraints_with_segments(unsigned m, real* result, unsigned x_len, const real* x, real* grad, void* f_data) { -#if BLAST_TRACE_LEVEL >= 1 - PROFILE_FUNCTION; -#endif - auto* opt = (Optimization*) f_data; +inline blast_fn void constraints_and_gradients_with_broadphase(const Array& x, Optimization& opt, Array& constraints, Matrix& grad) { + // BVH is currently for static objects only - Array xv; - xv.alias(x, x_len); + // constraints (p,v,a,tor) for each joint, for each segment + // [p1, p2,..., v1, v2,..., a1, a2,..., t1, t2,...] - Array constraints; - constraints.alias(result, m); + // basis: n_ctrl x n_points + // Assert(constraints.is_alias); todo: fix for validation + if (grad.size) { + Assert(grad.is_alias); + Assert(grad.rows == x.size); + Assert(grad.cols == constraints.size); + } + const int n_segments = (int) opt.bspline.n_ctrl - (int) opt.bspline.degree; + const int n_points_per_segment = (int) opt.bspline.n_points / n_segments; // todo: check if fine? + const int n_joints = (int) opt.manip.n_joints; + const int n_ctrl = (int) opt.bspline.n_ctrl; + const int x_len = (int) x.size; + const int n_capsules = opt.manip._n_caps; + const int n_constraints_per_segment = opt.constraints.n_constraints_per_segment; + Assert(constraints.size == n_segments * n_constraints_per_segment); - Matrix gradients; - if (grad) { - memset(grad, 0, m * x_len * sizeof(real)); - gradients.alias(grad, x_len, m); + const auto& world = opt.world; + + // limits + auto pmax = opt.manip.position_max; + auto pmin = opt.manip.position_min; + auto vmax = opt.manip.velocity_max; + auto amax = opt.manip.acceleration_max; + auto tau_max = opt.manip.torque_max; + auto tool_speed_max = opt.manip.tool_speed_max; + + for (int j = 0; j < n_joints; j++) { + // todo: document the current behaviour in the API + // (doesn't currently work if one is inf and the other is not) + if (pmax[j] == INF_REAL) // note: replace INF_REAL with huge value + pmax[j] = 1e300; + if (pmin[j] == -INF_REAL) // note: replace -INF_REAL with huge negative value + pmin[j] = -1e300; } - constraints_and_gradients_with_segments(xv, *opt, constraints, gradients); + ManipulatorTempData manip_data; + std::array max_pos_indices{}; + std::array max_vel_indices{}; + std::array max_acc_indices{}; + std::array max_tor_indices{}; + std::array max_collision_entities{}; + u8 max_internal_collision_index = 0; + u8 max_tool_index = 0; + + opt.bspline.compute_trajectory(x, opt.task); + + + for (int segment = 0; segment < n_segments; segment++) { +#if BLAST_TRACE_LEVEL >= 2 + PROFILE_SCOPE("All Segment Constraints"); +#endif + const int first_affected_control_point = std::max(3, segment); + const int last_affected_control_point = std::min((n_ctrl - 1) - 3, segment + (int) opt.bspline.degree); + const int n_affected_control_points = last_affected_control_point - first_affected_control_point + 1; // note: affected_control_points are inclusive, so when we have last = 5, first = 3, we want 3 (5 - 3) + 1 + const int start_point_for_segment = segment * n_points_per_segment; + Assert(n_affected_control_points >= 3); + Assert(n_affected_control_points <= 6); + + Matrix bp(&opt.bspline.basis_p(0, start_point_for_segment), n_ctrl, n_points_per_segment); + Matrix bv(&opt.bspline.basis_v(0, start_point_for_segment), n_ctrl, n_points_per_segment); + Matrix ba(&opt.bspline.basis_a(0, start_point_for_segment), n_ctrl, n_points_per_segment); + + Array max_pos_constraints(n_joints, -INF_REAL); + Array max_vel_constraints(n_joints, -INF_REAL); + Array max_acc_constraints(n_joints, -INF_REAL); + Array max_tor_constraints(n_joints, -INF_REAL); + real max_tool_speed_constraints = -INF_REAL; + real max_internal_col_constraints = -INF_REAL; // todo: worst or worst per capsule ? + Array max_col_constraints(n_capsules, -INF_REAL); + + for (int point_in_segment = 0; point_in_segment < n_points_per_segment; point_in_segment++) { + { +#if BLAST_TRACE_LEVEL >= 2 + PROFILE_SCOPE("All Point Constraints"); +#endif + auto p = opt.bspline.traj.pos.col(start_point_for_segment + point_in_segment); + auto v = opt.bspline.traj.vel.col(start_point_for_segment + point_in_segment); + auto a = opt.bspline.traj.acc.col(start_point_for_segment + point_in_segment); + + forward_kinematics(opt.manip, manip_data, p); + compute_collision_model(opt.manip, manip_data); + dynamics(opt.manip, manip_data, v, a); + + for (int j = 0; j < n_joints; j++) { + // position + if (opt.constraints.position) { + +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Pos Constraints"); +#endif + + if (const auto c = bound_constraint(p[j], pmin[j], pmax[j]); + c > max_pos_constraints[j]) { + max_pos_constraints[j] = c; + max_pos_indices[j] = point_in_segment; + } + } + // velocity + if (opt.constraints.velocity) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Vel Constraints"); +#endif + if (const auto c = std::abs(v[j]) / vmax[j] - 1.0; + c > max_vel_constraints[j]) { + max_vel_constraints[j] = c; + max_vel_indices[j] = point_in_segment; + } + } + // acceleration + if (opt.constraints.acceleration) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Acc Constraints"); +#endif + if (const auto c = std::abs(a[j]) / amax[j] - 1.0; + c > max_acc_constraints[j]) { + max_acc_constraints[j] = c; + max_acc_indices[j] = point_in_segment; + } + } + // torque + if (opt.constraints.torque) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Tau Constraints"); +#endif + if (const auto c = std::abs(manip_data.efforts[j]) / tau_max[j] - 1.0; + c > max_tor_constraints[j]) { + max_tor_constraints[j] = c; + max_tor_indices[j] = point_in_segment; + } + } + } + + // Tool speed + if (opt.constraints.tool_speed) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Tool Constraints"); +#endif + const auto J_tool = get_J_tool(&opt, manip_data); // todo: clean up get_J_tool to a get_tool_speed + const auto tool_speed = norm(get_J_tool(&opt, manip_data) * v); + if (const auto c = bound_constraint(tool_speed, 0.0, tool_speed_max); + c > max_tool_speed_constraints) { + max_tool_speed_constraints = c; + max_tool_index = point_in_segment; + } + } + + // self collision + if (opt.constraints.self_collisions) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Self Constraints"); +#endif + // check every internal collision + if (const auto c = max(-get_internal_collisions(opt.manip, manip_data)); + c > max_internal_col_constraints) { + max_internal_col_constraints = c; + max_internal_collision_index = point_in_segment; + } + } + + // external collision + if (opt.constraints.external_collisions) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Ext Constraints"); +#endif + int current_point = segment * n_points_per_segment + point_in_segment; + int max_point = n_segments * n_points_per_segment - 1; + real current_time = x.back() * ((real) current_point / (real) max_point) + opt.trajectory_start_time; // trajectory time * progression along trajectory + + // dynamic BVH doesn't have to be created inside capsule for-loop since its the same for every capsule + auto& static_bvh = opt.world.static_bounding_volume_hierarchy; + auto& dynamic_bvh = opt.world.dynamic_bounding_volume_hierarchy; + create_dynamic_bounding_volume_hierarchy(opt.world, dynamic_bvh, current_time); + + // find worst collision + for (int capsule_id = 0; capsule_id < n_capsules; capsule_id++) { + real dist_min = INF_REAL; + real dist_min_static = INF_REAL; + real dist_min_dynamic = INF_REAL; + + const auto capsule = manip_data.capsule_list[capsule_id]; + CollisionEntities collision_object_static{}, collision_object_dynamic{}; + CollisionEntities* collision_object; + + if (static_bvh.num_objects > 0) { + dist_min_static = minimum_distance_static(capsule, static_bvh, collision_object_static, point_in_segment); // static objects + } + if (dynamic_bvh.num_objects > 0) { + dist_min_dynamic = minimum_distance_dynamic(capsule, dynamic_bvh, dist_min_static, collision_object_dynamic, point_in_segment); // dynamic objects + } + + if (dist_min_static <= dist_min_dynamic) { + dist_min = -dist_min_static; // negative distance is positive constraint + collision_object = &collision_object_static; + } else { + dist_min = -dist_min_dynamic; + collision_object = &collision_object_dynamic; + } + + // update worst position for the current capsule if necessary + if (dist_min > max_col_constraints[capsule_id]) { + max_col_constraints[capsule_id] = dist_min; + max_collision_entities[capsule_id] = *collision_object; + } + } + } + } + } + + // at this point we have max constraints for pos, vel, acc, tor, and collisions for this segment + + // fill in the constraints for the current segment + // [p1, p2,..., v1, v2,..., a1, a2,..., t1, t2,...] + auto fill_idx = segment * n_constraints_per_segment; + if (opt.constraints.position) + std::copy_n(max_pos_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; // note (andre): we can use the comma operator because we don't need the output of copy_n() + if (opt.constraints.velocity) + std::copy_n(max_vel_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; + if (opt.constraints.acceleration) + std::copy_n(max_acc_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; + if (opt.constraints.torque) + std::copy_n(max_tor_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; + if (opt.constraints.tool_speed) + constraints[fill_idx++] = max_tool_speed_constraints; + if (opt.constraints.self_collisions) + constraints[fill_idx++] = max_internal_col_constraints; + if (opt.constraints.external_collisions) + std::copy_n(max_col_constraints.data, n_capsules, &constraints[fill_idx]), fill_idx += n_capsules; + + + // The gradient should be a (x_len)x(n_constraints) matrix that looks like this: + // [dp1/dx1, dp2/dx1, ..., dv1/dx1, dv2/dx1, ..., da1/dx1, da2/dx1, ..., dt1/dx1, dt2/dx1] + // [dp1/dx2, dp2/dx2, ..., dv1/dx2, dv2/dx2, ..., da1/dx2, da2/dx2, ..., dt1/dx2, dt2/dx2] + // [dp1/dx3, dp2/dx3, ..., dv1/dx3, dv2/dx3, ..., da1/dx3, da2/dx3, ..., dt1/dx3, dt2/dx3] + // [dp1/dx4, dp2/dx4, ..., dv1/dx4, dv2/dx4, ..., da1/dx4, da2/dx4, ..., dt1/dx4, dt2/dx4] + // [dp1/dx5, dp2/dx5, ..., dv1/dx5, dv2/dx5, ..., da1/dx5, da2/dx5, ..., dt1/dx5, dt2/dx5] + // [dp1/dx6, dp2/dx6, ..., dv1/dx6, dv2/dx6, ..., da1/dx6, da2/dx6, ..., dt1/dx6, dt2/dx6] + // [.....................] + // [.....................] + // [.....................] + // [.....................] + // [dp1/dT=0,dp2/dT=0,..., dv1/dT , dv1/dT , ..., da1/dT , da2/dT , ..., dt1/dT , dt2/dT ] + // *** per segment *** + // where x is the optimization vector + if (grad.size) { +#if BLAST_TRACE_LEVEL >= 2 + PROFILE_SCOPE("Grad"); +#endif + // Matrix (alias) in which we can insert the gradient for the current segment + Matrix grad_segment(&grad(0, segment * n_constraints_per_segment), x_len, n_constraints_per_segment); + Assert(grad_segment.is_alias); + + int con = 0; + int con_idx = 0; + + // positions + if (opt.constraints.position) { + for (int joint = 0; joint < n_joints; joint++) { + + // real pos = opt.bspline.traj.pos(joint, max_pos_indices[joint]); + + // Array of the column where to put the gradient for the current constraint + Array fill_column = grad_segment.col(con); + Assert(con == joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // Which values in 'x' affect the current joint's position + auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!! + // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector + x_idx += first_affected_control_point - 3; + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, max_pos_indices[joint]), n_affected_control_points); + Assert(bp_to_use.is_alias); + + real coeff = 2.0 * sign(opt.bspline.traj.pos(joint, start_point_for_segment + max_pos_indices[joint]) - (pmax[joint] + pmin[joint]) / 2) / (pmax[joint] - pmin[joint]); // note: + + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx++] = bp_to_use[i] * coeff; + } + + // note: dp/dT == 0 + con++; + } + con_idx += n_joints; + } + + // velocities + auto one_over_T = 1 / opt.bspline.traj.t.back(); + if (opt.constraints.velocity) { + for (int joint = 0; joint < n_joints; joint++) { + // Array of the column where to put the gradient for the current constraint + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // Which values in 'x' affect the current joint's position + auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!! + // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector + x_idx += first_affected_control_point - 3; + + Array bv_to_use(&bv(first_affected_control_point, max_vel_indices[joint]), n_affected_control_points); + Assert(bv_to_use.is_alias); + + real coeff = sign(opt.bspline.traj.vel(joint, start_point_for_segment + max_vel_indices[joint])) / vmax[joint] * one_over_T; + + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx++] = bv_to_use[i] * coeff; + } + + // dvj/dT = - (Cv + 1) / T + fill_column.back() = -(max_vel_constraints[joint] + 1) * one_over_T; + + con++; + } + con_idx += n_joints; + } + + // accelerations + auto one_over_T2 = one_over_T * one_over_T; + if (opt.constraints.acceleration) { + for (int joint = 0; joint < n_joints; joint++) { + // Array of the column where to put the gradient for the current constraint + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // Which values in 'x' affect the current joint's position + auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!! + // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector + x_idx += first_affected_control_point - 3; + + Array ba_to_use(&ba(first_affected_control_point, max_acc_indices[joint]), n_affected_control_points); + Assert(ba_to_use.is_alias); + + real coeff = sign(opt.bspline.traj.acc(joint, start_point_for_segment + max_acc_indices[joint])) / amax[joint] * one_over_T2; + + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx++] = ba_to_use[i] * coeff; + } + + // daj/dT = -2 * (Ca + 1) / T + fill_column.back() = -2 * (constraints[con] + 1) * one_over_T; + + con++; + } + con_idx += n_joints; + } + + // torque + // [dt0/dp0, dt1/dp0, ..., dt4/dp0, dt5/dp0] + // [dt0/dp1, dt1/dp1, ..., dt4/dp1, dt5/dp1] + // [dt0/dp2, dt1/dp2, ..., dt4/dp2, dt5/dp2] + // [dt0/dp3, dt1/dp3, ..., dt4/dp3, dt5/dp3] + // [. + // [. + // [. + if (opt.constraints.torque) { + for (int joint = 0; joint < n_joints; joint++) { + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + constexpr real eps = BLAST_FD_STEP; + const auto point = max_tor_indices[joint]; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + const auto v = opt.bspline.traj.vel.col(start_point_for_segment + point); + const auto a = opt.bspline.traj.acc.col(start_point_for_segment + point); + auto old_constraint = max_tor_constraints[joint]; + auto tau_max_now = tau_max[joint]; + + auto p_plus = p; + auto v_plus = v; + auto a_plus = a; + + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Array bv_to_use(&bv(first_affected_control_point, point), n_affected_control_points); + Array ba_to_use(&ba(first_affected_control_point, point), n_affected_control_points); + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + for (int j = 0; j < n_joints; j++) { + + // partial derivative of torque constraints w.r.t. position + // finite difference on position + p_plus[j] += eps; + // compute the derivative of constraint(joint) w.r.t. theta(j). (remember, joint != j) + forward_kinematics(opt.manip, manip_data, p_plus); + dynamics(opt.manip, manip_data, v_plus, a_plus); + const real new_constraint_p = std::abs(manip_data.efforts[joint]) / tau_max_now - 1; // todo: remove 1.01 and use for validation only + const real dtau_dp = (new_constraint_p - old_constraint) / eps; + // reset finite difference + p_plus[j] = p[j]; + + // note: reset forward kinematics because 'v' and 'a' don't change it + forward_kinematics(opt.manip, manip_data, p); // todo: precompute once + + // partial derivative of torque constraints w.r.t. velocity + // finite difference on velocity + v_plus[j] += eps; + dynamics(opt.manip, manip_data, v_plus, a_plus); + const real new_constraint_v = std::abs(manip_data.efforts[joint]) / tau_max_now - 1; // todo: remove 1.01 and use for validation only + + const real dtau_dv = (new_constraint_v - old_constraint) / eps; + // reset finite difference + v_plus[j] = v[j]; + + // partial derivative of torque constraints w.r.t. acceleration + // finite difference on acceleration + a_plus[j] += eps; + dynamics(opt.manip, manip_data, v_plus, a_plus); + const real new_constraint_a = std::abs(manip_data.efforts[joint]) / tau_max_now - 1; // todo: remove 1.01 and use for validation only + const real dtau_da = (new_constraint_a - old_constraint) / eps; + // reset finite difference + a_plus[j] = a[j]; + + // insert into the gradient + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dtau_dp + + bv_to_use[i] * dtau_dv * one_over_T + + ba_to_use[i] * dtau_da * one_over_T2; + } + x_idx += x_idx_skip; + + // gradient w.r.t. T + fill_column.back() += dtau_dv * (-v[j] * one_over_T) + dtau_da * (-2 * a[j] * one_over_T); + } + con++; // note: moved out of the inner j loop (only changes at the end of all j torques per joint) + } + con_idx += n_joints; + } + + // tool speed + if (opt.constraints.tool_speed) { + constexpr real eps = BLAST_FD_STEP; + const auto point = max_tool_index; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + const auto v = opt.bspline.traj.vel.col(start_point_for_segment + point); + auto p_plus = p; + auto v_plus = v; + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + Array fill_column = grad_segment.col(con); + Assert(con == con_idx); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Array bv_to_use(&bv(first_affected_control_point, point), n_affected_control_points); + Assert(bp_to_use.is_alias); + Assert(bv_to_use.is_alias); + for (int j = 0; j < n_joints; j++) { + // todo: check if the joint can actually move the current capsule + p_plus[j] += eps; + + // recompute tool_speed + forward_kinematics(opt.manip, manip_data, p_plus); + const auto J_tool_p = get_J_tool(&opt, manip_data); + const auto tool_speed_p = norm(J_tool_p * v_plus); + const auto new_constraint_p = bound_constraint(tool_speed_p, 0.0, tool_speed_max); + // partial difference d(tool_speed)/dp + const real dtool_speed_dp = (new_constraint_p - max_tool_speed_constraints) / eps; + p_plus[j] = p[j]; // reset finite difference + + forward_kinematics(opt.manip, manip_data, p_plus); + v_plus[j] += eps; + const auto J_tool_v = get_J_tool(&opt, manip_data); + const auto tool_speed_v = norm(J_tool_v * v_plus); + const auto new_constraint_v = bound_constraint(tool_speed_v, 0.0, tool_speed_max); + const real dtool_speed_dv = (new_constraint_v - max_tool_speed_constraints) / eps; + v_plus[j] = v[j]; + + // fill the gradient for the control points that affect current joint 'j' d(collision)/dp * dp/d(control point) + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dtool_speed_dp + + bv_to_use[i] * dtool_speed_dv * one_over_T; + } + x_idx += x_idx_skip; + // gradient w.r.t. T + fill_column.back() += dtool_speed_dv * (-v[j] * one_over_T); // todo: check this !! + } + con++; + con_idx++; + } + + // internal collisions + if (opt.constraints.self_collisions) { + constexpr real eps = BLAST_FD_STEP; + const auto point = max_internal_collision_index; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + auto p_plus = p; + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + Array fill_column = grad_segment.col(con); + Assert(con == con_idx); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Assert(bp_to_use.is_alias); + for (int j = 0; j < n_joints; j++) { + // todo: check if the joint can actually move the current capsule + p_plus[j] += eps; + + // recompute internal collisions at the worst point in segment + forward_kinematics(opt.manip, manip_data, p_plus); + compute_collision_model(opt.manip, manip_data); + const auto new_internal_collision_constraint = max(-get_internal_collisions(opt.manip, manip_data)); + // partial difference d(internal_collision)/dp + const real dint_coll_dp = (new_internal_collision_constraint - max_internal_col_constraints) / eps; + + // fill the gradient for the control points that affect current joint 'j' d(collision)/dp * dp/d(control point) + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dint_coll_dp; + } + x_idx += x_idx_skip; + + p_plus[j] = p[j]; // reset finite difference + } + con++; + con_idx++; + } + + // collisions + if (opt.constraints.external_collisions) { + for (int capsule_id = 0; capsule_id < opt.manip._n_caps; capsule_id++) { + constexpr real eps = BLAST_FD_STEP; + const auto point = max_collision_entities[capsule_id].point_in_segment; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + auto p_plus = p; + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + capsule_id); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Assert(bp_to_use.is_alias); + + // finite difference w.r.t. joint positions then multiply by relevant basis functions. + for (int j = 0; j < n_joints; j++) { + // todo: check if the joint can actually move the current capsule + p_plus[j] += eps; + + // recompute collision constraint, but only with the current capsule and the identified object. + forward_kinematics(opt.manip, manip_data, p_plus); + compute_collision_model(opt.manip, manip_data); + const auto capsule = manip_data.capsule_list[capsule_id]; + + real distance_plus; + const auto& objects = max_collision_entities[capsule_id]; + switch (objects.other_object_type) { + case CollisionObjectType::box: { + distance_plus = distance(capsule, objects.box); + break; + } + case CollisionObjectType::capsule: { + distance_plus = distance(capsule, objects.capsule); + break; + } + case CollisionObjectType::sphere: { + distance_plus = distance(capsule, objects.sphere); + break; + } + } + + distance_plus = -distance_plus; // negative distance is positive constraint + + // partial difference d(collision)/dp + const real dcoll_dp = (distance_plus - max_col_constraints[capsule_id]) / eps; + + // fill the gradient for the control points that affect current joint 'j' d(collision)/dp * dp/d(control point) + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dcoll_dp; + } + x_idx += x_idx_skip; + + p_plus[j] = p[j]; // reset finite difference + } + + con++; // finished filling the column with the gradient of the collision of the current capsule w.r.t. each joint position + } + } + } + } +} + +inline blast_fn void constraints_and_gradients_with_double_broadphase(const Array& x, Optimization& opt, Array& constraints, Matrix& grad) { + // ZoneScoped; + + // constraints (p,v,a,tor) for each joint, for each segment + // [p1, p2,..., v1, v2,..., a1, a2,..., t1, t2,...] + + // basis: n_ctrl x n_points + // Assert(constraints.is_alias); todo: fix for validation + if (grad.size) { + Assert(grad.is_alias); + Assert(grad.rows == x.size); + Assert(grad.cols == constraints.size); + } + const int n_segments = (int) opt.bspline.n_ctrl - (int) opt.bspline.degree; + const int n_points_per_segment = (int) opt.bspline.n_points / n_segments; // todo: check if fine? + const int n_joints = (int) opt.manip.n_joints; + const int n_ctrl = (int) opt.bspline.n_ctrl; + const int x_len = (int) x.size; + const int n_capsules = opt.manip._n_caps; + const int n_constraints_per_segment = opt.constraints.n_constraints_per_segment; + Assert(constraints.size == n_segments * n_constraints_per_segment); + + const auto& world = opt.world; + + // limits + auto pmax = opt.manip.position_max; + auto pmin = opt.manip.position_min; + auto vmax = opt.manip.velocity_max; + auto amax = opt.manip.acceleration_max; + auto tau_max = opt.manip.torque_max; + auto tool_speed_max = opt.manip.tool_speed_max; + + for (int j = 0; j < n_joints; j++) { + // todo: document the current behaviour in the API + // (doesn't currently work if one is inf and the other is not) + if (pmax[j] == INF_REAL) // note: replace INF_REAL with huge value + pmax[j] = 1e300; + if (pmin[j] == -INF_REAL) // note: replace -INF_REAL with huge negative value + pmin[j] = -1e300; + } + + ManipulatorTempData manip_data; + std::array max_pos_indices{}; + std::array max_vel_indices{}; + std::array max_acc_indices{}; + std::array max_tor_indices{}; + std::array max_collision_entities{}; + u8 max_internal_collision_index = 0; + u8 max_tool_index = 0; + + std::vector> capsules{}; + capsules.resize(n_points_per_segment); + + opt.bspline.compute_trajectory(x, opt.task); + + + for (int segment = 0; segment < n_segments; segment++) { +#if BLAST_TRACE_LEVEL >= 2 + PROFILE_SCOPE("All Segment Constraints"); +#endif + const int first_affected_control_point = std::max(3, segment); + const int last_affected_control_point = std::min((n_ctrl - 1) - 3, segment + (int) opt.bspline.degree); + const int n_affected_control_points = last_affected_control_point - first_affected_control_point + 1; // note: affected_control_points are inclusive, so when we have last = 5, first = 3, we want 3 (5 - 3) + 1 + const int start_point_for_segment = segment * n_points_per_segment; + Assert(n_affected_control_points >= 3); + Assert(n_affected_control_points <= 6); + + Matrix bp(&opt.bspline.basis_p(0, start_point_for_segment), n_ctrl, n_points_per_segment); + Matrix bv(&opt.bspline.basis_v(0, start_point_for_segment), n_ctrl, n_points_per_segment); + Matrix ba(&opt.bspline.basis_a(0, start_point_for_segment), n_ctrl, n_points_per_segment); + + Array max_pos_constraints(n_joints, -INF_REAL); + Array max_vel_constraints(n_joints, -INF_REAL); + Array max_acc_constraints(n_joints, -INF_REAL); + Array max_tor_constraints(n_joints, -INF_REAL); + real max_tool_speed_constraints = -INF_REAL; + real max_internal_col_constraints = -INF_REAL; // todo: worst or worst per capsule ? + Array max_col_constraints(n_capsules, -INF_REAL); + + for (int point_in_segment = 0; point_in_segment < n_points_per_segment; point_in_segment++) { + { +#if BLAST_TRACE_LEVEL >= 2 + PROFILE_SCOPE("All Point Constraints"); +#endif + auto p = opt.bspline.traj.pos.col(start_point_for_segment + point_in_segment); + auto v = opt.bspline.traj.vel.col(start_point_for_segment + point_in_segment); + auto a = opt.bspline.traj.acc.col(start_point_for_segment + point_in_segment); + + forward_kinematics(opt.manip, manip_data, p); + compute_collision_model(opt.manip, manip_data); + dynamics(opt.manip, manip_data, v, a); + + for (int j = 0; j < n_joints; j++) { + // position + if (opt.constraints.position) { + +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Pos Constraints"); +#endif + + if (const auto c = bound_constraint(p[j], pmin[j], pmax[j]); + c > max_pos_constraints[j]) { + max_pos_constraints[j] = c; + max_pos_indices[j] = point_in_segment; + } + } + // velocity + if (opt.constraints.velocity) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Vel Constraints"); +#endif + if (const auto c = std::abs(v[j]) / vmax[j] - 1.0; + c > max_vel_constraints[j]) { + max_vel_constraints[j] = c; + max_vel_indices[j] = point_in_segment; + } + } + // acceleration + if (opt.constraints.acceleration) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Acc Constraints"); +#endif + if (const auto c = std::abs(a[j]) / amax[j] - 1.0; + c > max_acc_constraints[j]) { + max_acc_constraints[j] = c; + max_acc_indices[j] = point_in_segment; + } + } + // torque + if (opt.constraints.torque) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Tau Constraints"); +#endif + if (const auto c = std::abs(manip_data.efforts[j]) / tau_max[j] - 1.0; + c > max_tor_constraints[j]) { + max_tor_constraints[j] = c; + max_tor_indices[j] = point_in_segment; + } + } + } + + // Tool speed + if (opt.constraints.tool_speed) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Tool Constraints"); +#endif + const auto J_tool = get_J_tool(&opt, manip_data); // todo: clean up get_J_tool to a get_tool_speed + const auto tool_speed = norm(get_J_tool(&opt, manip_data) * v); + if (const auto c = bound_constraint(tool_speed, 0.0, tool_speed_max); + c > max_tool_speed_constraints) { + max_tool_speed_constraints = c; + max_tool_index = point_in_segment; + } + } + + // self collision + if (opt.constraints.self_collisions) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Self Constraints"); +#endif + // check every internal collision + if (const auto c = max(-get_internal_collisions(opt.manip, manip_data)); + c > max_internal_col_constraints) { + max_internal_col_constraints = c; + max_internal_collision_index = point_in_segment; + } + } + + + // external collision + if (opt.constraints.external_collisions) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Ext Constraints"); +#endif + capsules[point_in_segment] = manip_data.capsule_list; + } + } + } + + if (opt.constraints.external_collisions) { +#if BLAST_TRACE_LEVEL >= 3 + PROFILE_SCOPE("Ext Constraints"); +#endif + + auto& static_bvh = opt.world.static_bounding_volume_hierarchy; + auto& dynamic_bvh = opt.world.dynamic_bounding_volume_hierarchy; + + create_time_bounding_volume_hierarchies(opt.time_bounding_volume_hierarchies, capsules, n_capsules, n_points_per_segment); + create_time_bvh_dynamic_objects(opt.world, dynamic_bvh, segment * n_points_per_segment, n_points_per_segment, x.back(), n_segments, opt.trajectory_start_time); + for (int capsule_id = 0; capsule_id < n_capsules; capsule_id++) { + CollisionEntities collision_object_static{}, collision_object_dynamic{}; + CollisionEntities* collision_object; + + real dist_min_static = INF_REAL; + real dist_min_dynamic = INF_REAL; + + // static objects + if (static_bvh.num_objects > 0) + dist_min_static = minimum_distance_static_objects_time(static_bvh, opt.time_bounding_volume_hierarchies[capsule_id], collision_object_static); + + // dynamic objects + if (dynamic_bvh.num_objects > 0) + dist_min_dynamic = minimum_distance_dynamic_objects_time(dynamic_bvh, opt.time_bounding_volume_hierarchies[capsule_id], dist_min_static, collision_object_dynamic, segment * n_points_per_segment, n_points_per_segment, x.back(), n_segments, opt.trajectory_start_time); + + // add constraint + if (dist_min_static <= dist_min_dynamic) { + max_col_constraints[capsule_id] = -dist_min_static; + max_collision_entities[capsule_id] = collision_object_static; + } else { + max_col_constraints[capsule_id] = -dist_min_dynamic; + max_collision_entities[capsule_id] = collision_object_dynamic; + } + } + } + + + // at this point we have max constraints for pos, vel, acc, tor, and collisions for this segment + + // fill in the constraints for the current segment + // [p1, p2,..., v1, v2,..., a1, a2,..., t1, t2,...] + auto fill_idx = segment * n_constraints_per_segment; + if (opt.constraints.position) + std::copy_n(max_pos_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; // note (andre): we can use the comma operator because we don't need the output of copy_n() + if (opt.constraints.velocity) + std::copy_n(max_vel_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; + if (opt.constraints.acceleration) + std::copy_n(max_acc_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; + if (opt.constraints.torque) + std::copy_n(max_tor_constraints.data, n_joints, &constraints[fill_idx]), fill_idx += n_joints; + if (opt.constraints.tool_speed) + constraints[fill_idx++] = max_tool_speed_constraints; + if (opt.constraints.self_collisions) + constraints[fill_idx++] = max_internal_col_constraints; + if (opt.constraints.external_collisions) + std::copy_n(max_col_constraints.data, n_capsules, &constraints[fill_idx]), fill_idx += n_capsules; + + + // The gradient should be a (x_len)x(n_constraints) matrix that looks like this: + // [dp1/dx1, dp2/dx1, ..., dv1/dx1, dv2/dx1, ..., da1/dx1, da2/dx1, ..., dt1/dx1, dt2/dx1] + // [dp1/dx2, dp2/dx2, ..., dv1/dx2, dv2/dx2, ..., da1/dx2, da2/dx2, ..., dt1/dx2, dt2/dx2] + // [dp1/dx3, dp2/dx3, ..., dv1/dx3, dv2/dx3, ..., da1/dx3, da2/dx3, ..., dt1/dx3, dt2/dx3] + // [dp1/dx4, dp2/dx4, ..., dv1/dx4, dv2/dx4, ..., da1/dx4, da2/dx4, ..., dt1/dx4, dt2/dx4] + // [dp1/dx5, dp2/dx5, ..., dv1/dx5, dv2/dx5, ..., da1/dx5, da2/dx5, ..., dt1/dx5, dt2/dx5] + // [dp1/dx6, dp2/dx6, ..., dv1/dx6, dv2/dx6, ..., da1/dx6, da2/dx6, ..., dt1/dx6, dt2/dx6] + // [.....................] + // [.....................] + // [.....................] + // [.....................] + // [dp1/dT=0,dp2/dT=0,..., dv1/dT , dv1/dT , ..., da1/dT , da2/dT , ..., dt1/dT , dt2/dT ] + // *** per segment *** + // where x is the optimization vector + if (grad.size) { +#if BLAST_TRACE_LEVEL >= 2 + PROFILE_SCOPE("Grad"); +#endif + // Matrix (alias) in which we can insert the gradient for the current segment + Matrix grad_segment(&grad(0, segment * n_constraints_per_segment), x_len, n_constraints_per_segment); + Assert(grad_segment.is_alias); + + int con = 0; + int con_idx = 0; + + // positions + if (opt.constraints.position) { + for (int joint = 0; joint < n_joints; joint++) { + + // real pos = opt.bspline.traj.pos(joint, max_pos_indices[joint]); + + // Array of the column where to put the gradient for the current constraint + Array fill_column = grad_segment.col(con); + Assert(con == joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // Which values in 'x' affect the current joint's position + auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!! + // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector + x_idx += first_affected_control_point - 3; + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, max_pos_indices[joint]), n_affected_control_points); + Assert(bp_to_use.is_alias); + + real coeff = 2.0 * sign(opt.bspline.traj.pos(joint, start_point_for_segment + max_pos_indices[joint]) - (pmax[joint] + pmin[joint]) / 2) / (pmax[joint] - pmin[joint]); // note: + + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx++] = bp_to_use[i] * coeff; + } + + // note: dp/dT == 0 + con++; + } + con_idx += n_joints; + } + + // velocities + auto one_over_T = 1 / opt.bspline.traj.t.back(); + if (opt.constraints.velocity) { + for (int joint = 0; joint < n_joints; joint++) { + // Array of the column where to put the gradient for the current constraint + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // Which values in 'x' affect the current joint's position + auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!! + // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector + x_idx += first_affected_control_point - 3; + + Array bv_to_use(&bv(first_affected_control_point, max_vel_indices[joint]), n_affected_control_points); + Assert(bv_to_use.is_alias); + + real coeff = sign(opt.bspline.traj.vel(joint, start_point_for_segment + max_vel_indices[joint])) / vmax[joint] * one_over_T; + + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx++] = bv_to_use[i] * coeff; + } + + // dvj/dT = - (Cv + 1) / T + fill_column.back() = -(max_vel_constraints[joint] + 1) * one_over_T; + + con++; + } + con_idx += n_joints; + } + + // accelerations + auto one_over_T2 = one_over_T * one_over_T; + if (opt.constraints.acceleration) { + for (int joint = 0; joint < n_joints; joint++) { + // Array of the column where to put the gradient for the current constraint + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // Which values in 'x' affect the current joint's position + auto x_idx = joint * (n_ctrl - 6); // todo: does not work with tasks that don't impose p,v,a for every joint!! + // shift to the first affected control point keeping in mind that the first 3 are not in the optimization vector + x_idx += first_affected_control_point - 3; + + Array ba_to_use(&ba(first_affected_control_point, max_acc_indices[joint]), n_affected_control_points); + Assert(ba_to_use.is_alias); + + real coeff = sign(opt.bspline.traj.acc(joint, start_point_for_segment + max_acc_indices[joint])) / amax[joint] * one_over_T2; + + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx++] = ba_to_use[i] * coeff; + } + + // daj/dT = -2 * (Ca + 1) / T + fill_column.back() = -2 * (constraints[con] + 1) * one_over_T; + + con++; + } + con_idx += n_joints; + } + + // torque + // [dt0/dp0, dt1/dp0, ..., dt4/dp0, dt5/dp0] + // [dt0/dp1, dt1/dp1, ..., dt4/dp1, dt5/dp1] + // [dt0/dp2, dt1/dp2, ..., dt4/dp2, dt5/dp2] + // [dt0/dp3, dt1/dp3, ..., dt4/dp3, dt5/dp3] + // [. + // [. + // [. + if (opt.constraints.torque) { + for (int joint = 0; joint < n_joints; joint++) { + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + joint); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + constexpr real eps = BLAST_FD_STEP; + const auto point = max_tor_indices[joint]; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + const auto v = opt.bspline.traj.vel.col(start_point_for_segment + point); + const auto a = opt.bspline.traj.acc.col(start_point_for_segment + point); + auto old_constraint = max_tor_constraints[joint]; + auto tau_max_now = tau_max[joint]; + + auto p_plus = p; + auto v_plus = v; + auto a_plus = a; + + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Array bv_to_use(&bv(first_affected_control_point, point), n_affected_control_points); + Array ba_to_use(&ba(first_affected_control_point, point), n_affected_control_points); + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + for (int j = 0; j < n_joints; j++) { + + // partial derivative of torque constraints w.r.t. position + // finite difference on position + p_plus[j] += eps; + // compute the derivative of constraint(joint) w.r.t. theta(j). (remember, joint != j) + forward_kinematics(opt.manip, manip_data, p_plus); + dynamics(opt.manip, manip_data, v_plus, a_plus); + const real new_constraint_p = std::abs(manip_data.efforts[joint]) / tau_max_now - 1; // todo: remove 1.01 and use for validation only + const real dtau_dp = (new_constraint_p - old_constraint) / eps; + // reset finite difference + p_plus[j] = p[j]; + + // note: reset forward kinematics because 'v' and 'a' don't change it + forward_kinematics(opt.manip, manip_data, p); // todo: precompute once + + // partial derivative of torque constraints w.r.t. velocity + // finite difference on velocity + v_plus[j] += eps; + dynamics(opt.manip, manip_data, v_plus, a_plus); + const real new_constraint_v = std::abs(manip_data.efforts[joint]) / tau_max_now - 1; // todo: remove 1.01 and use for validation only + + const real dtau_dv = (new_constraint_v - old_constraint) / eps; + // reset finite difference + v_plus[j] = v[j]; + + // partial derivative of torque constraints w.r.t. acceleration + // finite difference on acceleration + a_plus[j] += eps; + dynamics(opt.manip, manip_data, v_plus, a_plus); + const real new_constraint_a = std::abs(manip_data.efforts[joint]) / tau_max_now - 1; // todo: remove 1.01 and use for validation only + const real dtau_da = (new_constraint_a - old_constraint) / eps; + // reset finite difference + a_plus[j] = a[j]; + + // insert into the gradient + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dtau_dp + + bv_to_use[i] * dtau_dv * one_over_T + + ba_to_use[i] * dtau_da * one_over_T2; + } + x_idx += x_idx_skip; + + // gradient w.r.t. T + fill_column.back() += dtau_dv * (-v[j] * one_over_T) + dtau_da * (-2 * a[j] * one_over_T); + } + con++; // note: moved out of the inner j loop (only changes at the end of all j torques per joint) + } + con_idx += n_joints; + } + + // tool speed + if (opt.constraints.tool_speed) { + constexpr real eps = BLAST_FD_STEP; + const auto point = max_tool_index; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + const auto v = opt.bspline.traj.vel.col(start_point_for_segment + point); + auto p_plus = p; + auto v_plus = v; + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + Array fill_column = grad_segment.col(con); + Assert(con == con_idx); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Array bv_to_use(&bv(first_affected_control_point, point), n_affected_control_points); + Assert(bp_to_use.is_alias); + Assert(bv_to_use.is_alias); + for (int j = 0; j < n_joints; j++) { + // todo: check if the joint can actually move the current capsule + p_plus[j] += eps; + + // recompute tool_speed + forward_kinematics(opt.manip, manip_data, p_plus); + const auto J_tool_p = get_J_tool(&opt, manip_data); + const auto tool_speed_p = norm(J_tool_p * v_plus); + const auto new_constraint_p = bound_constraint(tool_speed_p, 0.0, tool_speed_max); + // partial difference d(tool_speed)/dp + const real dtool_speed_dp = (new_constraint_p - max_tool_speed_constraints) / eps; + p_plus[j] = p[j]; // reset finite difference + + forward_kinematics(opt.manip, manip_data, p_plus); + v_plus[j] += eps; + const auto J_tool_v = get_J_tool(&opt, manip_data); + const auto tool_speed_v = norm(J_tool_v * v_plus); + const auto new_constraint_v = bound_constraint(tool_speed_v, 0.0, tool_speed_max); + const real dtool_speed_dv = (new_constraint_v - max_tool_speed_constraints) / eps; + v_plus[j] = v[j]; + + // fill the gradient for the control points that affect current joint 'j' d(collision)/dp * dp/d(control point) + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dtool_speed_dp + + bv_to_use[i] * dtool_speed_dv * one_over_T; + } + x_idx += x_idx_skip; + // gradient w.r.t. T + fill_column.back() += dtool_speed_dv * (-v[j] * one_over_T); // todo: check this !! + } + con++; + con_idx++; + } + + // internal collisions + if (opt.constraints.self_collisions) { + constexpr real eps = BLAST_FD_STEP; + const auto point = max_internal_collision_index; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + auto p_plus = p; + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + Array fill_column = grad_segment.col(con); + Assert(con == con_idx); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Assert(bp_to_use.is_alias); + for (int j = 0; j < n_joints; j++) { + // todo: check if the joint can actually move the current capsule + p_plus[j] += eps; + + // recompute internal collisions at the worst point in segment + forward_kinematics(opt.manip, manip_data, p_plus); + compute_collision_model(opt.manip, manip_data); + const auto new_internal_collision_constraint = max(-get_internal_collisions(opt.manip, manip_data)); + // partial difference d(internal_collision)/dp + const real dint_coll_dp = (new_internal_collision_constraint - max_internal_col_constraints) / eps; + + // fill the gradient for the control points that affect current joint 'j' d(collision)/dp * dp/d(control point) + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dint_coll_dp; + } + x_idx += x_idx_skip; + + p_plus[j] = p[j]; // reset finite difference + } + con++; + con_idx++; + } + + // collisions + if (opt.constraints.external_collisions) { + for (int capsule_id = 0; capsule_id < opt.manip._n_caps; capsule_id++) { + constexpr real eps = BLAST_FD_STEP; + const auto point = max_collision_entities[capsule_id].point_in_segment; + const auto p = opt.bspline.traj.pos.col(start_point_for_segment + point); + auto p_plus = p; + + auto x_idx = first_affected_control_point - 3; + auto x_idx_skip = n_ctrl - 6; + + Array fill_column = grad_segment.col(con); + Assert(con == con_idx + capsule_id); + Assert(fill_column.size == x_len); + Assert(fill_column.is_alias); + + // 3 to 6 basis functions depending on the segment (first and last 3 control points are not in x) + Array bp_to_use(&bp(first_affected_control_point, point), n_affected_control_points); + Assert(bp_to_use.is_alias); + + // finite difference w.r.t. joint positions then multiply by relevant basis functions. + for (int j = 0; j < n_joints; j++) { + // todo: check if the joint can actually move the current capsule + p_plus[j] += eps; + + // recompute collision constraint, but only with the current capsule and the identified object. + forward_kinematics(opt.manip, manip_data, p_plus); + compute_collision_model(opt.manip, manip_data); + const auto capsule = manip_data.capsule_list[capsule_id]; + + real distance_plus; + const auto& objects = max_collision_entities[capsule_id]; + switch (objects.other_object_type) { + case CollisionObjectType::box: { + distance_plus = distance(capsule, objects.box); + break; + } + case CollisionObjectType::capsule: { + distance_plus = distance(capsule, objects.capsule); + break; + } + case CollisionObjectType::sphere: { + distance_plus = distance(capsule, objects.sphere); + break; + } + } + + distance_plus = -distance_plus; // negative distance is positive constraint + + // partial difference d(collision)/dp + const real dcoll_dp = (distance_plus - max_col_constraints[capsule_id]) / eps; + + // fill the gradient for the control points that affect current joint 'j' d(collision)/dp * dp/d(control point) + for (int i = 0; i < n_affected_control_points; i++) { + fill_column[x_idx + i] = bp_to_use[i] * dcoll_dp; + } + x_idx += x_idx_skip; + + p_plus[j] = p[j]; // reset finite difference + } + + con++; // finished filling the column with the gradient of the collision of the current capsule w.r.t. each joint position + } + } + } + } +} + +inline blast_fn void nlopt_constraints_with_segments(unsigned m, real* result, unsigned x_len, const real* x, real* grad, void* f_data) { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_FUNCTION; +#endif + auto* opt = (Optimization*) f_data; + + Array xv; + xv.alias(x, x_len); + + Array constraints; + constraints.alias(result, m); + + Matrix gradients; + if (grad) { + memset(grad, 0, m * x_len * sizeof(real)); + gradients.alias(grad, x_len, m); + } + + constraints_and_gradients_with_segments(xv, *opt, constraints, gradients); + + if (opt->constraints.collect_x_each_iteration) { + opt->constraints.x_list.push_back(xv); + } +} + +inline blast_fn void nlopt_constraints_with_broadphase(unsigned m, real* result, unsigned x_len, const real* x, real* grad, void* f_data) { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_FUNCTION; +#endif + auto* opt = (Optimization*) f_data; + + Array xv; + xv.alias(x, x_len); + + Array constraints; + constraints.alias(result, m); + + Matrix gradients; + if (grad) { + memset(grad, 0, m * x_len * sizeof(real)); + gradients.alias(grad, x_len, m); + } + + constraints_and_gradients_with_broadphase(xv, *opt, constraints, gradients); + + if (opt->constraints.collect_x_each_iteration) { + opt->constraints.x_list.push_back(xv); + } +} + +inline blast_fn void nlopt_constraints_with_double_broadphase(unsigned m, real* result, unsigned x_len, const real* x, real* grad, void* f_data) { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_FUNCTION; +#endif + auto* opt = (Optimization*) f_data; + + Array xv; + xv.alias(x, x_len); + + Array constraints; + constraints.alias(result, m); + + Matrix gradients; + if (grad) { + memset(grad, 0, m * x_len * sizeof(real)); + gradients.alias(grad, x_len, m); + } + + constraints_and_gradients_with_double_broadphase(xv, *opt, constraints, gradients); if (opt->constraints.collect_x_each_iteration) { opt->constraints.x_list.push_back(xv); diff --git a/blast/optimization/optimization.hpp b/blast/optimization/optimization.hpp index df1ff84..2fdcde8 100644 --- a/blast/optimization/optimization.hpp +++ b/blast/optimization/optimization.hpp @@ -621,6 +621,386 @@ inline Result optimize_with_segments_impl(Optimization* opt, u32 output_steps_ms return result; } +inline Result optimize_with_broadphase_impl(Optimization* opt, u32 output_steps_ms = 1 /*ms*/) { + auto T1 = get_tick_us(); + + // Initialization + // configure_internal_data(opt); // todo: Ensure we can remove + initialize_optimization_with_segments(opt); + n_con_with_segments(opt); + + Result result(opt); // todo: this is expensive + result.opt->task = opt->task; + + // Initial validation + if (!validate_task(opt)) { // todo: support validate_task when there are no capsules... + print(opt->task); + return result; + } + + const auto n = opt->bspline.x_len(opt->task); + + Array con_tol(opt->constraints.n_constraints, opt->success_tolerance); + Array x_tol(n, 0.001); + +#ifdef BLAST_USE_NATIVE_SQP + nlopt_stopping stop; + stop.n = n; + stop.minf_max = -HUGE_VAL; + stop.ftol_rel = 0; + stop.ftol_abs = 0.001; + stop.xtol_rel = 0; + stop.xtol_abs = x_tol.data; + stop.x_weights = nullptr; + stop.nevals_p = 0; + stop.maxeval = opt->max_eval; + stop.maxtime = opt->max_time; + stop.start = nlopt_seconds(); + stop.force_stop = false; + stop.stop_msg = nullptr; + + Array ub(n, INF_REAL); + Array lb(n, -INF_REAL); + ub.back() = 30.0; + lb.back() = 0.01; + + nlopt_constraint fc{}; + fc.m = opt->constraints.n_constraints; + fc.f = nullptr; + fc.mf = nlopt_constraints_with_broadphase; + fc.pre = nullptr; + fc.f_data = opt; + fc.tol = con_tol.data; +#else + nlopt_opt o = nlopt_create(NLOPT_LD_SLSQP, n); + nlopt_result nlopt_res; + nlopt_res = nlopt_add_inequality_mconstraint(o, opt->constraints.n_constraints, nlopt_constraints_with_broadphase, opt, con_tol.data); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_min_objective(o, objective_function, opt); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_lower_bound(o, (int) n - 1, 0.01); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_upper_bound(o, (int) n - 1, 60.0); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_ftol_abs(o, 0.0001); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_xtol_abs(o, x_tol.data); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_maxtime(o, opt->max_time); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_maxeval(o, opt->max_eval); + Assert(nlopt_res == NLOPT_SUCCESS); +#endif + + + auto start_guess = opt->guess; // save for restoration after restarts if necessary + int try_count = 0; + bool is_valid_more = false; + bool is_valid = false; + for (; try_count < opt->max_tries; try_count++) { // todo: add nlopt stop criteria to list, add max_time for full loop +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Optimization"); +#endif + + // initial guess + Array x; + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Initial guess"); +#endif + x = init_guess_segments(opt); + result.x0 = x; + } + + // launch optimization + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("NLopt optimization"); +#endif + + real f = HUGE_VAL; + // note: can we initialize grad to 0 here +#ifdef BLAST_USE_NATIVE_SQP + stop.nevals_p = 0; + result.nlopt_exit_criteria = sqp( + opt->bspline.x_len(opt->task), + objective_function, + opt, + 1, + &fc, + 0, + nullptr, + lb.data, + ub.data, + x.data, + &f, + &stop); + result.num_eval = stop.nevals_p; + +#else + result.nlopt_exit_criteria = nlopt_optimize(o, x.data, &f); + result.num_eval = nlopt_get_numevals(o); +#endif + } + + // validate solution + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Solution validation"); +#endif + Array constraints_points(opt->constraints.n_constraints); + Matrix gradient; + constraints_and_gradients_with_broadphase(x, *opt, constraints_points, gradient); + auto max_con = max(constraints_points); + result.max_constraint_idx = argmax(constraints_points); + result.max_constraint_value = max_con; + is_valid = max_con < opt->success_tolerance * 2; + } + + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Solution validation (more points)"); +#endif + u64 steps_ms = (u64) (std::ceil(x.back() * 1e3 / output_steps_ms)); + x.back() = (real) (std::ceil(x.back() * 1000.0 / output_steps_ms) * output_steps_ms) * 1e-3; + int points_more = (int) (steps_ms + 1); + + Bspline bspline_val_more(opt->bspline.n_ctrl, points_more, opt->bspline.degree, opt->manip.n_joints); // todo: this is expensive + bspline_val_more.compute_trajectory(x, opt->task); + auto opt_val_more(*opt); + opt_val_more.set_bspline(bspline_val_more); + n_con_with_segments(&opt_val_more); + Array constraints_more_points(opt_val_more.constraints.n_constraints); + Matrix gradient; + constraints_and_gradients_with_broadphase(x, opt_val_more, constraints_more_points, gradient); + // is_valid_more = max(constraints_more_points) < opt->success_tolerance; + auto max_con_more = max(constraints_more_points); + result.max_constraint_more_points_idx = argmax(constraints_more_points); + result.max_constraint_more_points_value = max_con_more; + is_valid_more = max_con_more < opt->success_tolerance * 2; + + result.x = x; + + if (is_valid && is_valid_more) { + result.trajectory = bspline_val_more.traj; + // break; + } else if (opt->guess.type != Guess::random && try_count == 0) { + opt->guess.type = Guess::random; + } + } +#if BLAST_TRACE_LEVEL >= 1 + FrameMark; +#endif + } + + opt->guess = start_guess; // reset to original + + auto time = (real) (get_tick_us() - T1) / 1000.0; + + // Output results + result.success = is_valid && is_valid_more; + result.success_false = is_valid && !is_valid_more; + result.compute_time = time; + result.opt = opt; + result.num_tries = try_count; + +#ifndef BLAST_USE_NATIVE_SQP + nlopt_destroy(o); +#endif + + return result; +} + +inline Result optimize_with_double_broadphase_impl(Optimization* opt, u32 output_steps_ms = 1 /*ms*/) { + auto T1 = get_tick_us(); + + // Initialization + // configure_internal_data(opt); // todo: Ensure we can remove + initialize_optimization_with_segments(opt); + n_con_with_segments(opt); + + Result result(opt); // todo: this is expensive + result.opt->task = opt->task; + + // Initial validation + if (!validate_task(opt)) { // todo: support validate_task when there are no capsules... + print(opt->task); + return result; + } + + const auto n = opt->bspline.x_len(opt->task); + + Array con_tol(opt->constraints.n_constraints, opt->success_tolerance); + Array x_tol(n, 0.001); + +#ifdef BLAST_USE_NATIVE_SQP + nlopt_stopping stop; + stop.n = n; + stop.minf_max = -HUGE_VAL; + stop.ftol_rel = 0; + stop.ftol_abs = 0.001; + stop.xtol_rel = 0; + stop.xtol_abs = x_tol.data; + stop.x_weights = nullptr; + stop.nevals_p = 0; + stop.maxeval = opt->max_eval; + stop.maxtime = opt->max_time; + stop.start = nlopt_seconds(); + stop.force_stop = false; + stop.stop_msg = nullptr; + + Array ub(n, INF_REAL); + Array lb(n, -INF_REAL); + ub.back() = 30.0; + lb.back() = 0.01; + + nlopt_constraint fc{}; + fc.m = opt->constraints.n_constraints; + fc.f = nullptr; + fc.mf = nlopt_constraints_with_double_broadphase; + fc.pre = nullptr; + fc.f_data = opt; + fc.tol = con_tol.data; +#else + nlopt_opt o = nlopt_create(NLOPT_LD_SLSQP, n); + nlopt_result nlopt_res; + nlopt_res = nlopt_add_inequality_mconstraint(o, opt->constraints.n_constraints, nlopt_constraints_with_double_broadphase, opt, con_tol.data); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_min_objective(o, objective_function, opt); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_lower_bound(o, (int) n - 1, 0.01); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_upper_bound(o, (int) n - 1, 60.0); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_ftol_abs(o, 0.0001); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_xtol_abs(o, x_tol.data); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_maxtime(o, opt->max_time); + Assert(nlopt_res == NLOPT_SUCCESS); + nlopt_res = nlopt_set_maxeval(o, opt->max_eval); + Assert(nlopt_res == NLOPT_SUCCESS); +#endif + + + auto start_guess = opt->guess; // save for restoration after restarts if necessary + int try_count = 0; + bool is_valid_more = false; + bool is_valid = false; + for (; try_count < opt->max_tries; try_count++) { // todo: add nlopt stop criteria to list, add max_time for full loop +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Optimization"); +#endif + + // initial guess + Array x; + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Initial guess"); +#endif + x = init_guess_segments(opt); + result.x0 = x; + } + + // launch optimization + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("NLopt optimization"); +#endif + + real f = HUGE_VAL; + // note: can we initialize grad to 0 here +#ifdef BLAST_USE_NATIVE_SQP + stop.nevals_p = 0; + result.nlopt_exit_criteria = sqp( + opt->bspline.x_len(opt->task), + objective_function, + opt, + 1, + &fc, + 0, + nullptr, + lb.data, + ub.data, + x.data, + &f, + &stop); + result.num_eval = stop.nevals_p; + +#else + result.nlopt_exit_criteria = nlopt_optimize(o, x.data, &f); + result.num_eval = nlopt_get_numevals(o); +#endif + } + + // validate solution + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Solution validation"); +#endif + Array constraints_points(opt->constraints.n_constraints); + Matrix gradient; + constraints_and_gradients_with_double_broadphase(x, *opt, constraints_points, gradient); + auto max_con = max(constraints_points); + result.max_constraint_idx = argmax(constraints_points); + result.max_constraint_value = max_con; + is_valid = max_con < opt->success_tolerance * 2; + } + + { +#if BLAST_TRACE_LEVEL >= 1 + PROFILE_SCOPE("Solution validation (more points)"); +#endif + u64 steps_ms = (u64) (std::ceil(x.back() * 1e3 / output_steps_ms)); + x.back() = (real) (std::ceil(x.back() * 1000.0 / output_steps_ms) * output_steps_ms) * 1e-3; + int points_more = (int) (steps_ms + 1); + + Bspline bspline_val_more(opt->bspline.n_ctrl, points_more, opt->bspline.degree, opt->manip.n_joints); // todo: this is expensive + bspline_val_more.compute_trajectory(x, opt->task); + auto opt_val_more(*opt); + opt_val_more.set_bspline(bspline_val_more); + n_con_with_segments(&opt_val_more); + Array constraints_more_points(opt_val_more.constraints.n_constraints); + Matrix gradient; + constraints_and_gradients_with_double_broadphase(x, opt_val_more, constraints_more_points, gradient); + // is_valid_more = max(constraints_more_points) < opt->success_tolerance; + auto max_con_more = max(constraints_more_points); + result.max_constraint_more_points_idx = argmax(constraints_more_points); + result.max_constraint_more_points_value = max_con_more; + is_valid_more = max_con_more < opt->success_tolerance * 2; + + result.x = x; + + if (is_valid && is_valid_more) { + result.trajectory = bspline_val_more.traj; + // break; + } else if (opt->guess.type != Guess::random && try_count == 0) { + opt->guess.type = Guess::random; + } + } +#if BLAST_TRACE_LEVEL >= 1 + FrameMark; +#endif + } + + opt->guess = start_guess; // reset to original + + auto time = (real) (get_tick_us() - T1) / 1000.0; + + // Output results + result.success = is_valid && is_valid_more; + result.success_false = is_valid && !is_valid_more; + result.compute_time = time; + result.opt = opt; + result.num_tries = try_count; + +#ifndef BLAST_USE_NATIVE_SQP + nlopt_destroy(o); +#endif + + return result; +} + // ------------------------- Accelerated functions -------------------------------- inline Result optimize_with_analytical_pva_impl(Optimization* opt, u32 output_steps_ms = 1 /*ms*/) { @@ -1002,6 +1382,10 @@ inline Result optimize(Optimization* opt, u32 output_steps_ms = 1 /*ms*/) { return optimize_with_analytical_dynamics_impl(opt, output_steps_ms); case OptimizationMethod::with_segments: return optimize_with_segments_impl(opt, output_steps_ms); + case OptimizationMethod::broadphase: + return optimize_with_broadphase_impl(opt, output_steps_ms); + case OptimizationMethod::double_broadphase: + return optimize_with_double_broadphase_impl(opt, output_steps_ms); } return optimize_with_segments_impl(opt, output_steps_ms); // unreachable } diff --git a/blast/world/broadphase.hpp b/blast/world/broadphase.hpp new file mode 100644 index 0000000..dc36b27 --- /dev/null +++ b/blast/world/broadphase.hpp @@ -0,0 +1,776 @@ +#pragma once +#include +#include + +#if ENABLE_TRACY +#include "tracy/Tracy.hpp" +#include "tracy/TracyC.h" +#endif + +namespace blast { + +inline host_fn void create_AABB_from_sphere(const Sphere& sphere, AxisAlignedBoundingBox& aabb, const void* ptr, int point_in_segment = -1) { + aabb.center = sphere.center; + aabb.extents.x = sphere.radius; + aabb.extents.y = sphere.radius; + aabb.extents.z = sphere.radius; + aabb.child_type = CollisionObjectType::sphere; + aabb.child_ptr = ptr; + aabb.point_in_segment = point_in_segment; +} + +inline host_fn void create_AABB_from_box(const Box& box, AxisAlignedBoundingBox& aabb, const void* ptr, int point_in_segment = -1) { + aabb.center = box.center; + + Vec3 x = {box.extents.x, 0, 0}; + Vec3 y = {0, box.extents.y, 0}; + Vec3 z = {0, 0, box.extents.z}; + + Vec3 x0 = box.rotation * x; + Vec3 y0 = box.rotation * y; + Vec3 z0 = box.rotation * z; + + aabb.extents.x = std::abs(x0[0]) + std::abs(y0[0]) + std::abs(z0[0]); + aabb.extents.y = std::abs(x0[1]) + std::abs(y0[1]) + std::abs(z0[1]); + aabb.extents.z = std::abs(x0[2]) + std::abs(y0[2]) + std::abs(z0[2]); + + aabb.child_type = CollisionObjectType::box; + aabb.child_ptr = ptr; + aabb.point_in_segment = point_in_segment; +} + +inline host_fn void create_AABB_from_door(const Box& box, AxisAlignedBoundingBox& aabb, const void* ptr, int point_in_segment = -1) { + aabb.center = box.center; + + Vec3 x = {box.extents.x, 0, 0}; + Vec3 y = {0, box.extents.y, 0}; + Vec3 z = {0, 0, box.extents.z}; + + Vec3 x0 = box.rotation * x; + Vec3 y0 = box.rotation * y; + Vec3 z0 = box.rotation * z; + + aabb.extents.x = std::abs(x0[0]) + std::abs(y0[0]) + std::abs(z0[0]); + aabb.extents.y = std::abs(x0[1]) + std::abs(y0[1]) + std::abs(z0[1]); + aabb.extents.z = std::abs(x0[2]) + std::abs(y0[2]) + std::abs(z0[2]); + + aabb.child_type = CollisionObjectType::door; + aabb.child_ptr = ptr; + aabb.point_in_segment = point_in_segment; +} + +inline host_fn void create_AABB_from_capsule(const Capsule& capsule, AxisAlignedBoundingBox& aabb, const void* ptr, int point_in_segment = -1) { + aabb.center = (capsule.p1 + capsule.p2) / 2; + Vec3 halfwidths = (capsule.p2 - capsule.p1) / 2; + aabb.extents.x = std::abs(halfwidths.x) + capsule.radius; + aabb.extents.y = std::abs(halfwidths.y) + capsule.radius; + aabb.extents.z = std::abs(halfwidths.z) + capsule.radius; + + aabb.child_type = CollisionObjectType::capsule; + aabb.child_ptr = ptr; + aabb.point_in_segment = point_in_segment; +} + +inline host_fn void create_AABB_from_AABBs(AxisAlignedBoundingBox& b1, AxisAlignedBoundingBox& b2, AxisAlignedBoundingBox& aabb, int id1, int id2) { + Vec3 a = b1.center + b1.extents; + Vec3 b = b2.center + b2.extents; + Vec3 max = {std::max(a[0], b[0]), std::max(a[1], b[1]), std::max(a[2], b[2])}; + + a = b1.center - b1.extents; + b = b2.center - b2.extents; + Vec3 min = {std::min(a[0], b[0]), std::min(a[1], b[1]), std::min(a[2], b[2])}; + + aabb.center = (max + min) * 0.5; + aabb.extents = (max - min) * 0.5; + + aabb.children[0] = id1; + aabb.children[1] = id2; + + aabb.child_type = CollisionObjectType::aabb; +} + +template +inline host_fn void find_boxes_to_merge(const BoundingVolumeHierarchy& BVH, std::vector& boxes, int& box1, int& box2, int num_boxes) { + real closest_dist, dist; // squared distances + closest_dist = INF_REAL; + Vec3 diff; + + for (int i = 0; i < num_boxes - 1; i++) { + for (int j = i + 1; j < num_boxes; j++) { + diff = BVH.leaves[boxes[i]].center - BVH.leaves[boxes[j]].center; + dist = dot(diff, diff); + if (dist < closest_dist) { + closest_dist = dist; + box1 = i; + box2 = j; + } + } + } +} + +// Use if worlds regularly contain more than 50 obstacles +template +inline host_fn void find_boxes_to_merge_sap(const BoundingVolumeHierarchy& BVH, std::vector& boxes, int& box1, int& box2, int num_boxes) { + if (num_boxes == 2) { + box1 = 0; + box2 = 1; + return; + } + + std::vector indices(num_boxes); + for (int i = 0; i < num_boxes; i++) { + indices[i] = i; + } + + std::stable_sort(indices.begin(), indices.end(), [&BVH, &boxes](int a, int b) { return BVH.leaves[boxes[a]].center.x < BVH.leaves[boxes[b]].center.x; }); + + Vec3 diff; + real closest_dist = INF_REAL, dist, dx; + + for (int i = 0; i < num_boxes - 1; i++) { + int idx1 = indices[i]; + for (int j = i + 1; j < num_boxes; j++) { + int idx2 = indices[j]; + dx = BVH.leaves[boxes[idx2]].center.x - BVH.leaves[boxes[idx1]].center.x; + if (dx * dx >= closest_dist) { + break; + } + diff = BVH.leaves[boxes[idx1]].center - BVH.leaves[boxes[idx2]].center; + dist = dot(diff, diff); + + if (dist < closest_dist) { + closest_dist = dist; + box1 = idx1; + box2 = idx2; + } + } + } +} + +template +inline host_fn void create_bounding_volume_hierarchy(std::vector& objects, BoundingVolumeHierarchy& BVH, int n_leaves) { + int num = objects.size(); + int i, j, min, max; + while (num > 1) { + find_boxes_to_merge(BVH, objects, i, j, num); + create_AABB_from_AABBs(BVH.leaves[objects[i]], BVH.leaves[objects[j]], BVH.leaves[n_leaves], objects[i], objects[j]); + n_leaves++; + min = i; + max = j; + if (max < min) + max = i, min = j; + + // removes merged aabbs and adds the new one + objects[min] = n_leaves - 1; + objects[max] = objects[num - 1]; + num--; + } + + // Assign remaining aabb to BVH root node + BVH.root = 2 * BVH.num_objects - 2; +} + +template +inline host_fn void create_bounding_volume_hierarchy_sap(std::vector& objects, BoundingVolumeHierarchy& BVH, int n_leaves) { + int num = objects.size(); + int i, j, min, max; + while (num > 1) { + find_boxes_to_merge_sap(BVH, objects, i, j, num); + create_AABB_from_AABBs(BVH.leaves[objects[i]], BVH.leaves[objects[j]], BVH.leaves[n_leaves], objects[i], objects[j]); + n_leaves++; + min = i; + max = j; + if (max < min) + max = i, min = j; + + // removes merged aabbs and adds the new one + objects[min] = n_leaves - 1; + objects[max] = objects[num - 1]; + num--; + } + // Assign remaining aabb to BVH parent aabb + BVH.root = 2 * BVH.num_objects - 2; +} + +template +inline host_fn void create_static_bounding_volume_hierarchy(World& world, BoundingVolumeHierarchy& BVH) { + // Create leaves + BVH.num_objects = world.boxes.size() + world.spheres.size() + world.capsules.size(); + + if (BVH.num_objects == 0) + return; + + BVH.leaves.resize(2 * BVH.num_objects - 1); + std::vector objects; + + int n_leaves = 0; + // Add objects + for (auto& box: world.boxes) { + create_AABB_from_box(box, BVH.leaves[n_leaves], &box); + n_leaves++; + } + for (auto& sphere: world.spheres) { + create_AABB_from_sphere(sphere, BVH.leaves[n_leaves], &sphere); + n_leaves++; + } + for (auto& capsule: world.capsules) { + create_AABB_from_capsule(capsule, BVH.leaves[n_leaves], &capsule); + n_leaves++; + } + + objects.resize(BVH.num_objects); + for (int i = 0; i < BVH.num_objects; i++) { + objects[i] = i; + } + + create_bounding_volume_hierarchy(objects, BVH, n_leaves); +} + +template +inline host_fn void create_static_bounding_volume_hierarchy_sap(World& world, BoundingVolumeHierarchy& BVH) { + // Create leaves + BVH.num_objects = world.boxes.size() + world.spheres.size() + world.capsules.size(); + + if (BVH.num_objects == 0) + return; + + BVH.leaves.resize(2 * BVH.num_objects - 1); + std::vector objects; + + int n_leaves = 0; + // Add objects + for (auto& box: world.boxes) { + create_AABB_from_box(box, BVH.leaves[n_leaves], &box); + n_leaves++; + } + for (auto& sphere: world.spheres) { + create_AABB_from_sphere(sphere, BVH.leaves[n_leaves], &sphere); + n_leaves++; + } + for (auto& capsule: world.capsules) { + create_AABB_from_capsule(capsule, BVH.leaves[n_leaves], &capsule); + n_leaves++; + } + + objects.resize(BVH.num_objects); + for (int i = 0; i < BVH.num_objects; i++) { + objects[i] = i; + } + + create_bounding_volume_hierarchy_sap(objects, BVH, n_leaves); +} + +template +inline host_fn void create_dynamic_bounding_volume_hierarchy(World& world, BoundingVolumeHierarchy& BVH, real time = 0.0) { + // Create leaves + BVH.num_objects = world.dynamic_boxes.size() + world.dynamic_spheres.size() + world.dynamic_capsules.size() + world.dynamic_doors.size(); + BVH.time = time; + if (BVH.num_objects == 0) + return; + + BVH.leaves.resize(2 * BVH.num_objects - 1); + std::vector objects; + + int n_leaves = 0; + // Add dynamic objects + for (auto& box: world.dynamic_boxes) { + create_AABB_from_box(box.lookup(time), BVH.leaves[n_leaves], &box); + n_leaves++; + } + for (auto& sphere: world.dynamic_spheres) { + create_AABB_from_sphere(sphere.lookup(time), BVH.leaves[n_leaves], &sphere); + n_leaves++; + } + for (auto& capsule: world.dynamic_capsules) { + create_AABB_from_capsule(capsule.lookup(time), BVH.leaves[n_leaves], &capsule); + n_leaves++; + } + for (auto& door: world.dynamic_doors) { + create_AABB_from_box(door.lookup(time), BVH.leaves[n_leaves], &door); + BVH.leaves[n_leaves].child_type = CollisionObjectType::door; + n_leaves++; + } + + objects.resize(BVH.num_objects); + for (int i = 0; i < BVH.num_objects; i++) { + objects[i] = i; + } + + create_bounding_volume_hierarchy(objects, BVH, n_leaves); +} + +template +inline host_fn void create_time_bvh_dynamic_objects(World& world, BoundingVolumeHierarchy& BVH, int start_point_in_segment, int n_points_per_segment, + real opt_time, int n_segments, real trajectory_start_time) { + // Create leaves + BVH.num_objects = (world.dynamic_boxes.size() + world.dynamic_spheres.size() + world.dynamic_capsules.size() + world.dynamic_doors.size()) * n_points_per_segment; + + if (BVH.num_objects == 0) + return; + + BVH.leaves.resize(2 * BVH.num_objects - 1); + std::vector objects; + int max_point = n_segments * n_points_per_segment - 1; + + int n_leaves = 0; + // Add objects + for (int point_in_segment = 0; point_in_segment < n_points_per_segment; point_in_segment++) { + int current_point = start_point_in_segment + point_in_segment; + real current_time = opt_time * ((real) current_point / (real) max_point) + trajectory_start_time; // trajectory time * progression along trajectory + + for (const auto& box: world.dynamic_boxes) { + create_AABB_from_box(box.lookup(current_time), BVH.leaves[n_leaves], &box, point_in_segment); + n_leaves++; + } + for (const auto& sphere: world.dynamic_spheres) { + create_AABB_from_sphere(sphere.lookup(current_time), BVH.leaves[n_leaves], &sphere, point_in_segment); + n_leaves++; + } + for (const auto& capsule: world.dynamic_capsules) { + create_AABB_from_capsule(capsule.lookup(current_time), BVH.leaves[n_leaves], &capsule, point_in_segment); + n_leaves++; + } + for (const auto& door: world.dynamic_doors) { + create_AABB_from_door(door.lookup(current_time), BVH.leaves[n_leaves], &door, point_in_segment); + BVH.leaves[n_leaves].child_type = CollisionObjectType::door; + n_leaves++; + } + } + + objects.resize(BVH.num_objects); + for (int i = 0; i < BVH.num_objects; i++) { + objects[i] = i; + } + + create_bounding_volume_hierarchy_sap(objects, BVH, n_leaves); +} + +// One BVH per capsule because one collision constraint needed per capsule +inline host_fn void create_time_bounding_volume_hierarchies(std::array, MAX_CAPSULES>& BVH, std::vector>& capsules, + const int num_capsules, const int num_points_in_segment) { + std::vector objects(num_points_in_segment); + + for (int caps_id = 0; caps_id < num_capsules; caps_id++) { + BVH[caps_id].num_objects = num_points_in_segment; + BVH[caps_id].leaves.resize(2 * num_points_in_segment - 1); + + for (int point = 0; point < num_points_in_segment; point++) { + create_AABB_from_capsule(capsules[point][caps_id], BVH[caps_id].leaves[point], &capsules[point][caps_id]); + BVH[caps_id].leaves[point].point_in_segment = point; + objects[point] = point; + } + + create_bounding_volume_hierarchy(objects, BVH[caps_id], num_points_in_segment); + } +} + +inline host_fn real minimum_distance_static(const Capsule& capsule, BoundingVolumeHierarchy& BVH, CollisionEntities& collision_objects, int point_in_segment) { + BVH.queue.clear_and_reserve(BVH.num_objects); + + real dist_min = INF_REAL; + real dist = INF_REAL; + + AxisAlignedBoundingBox arm; + create_AABB_from_capsule(capsule, arm, &capsule); + + std::vector& leaves = BVH.leaves; + AxisAlignedBoundingBox* object = &leaves[BVH.root]; + + leaves[object->children[0]].dist = distance(arm, leaves[object->children[0]]); + leaves[object->children[1]].dist = distance(arm, leaves[object->children[1]]); + BVH.queue.push(object->children[0]); + BVH.queue.push(object->children[1]); + + while (BVH.queue.size() > 0) { + object = &leaves[BVH.queue.top()]; + BVH.queue.pop(); + + // STOPPING CRITERIA: NO FURTHER IMPROVEMENT POSSIBLE + if (object->dist >= dist_min) { + return dist_min; + } + + switch (object->child_type) { + case CollisionObjectType::aabb: { + leaves[object->children[0]].dist = distance(arm, leaves[object->children[0]]); + leaves[object->children[1]].dist = distance(arm, leaves[object->children[1]]); + BVH.queue.push(object->children[0]); + BVH.queue.push(object->children[1]); + break; + } + case CollisionObjectType::box: { + Box& box = *((Box*) object->child_ptr); + dist = distance(capsule, box); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::box; + collision_objects.box = box; + collision_objects.point_in_segment = point_in_segment; + } + break; + } + case CollisionObjectType::sphere: { + Sphere& sphere = *((Sphere*) object->child_ptr); + dist = distance(capsule, sphere); // MAYBE: make function for aabb/sphere dist_min + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::sphere; + collision_objects.sphere = sphere; + collision_objects.point_in_segment = point_in_segment; + } + break; + } + case CollisionObjectType::capsule: { + Capsule& caps = *((Capsule*) object->child_ptr); + dist = distance(capsule, caps); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::capsule; + collision_objects.capsule = caps; + collision_objects.point_in_segment = point_in_segment; + } + break; + } + } + } + return dist_min; +} + +inline host_fn real minimum_distance_dynamic(const Capsule& capsule, BoundingVolumeHierarchy& BVH, real dist_min_old, CollisionEntities& collision_objects, int point_in_segment) { + BVH.queue.clear_and_reserve(BVH.num_objects); + + real dist_min = dist_min_old; + real dist = INF_REAL; + + AxisAlignedBoundingBox arm; + create_AABB_from_capsule(capsule, arm, &capsule); + + std::vector& leaves = BVH.leaves; + AxisAlignedBoundingBox* object = &leaves[BVH.root]; + + leaves[object->children[0]].dist = distance(arm, leaves[object->children[0]]); + leaves[object->children[1]].dist = distance(arm, leaves[object->children[1]]); + BVH.queue.push(object->children[0]); + BVH.queue.push(object->children[1]); + + while (BVH.queue.size() > 0) { + object = &leaves[BVH.queue.top()]; + BVH.queue.pop(); + + // STOPPING CRITERIA: NO FURTHER IMPROVEMENT POSSIBLE + if (object->dist >= dist_min) { + return dist_min; + } + + switch (object->child_type) { + case CollisionObjectType::aabb: { + leaves[object->children[0]].dist = distance(arm, leaves[object->children[0]]); + leaves[object->children[1]].dist = distance(arm, leaves[object->children[1]]); + BVH.queue.push(object->children[0]); + BVH.queue.push(object->children[1]); + break; + } + case CollisionObjectType::box: { + Box box = ((DynamicBox*) object->child_ptr)->lookup(BVH.time); + dist = distance(capsule, box); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::box; + collision_objects.box = box; + collision_objects.point_in_segment = point_in_segment; + } + break; + } + case CollisionObjectType::sphere: { + Sphere sphere = ((DynamicSphere*) object->child_ptr)->lookup(BVH.time); + dist = distance(capsule, sphere); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::sphere; + collision_objects.sphere = sphere; + collision_objects.point_in_segment = point_in_segment; + } + break; + } + case CollisionObjectType::capsule: { + Capsule caps = ((DynamicCapsule*) object->child_ptr)->lookup(BVH.time); + dist = distance(capsule, caps); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::capsule; + collision_objects.capsule = caps; + collision_objects.point_in_segment = point_in_segment; + } + break; + } + case CollisionObjectType::door: { + Box door = ((DynamicDoor*) object->child_ptr)->lookup(BVH.time); + dist = distance(capsule, door); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::box; + collision_objects.box = door; + collision_objects.point_in_segment = point_in_segment; + } + } + } + } + return dist_min; +} + +inline host_fn real minimum_distance_static_objects_time(BoundingVolumeHierarchy& BVH_obj, BoundingVolumeHierarchy& BVH_time, + CollisionEntities& collision_objects) { + + BVH_time.queue.clear_and_reserve(BVH_time.num_objects * BVH_obj.num_objects); + + real dist_min = INF_REAL; + real dist = INF_REAL; + + std::vector& leaves_obj = BVH_obj.leaves; + std::vector& leaves_time = BVH_time.leaves; + + AxisAlignedBoundingBox* object1 = &leaves_obj[BVH_obj.root]; + AxisAlignedBoundingBox* object2 = &leaves_time[BVH_time.root]; + + AABBPair pair; + + // Test BVHs' children against eachother and add to priority queue + pair.aabb_obj = object1->children[0]; + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_obj = object1->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + CollisionObjectType child_type1; + CollisionObjectType child_type2; + + while (BVH_time.queue.size() > 0) { + pair = BVH_time.queue.top(); + object1 = &leaves_obj[pair.aabb_obj]; + child_type1 = object1->child_type; + object2 = &leaves_time[pair.aabb_cap]; + child_type2 = object2->child_type; + BVH_time.queue.pop(); + + // STOPPING CRITERIA: NO FURTHER IMPROVEMENT POSSIBLE + if (pair.dist >= dist_min) { + return dist_min; + } + + if (child_type1 == CollisionObjectType::aabb && child_type2 == CollisionObjectType::capsule) { + pair.aabb_obj = object1->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_obj = object1->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } else if (child_type1 == CollisionObjectType::box && child_type2 == CollisionObjectType::capsule) { + Box& box = *((Box*) object1->child_ptr); + const Capsule& caps = *((Capsule*) object2->child_ptr); + dist = distance(caps, box); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::box; + collision_objects.box = box; + collision_objects.point_in_segment = object2->point_in_segment; + } + } else if (child_type1 == CollisionObjectType::aabb && child_type2 == CollisionObjectType::aabb) { + // Faster to only enter bigger AABB + if ((object1->extents.x * object1->extents.y * object1->extents.z) > (object2->extents.x * object2->extents.y * object2->extents.z)) { + pair.aabb_obj = object1->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_obj = object1->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } else { + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } + } else if (child_type1 != CollisionObjectType::aabb && child_type2 == CollisionObjectType::aabb) { + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } else if (child_type1 == CollisionObjectType::capsule && child_type2 == CollisionObjectType::capsule) { + Capsule& caps1 = *((Capsule*) object1->child_ptr); + Capsule& caps2 = *((Capsule*) object2->child_ptr); + dist = distance(caps1, caps2); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::capsule; + collision_objects.capsule = caps1; + collision_objects.point_in_segment = object2->point_in_segment; + } + } else if (child_type1 == CollisionObjectType::sphere && child_type2 == CollisionObjectType::capsule) { + Sphere& sphere = *((Sphere*) object1->child_ptr); + Capsule& caps = *((Capsule*) object2->child_ptr); + dist = distance(caps, sphere); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::sphere; + collision_objects.sphere = sphere; + collision_objects.point_in_segment = object2->point_in_segment; + } + } + } + return dist_min; +} + +inline host_fn real minimum_distance_dynamic_objects_time(BoundingVolumeHierarchy& BVH_obj, BoundingVolumeHierarchy& BVH_time, real dist_min_old, CollisionEntities& collision_objects, + int start_point_in_segment, int n_points_per_segment, real opt_time, int n_segments, int trajectory_start_time) { + + BVH_time.queue.clear_and_reserve(BVH_obj.num_objects * BVH_time.num_objects * 4); + + real dist_min = dist_min_old; + real dist = INF_REAL; + + std::vector& leaves_obj = BVH_obj.leaves; + std::vector& leaves_time = BVH_time.leaves; + + AxisAlignedBoundingBox* object1 = &leaves_obj[BVH_obj.root]; + AxisAlignedBoundingBox* object2 = &leaves_time[BVH_time.root]; + + AABBPair pair; + + // Test BVHs' children against eachother and add to priority queue + pair.aabb_obj = object1->children[0]; + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_obj = object1->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + CollisionObjectType child_type1; + CollisionObjectType child_type2; + + while (BVH_time.queue.size() > 0) { + pair = BVH_time.queue.top(); + object1 = &leaves_obj[pair.aabb_obj]; + child_type1 = object1->child_type; + object2 = &leaves_time[pair.aabb_cap]; + child_type2 = object2->child_type; + BVH_time.queue.pop(); + + // STOPPING CRITERIA: NO FURTHER IMPROVEMENT POSSIBLE + if (pair.dist >= dist_min) { + return dist_min; + } + + if (child_type1 == CollisionObjectType::aabb && child_type2 == CollisionObjectType::capsule) { + pair.aabb_obj = object1->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_obj = object1->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } else if (child_type1 == CollisionObjectType::aabb && child_type2 == CollisionObjectType::aabb) { + // Faster to only enter bigger AABB + if ((object1->extents.x * object1->extents.y * object1->extents.z) > (object2->extents.x * object2->extents.y * object2->extents.z)) { + pair.aabb_obj = object1->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_obj = object1->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } else { + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } + } else if (child_type1 != CollisionObjectType::aabb && child_type2 == CollisionObjectType::aabb) { + pair.aabb_cap = object2->children[0]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + + pair.aabb_cap = object2->children[1]; + pair.dist = distance(leaves_obj[pair.aabb_obj], leaves_time[pair.aabb_cap]); + BVH_time.queue.push(pair); + } else if (leaves_obj[pair.aabb_obj].point_in_segment == leaves_time[pair.aabb_cap].point_in_segment) { + int current_point = start_point_in_segment + leaves_obj[pair.aabb_obj].point_in_segment; + int max_point = n_segments * n_points_per_segment - 1; + real current_time = opt_time * ((real) current_point / (real) max_point) + trajectory_start_time; + + if (child_type1 == CollisionObjectType::box && child_type2 == CollisionObjectType::capsule) { + Box box = ((DynamicBox*) object1->child_ptr)->lookup(current_time); + const Capsule& caps = *((Capsule*) object2->child_ptr); + dist = distance(caps, box); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::box; + collision_objects.box = box; + collision_objects.point_in_segment = object2->point_in_segment; + } + } else if (child_type1 == CollisionObjectType::sphere && child_type2 == CollisionObjectType::capsule) { + Sphere sphere = ((DynamicSphere*) object1->child_ptr)->lookup(current_time); + Capsule& caps = *((Capsule*) object2->child_ptr); + dist = distance(caps, sphere); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::sphere; + collision_objects.sphere = sphere; + collision_objects.point_in_segment = object2->point_in_segment; + } + } else if (child_type1 == CollisionObjectType::capsule && child_type2 == CollisionObjectType::capsule) { + Capsule caps1 = ((DynamicCapsule*) object1->child_ptr)->lookup(current_time); + Capsule& caps2 = *((Capsule*) object2->child_ptr); + dist = distance(caps1, caps2); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::capsule; + collision_objects.capsule = caps1; + collision_objects.point_in_segment = object2->point_in_segment; + } + } else if (child_type1 == CollisionObjectType::door && child_type2 == CollisionObjectType::capsule) { + Box door = ((DynamicDoor*) object1->child_ptr)->lookup(current_time); + Capsule& caps = *((Capsule*) object2->child_ptr); + dist = distance(caps, door); + if (dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::box; + collision_objects.box = door; + collision_objects.point_in_segment = object2->point_in_segment; + } + } + } + } + return dist_min; +} +} // namespace blast diff --git a/blast/world/distance.hpp b/blast/world/distance.hpp index 1e6b589..5174283 100644 --- a/blast/world/distance.hpp +++ b/blast/world/distance.hpp @@ -536,6 +536,23 @@ inline blast_fn real distance(const Capsule& capsule1, const Capsule& capsule2) return dist_seg_seg - capsule1.radius - capsule2.radius; } +inline host_fn real distance(const AxisAlignedBoundingBox& aabb1, const AxisAlignedBoundingBox& aabb2) { + Vec3 d; + d.x = std::max((aabb1.center.x - aabb1.extents.x - aabb2.center.x - aabb2.extents.x), (aabb2.center.x - aabb2.extents.x - aabb1.center.x - aabb1.extents.x)); + d.y = std::max((aabb1.center.y - aabb1.extents.y - aabb2.center.y - aabb2.extents.y), (aabb2.center.y - aabb2.extents.y - aabb1.center.y - aabb1.extents.y)); + d.z = std::max((aabb1.center.z - aabb1.extents.z - aabb2.center.z - aabb2.extents.z), (aabb2.center.z - aabb2.extents.z - aabb1.center.z - aabb1.extents.z)); + + if (d.x > 0 || d.y > 0 || d.z > 0) { + d.x = std::max((real) 0.0, d.x); + d.y = std::max((real) 0.0, d.y); + d.z = std::max((real) 0.0, d.z); + return norm(d); + } else { + return std::max(d.x, std::max(d.y, d.z)); // todo: check this + // return -sqrt(dot(d, d)); + } +} + inline blast_fn Array test_collisions(const ObjMatrix& robot_capsules, const World* world, u32 n_lowest_distances, real start_time, real end_time) { Array dist_min(n_lowest_distances, INF_REAL); real dist; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 31606e6..bf29055 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -99,7 +99,8 @@ set(TESTS # World test_primitives world/test_primitives.cpp - test_gjk_epa world/test_gjk_epa.cpp + test_broadphase world/test_broadphase.cpp + # test_gjk_epa world/test_gjk_epa.cpp # Trajectory test_bspline trajectory/test_bsplines.cpp diff --git a/tests/world/test_broadphase.cpp b/tests/world/test_broadphase.cpp new file mode 100644 index 0000000..9688d45 --- /dev/null +++ b/tests/world/test_broadphase.cpp @@ -0,0 +1,1403 @@ +#define CATCH_CONFIG_MAIN +#define CATCH_CONFIG_ENABLE_BENCHMARKING + +#include +#include "catch2/catch.hpp" + +#define BENCHMARKING_ON 0 + +using namespace blast; + +// Helper functions -> skip around 600 lines to reach TEST_CASES +inline host_fn DynamicBox get_dynamic_box_1() { + DynamicBox box = { + 100, 0, 1, { + {{-1.000000, 2.500000, 1.000000}, {0.20, 0.40, 0.15}, {1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000}}, + {{-0.969697, 2.484848, 1.020202}, {0.20, 0.40, 0.15}, {0.999843, 0.007932, -0.015866, -0.007765, 0.999914, 0.010576, 0.015948, -0.010451, 0.999818}}, + {{-0.939394, 2.469697, 1.040404}, {0.20, 0.40, 0.15}, {0.999371, 0.015858, -0.031728, -0.015191, 0.999661, 0.021143, 0.032052, -0.020648, 0.999273}}, + {{-0.909091, 2.454545, 1.060606}, {0.20, 0.40, 0.15}, {0.998584, 0.023771, -0.047582, -0.022276, 0.999249, 0.031692, 0.048300, -0.030587, 0.998364}}, + {{-0.878788, 2.439394, 1.080808}, {0.20, 0.40, 0.15}, {0.997484, 0.031664, -0.063424, -0.029018, 0.998687, 0.042213, 0.064677, -0.040267, 0.997094}}, + {{-0.848485, 2.424242, 1.101010}, {0.20, 0.40, 0.15}, {0.996071, 0.039531, -0.079250, -0.035415, 0.997982, 0.052698, 0.081173, -0.049684, 0.995461}}, + {{-0.818182, 2.409091, 1.121212}, {0.20, 0.40, 0.15}, {0.994344, 0.047366, -0.095056, -0.041464, 0.997143, 0.063137, 0.097775, -0.058838, 0.993468}}, + {{-0.787879, 2.393939, 1.141414}, {0.20, 0.40, 0.15}, {0.992306, 0.055163, -0.110838, -0.047166, 0.996178, 0.073521, 0.114470, -0.067727, 0.991115}}, + {{-0.757576, 2.378788, 1.161616}, {0.20, 0.40, 0.15}, {0.989958, 0.062914, -0.126592, -0.052519, 0.995094, 0.083841, 0.131246, -0.076351, 0.988405}}, + {{-0.727273, 2.363636, 1.181818}, {0.20, 0.40, 0.15}, {0.987299, 0.070613, -0.142315, -0.057523, 0.993901, 0.094089, 0.148091, -0.084707, 0.985339}}, + {{-0.696970, 2.348485, 1.202020}, {0.20, 0.40, 0.15}, {0.984333, 0.078254, -0.158001, -0.062178, 0.992605, 0.104254, 0.164991, -0.092797, 0.981920}}, + {{-0.666667, 2.333333, 1.222222}, {0.20, 0.40, 0.15}, {0.981060, 0.085832, -0.173648, -0.066484, 0.991216, 0.114329, 0.181936, -0.100619, 0.978149}}, + {{-0.636364, 2.318182, 1.242424}, {0.20, 0.40, 0.15}, {0.977482, 0.093338, -0.189251, -0.070442, 0.989740, 0.124305, 0.198912, -0.108174, 0.974029}}, + {{-0.606061, 2.303030, 1.262626}, {0.20, 0.40, 0.15}, {0.973602, 0.100768, -0.204807, -0.074053, 0.988187, 0.134172, 0.215908, -0.115464, 0.969563}}, + {{-0.575758, 2.287879, 1.282828}, {0.20, 0.40, 0.15}, {0.969420, 0.108115, -0.220311, -0.077319, 0.986564, 0.143923, 0.232911, -0.122487, 0.964754}}, + {{-0.545455, 2.272727, 1.303030}, {0.20, 0.40, 0.15}, {0.964939, 0.115373, -0.235759, -0.080241, 0.984878, 0.153548, 0.249909, -0.129246, 0.959605}}, + {{-0.515152, 2.257576, 1.323232}, {0.20, 0.40, 0.15}, {0.960161, 0.122535, -0.251148, -0.082821, 0.983137, 0.163039, 0.266891, -0.135743, 0.954119}}, + {{-0.484848, 2.242424, 1.343434}, {0.20, 0.40, 0.15}, {0.955090, 0.129596, -0.266474, -0.085063, 0.981350, 0.172387, 0.283845, -0.141978, 0.948301}}, + {{-0.454545, 2.227273, 1.363636}, {0.20, 0.40, 0.15}, {0.949727, 0.136550, -0.281733, -0.086967, 0.979522, 0.181585, 0.300759, -0.147955, 0.942154}}, + {{-0.424242, 2.212121, 1.383838}, {0.20, 0.40, 0.15}, {0.944075, 0.143391, -0.296920, -0.088539, 0.977662, 0.190624, 0.317622, -0.153675, 0.935682}}, + {{-0.393939, 2.196970, 1.404040}, {0.20, 0.40, 0.15}, {0.938137, 0.150113, -0.312033, -0.089781, 0.975777, 0.199496, 0.334422, -0.159140, 0.928890}}, + {{-0.363636, 2.181818, 1.424242}, {0.20, 0.40, 0.15}, {0.931917, 0.156709, -0.327068, -0.090697, 0.973873, 0.208194, 0.351149, -0.164355, 0.921782}}, + {{-0.333333, 2.166667, 1.444444}, {0.20, 0.40, 0.15}, {0.925417, 0.163176, -0.342020, -0.091290, 0.971959, 0.216708, 0.367791, -0.169322, 0.914363}}, + {{-0.303030, 2.151515, 1.464646}, {0.20, 0.40, 0.15}, {0.918640, 0.169506, -0.356886, -0.091567, 0.970039, 0.225032, 0.384338, -0.174044, 0.906638}}, + {{-0.272727, 2.136364, 1.484848}, {0.20, 0.40, 0.15}, {0.911591, 0.175695, -0.371662, -0.091530, 0.968122, 0.233158, 0.400779, -0.178526, 0.898613}}, + {{-0.242424, 2.121212, 1.505051}, {0.20, 0.40, 0.15}, {0.904273, 0.181736, -0.386345, -0.091185, 0.966213, 0.241078, 0.417104, -0.182771, 0.890291}}, + {{-0.212121, 2.106061, 1.525253}, {0.20, 0.40, 0.15}, {0.896689, 0.187625, -0.400931, -0.090538, 0.964318, 0.248785, 0.433303, -0.186784, 0.881680}}, + {{-0.181818, 2.090909, 1.545455}, {0.20, 0.40, 0.15}, {0.888844, 0.193356, -0.415415, -0.089594, 0.962443, 0.256273, 0.449365, -0.190568, 0.872786}}, + {{-0.151515, 2.075758, 1.565657}, {0.20, 0.40, 0.15}, {0.880741, 0.198924, -0.429795, -0.088358, 0.960595, 0.263533, 0.465282, -0.194129, 0.863612}}, + {{-0.121212, 2.060606, 1.585859}, {0.20, 0.40, 0.15}, {0.872386, 0.204324, -0.444067, -0.086836, 0.958779, 0.270560, 0.481044, -0.197472, 0.854167}}, + {{-0.090909, 2.045455, 1.606061}, {0.20, 0.40, 0.15}, {0.863781, 0.209551, -0.458227, -0.085036, 0.956999, 0.277346, 0.496641, -0.200601, 0.844457}}, + {{-0.060606, 2.030303, 1.626263}, {0.20, 0.40, 0.15}, {0.854931, 0.214600, -0.472271, -0.082963, 0.955262, 0.283886, 0.512064, -0.203521, 0.834487}}, + {{-0.030303, 2.015152, 1.646465}, {0.20, 0.40, 0.15}, {0.845841, 0.219466, -0.486197, -0.080625, 0.953572, 0.290172, 0.527306, -0.206240, 0.824265}}, + {{0.000000, 2.000000, 1.666667}, {0.20, 0.40, 0.15}, {0.836516, 0.224144, -0.500000, -0.078027, 0.951934, 0.296198, 0.542358, -0.208761, 0.813798}}, + {{0.030303, 1.984848, 1.686869}, {0.20, 0.40, 0.15}, {0.826961, 0.228630, -0.513677, -0.075178, 0.950352, 0.301959, 0.557211, -0.211091, 0.803092}}, + {{0.060606, 1.969697, 1.707071}, {0.20, 0.40, 0.15}, {0.817179, 0.232920, -0.527225, -0.072085, 0.948830, 0.307449, 0.571858, -0.213236, 0.792154}}, + {{0.090909, 1.954545, 1.727273}, {0.20, 0.40, 0.15}, {0.807177, 0.237009, -0.540641, -0.068755, 0.947373, 0.312662, 0.586292, -0.215202, 0.780993}}, + {{0.121212, 1.939394, 1.747475}, {0.20, 0.40, 0.15}, {0.796959, 0.240892, -0.553920, -0.065196, 0.945983, 0.317593, 0.600505, -0.216996, 0.769615}}, + {{0.151515, 1.924242, 1.767677}, {0.20, 0.40, 0.15}, {0.786531, 0.244566, -0.567060, -0.061416, 0.944665, 0.322237, 0.614490, -0.218623, 0.758028}}, + {{0.181818, 1.909091, 1.787879}, {0.20, 0.40, 0.15}, {0.775897, 0.248027, -0.580057, -0.057423, 0.943421, 0.326588, 0.628241, -0.220090, 0.746240}}, + {{0.212121, 1.893939, 1.808081}, {0.20, 0.40, 0.15}, {0.765064, 0.251271, -0.592908, -0.053225, 0.942254, 0.330643, 0.641751, -0.221405, 0.734259}}, + {{0.242424, 1.878788, 1.828283}, {0.20, 0.40, 0.15}, {0.754037, 0.254294, -0.605610, -0.048831, 0.941167, 0.334395, 0.655015, -0.222574, 0.722092}}, + {{0.272727, 1.863636, 1.848485}, {0.20, 0.40, 0.15}, {0.742821, 0.257093, -0.618159, -0.044249, 0.940162, 0.337842, 0.668026, -0.223603, 0.709748}}, + {{0.303030, 1.848485, 1.868687}, {0.20, 0.40, 0.15}, {0.731422, 0.259663, -0.630553, -0.039488, 0.939242, 0.340978, 0.680781, -0.224500, 0.697236}}, + {{0.333333, 1.833333, 1.888889}, {0.20, 0.40, 0.15}, {0.719846, 0.262003, -0.642788, -0.034555, 0.938407, 0.343800, 0.693273, -0.225271, 0.684562}}, + {{0.363636, 1.818182, 1.909091}, {0.20, 0.40, 0.15}, {0.708099, 0.264107, -0.654861, -0.029461, 0.937659, 0.346304, 0.705498, -0.225925, 0.671737}}, + {{0.393939, 1.803030, 1.929293}, {0.20, 0.40, 0.15}, {0.696187, 0.265975, -0.666769, -0.024214, 0.937001, 0.348488, 0.717452, -0.226467, 0.658768}}, + {{0.424242, 1.787879, 1.949495}, {0.20, 0.40, 0.15}, {0.684116, 0.267601, -0.678509, -0.018823, 0.936431, 0.350346, 0.729130, -0.226906, 0.645664}}, + {{0.454545, 1.772727, 1.969697}, {0.20, 0.40, 0.15}, {0.671891, 0.268985, -0.690079, -0.013296, 0.935952, 0.351877, 0.740530, -0.227248, 0.632435}}, + {{0.484848, 1.757576, 1.989899}, {0.20, 0.40, 0.15}, {0.659520, 0.270122, -0.701475, -0.007644, 0.935563, 0.353078, 0.751648, -0.227500, 0.619087}}, + {{0.515152, 1.742424, 2.010101}, {0.20, 0.40, 0.15}, {0.647008, 0.271011, -0.712694, -0.001874, 0.935264, 0.353945, 0.762481, -0.227670, 0.605632}}, + {{0.545455, 1.727273, 2.030303}, {0.20, 0.40, 0.15}, {0.634362, 0.271650, -0.723734, 0.004004, 0.935056, 0.354478, 0.773026, -0.227765, 0.592076}}, + {{0.575758, 1.712121, 2.050505}, {0.20, 0.40, 0.15}, {0.621588, 0.272035, -0.734592, 0.009981, 0.934937, 0.354673, 0.783281, -0.227793, 0.578431}}, + {{0.606061, 1.696970, 2.070707}, {0.20, 0.40, 0.15}, {0.608693, 0.272166, -0.745264, 0.016048, 0.934907, 0.354529, 0.793244, -0.227760, 0.564703}}, + {{0.636364, 1.681818, 2.090909}, {0.20, 0.40, 0.15}, {0.595682, 0.272039, -0.755750, 0.022196, 0.934965, 0.354044, 0.802913, -0.227673, 0.550904}}, + {{0.666667, 1.666667, 2.111111}, {0.20, 0.40, 0.15}, {0.582563, 0.271654, -0.766044, 0.028416, 0.935110, 0.353218, 0.812288, -0.227540, 0.537041}}, + {{0.696970, 1.651515, 2.131313}, {0.20, 0.40, 0.15}, {0.569343, 0.271008, -0.776146, 0.034699, 0.935339, 0.352047, 0.821368, -0.227367, 0.523125}}, + {{0.727273, 1.636364, 2.151515}, {0.20, 0.40, 0.15}, {0.556027, 0.270101, -0.786053, 0.041036, 0.935651, 0.350533, 0.830151, -0.227163, 0.509163}}, + {{0.757576, 1.621212, 2.171717}, {0.20, 0.40, 0.15}, {0.542623, 0.268931, -0.795762, 0.047418, 0.936044, 0.348674, 0.838637, -0.226932, 0.495166}}, + {{0.787879, 1.606061, 2.191919}, {0.20, 0.40, 0.15}, {0.529136, 0.267497, -0.805270, 0.053837, 0.936515, 0.346470, 0.846827, -0.226683, 0.481143}}, + {{0.818182, 1.590909, 2.212121}, {0.20, 0.40, 0.15}, {0.515575, 0.265797, -0.814576, 0.060283, 0.937062, 0.343920, 0.854721, -0.226422, 0.467103}}, + {{0.848485, 1.575758, 2.232323}, {0.20, 0.40, 0.15}, {0.501946, 0.263832, -0.823677, 0.066749, 0.937681, 0.341026, 0.862320, -0.226156, 0.453055}}, + {{0.878788, 1.560606, 2.252525}, {0.20, 0.40, 0.15}, {0.488255, 0.261600, -0.832570, 0.073225, 0.938370, 0.337786, 0.869624, -0.225890, 0.439008}}, + {{0.909091, 1.545455, 2.272727}, {0.20, 0.40, 0.15}, {0.474509, 0.259102, -0.841254, 0.079703, 0.939125, 0.334202, 0.876635, -0.225632, 0.424972}}, + {{0.939394, 1.530303, 2.292929}, {0.20, 0.40, 0.15}, {0.460716, 0.256335, -0.849725, 0.086175, 0.939943, 0.330275, 0.883354, -0.225388, 0.410957}}, + {{0.969697, 1.515152, 2.313131}, {0.20, 0.40, 0.15}, {0.446881, 0.253301, -0.857983, 0.092633, 0.940819, 0.326005, 0.889784, -0.225163, 0.396970}}, + {{1.000000, 1.500000, 2.333333}, {0.20, 0.40, 0.15}, {0.433013, 0.250000, -0.866025, 0.099068, 0.941749, 0.321394, 0.895927, -0.224963, 0.383022}}, + {{1.030303, 1.484848, 2.353535}, {0.20, 0.40, 0.15}, {0.419117, 0.246431, -0.873849, 0.105474, 0.942729, 0.316443, 0.901785, -0.224795, 0.369122}}, + {{1.060606, 1.469697, 2.373737}, {0.20, 0.40, 0.15}, {0.405201, 0.242595, -0.881453, 0.111842, 0.943755, 0.311155, 0.907361, -0.224664, 0.355278}}, + {{1.090909, 1.454545, 2.393939}, {0.20, 0.40, 0.15}, {0.391271, 0.238492, -0.888835, 0.118165, 0.944822, 0.305531, 0.912658, -0.224575, 0.341500}}, + {{1.121212, 1.439394, 2.414141}, {0.20, 0.40, 0.15}, {0.377335, 0.234123, -0.895994, 0.124435, 0.945924, 0.299574, 0.917679, -0.224533, 0.327797}}, + {{1.151515, 1.424242, 2.434343}, {0.20, 0.40, 0.15}, {0.363399, 0.229489, -0.902927, 0.130646, 0.947056, 0.293286, 0.922428, -0.224543, 0.314177}}, + {{1.181818, 1.409091, 2.454545}, {0.20, 0.40, 0.15}, {0.349469, 0.224590, -0.909632, 0.136790, 0.948214, 0.286669, 0.926909, -0.224610, 0.300650}}, + {{1.212121, 1.393939, 2.474747}, {0.20, 0.40, 0.15}, {0.335554, 0.219428, -0.916108, 0.142861, 0.949391, 0.279728, 0.931125, -0.224739, 0.287224}}, + {{1.242424, 1.378788, 2.494949}, {0.20, 0.40, 0.15}, {0.321659, 0.214004, -0.922354, 0.148852, 0.950582, 0.272464, 0.935082, -0.224935, 0.273909}}, + {{1.272727, 1.363636, 2.515152}, {0.20, 0.40, 0.15}, {0.307792, 0.208320, -0.928368, 0.154757, 0.951782, 0.264882, 0.938783, -0.225200, 0.260712}}, + {{1.303030, 1.348485, 2.535354}, {0.20, 0.40, 0.15}, {0.293959, 0.202376, -0.934148, 0.160570, 0.952983, 0.256985, 0.942234, -0.225539, 0.247642}}, + {{1.333333, 1.333333, 2.555556}, {0.20, 0.40, 0.15}, {0.280166, 0.196175, -0.939693, 0.166285, 0.954180, 0.248776, 0.945440, -0.225955, 0.234708}}, + {{1.363636, 1.318182, 2.575758}, {0.20, 0.40, 0.15}, {0.266422, 0.189718, -0.945001, 0.171896, 0.955367, 0.240261, 0.948405, -0.226453, 0.221919}}, + {{1.393939, 1.303030, 2.595960}, {0.20, 0.40, 0.15}, {0.252731, 0.183008, -0.950071, 0.177399, 0.956537, 0.231444, 0.951134, -0.227035, 0.209281}}, + {{1.424242, 1.287879, 2.616162}, {0.20, 0.40, 0.15}, {0.239101, 0.176046, -0.954902, 0.182788, 0.957684, 0.222328, 0.953635, -0.227704, 0.196804}}, + {{1.454545, 1.272727, 2.636364}, {0.20, 0.40, 0.15}, {0.225539, 0.168836, -0.959493, 0.188058, 0.958801, 0.212919, 0.955911, -0.228462, 0.184496}}, + {{1.484848, 1.257576, 2.656566}, {0.20, 0.40, 0.15}, {0.212050, 0.161379, -0.963842, 0.193205, 0.959882, 0.203222, 0.957970, -0.229313, 0.172363}}, + {{1.515152, 1.242424, 2.676768}, {0.20, 0.40, 0.15}, {0.198641, 0.153678, -0.967949, 0.198225, 0.960919, 0.193242, 0.959817, -0.230257, 0.160415}}, + {{1.545455, 1.227273, 2.696970}, {0.20, 0.40, 0.15}, {0.185319, 0.145737, -0.971812, 0.203113, 0.961905, 0.182983, 0.961458, -0.231298, 0.148658}}, + {{1.575758, 1.212121, 2.717172}, {0.20, 0.40, 0.15}, {0.172090, 0.137556, -0.975430, 0.207866, 0.962835, 0.172453, 0.962900, -0.232436, 0.137101}}, + {{1.606061, 1.196970, 2.737374}, {0.20, 0.40, 0.15}, {0.158960, 0.129141, -0.978802, 0.212480, 0.963701, 0.161656, 0.964149, -0.233673, 0.125750}}, + {{1.636364, 1.181818, 2.757576}, {0.20, 0.40, 0.15}, {0.145935, 0.120494, -0.981929, 0.216953, 0.964495, 0.150599, 0.965212, -0.235010, 0.114612}}, + {{1.666667, 1.166667, 2.777778}, {0.20, 0.40, 0.15}, {0.133022, 0.111619, -0.984808, 0.221281, 0.965212, 0.139287, 0.966095, -0.236447, 0.103696}}, + {{1.696970, 1.151515, 2.797980}, {0.20, 0.40, 0.15}, {0.120227, 0.102518, -0.987439, 0.225462, 0.965843, 0.127728, 0.966805, -0.237986, 0.093006}}, + {{1.727273, 1.136364, 2.818182}, {0.20, 0.40, 0.15}, {0.107554, 0.093196, -0.989821, 0.229493, 0.966382, 0.115926, 0.967350, -0.239625, 0.082551}}, + {{1.757576, 1.121212, 2.838384}, {0.20, 0.40, 0.15}, {0.095012, 0.083657, -0.991955, 0.233373, 0.966822, 0.103890, 0.967734, -0.241366, 0.072336}}, + {{1.787879, 1.106061, 2.858586}, {0.20, 0.40, 0.15}, {0.082604, 0.073903, -0.993838, 0.237099, 0.967155, 0.091626, 0.967967, -0.243207, 0.062368}}, + {{1.818182, 1.090909, 2.878788}, {0.20, 0.40, 0.15}, {0.070337, 0.063940, -0.995472, 0.240672, 0.967375, 0.079141, 0.968055, -0.245148, 0.052653}}, + {{1.848485, 1.075758, 2.898990}, {0.20, 0.40, 0.15}, {0.058216, 0.053772, -0.996855, 0.244088, 0.967474, 0.066442, 0.968004, -0.247188, 0.043198}}, + {{1.878788, 1.060606, 2.919192}, {0.20, 0.40, 0.15}, {0.046248, 0.043402, -0.997987, 0.247348, 0.967447, 0.053536, 0.967822, -0.249326, 0.034007}}, + {{1.909091, 1.045455, 2.939394}, {0.20, 0.40, 0.15}, {0.034437, 0.032835, -0.998867, 0.250451, 0.967285, 0.040432, 0.967517, -0.251560, 0.025086}}, + {{1.939394, 1.030303, 2.959596}, {0.20, 0.40, 0.15}, {0.022788, 0.022076, -0.999497, 0.253398, 0.966982, 0.027135, 0.967094, -0.253888, 0.016442}}, + {{1.969697, 1.015152, 2.979798}, {0.20, 0.40, 0.15}, {0.011308, 0.011130, -0.999874, 0.256187, 0.966531, 0.013656, 0.966561, -0.256309, 0.008078}}, + {{2.000000, 1.000000, 3.000000}, {0.20, 0.40, 0.15}, {0.000000, 0.000000, -1.000000, 0.258819, 0.965926, 0.000000, 0.965926, -0.258819, 0.000000}}, + }}; + return box; +} + +inline host_fn DynamicSphere get_dynamic_sphere_1() { + DynamicSphere sphere = { + 100, 0, 1, { + {{0.500000, -1.000000, 2.000000}, 0.25}, + {{0.510101, -0.959596, 1.984848}, 0.25}, + {{0.520202, -0.919192, 1.969697}, 0.25}, + {{0.530303, -0.878788, 1.954545}, 0.25}, + {{0.540404, -0.838384, 1.939394}, 0.25}, + {{0.550505, -0.797980, 1.924242}, 0.25}, + {{0.560606, -0.757576, 1.909091}, 0.25}, + {{0.570707, -0.717172, 1.893939}, 0.25}, + {{0.580808, -0.676768, 1.878788}, 0.25}, + {{0.590909, -0.636364, 1.863636}, 0.25}, + {{0.601010, -0.595960, 1.848485}, 0.25}, + {{0.611111, -0.555556, 1.833333}, 0.25}, + {{0.621212, -0.515152, 1.818182}, 0.25}, + {{0.631313, -0.474747, 1.803030}, 0.25}, + {{0.641414, -0.434343, 1.787879}, 0.25}, + {{0.651515, -0.393939, 1.772727}, 0.25}, + {{0.661616, -0.353535, 1.757576}, 0.25}, + {{0.671717, -0.313131, 1.742424}, 0.25}, + {{0.681818, -0.272727, 1.727273}, 0.25}, + {{0.691919, -0.232323, 1.712121}, 0.25}, + {{0.702020, -0.191919, 1.696970}, 0.25}, + {{0.712121, -0.151515, 1.681818}, 0.25}, + {{0.722222, -0.111111, 1.666667}, 0.25}, + {{0.732323, -0.070707, 1.651515}, 0.25}, + {{0.742424, -0.030303, 1.636364}, 0.25}, + {{0.752525, 0.010101, 1.621212}, 0.25}, + {{0.762626, 0.050505, 1.606061}, 0.25}, + {{0.772727, 0.090909, 1.590909}, 0.25}, + {{0.782828, 0.131313, 1.575758}, 0.25}, + {{0.792929, 0.171717, 1.560606}, 0.25}, + {{0.803030, 0.212121, 1.545455}, 0.25}, + {{0.813131, 0.252525, 1.530303}, 0.25}, + {{0.823232, 0.292929, 1.515152}, 0.25}, + {{0.833333, 0.333333, 1.500000}, 0.25}, + {{0.843434, 0.373737, 1.484848}, 0.25}, + {{0.853535, 0.414141, 1.469697}, 0.25}, + {{0.863636, 0.454545, 1.454545}, 0.25}, + {{0.873737, 0.494949, 1.439394}, 0.25}, + {{0.883838, 0.535354, 1.424242}, 0.25}, + {{0.893939, 0.575758, 1.409091}, 0.25}, + {{0.904040, 0.616162, 1.393939}, 0.25}, + {{0.914141, 0.656566, 1.378788}, 0.25}, + {{0.924242, 0.696970, 1.363636}, 0.25}, + {{0.934343, 0.737374, 1.348485}, 0.25}, + {{0.944444, 0.777778, 1.333333}, 0.25}, + {{0.954545, 0.818182, 1.318182}, 0.25}, + {{0.964646, 0.858586, 1.303030}, 0.25}, + {{0.974747, 0.898990, 1.287879}, 0.25}, + {{0.984848, 0.939394, 1.272727}, 0.25}, + {{0.994949, 0.979798, 1.257576}, 0.25}, + {{1.005051, 1.020202, 1.242424}, 0.25}, + {{1.015152, 1.060606, 1.227273}, 0.25}, + {{1.025253, 1.101010, 1.212121}, 0.25}, + {{1.035354, 1.141414, 1.196970}, 0.25}, + {{1.045455, 1.181818, 1.181818}, 0.25}, + {{1.055556, 1.222222, 1.166667}, 0.25}, + {{1.065657, 1.262626, 1.151515}, 0.25}, + {{1.075758, 1.303030, 1.136364}, 0.25}, + {{1.085859, 1.343434, 1.121212}, 0.25}, + {{1.095960, 1.383838, 1.106061}, 0.25}, + {{1.106061, 1.424242, 1.090909}, 0.25}, + {{1.116162, 1.464646, 1.075758}, 0.25}, + {{1.126263, 1.505051, 1.060606}, 0.25}, + {{1.136364, 1.545455, 1.045455}, 0.25}, + {{1.146465, 1.585859, 1.030303}, 0.25}, + {{1.156566, 1.626263, 1.015152}, 0.25}, + {{1.166667, 1.666667, 1.000000}, 0.25}, + {{1.176768, 1.707071, 0.984848}, 0.25}, + {{1.186869, 1.747475, 0.969697}, 0.25}, + {{1.196970, 1.787879, 0.954545}, 0.25}, + {{1.207071, 1.828283, 0.939394}, 0.25}, + {{1.217172, 1.868687, 0.924242}, 0.25}, + {{1.227273, 1.909091, 0.909091}, 0.25}, + {{1.237374, 1.949495, 0.893939}, 0.25}, + {{1.247475, 1.989899, 0.878788}, 0.25}, + {{1.257576, 2.030303, 0.863636}, 0.25}, + {{1.267677, 2.070707, 0.848485}, 0.25}, + {{1.277778, 2.111111, 0.833333}, 0.25}, + {{1.287879, 2.151515, 0.818182}, 0.25}, + {{1.297980, 2.191919, 0.803030}, 0.25}, + {{1.308081, 2.232323, 0.787879}, 0.25}, + {{1.318182, 2.272727, 0.772727}, 0.25}, + {{1.328283, 2.313131, 0.757576}, 0.25}, + {{1.338384, 2.353535, 0.742424}, 0.25}, + {{1.348485, 2.393939, 0.727273}, 0.25}, + {{1.358586, 2.434343, 0.712121}, 0.25}, + {{1.368687, 2.474747, 0.696970}, 0.25}, + {{1.378788, 2.515152, 0.681818}, 0.25}, + {{1.388889, 2.555556, 0.666667}, 0.25}, + {{1.398990, 2.595960, 0.651515}, 0.25}, + {{1.409091, 2.636364, 0.636364}, 0.25}, + {{1.419192, 2.676768, 0.621212}, 0.25}, + {{1.429293, 2.717172, 0.606061}, 0.25}, + {{1.439394, 2.757576, 0.590909}, 0.25}, + {{1.449495, 2.797980, 0.575758}, 0.25}, + {{1.459596, 2.838384, 0.560606}, 0.25}, + {{1.469697, 2.878788, 0.545455}, 0.25}, + {{1.479798, 2.919192, 0.530303}, 0.25}, + {{1.489899, 2.959596, 0.515152}, 0.25}, + {{1.500000, 3.000000, 0.500000}, 0.25}, + }}; + return sphere; +} + +inline host_fn DynamicCapsule get_dynamic_capsule() { + DynamicCapsule capsule = { + 100, 0, 1, { + {{0.000000, 0.000000, 1.500000}, {0.367423, 0.367423, 1.800000}, 0.08}, + {{0.012121, -0.015152, 1.507071}, {0.376313, 0.350972, 1.812550}, 0.08}, + {{0.024242, -0.030303, 1.514141}, {0.385172, 0.334465, 1.825066}, 0.08}, + {{0.036364, -0.045455, 1.521212}, {0.394000, 0.317902, 1.837547}, 0.08}, + {{0.048485, -0.060606, 1.528283}, {0.402799, 0.301284, 1.849993}, 0.08}, + {{0.060606, -0.075758, 1.535354}, {0.411568, 0.284610, 1.862403}, 0.08}, + {{0.072727, -0.090909, 1.542424}, {0.420308, 0.267880, 1.874776}, 0.08}, + {{0.084848, -0.106061, 1.549495}, {0.429020, 0.251095, 1.887112}, 0.08}, + {{0.096970, -0.121212, 1.556566}, {0.437706, 0.234254, 1.899411}, 0.08}, + {{0.109091, -0.136364, 1.563636}, {0.446364, 0.217358, 1.911671}, 0.08}, + {{0.121212, -0.151515, 1.570707}, {0.454996, 0.200406, 1.923891}, 0.08}, + {{0.133333, -0.166667, 1.577778}, {0.463604, 0.183399, 1.936073}, 0.08}, + {{0.145455, -0.181818, 1.584848}, {0.472186, 0.166337, 1.948214}, 0.08}, + {{0.157576, -0.196970, 1.591919}, {0.480744, 0.149219, 1.960315}, 0.08}, + {{0.169697, -0.212121, 1.598990}, {0.489279, 0.132046, 1.972374}, 0.08}, + {{0.181818, -0.227273, 1.606061}, {0.497792, 0.114818, 1.984392}, 0.08}, + {{0.193939, -0.242424, 1.613131}, {0.506282, 0.097535, 1.996368}, 0.08}, + {{0.206061, -0.257576, 1.620202}, {0.514751, 0.080196, 2.008300}, 0.08}, + {{0.218182, -0.272727, 1.627273}, {0.523199, 0.062803, 2.020189}, 0.08}, + {{0.230303, -0.287879, 1.634343}, {0.531628, 0.045356, 2.032034}, 0.08}, + {{0.242424, -0.303030, 1.641414}, {0.540037, 0.027854, 2.043835}, 0.08}, + {{0.254545, -0.318182, 1.648485}, {0.548427, 0.010297, 2.055590}, 0.08}, + {{0.266667, -0.333333, 1.655556}, {0.556800, -0.007314, 2.067301}, 0.08}, + {{0.278788, -0.348485, 1.662626}, {0.565155, -0.024978, 2.078965}, 0.08}, + {{0.290909, -0.363636, 1.669697}, {0.573493, -0.042697, 2.090582}, 0.08}, + {{0.303030, -0.378788, 1.676768}, {0.581816, -0.060470, 2.102152}, 0.08}, + {{0.315152, -0.393939, 1.683838}, {0.590123, -0.078295, 2.113675}, 0.08}, + {{0.327273, -0.409091, 1.690909}, {0.598416, -0.096175, 2.125150}, 0.08}, + {{0.339394, -0.424242, 1.697980}, {0.606695, -0.114107, 2.136576}, 0.08}, + {{0.351515, -0.439394, 1.705051}, {0.614961, -0.132092, 2.147952}, 0.08}, + {{0.363636, -0.454545, 1.712121}, {0.623214, -0.150130, 2.159280}, 0.08}, + {{0.375758, -0.469697, 1.719192}, {0.631455, -0.168221, 2.170557}, 0.08}, + {{0.387879, -0.484848, 1.726263}, {0.639686, -0.186363, 2.181784}, 0.08}, + {{0.400000, -0.500000, 1.733333}, {0.647906, -0.204558, 2.192960}, 0.08}, + {{0.412121, -0.515152, 1.740404}, {0.656116, -0.222804, 2.204084}, 0.08}, + {{0.424242, -0.530303, 1.747475}, {0.664317, -0.241101, 2.215157}, 0.08}, + {{0.436364, -0.545455, 1.754545}, {0.672509, -0.259450, 2.226177}, 0.08}, + {{0.448485, -0.560606, 1.761616}, {0.680694, -0.277849, 2.237145}, 0.08}, + {{0.460606, -0.575758, 1.768687}, {0.688872, -0.296299, 2.248059}, 0.08}, + {{0.472727, -0.590909, 1.775758}, {0.697043, -0.314799, 2.258920}, 0.08}, + {{0.484848, -0.606061, 1.782828}, {0.705209, -0.333349, 2.269726}, 0.08}, + {{0.496970, -0.621212, 1.789899}, {0.713369, -0.351948, 2.280478}, 0.08}, + {{0.509091, -0.636364, 1.796970}, {0.721526, -0.370597, 2.291176}, 0.08}, + {{0.521212, -0.651515, 1.804040}, {0.729679, -0.389295, 2.301818}, 0.08}, + {{0.533333, -0.666667, 1.811111}, {0.737828, -0.408041, 2.312404}, 0.08}, + {{0.545455, -0.681818, 1.818182}, {0.745976, -0.426835, 2.322934}, 0.08}, + {{0.557576, -0.696970, 1.825253}, {0.754122, -0.445677, 2.333408}, 0.08}, + {{0.569697, -0.712121, 1.832323}, {0.762266, -0.464566, 2.343824}, 0.08}, + {{0.581818, -0.727273, 1.839394}, {0.770411, -0.483502, 2.354184}, 0.08}, + {{0.593939, -0.742424, 1.846465}, {0.778556, -0.502485, 2.364486}, 0.08}, + {{0.606061, -0.757576, 1.853535}, {0.786702, -0.521514, 2.374730}, 0.08}, + {{0.618182, -0.772727, 1.860606}, {0.794849, -0.540589, 2.384916}, 0.08}, + {{0.630303, -0.787879, 1.867677}, {0.802999, -0.559709, 2.395043}, 0.08}, + {{0.642424, -0.803030, 1.874747}, {0.811152, -0.578875, 2.405111}, 0.08}, + {{0.654545, -0.818182, 1.881818}, {0.819308, -0.598084, 2.415119}, 0.08}, + {{0.666667, -0.833333, 1.888889}, {0.827469, -0.617338, 2.425068}, 0.08}, + {{0.678788, -0.848485, 1.895960}, {0.835635, -0.636635, 2.434958}, 0.08}, + {{0.690909, -0.863636, 1.903030}, {0.843806, -0.655976, 2.444786}, 0.08}, + {{0.703030, -0.878788, 1.910101}, {0.851984, -0.675359, 2.454554}, 0.08}, + {{0.715152, -0.893939, 1.917172}, {0.860168, -0.694784, 2.464262}, 0.08}, + {{0.727273, -0.909091, 1.924242}, {0.868360, -0.714251, 2.473907}, 0.08}, + {{0.739394, -0.924242, 1.931313}, {0.876560, -0.733759, 2.483492}, 0.08}, + {{0.751515, -0.939394, 1.938384}, {0.884769, -0.753308, 2.493015}, 0.08}, + {{0.763636, -0.954545, 1.945455}, {0.892988, -0.772897, 2.502475}, 0.08}, + {{0.775758, -0.969697, 1.952525}, {0.901216, -0.792526, 2.511874}, 0.08}, + {{0.787879, -0.984848, 1.959596}, {0.909455, -0.812194, 2.521209}, 0.08}, + {{0.800000, -1.000000, 1.966667}, {0.917705, -0.831900, 2.530482}, 0.08}, + {{0.812121, -1.015152, 1.973737}, {0.925967, -0.851645, 2.539692}, 0.08}, + {{0.824242, -1.030303, 1.980808}, {0.934241, -0.871427, 2.548839}, 0.08}, + {{0.836364, -1.045455, 1.987879}, {0.942529, -0.891246, 2.557921}, 0.08}, + {{0.848485, -1.060606, 1.994949}, {0.950830, -0.911101, 2.566941}, 0.08}, + {{0.860606, -1.075758, 2.002020}, {0.959145, -0.930993, 2.575896}, 0.08}, + {{0.872727, -1.090909, 2.009091}, {0.967475, -0.950919, 2.584787}, 0.08}, + {{0.884848, -1.106061, 2.016162}, {0.975821, -0.970880, 2.593613}, 0.08}, + {{0.896970, -1.121212, 2.023232}, {0.984182, -0.990876, 2.602375}, 0.08}, + {{0.909091, -1.136364, 2.030303}, {0.992560, -1.010905, 2.611072}, 0.08}, + {{0.921212, -1.151515, 2.037374}, {1.000956, -1.030967, 2.619704}, 0.08}, + {{0.933333, -1.166667, 2.044444}, {1.009369, -1.051061, 2.628271}, 0.08}, + {{0.945455, -1.181818, 2.051515}, {1.017800, -1.071186, 2.636773}, 0.08}, + {{0.957576, -1.196970, 2.058586}, {1.026250, -1.091343, 2.645209}, 0.08}, + {{0.969697, -1.212121, 2.065657}, {1.034719, -1.111531, 2.653580}, 0.08}, + {{0.981818, -1.227273, 2.072727}, {1.043208, -1.131748, 2.661884}, 0.08}, + {{0.993939, -1.242424, 2.079798}, {1.051718, -1.151994, 2.670123}, 0.08}, + {{1.006061, -1.257576, 2.086869}, {1.060249, -1.172269, 2.678296}, 0.08}, + {{1.018182, -1.272727, 2.093939}, {1.068801, -1.192572, 2.686403}, 0.08}, + {{1.030303, -1.287879, 2.101010}, {1.077375, -1.212902, 2.694443}, 0.08}, + {{1.042424, -1.303030, 2.108081}, {1.085971, -1.233258, 2.702417}, 0.08}, + {{1.054545, -1.318182, 2.115152}, {1.094591, -1.253641, 2.710324}, 0.08}, + {{1.066667, -1.333333, 2.122222}, {1.103234, -1.274048, 2.718165}, 0.08}, + {{1.078788, -1.348485, 2.129293}, {1.111902, -1.294480, 2.725939}, 0.08}, + {{1.090909, -1.363636, 2.136364}, {1.120593, -1.314936, 2.733647}, 0.08}, + {{1.103030, -1.378788, 2.143434}, {1.129310, -1.335416, 2.741287}, 0.08}, + {{1.115152, -1.393939, 2.150505}, {1.138052, -1.355917, 2.748861}, 0.08}, + {{1.127273, -1.409091, 2.157576}, {1.146820, -1.376441, 2.756368}, 0.08}, + {{1.139394, -1.424242, 2.164646}, {1.155615, -1.396986, 2.763807}, 0.08}, + {{1.151515, -1.439394, 2.171717}, {1.164436, -1.417551, 2.771180}, 0.08}, + {{1.163636, -1.454545, 2.178788}, {1.173285, -1.438135, 2.778486}, 0.08}, + {{1.175758, -1.469697, 2.185859}, {1.182162, -1.458739, 2.785724}, 0.08}, + {{1.187879, -1.484848, 2.192929}, {1.191067, -1.479361, 2.792896}, 0.08}, + {{1.200000, -1.500000, 2.200000}, {1.200000, -1.500000, 2.800000}, 0.08}, + }}; + return capsule; +} + +inline host_fn DynamicSphere get_dynamic_sphere_2() { + DynamicSphere sphere = { + 100, 0, 1, { + {{2.000000, 0.000000, 1.200000}, 0.18}, + {{1.974747, 0.005051, 1.206061}, 0.18}, + {{1.949495, 0.010101, 1.212121}, 0.18}, + {{1.924242, 0.015152, 1.218182}, 0.18}, + {{1.898990, 0.020202, 1.224242}, 0.18}, + {{1.873737, 0.025253, 1.230303}, 0.18}, + {{1.848485, 0.030303, 1.236364}, 0.18}, + {{1.823232, 0.035354, 1.242424}, 0.18}, + {{1.797980, 0.040404, 1.248485}, 0.18}, + {{1.772727, 0.045455, 1.254545}, 0.18}, + {{1.747475, 0.050505, 1.260606}, 0.18}, + {{1.722222, 0.055556, 1.266667}, 0.18}, + {{1.696970, 0.060606, 1.272727}, 0.18}, + {{1.671717, 0.065657, 1.278788}, 0.18}, + {{1.646465, 0.070707, 1.284848}, 0.18}, + {{1.621212, 0.075758, 1.290909}, 0.18}, + {{1.595960, 0.080808, 1.296970}, 0.18}, + {{1.570707, 0.085859, 1.303030}, 0.18}, + {{1.545455, 0.090909, 1.309091}, 0.18}, + {{1.520202, 0.095960, 1.315152}, 0.18}, + {{1.494949, 0.101010, 1.321212}, 0.18}, + {{1.469697, 0.106061, 1.327273}, 0.18}, + {{1.444444, 0.111111, 1.333333}, 0.18}, + {{1.419192, 0.116162, 1.339394}, 0.18}, + {{1.393939, 0.121212, 1.345455}, 0.18}, + {{1.368687, 0.126263, 1.351515}, 0.18}, + {{1.343434, 0.131313, 1.357576}, 0.18}, + {{1.318182, 0.136364, 1.363636}, 0.18}, + {{1.292929, 0.141414, 1.369697}, 0.18}, + {{1.267677, 0.146465, 1.375758}, 0.18}, + {{1.242424, 0.151515, 1.381818}, 0.18}, + {{1.217172, 0.156566, 1.387879}, 0.18}, + {{1.191919, 0.161616, 1.393939}, 0.18}, + {{1.166667, 0.166667, 1.400000}, 0.18}, + {{1.141414, 0.171717, 1.406061}, 0.18}, + {{1.116162, 0.176768, 1.412121}, 0.18}, + {{1.090909, 0.181818, 1.418182}, 0.18}, + {{1.065657, 0.186869, 1.424242}, 0.18}, + {{1.040404, 0.191919, 1.430303}, 0.18}, + {{1.015152, 0.196970, 1.436364}, 0.18}, + {{0.989899, 0.202020, 1.442424}, 0.18}, + {{0.964646, 0.207071, 1.448485}, 0.18}, + {{0.939394, 0.212121, 1.454545}, 0.18}, + {{0.914141, 0.217172, 1.460606}, 0.18}, + {{0.888889, 0.222222, 1.466667}, 0.18}, + {{0.863636, 0.227273, 1.472727}, 0.18}, + {{0.838384, 0.232323, 1.478788}, 0.18}, + {{0.813131, 0.237374, 1.484848}, 0.18}, + {{0.787879, 0.242424, 1.490909}, 0.18}, + {{0.762626, 0.247475, 1.496970}, 0.18}, + {{0.737374, 0.252525, 1.503030}, 0.18}, + {{0.712121, 0.257576, 1.509091}, 0.18}, + {{0.686869, 0.262626, 1.515152}, 0.18}, + {{0.661616, 0.267677, 1.521212}, 0.18}, + {{0.636364, 0.272727, 1.527273}, 0.18}, + {{0.611111, 0.277778, 1.533333}, 0.18}, + {{0.585859, 0.282828, 1.539394}, 0.18}, + {{0.560606, 0.287879, 1.545455}, 0.18}, + {{0.535354, 0.292929, 1.551515}, 0.18}, + {{0.510101, 0.297980, 1.557576}, 0.18}, + {{0.484848, 0.303030, 1.563636}, 0.18}, + {{0.459596, 0.308081, 1.569697}, 0.18}, + {{0.434343, 0.313131, 1.575758}, 0.18}, + {{0.409091, 0.318182, 1.581818}, 0.18}, + {{0.383838, 0.323232, 1.587879}, 0.18}, + {{0.358586, 0.328283, 1.593939}, 0.18}, + {{0.333333, 0.333333, 1.600000}, 0.18}, + {{0.308081, 0.338384, 1.606061}, 0.18}, + {{0.282828, 0.343434, 1.612121}, 0.18}, + {{0.257576, 0.348485, 1.618182}, 0.18}, + {{0.232323, 0.353535, 1.624242}, 0.18}, + {{0.207071, 0.358586, 1.630303}, 0.18}, + {{0.181818, 0.363636, 1.636364}, 0.18}, + {{0.156566, 0.368687, 1.642424}, 0.18}, + {{0.131313, 0.373737, 1.648485}, 0.18}, + {{0.106061, 0.378788, 1.654545}, 0.18}, + {{0.080808, 0.383838, 1.660606}, 0.18}, + {{0.055556, 0.388889, 1.666667}, 0.18}, + {{0.030303, 0.393939, 1.672727}, 0.18}, + {{0.005051, 0.398990, 1.678788}, 0.18}, + {{-0.020202, 0.404040, 1.684848}, 0.18}, + {{-0.045455, 0.409091, 1.690909}, 0.18}, + {{-0.070707, 0.414141, 1.696970}, 0.18}, + {{-0.095960, 0.419192, 1.703030}, 0.18}, + {{-0.121212, 0.424242, 1.709091}, 0.18}, + {{-0.146465, 0.429293, 1.715152}, 0.18}, + {{-0.171717, 0.434343, 1.721212}, 0.18}, + {{-0.196970, 0.439394, 1.727273}, 0.18}, + {{-0.222222, 0.444444, 1.733333}, 0.18}, + {{-0.247475, 0.449495, 1.739394}, 0.18}, + {{-0.272727, 0.454545, 1.745455}, 0.18}, + {{-0.297980, 0.459596, 1.751515}, 0.18}, + {{-0.323232, 0.464646, 1.757576}, 0.18}, + {{-0.348485, 0.469697, 1.763636}, 0.18}, + {{-0.373737, 0.474747, 1.769697}, 0.18}, + {{-0.398990, 0.479798, 1.775758}, 0.18}, + {{-0.424242, 0.484848, 1.781818}, 0.18}, + {{-0.449495, 0.489899, 1.787879}, 0.18}, + {{-0.474747, 0.494949, 1.793939}, 0.18}, + {{-0.500000, 0.500000, 1.800000}, 0.18}, + }}; + return sphere; +} + +inline host_fn DynamicBox get_dynamic_box_2() { + DynamicBox box = { + 100, 0, 1, { + {{0.800000, -2.000000, 0.000000}, {0.30, 0.30, 0.30}, {0.750000, 0.433013, -0.500000, -0.216506, 0.875000, 0.433013, 0.625000, -0.216506, 0.750000}}, + {{0.800000, -1.959596, 0.015152}, {0.30, 0.30, 0.30}, {0.738400, 0.447405, -0.504573, -0.216198, 0.865785, 0.451304, 0.638767, -0.224155, 0.736024}}, + {{0.800000, -1.919192, 0.030303}, {0.30, 0.30, 0.30}, {0.726508, 0.461487, -0.509132, -0.215273, 0.856481, 0.469146, 0.652567, -0.231236, 0.721586}}, + {{0.800000, -1.878788, 0.045455}, {0.30, 0.30, 0.30}, {0.714331, 0.475254, -0.513677, -0.213735, 0.847117, 0.486528, 0.666370, -0.237751, 0.706701}}, + {{0.800000, -1.838384, 0.060606}, {0.30, 0.30, 0.30}, {0.701878, 0.488700, -0.518208, -0.211589, 0.837723, 0.503438, 0.680145, -0.243705, 0.691383}}, + {{0.800000, -1.797980, 0.075758}, {0.30, 0.30, 0.30}, {0.689158, 0.501817, -0.522724, -0.208840, 0.828328, 0.519864, 0.693864, -0.249103, 0.675648}}, + {{0.800000, -1.757576, 0.090909}, {0.30, 0.30, 0.30}, {0.676179, 0.514602, -0.527225, -0.205496, 0.818959, 0.535797, 0.707498, -0.253952, 0.659511}}, + {{0.800000, -1.717172, 0.106061}, {0.30, 0.30, 0.30}, {0.662950, 0.527048, -0.531712, -0.201565, 0.809643, 0.551225, 0.721019, -0.258260, 0.642988}}, + {{0.800000, -1.676768, 0.121212}, {0.30, 0.30, 0.30}, {0.649480, 0.539150, -0.536184, -0.197058, 0.800409, 0.566140, 0.734401, -0.262037, 0.626093}}, + {{0.800000, -1.636364, 0.136364}, {0.30, 0.30, 0.30}, {0.635777, 0.550904, -0.540641, -0.191986, 0.791280, 0.580531, 0.747615, -0.265293, 0.608844}}, + {{0.800000, -1.595960, 0.151515}, {0.30, 0.30, 0.30}, {0.621851, 0.562304, -0.545083, -0.186361, 0.782284, 0.594391, 0.760638, -0.268041, 0.591256}}, + {{0.800000, -1.555556, 0.166667}, {0.30, 0.30, 0.30}, {0.607712, 0.573347, -0.549509, -0.180198, 0.773444, 0.607712, 0.773444, -0.270293, 0.573347}}, + {{0.800000, -1.515152, 0.181818}, {0.30, 0.30, 0.30}, {0.593368, 0.584027, -0.553920, -0.173509, 0.764783, 0.620485, 0.786008, -0.272065, 0.555132}}, + {{0.800000, -1.474747, 0.196970}, {0.30, 0.30, 0.30}, {0.578828, 0.594341, -0.558316, -0.166313, 0.756325, 0.632703, 0.798309, -0.273371, 0.536629}}, + {{0.800000, -1.434343, 0.212121}, {0.30, 0.30, 0.30}, {0.564103, 0.604286, -0.562696, -0.158625, 0.748089, 0.644360, 0.810324, -0.274228, 0.517855}}, + {{0.800000, -1.393939, 0.227273}, {0.30, 0.30, 0.30}, {0.549202, 0.613857, -0.567060, -0.150463, 0.740098, 0.655450, 0.822033, -0.274653, 0.498827}}, + {{0.800000, -1.353535, 0.242424}, {0.30, 0.30, 0.30}, {0.534134, 0.623052, -0.571408, -0.141847, 0.732370, 0.665968, 0.833415, -0.274664, 0.479562}}, + {{0.800000, -1.313131, 0.257576}, {0.30, 0.30, 0.30}, {0.518909, 0.631867, -0.575741, -0.132794, 0.724924, 0.675907, 0.844452, -0.274280, 0.460078}}, + {{0.800000, -1.272727, 0.272727}, {0.30, 0.30, 0.30}, {0.503537, 0.640300, -0.580057, -0.123326, 0.717776, 0.685265, 0.855126, -0.273520, 0.440393}}, + {{0.800000, -1.232323, 0.287879}, {0.30, 0.30, 0.30}, {0.488028, 0.648348, -0.584357, -0.113464, 0.710943, 0.694036, 0.865422, -0.272406, 0.420524}}, + {{0.800000, -1.191919, 0.303030}, {0.30, 0.30, 0.30}, {0.472391, 0.656010, -0.588641, -0.103228, 0.704438, 0.702218, 0.875323, -0.270957, 0.400489}}, + {{0.800000, -1.151515, 0.318182}, {0.30, 0.30, 0.30}, {0.456636, 0.663282, -0.592908, -0.092641, 0.698276, 0.709808, 0.884817, -0.269196, 0.380306}}, + {{0.800000, -1.111111, 0.333333}, {0.30, 0.30, 0.30}, {0.440774, 0.670164, -0.597159, -0.081726, 0.692469, 0.716803, 0.893890, -0.267145, 0.359992}}, + {{0.800000, -1.070707, 0.348485}, {0.30, 0.30, 0.30}, {0.424813, 0.676654, -0.601393, -0.070505, 0.687028, 0.723203, 0.902531, -0.264825, 0.339566}}, + {{0.800000, -1.030303, 0.363636}, {0.30, 0.30, 0.30}, {0.408765, 0.682750, -0.605610, -0.059001, 0.681962, 0.729004, 0.910731, -0.262259, 0.319045}}, + {{0.800000, -0.989899, 0.378788}, {0.30, 0.30, 0.30}, {0.392638, 0.688453, -0.609810, -0.047239, 0.677279, 0.734208, 0.918479, -0.259471, 0.298448}}, + {{0.800000, -0.949495, 0.393939}, {0.30, 0.30, 0.30}, {0.376443, 0.693760, -0.613993, -0.035241, 0.672988, 0.738813, 0.925769, -0.256484, 0.277791}}, + {{0.800000, -0.909091, 0.409091}, {0.30, 0.30, 0.30}, {0.360190, 0.698672, -0.618159, -0.023032, 0.669094, 0.742821, 0.932594, -0.253320, 0.257093}}, + {{0.800000, -0.868687, 0.424242}, {0.30, 0.30, 0.30}, {0.343889, 0.703188, -0.622308, -0.010635, 0.665601, 0.746232, 0.938950, -0.250003, 0.236371}}, + {{0.800000, -0.828283, 0.439394}, {0.30, 0.30, 0.30}, {0.327549, 0.707309, -0.626439, 0.001926, 0.662514, 0.749047, 0.944832, -0.246556, 0.215644}}, + {{0.800000, -0.787879, 0.454545}, {0.30, 0.30, 0.30}, {0.311181, 0.711034, -0.630553, 0.014626, 0.659833, 0.751270, 0.950238, -0.243003, 0.194928}}, + {{0.800000, -0.747475, 0.469697}, {0.30, 0.30, 0.30}, {0.294794, 0.714365, -0.634649, 0.027443, 0.657560, 0.752902, 0.955167, -0.239367, 0.174241}}, + {{0.800000, -0.707071, 0.484848}, {0.30, 0.30, 0.30}, {0.278398, 0.717302, -0.638727, 0.040352, 0.655696, 0.753946, 0.959618, -0.235671, 0.153600}}, + {{0.800000, -0.666667, 0.500000}, {0.30, 0.30, 0.30}, {0.262003, 0.719846, -0.642788, 0.053330, 0.654237, 0.754407, 0.963592, -0.231937, 0.133022}}, + {{0.800000, -0.626263, 0.515152}, {0.30, 0.30, 0.30}, {0.245618, 0.721999, -0.646830, 0.066355, 0.653183, 0.754287, 0.967093, -0.228187, 0.112525}}, + {{0.800000, -0.585859, 0.530303}, {0.30, 0.30, 0.30}, {0.229254, 0.723762, -0.650855, 0.079404, 0.652529, 0.753592, 0.970123, -0.224444, 0.092125}}, + {{0.800000, -0.545455, 0.545455}, {0.30, 0.30, 0.30}, {0.212919, 0.725136, -0.654861, 0.092455, 0.652270, 0.752327, 0.972686, -0.220730, 0.071839}}, + {{0.800000, -0.505051, 0.560606}, {0.30, 0.30, 0.30}, {0.196624, 0.726125, -0.658849, 0.105485, 0.652400, 0.750498, 0.974788, -0.217065, 0.051682}}, + {{0.800000, -0.464646, 0.575758}, {0.30, 0.30, 0.30}, {0.180378, 0.726730, -0.662818, 0.118474, 0.652913, 0.748110, 0.976436, -0.213469, 0.031672}}, + {{0.800000, -0.424242, 0.590909}, {0.30, 0.30, 0.30}, {0.164190, 0.726953, -0.666769, 0.131402, 0.653800, 0.745171, 0.977638, -0.209964, 0.011824}}, + {{0.800000, -0.383838, 0.606061}, {0.30, 0.30, 0.30}, {0.148069, 0.726798, -0.670701, 0.144247, 0.655053, 0.741686, 0.978401, -0.206567, -0.007846}}, + {{0.800000, -0.343434, 0.621212}, {0.30, 0.30, 0.30}, {0.132025, 0.726267, -0.674615, 0.156992, 0.656662, 0.737664, 0.978735, -0.203299, -0.027322}}, + {{0.800000, -0.303030, 0.636364}, {0.30, 0.30, 0.30}, {0.116067, 0.725364, -0.678509, 0.169617, 0.658617, 0.733113, 0.978651, -0.200176, -0.046591}}, + {{0.800000, -0.262626, 0.651515}, {0.30, 0.30, 0.30}, {0.100203, 0.724093, -0.682385, 0.182104, 0.660905, 0.728040, 0.978160, -0.197217, -0.065636}}, + {{0.800000, -0.222222, 0.666667}, {0.30, 0.30, 0.30}, {0.084443, 0.722455, -0.686242, 0.194437, 0.663515, 0.722455, 0.977274, -0.194437, -0.084443}}, + {{0.800000, -0.181818, 0.681818}, {0.30, 0.30, 0.30}, {0.068795, 0.720457, -0.690079, 0.206599, 0.666434, 0.716367, 0.976004, -0.191852, -0.102998}}, + {{0.800000, -0.141414, 0.696970}, {0.30, 0.30, 0.30}, {0.053269, 0.718101, -0.693897, 0.218574, 0.669648, 0.709786, 0.974365, -0.189478, -0.121287}}, + {{0.800000, -0.101010, 0.712121}, {0.30, 0.30, 0.30}, {0.037872, 0.715392, -0.697696, 0.230349, 0.673144, 0.702721, 0.972371, -0.187327, -0.139297}}, + {{0.800000, -0.060606, 0.727273}, {0.30, 0.30, 0.30}, {0.022612, 0.712335, -0.701475, 0.241909, 0.676905, 0.695183, 0.970035, -0.185413, -0.157014}}, + {{0.800000, -0.020202, 0.742424}, {0.30, 0.30, 0.30}, {0.007499, 0.708935, -0.705234, 0.253242, 0.680917, 0.687183, 0.967374, -0.183748, -0.174425}}, + {{0.800000, 0.020202, 0.757576}, {0.30, 0.30, 0.30}, {-0.007460, 0.705195, -0.708974, 0.264335, 0.685165, 0.678731, 0.964402, -0.182344, -0.191519}}, + {{0.800000, 0.060606, 0.772727}, {0.30, 0.30, 0.30}, {-0.022256, 0.701122, -0.712694, 0.275178, 0.689631, 0.669840, 0.961136, -0.181210, -0.208282}}, + {{0.800000, 0.101010, 0.787879}, {0.30, 0.30, 0.30}, {-0.036883, 0.696720, -0.716394, 0.285761, 0.694300, 0.660521, 0.957591, -0.180356, -0.224704}}, + {{0.800000, 0.141414, 0.803030}, {0.30, 0.30, 0.30}, {-0.051332, 0.691996, -0.720074, 0.296075, 0.699155, 0.650786, 0.953784, -0.179790, -0.240772}}, + {{0.800000, 0.181818, 0.818182}, {0.30, 0.30, 0.30}, {-0.065596, 0.686954, -0.723734, 0.306112, 0.704178, 0.640647, 0.949733, -0.179520, -0.256476}}, + {{0.800000, 0.222222, 0.833333}, {0.30, 0.30, 0.30}, {-0.079668, 0.681602, -0.727374, 0.315865, 0.709352, 0.630118, 0.945454, -0.179552, -0.271806}}, + {{0.800000, 0.262626, 0.848485}, {0.30, 0.30, 0.30}, {-0.093540, 0.675944, -0.730993, 0.325327, 0.714661, 0.619211, 0.940964, -0.179891, -0.286752}}, + {{0.800000, 0.303030, 0.863636}, {0.30, 0.30, 0.30}, {-0.107205, 0.669987, -0.734592, 0.334494, 0.720085, 0.607940, 0.936280, -0.180542, -0.301303}}, + {{0.800000, 0.343434, 0.878788}, {0.30, 0.30, 0.30}, {-0.120658, 0.663737, -0.738170, 0.343361, 0.725609, 0.596318, 0.931421, -0.181508, -0.315452}}, + {{0.800000, 0.383838, 0.893939}, {0.30, 0.30, 0.30}, {-0.133890, 0.657201, -0.741728, 0.351925, 0.731214, 0.584359, 0.926403, -0.182793, -0.329188}}, + {{0.800000, 0.424242, 0.909091}, {0.30, 0.30, 0.30}, {-0.146896, 0.650386, -0.745264, 0.360184, 0.736882, 0.572077, 0.921243, -0.184397, -0.342504}}, + {{0.800000, 0.464646, 0.924242}, {0.30, 0.30, 0.30}, {-0.159670, 0.643299, -0.748780, 0.368137, 0.742598, 0.559485, 0.915959, -0.186321, -0.355392}}, + {{0.800000, 0.505051, 0.939394}, {0.30, 0.30, 0.30}, {-0.172205, 0.635946, -0.752276, 0.375783, 0.748342, 0.546600, 0.910567, -0.188565, -0.367846}}, + {{0.800000, 0.545455, 0.954545}, {0.30, 0.30, 0.30}, {-0.184496, 0.628334, -0.755750, 0.383122, 0.754100, 0.533434, 0.905085, -0.191128, -0.379856}}, + {{0.800000, 0.585859, 0.969697}, {0.30, 0.30, 0.30}, {-0.196536, 0.620472, -0.759202, 0.390156, 0.759853, 0.520003, 0.899529, -0.194008, -0.391419}}, + {{0.800000, 0.626263, 0.984848}, {0.30, 0.30, 0.30}, {-0.208322, 0.612365, -0.762634, 0.396886, 0.765585, 0.506321, 0.893915, -0.197201, -0.402527}}, + {{0.800000, 0.666667, 1.000000}, {0.30, 0.30, 0.30}, {-0.219846, 0.604023, -0.766044, 0.403317, 0.771281, 0.492404, 0.888258, -0.200706, -0.413176}}, + {{0.800000, 0.707071, 1.015152}, {0.30, 0.30, 0.30}, {-0.231105, 0.595452, -0.769433, 0.409452, 0.776924, 0.478266, 0.882576, -0.204516, -0.423360}}, + {{0.800000, 0.747475, 1.030303}, {0.30, 0.30, 0.30}, {-0.242094, 0.586660, -0.772801, 0.415294, 0.782500, 0.463924, 0.876882, -0.208626, -0.433075}}, + {{0.800000, 0.787879, 1.045455}, {0.30, 0.30, 0.30}, {-0.252808, 0.577655, -0.776146, 0.420850, 0.787993, 0.449391, 0.871191, -0.213032, -0.442317}}, + {{0.800000, 0.828283, 1.060606}, {0.30, 0.30, 0.30}, {-0.263242, 0.568445, -0.779471, 0.426126, 0.793389, 0.434684, 0.865517, -0.217725, -0.451083}}, + {{0.800000, 0.868687, 1.075758}, {0.30, 0.30, 0.30}, {-0.273393, 0.559038, -0.782773, 0.431128, 0.798675, 0.419818, 0.859875, -0.222699, -0.459369}}, + {{0.800000, 0.909091, 1.090909}, {0.30, 0.30, 0.30}, {-0.283257, 0.549442, -0.786053, 0.435862, 0.803837, 0.404808, 0.854277, -0.227946, -0.467173}}, + {{0.800000, 0.949495, 1.106061}, {0.30, 0.30, 0.30}, {-0.292829, 0.539665, -0.789311, 0.440339, 0.808863, 0.389670, 0.848736, -0.233457, -0.474494}}, + {{0.800000, 0.989899, 1.121212}, {0.30, 0.30, 0.30}, {-0.302108, 0.529716, -0.792548, 0.444564, 0.813740, 0.374419, 0.843264, -0.239224, -0.481330}}, + {{0.800000, 1.030303, 1.136364}, {0.30, 0.30, 0.30}, {-0.311088, 0.519603, -0.795762, 0.448549, 0.818457, 0.359071, 0.837871, -0.245235, -0.487679}}, + {{0.800000, 1.070707, 1.151515}, {0.30, 0.30, 0.30}, {-0.319768, 0.509334, -0.798954, 0.452301, 0.823004, 0.343641, 0.832570, -0.251482, -0.493542}}, + {{0.800000, 1.111111, 1.166667}, {0.30, 0.30, 0.30}, {-0.328144, 0.498919, -0.802123, 0.455831, 0.827369, 0.328144, 0.827369, -0.257954, -0.498919}}, + {{0.800000, 1.151515, 1.181818}, {0.30, 0.30, 0.30}, {-0.336214, 0.488364, -0.805270, 0.459149, 0.831544, 0.312596, 0.822279, -0.264639, -0.503809}}, + {{0.800000, 1.191919, 1.196970}, {0.30, 0.30, 0.30}, {-0.343976, 0.477680, -0.808395, 0.462265, 0.835521, 0.297012, 0.817307, -0.271527, -0.508214}}, + {{0.800000, 1.232323, 1.212121}, {0.30, 0.30, 0.30}, {-0.351428, 0.466874, -0.811497, 0.465191, 0.839290, 0.281408, 0.812463, -0.278606, -0.512135}}, + {{0.800000, 1.272727, 1.227273}, {0.30, 0.30, 0.30}, {-0.358567, 0.455956, -0.814576, 0.467938, 0.842844, 0.265797, 0.807752, -0.285864, -0.515575}}, + {{0.800000, 1.313131, 1.242424}, {0.30, 0.30, 0.30}, {-0.365393, 0.444933, -0.817632, 0.470516, 0.846177, 0.250196, 0.803183, -0.293289, -0.518535}}, + {{0.800000, 1.353535, 1.257576}, {0.30, 0.30, 0.30}, {-0.371904, 0.433815, -0.820666, 0.472939, 0.849284, 0.234619, 0.798759, -0.300869, -0.521019}}, + {{0.800000, 1.393939, 1.272727}, {0.30, 0.30, 0.30}, {-0.378098, 0.422610, -0.823677, 0.475216, 0.852158, 0.219081, 0.794488, -0.308591, -0.523030}}, + {{0.800000, 1.434343, 1.287879}, {0.30, 0.30, 0.30}, {-0.383975, 0.411327, -0.826664, 0.477361, 0.854795, 0.203596, 0.790373, -0.316442, -0.524571}}, + {{0.800000, 1.474747, 1.303030}, {0.30, 0.30, 0.30}, {-0.389534, 0.399974, -0.829629, 0.479385, 0.857192, 0.188178, 0.786418, -0.324410, -0.525648}}, + {{0.800000, 1.515152, 1.318182}, {0.30, 0.30, 0.30}, {-0.394776, 0.388561, -0.832570, 0.481300, 0.859346, 0.172842, 0.782625, -0.332483, -0.526263}}, + {{0.800000, 1.555556, 1.333333}, {0.30, 0.30, 0.30}, {-0.399698, 0.377096, -0.835488, 0.483119, 0.861254, 0.157601, 0.778998, -0.340647, -0.526424}}, + {{0.800000, 1.595960, 1.348485}, {0.30, 0.30, 0.30}, {-0.404303, 0.365588, -0.838382, 0.484852, 0.862914, 0.142470, 0.775537, -0.348890, -0.526134}}, + {{0.800000, 1.636364, 1.363636}, {0.30, 0.30, 0.30}, {-0.408589, 0.354044, -0.841254, 0.486511, 0.864327, 0.127461, 0.772245, -0.357200, -0.525401}}, + {{0.800000, 1.676768, 1.378788}, {0.30, 0.30, 0.30}, {-0.412558, 0.342475, -0.844101, 0.488109, 0.865490, 0.112588, 0.769120, -0.365564, -0.524230}}, + {{0.800000, 1.717172, 1.393939}, {0.30, 0.30, 0.30}, {-0.416210, 0.330889, -0.846925, 0.489657, 0.866406, 0.097864, 0.766163, -0.373970, -0.522628}}, + {{0.800000, 1.757576, 1.409091}, {0.30, 0.30, 0.30}, {-0.419546, 0.319293, -0.849725, 0.491165, 0.867074, 0.083302, 0.763373, -0.382406, -0.520603}}, + {{0.800000, 1.797980, 1.424242}, {0.30, 0.30, 0.30}, {-0.422567, 0.307697, -0.852502, 0.492646, 0.867497, 0.068914, 0.760747, -0.390861, -0.518161}}, + {{0.800000, 1.838384, 1.439394}, {0.30, 0.30, 0.30}, {-0.425276, 0.296108, -0.855255, 0.494110, 0.867676, 0.054713, 0.758285, -0.399322, -0.515312}}, + {{0.800000, 1.878788, 1.454545}, {0.30, 0.30, 0.30}, {-0.427672, 0.284536, -0.857983, 0.495567, 0.867615, 0.040709, 0.755983, -0.407778, -0.512062}}, + {{0.800000, 1.919192, 1.469697}, {0.30, 0.30, 0.30}, {-0.429759, 0.272989, -0.860688, 0.497028, 0.867317, 0.026915, 0.753837, -0.416219, -0.508421}}, + {{0.800000, 1.959596, 1.484848}, {0.30, 0.30, 0.30}, {-0.431539, 0.261474, -0.863369, 0.498503, 0.866786, 0.013342, 0.751844, -0.424634, -0.504397}}, + {{0.800000, 2.000000, 1.500000}, {0.30, 0.30, 0.30}, {-0.433013, 0.250000, -0.866025, 0.500000, 0.866025, 0.000000, 0.750000, -0.433013, -0.500000}}, + }}; + return box; +} + +inline host_fn DynamicDoor get_dynamic_door() { + DynamicDoor door = { + {0.02, 0.25, 0.15}, + {1.0, -0.5, 1.0}, + {0.0, 0.25, 0.0}, + 0.0, + 1.57, + 0.0, + 1.0, + {0, 0, 1}}; + + return door; +} + +inline host_fn void add_dynamic_obstacles(World& world) { + world.add_dynamic_box(get_dynamic_box_1()); + world.add_dynamic_box(get_dynamic_box_2()); + world.add_dynamic_sphere(get_dynamic_sphere_1()); + world.add_dynamic_sphere(get_dynamic_sphere_2()); + world.add_dynamic_capsule(get_dynamic_capsule()); + world.dynamic_doors.push_back(get_dynamic_door()); +} + +inline host_fn World create_static_world_with_dynamic_object() { + using namespace blast; + + std::vector centers = {{2, 2, 0}, {6, 4, 0}, {5, -2, -1}, {-1, -2, 1}, {4, 0, 2}}; + Sphere sphere; + sphere.radius = 1; + + DynamicSphere spheres; + spheres.start_time = 0.0; + spheres.end_time = 1.0; + for (Vec3 c: centers) { + sphere.center = c; + spheres.trajectory.push_back(sphere); + } + + spheres.n_points = spheres.trajectory.size(); + + World world; + world.dynamic_spheres = {spheres}; + + return world; +} + +std::vector> worlds = { + {get_bookshelf_small(), "get_bookshelf_small()"}, + {get_bookshelf_tall(), "get_bookshelf_tall()"}, + {get_bookshelf_thin(), "get_bookshelf_thin()"}, + {get_scene_box(), "get_scene_box()"}, + {get_scene_cage(), "get_scene_cage()"}, + {get_scene_table(), "get_scene_table()"}, + {get_kitchen_no_doors(), "get_kitchen_no_doors()"}, + {get_lab_world(), "get_lab_world()"}, +}; + +std::vector capsules = { + {{0.42, -0.30, 0.60}, {0.52, -0.20, 0.60}, 0.05}, + {{1.00, -0.30, 1.20}, {1.00, -0.10, 1.30}, 0.04}, + {{0.42, -1.30, 0.00}, {0.42, -1.30, 0.50}, 0.05}, + {{0.42, -0.30, 1.00}, {0.50, -0.30, 1.05}, 0.03}, + {{0.42, 0.00, -0.70}, {0.62, 0.00, -0.70}, 0.08}, + {{0.42, -0.30, 1.56}, {0.52, -0.30, 1.56}, 0.00}, + {{0.42, -0.30, 0.395}, {0.52, -0.20, 0.395}, 0.024}, + {{0.78729, -0.30, 1.20}, {0.78729, -0.10, 1.20}, 0.00}, + {{0.35698, -1.1846, 0.00}, {0.35698, -1.1846, 0.20}, 0.00}, + {{0.10, -0.29838, 1.2074}, {0.10, 0.20, 1.2074}, 0.033}, + {{0.427, -1.0847, 1.1974}, {0.427, -1.0847, 1.1974}, 0.10}, + {{0.42, -0.30, 1.2074}, {0.42, -0.20, 1.2074}, 0.05}, + {{0.35698, 0.425, 0.00}, {0.35698, 0.425, 0.00}, 0.20}, + {{0.42, -1.16, -0.066}, {0.42, -1.14, -0.066}, 0.04}, + {{0.72729, -0.35, 1.2074}, {0.72729, -0.25, 1.2074}, 0.03}, + {{0.427, -0.29838, 0.80}, {0.427, -0.29838, 1.60}, 0.01}, + {{0.427, -1.20, 1.2074}, {0.427, 0.60, 1.2074}, 0.01}, + {{0.35698, -0.80106, -0.055}, {0.35698, -0.80106, -0.055}, 0.00}, + {{0.75, -0.30, 1.50}, {0.71, -0.29, 1.55}, 0.02}, + {{0.427, 0.48791, 1.1974}, {0.427, 0.48791, 1.1974}, 0.001}}; + +TEST_CASE("Benchmark - Broadphase vs Blast (min_dist computation + only static obstacles)", "[World]") { + + // World initialization + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution rand_int(0, worlds.size() - 1); + auto world = worlds[rand_int(gen)].first; + + // std::cout << worlds[rand_int(gen)].second << std::endl; + + const int n_capsules = 20; + const int point_in_segment = 0; + Array max_col_constraints(n_capsules, -INF_REAL); + std::array max_collision_entities{}; + std::array capsule_list{}; + for (int i = 0; i < capsule_list.size(); i++) { + capsule_list[i] = capsules[i]; + } + + BoundingVolumeHierarchy BVH; + BENCHMARK("BVH construction") { + create_static_bounding_volume_hierarchy(world, BVH); + return BVH; + }; + CollisionObjectType collision_object_type; + + real dist_min; + BENCHMARK("Blast - find closest object") { + // check every capsule with world + for (int capsule_id = 0; capsule_id < n_capsules; capsule_id++) { + dist_min = INF_REAL; + const auto capsule = capsule_list[capsule_id]; + + CollisionEntities collision_objects{}; + + // check against boxes + int count = 0; + for (const auto& box: world.boxes) { + if (const auto dist = distance(capsule, box); + dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::box; + collision_objects.box = box; + collision_objects.point_in_segment = point_in_segment; + } + count++; + } + + // check against capsules + count = 0; + for (const auto caps: world.capsules) { + if (const auto dist = distance(capsule, caps); + dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::capsule; + collision_objects.capsule = capsule; + collision_objects.point_in_segment = point_in_segment; + } + count++; + } + + // check against spheres + count = 0; + for (const auto sphere: world.spheres) { + if (const auto dist = distance(capsule, sphere); + dist < dist_min) { + dist_min = dist; + collision_objects.other_object_type = CollisionObjectType::sphere; + collision_objects.sphere = sphere; + collision_objects.point_in_segment = point_in_segment; + } + count++; + } + + // update worst position for the current capsule if necessary + if (dist_min > max_col_constraints[capsule_id]) { + max_col_constraints[capsule_id] = dist_min; + max_collision_entities[capsule_id] = collision_objects; + } + } // end of for-loop + return dist_min; + }; + + // broadphase benchmark + real dist_min_broadphase; + CollisionEntities collision_objects{}; + Array max_col_constraints_broadphase(n_capsules, -INF_REAL); + BENCHMARK("Broad phase - find closest object") { + for (int capsule_id = 0; capsule_id < n_capsules; capsule_id++) { + max_col_constraints_broadphase[capsule_id] = minimum_distance_static(capsule_list[capsule_id], BVH, collision_objects, 1); + } + return max_col_constraints_broadphase[n_capsules - 1]; + }; + for (int i = 0; i < n_capsules; i++) { + CHECK(is_close(max_col_constraints[i], max_col_constraints_broadphase[i], 1e-5)); + } +} + +inline host_fn Manipulator get_generic_Link6() { + // Manipulator + constexpr u32 joints = 6; + // limits + ManipulatorLimits limits; + limits.position_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad + limits.position_min = -limits.position_max; + + limits.velocity_max = {3.4907f, 3.4907f, 3.4907f, 5.5851f, 5.5851f, 5.5851f}; // rad/s + // limits.vmin = -limits.velocity_max; + + limits.acceleration_max = {deg2rad(600), deg2rad(600), deg2rad(600), deg2rad(600), deg2rad(600), deg2rad(600)}; // rad/s^2 + // limits.amin = -limits.acceleration_max; + + limits.torque_max = {210, 210, 210, 100, 100, 100}; // Nm + // limits.tau_min = -limits.torque_max; + + limits.tool_speed_max = 2.0; + + // kinematic properties + ManipulatorKinematics kinematics; // using default Q_base + kinematics.joint_offsets = { + Vec3{0.11024, -0.06926, -0.1375}, + Vec3{0.0, 0.4850, 0.0}, + Vec3{0.0, -0.15216, -0.0917}, + Vec3{0.0, -0.06296, -0.22275}, + Vec3{0.08703, 0.0860, -0.07692}, + Vec3{0.0, 0.0, -0.0920}}; // vector to next joint + kinematics.joint_axes = { + Vec3{0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}}; // direction vectors of joint + kinematics.first_joint_position = {0.0, 0.0, 0.0530f}; + kinematics.base_position = {0.0, 0.0, 0.0}; + kinematics.base_rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + kinematics.static_rotations[0] = {1, 0, 0, 0, -1, 0, 0, 0, -1}; + kinematics.static_rotations[1] = {1, 0, 0, 0, 0, -1, 0, 1, 0}; + kinematics.static_rotations[2] = {1, 0, 0, 0, -1, 0, 0, 0, -1}; + kinematics.static_rotations[3] = {1, 0, 0, 0, 0, -1, 0, 1, 0}; + kinematics.static_rotations[4] = {1, 0, 0, 0, 0, -1, 0, 1, 0}; + kinematics.static_rotations[5] = {0, 0, 1, 0, 1, 0, -1, 0, 0}; + kinematics.static_rotations[6] = {1, 0, 0, 0, -1, 0, 0, 0, -1}; + + // dynamic properties + ManipulatorDynamics dynamics; + dynamics.link_masses = { + 4.8257f, + 5.9860f, + 3.4159f, + 2.0849f, + 2.0076f, + 1.5193f}; // link masses + dynamics.inertia_tensors = { + Mat3{0.0192746f, -0.00239802f, -0.00896331f, -0.00239802f, 0.03087806f, 0.0016298f, -0.00896331f, 0.0016298f, 0.02134949f}, + {0.25899206f, -2.89E-05f, -1.23E-06f, -2.89E-05f, 0.01755445f, -0.02128064f, -1.23E-06f, -0.02128064f, 0.25291674f}, + {0.01742043f, -3.55E-06f, 8.4E-07f, -3.55E-06f, 0.01119175f, 0.00518163f, 8.4E-07f, 0.00518163f, 0.01212876f}, + {0.02454276f, 2.61E-06f, 1.799E-05f, 2.61E-06f, 0.02385702f, 0.00315758f, 1.799E-05f, 0.00315758f, 0.00294903f}, + {0.00734684f, 0.00124927f, -0.00090156f, 0.00124927f, 0.00464684f, -0.00236128f, -0.00090156f, -0.00236128f, 0.00589508f}, + {0.00390762f, -1.13E-06f, 1.16E-06f, -1.13E-06f, 0.00390722f, -2.21E-05f, 1.16E-06f, -2.21E-05f, 0.0013928f}}; // Inertial tensors + dynamics.cog_offsets = { + Vec3{0.03930119f, -0.00705889f, -0.08462154f}, + {2.53E-06f, 0.18829586f, -0.03988382f}, + {4.64E-06f, -0.02451414f, -0.02997969f}, + {-0.00010793f, -0.01056422f, -0.08091102f}, + {0.01243595f, 0.03284165f, -0.04091434f}, + {0.0f, 0.00050624f, -0.00388589f}}; // centers of mass + + // capsules & internal collision data + // Collision model + ManipulatorCapsules collisions; + Sphere base; + base.center = {0, 0, 0.0}; // because this is relative to p_base and p_base is {0, 0, 0.053} + base.radius = 0.2375; + collisions.base_sphere = base; + collisions.collision_base = {0, 0, 0, 1, 1, 1, 1}; + + collisions.collision_matrix.resize(7, 7); + collisions.collision_matrix(5, 0) = 1; + collisions.collision_matrix(6, 0) = 1; + collisions.collision_matrix(4, 1) = 1; + collisions.collision_matrix(5, 1) = 1; + collisions.collision_matrix(6, 1) = 1; + + // collisions.collision_matrix.resize(6, 6); + // collisions.collision_matrix(5, 0) = 1; + // collisions.collision_matrix(4, 1) = 1; + // collisions.collision_matrix(5, 1) = 1; + + // Collision model + CollisionModelCapsule model_caps; + + // Capsule 1 + model_caps.joint_frame = 1; + model_caps.p1 = {0, 0, -0.065}; + model_caps.p2 = {0, 0, 0.045}; + model_caps.radius = 0.065; + collisions.capsule_list.push_back(model_caps); + + // Capsule 2 + model_caps.joint_frame = 1; + // model_caps.p1 = {0, 0, -0.065}; + model_caps.p1 = {0, 0, -0.08}; + // model_caps.p2 = {0, 0.485, -0.065}; + model_caps.p2 = {0, 0.485, -0.08}; + model_caps.radius = 0.065; + collisions.capsule_list.push_back(model_caps); + + // Capsule 3 + model_caps.joint_frame = 2; + model_caps.p1 = {0, 0, -0.065}; + model_caps.p2 = {0, 0, 0.085}; + model_caps.radius = 0.065; + collisions.capsule_list.push_back(model_caps); + + // Capsule 4 + model_caps.joint_frame = 2; + model_caps.p1 = {0, 0.00695, -0.0917}; + model_caps.p2 = {0, -0.36805, -0.0917}; + model_caps.radius = 0.061; + collisions.capsule_list.push_back(model_caps); + + // Capsule 5 + model_caps.joint_frame = 4; + model_caps.p1 = {0, 0, 0}; + model_caps.p2 = {0, 0, -0.08}; + model_caps.radius = 0.060; + collisions.capsule_list.push_back(model_caps); + + // Capsule 6 + model_caps.joint_frame = 5; + model_caps.p1 = {0, 0, 0.08583}; + model_caps.p2 = {0, 0, -0.06417}; + model_caps.radius = 0.060; + collisions.capsule_list.push_back(model_caps); + + // Capsule 7 + model_caps.joint_frame = 5; + model_caps.p1 = {0, 0.02125, -0.007}; + // model_caps.p2 = {0, 0.02125, -0.013}; + model_caps.p2 = {0, 0.02125, 0.143}; + model_caps.radius = 0.085; + collisions.capsule_list.push_back(model_caps); // todo: change camera capsule + + // create manipulator link6 + Manipulator link6(joints, limits, kinematics, &dynamics, &collisions); + + return link6; +} + +inline host_fn Task get_link6_task() { + Array pi = deg2rad({-40.445762634277344, + -26.876392364501953, + 83.60868835449219, + 1.49383544921875, + 19.951095581054688, + -42.22943115234375}); // w1 + 10 + Array pf = deg2rad({51.851436614990234, + -13.578636169433594, + 107.87167358398438, + 3.6194305419921875, + 33.133209228515625, + 51.21833801269531}); // wb1 + return Task::stop_to_stop(pi, pf); +} + +inline host_fn Optimization get_generic_link6_opt() { + // Manip + Manipulator manip = get_generic_Link6(); + + // Task + auto task = get_link6_task(); + + // Create optimization + Optimization opt(manip, task); + + // world + World world = get_lab_world(); + opt.set_world(world); + + // Constraints + ConstraintSelection constraints; + constraints.position = true; + constraints.velocity = true; + constraints.acceleration = true; + constraints.torque = true; + constraints.tool_speed = true; + constraints.self_collisions = true; + constraints.external_collisions = true; + opt.set_constraints(constraints); + + // Objective + Objective objective; + objective.time_weight = 1; + opt.set_objective(objective); + + // B-spline + Bspline bspline(manip.n_joints); + opt.set_bspline(bspline); + + // Guess + opt.guess.type = Guess::random; + opt.guess.n_random_shots = 100; + + return opt; +} + +inline host_fn std::vector get_UR5e_tasks() { + std::vector start_pos; + start_pos.push_back({1.94822, 0.473555, -0.0255247, -0.448375, 0.370356, -3.12883}); + start_pos.push_back({2.4316, 0.0965, 1.1824, 1.8626, -0.8567, 0.0250}); + start_pos.push_back({-0.704868, 2.17507, 0.834305, 0.132228, 2.27799, 0.00328674}); + start_pos.push_back({-0.819005, 2.50686, 0.344732, -2.85083, -2.39379, -2.93992}); + start_pos.push_back({2.04549, 0.675615, 0.137326, -0.814844, 0.449157, -3.08683}); + start_pos.push_back({2.3499, 1.44305, -1.11122, -0.333013, 0.751039, -3.09482}); + start_pos.push_back({2.36232, 1.06656, -0.132829, -0.933749, 0.788944, -3.13519}); + start_pos.push_back({2.22452, 1.15771, -0.467326, -0.690436, 0.641695, -3.12205}); + + std::vector end_pos; + end_pos.push_back({2.5825, 0.0700, -0.3892, 0.3196, 0.9927, -3.17328}); + end_pos.push_back({0.1706, -2.5340, -1.2649, -2.4857, -1.4118, 2.9011}); + end_pos.push_back({0.3572, 2.9674, 0.4629, 2.8527, -1.2170, 3.1408}); + end_pos.push_back({-0.0436368, -2.65335, 0.593745, 2.05947, -1.61882, -3.12196}); + end_pos.push_back({-2.6558, -0.6703, 0.0599, -2.5311, -2.0486, -0.0127}); + end_pos.push_back({3.12394, -0.553571, -0.509807, 1.06314, 1.52902, -3.10058}); + end_pos.push_back({2.4552, -0.1792, -1.0268, -1.9354, -0.8723, -0.0198}); + end_pos.push_back({2.14686, -0.283198, -0.264971, -2.59327, -0.566506, -0.0169222}); + + std::vector task_list; + task_list.reserve(64); + for (auto& s: start_pos) + for (auto& e: end_pos) + task_list.push_back(Task::stop_to_stop(s, e)); + return task_list; +} + +inline host_fn World get_kitchen_open_doors() { + World world; + world.add_box(Vec3{0.427, -1.0847, 1.1974}, Vec3{0.36029, 0.06, 0.36242}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.427, 0.48791, 1.1974}, Vec3{0.36029, 0.06, 0.36242}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.427, -0.29838, 1.5298}, Vec3{0.36029, 0.84629, 0.03}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.427, -0.29838, 1.2074}, Vec3{0.36029, 0.84629, 0.03}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.427, -0.29838, 0.86498}, Vec3{0.36029, 0.84629, 0.03}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.72729, -0.29838, 1.2074}, Vec3{0.06, 0.84629, 0.36242}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{-0.20911, 0.86968, 1.2009}, Vec3{0.032523, 0.42256, 0.36242}, Mat3{-0.70706, -0.70716, 0, -0.70716, -0.70706, 0, 0, 0, 1}); + world.add_box(Vec3{-0.20549, -1.4701, 1.2009}, Vec3{0.037648, 0.42257, 0.36244}, Mat3{-0.70706, 0.70716, 0, 0.70716, -0.70706, 0, 0, 0, 1}); + world.add_box(Vec3{-0.47232, -0.77127, -0.48346}, Vec3{0.023735, 0.41378, 0.41552}, Mat3{4.6327e-05, 0, 1, 0, 1, 0, 1, 0, 4.6327e-05}); + world.add_box(Vec3{0.35698, 0.425, -0.066059}, Vec3{0.41378, 0.8425, 0.43736}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.35698, -1.1546, -0.066059}, Vec3{0.41378, 0.03, 0.43736}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.35698, -0.80106, 0.31136}, Vec3{0.41378, 0.38356, 0.06}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.35698, -0.80106, -0.055}, Vec3{0.41378, 0.38356, 0.025}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + world.add_box(Vec3{0.35698, -0.80106, -0.47336}, Vec3{0.41378, 0.38356, 0.03}, Mat3{1, 0, 0, 0, 1, 0, 0, 0, 1}); + return world; +} + +inline Manipulator get_generic_ur5e() { + // Manipulator + u32 joints = 6; + + // limits + ManipulatorLimits limits; + limits.position_max = {6.283200, 6.283200, 3.141600, 6.283200, 6.283200, 6.283200}; + limits.position_min = {-6.283200, -6.283200, -3.141600, -6.283200, -6.283200, -6.283200}; + limits.velocity_max = {3.141600, 3.141600, 3.141600, 3.141600, 3.141600, 3.141600}; + // limits.vmin = {-3.141600, -3.141600, -3.141600, -3.141600, -3.141600, -3.141600}; + limits.acceleration_max = {13.96, 13.96, 13.96, 13.96, 13.96, 13.96}; + // limits.amin = {-13.96, -13.96, -13.96, -13.96, -13.96, -13.96}; + limits.torque_max = {150.000000, 150.000000, 150.000000, 28.000000, 28.000000, 28.000000}; + // limits.tau_min = {-150.000000, -150.000000, -150.000000, -28.000000, -28.000000, -28.000000}; + limits.tool_speed_max = 2.0; // todo: verify this value + + ManipulatorKinematics kinematics; + kinematics.joint_offsets = { + Vec3{0, 0, 0}, + {-0.425, 0, 0}, + {-0.3922, 0, 0.1333}, + {0, -0.0997, -0}, + {0, 0.0996, -0}, + {0, 0, 0} // to end effector + }; + kinematics.joint_axes = { + Vec3{0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}, + {0, 0, 1}}; + // kinematics.static_rotations.resize(6); + kinematics.static_rotations[0] = {-1.000000, 0.000000, 0.000000, -0.000000, -1.000000, 0.000000, 0.000000, -0.000000, 1.000000}; // modified + kinematics.static_rotations[1] = {1.000000, 0.000000, 0.000000, -0.000000, -0.000000, 1.000000, 0.000000, -1.000000, -0.000000}; + kinematics.static_rotations[2] = {1.000000, 0.000000, 0.000000, -0.000000, 1.000000, 0.000000, 0.000000, -0.000000, 1.000000}; + kinematics.static_rotations[3] = {1.000000, 0.000000, 0.000000, -0.000000, 1.000000, 0.000000, 0.000000, -0.000000, 1.000000}; + kinematics.static_rotations[4] = {1.000000, 0.000000, 0.000000, -0.000000, -0.000000, 1.000000, 0.000000, -1.000000, -0.000000}; + kinematics.static_rotations[5] = {1.000000, -0.000000, 0.000000, 0.000000, -0.000000, -1.000000, 0.000000, 1.000000, -0.000000}; + // kinematics.static_rotations[6] = {1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000}; + kinematics.first_joint_position = {0.000000, 0.000000, 0.162500}; + + ManipulatorDynamics dynamics; + dynamics.link_masses = {3.700000, 8.393000, 2.275000, 1.219000, 1.219000, 0.187900}; + dynamics.inertia_tensors = { + Mat3{0.0103, 0, 0, 0, 0.0103, 0, 0, 0, 0.0067}, + {0.1339, 0, 0, 0, 0.1339, 0, 0, 0, 0.0151}, + {0.0312, 0, 0, 0, 0.0312, 0, 0, 0, 0.0041}, + {0.0026, 0, 0, 0, 0.0026, 0, 0, 0, 0.0022}, + {0.0026, 0, 0, 0, 0.0026, 0, 0, 0, 0.0022}, + {0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001}}; + dynamics.cog_offsets = { + Vec3{0, 0, 0}, + {-0.2125, 0, 0.138}, + {-0.1961, 0, 0.007}, + {0, 0, 0}, + {0, 0, 0}, + {0, 0, -0.0229}}; + + ManipulatorCapsules collisions; + + Sphere base; + base.center = {0, 0, 0.0325}; + base.radius = 0.09; + collisions.base_sphere = base; + collisions.collision_base = {0, 0, 0, 1, 1, 1, 1}; + + collisions.collision_matrix.resize(7, 7); + collisions.collision_matrix(3, 0) = 1; + collisions.collision_matrix(4, 0) = 1; + collisions.collision_matrix(5, 0) = 1; + collisions.collision_matrix(6, 0) = 1; + + collisions.collision_matrix(4, 1) = 1; + collisions.collision_matrix(5, 1) = 1; + collisions.collision_matrix(6, 1) = 1; + + collisions.collision_matrix(6, 3) = 1; + + // mirrored just for consistency + // collisions.collision_matrix(0, 3) = 1; + // collisions.collision_matrix(0, 4) = 1; + // collisions.collision_matrix(0, 5) = 1; + // collisions.collision_matrix(0, 6) = 1; + // + // collisions.collision_matrix(1, 4) = 1; + // collisions.collision_matrix(1, 5) = 1; + // collisions.collision_matrix(1, 6) = 1; + // + // collisions.collision_matrix(3, 6) = 1; + + CollisionModelCapsule capsule; + capsule.joint_frame = 0; + capsule.p1 = {0, 0, 0}; + capsule.p2 = {0, -0.15, 0}; + capsule.radius = 0.09; + collisions.capsule_list.push_back(capsule); + + capsule.joint_frame = 1; + capsule.p1 = {-0.42, 0, 0.1375}; + capsule.p2 = {0, 0, 0.1375}; + capsule.radius = 0.06; + collisions.capsule_list.push_back(capsule); + + capsule.joint_frame = 2; + capsule.p1 = {0, 0, 0.02}; + capsule.p2 = {0, 0, 0.18}; + capsule.radius = 0.065; + collisions.capsule_list.push_back(capsule); + + capsule.joint_frame = 2; + capsule.p1 = {-0.373156, 0, 0.00850418}; + capsule.p2 = {0.000440611, 0.000121443, 0.00850418}; + capsule.radius = 0.05; + collisions.capsule_list.push_back(capsule); + + capsule.joint_frame = 3; + capsule.p1 = {0, 0, 0}; + capsule.p2 = {0, 0, -0.155}; + capsule.radius = 0.0425; + collisions.capsule_list.push_back(capsule); + + capsule.joint_frame = 3; + capsule.p1 = {0, 0.03, 0}; + capsule.p2 = {0, -0.1, 0}; + capsule.radius = 0.0425; + collisions.capsule_list.push_back(capsule); + + capsule.joint_frame = 5; + capsule.p1 = {0, 0, -0.14}; + capsule.p2 = {0, 0, -0.01}; + capsule.radius = 0.038; + collisions.capsule_list.push_back(capsule); + + Manipulator generic_manip(joints, limits, kinematics, &dynamics, &collisions); + return generic_manip; +} + +TEST_CASE("Constraints calculation", "[World]") { + std::vector> worlds = { + {get_bookshelf_small(), "get_bookshelf_small()"}, + {get_bookshelf_tall(), "get_bookshelf_tall()"}, + {get_bookshelf_thin(), "get_bookshelf_thin()"}, + {get_scene_box(), "get_scene_box()"}, + {get_scene_cage(), "get_scene_cage()"}, + {get_scene_table(), "get_scene_table()"}, + {get_kitchen_no_doors(), "get_kitchen_no_doors()"}, + {get_lab_world(), "get_lab_world()"}, + }; +#if BENCHMARKING_ON + int num_tests_per_world = 1; +#else + int num_tests_per_world = 1e3; +#endif + + for (int worlds_id = 0; worlds_id < worlds.size(); worlds_id++) { + Optimization opt = get_generic_link6_opt(); + opt.constraints.n_constraints_per_segment = 33; + + initialize_optimization_with_segments(&opt); + n_con_with_segments(&opt); + + opt.world = worlds[worlds_id].first; + add_dynamic_obstacles(opt.world); + create_static_bounding_volume_hierarchy(opt.world, opt.world.static_bounding_volume_hierarchy); + for (int test = 0; test < num_tests_per_world; test++) { + Array x = guess_random(opt.bspline, opt.task); + + Array constraints(opt.constraints.n_constraints); + Matrix gradient; + Array constraints_broadphase(opt.constraints.n_constraints); + Matrix gradient_broadphase; + Array constraints_double_bvh(opt.constraints.n_constraints); + Matrix gradient_double_bvh; + +#if BENCHMARKING_ON + std::cout << "\n" + << worlds[worlds_id].second << "=====================================" << std::endl; + BENCHMARK("_with_segments") { + constraints_and_gradients_with_segments(x, opt, constraints, gradient); + return constraints; + }; + BENCHMARK("Broadphase") { + constraints_and_gradients_with_broadphase(x, opt, constraints_broadphase, gradient_broadphase); + return constraints_broadphase; + }; + BENCHMARK("Broadphase - double BVH") { + constraints_and_gradients_with_double_broadphase(x, opt, constraints_double_bvh, gradient_double_bvh); + return constraints_double_bvh; + }; +#else + constraints_and_gradients_with_segments(x, opt, constraints, gradient); + constraints_and_gradients_with_broadphase(x, opt, constraints_broadphase, gradient_broadphase); + constraints_and_gradients_with_double_broadphase(x, opt, constraints_double_bvh, gradient_double_bvh); +#endif + + CHECK(is_close(constraints, constraints_broadphase)); + CHECK(is_close(constraints, constraints_double_bvh)); + CHECK(is_close(gradient, gradient_broadphase)); + CHECK(is_close(gradient, gradient_double_bvh)); + + if (!is_close(constraints, constraints_broadphase)) { + std::cout << "Fail broadphase" << std::endl; + std::cout << worlds[worlds_id].second << std::endl; + std::cout << "x = {"; + for (int i = 0; i < x.size; i++) { + std::cout << std::fixed << std::setprecision(10) << x[i] << ", "; + } + std::cout << "};" << std::endl; + for (int i = 0; i < constraints.size; i++) { + std::cout << std::fixed << std::setprecision(7) << constraints[i] << ", " << constraints_broadphase[i] << std::endl; + } + } + if (!is_close(constraints, constraints_double_bvh)) { + std::cout << "Fail double bvh" << std::endl; + std::cout << worlds[worlds_id].second << std::endl; + std::cout << "x = {"; + for (int i = 0; i < x.size; i++) { + std::cout << std::fixed << std::setprecision(10) << x[i] << ", "; + } + std::cout << "};" << std::endl; + } + } + } + std::cout << std::endl; +} + +TEST_CASE("Optimization benchmark", "[World]") { + std::vector task_list = get_UR5e_tasks(); + + std::vector> method = { + {OptimizationMethod::with_segments, "with_segments"}, + {OptimizationMethod::broadphase, "broadphase"}, + {OptimizationMethod::double_broadphase, "double_broadphase"}, + }; + + std::vector x0; + for (int task_id = 0; task_id < 1; task_id++) { + std::cout << "Task " << task_id << std::endl; + for (int method_id = 0; method_id < method.size(); method_id++) { + Manipulator ur5e = get_generic_ur5e(); + ur5e.base_position = {-0.5, -0.3, 0.35}; + ur5e.base_rotation = {-1, 0, 0, 0, -1, 0, 0, 0, 1}; + World world = get_kitchen_open_doors(); + Optimization opt(ur5e, task_list[task_id]); + opt.bspline = Bspline(16, 110, 5, ur5e.n_joints); + opt.world = world; + + opt.constraints.position = true; + opt.constraints.velocity = true; + opt.constraints.acceleration = true; + opt.constraints.torque = true; + opt.constraints.tool_speed = true; + opt.constraints.self_collisions = true; // avoid self-contact + opt.constraints.external_collisions = true; // avoid world obstacles + + opt.success_tolerance = 0.01; + create_static_bounding_volume_hierarchy(opt.world, opt.world.static_bounding_volume_hierarchy); + + const int max_attempts = 3; + Result result(&opt); // initialise with a pointer to opt + + std::cout << "\nMethod: " << method[method_id].second << "\n" + << std::endl; + for (int attempt = 1; attempt <= max_attempts; ++attempt) { + + // Keep initial guesses consistent across methods + opt.guess.type = Guess::custom; + if (method_id == 0) { + x0.push_back(guess_random(opt.bspline, opt.task)); + } + + int n = attempt - 1 <= x0.size() ? attempt - 1 : x0.size(); + opt.guess.initial_x = x0[n]; + opt.method = method[method_id].first; + result = optimize(&opt); + + if (result.success) + break; + } + + std::cout << (result.success ? "success" : "failed") + << " (time: " << result.compute_time << " ms)\n"; + + + std::cout << "\n--- Final result ---\n"; + std::cout << "Success: " << (result.success ? "yes" : "no") << "\n"; + std::cout << "Compute time (ms): " << result.compute_time << "\n"; + std::cout << "Function evaluations: " << result.num_eval << "\n"; + std::cout << "Max constraint violation: " << result.max_constraint_value << "\n"; + + if (!result.x.is_empty()) { + std::cout << "Trajectory duration (s): " << result.x.back() << "\n"; + } + + if (!result.success) { + std::cout << "\nNote: no collision-free trajectory was found in " + << max_attempts << " attempts. " + << "Consider adjusting the obstacle, the task, or increasing max_attempts.\n"; + } + } + } +} + + +#include + +using blast::Optimization; +using blast::OptimizationMethod; +using blast::optimize; +using blast::real; +using blast::Result; + +struct MethodStats { + real success_rate = 0; + real mean_compute_time_ms = 0; + real mean_traj_time_s = 0; + real mean_max_constraint = 0; +}; + +inline MethodStats run_trials(OptimizationMethod method, int n_trials, Task task, Array& x0) { + using namespace blast; + int n_success = 0; + real total_compute_time = 0; + real total_traj_time = 0; + real total_max_constraint = 0; + + Manipulator ur5e = get_generic_ur5e(); + ur5e.base_position = {-0.5, -0.3, 0.35}; + ur5e.base_rotation = {-1, 0, 0, 0, -1, 0, 0, 0, 1}; + + Optimization opt(ur5e, task); + + opt.world = get_kitchen_open_doors(); + create_static_bounding_volume_hierarchy(opt.world, opt.world.static_bounding_volume_hierarchy); + + Bspline bspline(16, 110, 5, 6); + opt.bspline = bspline; + + opt.constraints.position = true; + opt.constraints.velocity = true; + opt.constraints.acceleration = true; + opt.constraints.torque = true; + opt.constraints.tool_speed = true; + opt.constraints.self_collisions = true; + opt.constraints.external_collisions = true; + + opt.constraints.n_collision_constraints = 1; + opt.max_tries = 1; + opt.success_tolerance = 0.01; + + opt.guess.type = Guess::custom; + + auto t1 = get_tick_us(); + Result result(&opt); + + for (int i = 0; i < n_trials; i++) { + if (method == OptimizationMethod::with_segments && i == 0) { + opt.guess.initial_x = guess_random(opt.bspline, opt.task); + x0 = opt.guess.initial_x; + } else { + opt.guess.initial_x = x0; + } + + opt.method = method; + result = optimize(&opt); + + if (result.success) { + n_success++; + total_traj_time += result.x.back(); + total_compute_time += result.compute_time; + total_max_constraint = std::max(total_max_constraint, result.max_constraint_value); + } + } + + MethodStats s; + s.success_rate = 100.0 * (real) n_success / n_trials; + s.mean_compute_time_ms = total_compute_time / n_trials; + s.mean_traj_time_s = n_success > 0 ? total_traj_time / n_success : 0; + s.mean_max_constraint = total_max_constraint / n_trials; + return s; +} + +TEST_CASE("Test", "[World]") { + constexpr int n_trials = 2; + + struct Entry { + OptimizationMethod method; + const char* name; + }; + + constexpr std::array methods = {{ + {OptimizationMethod::with_segments, "with_segments"}, + {OptimizationMethod::broadphase, "broadphase"}, + {OptimizationMethod::double_broadphase, "double_broadphase"}, + }}; + + auto task_list = get_UR5e_tasks(); + Array x0; + + for (int task_id = 0; task_id < 1; task_id++) { + std::cout << "\n--- Optimization Method Benchmark (" << n_trials << " trials each) ---\n"; + std::cout << std::left + << std::setw(32) << "Method" + << std::setw(14) << "Success" + << std::setw(20) << "Compute time (ms)" + << std::setw(16) << "Traj time (s)" + << "Max constraint\n"; + std::cout << std::string(92, '-') << "\n"; + + int rand = std::round(std::abs(task_list.size() * random_real())); + std::cout << "Task: " << rand << std::endl; + for (const auto& e: methods) { + auto s = run_trials(e.method, n_trials, task_list[task_id], x0); + std::cout << std::left + << std::setw(32) << e.name + << std::setw(14) << s.success_rate + << std::setw(20) << s.mean_compute_time_ms + << std::setw(16) << s.mean_traj_time_s + << s.mean_max_constraint << std::endl; + } + } +}