/**
* Method to watch the robots rotation and detect falls
* publishes the result of this computation to other classes.
*
* Also builds in the ability to shutoff gains when a fall is detected,
* but this still has some problems. Mainly, it sometimes triggers a fall
* when the robot is rotated back up again, but only when the robot is
* 'over rotated' during the righting. This feature can be enabled
* by calling 'enableFallProtection'.
* Also, currently the robot will print whenever it believes it is in
* the process of falling. This will allow us to monitor how well this code
* works.
*
*/
void GuardianModule::checkFalling()
{
if (!useFallProtection)
{
return;
}
struct Inertial inertial = {inertialInput.message().angle_x(),
inertialInput.message().angle_y() };
/***** Determine if the robot is in the process of FALLING ****/
//Using just the magnitude:
const float angleMag = sqrtf(std::pow(inertial.angleX,2) +
std::pow(inertial.angleY,2));
const float lastAngleMag = sqrtf(std::pow(lastInertial.angleX,2) +
std::pow(lastInertial.angleY,2));
const float angleSpeed = angleMag - lastAngleMag;
const bool falling_critical_angle = angleMag > FALLING_ANGLE_THRESH;
if(isFalling(angleMag, angleSpeed))
{
// If falling, increment falling frames counter.
fallingFrames += 1;
notFallingFrames = 0;
}
else
{
// Otherwise, not falling.
fallingFrames = 0;
notFallingFrames += 1;
}
// if(angleMag >= FALLING_ANGLE_THRESH)
// {
// std::cout << "angleSpeed " << angleSpeed
// << " and angleMag" << angleMag << std::endl
// << " fallingFrames is " << fallingFrames
// << " notFallingFrames is " << notFallingFrames
// << " and critical angle is " << falling_critical_angle
// << std::endl;
// }
//If the robot has been falling for a while, and the robot is inclined
//already at a 45 degree angle, than we know we are falling
if (fallingFrames > FALLING_FRAMES_THRESH)
{
// When falling, execute the fall protection method.
//cout << "GuardianModule::checkFalling() : FALLING!" << endl;
falling = true;
//processFallingProtection(); // Should be called later in run_()
}
else if(notFallingFrames > FALLING_FRAMES_THRESH)
{
falling = false;
}
// To calculate the angular speed of the fall next time.
lastInertial = inertial;
}
bool NPC::shouldCollide (const IEntity* entity) const
{
if (isDying()) {
return false;
}
if (isDazed()) {
return entity->isSolid() || entity->isWater();
}
if (entity->isWater()) {
return true;
}
if (isFalling()) {
return entity->isWater();
}
if (entity->isPlayer()) {
const Player* player = static_cast<const Player*>(entity);
return !player->isCrashed();
}
return entity->isSolid();
}
void Player::changeGravity()
{
if(!isFalling()){
if(gravity() == Gravity::Down)
setGravity(Gravity::Up);
else
setGravity(Gravity::Down);
setFalling(true);
}
}
void DynamicGoo::update(){
if (isSelected() && !isFalling()) {
body->SetLinearVelocity(b2Vec2(0,0));
return;
}
if (!isSleeping()){
if (hasJoint()){
body->SetAngularVelocity(0.0);
}
moveToTarget();
}
else
emit this->checkForNeighbors(getPPosition());
}
static void accel_data_handler(AccelData *data, uint32_t num_samples)
{
if (num_samples <= SAMPLE_BUF)
{
APP_LOG(APP_LOG_LEVEL_DEBUG, "récolte des données.");
if (isFalling(data))
{
/*
accel_data_service_unsubscribe();
tick_timer_service_unsubscribe();
*/
show_main_layout();
}
}
}
update_status GravityComponent::update() {
if (motion != nullptr) {
//apply force gravity
motion->velocity.y += 1.0f * gravity *(App->timer->getDeltaFrame() / 1000.0f);
//if falling, check tolerance under the entity
if (motion->velocity.y > 0) {
std::list<Collider*> collisions=App->collisions->checkCollisions(gravityCollider);
//si la lista está vacia, está cayendo, setear FALL
//si no lo esta ... nada, las colisiones especificas se mirara en onCollision
if (collisions.size() > 0) {
//si está en salto, no tenerlo en cuenta!!
TypeJump jumping = parent->controller.stateJump;
if (jumping == TypeJump::FALL) {
//not if is dead
if (life!=nullptr && life->isAlive()) {
playSound();
}
parent->controller.stateJump = TypeJump::NONE;
}
//ignore collisions jumping down
this->onCollisionEnter(collision, collisions.front());
} else {
if (isFalling()) {
//check with another circle for tolerance of making fall
parent->controller.stateJump = TypeJump::FALL;
}
}
}
//control de gravedad máxima
if (motion->velocity.y > maxVelocity) {
motion->velocity.y = maxVelocity;
}
}
return UPDATE_CONTINUE;
}
void monster::update(double _timeFactor) { //Update the entity
if (beingHit) {
if (hitAnimationTimer>0) { hitAnimationTimer-=0.06*_timeFactor; }else{
beingHit=false;
stateChanged();
}
this->setX(this->getX()+(double)this->getKnockbackSpeed()*_timeFactor);
}else{
this->setX(this->getX()+(double)this->getSpeed()*_timeFactor);
}
mRenderer->updateAnimation(_timeFactor);
if (getX()>platform->p2.x) { setX(platform->p2.x); }
if (getX()<platform->p1.x) { setX(platform->p1.x); }
if (isFalling()) {
if (getY()>platform->p1.y) {
stopFalling();
}
setY(getY()+2*_timeFactor);
}
}
void DynamicGoo::update() {
QList <int > toRemove;
for (int i=0; i<sources.length(); i++) {
if (!sSystem->sourceStatus(sources[i].first)) {
sSystem->deleteSource(sources[i]);
toRemove.push_back(i);
}
}
for (int i=0; i<toRemove.length(); i++)
sources.removeAt(toRemove[i]);
if (isSelected() && !isFalling()) {
body->SetLinearVelocity(b2Vec2(0,0));
return;
}
if (!isSleeping()) {
if (hasJoint()) {
body->SetAngularVelocity(0.0);
}
moveToTarget();
}
else
emit this->checkForNeighbors(getPPosition());
}
void SlimePetEntity::animate(float delay)
{
if (age < 0.0f)
{
age += delay;
}
else
{
attackDelay -= delay;
if (isJumping)
{
hVelocity -= 700.0f * delay;
h += hVelocity * delay;
bool firstTimeGround = false;
if (h <= 0.0f)
{
h = 0.0f;
if (isFalling())
{
fall();
}
else
{
if (isFirstJumping)
{
isFirstJumping = false;
firstTimeGround = true;
hVelocity = 160.0f;
SoundManager::getInstance().playSound(SOUND_SLIME_IMAPCT);
}
else
{
jumpingDelay = 0.3f + 0.1f * (rand() % 15);
isJumping = false;
SoundManager::getInstance().playSound(SOUND_SLIME_IMAPCT_WEAK);
}
}
}
if (firstTimeGround) frame = 0;
else if (hVelocity > -190.0f) frame = 2;
else frame = 1;
}
else if (isFalling())
{
fall();
}
else
{
jumpingDelay -= delay;
if (jumpingDelay < 0.0f)
{
SoundManager::getInstance().playSound(SOUND_SLIME_JUMP);
hVelocity = 300.0f + rand() % 250;
isJumping = true;
isFirstJumping = true;
float randVel = 250.0f + rand() % 250;
setVelocity(Vector2D(x, y).vectorTo(game().getPlayerPosition(), randVel ));
}
else if (jumpingDelay < 0.1f)
frame = 1;
else frame = 0;
}
BaseCreatureEntity::animate(delay);
if (canCollide()) testSpriteCollisions();
}
z = y + 14;
}
void DynamicGoo::moveToTarget(){
//Stop to follow if the goo is dragged for the use
if (isDragging()) {
stopFollow();
return;
}
if (onGround && groundPoint!=getPPosition()) {
onGround=false;
}
if (isFalling()) return;
if (hasJoint()){
return;
}
if (!hasJoint() && !isDragging() && isOnGround() && target==NULL ){
emit this->nextTargetPlease(NULL);
}
if (following && !falling){
if (prevTarget){
if (!target->isLinked(prevTarget)){
drop();
return;
}
QPoint meTarget=target->getPPosition()-getPPosition();
if (toVec(meTarget).Length()>15+radius/10) {
stopFollow();
fallDown();
return;
}
//Compute mx and my for the position
float mx,my;
mx=(target->getVPosition().x-prevTarget->getVPosition().x);
my=(target->getVPosition().y-prevTarget->getVPosition().y);
float d=qSqrt(mx*mx+my*my);
float md=qSqrt(qPow((target->getVPosition().x-getVPosition().x),2)+qPow((target->getVPosition().y-getVPosition().y),2));
mx/=d;
my/=d;
float tx,ty;
//Compute tehorical x and y
tx=(getVPosition().x-prevTarget->getVPosition().x)/mx;
ty=(getVPosition().y-prevTarget->getVPosition().y)/my;
float yt=my*tx+prevTarget->getVPosition().y;
float xt=mx*ty+prevTarget->getVPosition().x;
//if my y position is different at least of 12 falldown and return
if ((qAbs(getVPosition().y-yt)>getRadius() && qAbs(getVPosition().x-xt)>getRadius()) || (md>d+radius)){
stopFollow();
fallDown();
return;
}
}
//this is a work around for the very ugly bug of the "FLYING GOOS"
//description of the bug if no prevTarget is setted but target is and the goo tower start to fall down the goo start to fly
//for reach his target!
//check if prevtarget is not setted and target is
if (!prevTarget && target && !target->isOnGround()){
//compute distance between target and me
float d=(target->getVPosition()-getVPosition()).Length();
//if that distance is more than 25 falldown and return
//PS: remember that 30 is a single point contact between two goos
if (d>=radius*0.2) {
stopFollow();
fallDown();
return;
}
else if (d<radius*0.2){
emit this->nextTargetPlease(target);
body->SetLinearVelocity(b2Vec2(0,0));
return;
}
}
b2Vec2 dP;
if (target!=NULL /*&& target->getBody()!=NULL*/)
dP=target->getVPosition()-this->getVPosition();
else {
stopFollow();
fallDown();
return;
}
if (dP.Length()<=(radius-radius/4.0)/10.0){
emit this->nextTargetPlease(target);
body->SetLinearVelocity(b2Vec2(0,0));
return;
}
if (!prevTarget){
b2Vec2 dvec=(target->getVPosition()-getVPosition());
float d=qSqrt(dvec.x*dvec.x+dvec.y*dvec.y);
if (onGround && target->isOnGround() && d<distanceToJoint){
double omega =(dP.x>0 ? speed*body->GetMass() : -speed*body->GetMass());
body->SetAngularVelocity(omega);
body->ApplyForceToCenter(body->GetMass()*body->GetWorld()->GetGravity());
}
else if (!onGround && d<radius*2) {
emit this->nextTargetPlease(target);
body->SetLinearVelocity(b2Vec2(0,0));
return;
dP.x=(dP.x>0 ? speed*2 : -speed*2);
dP.y=body->GetMass()*body->GetWorld()->GetGravity().y;
body->ApplyForceToCenter(dP);
}
else{
body->SetGravityScale(1);
stopFollow();
}
}
else {
dP.x=dP.x*speed/dP.Length();
dP.y=dP.y*speed/dP.Length() ;
body->SetLinearVelocity(dP);
}
}
}
void DynamicGoo::paint(QPainter &p){
//Check rutine
//Real paint stuff
//check if is dragged
if (isDragging()){
p.setBrush(Qt::transparent);
p.setPen(QPen(color,3));
p.drawEllipse(toPoint(body->GetPosition()), getRadius()+10,getRadius()+10);
//p.drawRect(boundingRect());
}
//check if is selected and draggable
else if (selected && isDragable()){
if (!hasJoint())
p.setPen(QPen(color,3,Qt::DashLine));
else
p.setPen(QPen(color,3,Qt::DotLine));
p.setBrush(Qt::transparent);
p.drawEllipse(toPoint(body->GetPosition()), getRadius()+10,getRadius()+10);
}
//paint goo
p.setPen(secondaryColor);
QColor center=secondaryColor;
center.setRgb((center.red()+50 > 255 ? 255 : center.red()+50),(center.green()+50 > 255 ? 255 : center.green()+50),(center.blue()+50 > 255 ? 255 : center.blue()+50));
if (sleeping){
p.setBrush(center);
p.drawEllipse(getPPosition(),getRadius(),getRadius());
}
else {
b2Vec2 speed=body->GetLinearVelocity();
float angle=qAtan2(speed.x,speed.y);
float module=(isFalling() ? speed.Length()/8 : (!hasJoint() ? speed.Length()/15 : 0));
p.save();
p.translate(getPPosition());
if (!isDragging()) p.rotate((hasJoint() ? 0 :-angle*180.0/3.141628));
else p.rotate(this->angle);
p.setBrush(secondaryColor);
p.drawEllipse(QPoint(0,0),qRound(getRadius()-module),qRound(getRadius()+module));
rx=3;//+=(rand()%5-2);
ry=-2;//+=(rand()%5-2);
counter++;
QRadialGradient rg(rx,ry,getRadius()+5);
rg.setColorAt(0,center);
rg.setColorAt(1,Qt::transparent);
p.setBrush(rg);
p.drawEllipse(QPoint(0,0),qRound(getRadius()-module),qRound(getRadius()+module));
if ((counter>=delay && !hasJoint()) || isDragging() || isFalling()){
if (!isDragging() && !isFalling()) {
bool nE=!(rand()%5);
if (eye) nE=rand()%3;
if (eye!=nE && nE){
eyeSizeL=qrand()%3+5;
eyeSizeR=eyeSizeL+qrand()%4-1;
}
eye=nE;
counter=0;
}
else if (!eye) {
eyeSizeL=qrand()%3+5;
eyeSizeR=eyeSizeL+qrand()%4-1;
eye=true;
this->angle=qrand()%360;
}
}
if (eye && !hasJoint()&& !onGround) {
p.setPen(secondaryColor);
QRadialGradient rG2(-9,13,eyeSizeL);
QRadialGradient rG3(9,13,eyeSizeR);
rG2.setColorAt(0,Qt::white);
rG2.setColorAt(1,Qt::lightGray);
p.setBrush(rG2);
p.drawEllipse(QPoint(-9,12),eyeSizeL,eyeSizeL);
rG3.setColorAt(0,Qt::white);
rG3.setColorAt(1,Qt::lightGray);
p.setBrush(rG3);
p.drawEllipse(QPoint(9,12),eyeSizeR,eyeSizeR);
p.setBrush(Qt::black);
p.setPen(Qt::transparent);
p.drawEllipse(QPoint(-9,14),2,2);
p.drawEllipse(QPoint(9,13),2,2);
}
p.restore();
}
}
/*! @brief Returns true if the robot is currently incapacitated. A robot is incapacitated if it is falling, fallen, not on the ground or getting up
*/
bool NUSensorsData::isIncapacitated()
{
bool gettingup = false;
get(MotionGetupActive, gettingup);
return isFalling() or isFallen() or not isOnGround() or gettingup;
}
