int AStar(){
int i;
double tftheta, cost;
double center_x1, center_y1, center_x2, center_y2;
int grid_x, grid_y, grid_theta;
int norm_x, norm_y, norm_theta;
struct node_t temp;
geometry_msgs::Pose pose, tfpose;
// x1,y1: right circle
CalcCenter(¢er_x1, ¢er_y1, ¢er_x2, ¢er_y2);
Astar_count++;
//std::cout << Astar_count << std::endl;
//std::cout << "openlist size: " << openlist.size() << std::endl;
/* calculate tf point */
tftheta = current_theta - origin_yaw;
tftheta = ChangeTheta(tftheta);
current_theta = ChangeTheta(current_theta);
//if (tftheta < 0 || tftheta >= 2*M_PI) {std::cout << "ERROR!!" << std::endl;}
tfpose.position.x = current_x; tfpose.position.y = current_y; tfpose.position.z = 0.0;
tf::Vector3 tfpoint = TransformPoint(tfpose);
/* current grid cell */
norm_x = (int)(tfpoint.getX()*10); norm_y = (int)(tfpoint.getY()*10);
norm_theta = (int)(tftheta/THETA);
/// skip closed?
//if (map[norm_y][norm_x].status == CLOSED){
// openlist.pop();
// if(openlist.empty()) {return 1;}
// temp = openlist.top();
// x = temp.x; y = temp.y; theta = temp.theta;
// return -1;
//}
///
map[norm_y][norm_x][norm_theta].status = CLOSED;
//closelist.push_back(openlist.top()); //is this necessary?
openlist.pop();
/* for visualization */
//posestamped.header.frame_id = PATH_FRAME;
tf::Quaternion quat;
quat.setEuler(0.0,0.0,map[norm_y][norm_x][norm_theta].theta);//setEulerZYX?
pose.orientation.x = quat.getX();
pose.orientation.y = quat.getY();
pose.orientation.z = quat.getZ();
pose.orientation.w = quat.getW();
pose.position.x = map[norm_y][norm_x][norm_theta].x;
pose.position.y = map[norm_y][norm_x][norm_theta].y;
pose.position.z = 0.0;
//posestamped.orientation.w = 1.0;
plan_poses.poses.push_back(pose);
///////////
// search for 6 states //
for (i = 0; i < 6; i++){
switch (i){
case 0:// forward //
//continue;
if (map[norm_y][norm_x][norm_theta].steering == 1) {continue;}
temp.x = current_x + ARC*cos(current_theta);
temp.y = current_y + ARC*sin(current_theta);
temp.theta = current_theta;
cost = ARC*W_STRAIGHT;
break;
case 1:// backward //
//continue;
if (map[norm_y][norm_x][norm_theta].steering == 0) {continue;}
temp.x = current_x + ARC*cos(current_theta+M_PI);
temp.y = current_y + ARC*sin(current_theta+M_PI);
temp.theta = current_theta;
cost = ARC*W_STRAIGHT;
break;
case 2:// left forward //
//continue;
if (map[norm_y][norm_x][norm_theta].steering == 3 ||
map[norm_y][norm_x][norm_theta].steering == 4) {continue;}
temp.x = (ARC/THETA)*cos(current_theta + 3*M_PI/2 + THETA) + center_x2;
temp.y = (ARC/THETA)*sin(current_theta + 3*M_PI/2 + THETA) + center_y2;
temp.theta = current_theta + THETA;
cost = ARC*W_CURVE;
break;
case 3:// left backward //
//continue;
if (map[norm_y][norm_x][norm_theta].steering == 2 ||
map[norm_y][norm_x][norm_theta].steering == 5) {continue;}
temp.x = (ARC/THETA)*cos(current_theta + 3*M_PI/2 - THETA) + center_x2;
temp.y = (ARC/THETA)*sin(current_theta + 3*M_PI/2 - THETA) + center_y2;
temp.theta = current_theta - THETA;
cost = ARC*W_CURVE;
break;
case 4:// right forward //
//continue;
if (map[norm_y][norm_x][norm_theta].steering == 5 ||
map[norm_y][norm_x][norm_theta].steering == 2) {continue;}
temp.x = (ARC/THETA)*cos(current_theta + M_PI/2 - THETA) + center_x1;
temp.y = (ARC/THETA)*sin(current_theta + M_PI/2 - THETA) + center_y1;
temp.theta = current_theta - THETA;
cost = ARC*W_CURVE;
break;
case 5:// right backward //
//continue;
if (map[norm_y][norm_x][norm_theta].steering == 4 ||
map[norm_y][norm_x][norm_theta].steering == 3) {continue;}
temp.x = (ARC/THETA)*cos(current_theta + M_PI/2 + THETA) + center_x1;
temp.y = (ARC/THETA)*sin(current_theta + M_PI/2 + THETA) + center_y1;
temp.theta = current_theta + THETA;
cost = ARC*W_CURVE;
break;
}
// calculate grid cell in each state
tftheta = temp.theta - origin_yaw;
tftheta = ChangeTheta(tftheta);
if (tftheta < 0 || tftheta >= 2*M_PI) {std::cout << "ERROR!!" << std::endl;}///
tfpose.position.x = temp.x; tfpose.position.y = temp.y; tfpose.position.z = 0.0;
tfpoint = TransformPoint(tfpose);
grid_x = (int)(tfpoint.getX()*10); grid_y = (int)(tfpoint.getY()*10);
grid_theta = (int)(tftheta/THETA);
/* process for Astar search */
if (grid_x < 0 || grid_x >= SIZE_X ||
grid_y < 0 || grid_y >= SIZE_Y){
continue;
} else if(Collide(tfpoint.getX(), tfpoint.getY(), tftheta)){
continue;
} else if (map[grid_y][grid_x][grid_theta].goal &&
GoalAngle(temp.theta)){ //current node is GOAL
std::cout << Astar_count << std::endl;
map[grid_y][grid_x][grid_theta].x = temp.x;
map[grid_y][grid_x][grid_theta].y = temp.y;
map[grid_y][grid_x][grid_theta].theta = temp.theta;
map[grid_y][grid_x][grid_theta].parent = &map[norm_y][norm_x][norm_theta];
map[grid_y][grid_x][grid_theta].steering = i;
gx = grid_x; gy = grid_y;
gtheta = grid_theta;
return 0;
} else if (map[grid_y][grid_x][grid_theta].status == OPEN){
if (map[norm_y][norm_x][norm_theta].gc+cost <
map[grid_y][grid_x][grid_theta].gc){
map[grid_y][grid_x][grid_theta].x = temp.x;
map[grid_y][grid_x][grid_theta].y = temp.y;
map[grid_y][grid_x][grid_theta].theta = temp.theta;
map[grid_y][grid_x][grid_theta].gc = map[norm_y][norm_x][norm_theta].gc+cost;
//map[grid_y][grid_x][grid_theta].hc = W_HC * hypot(dgx-temp.x, dgy-temp.y);
map[grid_y][grid_x][grid_theta].parent = &map[norm_y][norm_x][norm_theta];
map[grid_y][grid_x][grid_theta].steering = i;
openlist.push(map[grid_y][grid_x][grid_theta]); // can't erase old node?
}
} else if (map[grid_y][grid_x][grid_theta].status == CLOSED){
if (map[norm_y][norm_x][norm_theta].gc+cost <
map[grid_y][grid_x][grid_theta].gc){
map[grid_y][grid_x][grid_theta].x = temp.x;
map[grid_y][grid_x][grid_theta].y = temp.y;
map[grid_y][grid_x][grid_theta].theta = temp.theta;
map[grid_y][grid_x][grid_theta].gc = map[norm_y][norm_x][norm_theta].gc+cost;
//map[grid_y][grid_x][grid_theta].hc = W_HC * hypot(dgx-temp.x, dgy-temp.y);
map[grid_y][grid_x][grid_theta].parent = &map[norm_y][norm_x][norm_theta];
map[grid_y][grid_x][grid_theta].steering = i;
openlist.push(map[grid_y][grid_x][grid_theta]);
}
} else {
map[grid_y][grid_x][grid_theta].x = temp.x;
map[grid_y][grid_x][grid_theta].y = temp.y;
map[grid_y][grid_x][grid_theta].theta = temp.theta;
map[grid_y][grid_x][grid_theta].status = OPEN;
map[grid_y][grid_x][grid_theta].gc = map[norm_y][norm_x][norm_theta].gc+cost;
//map[grid_y][grid_x][grid_theta].hc = W_HC * hypot(dgx-temp.x, dgy-temp.y);
map[grid_y][grid_x][grid_theta].parent = &map[norm_y][norm_x][norm_theta];
map[grid_y][grid_x][grid_theta].steering = i;
openlist.push(map[grid_y][grid_x][grid_theta]);
}
//std::cout << "openlist size: " << openlist.size() << std::endl;
}
// if opnelist is empty, failed
if (openlist.empty()){
std::cout << "EMPTY!!" << std::endl;
return 1;
}
// set next start point
temp = openlist.top();
current_x = temp.x; current_y = temp.y;
current_theta = temp.theta;
// terminate search
if (Astar_count == SEARCH_LIMIT)
return 1;
return -1;//for loop
}
bool CMine::CalcDeltaLights (double fLightScale, int force, int recursion_depth)
{
// initialize totals
CDSegment *srcseg, *childseg;
int source_segnum, child_segnum;
double effect[4];
GameInfo ().delta_lights.count = 0;
GameInfo ().dl_indices.count = 0;
bool bWall, bD2XLights = (level_version >= 15) && (GameInfo ().fileinfo_version >= 34);
fLightScale = 1.0; ///= 100.0;
for (source_segnum = 0, srcseg = Segments ();
source_segnum < SegCount ();
source_segnum++, srcseg++) {
// skip if not marked unless we are automatically saving
if (!(srcseg->wall_bitmask & MARKED_MASK) && !force)
continue;
// loop on all sides
int source_sidenum;
for (source_sidenum = 0; source_sidenum < 6; source_sidenum++) {
INT16 tmapnum = srcseg->sides [source_sidenum].nBaseTex & 0x3fff;
INT16 tmapnum2 = srcseg->sides [source_sidenum].nOvlTex & 0x3fff;
INT16 trignum;
bool bl1 = (bool) (IsLight (tmapnum) != -1);
bool bl2 = (bool) (IsLight (tmapnum2) != -1);
if (!(bl1 || bl2))
continue; // no lights on this side
bool bCalcDeltas = false;
// if the current side is a wall and has a light and is the target of a trigger
// than can make the wall appear/disappear, calculate delta lights for it
if ((bWall = (FindWall (source_segnum, source_sidenum) != NULL)) &&
((trignum = FindTriggerTarget (0, source_segnum, source_sidenum)) >= 0)) {
INT8 trigtype = Triggers (trignum)->type;
bCalcDeltas =
(trigtype == TT_ILLUSION_OFF) ||
(trigtype == TT_ILLUSION_ON) ||
(trigtype == TT_CLOSE_WALL) ||
(trigtype == TT_OPEN_WALL) ||
(trigtype == TT_LIGHT_OFF) ||
(trigtype == TT_LIGHT_ON);
}
if (!bCalcDeltas)
bCalcDeltas = IsFlickeringLight (source_segnum, source_sidenum);
if (!bCalcDeltas) {
bool bb1 = IsBlastableLight (tmapnum);
bool bb2 = IsBlastableLight (tmapnum2);
if (bb1 == bb2)
bCalcDeltas = bb1; // both lights blastable or not
else if (!(bb1 ? bl2 : bl1)) // i.e. one light blastable and the other texture not a non-blastable light
bCalcDeltas = true;
}
if (!bCalcDeltas) { //check if light is target of a "light on/off" trigger
int trignum = FindTriggerTarget (0, source_segnum, source_sidenum);
if ((trignum >= 0) && (Triggers (trignum)->type >= TT_LIGHT_OFF))
bCalcDeltas = true;
}
if (!bCalcDeltas)
continue;
// only set lights for textures which have a nOvlTex
//if (tmapnum2 == 0)
// continue;
INT16 srcwall = srcseg->sides [source_sidenum].nWall;
if ((srcseg->children [source_sidenum] != -1) &&
((srcwall >= GameInfo ().walls.count) || (Walls (srcwall)->type == WALL_OPEN)))
continue;
// if ((IsLight (tmapnum) == -1) && (IsLight (tmapnum2) == -1))
// continue;
if (GameInfo ().dl_indices.count >= MAX_DL_INDICES) {
char szMsg [256];
sprintf (szMsg, " Light tool: Too many dynamic lights at render depth %d", recursion_depth);
DEBUGMSG (szMsg);
return false;
}
vms_vector A,source_center;
// get index number and increment total number of dl_indices
int dl_index_num = (int)GameInfo ().dl_indices.count++;
dl_index *pdli = DLIndex (dl_index_num);
if (bD2XLights) {
pdli->d2x.segnum = source_segnum;
pdli->d2x.sidenum = source_sidenum;
pdli->d2x.count = 0; // will be incremented below
}
else {
pdli->d2.segnum = source_segnum;
pdli->d2.sidenum = source_sidenum;
pdli->d2.count = 0; // will be incremented below
}
pdli->d2.index = (INT16)GameInfo ().delta_lights.count;
// find orthogonal angle of source segment
CalcOrthoVector(A,source_segnum,source_sidenum);
// remember to flip the sign since we want it to point inward
A.x = -A.x;
A.y = -A.y;
A.z = -A.z;
// calculate the center of the source segment
CalcCenter(source_center,source_segnum,source_sidenum);
// mark those Segments () within N children of current cube
//(note: this is done once per light instead of once per segment
// even though some Segments () have multiple lights.
// This actually reduces the number of calls since most
// Segments () do not have lights)
int h;
for (h = 0; h < SegCount (); h++)
Segments (h)->seg_number = -1;
SetSegmentChildNum (srcseg, source_segnum, recursion_depth);
srcseg->seg_number = recursion_depth;
// setup source corner vertex for length calculation later
vms_vector source_corner[4];
int j;
for (j = 0; j < 4; j++) {
UINT8 vertnum = side_vert[source_sidenum][j];
int h = srcseg->verts[vertnum];
source_corner[j].x = Vertices (h)->x;
source_corner[j].y = Vertices (h)->y;
source_corner[j].z = Vertices (h)->z;
}
// loop on child Segments ()
for (child_segnum = 0, childseg = Segments ();
child_segnum < SegCount ();
child_segnum++, childseg++) {
if (childseg->seg_number < 0)
continue;
// loop on child sides
int child_sidenum;
for (child_sidenum = 0; child_sidenum < 6; child_sidenum++) {
// if texture has a child..
#ifdef _DEBUG
CBRK (source_segnum == 6 && source_sidenum == 2 &&
child_segnum == 10 && child_sidenum == 1);
#endif
if (childseg->children[child_sidenum] >= 0) {
UINT16 nWall = childseg->sides[child_sidenum].nWall;
// .. if there is no wall ..
if (nWall >= GameInfo ().walls.count)
continue;
// .. or its not a door ..
if (Walls (nWall)->type == WALL_OPEN)
continue; // don't put light because there is no texture here
}
// don't affect non-flickering light emitting textures (e.g. lava)
tmapnum = childseg->sides [child_sidenum].nBaseTex;
tmapnum2 = childseg->sides [child_sidenum].nOvlTex & 0x3fff;
if (m_nNoLightDeltas == 1) {
if (((IsLight (tmapnum) >= 0) || (IsLight (tmapnum2) >= 0))
&& !IsFlickeringLight (child_segnum, child_sidenum))
continue;
}
else if ((m_nNoLightDeltas == 2) && (IsLava (tmapnum) || IsLava (tmapnum2)))
continue;
// if the child side is the same as the source side, then set light and continue
if (child_sidenum == source_sidenum && child_segnum == source_segnum) {
if ((GameInfo ().delta_lights.count >= MAX_DELTA_LIGHTS) ||
(bD2XLights ? pdli->d2x.count == 8191 : pdli->d2.count == 255)) {
char szMsg [256];
sprintf (szMsg, " Light tool: Too many dynamic lights at render depth %d", recursion_depth);
DEBUGMSG (szMsg);
return false;
}
delta_light *dl = DeltaLights (GameInfo ().delta_lights.count++);
dl->segnum = child_segnum;
dl->sidenum = child_sidenum;
dl->dummy = 0;
dl->vert_light [0] =
dl->vert_light [1] =
dl->vert_light [2] =
dl->vert_light [3] = (UINT8) min (32, 32 * fLightScale);
if (bD2XLights)
pdli->d2x.count++;
else
pdli->d2.count++;
continue;
}
// calculate vector between center of source segment and center of child
#ifdef _DEBUG
CBRK (child_segnum == qqq1 && child_sidenum == qqq2);
#endif
if (CalcSideLights (child_segnum, child_sidenum, source_center, source_corner, A, effect, fLightScale, bWall)) {
theApp.SetModified (TRUE);
if ((GameInfo ().delta_lights.count >= MAX_DELTA_LIGHTS) ||
(bD2XLights ? pdli->d2x.count == 8191 : pdli->d2.count == 255)) {
char szMsg [256];
sprintf (szMsg, " Light tool: Too many dynamic lights at render depth %d", recursion_depth);
DEBUGMSG (szMsg);
return false;
}
delta_light *dl = DeltaLights (GameInfo ().delta_lights.count++);
dl->segnum = child_segnum;
dl->sidenum = child_sidenum;
dl->dummy = 0;
int iCorner;
for (iCorner = 0; iCorner < 4; iCorner++)
dl->vert_light [iCorner] = (UINT8) min(32, effect [iCorner]);
if (bD2XLights)
pdli->d2x.count++;
else
pdli->d2.count++;
}
}
}
// }
}
}
return true;
}
bool CMine::CalcSideLights (int segnum, int sidenum, vms_vector& source_center,
vms_vector *source_corner, vms_vector& A, double *effect,
double fLightScale, bool bIgnoreAngle)
{
CDSegment *seg = Segments (segnum);
// calculate vector between center of source segment and center of child
vms_vector B,center;
CalcCenter (center,segnum,sidenum);
B.x = center.x - source_center.x;
B.y = center.y - source_center.y;
B.z = center.z - source_center.z;
// calculate angle between vectors (use dot product equation)
if (!bIgnoreAngle) {
double ratio,angle;
double A_dot_B = (double)A.x * (double)B.x
+ (double)A.y * (double)B.y
+ (double)A.z * (double)B.z;
double mag_A = my_sqrt( (double)A.x*(double)A.x
+(double)A.y*(double)A.y
+(double)A.z*(double)A.z);
double mag_B = my_sqrt( (double)B.x*(double)B.x
+(double)B.y*(double)B.y
+(double)B.z*(double)B.z);
if (mag_A == 0 || mag_B == 0)
angle = (200.0 * M_PI)/180.0; // force a failure
else {
ratio = A_dot_B/(mag_A * mag_B);
ratio = ((double)((int)(ratio*1000.0))) / 1000.0;
if (ratio < -1.0 || ratio > (double)1.0)
angle = (199.0 * M_PI)/180.0; // force a failure
else
angle = acos(ratio);
}
// if angle is less than 110 degrees
// then we found a match
if (angle >= (180.0 * M_PI)/180.0)
return false;
}
int i, j;
for (j = 0; j < 4; j++) {
vms_vector corner;
int vertnum = side_vert[sidenum][j];
int h = seg->verts[vertnum];
corner.x = Vertices (h)->x;
corner.y = Vertices (h)->y;
corner.z = Vertices (h)->z;
double length = 20.0 * m_lightRenderDepth;
for (i = 0; i < 4; i++)
length = min (length, CalcLength (source_corner + i, &corner) / F1_0);
length /= 10.0 * m_lightRenderDepth / 6.0; // divide by 1/2 a cubes length so opposite side
// light is recuded by 1/4
effect [j] = 32;
if (length > 1.0)//if (length < 20.0 * m_lightRenderDepth) // (roughly 4 standard cube lengths)
effect [j] /= (length * length);
effect [j] *= fLightScale;
// else
// effect [j] = 0;
}
// if any of the effects are > 0, then increment the
// light for that side
return (effect [0] != 0 || effect [1] != 0 || effect [2] != 0 || effect [3] != 0);
}
void CMine::Illuminate (
INT16 source_segnum,
INT16 source_sidenum,
UINT32 brightness,
double fLightScale,
bool bAll, bool
bCopyTexLights)
{
CDSegment *seg = Segments ();
CDSegment *child_seg;
double effect[4];
// find orthogonal angle of source segment
vms_vector A;
//fLightScale /= 100.0;
CalcOrthoVector (A,source_segnum,source_sidenum);
// remember to flip the sign since we want it to point inward
A.x = -A.x;
A.y = -A.y;
A.z = -A.z;
// calculate the center of the source segment
vms_vector source_center;
CalcCenter (source_center,source_segnum,source_sidenum);
if ((source_segnum == 911) && (source_sidenum == 3))
A = A;
// mark those Segments () within N children of current cube
// set child numbers
//Segments ()[source_segnum].seg_number = m_lightRenderDepth;
int i;
for (i = SegCount (); i; i--, seg++)
seg->seg_number = -1;
SetSegmentChildNum (NULL, source_segnum, m_lightRenderDepth);
CDColor *plc = LightColor (source_segnum, source_sidenum);
if (!plc->index) {
plc->index = 255;
plc->color.r =
plc->color.g =
plc->color.b = 1.0;
}
if (UseTexColors () && bCopyTexLights) {
CDColor *psc = LightColor (source_segnum, source_sidenum, false);
*psc = *plc;
}
seg = Segments (source_segnum);
seg->seg_number = m_lightRenderDepth;
bool bWall = false; //FindWall (source_segnum, source_sidenum) != NULL;
// loop on child Segments ()
int child_segnum;
for (child_segnum=0, child_seg = Segments ();child_segnum<SegCount ();child_segnum++, child_seg++) {
// skip if this is not viewable
if (child_seg->seg_number < 0)
continue;
// skip if not marked
// if (!(bAll || (child_seg->wall_bitmask & MARKED_MASK)))
// continue;
// setup source corner vertex for length calculation later
vms_vector source_corner[4];
int j;
for (j = 0; j < 4; j++) {
int vertnum = side_vert [source_sidenum][j];
int h = seg->verts [vertnum];
source_corner[j].x = Vertices (h)->x;
source_corner[j].y = Vertices (h)->y;
source_corner[j].z = Vertices (h)->z;
}
// loop on child sides
int child_sidenum;
for (child_sidenum = 0; child_sidenum < 6; child_sidenum++) {
// if side has a child..
if (!(bAll || SideIsMarked (child_segnum, child_sidenum)))
continue;
if (child_seg->children[child_sidenum] >= 0) {
UINT16 nWall = child_seg->sides[child_sidenum].nWall;
// .. but there is no wall ..
if (nWall >= GameInfo ().walls.count)
continue;
// .. or its not a door ..
if (Walls (nWall)->type == WALL_OPEN)
continue;
}
// CBRK (psc->index > 0);
// if the child side is the same as the source side, then set light and continue
#ifdef _DEBUG
CBRK (child_segnum == qqq1 && child_sidenum == qqq2);
#endif
if (child_sidenum == source_sidenum && child_segnum == source_segnum) {
uvl *uvlP = child_seg->sides [child_sidenum].uvls;
UINT32 vBr, lBr;
theApp.SetModified (TRUE);
int j;
for (j = 0; j < 4; j++, uvlP++) {
CDColor *pvc = VertexColors (child_seg->verts [side_vert [child_sidenum][j]]);
vBr = (UINT16) uvlP->l;
lBr = (UINT32) (brightness * fLightScale);
BlendColors (plc, pvc, lBr, vBr);
vBr += lBr;
vBr = min (0x8000, vBr);
uvlP->l = (UINT16) vBr;
}
continue;
}
// calculate vector between center of source segment and center of child
// CBRK (child_segnum == 1 && child_sidenum == 2);
if (CalcSideLights (child_segnum, child_sidenum, source_center, source_corner, A, effect, fLightScale, bWall)) {
UINT32 vBr, lBr; //vertex brightness, light brightness
uvl *uvlP = child_seg->sides [child_sidenum].uvls;
theApp.SetModified (TRUE);
int j;
for (j = 0; j < 4; j++, uvlP++) {
CDColor *pvc = VertexColors (child_seg->verts [side_vert [child_sidenum][j]]);
if (child_seg->verts [side_vert [child_sidenum][j]] == 2368)
j = j;
vBr = (UINT16) uvlP->l;
lBr = (UINT16) (brightness * effect [j] / 32);
BlendColors (plc, pvc, lBr, vBr);
vBr += lBr;
vBr = min (0x8000, vBr);
uvlP->l = (UINT16) vBr;
}
}
}
}
}
void ribi::trim::SetWindingHorizontal(std::vector<boost::shared_ptr<Edge>>& edges,const Winding winding)
{
assert(CalcWindingHorizontal(AddConst(edges)) != winding);
switch(winding)
{
case Winding::indeterminate:
{
edges[1]->Reverse();
}
break;
case Winding::clockwise:
{
assert(edges.size() == 3 && "Otherwise I am not sure this will work");
const Coordinat3D center { CalcCenter(edges) };
const int n_edges { static_cast<int>(edges.size()) };
for (int i=0; i!=n_edges; ++i)
{
//Fix winding
if (!IsClockwiseHorizontal(edges[i],center)) { edges[i]->Reverse(); }
assert(IsClockwiseHorizontal(edges[i],center));
//Fix ordering of elements
if (edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo()) continue;
//Try to swap the next element's order
assert(i + 1 < n_edges);
if (edges[i+1]->GetFrom() != edges[i]->GetTo()) { edges[i+1]->Reverse(); }
if (edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo()) continue;
//Swap the next and its next elements
assert(i + 2 < n_edges);
if (edges[i+1]->GetFrom() != edges[i]->GetTo()) { std::swap(edges[i+1],edges[i+2]); }
if (edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo()) continue;
//Try to swap the next element's order
if (edges[i+1]->GetFrom() != edges[i]->GetTo()) { edges[i+1]->Reverse(); }
assert(edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo());
}
}
break;
case Winding::counter_clockwise:
{
assert(edges.size() == 3 && "Otherwise I am not sure this will work");
const Coordinat3D center { CalcCenter(edges) };
const int n_edges { static_cast<int>(edges.size()) };
for (int i=0; i!=n_edges; ++i)
{
//Fix winding
if (IsClockwiseHorizontal(edges[i],center)) { edges[i]->Reverse(); }
assert(!IsClockwiseHorizontal(edges[i],center));
//Fix ordering of elements
if (edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo()) continue;
//Try to swap the next element's order
assert(i + 1 < n_edges);
if (edges[i+1]->GetFrom() != edges[i]->GetTo()) { edges[i+1]->Reverse(); }
if (edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo()) continue;
//Swap the next and its next elements
assert(i + 2 < n_edges);
if (edges[i+1]->GetFrom() != edges[i]->GetTo()) { std::swap(edges[i+1],edges[i+2]); }
if (edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo()) continue;
//Try to swap the next element's order
if (edges[i+1]->GetFrom() != edges[i]->GetTo()) { edges[i+1]->Reverse(); }
assert(edges[(i+1) % n_edges]->GetFrom() == edges[i]->GetTo());
}
}
break;
}
assert(CalcWindingHorizontal(AddConst(edges)) == winding);
}
