12 Vec3<T> center2cp_local = _orientation.transpose() * (cp_pos - _position);
14 bool inside_box(
true);
16 for (
size_t i(0); i < 3; ++i) {
18 if (center2cp_local[i] > _size[i] / 2.) {
22 else if (center2cp_local[i] < -_size[i] / 2.) {
27 err[i] = _size[i] / 2. - std::abs(center2cp_local[i]);
32 if (err[2] < std::min(err[0], err[1])) {
33 if (center2cp_local[2] > 0.) {
34 cp_frame = _orientation;
36 cp_frame = -_orientation;
38 penetration = -err[2];
39 }
else if (err[1] < std::min(err[0], err[2])) {
40 if (center2cp_local[1] > 0.) {
42 cp_frame.template block<3, 1>(0, 0) =
43 _orientation.template block<3, 1>(0, 0);
45 cp_frame.template block<3, 1>(0, 1) =
46 -_orientation.template block<3, 1>(0, 2);
48 cp_frame.template block<3, 1>(0, 2) =
49 _orientation.template block<3, 1>(0, 1);
52 cp_frame.template block<3, 1>(0, 0) =
53 _orientation.template block<3, 1>(0, 0);
55 cp_frame.template block<3, 1>(0, 1) =
56 _orientation.template block<3, 1>(0, 2);
58 cp_frame.template block<3, 1>(0, 2) =
59 -_orientation.template block<3, 1>(0, 1);
61 penetration = -err[1];
63 if (center2cp_local[0] > 0.) {
65 cp_frame.template block<3, 1>(0, 0) =
66 _orientation.template block<3, 1>(0, 1);
68 cp_frame.template block<3, 1>(0, 1) =
69 -_orientation.template block<3, 1>(0, 2);
71 cp_frame.template block<3, 1>(0, 2) =
72 _orientation.template block<3, 1>(0, 0);
75 cp_frame.template block<3, 1>(0, 0) =
76 _orientation.template block<3, 1>(0, 0);
78 cp_frame.template block<3, 1>(0, 1) =
79 _orientation.template block<3, 1>(0, 1);
81 cp_frame.template block<3, 1>(0, 2) =
82 -_orientation.template block<3, 1>(0, 0);
84 penetration = -err[0];
typename Eigen::Matrix< T, 3, 3 > Mat3
typename Eigen::Matrix< T, 3, 1 > Vec3
virtual bool ContactDetection(const Vec3< T > &cp_pos, T &penetration, Mat3< T > &cp_frame)
Collision logic for a box.