C++ Vector2D::length方法代码示例

Vector2D Knowledge::PredictDestination(Vector2D sourcePos, Vector2D targetPos, double sourceSpeed, Vector2D targetSpeed)
{
    double factor = _wm->var[3] / 250.0;
    if(factor < 0)
    {
        factor = 0;
    }

    double Vm = sourceSpeed;
    double k = Vm / targetSpeed.length();
    double gamaT = targetSpeed.dir().radian();
    Vector2D delta;

    delta = targetPos - sourcePos;
    double landa = atan2(delta.y, delta.x);
    double theta = gamaT - landa;

    if (theta > AngleDeg::PI)
    {
        theta -= 2 * AngleDeg::PI;
    }

    if (theta < - AngleDeg::PI)
    {
        theta += 2 * AngleDeg::PI;
    }

    double dlta = 0;
    if(k != 0 && fabs(sin(theta) / k) < 1)
    {
        dlta = asin(sin(theta)/k);
    }
    // No solution.
    else
    {
        //qDebug() << "Prediction: No solution.";
        return targetPos;//Vector2D::INVALIDATED;
    }

    double tf = factor * (delta.length() / 1000) / (Vm*cos(dlta) - targetSpeed.length() * cos(theta));

    // No solution.
    if(tf < 0)
    {
        //qDebug() << "Prediction: No solution.";
        return targetPos;   //Vector2D(0,0); //INVALIDATED;
    }

    double catchDist = targetSpeed.length() * tf * 1000;
    Vector2D catchDiff(catchDist * cos(gamaT), catchDist * sin(gamaT));
    Vector2D Pred_pos=targetPos + catchDiff;

    //_wm->predict_pos.append(Pred_pos);
    return Pred_pos;
}

DelayCoeff PlaneWave::calcDelayCoeff( const Speaker& speaker, const Angle& ang )
{
    Vector2D diff = speaker.getPos() - position.getCurrentValue();
    float cospsi  = ( ang.getNormal() * diff ) / diff.length();
    float delay   = diff.length() * cospsi;
    
    // factor is determined by angle between speaker normal and direction of the plane wave
    float factor = speaker.getNormal() * ang.getNormal();

    if( factor <= 0.0 )
        return DelayCoeff( 0.0, 0.0 );
    else
        return DelayCoeff( delay, factor * speaker.getCosAlpha() * twonderConf->planeComp );
}

Vector2D force(Vector2D f, double t){


	if(f.x == 0 || f.y == 0)
		return Vector2D(0,0);

	double fireTemp = 1;
	double fireFalloff = 1;

	double r = radius(t);
	double distFromCircle = f.length() - r;

	return f / f.length() * fireTemp * exp(-fireFalloff * distFromCircle);

}

std::vector<RobotState> RobotNeighbours::operator()(RobotState s)
{
    static std::vector<Vector2D> lookUp = accs(constraints.maxAcc);
    std::vector<RobotState> result;


    Vector2D newPos = s.pos.add(s.speed);
    unsigned int newTime = s.elapsedTime + 1;
    Environment currentEnv = env.getAfterTime(newTime);

    if(!currentEnv.isSound(Circle(newPos,constraints.r)))
    {
        return result;
    }

    for(auto dV : lookUp)
    {
        Vector2D newV = s.speed.add(dV);
        if(newV.length() <= constraints.maxV)
        {
            RobotState newState;
            newState.pos = newPos;
            newState.speed = newV;
            newState.elapsedTime = newTime;

            result.push_back(newState);
        }
    }

    return result;
}

void DistanceConstraint::checkInverseCollisions()
{
	if ((NULL == node1_) || (NULL == node2_))
		return;

	if (node1_->isFixed() && node2_->isFixed())
		return;

	float w1 = node1_->getW();
	float w2 = node2_->getW();

	Vector2D p1 = node1_->getNewPosition();
	Vector2D p2 = node2_->getNewPosition();
	Vector2D direction = p1 - p2;
	float distance = direction.length();
	direction = direction.normalVec();
	float t1 = w1 / (w1 + w2) * (distance - distance_) * stiffness_;
	float t2 = w2 / (w1 + w2) * (distance - distance_) * stiffness_;

	Vector2D dp1 = direction.invert() * t1;
	Vector2D dp2 = direction * t2;

	node1_->setNewPosition(p1 + dp1);
	node2_->setNewPosition(p2 + dp2);
}

JET_END_TEST_F

