---

DistanceCalculator.hpp

Go to the documentation of this file.

//
//       Title           : DistanceCalculator.hpp
//       Project         : D&D Demos
//       Created         : Jan 20, 1997
//       Author          : Ethan Swint, Andrew Spencer
//       Platforms       :
//       Copyright       : Copyright© The University of Texas at Austin, 2002. All rights reserved.
//                 
//          This software and documentation constitute an unpublished work
//          and contain valuable trade secrets and proprietary information
//          belonging to the University. None of the foregoing material may be
//          copied or duplicated or disclosed without the express, written
//          permission of University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
//          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
//          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
//          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
//          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
//          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
//          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
//          University be liable for incidental, special, indirect, direct or
//          consequential damages or loss of profits, interruption of business,
//          or related expenses which may arise from use of software or documentation,
//          including but not limited to those resulting from defects in software
//          and/or documentation, or loss or inaccuracy of data of any kind.
//       Purpose         : Container  to hold obstacle information.
//
//
//----------------------------------------------------------------------------
//
//       $Revisions$
//
//       $Log: DistanceCalculator.hpp,v $
//       Revision 1.8  2007/04/23 06:04:05  jknoll
//       Added #include for Polytope.h
//
//       Revision 1.7  2007/03/30 20:41:27  aspencer
//       Changed obstacle avoidance criteria to use the new node structure as the basis for its modeling.
//
//       Revision 1.6  2006/04/09 20:47:59  mitch
//       moved Dhat from ipp (inline) to cpp file
//
//       Revision 1.5  2005/07/13 20:22:33  aspencer
//       Made three Vector3's member variables so that they wouldn't have to be created every time a node-node distance calculation was performed.
//
//       Revision 1.4  2005/06/23 21:28:43  josh
//       Fixed bugs in CalcDist between two nodes. Changed method signatures in DistanceResults.
//
//       Revision 1.3  2005/06/21 15:54:52  aspencer
//       Changed workcell calcdist function so that it now uses a DistanceCalculator object.
//
//       Revision 1.2  2005/06/20 22:38:33  aspencer
//       Added the distance calculator, and tested it. Accounted for a bug in the SOLID software.
//
//       Revision 1.1  2005/06/15 21:53:16  aspencer
//       Initial Commit
//
//
//

#ifndef DistanceCalculator_h
#define DistanceCalculator_h

#ifdef _MSC_VER
        #pragma warning (disable: 4786)
#endif //_MSC_VER

#include "DistanceResults.hpp"
#include "Node.hpp"
#include "GeometricFunctions.hpp"
#include "Cylisphere.hpp"
#include "Sphere.hpp"
#include "Plane.hpp"
#include "Cone.hpp"
#include "Box.hpp"
#include "Polytope.h"

#include "ForwardKinematics/FKAcceleration.h"
#include <set>

namespace OSCAR {

class DistanceResults;

class DistanceCalculator : public Base {
public:

  DistanceCalculator(const String& name= String(), OSCARError& err=DUMMY_ERROR(noError));

  virtual ~DistanceCalculator();


  virtual bool CalcDist(const Node& from, const Node& to, 
                DistanceResults& results, bool checkSelf = false,
                                                                const std::set<const ModelingObject*>& ignoreSet = std::set<const ModelingObject*>());
                  
  bool CalcDist(const Primitive& from, const Primitive& to,
    Vector3& fromWit, Vector3& toWit,
    Vector3& unitVec, double& dist);


  virtual bool CalcGFunctions(const Node& from, const Node& to, 
    DistanceResults& results, FKVelocity* fromFK, bool checkSelf = false,
                const std::set<const ModelingObject*>& ignoreSet = std::set<const ModelingObject*>());

  virtual bool CalcHFunctions(const Node& from, const Node& to, 
    DistanceResults& results, FKAcceleration* fromFK, bool checkSelf = false,
                const std::set<const ModelingObject*>& ignoreSet = std::set<const ModelingObject*>());

  bool CalcGFunction(const Primitive& from, const Primitive& to,
    DistanceResults& results, FKVelocity* fromFK, unsigned int fromFrame);

  bool CalcHFunction(const Primitive& from, const Primitive& to,
    DistanceResults& results, FKVelocity* fromFK, unsigned int fromFrame);
                
protected:

  unsigned int frameNumber; Matrix tempMat;

  Vector3 wit0, wit1, dHat; //Used as scratch Vector3's when doing calculations.
  Vector3 tempVec3; Vector3 secTempVec3;

  double sphereSphereDist(const Sphere& from, const Sphere& to,
    Vector3& fromWit, Vector3& toWit,
    Vector3& unitVec, double& dist);

  double spherePlaneDist(const Sphere& from, const Plane& to,
    Vector3& fromWit, Vector3& toWit,
    Vector3& unitVec, double& dist);

  double cylisphereCylisphereDist(const Cylisphere& from, const Cylisphere& to,
    Vector3& fromWit, Vector3& toWit,
    Vector3& unitVec, double& dist);

  double cylisphereSphereDist(const Cylisphere& from, const Sphere& to,
    Vector3& fromWit, Vector3& toWit,
    Vector3& unitVec, double& dist);

  double cylispherePlaneDist (const Cylisphere& from, const Plane& to,
    Vector3& fromWit, Vector3& toWit,
    Vector3& unitVec, double& dist,
    double& t1, double& t2);

  //note: the H functions are not complete and not currently functional.

  bool pointPointH(Vector3& robPt, Vector3& obstPt, const Primitive& robPrim, const Primitive& obstPrim,
                                         DistanceResults& results, double robRadius, double obstRadius, FKAcceleration* fromFK, unsigned int robotFrameNumber);

  bool linePointH(Vector3& robPt, Vector3& obstPt, const Primitive& robPrim, const Primitive& obstPrim,
                                         DistanceResults& results, double robRadius, double obstRadius, FKAcceleration* fromFK, unsigned int robotFrameNumber);

  bool lineLineH(Vector3& robPt, Vector3& obstPt, const Primitive& robPrim, const Primitive& obstPrim,
      DistanceResults& results, double robRadius, double obstRadius, FKAcceleration* fromFK, unsigned int robotFrameNumber);

  bool LinePlaneH();
                  
};

}  //namespace OSCAR
#endif //DistanceCalculator_h

RRG Homepage

OSCAR Overview

OSCAR Tutorials

Simulations