QPainterPath *getPath(const CLRenderCurve* pCurve, const CLBoundingBox *pBB)
{
QPainterPath *path = new QPainterPath();
const std::vector<CLRenderPoint*>& elements = *pCurve->getListOfCurveElements();
std::vector<CLRenderPoint*>::const_iterator it = elements.begin();
bool first = true;
for (; it != elements.end(); ++it)
{
const CLRenderPoint* current = *it;
const CLRenderCubicBezier* cubic = dynamic_cast<const CLRenderCubicBezier*>(current);
if (first)
{
moveToPoint(*path, current, pBB);
first = false;
continue;
}
if (cubic != NULL)
{
addToPath(*path, cubic, pBB);
}
else
{
addToPath(*path, current, pBB);
}
}
return path;
}
void BaseBug::ccTouchEnded(cocos2d::CCTouch *pTouch, cocos2d::CCEvent *event){
if(!hasDie()){
CCSize s = CCDirector::sharedDirector()->getWinSize();
CCPoint p = convertToNodeSpace(ccp(s.width/2, s.height/2));
moveToPoint(p);
}
}
void BaseBug::onEnter(){
//接收对象, 优先级，ture时阻止其他类的move and end
CCDirector::sharedDirector()->getTouchDispatcher()->addTargetedDelegate(this, CC_MAX_TOUCHES,true);
CCSize s = CCDirector::sharedDirector()->getWinSize();
CCPoint p = ccp(s.width/2, s.height/2);
p = convertToNodeSpace(p);
loaderBug(p);
rotationToPoint(p);
moveToPoint(p);
CCSprite::onEnter();
}
void Monsters::moveToPlayer(Player* p){
int dx = (p->position().x > position().x)? 1:-1;
int dy = (p->position().y > position().y)? 1:-1;
point2D moveX(position().x+dx,position().y);
point2D moveY(position().x, position().y+dy);
if (p->position() == moveX || p->position() == moveY) {
attack(p);
return;
}
if (position().x == p->position().x){
moveToPoint(moveY);
return;
}
if (position().y == p->position().y) {
moveToPoint(moveX);
return;
}
if (moveToPoint(moveX)) {
return;
}
else{
moveToPoint(moveY);
}
}
//-------------------------------------------------------------------------------------
PyObject* Entity::pyMoveToPoint(PyObject_ptr pyDestination, float velocity, PyObject_ptr userData,
int32 faceMovement, int32 moveVertically)
{
Position3D destination;
// 将坐标信息提取出来
script::ScriptVector3::convertPyObjectToVector3(destination, pyDestination);
Py_INCREF(userData);
if(moveToPoint(destination, velocity, userData, faceMovement > 0, moveVertically > 0)){
Py_RETURN_TRUE;
}
Py_RETURN_FALSE;
}
void
Drone::update()
{
assert(ask_ != 0);
/* rotateVelocity(); */
if (current_path_ == 0) {
vel_ *= 0;
}
static bool once = true;
float elpsd = clk_.GetElapsedTime();
if (elpsd >= Drone::kSeekTime && once) {
ask_->resetWeights();
ask_->setAutoTarget();
if (ask_->seek()) {
ask_->buildPath();
delete current_path_;
current_path_ = 0;
current_path_ = new Path;
*current_path_ = ask_->path();
moveToPoint(ask_->startCell()->screen_position);
}
clk_.Reset();
once = false;
} else {
followPath();
}
pos_ += vel_ * speed_;
Entity::update();
}
void Steering::update()
{
if (mSteeringEnabled) {
if (mUpdateNeeded) {
updatePath();
}
auto entity = mAvatar.getEntity();
if (!mPath.empty()) {
const auto& finalDestination = mPath.back();
auto entity3dPosition = entity->getViewPosition();
const WFMath::Point<2> entityPosition(entity3dPosition.x(), entity3dPosition.z());
//First check if we've arrived at our actual destination.
if (WFMath::Distance(WFMath::Point<2>(finalDestination.x(), finalDestination.z()), entityPosition) < 0.1f) {
//We've arrived at our destination. If we're moving we should stop.
if (mLastSentVelocity.isValid() && mLastSentVelocity != WFMath::Vector<2>::ZERO()) {
moveInDirection(WFMath::Vector<2>::ZERO());
}
stopSteering();
} else {
//We should send a move op if we're either not moving, or we've reached a waypoint, or we need to divert a lot.
WFMath::Point<2> nextWaypoint(mPath.front().x(), mPath.front().z());
if (WFMath::Distance(nextWaypoint, entityPosition) < 0.1f) {
mPath.pop_front();
nextWaypoint = WFMath::Point<2>(mPath.front().x(), mPath.front().z());
}
WFMath::Vector<2> velocity = nextWaypoint - entityPosition;
WFMath::Point<2> destination;
velocity.normalize();
if (mPath.size() == 1) {
//if the next waypoint is the destination we should send a "move to position" update to the server, to make sure that we stop when we've arrived.
//otherwise, if there's too much lag, we might end up overshooting our destination and will have to double back
destination = nextWaypoint;
}
//Check if we need to divert in order to avoid colliding.
WFMath::Vector<2> newVelocity;
bool avoiding = mAwareness.avoidObstacles(entityPosition, velocity * mSpeed, newVelocity);
if (avoiding) {
auto newMag = newVelocity.mag();
auto relativeMag = mSpeed / newMag;
velocity = newVelocity;
velocity.normalize();
velocity *= relativeMag;
mUpdateNeeded = true;
}
bool shouldSend = false;
if (velocity.isValid()) {
if (mLastSentVelocity.isValid()) {
//If the entity has stopped, and we're not waiting for confirmation to a movement request we've made, we need to start moving.
if (!entity->isMoving() && !mExpectingServerMovement) {
shouldSend = true;
} else {
auto currentTheta = std::atan2(mLastSentVelocity.y(), mLastSentVelocity.x());
auto newTheta = std::atan2(velocity.y(), velocity.x());
//If we divert too much from where we need to go we must adjust.
if (std::abs(currentTheta - newTheta) > WFMath::numeric_constants<double>::pi() / 20) {
shouldSend = true;
}
}
} else {
//If we've never sent a movement update before we should do that now.
shouldSend = true;
}
}
if (shouldSend) {
//If we're moving to a certain destination and aren't avoiding anything we should tell the server to move to the destination.
if (destination.isValid() && !avoiding) {
moveToPoint(WFMath::Point<3>(destination.x(), entity3dPosition.y(), destination.y()));
} else {
moveInDirection(velocity);
}
}
}
} else {
//We are steering, but the path is empty, which means we can't find any path. If we're moving we should stop movement.
//But we won't stop steering; perhaps we'll find a path later.
if (mLastSentVelocity.isValid() && mLastSentVelocity != WFMath::Vector<2>::ZERO()) {
moveInDirection(WFMath::Vector<2>::ZERO());
}
}
}
}
void stewartGoughNodeNamespace::StewartGoughNode::onSetInstruction(const rexos_module::SetInstructionGoalConstPtr &goal){
REXOS_INFO_STREAM("parsing hardwareStep: " << goal->json);
Json::Reader reader;
Json::Value equipletStepNode;
reader.parse(goal->json, equipletStepNode);
rexos_datatypes::EquipletStep equipletStep(equipletStepNode);
rexos_module::SetInstructionResult result;
result.OID = goal->OID;
rexos_stewart_gough::StewartGoughLocation origin;
// the rotation of the axis of the tangent space against the normal space in radians
double rotationX, rotationY, rotationZ = 0;
// determine the position of the origin and the rotation of the axis
switch(equipletStep.getOriginPlacement().getOriginPlacementType()) {
case rexos_datatypes::OriginPlacement::RELATIVE_TO_IDENTIFIER: {
// set the origin to the result of the lookup
if(equipletStep.getOriginPlacement().getLookupResult().isMember("location") == false) {
throw std::runtime_error("lookup result does not contain location");
} else if(equipletStep.getOriginPlacement().getLookupResult().isMember("rotation") == false) {
throw std::runtime_error("lookup result does not contain rotation");
}
Json::Value location = equipletStep.getOriginPlacement().getLookupResult()["location"];
origin.location.set(location["x"].asDouble(), location["y"].asDouble(), location["z"].asDouble());
origin.location = convertToModuleCoordinate(origin.location);
Json::Value rotation = equipletStep.getOriginPlacement().getLookupResult()["rotation"];
rotationZ = rotation["z"].asDouble();
break;
}
case rexos_datatypes::OriginPlacement::RELATIVE_TO_CURRENT_POSITION: {
// set the origin to the current position of the effector
origin.location.set(stewartGough->getEffectorLocation().location.x, stewartGough->getEffectorLocation().location.y,
stewartGough->getEffectorLocation().location.z);
origin.rotationX = stewartGough->getEffectorLocation().rotationX;
origin.rotationY = stewartGough->getEffectorLocation().rotationY;
origin.rotationZ = stewartGough->getEffectorLocation().rotationZ;
break;
}
case rexos_datatypes::OriginPlacement::RELATIVE_TO_MODULE_ORIGIN: {
// set the origin to the origin of the module (eg set it to 0, 0, 0)
origin.location.set(0, 0, 0);
origin.rotationX = 0;
origin.rotationY = 0;
origin.rotationZ = 0;
break;
}
case rexos_datatypes::OriginPlacement::RELATIVE_TO_EQUIPLET_ORIGIN: {
// set the origin to the origin of the module (eg set it to 0, 0, 0)
origin.location = convertToModuleCoordinate(Vector3(0, 0, 0));
origin.rotationX = 0;
origin.rotationY = 0;
origin.rotationZ = 0;
break;
}
}
// get the vector from the instruction data
Json::Value instructionData = equipletStep.getInstructionData();
if(instructionData.isMember("move") == false) {
throw std::runtime_error("instruction data does not contain move");
}
Json::Value moveCommand = equipletStep.getInstructionData()["move"];
Vector3 offsetVector;
if(moveCommand.isMember("x")) offsetVector.x = moveCommand["x"].asDouble();
else offsetVector.x = stewartGough->getEffectorLocation().location.x;
if(moveCommand.isMember("y")) offsetVector.y = moveCommand["y"].asDouble();
else offsetVector.y = stewartGough->getEffectorLocation().location.y;
if(moveCommand.isMember("z")) offsetVector.z = moveCommand["z"].asDouble();
else offsetVector.z = stewartGough->getEffectorLocation().location.z;
// get the max acceleration
double maxAcceleration;
if(moveCommand.isMember("maxAcceleration") == false) {
REXOS_WARN("move command does not contain maxAcceleration, assuming ");
maxAcceleration = 50.0;
} else {
maxAcceleration = moveCommand["maxAcceleration"].asDouble();
}
// get the rotation from the instruction data
if(instructionData.isMember("rotate") == false) {
throw std::runtime_error("instruction data does not contain rotate");
}
Json::Value rotateCommand = equipletStep.getInstructionData()["rotate"];
double targetRotationX, targetRotationY, targetRotationZ;
if(rotateCommand.isMember("x")) targetRotationX = rotateCommand["x"].asDouble();
else targetRotationX = stewartGough->getEffectorLocation().rotationX;
if(rotateCommand.isMember("y")) targetRotationY = rotateCommand["y"].asDouble();
else targetRotationY = stewartGough->getEffectorLocation().rotationY;
if(rotateCommand.isMember("z")) targetRotationZ = rotateCommand["z"].asDouble();
else targetRotationZ = stewartGough->getEffectorLocation().rotationZ;
// calculate the target vector
Matrix4 rotationMatrix;
rotationMatrix.rotateX(rotationX);
rotationMatrix.rotateY(rotationY);
rotationMatrix.rotateZ(rotationZ);
Vector3 targetVector = origin.location + rotationMatrix * offsetVector;
targetRotationX += origin.rotationX;
targetRotationY += origin.rotationY;
targetRotationZ += origin.rotationZ;
rexos_stewart_gough::StewartGoughLocation targetLocation(targetVector.x, targetVector.y, targetVector.z,
targetRotationX, targetRotationY, targetRotationZ);
REXOS_INFO_STREAM("moving from " << stewartGough->getEffectorLocation() << " to " << targetLocation);
// finally move to point
if(moveToPoint(targetLocation, maxAcceleration)) {
setInstructionActionServer.setSucceeded(result);
} else {
REXOS_WARN("Failed moving to point");
setInstructionActionServer.setAborted(result);
}
}
//Refer findBall.mm in Dropbox/ for flowchart
//TODO: prev_ballx is compared with 320. Replace it by image-width/2 if possible, also same thing in rest of code
//TODO: there are constant 4 passes for finding ball. Replace by default variable possibly?
BallReturns CamControl::findBall(FeatureDetection &fd, HeadMotor &hm) // new findball function
{
if(fd.ballFound()==true) // function to return ball coordinates if found
{
prev_img_flag=1; // flag to check whether prev img has ball in it or not. to be used ltr.
prev_ballx=fd.ballX();
prev_bally=fd.ballY();
float ballx_center=fd.ballX()-(IMAGE_WIDTH/2);
float bally_center=fd.ballY()-(IMAGE_HEIGHT/2);
if(inCentreRect(fd.ballX(), fd.ballY(), CENTRE_RECT_X1, CENTRE_RECT_Y1, CENTRE_RECT_X2, CENTRE_RECT_Y2)) // function which checks whether ball in centre or not
{
//ball is found and it is in centre
pass_counter=0;
prev_img_flag = 0;
return BALLFOUND;
//distance and angle stored in ball.r and ball.theta
}
else // if ball not in centre
{
switch(moveToPoint(fd.ballX(), fd.ballY(), hm)) /*function which controls motor movements and
Returns 0 on success. -2: can't move along x, -4: unable to read, -10: turn left, -20: turn right*/
{
case 0:
case -2:
case -4:
return BALLFINDING;
case -10:
return TURNLEFT;
case -20:
return TURNRIGHT;
}
}
}
else // if ball not found
{
if(prev_img_flag==1)// if ball in prev img then w'll move motors accordingly...
{
prev_img_flag=0;
if(prev_ballx>(IMAGE_WIDTH/2))//for turning right
{
state_of_motion = 0;
}
else//for turning left
{
state_of_motion = 1;
}
moveSearch(hm, 30, 70);
}
else// ball not in prev. img either....
{
if(!hm.ismoving_motor(18))//if motor is not moving
{
moveSearch(hm, 30, 70);
pass_counter++;
if(pass_counter>1)
{
pass_counter=0;
return TURNRIGHT;
}
}
//hm.read_motor(thetaX, thetaY);
}
return BALLFINDING;
}
}
void Robot::makeDecision() {
bool move_random = true;
std::cout << objectStorage.size();
for (auto it = objectStorage.begin(); it != objectStorage.end(); ++it) {
std::cout << "cycle\n";
//make sure that an object isn't our robot
if (*it != this) {
//if an object is another robot
Robot* is_robot = dynamic_cast<Robot*>(*it);
if (is_robot) {
double destination = hypot((*it)->x - x,(*it)->y - y);
std::cout << "destination:" << destination << std::endl;
if (destination < Robot::safeDestForRobots) {
//we're going to shoot
//evaluating energy to shoot
int energy_to_shoot=0;
if (destination <= safeDestForRobots * 3 / 10)
energy_to_shoot = energy * 15 / 100;
else if (destination > safeDestForRobots * 3 / 10 && destination <= safeDestForRobots * 7 / 10)
energy_to_shoot = energy * 10 / 100;
else
energy_to_shoot = energy * 5 / 100;
// if energy_to_shoot is 0, we won't shoot
if (!energy_to_shoot)
continue;
std::cout << "shoot" << energy_to_shoot << std::endl;
//evaluate future position of enemy
double a = pow(Robot::robot_speed, 2) - pow(Bolt::bolt_speed, 2);
double b = 2 * (((*it)->x - x) * (*it)->vx + ((*it)->y - y) * (*it)->vy);
double c = pow(((*it)->x - x), 2) + pow(((*it)->y - y), 2);
double D = b * b - 4 * a * c;
double ans = (-b - sqrt(D)) / (2 * a);
//descending robot's energy
energy -= energy_to_shoot;
//shooting
shoot((*it)->x + (*it)->vx * ans, (*it)->y + (*it)->vy * ans, energy_to_shoot * 2);
vx *= -1;
vy *= -1;
if ((x + vx * 1.0 / FPS) <= 0 || (x + vx * 1.0 / FPS) >= arenaXSize) {
vx *= -1;
}
if ((y + vy * 1.0 / FPS) <= 0 || (y + vy * 1.0 / FPS) >= arenaYSize) {
vy *= -1;
}
move(1.0 / FPS);
move_random = false;
}
}
//if an object is a bolt
else {
double destination = hypot(x - (*it)->x, y - (*it)->y);
if (destination < Robot::safeDestForBolts) {
vx *= -1;
vy *= -1;
if ((x + vx * 1.0 / FPS) <= 0 || (x + vx * 1.0 / FPS) >= arenaXSize) {
vx *= -1;
}
if ((y + vy * 1.0 / FPS) <= 0 || (y + vy * 1.0 / FPS) >= arenaYSize) {
vy *= -1;
}
move(1.0 / FPS);
move_random = false;
}
}
}
}
if (move_random) {
int random_x = (rand() % (int)(arenaXSize - robot_radius)) + robot_radius;
int random_y = (rand() % (int)(arenaYSize - robot_radius)) + robot_radius;
moveToPoint(random_x, random_y);
move(1.0 / FPS);
}
}