JET_BEGIN_TEST_F(FlipSolver2, Rotation) {
    // Build solver
    auto solver = FlipSolver2::builder()
        .withResolution({10, 10})
        .withDomainSizeX(1.0)
        .makeShared();

    solver->setGravity({0, 0});
    solver->setPressureSolver(nullptr);

    // Build emitter
    auto box = Sphere2::builder()
        .withCenter({0.5, 0.5})
        .withRadius(0.4)
        .makeShared();

    auto emitter = VolumeParticleEmitter2::builder()
        .withSurface(box)
        .withSpacing(1.0 / 20.0)
        .withIsOneShot(true)
        .makeShared();

    solver->setParticleEmitter(emitter);

    Array1<double> r;

    for (Frame frame; frame.index < 360; ++frame) {
        auto x = solver->particleSystemData()->positions();
        auto v = solver->particleSystemData()->velocities();
        r.resize(x.size());
        for (size_t i = 0; i < x.size(); ++i) {
            r[i] = (x[i] - Vector2D(0.5, 0.5)).length();
        }

        solver->update(frame);

        if (frame.index == 0) {
            x = solver->particleSystemData()->positions();
            v = solver->particleSystemData()->velocities();
            for (size_t i = 0; i < x.size(); ++i) {
                Vector2D rp = x[i] - Vector2D(0.5, 0.5);
                v[i].x = rp.y;
                v[i].y = -rp.x;
            }
        } else {
            for (size_t i = 0; i < x.size(); ++i) {
                Vector2D rp = x[i] - Vector2D(0.5, 0.5);
                if (rp.lengthSquared() > 0.0) {
                    double scale = r[i] / rp.length();
                    x[i] = scale * rp + Vector2D(0.5, 0.5);
                }
            }
        }

        saveParticleDataXy(solver->particleSystemData(), frame.index);
    }
}

Vector2D
Vector2D::normalised() const {
    Vector2D tmp = (*this);

    tmp /= tmp.length();

    return tmp;
}

double Vector2D::scalarProduct(const Vector2D &to)const
{
	double result;
	
	result=acos( (to.x*this->x +to.y*this->y)/(to.length()*this->length()) );
	
	return result;
}

double
Vector2D::angle(const Vector2D &v) const {
    double cosine = this->dot(v) / (this->length() * v.length());

    if (cosine > 1.0) {
        cosine = 1.0;
    } else if (cosine < -1.0) {
        cosine = -1.0;
    }

    return acos(cosine);
}

Vector2D Vector2D::normalVec() const
{
	Vector2D val = (*this);
	float len = val.length();
	if (0.0f != len)
	{
		val.x /= len;
		val.y /=len;
	}

	return val;
}

Vector2D Game::getCameraPosition() {
    Vector2D move = currentLevel->getPlayerObject()->getPosition() - oldCameraPosition;
    if (move.length() > cam_radius) {
        oldCameraPosition += move.normalized() * (move.length()-cam_radius);
    }
    
    if (oldCameraPosition.getX() + 320 > currentLevel->getWidth()) {
        oldCameraPosition.setX(currentLevel->getWidth() - 320);
    }
    if (oldCameraPosition.getX() < 320) {
        oldCameraPosition.setX(320);
    }
    if (oldCameraPosition.getY() + 240 > currentLevel->getHeight()) {
        oldCameraPosition.setY(currentLevel->getHeight() - 240);
    }
    if (oldCameraPosition.getY() < 240) {
        oldCameraPosition.setY(240);
    }
    // TODO use Display size etc
    
    return oldCameraPosition;
}

Vector2D	Vector2D :: getRandomVector ( float len )
{
	Vector2D	v;

	for ( ; ; )
	{
		v.x = rnd ();
		v.y = rnd ();

		if ( v.lengthSq () < EPS )
			continue;

		v *= len / v.length ();

		return v;
	}
}

TacticTest::TacticTest(WorldModel *worldmodel, QObject *parent) :
    Tactic("TacticTest", worldmodel, parent)
{
    circularBorder.assign(Vector2D(1500,0),1700/2);
    circularBorderOut.assign(Vector2D(1500,0),2100/2);// a circle may use to push balls with some risks
    circularMid.assign(Vector2D(1500,0),720); // a circle which is between holes and border circle
    hole1.assign(Vector2D(1500,1700/4),250);
    hole2.assign(Vector2D(1500,-1700/4),250);
    Vector2D Cdist = (hole1.center() - circularBorder.center());
    double deltaAngle=1.1*asin(hole1.radius()/(Cdist.length())); // 1.1 is safety factor
    Uangle1=Cdist.dir().radian() + deltaAngle ;
    Dangle1=Uangle1 - 2*deltaAngle;
    Cdist = (hole2.center() - circularBorder.center());
    Uangle2=Cdist.dir().radian() + deltaAngle ;
    Dangle2=Uangle2 - 2*deltaAngle;
    state=0;

}

