public bool IntersectsOtherCarWithFront(Car car) { if (!car.Equals(this)) { //sprawdzanie czy punkt sprawdzajacy jest wewnatrz ramek auta //wystarczy sprawdzic czy przecina sie z ktoras z ramek auta for (int i = 0; i < 2; i++) { Vector2 frameCheck = framePointFL; switch (i) { //case 0: case 1: frameCheck = framePointFR; break; } if (OrientationHelper.Intersection(frameCheck, position, car.framePointFL, car.framePointFR)) { return(true); } else if (OrientationHelper.Intersection(frameCheck, position, car.framePointFR, car.framePointRR)) { return(true); } else if (OrientationHelper.Intersection(frameCheck, position, car.framePointRR, car.framePointRL)) { return(true); } else if (OrientationHelper.Intersection(frameCheck, position, car.framePointRL, car.framePointFL)) { return(true); } } } return(false); }
private int GetCarInterectingLightId(Car car) { //iterate over lights, intersection with: car.framePointF - car.framePointR for (int lightId = 0; lightId < lightList.Count; lightId++) { var light = lightList[lightId]; var lightVector1 = new Vector2(light.x1, light.y1); var lightVector2 = new Vector2(light.x2, light.y2); if (OrientationHelper.Intersection(car.framePointF, car.framePointR, lightVector1, lightVector2)) { return(lightId); } } return(-1); }
public void DoAI(LBMElement[,] tabLBM) { userAcc = 0; userSteer = 0; bool intersectsSmth = false; Vector2 leftSeeker = new Vector2(((float)Math.Cos(rotation - MathHelper.PiOver2)), ((float)Math.Sin(rotation - MathHelper.PiOver2))); Vector2 frontSeeker = new Vector2(((float)Math.Cos(rotation)), ((float)Math.Sin(rotation))); desiredAngle = rotation;//przechowuje sugerowany kierunek ruchu wynikajacy z tabLBM #region following LBM vector map int tx = (int)(framePointF.X / (float)parent.elementSize); int ty = (int)(framePointF.Y / (float)parent.elementSize); //on the border use center car position if (!(tx < (parent.countX - 2) && ty < (parent.countY - 2) && tx > 1 && ty > 1)) { tx = (int)(position.X / (float)parent.elementSize); ty = (int)(position.Y / (float)parent.elementSize); } float vx, vy;//przechowuja kierunek //jako sugestie kierunku bierzemy pole na ktorym auto jest z waga *4 + cztery okoliczne pola if (tx < (parent.countX - 1) && ty < (parent.countY - 1) && tx > 0 && ty > 0) { vx = tabLBM[tx, ty].x * 4f; vy = tabLBM[tx, ty].y * 4f; vx += tabLBM[tx + 1, ty].x; vy += tabLBM[tx + 1, ty].y; vx += tabLBM[tx - 1, ty].x; vy += tabLBM[tx - 1, ty].y; vx += tabLBM[tx, ty + 1].x; vy += tabLBM[tx, ty + 1].y; vx += tabLBM[tx, ty - 1].x; vy += tabLBM[tx, ty - 1].y; } else { vx = tabLBM[tx, ty].x; vy = tabLBM[tx, ty].y; } if ((Math.Abs(vx) > 0.0001f) || (Math.Abs(vy) > 0.0001f)) { //zamiana wektora z gazu na kątowy kierunek float vectorAngle = (float)Math.Atan2(vy, vx); desiredAngle = vectorAngle; //obliczenie roznicy kierunku jazdy oraz kata z gazu float angleDiff = vectorAngle - rotation; angleDiff = GeneralHelper.NormalizeAngleSteering(angleDiff); ////odpowiednie skrecenie kol w aucie if (angleDiff < 0) { //skrecamy w prawo if (angleDiff < -(float)(Math.PI / 4.0)) { userSteer = -1; } else { userSteer = (float)(angleDiff * 4.0 / Math.PI); } } else { if (angleDiff > (float)(Math.PI / 4.0)) { userSteer = 1; } else { userSteer = (float)(angleDiff * 4.0 / Math.PI); } } float desiredSpeed = 1 + (1f - Math.Abs(userSteer)) * (float)(maxSpeed * (aggressiveness * 0.8 + 0.6)); userAcc = (desiredSpeed - velocity) / 2f; userAcc = MathHelper.Clamp(userAcc, -(0.7f + 0.3f * aggressiveness), 0.3f + 0.7f * aggressiveness); } else if (tabLBM[tx, ty].isSource) { userAcc = 1; } //drive to closest road else if (tabLBM[tx, ty].isWall) { intersectsSmth = true; var target = FindClosestNormalCell(tabLBM, position, parent.elementSize); var angle = GeneralHelper.NormalizeAngleSteering((float)Math.Atan2(target.Y - position.Y, target.X - position.X) - rotation); userSteer = MathHelper.Clamp(angle, -(float)(Math.PI / 4.0), (float)(Math.PI / 4.0)); userAcc = MathHelper.Clamp(1f - velocity, 0, 1); return; } #endregion ///////////////////////// Sekcja swiadomosci pasow oraz chodnikow ///////////////////////////////////////// //czyli nie wjezdzanie na chodniki + nie przekraczanie pasow jesli nie ma potrzeby (odpowiednio mocny user steer) z poprzedniej sekcji //kontrola gazem jak jedziemy z duza predkoscia i sie do czegos zblizamy //zalezne od predkosc - 30f to ok 100kmh, hamowanie z 0.8G //jak cos znajdzie sie miedzy wyliczonym punktem a autem to trzeba na maksa hamowac // d = v^2 / a //punktowy vektor rownolegly z przednim zderzakiem #region acceleration control: walkway, traffic lights, other cars + giving way to the right (or left) float dist = velocity * velocity / force_braking; if (velocity < 0) { if (dist > 1f) { dist = 1f; } } dist /= (1f + aggressiveness / 2f); #region inni uczestnicy ruchu { float dist2 = (dist * pixelToMeterRatio) + parent.elementSize; Vector2 posBumperLeftCar = new Vector2();//do wykrywania innych aut Vector2 posBumperRightCar = new Vector2(); Vector2 posBumperFront = new Vector2(); if (velocity > 0.05f) { posBumperLeftCar.X = framePointFL.X + ((float)Math.Cos(rotation) * dist2) + (leftSeeker.X * scale * spriteWidth * 0.1f); posBumperLeftCar.Y = framePointFL.Y + ((float)Math.Sin(rotation) * dist2) + (leftSeeker.Y * scale * spriteWidth * 0.1f); posBumperRightCar.X = framePointFR.X + ((float)Math.Cos(rotation) * dist2) - (leftSeeker.X * scale * spriteWidth * 0.2f); posBumperRightCar.Y = framePointFR.Y + ((float)Math.Sin(rotation) * dist2) - (leftSeeker.Y * scale * spriteWidth * 0.2f); } else { posBumperLeftCar.X = framePointFL.X + ((float)Math.Cos(rotation) * dist2) + (leftSeeker.X * scale * spriteWidth * 0.05f); posBumperLeftCar.Y = framePointFL.Y + ((float)Math.Sin(rotation) * dist2) + (leftSeeker.Y * scale * spriteWidth * 0.05f); posBumperRightCar.X = framePointFR.X + ((float)Math.Cos(rotation) * dist2) - (leftSeeker.X * scale * spriteWidth * 0.1f); posBumperRightCar.Y = framePointFR.Y + ((float)Math.Sin(rotation) * dist2) - (leftSeeker.Y * scale * spriteWidth * 0.1f); } posBumperFront.X = framePointF.X + ((float)Math.Cos(rotation) * dist2); posBumperFront.Y = framePointF.Y + ((float)Math.Sin(rotation) * dist2); Parallel.ForEach(parent.cars, otherCar => { if (userAcc == -1 || otherCar.Equals(this)) { return; } //sprawdzanie czy punkt sprawdzajacy jest wewnatrz ramek auta //wystarczy sprawdzic czy przecina sie z ktoras z ramek auta //sprawdzamy jak nasze i(czujka)+dlugosc auta jest wieksze od roznicy odleglosci aut if ((otherCar.position - this.position).Length() <= (dist + 2 * spriteHeight)) { if ((OrientationHelper.Intersection(posBumperLeftCar, position, otherCar.framePointFL, otherCar.framePointFR)) || (OrientationHelper.Intersection(posBumperLeftCar, position, otherCar.framePointFR, otherCar.framePointRR)) || (OrientationHelper.Intersection(posBumperLeftCar, position, otherCar.framePointRR, otherCar.framePointRL)) || (OrientationHelper.Intersection(posBumperLeftCar, position, otherCar.framePointRL, otherCar.framePointFL)) || (OrientationHelper.Intersection(posBumperRightCar, position, otherCar.framePointFL, otherCar.framePointFR)) || (OrientationHelper.Intersection(posBumperRightCar, position, otherCar.framePointFR, otherCar.framePointRR)) || (OrientationHelper.Intersection(posBumperRightCar, position, otherCar.framePointRR, otherCar.framePointRL)) || (OrientationHelper.Intersection(posBumperRightCar, position, otherCar.framePointRL, otherCar.framePointFL)) || (OrientationHelper.Intersection(posBumperFront, position, otherCar.framePointRR, otherCar.framePointRL))) { intersectsSmth = true; userAcc = (velocity == 0 ? -1 : -velocity); if (this.IntersectsOtherCar(otherCar)) { SeparateCars(this, otherCar); } } #region giving way to the right (or left) else { float revGiveWay = Config.GiveWayToLeft ? -1f : 1f; float relativeAngleOtherCar = revGiveWay * ((float)Math.Atan2(otherCar.position.Y - posBumperFront.Y, otherCar.position.X - posBumperFront.X) - desiredAngle); relativeAngleOtherCar = GeneralHelper.NormalizeAngle(relativeAngleOtherCar); //if it is on the right (left) side of car... if ((relativeAngleOtherCar > 0f) && (relativeAngleOtherCar < Math.PI))//MathHelper.PiOver2)) { float angleRelative = revGiveWay * (desiredAngle - otherCar.rotation); angleRelative = GeneralHelper.NormalizeAngle(angleRelative); float perpendicularity = (float)Math.Sin(angleRelative); float minLengthDiff = Config.GiveWayToLeft ? GeneralHelper.Min((framePointFL - otherCar.framePointFR).Length(), (framePointFL - otherCar.framePointRR).Length()) : GeneralHelper.Min((framePointFR - otherCar.framePointFL).Length(), (framePointFR - otherCar.framePointRL).Length()); float otherCarVel = ((otherCar.velocity < 3f) && (otherCar.velocity > 0.1f)) ? 3f : otherCar.velocity; float addParam = spriteWidth / 2f; if ((otherCarVel * velocity) < 1f) { addParam *= (otherCarVel * velocity); } //Calculating whether other car braking distance is bigger than perpendicular distance to current car if (((otherCarVel * velocity * pixelToMeterRatio / (otherCar.force_braking * 0.7f) + addParam) * perpendicularity) > minLengthDiff) { intersectsSmth = true; var newAcc = (velocity <= 0 ? 0 : -velocity); userAcc = newAcc < userAcc ? newAcc : userAcc; } } } #endregion } }); } #endregion //parametrem bedzie userAcc czy == -1 for (int i = (int)(dist * pixelToMeterRatio) + parent.elementSize; i >= 0; i -= parent.elementSize) { if (userAcc == -1) { break; } Vector2 posBumperLeft = new Vector2(); Vector2 posBumperRight = new Vector2(); Vector2 posBumperFront = new Vector2(); //lewy rog przedniego zderzaka: pozycja + dlugosc + pol szerokosci auta w lewo wzg osi + dlugosc czujki posBumperLeft.X = framePointFL.X + ((float)Math.Cos(rotation) * i); posBumperLeft.Y = framePointFL.Y + ((float)Math.Sin(rotation) * i); posBumperRight.X = framePointFR.X + ((float)Math.Cos(rotation) * i); posBumperRight.Y = framePointFR.Y + ((float)Math.Sin(rotation) * i); posBumperFront.X = framePointF.X + ((float)Math.Cos(rotation) * i); posBumperFront.Y = framePointF.Y + ((float)Math.Sin(rotation) * i); posBumperLeft = GeneralHelper.NormalizeVector(posBumperLeft); posBumperRight = GeneralHelper.NormalizeVector(posBumperRight); #region braking for walkway if (velocity > 5f) { Color retrievedColor = parent.GetColorFromLogicMapAtPoint((int)posBumperLeft.X, (int)posBumperLeft.Y); tx = (int)(posBumperLeft.X / (float)parent.elementSize); ty = (int)(posBumperLeft.Y / (float)parent.elementSize); if (((retrievedColor.A > 254) && (retrievedColor.G > 128)) || (tabLBM[tx, ty].isWall)) { intersectsSmth = true; userAcc = -1; } retrievedColor = parent.GetColorFromLogicMapAtPoint((int)posBumperRight.X, (int)posBumperRight.Y); tx = (int)(posBumperRight.X / (float)parent.elementSize); ty = (int)(posBumperRight.Y / (float)parent.elementSize); if (((retrievedColor.A > 254) && (retrievedColor.G > 128)) || (tabLBM[tx, ty].isWall)) { intersectsSmth = true; userAcc = -1; } } #endregion #region sciany ze swiatlami if (velocity >= -0.1f) { if (parent.lightTabLBM[(int)posBumperLeft.X / parent.elementSize, (int)posBumperLeft.Y / parent.elementSize].isWall) { userAcc = -1; } else if (parent.lightTabLBM[(int)posBumperRight.X / parent.elementSize, (int)posBumperRight.Y / parent.elementSize].isWall) { userAcc = -1; } else if (parent.lightTabLBM[(int)framePointF.X / parent.elementSize, (int)framePointF.Y / parent.elementSize].isWall) { userAcc = -1; } else if (parent.lightTabLBM[(int)framePointFL.X / parent.elementSize, (int)framePointFL.Y / parent.elementSize].isWall) { userAcc = -1; } else if (parent.lightTabLBM[(int)framePointFR.X / parent.elementSize, (int)framePointFR.Y / parent.elementSize].isWall) { userAcc = -1; } } #endregion } #endregion #region turning to avoid crossing lane markings - correction after vector map base //starting from center of front bumper, seek lane marking outwards var maxSeekWidth = (scale * spriteWidth * 0.55f); for (float i = 1f; i <= maxSeekWidth; i += 1f) { Color c = parent.GetColorFromLogicMapAtPoint(framePointF + (leftSeeker * i)); if (c.A > 254 && c.B > 128) { userSteer = 0.75f * (maxSeekWidth - i) / maxSeekWidth; break; } else { c = parent.GetColorFromLogicMapAtPoint(framePointF - (leftSeeker * i)); if (c.A > 254 && c.B > 128) { userSteer = -0.75f * (maxSeekWidth - i) / maxSeekWidth; break; } } } #endregion #region turning to overtake slow cars if (velocity < 4f) { float frontDetector = dist * pixelToMeterRatio + parent.elementSize + 1f; Vector2 frontBumperL = framePointF + frontDetector * frontSeeker + (leftSeeker * (scale * spriteWidth * 0.55f + dist * 3f)); Vector2 frontBumperR = framePointF + frontDetector * frontSeeker - (leftSeeker * (scale * spriteWidth * 0.55f + dist * 3f)); bool frontBumterLintersects = false; bool frontBumterRintersects = false; Parallel.ForEach(parent.cars, otherCar => { if (frontBumterLintersects && frontBumterRintersects) { return; } if (!otherCar.Equals(this) && ((otherCar.position - this.position).Length() <= (2 * spriteHeight))) { if (OrientationHelper.Intersection(frontBumperL, position, otherCar.framePointRL, otherCar.framePointRR) || OrientationHelper.Intersection(frontBumperL, position, otherCar.framePointRL, otherCar.framePointFL) || OrientationHelper.Intersection(frontBumperL, position, otherCar.framePointRR, otherCar.framePointRR)) { frontBumterLintersects = true; } if (OrientationHelper.Intersection(frontBumperR, position, otherCar.framePointRL, otherCar.framePointRR) || OrientationHelper.Intersection(frontBumperR, position, otherCar.framePointRL, otherCar.framePointFL) || OrientationHelper.Intersection(frontBumperR, position, otherCar.framePointRR, otherCar.framePointRR)) { frontBumterRintersects = true; } } }); //jak tylko jeden z nich zostal wykryty to mozna zaczac wymijanie //XOR :) if (frontBumterLintersects ^ frontBumterRintersects) { if (frontBumterLintersects) { userSteer = 1; } else { userSteer = -1; } } } #endregion #region skrecanie - ograniczenie kierunku ruchu jesli zbyt odstaje od pozadanego { //obliczenie roznicy kierunku jazdy oraz kata z gazu float angleDiff = desiredAngle - rotation; while (angleDiff > (float)(Math.PI)) { angleDiff -= (float)(2.0 * Math.PI); } while (angleDiff < -(float)(Math.PI)) { angleDiff += (float)(2.0 * Math.PI); } ////odpowiednie skrecenie kol w aucie if (angleDiff < -(float)(Math.PI / 3.0)) { userSteer = -1; } if (angleDiff > (float)(Math.PI / 3.0)) { userSteer = 1; } } #endregion #region turning to avoid other cars Parallel.ForEach(parent.cars, otherCar => { if (!otherCar.Equals(this) && ((otherCar.position - this.position).Length() <= (2 * spriteHeight))) { Vector2 posBumperLeft = new Vector2(), posBumperRight = new Vector2(); //specialnie poszerzony przedni zderzak by najpierw odbil a potem hamowal for (int i = 0; i < 2; i++) { if (i == 1) { posBumperLeft.X = framePointF.X + ((float)Math.Cos(rotation - MathHelper.PiOver2) * scale * spriteWidth * 0.9f); posBumperLeft.Y = framePointF.Y + ((float)Math.Sin(rotation - MathHelper.PiOver2) * scale * spriteWidth * 0.9f); posBumperRight.X = framePointF.X + ((float)Math.Cos(rotation + MathHelper.PiOver2) * scale * spriteWidth * 0.9f); posBumperRight.Y = framePointF.Y + ((float)Math.Sin(rotation + MathHelper.PiOver2) * scale * spriteWidth * 0.9f); } else { posBumperLeft.X = framePointF.X + ((float)Math.Cos(rotation - MathHelper.PiOver2) * scale * spriteWidth * 0.9f) - (frontSeeker.X * spriteHeight * scale * 0.25f); posBumperLeft.Y = framePointF.Y + ((float)Math.Sin(rotation - MathHelper.PiOver2) * scale * spriteWidth * 0.9f) - (frontSeeker.Y * spriteHeight * scale * 0.25f); posBumperRight.X = framePointF.X + ((float)Math.Cos(rotation + MathHelper.PiOver2) * scale * spriteWidth * 0.9f) - (frontSeeker.X * spriteHeight * scale * 0.25f); posBumperRight.Y = framePointF.Y + ((float)Math.Sin(rotation + MathHelper.PiOver2) * scale * spriteWidth * 0.9f) - (frontSeeker.Y * spriteHeight * scale * 0.25f); } //sprawdzanie czy punkt sprawdzajacy jest wewnatrz ramek auta //wystarczy sprawdzic czy przecina sie z ktoras z ramek auta if ((OrientationHelper.Intersection(posBumperRight, position, otherCar.framePointFL, otherCar.framePointFR)) || (OrientationHelper.Intersection(posBumperRight, position, otherCar.framePointFR, otherCar.framePointRR)) || (OrientationHelper.Intersection(posBumperRight, position, otherCar.framePointRR, otherCar.framePointRL)) || (OrientationHelper.Intersection(posBumperRight, position, otherCar.framePointRL, otherCar.framePointFL))) { userSteer = -1; } else if ((OrientationHelper.Intersection(posBumperLeft, position, otherCar.framePointFL, otherCar.framePointFR)) || (OrientationHelper.Intersection(posBumperLeft, position, otherCar.framePointFR, otherCar.framePointRR)) || (OrientationHelper.Intersection(posBumperLeft, position, otherCar.framePointRR, otherCar.framePointRL)) || (OrientationHelper.Intersection(posBumperLeft, position, otherCar.framePointRL, otherCar.framePointFL))) { userSteer = 1; } } } }); #endregion #region turning to avoid walkways and LBM walls { var startSeekWidth = (scale * spriteWidth * 0.25f); maxSeekWidth = (scale * spriteWidth * 0.75f); var steerToAvoid = 0f; for (float i = startSeekWidth; i <= maxSeekWidth; i += 0.5f) { var frontLeft = framePointF + (leftSeeker * i); var c = parent.GetColorFromLogicMapAtPoint(frontLeft); if (parent.IsWalkway(c) || LBMTypeAtPosition(tabLBM, frontLeft) == LBMNodeType.Wall) { steerToAvoid += 0.75f * (maxSeekWidth - i) / (maxSeekWidth - startSeekWidth); break; } } for (float i = startSeekWidth; i <= maxSeekWidth; i += 0.5f) { var frontRight = framePointF - (leftSeeker * i); var c = parent.GetColorFromLogicMapAtPoint(frontRight); if (parent.IsWalkway(c) || LBMTypeAtPosition(tabLBM, frontRight) == LBMNodeType.Wall) { steerToAvoid -= 0.75f * (maxSeekWidth - i) / (maxSeekWidth - startSeekWidth); break; } } if (steerToAvoid != 0) { userSteer = steerToAvoid; } } #endregion #region skrecanie - nie wyjezdzanie z toru jazdy (poza nim brak danych gdzie jechac //{ // Vector2 posBumperLeft = new Vector2(), posBumperRight = new Vector2(); // //jak najezdza przednim zderzakiem na sciane to zeby odpowiednio skrecil by dalej nie najezdzac // posBumperLeft = framePointFL + (leftSeeker * scale * spriteWidth * 0.25f); // posBumperRight = framePointFR - (leftSeeker * scale * spriteWidth * 0.25f); // Vector2 posBumperLeft2 = posBumperLeft - frontSeeker * scale * spriteWidth * 0.50f; // Vector2 posBumperRight2 = posBumperRight - frontSeeker * scale * spriteWidth * 0.50f; // if ( // ((posBumperLeft.X / parent.elementSize) < parent.countX - 1) && // (posBumperLeft.X > parent.elementSize) && // ((posBumperRight.X / parent.elementSize) < parent.countX - 1) && // (posBumperRight.X > parent.elementSize) && // ((posBumperLeft.Y / parent.elementSize) < parent.countY - 1) && // (posBumperLeft.Y > parent.elementSize) && // ((posBumperRight.Y / parent.elementSize) < parent.countY - 1) && // (posBumperRight.Y > parent.elementSize) // ) // { // if (tabLBM[(int)posBumperLeft.X / parent.elementSize, (int)posBumperLeft.Y / parent.elementSize].isWall) // userSteer = 1;//by odbil w prawo // else if (tabLBM[(int)posBumperRight.X / parent.elementSize, (int)posBumperRight.Y / parent.elementSize].isWall) // userSteer = -1;//by odbil w lewo // } // posBumperLeft = posBumperLeft2; posBumperRight = posBumperRight2; // if ( // ((posBumperLeft.X / parent.elementSize) < parent.countX - 1) && // (posBumperLeft.X > parent.elementSize) && // ((posBumperRight.X / parent.elementSize) < parent.countX - 1) && // (posBumperRight.X > parent.elementSize) && // ((posBumperLeft.Y / parent.elementSize) < parent.countY - 1) && // (posBumperLeft.Y > parent.elementSize) && // ((posBumperRight.Y / parent.elementSize) < parent.countY - 1) && // (posBumperRight.Y > parent.elementSize) // ) // if (tabLBM[(int)posBumperLeft.X / parent.elementSize, (int)posBumperLeft.Y / parent.elementSize].isWall) // userSteer = 1;//by odbil w prawo // else if (tabLBM[(int)posBumperRight.X / parent.elementSize, (int)posBumperRight.Y / parent.elementSize].isWall) // userSteer = -1;//by odbil w lewo //} #endregion //jak jedzie do tylu to zeby skrecil na odwrot... if (velocity < 0) { userSteer *= -1f; } }