void PitchingRobotArm::EasyThrowingBall::throwBall
(const Point3d& throw_point, const Rectangle2D* target_field, MyBall& ball) const
{
if(target_field == NULL){
double vec = XorShift::instance().rand() % 50;
vec = vec / 50.0 + 0.5;
ball.setVector(0.0, 0.0, vec); // ストレートを投げる
}
else{
int field_width = static_cast<int>(target_field->getWidth());
int field_height = static_cast<int>(target_field->getHeight());
cout << "field(" << field_width << ", " << field_height << ")" << endl;
double width = XorShift::instance().rand() % (100 * field_width + 1);
width = width / 100 - static_cast<double>(field_width) / 2;
double height = XorShift::instance().rand() % (100 * field_height + 1);
height = height / 100 - static_cast<double>(field_height) / 2;
double v = XorShift::instance().rand() % 100;
v = v / 500 + 0.7;
cout << "target(" << width << ", " << height << ")" << endl;
Vector3d vec;
vec.x = (throw_point.x - target_field->getPoint().x) - width;
vec.y = (throw_point.y - target_field->getPoint().y) - height;
vec.z = (throw_point.z - target_field->getPoint().z);
vec *= -v / vec.getMagnitude();
double dis = throw_point.z - target_field->getPoint().z;
if(vec.getMagnitude() <= 1.0){
vec.y += 1.0 / 2 * ball.getGravity() * dis / -vec.z;
cout << "vec(" << vec.x << ", " << vec.y << ", " << vec.z << "), ";
cout << "|vec| = " << vec.getMagnitude() << endl;
ball.setVector(vec.x, vec.y, vec.z);
ball.emit(new MyBall::SlowBall());
}
else{
vec.y += 1.0 / 2 * (0.7 * ball.getGravity()) * dis / -vec.z;
cout << "vec(" << vec.x << ", " << vec.y << ", " << vec.z << "), ";
cout << "|vec| = " << vec.getMagnitude() << endl;
ball.setVector(vec.x, vec.y, vec.z);
ball.emit(new MyBall::Straight(0.3 * ball.getGravity()));
}
}
}
std::vector<std::shared_ptr<TestCaseError>> PositionVerificationTest::verify(TestCaseResult &result)
{
//todo find reference to closest plane for error report
bool correct = true;
std::vector<std::shared_ptr<TestCaseError>> errors;
for(int planeIndex = 0; planeIndex < tc->getPlanes().size() && correct; planeIndex++)
{
TestServerPlane testServerPlane = tc->getPlanes()[planeIndex];
Vector3d* closestPosition = nullptr;
bool match = false;
//if there are planes to compare, set closest plane to first one for testing
if(result.getPlanes().size() > 0)
{
closestPosition = new Vector3d(result.getPlanes()[0].getPosition());
}
for (int resultPlaneIndex = 0; resultPlaneIndex < result.getPlanes().size(); resultPlaneIndex++)
{
Vector3d resultPlanePos = result.getPlanes()[resultPlaneIndex].getPosition();
Vector3d distance = getDifference(testServerPlane.getLatLongAlt(), tc->getOwnship().getLatLongAlt()) -
resultPlanePos;
bool correctLat = fabs(distance.x) < THRESHOLD;
bool correctLong = fabs(distance.y) < THRESHOLD;
bool correctAlt = fabs(distance.z) < THRESHOLD;
bool localMatch = correctLat && correctLong && correctAlt;
match |= localMatch;
//if plane is too far away to be a match, but is closest of the 'wrong' planes, report this as an error
if(!localMatch && distance.getMagnitude() < closestPosition->getMagnitude())
{
closestPosition = new Vector3d(distance);
}
}
PositionTestCaseError error = PositionTestCaseError(result.getTime(),testServerPlane.getLatLongAlt(),*closestPosition);
error.setError(!match && closestPosition);
errors.push_back(std::make_shared<PositionTestCaseError>(error));
/*
* if(!match && closestPosition)
{
errors.push_back(std::make_shared<PositionTestCaseError>(PositionTestCaseError(result.getTime(),testServerPlane.getLatLongAlt(),*closestPosition)));
}
*/
}
return errors;
}