void Chaser::update()
{
	if (m_playerPos != nullptr)
	{
		Vector2D diff = *m_playerPos - m_position;
		// Check if player is inside Chaser field of vision
		if (diff.length() <= m_viewDistance)
		{
			if (abs(diff.x()) < m_treshold)
			{
				// Player caught in x axis, stop
				m_velocity.x(0);
			}
			else
			{
				if (diff.x() < 0)
				{
					// x speed can be negative so we have to take abs value
					m_velocity.x(-abs(m_xSpeed));
				}
				else if (diff.x() > 0)
				{
					//m_velocity.x(m_xSpeed);
					m_velocity.x(abs(m_xSpeed));
				}
			}
		}
		else
		{
			// Player is outside Chaser's vision camp
			m_velocity.x(0);
		}
		m_velocity.y(m_ySpeed);
		handleMovement(m_velocity);
	}
	handleAnimation();
}

RobotCommand TacticPush2Goal::getCommand()
{
    oppIsValid = wm->ourRobot[0].isValid;//wm->theirRobot.IsValid;// opposite robot
    addData();

    sortData();
    if(!oppIsValid) opp = Vector2D(1000000,1000000);
    opp = wm->ourRobot[0].pos.loc;//wm->theirRobot.position;//wm->ourRobot[8].pos.loc;
    OppIsKhoraak=!circularBorder2.contains(opp);//out of his field
    bool reach=false;
    Avoided=false;
    AllIn=true;
    AnyIn=false;
    AllInCorner=true;
    AllUnAccessible=true;

    RobotCommand rc;
    if(!wm->ourRobot[id].isValid) return rc;
    rc.fin_pos.loc=Vector2D(400,0);//circularBorder.center();
    rc.maxSpeed = 1.2;


    index=-1;
    int tah=balls.size()-1;
    if(tah != -1)
    {
        rc.fin_pos.loc=circularBorder2.nearestpoint(balls.at(tah)->pos.loc);
        rc.fin_pos.dir=(circularBorder.center()-rc.fin_pos.loc).dir().radian();
    }


    FindBall(); // find approtiate ball

    if( index != -1 )
    {
        point2 = balls.at(index)->pos.loc;

        FindHole();

        Vector2D nrstpnt = (circularBorder.center()-point2); //nearest Point
        nrstpnt=nrstpnt.setLength(nrstpnt.length()-circularBorder2.radius());
        diff2 = nrstpnt;
        nrstpnt=point2+nrstpnt;
        //        state2=0;
        switch(state)
        {
        case 0:{ //Go Behind the Object

            vec2goal.setLength(250);
            // qDebug()<<"VEC 2 GOAL LENGTH = " << vec2goal.length();
            rc.maxSpeed=1.3;
            rc.useNav = false;//true;
            rc.isBallObs = false;//true;
            rc.isKickObs = false;//true;
            rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2  - vec2goal,
                                                      rc.maxSpeed,balls.at(index)->vel.loc);//point2  - vec2goal;
            //            wm->kn->PredictDestination()
            int rad = 150+ROBOT_RADIUS;
            Circle2D c(point2,rad);

            rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
            //            if(Avoided) rc.maxSpeed=rc.maxSpeed;
            rc.fin_pos.dir=vec2goal.dir().radian();
//            KeepInField(rc);
            reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100);
            if(reach && !Avoided) state = 1;

        }
            break;
        case 1:{//Push

            rc.useNav = false;
            rc.isBallObs = false;
            rc.isKickObs = false;
            rc.maxSpeed=1.1;
            vec2goal.setLength(100);
            rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2  + vec2goal,
                                                      rc.maxSpeed,balls.at(index)->vel.loc);
            rc.fin_pos.dir=vec2goal.dir().radian();

            KeepInField(rc);
            if(((wm->ourRobot[id].pos.loc-point2).length())>800) state=0;
            if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;


            if(hole1.contains(rc.fin_pos.loc) || hole2.contains(rc.fin_pos.loc))
            {
                //                vec2goal.setLength(100);
                rc.fin_pos.loc=point2  ;//+ vec2goal;
                //                rc.fin_pos.loc=point2;
            }

            //            reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,40);
            //            if(reach)
            //                state2 = 0;
        }
            break;
        }
        int mrgn=200;
        Vector2D dlta;
//.........这里部分代码省略.........