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; } } velocity = bestVelocity; float approachTime=2.0; float time = timeToImpact(); if(time<approachTime && time>0) velocity*=(time/approachTime); const float t = 0.5; // варьируем инертность объекта. mover->directionControl(velocity, turnOnly ? 0.f : velocity.length()); //object->newVelocity=object->velocity*(1.f-t)+velocity*t; }
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]); } }