Exemple #1
0
GridRayTrace::GridRayTrace(tf::Vector3 start, tf::Vector3 end, const OccupancyGrid& grid_ref)
	: _x_store(), _y_store()
{
	//Transform (x,y) into map frame
	start = grid_ref.toGridFrame(start);
	end = grid_ref.toGridFrame(end);
	
	double x0 = start.getX(), y0 = start.getY();
	double x1 = end.getX(), y1 = end.getY();
	
	//ROS_WARN("start: (%f, %f) -> end: (%f, %f)", x0, y0, x1, y1);
	
	bresenham
		(
			floor(x0), floor(y0), 
			floor(x1), floor(y1), 
			_x_store, _y_store
		);
	
	_cur_idx = 0;
	_max_idx = std::min(_x_store.size(), _y_store.size());
}
int Graph::drawLine( Point p1, Point p2,  float r, float g, float b, int method){ 
  //DPRINT( "drawLine: (%d, %d) and (%d, %d)\n", p1.x, p1.y, p2.x, p2.y);
  if(p1.x == p2.x){ //vertical line
    int y,y_end; 
    if(p1.y <= p2.y){
      y = p1.y;
      y_end = p2.y;
    }else{
      y = p2.y;
      y_end = p1.y;
    }
    for(; y<=y_end; y++)
      drawPixel(p1.x, y, r,g,b);
    return 0;
  }
  else if(p1.y == p2.y){ // horizontal line
    int x, x_end;
    if(p1.x <= p2.x){
      x = p1.x;
      x_end =  p2.x;
    }
    else{
      x = p2.x;
      x_end = p1.x; 
    }
    for(; x <= x_end; x++)
      drawPixel(x, p1.y, r,g,b);
    return 0;
  }

  // all other cases are taken care below
  if(method == BRESENHAM) 
    bresenham(p1, p2, r, g, b);
  else
    dda(p1,p2,r,g,b);
  //
  return 0;
}
void FastLaplaceComper::_generateMesh()
{
    // bake in scaling (i.e., apply before meshing),
    // and optionally bake in rotation
    SimilarityTransform first_xfrm(pt_t(),
                                   m_rotationBaked ? _tgt_xfrm.getRot() : 0,
                                   _tgt_xfrm.getCenter(),
                                   _tgt_xfrm.getScale());

    std::vector<pt_t> xfrmed_source_poly;
    size_t n = _sourcePoly.size();
    for(size_t i = 0; i < n; i++)
    {
        xfrmed_source_poly.push_back(first_xfrm.apply(_sourcePoly[i]));
    }

    std::vector<Pt<gdouble> > source_poly_pts;
    // for each segment of the poly, we want to rasterize it
    for(size_t i = 0; i < n; i++)
    {
        int x0 = cvRound(xfrmed_source_poly[i].x);
        int y0 = cvRound(xfrmed_source_poly[i].y);
        int x1 = cvRound(xfrmed_source_poly[(i+1)%n].x);
        int y1 = cvRound(xfrmed_source_poly[(i+1)%n].y);
        std::vector<Pt<gdouble> > line_pts;
        bresenham(x0, y0, x1, y1, line_pts);

        source_poly_pts.insert(source_poly_pts.end(), line_pts.begin(), line_pts.end());
    }

    source_poly_pts.erase(std::unique(source_poly_pts.begin(), source_poly_pts.end()), source_poly_pts.end());

    source_poly_pts.pop_back();

    if(_cd) delete _cd;

    _cd = new ConstrainedDelaunay(&source_poly_pts[0], (int)source_poly_pts.size(), .84);
}
Exemple #4
0
void SaxsviewMask::Private::setValue(SaxsviewMask *mask, const QPolygonF& p, double value) {
  if (SaxsviewFrameData *d = (SaxsviewFrameData*)(mask->data())) {
    // QPolygonF::toPolygon() internally uses toPoint() rounding, see above.
    QPolygon polygon;
    foreach (QPointF pt, p)
      polygon << QPoint((int)pt.x(), (int)pt.y());

    const QRect r = polygon.boundingRect();

    if (polygon.size() > 2) {
      for (int x = r.x(); x <= r.x() + r.width(); ++x)
        for (int y = r.y(); y <= r.y() + r.height(); ++y)
          if (polygon.containsPoint(QPoint(x, y), Qt::OddEvenFill))
            d->setValue(x, y, value);

    } else
      bresenham(polygon[0].x(), polygon[0].y(),
                polygon[1].x(), polygon[1].y(),
                d, value);

    modified = true;
    mask->plot()->replot();
  }
}
Exemple #5
0
void envire::lineBresenham(const envire::GridBase::Position& p1, const envire::GridBase::Position& p2, std::vector< envire::GridBase::Position >& positions)
{
    Bresenham bresenham(p1, p2);
    
    bresenham.getLine(positions);
}
Exemple #6
0
void Line::update_points() {
    unsigned int size = maximum(abs(_end.x - _start.x), abs(_end.y - _start.y));
    _points.resize(size+1);
    _points[0] = _start;
    
    int from_x = _start.x;
    int from_y = _start.y;
    int x = _end.x;
    int y = _end.y;

    int dx = x - from_x;
    int dy = y - from_y;
    
    if (dx == 0) {
        // special case to prevent division by zero
        int current_x = from_x;
        int current_y = from_y;
        if (dy > 0) {
            unsigned int index = 1;
            current_y++;
            for (; current_y <= y; ++current_y) {
                _points[index++] = Position(current_x, current_y);
            }
        } else if (dy < 0) {
            unsigned int index = 1;
            current_y--;
            for (; current_y >= y; --current_y) {
                _points[index++] = Position(current_x, current_y);
            }
        }
    } else if (dx > 0 && dy >= 0) {
        if ((float)dy / (float)dx < 1.0f) {
            /*
             //       ^y
             //   \   |   /
             //     \ | / X
             // ------|------>x
             //     / | \
             //   /   |   \
             */
            bresenham(from_x, from_y, x, y, Transformation(1,0,0,1), _points);
        } else {
            /*
             //       ^y
             //   \   | X /
             //     \ | /
             // ------|------>x
             //     / | \
             //   /   |   \
             */
            bresenham(from_y, from_x, y, x, Transformation(0,1,1,0), _points);
        }
    } else if (dx < 0 && dy >= 0) {
        if ((float)dy / (float)dx < -1.0f) {
            /*
             //       ^y
             //   \   |   /
             //   X \ | /
             // ------|------>x
             //     / | \
             //   /   |   \
             */
            bresenham(from_y, -from_x, y, -x, Transformation(0,-1,1,0), _points);
        } else {
            /*
             //       ^y
             //   \ X |   /
             //     \ | /
             // ------|------>x
             //     / | \
             //   /   |   \
             */
            bresenham(-from_x, from_y, -x, y, Transformation(-1,0,0,1), _points);
        }
    } else if (dx > 0 && dy < 0) {
        if ((float)dy / (float)dx >= -1.0f) {
            /*
             //       ^y
             //   \   |   /
             //     \ | /
             // ------|------>x
             //     / | \ X
             //   /   |   \
             */
            bresenham(from_x, -from_y, x, -y, Transformation(1,0,0,-1), _points);
        } else {
            /*
             //       ^y
             //   \   |   /
             //     \ | /
             // ------|------>x
             //     / | \
             //   /   | X \
             */
            bresenham(-from_y, from_x, -y, x, Transformation(0,1,-1,0), _points);
        }
    } else { // dx < 0 && dy < 0
        if ((float)dy / (float)dx > 1.0f) {
            /*
             //       ^y
             //   \   |   /
             //     \ | /
             // ------|------>x
             //     / | \
             //   / X |   \
             */
            bresenham(-from_y, -from_x, -y, -x, Transformation(0,-1,-1,0), _points);
        } else {
            /*
             //       ^y
             //   \   |   /
             //     \ | /
             // ------|------>x
             //   X / | \
             //   /   |   \
             */
            bresenham(-from_x, -from_y, -x, -y, Transformation(-1,0,0,-1), _points);
        }
    }
}
void FrameBuffer::drawPolygon(Polygon *polygon) {
    int point = 0;
    int yMax = (polygon->getPointY(0));
    int yMin = (polygon->getPointY(0));
    int nPoint = polygon->getPointCount();

    while (point < nPoint) {
        if (polygon->getPointType(point) == 1) {
          int x1, y1, x2, y2;
          x1 = (polygon->getPointX(point));
          y1 = (polygon->getPointY(point));
          x2 = (polygon->getPointX(point+1));
          y2 = (polygon->getPointY(point+1));

          if (yMax < std::max(y1,y2)) yMax = std::max(y1,y2);
          if (yMin > std::min(y1,y2)) yMin = std::min(y1,y2);

          bresenham(x1, y1, x2, y2, 255, 255, 255, point);

        } else {
          int startN = point;
          int total = 0;
          int endN = point;

          int stop = 1;

          if (polygon->getPointType(point) == 3) {
            ++point;
            ++startN;
          }

          while ( stop && ( point < nPoint ) ) {
            if ( polygon->getPointType(point) != 0 ) {
              stop = 0;
              --point;
            }
            ++point;
            ++total;
          }

          endN = point;
          --point;

          if (total > 1) {
            float to = 0.001;
            float t = 0;

            Point lastPoint = Point((polygon->getPointX(startN)), (polygon->getPointY(startN)), 0);

            Point** bezierArray = (Point **) malloc(sizeof(Point) * total);

            for ( int i = 0; i < total; i++ ) {
              if ( (bezierArray[i] = (Point *) malloc((i + 1) * sizeof(Point))) == NULL ) {
                /* Error */
              } else {
                bezierArray[i][0] = Point((polygon->getPointX(i + startN)), (polygon->getPointY(i + startN)), 0);
              }
            }
            
            while ( t <= 1 ) {
              for ( int j = 1; j < total; j++ ) {
                for ( int i = 1; i < total; i++ ) {
                  if ( j < i + 1 ) {
                    float x = (((float)(1 - t) * bezierArray[i-1][j-1].getX()) + (float)(t * bezierArray[i][j-1].getX()));
                    float y = (((float)(1 - t) * bezierArray[i-1][j-1].getY()) + (float)(t * bezierArray[i][j-1].getY()));

                    bezierArray[i][j] = Point(x, y, 0);
                    
                    if ( ( i == total-1 ) && ( j == total-1 ) ) {
                      bresenham((int)lastPoint.getX(), (int)lastPoint.getY(), (int)bezierArray[i][j].getX(), (int)bezierArray[i][j].getY(), 255, 255, 255, 1);
                      
                      int y1 = (int)lastPoint.getY();
                      int y2 = (int)bezierArray[i][j].getY();
                      lastPoint = Point(bezierArray[i][j].getX(), bezierArray[i][j].getY(), 0);

                      if (yMax < std::max(y1,y2)) yMax = std::max(y1,y2);
                      if (yMin > std::min(y1,y2)) yMin = std::min(y1,y2);
                    }
                  }
                } 
              }
              t += to;
            }
          } else {
            plot((polygon->getPointX(point)), (polygon->getPointY(point)), 255, 255, 255);
          }

        }

        ++point;
    }
    //fillPolygon(polygon, yMin, yMax);

}
Exemple #8
0
bool World::visible(const int x, const int y, const int z,
                    const int from_x, const int from_y, const int from_z) const {
    if (z > from_z) {
        return false;
    }
    
    int dx = x - from_x;
    int dy = y - from_y;
    
    if (dx == 0) {
        // special case to prevent division by zero
        int current_x = from_x;
        int current_y = from_y;
        if (dy > 0) {
            for (; current_y < y; ++current_y) {
                if (!transparent(current_x, current_y, from_z)) {
                    return false;
                }
            }
        } else if (dy < 0) {
            for (; current_y > y; --current_y) {
                if (!transparent(current_x, current_y, from_z)) {
                    return false;
                }
            }
        }
    } else if (dx > 0 && dy >= 0) {
        if ((float)dy / (float)dx < 1.0f) {
            /*
            //       ^y
            //   \   |   /
            //     \ | / X
            // ------|------>x
            //     / | \
            //   /   |   \
            */
            return bresenham(from_x, from_y, x, y, from_z, BresenhamBackTransformation(1,0,0,1));
        } else {
            /*
            //       ^y
            //   \   | X /
            //     \ | /
            // ------|------>x
            //     / | \
            //   /   |   \
            */
            return bresenham(from_y, from_x, y, x, from_z, BresenhamBackTransformation(0,1,1,0));
        }
    } else if (dx < 0 && dy >= 0) {
        if ((float)dy / (float)dx < -1.0f) {
            /*
            //       ^y
            //   \   |   /
            //   X \ | /
            // ------|------>x
            //     / | \
            //   /   |   \
            */
            return bresenham(from_y, -from_x, y, -x, from_z, BresenhamBackTransformation(0,-1,1,0));
        } else {
            /*
            //       ^y
            //   \ X |   /
            //     \ | /
            // ------|------>x
            //     / | \
            //   /   |   \
            */
            return bresenham(-from_x, from_y, -x, y, from_z, BresenhamBackTransformation(-1,0,0,1));
        }
    } else if (dx > 0 && dy < 0) {
        if ((float)dy / (float)dx >= -1.0f) {
            /*
            //       ^y
            //   \   |   /
            //     \ | /
            // ------|------>x
            //     / | \ X
            //   /   |   \
            */
            return bresenham(from_x, -from_y, x, -y, from_z, BresenhamBackTransformation(1,0,0,-1));
        } else {
            /*
            //       ^y
            //   \   |   /
            //     \ | /
            // ------|------>x
            //     / | \
            //   /   | X \
            */
            return bresenham(-from_y, from_x, -y, x, from_z, BresenhamBackTransformation(0,1,-1,0));
        }
    } else { // dx < 0 && dy < 0
        if ((float)dy / (float)dx > 1.0f) {
            /*
            //       ^y
            //   \   |   /
            //     \ | /
            // ------|------>x
            //     / | \
            //   / X |   \
            */
            return bresenham(-from_y, -from_x, -y, -x, from_z, BresenhamBackTransformation(0,-1,-1,0));
        } else {
            /*
            //       ^y
            //   \   |   /
            //     \ | /
            // ------|------>x
            //   X / | \
            //   /   |   \
            */
            return bresenham(-from_x, -from_y, -x, -y, from_z, BresenhamBackTransformation(-1,0,0,-1));
        }
    }
    return true;
}
Exemple #9
0
int main() {
  bresenham(10, 6);
}
Exemple #10
0
void analyze_2d (BinaryData *binary)
{
	printf("Analyze 2D started (%s : %ld bytes)\n", binary->name, binary->size);

	// Pixel
	unsigned char pixel_pos[2];
	unsigned char pixel_last_pos[2];
	unsigned char pixel_color;

	// File Offsets
	long int cur_offset   = 0;
	long int start_offset = 0;
	int first_iteration   = 1;
	int first_curve_point = 1;
	unsigned int c;

	// Frame related
	unsigned int points_in_frame = 0;
	unsigned int points_far = 0;
	double frame_distance = 0.0;
	double frame_distance_square = 0.0;
	double last_distance = -1.0;
	// Distance curve
	double last_curve_progress   = 0.0;
	double last_curve_ecart_type = -1.0;
	double last_curve_moyenne    = -1.0;
	double last_curve_rsd        = -1.0;
	unsigned int points_in_curve = 0;
	double curve_distance_square = 0.0;
	double curve_distance        = 0.0;

	// Colors
	rgb_pixel_t red   = {0,   0,   255, 0};
	rgb_pixel_t blue  = {255, 128, 128, 0};
	rgb_pixel_t green = {128, 255, 128, 0};
	rgb_pixel_t black = {0,   0,   0,   0};

	printf ("     Offset       |    Distance\n");
	pixel_pos[1] = fgetc (binary->handler);
	while ((c = fgetc (binary->handler)) != EOF)
	{
		// Cortesi algorithm
		pixel_pos[0] = pixel_pos[1];
		pixel_pos[1] = c;
		points_in_frame++;
		points_in_curve++;

		// Color computation
		double progress = ((double) cur_offset / binary->size);
		pixel_color = progress * 256;

		// Draw in bitmap
		rgb_pixel_t pixel = {255 - pixel_color, 128 - (pixel_color / 2.0), pixel_color, 0};
		binary_draw_pixel (binary, pixel_pos, pixel);

		// Get the distance from the last point
		if (first_iteration)
		{
			pixel_last_pos[0] = pixel_pos[0];
			pixel_last_pos[1] = pixel_pos[1];
		}

		double distance = get_euclidian_distance (pixel_pos[0], pixel_pos[1], pixel_last_pos[0], pixel_last_pos[1]);
		frame_distance += distance;
		frame_distance_square += pow (distance, 2);
		curve_distance += distance;
		curve_distance_square += pow (distance, 2);

		if (first_iteration)
			last_distance = distance;

		double moyenne        = compute_moyenne (frame_distance, points_in_frame);
		double ecart_type     = compute_ecart_type (moyenne, frame_distance_square, points_in_frame);
		double last_moyenne   = compute_moyenne (distance + last_distance, 2);

		// Draw frames
		if (last_moyenne > ecart_type * (moyenne / last_moyenne))
		{
			long int maxsize = binary->size / 10;
			if (maxsize > 256 * 256)
				maxsize = 256 * 256;

			if (points_far++ > maxsize && frame_distance > 800000.0)
			{
				printf ("%.8lx-%.8lx | %f\n", start_offset, cur_offset, frame_distance);
				binary_save (binary, start_offset, cur_offset);

				bresenham (binary,
					 (int)(progress * CURVE_WIDTH), CURVE_HEIGHT,
					 (int)(progress * CURVE_WIDTH), 0,
					black
				);

				points_in_frame = 0;
				frame_distance  = 0.0;
				frame_distance_square = 0.0;
				points_far = 0;
				start_offset = cur_offset;
			}
		}

		// Draw distance curve
		if ((int)(CURVE_WIDTH * progress) != (int)(CURVE_WIDTH * last_curve_progress))
		{
			double curve_moyenne    = compute_moyenne (curve_distance, points_in_curve);
			double curve_ecart_type = compute_ecart_type (curve_moyenne, curve_distance_square, points_in_curve);
			double curve_rsd        = compute_rsd (curve_ecart_type, curve_moyenne) * (CURVE_HEIGHT / 2.0);

			if (first_curve_point)
			{
				last_curve_ecart_type = curve_ecart_type;
				last_curve_moyenne    = curve_moyenne;
				last_curve_rsd        = curve_rsd;
			}

			bresenham (binary,
				 (int)(last_curve_progress * CURVE_WIDTH), (int)(CURVE_HEIGHT - last_curve_ecart_type - 1),
				 (int)(progress            * CURVE_WIDTH), (int)(CURVE_HEIGHT - curve_ecart_type - 1),
				red
			);

			bresenham (binary,
				 (int)(last_curve_progress * CURVE_WIDTH), (int)(CURVE_HEIGHT - last_curve_moyenne - 1),
				 (int)(progress            * CURVE_WIDTH), (int)(CURVE_HEIGHT - curve_moyenne - 1),
				blue
			);

			bresenham (binary,
				 (int)(last_curve_progress * CURVE_WIDTH), (int)(CURVE_HEIGHT - last_curve_rsd - 1),
				 (int)(progress            * CURVE_WIDTH), (int)(CURVE_HEIGHT - curve_rsd - 1),
				green
			);

			last_curve_ecart_type = curve_ecart_type;
			last_curve_moyenne    = curve_moyenne;
			last_curve_rsd        = curve_rsd;
			last_curve_progress   = progress;

			points_in_curve = 0;
			curve_distance  = 0.0;
			curve_distance_square = 0.0;
			first_curve_point = 0;
		}

		// Final computation before next iteration
		pixel_last_pos[0] = pixel_pos[0];
		pixel_last_pos[1] = pixel_pos[1];
		cur_offset++;
		last_distance = distance;
		first_iteration = 0;
	}

	if (points_in_frame > 0)
		binary_save (binary, start_offset, cur_offset);

	bmp_save_distance_curve (binary);
}
void ComponentWallConstruction::buildingPhase()
{
	parent->position = graphicsEngine->getMousePositionOnGround();
	Vector2d pos = aiAux->getFrame(parent->position);
	lastPosition = aiAux->getFrameCenter(pos.y,pos.x);
	bresenham();

	begin->setRotation((lastPosition-firstPosition).getAngle());
	end->setPosition(lastPosition);
	end->setRotation((firstPosition-lastPosition).getAngle());

	if(aiAux->checkMap(aiAux->getFrame(lastPosition).y,aiAux->getFrame(lastPosition).x) == 'W' || lastPosition.getSqrDistanceFrom(Vector2d(900,0)) < 122500)
	{
		canBuild = false;
	}
	else
	{
		canBuild = true;
	}
	float distance = firstPosition.getDistanceFrom(lastPosition);
	Vector2d center = lastPosition+((firstPosition-lastPosition).normalize() * distance/2);
		
	float scale = (distance-10)/15;
	if(scale>0)
	{
		wall->setPosition(center);
		wall->setScale(Vector3d(scale,1,1));
		wall->setRotation((lastPosition-firstPosition).getAngle());
	}
	else
	{
		wall->setPosition(Vector2d(5000,5000));
	}

	if(firstPosition == lastPosition)
	{
		canBuild = false;
	}

	if(canBuild)
	{
		Vector2d direction = firstPosition - lastPosition;
		direction.normalize();
		direction *= 5;
		Vector2d downright = (firstPosition+direction*4/5*-1).rotateBy(90,firstPosition+direction);
		Vector2d downleft = (firstPosition+direction*4/5*-1).rotateBy(-90,firstPosition+direction);
		Vector2d upright = (lastPosition+direction*4/5).rotateBy(-90,lastPosition-direction);
		Vector2d upleft = (lastPosition+direction*4/5).rotateBy(90,lastPosition-direction);
		/*if(!GameManager::getInstance()->isServer())
		{
			GameManager::getInstance()->getGraphicsEngine()->drawDebugLine(upright.asVector3d(),downright.asVector3d());
			GameManager::getInstance()->getGraphicsEngine()->drawDebugLine(downright.asVector3d(),downleft.asVector3d());
			GameManager::getInstance()->getGraphicsEngine()->drawDebugLine(downleft.asVector3d(),upleft.asVector3d());
			GameManager::getInstance()->getGraphicsEngine()->drawDebugLine(upleft.asVector3d(),upright.asVector3d());
		}*/
		std::vector<GameObject*> list = GameManager::getInstance()->getCollisionManager()->getGameObjectBetween(upright, upleft, downright, downleft);
		if(list.size() > 0)
		{
			canBuild = false;
		}
	}

	if(canBuild)
	{
		begin->setColor(2);
		end->setColor(2);
	}
	else
	{
		begin->setColor(0);
		end->setColor(0);
	}
	Vector2d textPosition = Vector2d(30,-30) + lastPosition;
	priceToShow->setText((std::to_wstring(price*distance)).c_str());
	priceToShow->setPosition(textPosition);

	if(GameManager::getInstance()->getEventManager()->mouseState.leftButtonDown)
	{
		if(canBuild)
		{
			Message message;
			message.type = Message::TRY_BUY;
			message.value = price*distance;
			message.gameObject = parent;
			GameManager::getInstance()->getGameObjectManager()->getMainPlayer()->broadcastMessage(message);
		}	
	}
}
int bresenham(int rs, int cs, int re, int ce, int* rr, int* cc)
{
    //
    // [1]中针对0<m<1的情形给出了讲解,由此可总结出一般的步骤:
    //
    // 1.当|m|<1时
    //   决策变量满足
    //      _
    //     /  p0   = IncreY*2*Dy - IncreX*Dx                  k = 0
    //    |
    //     \_ pk+1 = pk + (2*Dy*Sx - 2*Dx*Sy)*IncreX*IncreY   k > 0
    //
    //   其中,IncreX - 自己推导后引入的控制因子,增加时取1,否则-1
    //         IncreY - 自己推导后引入的控制因子,y增加时取1,否则-1
    //         Sx - 在x方向上的步进量,{1,-1}
    //         Sy - 在y方向上的步进量,{0,IncreY}
    //   于是有:
    //   若pk < 0,Sy = 0;否则Sy = IncreY
    //   下一个待画点总是(xk+Sx, yk+Sy),且pk总满足上面的递归式
    //   进行Dx次,总共求出Dx+1个“点对”
    //
    // 2.当|m|>1时,由对称性,只需将1.中所有的x,y反过来。
    //
    // 注意!!
    // x对应列(col),y对应行(row)!!
    //
    int p = 0;
    int IncreX = 0, IncreY = 0;
    int Sx = 0, Sy = 0;
    int Dx, Dy;
    Dx = ce - cs;
    Dy = re - rs;
#ifdef _DEBUG
    std::cout << "x0 = " << cs << ", "
              << "y0 = " << rs << std::endl;
    std::cout << "x1 = " << ce << ", "
              << "y1 = " << re << std::endl;
    std::cout << "Dx = " << Dx << ", "
              << "Dy = " << Dy << std::endl;
#endif // _DEBUG

    if (my::abs(Dy) <= my::abs(Dx)) { // |m| <= 1
        if (ce > cs) Sx = 1, IncreX = 1;
        else Sx = -1, IncreX = -1;
        if (re > rs) IncreY = 1;
        else IncreY = -1;
        int totalLen = my::abs(Dx) + 1;

#ifdef _DEBUG
        std::cout << "Sx = " << Sx << std::endl;
        std::cout << "Incre = " << IncreX << std::endl;
        std::cout << std::endl;
#endif // _DEBUG

        for (int i = 0; i < totalLen; ++i ) {
            if (i == 0) {
                *(cc+i) = cs;
                *(rr+i) = rs;
                p = IncreY*2*Dy - IncreX*Dx;
            }
            else { // i > 0
                if (p < 0) Sy = 0;
                else Sy = IncreY;
                *(cc+i) = *(cc+i-1) + Sx;
                *(rr+i) = *(rr+i-1) + Sy;
                p += (2*Dy*Sx - 2*Dx*Sy)*IncreX*IncreY;
            }

#ifdef _DEBUG
            std::cout << "(" << *(cc+i) << ", " << *(rr+i) << ") " << std::endl;
            std::cout << "p = " << p <<  ", " << "Sy = " << Sy << std::endl;
            std::cout << std::endl;
#endif // _DEBUG

        }
        return totalLen;
    }
    else { // |m| > 1 :只需反转所有的x,y
        return bresenham(cs, rs, ce, re, cc, rr);
    }


}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs,
                 const mxArray *prhs[])
{
  int len;  
  double rs, cs, re, ce;
  int *rr, *cc;
  double *p;
  int i;
  int row, col;
    
  /* Check for proper number of arguments. */
  if (nrhs != 5 || nlhs > 1) {
    mexErrMsgTxt("SegInMat::Invalid calling method...please type help LineTwoPntsInMat in"                  "command for help\n");
  } 
  rs = mxGetScalar(prhs[1]);
  cs = mxGetScalar(prhs[2]);
  re = mxGetScalar(prhs[3]);
  ce = mxGetScalar(prhs[4]);
  /* 获取所得线段的长度 */
  len = bresenham_len((int)rs, (int)cs, (int)re, (int)ce);
  rr = (int*)mxCalloc(len, sizeof(int));
  cc = (int*)mxCalloc(len, sizeof(int));
  
  
  /* 求线段上所有点 */
  bresenham((int)rs, (int)cs, (int)re, (int)ce, rr, cc);
  
  /* 
   * 将对应点上的值填入输出.
   * 注意!!Matlab中矩阵按列存储,下标从1开始。C中下标从0开始
   */
  
  int nrow = mxGetM(prhs[0]);
  int ncol = mxGetN(prhs[0]);

  // 注意:检查行列是否在范围内
  if ( rs > nrow || rs < 1
    || re > nrow || re < 1
    || cs > ncol || cs < 1
    || ce > ncol || ce < 1) 
  {
    mexErrMsgTxt("SegInMat::Input point out of range...\n");
  }
  else 
  {
    plhs[0] = mxCreateDoubleMatrix(1, len, mxREAL);
    p = mxGetPr(plhs[0]);
  }

  
  if (mxIsDouble(prhs[0])) {
    double* pmat = (double* )mxGetPr(prhs[0]);
    for (i = 0; i < len; ++i) {
      row = *(rr+i);
      col = *(cc+i);
      *(p+i) = (double)(*(pmat + (col-1)*nrow + row - 1));
    }
    return;
  }
  else {
    mexErrMsgTxt("SegInMat::Only supports double matrix\n" 
      "Try forced casting:\n"
      "SegInMat(double(mat),rs,cs, re,ce)\n"
      "Or use:\n"
      "  [rr,cc] = LineTwoPnts(rs,ce, re,ce);\n"
      "  idx     = sub2ind(rr,cc);\n"
      "  elems   = mat(idx);\n"
      "as alternative.");
  }
  
  

}