float ADynamicCarController::getBrakeDistance() const
{
float velocityLength = to2D(velocity).Size(); // UKismetMathLibrary::VSize(velocity);
velocityLength = velocityLength * velocityLength / (aMax * 2);
return velocityLength;
}
void CollisionActor::update(float d_t) {
// qDebug() << "Actor.update()" << this->id.c_str();
assert(sceneNode != NULL);
moveVector *= possibleMoveRatio;
sceneNode = (Ogre::SceneNode*) sceneNode;
sceneNode->translate(to3D(moveVector, sceneNode->getPosition().z));
collisionShape.translate(to2D(sceneNode->getPosition()));
moveVector = Ogre::Vector2::ZERO;
possibleMoveRatio = 1.0;
}
float ADynamicCarController::getAcceleration(float deltaSec) const
{
float a = aMax * deltaSec;
float velocityLength = getBrakeDistance();
float distLeftLength = (target - to2D(agent->GetActorLocation())).Size() - safetyBuffer;
if (velocityLength >= distLeftLength) {
// Check if we should start breaking
FVector2D normVelocity = to2D(velocity);
normVelocity.Normalize();
FVector2D normDistLeft = target - to2D(agent->GetActorLocation());
normDistLeft.Normalize();
if (normVelocity.Equals(normDistLeft, 0.1)) {
return -a;
}
}
return a;
}
void ADynamicCarController::Tick(float DeltaSeconds)
{
Super::Tick(DeltaSeconds);
if (play) {
float deltaSec = GWorld->GetWorld()->GetDeltaSeconds();
updateTarget();
if (first) {
first = false;
FRotator rotation = agent->GetActorRotation();
rotation.Yaw = getRotation(agent->GetActorLocation(), target);
agent->SetActorRotation(rotation);
if (followPath) {
waypoints = DubinsPath::getPath(waypoints, to2D(agent->GetActorLocation()), rotation.Yaw, maxAngle, L, graph, errorTolerance);
writeWaypointsToFile("Waypoints2.txt");
if (waypoints.Num() > 0) {
target = waypoints[0];
}
}
}
if (waypointReached()) {
bool t35 = followPath && waypointsIndex >= waypoints.Num();
bool t4 = avoidAgents && !followPath;
if (t35 || t4) {
play = false;
GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Green, FString::Printf(TEXT("Time: %f\r\n"), totalTime));
}
return;
}
float a = getAcceleration(deltaSec);
v += a;
v = UKismetMathLibrary::FClamp(v, -vMax, vMax);
float rotation = rotate(deltaSec);
FVector vPref = FVector(v * UKismetMathLibrary::DegCos(rotation) * deltaSec, v * UKismetMathLibrary::DegSin(rotation) * deltaSec, 0);
vPref = vPref.GetClampedToMaxSize2D(UKismetMathLibrary::FMin(v * deltaSec, FVector2D::Distance(to2D(agent->GetActorLocation()), target)));
FVector oldVel = velocity;
if (avoidAgents) {
adjustVelocity(to2D(vPref), deltaSec);
//velocity = vPref;
} else {
velocity = vPref;
}
FVector2D q = to2D(velocity - oldVel);
float dv = FMath::Sqrt(FVector2D::DotProduct(q, q));
if (dv > aMax * deltaSec) {
float f = aMax * deltaSec / dv;
velocity = (1 - f) * oldVel + f * velocity;
}
float rot = agent->GetActorRotation().Yaw;
if (velocity.Size2D() != 0) {
if (angleDiff(rot, velocity.Rotation().Yaw) > 90) {
velocity = -velocity;
setRotation();
velocity = -velocity;
} else {
setRotation();
}
}
/*
if (UKismetMathLibrary::Abs(angleDiff(rot, agent->GetActorRotation().Yaw)) / deltaSec > maxAngle * (v / L) + 5) {
GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Red, FString::Printf(TEXT("rot: %f yaw: %f -> %f (%f) %d %s -> %s"), rot, agent->GetActorRotation().Yaw, angleDiff(rot, agent->GetActorRotation().Yaw), UKismetMathLibrary::Abs(angleDiff(rot, agent->GetActorRotation().Yaw)) / deltaSec, vPref.Equals(velocity), *to2D(vPref).ToString(), *to2D(velocity).ToString()));
DrawDebugLine(GWorld->GetWorld(), FVector(to2D(agent->GetActorLocation()), 20), FVector(to2D(agent->GetActorLocation()), 30), FColor::Red, false, 0.5, 0, 1);
GEngine->DeferredCommands.Add(TEXT("pause"));
}*/
agent->SetActorLocation(agent->GetActorLocation() + velocity);
/*
DrawDebugLine(GWorld->GetWorld(), to3D(target), to3D(target) + collisionSize, collisionColor, false, 0.1, 0, 1);
float a = getAcceleration(deltaSec);
v += a;
v = UKismetMathLibrary::FClamp(v, -vMax, vMax);
float rotation = rotate(deltaSec);
acceleration.X = a * UKismetMathLibrary::DegCos(rotation);
acceleration.Y = a * UKismetMathLibrary::DegSin(rotation);
drawLine(2 * acceleration / deltaSec, accelerationColor);
velocity = FVector(v * UKismetMathLibrary::DegCos(rotation) * deltaSec, v * UKismetMathLibrary::DegSin(rotation) * deltaSec, 0);
float rot = agent->GetActorRotation().Yaw;
setRotation();
GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Red, FString::Printf(TEXT("%f"), UKismetMathLibrary::Abs(rot - agent->GetActorRotation().Yaw) / deltaSec));
agent->SetActorLocation(agent->GetActorLocation() + velocity);
DrawDebugLine(GWorld->GetWorld(), agent->GetActorLocation(), agent->GetActorLocation() + collisionSize, FColor::Green, false, 0.1, 0, 1);
*/
}
}
void RectScene::mousePressEvent(QGraphicsSceneMouseEvent *event)
{
QPointF pos = event->scenePos();
MyRect * item = dynamic_cast<MyRect *>(itemAt(pos,QTransform()));
switch (mode)
{
case Add:
{
buffer.push_back(pos);
if (buffer.size() == 2)
{
MyRect * newrect = new MyRect(buffer[0],buffer[1]);
addItem(newrect);
buffer.clear();
newrect->label = giveName(namePool++);
auto x = new QGraphicsTextItem(newrect->label,newrect);
x->setPos(newrect->rect().center());
treescene->tree->insert(newrect->entry);
update();
treescene->drawTree();
}
}
break;
case Remove:
{
if (item)
{
removeItem(item);
treescene->tree->deleteEntry(item->entry);
delete item;
update();
treescene->drawTree();
}
}
break;
case Search:
{
if (!insearch)
{
insearch = true;
buffer.push_back(pos);
}
else
{
insearch = false;
QRectF rect(pos,buffer[0]);
searchBuf.push_back(new QGraphicsRectItem(rect));
addItem(searchBuf.back());
deque<LeafEntry*> x = treescene->tree->searchOverlap(to2D(rect));
MyRect * rectpt;
int n = x.size();
for (int i = 0; i<n; ++i)
{
rectpt =(MyRect*) x[i]->data;
rectpt->setSelected(true);
}
buffer.clear();
}
}
break;
}
}
void MyRect::postinit()
{
entry = new LeafEntry(this,to2D(rect()));
}
CollisionActor* CollisionActor::teleport(Ogre::Vector2 to) {
this->moveVector = Ogre::Vector2::ZERO;
sceneNode->setPosition(to3D(to, sceneNode->getPosition().z));
collisionShape.translate(to2D(sceneNode->getPosition()));
return this;
}
