text: /* -------------------------------------------------------------------------- *
   *                        Simbody(tm): SimTKmath                              *
   * -------------------------------------------------------------------------- *
   * This is part of the SimTK biosimulation toolkit originating from           *
   * Simbios, the NIH National Center for Physics-Based Simulation of           *
   * Biological Structures at Stanford, funded under the NIH Roadmap for        *
   * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
   *                                                                            *
   * Portions copyright (c) 2011-12 Stanford University and the Authors.        *
   * Authors: Michael Sherman                                                   *
   * Contributors:                                                              *
   *                                                                            *
   * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
   * not use this file except in compliance with the License. You may obtain a  *
   * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
   *                                                                            *
   * Unless required by applicable law or agreed to in writing, software        *
   * distributed under the License is distributed on an "AS IS" BASIS,          *
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
   * See the License for the specific language governing permissions and        *
   * limitations under the License.                                             *
   * -------------------------------------------------------------------------- */
  
  /** @file
  Non-inline static methods from the Geo::Box_ class. **/
  
  #include "SimTKcommon.h"
  #include "simmath/internal/common.h"
  #include "simmath/internal/Geo.h"
  #include "simmath/internal/Geo_Box.h"
  #include "simmath/internal/Geo_Sphere.h"
  
  #include <cstdio>
  #include <iostream>
  using std::cout; using std::endl;
  
  namespace SimTK {
  
  //==============================================================================
  //                               GEO :: BOX
  //==============================================================================
  
  // For testing whether this box intersects a given oriented box, we use the
  // separating axis method first described in Gottschalk, S., Lin, MC, 
  // Manocha, D, "OBBTree: a hierarchical structure for rapid interference 
  // detection." Proceedings of the 23rd Annual Conference on Computer 
  // Graphics and Interactive Techniques, pp. 171-180, 1996. This is a series
  // of 15 tests used to determine whether there exists a separating plane
  // between the boxes. The possible plane normals are the 3 faces of each box,
  // and the 9 possible cross products of the three edge directions from each
  // box. In each case we project the boxes onto the plane normal lines
  // and see whether the projections overlap. If they don't, we have found a
  // separating plane and can return right away. This can be sped up by doing
  // only the first six face tests and accepting the occasional false positive
  // (allegedly less than 10%); we provide a method for doing that abbreviated 
  // version of the test as well as the full method.
  
  // If the boxes have a parallel edge, they have to be handled specially because
  // the cross product of those edges will be zero. Then the projections of both
  // the box dimensions and the center-to-center distance will be zero, creating 
  // a false appearance of having found a separating plane. Christer Ericson 
  // advocates use of a "fudge factor" to ensure a conservative result, but David
  // Eberly tackles it more directly by noting that if any edge is parallel then 
  // one of the six faces *must* be the separating plane if there is one. So if 
  // you've done six tests and not found a separating plane there is no need to 
  // check the edge cross products; the boxes do intersect. We follow Eberly's 
  // approach.
      
  
  
  // First check only the B and O face normals as possible separating planes. If 
  // we find one of those works the boxes can't be intersecting, otherwise they
  // might be. Ordering for speed:
  //  - first check faces of this box B, because we're working in B frame
  //    making these tests cheaper
  //  - when working on a box, test smallest dimension first because that 
  //    covers the most space in one test
  // For a few extra flops (1 per axis), we can also spot whether one box center
  // is completely inside the other and if so report early that the boxes are 
  // definitely intersecting. Worst case here is about 74 flops. Minimum is 16 
  // flops if the first axis checked is a separating axis.
  
  // Private helper method returns 0 if a separating plane is found (definitely
  // no intersection), 2 if the centers overlap (definitely an intersection)
  // and 1 otherwise (probably an intersection, but maybe not).
  
  // It is convenient to calculate the absolute value of the Transform matrix
  // because those are reused. We'll do that here and return the results in case
  // the caller needs to use them. Note that the absolute value of a Rotation
  // matrix is not necessarily a Rotation since the resulting vectors might not
  // even be perpendicular.
  template <class P> int Geo::Box_<P>::
  intersectsOrientedBoxHelper(const OrientedBox_<P>&  O,
                              Mat33P&                 absR_BO, 
                              Vec3P&                  absP_BO) const {
      const RotationP& R_BO = O.getTransform().R();
      const Vec3P&     p_BO = O.getTransform().p(); // center location
  
      absR_BO = R_BO.asMat33().abs(); // about 6 flops
      absP_BO = p_BO.abs();           // about 2 flops
      const Geo::Box_<P>& ob = O.getBox();
      const Vec3P&        oh = ob.getHalfLengths();
  
      bool centerOisInsideB = true; // set false when we see a counterexample
  
      // Try each face of B in increasing order of box dimension so that we
      // start with the skinniest slab of box B. Cost is 8 flops per axis.
      // "V" below is the 1st quadrant (+++) vertex of each box.
      for (int i=0; i<3; ++i) {
          const CoordinateAxis b = getOrderedAxis(i);
          const RealP rb = h[b];                  // proj. p_BV onto axis b
          const RealP ro = dot(oh, absR_BO[b]);   // proj. p_OV onto axis b (in O) 
          const RealP d  = absP_BO[b];            // ctr-ctr distance along b
          if (d > rb+ro)
              return 0; // axis b is a separating plane normal
          if (d > rb)
              centerOisInsideB = false; // saw a counterexample
      }
  
      if (centerOisInsideB)
          return 2; // definitely does intersect!
  
      bool centerBisInsideO = true;
  
      // Try each face of O in increasing order of box dimension so that we
      // start with the skinniest slab of box O. Cost is 14 flops per axis.
      for (int i=0; i<3; ++i) {
          const CoordinateAxis o = ob.getOrderedAxis(i);
          // Note: round brackets indexing selects a column of R_BO1.
          const RealP rb = dot(h, absR_BO(o));    // proj. p_BV onto axis o (in B)
          const RealP ro = oh[o];                 // proj. of p_OV onto axis o
          const RealP d  = std::abs(dot(p_BO, R_BO(o))); //B-O dist along o (in B)
          if (d > rb+ro)
              return 0; // axis o is a separating plane normal
          if (d > ro)
              centerBisInsideO = false; // saw a counterexample
      }
  
      if (centerBisInsideO)
          return 2; // definitely does intersect!
  
      return 1; // probably intersect
  }
  
  // Worst case 74 flops.
  template <class P> bool Geo::Box_<P>::
  mayIntersectOrientedBox(const Geo::OrientedBox_<P>& ob) const {
      Mat33P absR_BO; Vec3P absP_BO;
      const int result = intersectsOrientedBoxHelper(ob, absR_BO, absP_BO);
      return result != 0;
  }
  
  // Worst case 74 + 9 + 9*12 = 191 flops.
  template <class P> bool Geo::Box_<P>::
  intersectsOrientedBox(const Geo::OrientedBox_<P>& ob) const {
      Mat33P absR_BO; 
      Vec3P  absP_BO;
      const int result = intersectsOrientedBoxHelper(ob, absR_BO, absP_BO);
      if (result == 0) return false;
      else if (result == 2) return true;
  
      // Indeterminate -- we still don't know if there is an intersection.
  
      const RotationP& R_BO = ob.getTransform().R();
      const Vec3P&     p_BO = ob.getTransform().p(); // center location
      const Vec3P&     oh   = ob.getHalfLengths();
  
      // First we have to determine whether the boxes have a pair of edges that
      // are (almost) parallel. If so, one of the cross product tests below will 
      // produce a near-zero separating vector candidate. Projections onto that 
      // line will yield zero lengths and we'll end up comparing noise to noise
      // to see if we found a separating vector, producing unacceptable false
      // negatives. On the other hand, if there is a pair of parallel edges, then
      // one of the face normals we just tested would have to be a separating 
      // plane if there is one (draw a picture). In that case we know that we
      // are intersecting and can return true now. This also serves as a fast
      // out for the common case of parallel boxes.
  
      // If there is a parallel edge, one of the entries for O's axes in |R_BO| 
      // will be within noise of one of B's axes, that is, 100, 010, or 001. 
      // David Eberly's algorithm looks for a "1" as cos(near zero) but that's a
      // bad idea because sin() and cos() are very flat near 1. It is better to 
      // look for "0", as sin(near zero) because sin(theta)~=theta for small 
      // angles. So we pick an angle tolerance that we declare is small enough to
      // be considered parallel, and then see if a column of |R_BO| has two 
      // entries that size or smaller. It costs 9 flops to do this test.
  
      const RealP tol = Geo::getDefaultTol<P>(); // TODO: too tight?
      for (int i=0; i<3; ++i) {
          int numZeroes=0;
          const Vec3P& v = absR_BO(i); // get column; row would work too
          if (v[0] < tol) ++numZeroes; // 3 flops
          if (v[1] < tol) ++numZeroes;
          if (v[2] < tol) ++numZeroes;
          assert(numZeroes < 3); // can't happen in a Rotation!
          if (numZeroes==2) 
              return true; // boxes intersect
      }
  
      // There are no parallel edges. Check the 9 remaining possibilites for
      // separating axes formed by the cross product of an edge from B and an
      // edge from O. B's edges are 100,010,001 so the cross products simplify
      // considerably: 100 X [o0 o1 o2] = [  0 -o2  o1]
      //               010 X [o0 o1 o2] = [ o2   0 -o0]
      //               001 X [o0 o1 o2] = [-o1  o0   0] 
      // I don't know of any particularly good ordering for this step.
  
      RealP rb, ro; // Projections of box dimensions on separating vector.
      RealP d;      // Projection of center-center distance on separating vector.
  
      const Mat33P&   R    = R_BO.asMat33();       // abbreviations
      const Vec3P&    p    = p_BO;
      const Mat33P&   Rabs = absR_BO;
  
      // Each of these tests takes about 12 flops.
      rb = h[1] *Rabs(2, 0)+h[2] *Rabs(1, 0);         // b0 X oO
      ro = oh[1]*Rabs(0, 2)+oh[2]*Rabs(0, 1);
      d  = std::abs(p[2]*R(1, 0) - p[1]*R(2, 0)); 
      if (d > rb+ro) return false;
  
      rb = h[1] *Rabs(2, 1)+h[2] *Rabs(1, 1);         // b0 X o1
      ro = oh[0]*Rabs(0, 2)+oh[2]*Rabs(0, 0);
      d  = std::abs(p[2]*R(1, 1) - p[1]*R(2, 1)); 
      if (d > rb+ro) return false;
  
      rb = h[1] *Rabs(2, 2)+h[2] *Rabs(1, 2);         // b0 X o2
      ro = oh[0]*Rabs(0, 1)+oh[1]*Rabs(0, 0);
      d  = std::abs(p[2]*R(1, 2) - p[1]*R(2, 2)); 
      if (d > rb+ro) return false;
  
      rb = h[0] *Rabs(2, 0)+h[2] *Rabs(0, 0);         // b1 X oO
      ro = oh[1]*Rabs(1, 2)+oh[2]*Rabs(1, 1);
      d  = std::abs(p[0]*R(2, 0) - p[2]*R(0, 0)); 
      if (d > rb+ro) return false;
  
      rb = h[0] *Rabs(2, 1)+h[2] *Rabs(0, 1);         // b1 X o1
      ro = oh[0]*Rabs(1, 2)+oh[2]*Rabs(1, 0);
      d  = std::abs(p[0]*R(2, 1) - p[2]*R(0, 1)); 
      if (d > rb+ro) return false;
  
      rb = h[0] *Rabs(2, 2)+h[2] *Rabs(0, 2);         // b1 X o2
      ro = oh[0]*Rabs(1, 1)+oh[1]*Rabs(1, 0);
      d  = std::abs(p[0]*R(2, 2) - p[2]*R(0, 2)); 
      if (d > rb+ro) return false;
  
      rb = h[0] *Rabs(1, 0)+h[1] *Rabs(0, 0);         // b2 X oO
      ro = oh[1]*Rabs(2, 2)+oh[2]*Rabs(2, 1);
      d  = std::abs(p[1]*R(0, 0) - p[0]*R(1, 0)); 
      if (d > rb+ro) return false;
  
      rb = h[0] *Rabs(1, 1)+h[1] *Rabs(0, 1);         // b2 X o1
      ro = oh[0]*Rabs(2, 2)+oh[2]*Rabs(2, 0);
      d  = std::abs(p[1]*R(0, 1) - p[0]*R(1, 1)); 
      if (d > rb+ro) return false;
  
      rb = h[0] *Rabs(1, 2)+h[1] *Rabs(0, 2);         // b2 X o2
      ro = oh[0]*Rabs(2, 1)+oh[1]*Rabs(2, 0);
      d  = std::abs(p[1]*R(0, 2) - p[0]*R(1, 2)); 
      if (d > rb+ro) return false;
  
      return true; // The boxes intersect.
  }
  
  
      
  // Explicit instantiations for float and double.
  template class Geo::Box_<float>;
  template class Geo::Box_<double>;
      
  //==============================================================================
  //                           GEO :: ALIGNED BOX
  //==============================================================================
  // Explicit instantiations for float and double.
  template class Geo::AlignedBox_<float>;
  template class Geo::AlignedBox_<double>;
      
  //==============================================================================
  //                           GEO :: ORIENTED BOX
  //==============================================================================
  
  
  
  // Explicit instantiations for float and double.
  template class Geo::OrientedBox_<float>;
  template class Geo::OrientedBox_<double>;
  
  }  // End of namespace SimTK