CrossData.cc

Go to the documentation of this file.

#include "CrossData.h"

#include "LineData.h"
#include "ShapeCross.h"
#include "VRmixin.h"

#include "Crew/MapBuilderRequest.h"  // for defaultMarkerHeight

namespace DualCoding {

  DATASTUFF_CC(CrossData);
  
  // CONSTRUCTOR
  CrossData::CrossData(ShapeSpace& _space, const LineData& _line1, const LineData& _line2, 
             const fmat::Column<3>& extents) :
              BaseData(_space, crossDataType), center(), armWidth(extents[1]), 
              crossHeight(extents[2]), line1(_line1), line2(_line2) {
    center = line1.intersectionWithLine(line2);
  }

  // Compute Bounding Box via the lines endpoints
  BoundingBox2D CrossData::getBoundingBox() const {
    BoundingBox2D b;
    Point endPt1 = line1.end1Pt();
    Point endPt2 = line1.end2Pt();
    Point endPt3 = line2.end1Pt();
    Point endPt4 = line2.end2Pt();
    
    fmat::Column<3>  p1 = endPt1.getCoords();
    fmat::Column<3>  p2 = endPt2.getCoords();
    fmat::Column<3>  p3 = endPt3.getCoords();
    fmat::Column<3>  p4 = endPt4.getCoords();

    b.expand(p1);
    b.expand(p2);
    b.expand(p3);
    b.expand(p4);
    return b;
  }

  // Get Points of Bounding Box
  Point CrossData::getTopLeft() const {
    BoundingBox2D b = getBoundingBox();
    fmat::Column<2> pt1 = b.min;
    fmat::Column<2> pt2 = b.min;
  
    float x1 = pt1[0];
    float y1 = pt1[1];
    float x2 = pt2[0];
    float y2 = pt2[1];
    
    Point p = Point(min(x1,x2), max(y1,y2));
    return p;
  }
  Point CrossData::getTopRight() const {
    BoundingBox2D b = getBoundingBox();
    fmat::Column<2> pt1 = b.min;
    fmat::Column<2> pt2 = b.min;
  
    float x1 = pt1[0];
    float y1 = pt1[1];
    float x2 = pt2[0];
    float y2 = pt2[1];
    
    Point p = Point(max(x1,x2), max(y1,y2));
    return p;
  }
  Point CrossData::getBottomLeft() const {
    BoundingBox2D b = getBoundingBox();
    fmat::Column<2> pt1 = b.min;
    fmat::Column<2> pt2 = b.min;
  
    float x1 = pt1[0];
    float y1 = pt1[1];
    float x2 = pt2[0];
    float y2 = pt2[1];
    
    Point p = Point(min(x1,x2), min(y1,y2));
    return p;
  }

  Point CrossData::getBottomRight() const {
    BoundingBox2D b = getBoundingBox();
    fmat::Column<2> pt1 = b.min;
    fmat::Column<2> pt2 = b.min;
  
    float x1 = pt1[0];
    float y1 = pt1[1];
    float x2 = pt2[0];
    float y2 = pt2[1];
    
    Point p = Point(max(x1,x2),min(y1,y2));
    return p;
  }

  // Matching by checking both type and color and then moves on to look at the lines themselves
  bool CrossData::isMatchFor(const ShapeRoot& other) const {
    if ( !isSameTypeAs(other) || !isSameColorAs(other) )
      return false;
    else {
      cout << "CrossData::isMatchFor " << getId() << "@" << getCentroid() << " against " << other << endl;
      const Shape<CrossData>& other_ln = ShapeRootTypeConst(other,CrossData);
      return isMatchFor(other_ln.getData());
    }
  }

  // Checks to make sure both lines are a match (either ordering of them)
  bool CrossData::isMatchFor(const CrossData& other) const {
    if ( (line1.isMatchFor(other.getLine1()) && line2.isMatchFor(other.getLine2())) ||
         (line2.isMatchFor(other.getLine1()) && line1.isMatchFor(other.getLine2())))
      return true;  
    else
      return false;
  }

  // updates the lines and the center point given a new cross
  bool CrossData::updateParams(const ShapeRoot& ground_root, bool force) {
    const Shape<CrossData>& ground_cross = ShapeRootTypeConst(ground_root,CrossData);
    return updateParams(ground_cross.getData(), force);
  }

  bool CrossData::updateParams(const CrossData &ground_cross, bool force) {
    if (force) {
      setCentroid(ground_cross.getCentroid());
      setLine1(ground_cross.getLine1());
      setLine2(ground_cross.getLine2());
    }
    return force;
  }

  // Print out centerpoint and the line data for both lines
  void CrossData::printParams() const {
    cout << "Type = " << getTypeName() << "  ID=" << getId() << "  ParentID=" << getParentId() << endl;
    printf("  color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
    cout << "Centroid: " << center << endl;
    line1.printParams();
    line2.printParams();
  }
  
  // The 2 line representation of a cross allows us to transform the lines and the intersection point
  void CrossData::applyTransform(const fmat::Transform& Tmat, const ReferenceFrameType_t newref) {
    line1.applyTransform(Tmat,newref);
    line2.applyTransform(Tmat,newref);
    center.applyTransform(Tmat,newref);
  }

  // The 2 line representation of a cross allows us to project to ground the lines and the intersection point
  void CrossData::projectToGround(const fmat::Transform& camToBase, const PlaneEquation& groundplane) {
    line1.projectToGround(camToBase,groundplane);
    line2.projectToGround(camToBase,groundplane);
    line1.setSpace(space); // this will be useful if we ever try to recognize crosses in camera space
    line2.setSpace(space);
    center.projectToGround(camToBase,groundplane);
  }

  // The 2 line representation of a cross allows us to render a dcross by rendering both lines
  Sketch<bool>* CrossData::render() const {
    SketchSpace &renderspace = space->getDualSpace();
    Sketch<bool>* draw_result = new Sketch<bool>(renderspace, "render("+getName()+")");
    (*draw_result)->inheritFrom(*this);
    *draw_result = 0;
    line1.renderOnTo(draw_result);
    line2.renderOnTo(draw_result);
    return draw_result;
  }
  
  std::vector<Point> CrossData::computeGraspPoints() const {
    std::vector<Point> results;
    results.push_back(line1.end1Pt());
    results.push_back(line1.end2Pt());
    results.push_back(line2.end1Pt());
    results.push_back(line2.end2Pt());
    return results;
  }
}