bresenham.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Maxim Likhachev
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Maxim Likhachev nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Benjamin Cohen */
00036 
00037 #include <sbpl_interface/bresenham.h>
00038 
00039 void get_bresenham3d_parameters(int p1x, int p1y, int p1z, int p2x, int p2y, int p2z, bresenham3d_param_t *params)
00040 {
00041   params->X1=p1x;
00042   params->Y1=p1y;
00043   params->Z1=p1z;
00044   params->X2=p2x;
00045   params->Y2=p2y;
00046   params->Z2=p2z;
00047 
00048   params->XIndex = params->X1;
00049   params->YIndex = params->Y1;
00050   params->ZIndex = params->Z1;
00051 
00052   params->dx = fabs((double)(p2x-p1x));
00053   params->dy = fabs((double)(p2y-p1y));
00054   params->dz = fabs((double)(p2z-p1z));
00055   params->dx2 = params->dx << 1;
00056   params->dy2 = params->dy << 1;
00057   params->dz2 = params->dz << 1;
00058 
00059   //get direction of slope
00060   if((double)(p2x-p1x) < 0)
00061     params->IncX = -1;
00062   else
00063     params->IncX = 1;
00064 
00065   if((double)(p2y-p1y) < 0)
00066     params->IncY = -1;
00067   else
00068     params->IncY = 1;
00069 
00070   if((double)(p2z-p1z) < 0)
00071     params->IncZ = -1;
00072   else
00073     params->IncZ = 1;
00074 
00075   //choose which axis to use as the index
00076   if(params->dx >= params->dy && params->dx >= params->dz)
00077   {
00078     params->UsingXYZIndex = 0;
00079     params->err1 = params->dy2 - params->dx;
00080     params->err2 = params->dz2 - params->dx;
00081   }
00082   else if(params->dy >= params->dx && params->dy >= params->dz)
00083   {
00084     params->UsingXYZIndex = 1;
00085     params->err1 = params->dx2 - params->dy;
00086     params->err2 = params->dz2 - params->dy;
00087   }
00088   else
00089   {
00090     params->UsingXYZIndex = 2;
00091     params->err1 = params->dy2 - params->dz;
00092     params->err2 = params->dx2 - params->dz;
00093   }
00094 }
00095 
00096 void get_current_point3d(bresenham3d_param_t *params, int *x, int *y, int *z)
00097 {
00098   *x = params->XIndex;
00099   *y = params->YIndex;
00100   *z = params->ZIndex;
00101 }
00102 
00103 int get_next_point3d(bresenham3d_param_t *params)
00104 {
00105   //check to see if at end of line
00106   if (params->XIndex == params->X2 && params->YIndex == params->Y2 && params->ZIndex == params->Z2)
00107     return 0;
00108 
00109   if (params->UsingXYZIndex == 0)
00110   {
00111     if (params->err1 > 0) {
00112       params->YIndex += params->IncY;
00113       params->err1 -= params->dx2;
00114     }
00115     if (params->err2 > 0) {
00116       params->ZIndex += params->IncZ;
00117       params->err2 -= params->dx2;
00118     }
00119     params->err1 += params->dy2;
00120     params->err2 += params->dz2;
00121     params->XIndex += params->IncX;
00122   }
00123   else if(params->UsingXYZIndex == 1)
00124   {
00125     if (params->err1 > 0) {
00126       params->XIndex += params->IncX;
00127       params->err1 -= params->dy2;
00128     }
00129     if (params->err2 > 0) {
00130       params->ZIndex += params->IncZ;
00131       params->err2 -= params->dy2;
00132     }
00133     params->err1 += params->dx2;
00134     params->err2 += params->dz2;
00135     params->YIndex += params->IncY;
00136   }
00137   else
00138   {
00139     if (params->err1 > 0) {
00140       params->YIndex += params->IncY;
00141       params->err1 -= params->dz2;
00142     }
00143     if (params->err2 > 0) {
00144       params->XIndex += params->IncX;
00145       params->err2 -= params->dz2;
00146     }
00147     params->err1 += params->dy2;
00148     params->err2 += params->dx2;
00149     params->ZIndex += params->IncZ;
00150   }
00151   return 1;
00152 }


sbpl_interface
Author(s): Gil Jones
autogenerated on Sun Jan 17 2016 12:57:03