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; }