Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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 }