Line data    Source code

       1              : #pragma once
       2              : #include "rbc/shapes/ShapeTypes.hpp"
       3              : #include <math3d/math3d.hpp>
       4              : 
       5              : namespace rbc
       6              : {
       7              :     struct MinkowskiDiff
       8              :     {
       9              :         const Shape *shape_a;
      10              :         const Shape *shape_b;
      11              :         m3d::tf tf_a;
      12              :         m3d::tf tf_b;
      13              : 
      14           19 :         MinkowskiDiff(const Shape *a, const Shape *b,
      15              :                       const m3d::tf &ta, const m3d::tf &tb)
      16           19 :             : shape_a(a), shape_b(b), tf_a(ta), tf_b(tb) {}
      17              : 
      18              :         // Calculates: Support(A) - Support(B) in global coordinates
      19              :         m3d::vec3 support(const m3d::vec3 &dir) const
      20              :         {
      21              :             // 1. Transformar dirección al espacio local de A, buscar support, devolver a global
      22              :             m3d::vec3 local_dir_a = tf_a.inverse_rotate_vector(dir);
      23              :             m3d::vec3 p_a = tf_a.transform_point(shape_support(*shape_a, local_dir_a));
      24              : 
      25              :             // 2. Transformar dirección opuesta al espacio local de B, buscar support, devolver a global
      26              :             m3d::vec3 local_dir_b = tf_b.inverse_rotate_vector(-dir);
      27              :             m3d::vec3 p_b = tf_b.transform_point(shape_support(*shape_b, local_dir_b));
      28              : 
      29              :             return p_a - p_b;
      30              :         }
      31              :     };
      32              : }