GameObject* S013010C_Aaron_Smith_Steering::GetClosestObject(Vector2D ahead1, Vector2D ahead2) { GameObject* closestObj = nullptr; obstacles = ObstacleManager::Instance()->GetObstacles(); for (GameObject* obj : obstacles) { //bool collision = CheckLineInstersect(ahead1, ahead2, obj); bool collision = Collisions::Instance()->PointInBox(ahead2, obj->GetAdjustedBoundingBox()); if (collision) { if (closestObj == NULL) closestObj = obj; else { if (GetDistance(mTank->GetCentrePosition(), obj->GetCentralPosition()) < GetDistance(mTank->GetCentrePosition(), closestObj->GetCentralPosition())) closestObj = obj; } } } return closestObj; }
Vector2D S013010C_Aaron_Smith_Steering::ObstacleAvoidance() { bool safe = true; //Vector2D leftAhead1 = mTank->GetLeftAhead1(); //Vector2D leftAhead2 = mTank->GetLeftAhead2(); //Vector2D RightAhead1 = mTank->GetRightAhead1(); //Vector2D RightAhead2 = mTank->GetRightAhead2(); //Vector2D Ahead1 = mTank->GetAhead1(); //Vector2D Ahead2 = mTank->GetAhead2(); Vector2D leftAhead1 = mTank->mLeftFeelers[0]; Vector2D leftAhead2 = mTank->mLeftFeelers[1]; Vector2D RightAhead1 = mTank->mRightFeelers[0]; Vector2D RightAhead2 = mTank->mRightFeelers[1]; Vector2D Ahead1 = mTank->mStraightFeelers[0]; Vector2D Ahead2 = mTank->mStraightFeelers[1]; Vector2D rightSide1 = mTank->mRightSideFeelers[0]; Vector2D rightSide2 = mTank->mRightSideFeelers[1]; Vector2D leftSide1 = mTank->mLeftSideFeelers[0]; Vector2D leftSide2 = mTank->mLeftSideFeelers[1]; double maxAvoidForce = 500; //now find most threatening obstacle /*GameObject* closestObj = GetClosestObject(leftAhead1, leftAhead2);*/ //check left feeler //check straight feeler //check right feeler GameObject* closestLeftObj = GetClosestObject(leftAhead1, leftAhead2); GameObject* closestStraightObj = GetClosestObject(Ahead1, Ahead2); GameObject* closestRightObj = GetClosestObject(RightAhead1, RightAhead2); GameObject* closestLeftSideObj = GetClosestObject(leftSide1, leftSide2); GameObject* closestRightSideObj = GetClosestObject(rightSide1, rightSide2); /* then compare the distance of them to find closest one */ double leftDistance = 999; double straightDistance = 999; double rightDistance = 999; double leftSideDistance = 999; double rightSideDistance = 999; if(closestLeftObj != nullptr) leftDistance = GetDistance(closestLeftObj->GetCentralPosition(), mTank->GetCentrePosition()); if (closestStraightObj != nullptr) straightDistance = GetDistance(closestStraightObj->GetCentralPosition(), mTank->GetCentrePosition()); if (closestRightObj != nullptr) rightDistance = GetDistance(closestRightObj->GetCentralPosition(), mTank->GetCentrePosition()); if (closestLeftSideObj != nullptr) leftSideDistance = GetDistance(closestLeftSideObj->GetCentralPosition(), mTank->GetCentrePosition()); if (closestRightSideObj != nullptr) rightSideDistance = GetDistance(closestRightSideObj->GetCentralPosition(), mTank->GetCentrePosition()); Vector2D avoidForce; Vector2D totalForce = Vector2D(0,0); if (closestLeftObj != nullptr) { avoidForce = leftAhead1 - closestLeftObj->GetCentralPosition(); avoidForce = Vec2DNormalize(avoidForce); avoidForce *= maxAvoidForce; totalForce += avoidForce; } if (closestRightObj != nullptr) { avoidForce = RightAhead1 - closestRightObj->GetCentralPosition(); avoidForce = Vec2DNormalize(avoidForce); avoidForce *= maxAvoidForce; totalForce += avoidForce; } if (closestStraightObj != nullptr) { avoidForce = Ahead1 - closestStraightObj->GetCentralPosition(); avoidForce = Vec2DNormalize(avoidForce); avoidForce *= maxAvoidForce; totalForce += avoidForce; } if (closestLeftSideObj != nullptr) { avoidForce = leftSide1 - closestLeftSideObj->GetCentralPosition(); avoidForce = Vec2DNormalize(avoidForce); avoidForce *= maxAvoidForce; totalForce += avoidForce; } if (closestRightSideObj != nullptr) { avoidForce = rightSide1 - closestRightSideObj->GetCentralPosition(); avoidForce = Vec2DNormalize(avoidForce); avoidForce *= maxAvoidForce; totalForce += avoidForce; } return totalForce; }