C++ Pose::axisX方法代码示例

void VO2Controller::update(float dt)
{
	updatePath(dt);
	updateObstacles(0);

	float maxVelocity = mover->getMaxVelocity(0);
	// Определяем, как далеко осталось до ближайшего вейпоинта и 
	// выбираем соответствующую стратегию движения
	//Pose::vec targetDir = this->pathDirection();
	//float distance=targetDir.length();
	//const float targetCloseFactor = 0.2f;
	//vec2f size = mover->getUnit()->getOOBB().size();
	//float sphereSize=1.3 * mover->getUnit()->getBoundingSphere().radius;
	float turningSpeed = mover->getMaxVelocity(1);
	float turningRadius = maxVelocity / turningSpeed;
	Pose moverPose = mover->getGlobalPose();
	Pose::vec velocity = Pose::vec::zero();
	Pose::vec preferedDirection = Pose::vec::zero();
	
	float preferedAngle = 0;
	float preferedSpeed = 0;
	bool findMax = true;
	bool turnOnly = false;
	if( pathCurrent != -1 )
	{
		Pose::vec pathDir = pathDirection();
		Pose::vec targ = targetScale * pathDir;
		Pose::vec path = pathErrorScale * pathError();

		if( fabs( pathDir[0] ) < 2 * turningRadius && fabs( pathDir[1] ) < 2*turningRadius)
		{
			float x = pathDir & moverPose.axisX();
			float y = fabs(pathDir & moverPose.axisY()) - turningRadius;
			turnOnly = ( x*x  + y*y < turningRadius*turningRadius );
		}

		preferedDirection = normalise(path + targ);
		preferedAngle = atan2(preferedDirection[1], preferedDirection[0]);	// а не пойти ли нам вон туда
		preferedSpeed = maxVelocity;										// и побыстрее
		MoverVehicle * mv = (MoverVehicle*)mover;
		if(mv->definition->kinematic)
		{
			Pose::vec bodyVelocityLinear = conv(mover->getBody()->GetLinearVelocity());
			float bodyVelocityAngular = mover->getBody()->GetAngularVelocity();
			/*
			velProj = (vel & pathDir) / |pathDir|
			time = |pathDir| / |velProj| = |pathDir| * |pathDir| / (vel & pathDir)
			*/
			//float distanceLeft = bodyVelocityLinear & pathDir.normalise();
			/*
			float arriveTime = pathDir.length_squared() / (bodyVelocityLinear & pathDir);
			float brakeTime = bodyVelocityLinear.length() / mv->definition->acceleration[0];
			if(arriveTime > 0 && arriveTime < brakeTime)
			{
				preferedSpeed = arriveTime * mv->definition->acceleration[0];
				if(preferedSpeed > maxVelocity) 
					preferedSpeed = maxVelocity;
			}*/
		}
	}
	else
	{
		findMax = false;
		preferedSpeed = 0;													// хотелось бы стоять на месте и не двигаться
		preferedAngle = mover->getGlobalPose().orientation;					// и смотреть в ту же сторону		
	}

	float bestRate = 2 * maxVelocity;				// лучший рейтинг, для начала берём самый худший
	Pose::vec bestVelocity = Pose::vec::zero();		// лучшая скорость
	// choose best segment
	
	::rayCast(velocitySpace,preferedAngle,steps,rays);

	/*
	сначала отсечь всё промежутком [0,maxVelocity],
	затем надо перебрать все оставшиеся промежутки, и выбрать
	максимальную границу.
	*/
	std::vector<float> bestRanges;
	if(findMax)
		getMaxRanges(rays, preferedSpeed, bestRanges);
	else
		getMinRanges(rays, 0, bestRanges);
	/*
	затем выбираем наилучшее направление
	*/
	vec2f preferedVelocity = preferedDirection * preferedSpeed;
	for(int i = 0; i < rays.size();i++)
	{
		float angle = rays[i].first;
		float distance = bestRanges[i];
		if( distance < 0 )
			continue;
		Pose::vec vel = distance * Pose::vec(cosf(angle),sinf(angle));
		float rate = vecDistance(vel, preferedVelocity);
		if(distance > 0 && rate < bestRate)
		{
			bestRate = rate;
			bestVelocity = vel;
		}
//.........这里部分代码省略.........

void MoverVehicle::update(float dt)
{
	auto body=getBody();
	Pose pose = getGlobalPose();
	
	vec2f vel = conv(getBody()->GetLinearVelocity());	

	vec2f X = pose.axisX();
	vec2f Y = pose.axisY();
	
	float totalVel = vel.length();
	currentVelocity[0] = vel & X;
	currentVelocity[1] = getBody()->GetAngularVelocity();
	const float strafeScale = 0.9;
	float strafe = strafeScale * (vel & Y);

	if(driver && deviceMode == dmDriver)
		driver->update(dt);
	if(definition->pathProcess)
	{
		float size = this->getMaster()->getBoundingSphere().radius;
		definition->pathProcess->setSize(size);
	}
/*
	if(state == Idle)
	{
		accelerate = 0.f;
		turning = 0.f;
	}
*/
	
	if(definition->kinematic)
	{		
		b2Body * body = getBody();
		b2MassData massData;
		body->GetMassData(&massData);

		float linearDamping = definition->acceleration[0]/(definition->maxVelocity[0] );//*definition->maxVelocity[0]);
		float angularDamping = definition->acceleration[1]/(definition->maxVelocity[1] );//*definition->maxVelocity[1]);
		body->SetLinearDamping(linearDamping);
		body->SetAngularDamping(angularDamping);

		if(true)
		{
			// Using force model
			body->ApplyForce(b2conv(accelerate*definition->acceleration[0] * massData.mass *pose.getDirection()),b2conv(pose.position));
			body->ApplyTorque(turning*definition->acceleration[1] * massData.I);
			// Using impulse model
			//body->ApplyLinearImpulse(b2conv((accelerate/dt)*definition->acceleration[0] * massData.mass *pose.getDirection()),b2conv(pose.position));
			//body->ApplyAngularImpulse((turning/dt)*definition->acceleration[1] * massData.I);
		}
		else
		{
			body->ApplyForce(b2conv(accelerate*definition->acceleration[0] *pose.getDirection()),b2conv(pose.position));
			body->ApplyTorque(turning*definition->acceleration[1]);
		}
		// Comlensate steering - apply friction analog
		body->ApplyLinearImpulse(b2conv(-strafe * Y * massData.mass), b2conv(pose.position));
		if(getState() == Idle)
		{
			//body->ApplyLinearImpulse(b2conv( -currentVelocity[0] * X * massData.mass), b2conv(pose.position));
			//body->ApplyAngularImpulse(-currentVelocity[1] * massData.I);
		}
	}
	else
	{
		body->SetLinearVelocity(b2conv(accelerate*definition->maxVelocity[0]*pose.getDirection()));
		body->SetAngularVelocity(turning*definition->maxVelocity[1]);
	}
}