intersectsWithObb3 method

Implementation

bool intersectsWithObb3(Obb3 other, [double epsilon = 1e-3]) {
  // Compute rotation matrix expressing other in this's coordinate frame
  _r
    ..setEntry(0, 0, _axis0.dot(other._axis0))
    ..setEntry(1, 0, _axis1.dot(other._axis0))
    ..setEntry(2, 0, _axis2.dot(other._axis0))
    ..setEntry(0, 1, _axis0.dot(other._axis1))
    ..setEntry(1, 1, _axis1.dot(other._axis1))
    ..setEntry(2, 1, _axis2.dot(other._axis1))
    ..setEntry(0, 2, _axis0.dot(other._axis2))
    ..setEntry(1, 2, _axis1.dot(other._axis2))
    ..setEntry(2, 2, _axis2.dot(other._axis2));

  // Compute translation vector t
  _t
    ..setFrom(other._center)
    ..sub(_center);

  // Bring translation into this's coordinate frame
  _t.setValues(_t.dot(_axis0), _t.dot(_axis1), _t.dot(_axis2));

  // Compute common subexpressions. Add in an epsilon term to
  // counteract arithmetic errors when two edges are parallel and
  // their cross product is (near) null.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      _absR.setEntry(i, j, _r.entry(i, j).abs() + epsilon);
    }
  }

  double ra;
  double rb;

  // Test axes L = A0, L = A1, L = A2
  for (int i = 0; i < 3; i++) {
    ra = _halfExtents[i];
    rb = other._halfExtents[0] * _absR.entry(i, 0) +
        other._halfExtents[1] * _absR.entry(i, 1) +
        other._halfExtents[2] * _absR.entry(i, 2);

    if (_t[i].abs() > ra + rb) {
      return false;
    }
  }

  // Test axes L = B0, L = B1, L = B2
  for (int i = 0; i < 3; i++) {
    ra = _halfExtents[0] * _absR.entry(0, i) +
        _halfExtents[1] * _absR.entry(1, i) +
        _halfExtents[2] * _absR.entry(2, i);
    rb = other._halfExtents[i];

    if ((_t[0] * _r.entry(0, i) +
                _t[1] * _r.entry(1, i) +
                _t[2] * _r.entry(2, i))
            .abs() >
        ra + rb) {
      return false;
    }
  }

  // Test axis L = A0 x B0
  ra = _halfExtents[1] * _absR.entry(2, 0) +
      _halfExtents[2] * _absR.entry(1, 0);
  rb = other._halfExtents[1] * _absR.entry(0, 2) +
      other._halfExtents[2] * _absR.entry(0, 1);
  if ((_t[2] * _r.entry(1, 0) - _t[1] * _r.entry(2, 0)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A0 x B1
  ra = _halfExtents[1] * _absR.entry(2, 1) +
      _halfExtents[2] * _absR.entry(1, 1);
  rb = other._halfExtents[0] * _absR.entry(0, 2) +
      other._halfExtents[2] * _absR.entry(0, 0);
  if ((_t[2] * _r.entry(1, 1) - _t[1] * _r.entry(2, 1)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A0 x B2
  ra = _halfExtents[1] * _absR.entry(2, 2) +
      _halfExtents[2] * _absR.entry(1, 2);
  rb = other._halfExtents[0] * _absR.entry(0, 1) +
      other._halfExtents[1] * _absR.entry(0, 0);
  if ((_t[2] * _r.entry(1, 2) - _t[1] * _r.entry(2, 2)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A1 x B0
  ra = _halfExtents[0] * _absR.entry(2, 0) +
      _halfExtents[2] * _absR.entry(0, 0);
  rb = other._halfExtents[1] * _absR.entry(1, 2) +
      other._halfExtents[2] * _absR.entry(1, 1);
  if ((_t[0] * _r.entry(2, 0) - _t[2] * _r.entry(0, 0)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A1 x B1
  ra = _halfExtents[0] * _absR.entry(2, 1) +
      _halfExtents[2] * _absR.entry(0, 1);
  rb = other._halfExtents[0] * _absR.entry(1, 2) +
      other._halfExtents[2] * _absR.entry(1, 0);
  if ((_t[0] * _r.entry(2, 1) - _t[2] * _r.entry(0, 1)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A1 x B2
  ra = _halfExtents[0] * _absR.entry(2, 2) +
      _halfExtents[2] * _absR.entry(0, 2);
  rb = other._halfExtents[0] * _absR.entry(1, 1) +
      other._halfExtents[1] * _absR.entry(1, 0);
  if ((_t[0] * _r.entry(2, 2) - _t[2] * _r.entry(0, 2)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A2 x B0
  ra = _halfExtents[0] * _absR.entry(1, 0) +
      _halfExtents[1] * _absR.entry(0, 0);
  rb = other._halfExtents[1] * _absR.entry(2, 2) +
      other._halfExtents[2] * _absR.entry(2, 1);
  if ((_t[1] * _r.entry(0, 0) - _t[0] * _r.entry(1, 0)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A2 x B1
  ra = _halfExtents[0] * _absR.entry(1, 1) +
      _halfExtents[1] * _absR.entry(0, 1);
  rb = other._halfExtents[0] * _absR.entry(2, 2) +
      other._halfExtents[2] * _absR.entry(2, 0);
  if ((_t[1] * _r.entry(0, 1) - _t[0] * _r.entry(1, 1)).abs() > ra + rb) {
    return false;
  }

  // Test axis L = A2 x B2
  ra = _halfExtents[0] * _absR.entry(1, 2) +
      _halfExtents[1] * _absR.entry(0, 2);
  rb = other._halfExtents[0] * _absR.entry(2, 1) +
      other._halfExtents[1] * _absR.entry(2, 0);
  if ((_t[1] * _r.entry(0, 2) - _t[0] * _r.entry(1, 2)).abs() > ra + rb) {
    return false;
  }

  // Since no separating axis is found, the OBBs must be intersecting
  return true;
}