Ejemplo n.º 1
0
/** Returns the hit effect object to use when this objects hits something.
 *  \returns The hit effect object, or NULL if no hit effect should be played.
 */
HitEffect* Bowling::getHitEffect() const
{
    if(m_has_hit_kart)
        return new HitSFX(getXYZ(), "strike");
    else
        return new HitSFX(getXYZ(), "crash");
}   // getHitEffect
void ITG3200::zeroCalibrate(unsigned int samples, unsigned int sampleDelayMS) 
{
  int16_t x_offset_temp = 0;
  int16_t y_offset_temp = 0;
  int16_t z_offset_temp = 0;
  int16_t x,y,z;
  x_offset = 0;
  y_offset = 0;
  z_offset = 0;
  getXYZ(&x,&y,&z);//
  for (int i = 0;i < samples;i++){
    delay(sampleDelayMS);
    getXYZ(&x,&y,&z);
    x_offset_temp += x;
    y_offset_temp += y;
    z_offset_temp += z;
  }

  x_offset = abs(x_offset_temp)/samples;
  y_offset = abs(y_offset_temp)/samples;
  z_offset = abs(z_offset_temp)/samples;
  if(x_offset_temp > 0)x_offset = -x_offset;
  if(y_offset_temp > 0)y_offset = -y_offset;
  if(z_offset_temp > 0)z_offset = -z_offset;
}
Ejemplo n.º 3
0
/** Creates the explosion physical effect, i.e. pushes the karts and ph
 *  appropriately. The corresponding visual/sfx needs to be added manually!
 *  \param kart_hit If non-NULL a kart that was directly hit.
 *  \param object If non-NULL a physical item that was hit directly.
 *  \param secondary_hits True if items that are not directly hit should
 *         also be affected.
 */
void Flyable::explode(AbstractKart *kart_hit, PhysicalObject *object,
                      bool secondary_hits)
{
    // Apply explosion effect
    // ----------------------
    World *world = World::getWorld();
    for ( unsigned int i = 0 ; i < world->getNumKarts() ; i++ )
    {
        AbstractKart *kart = world->getKart(i);

        // If no secondary hits should be done, only hit the
        // direct hit kart.
        if(!secondary_hits && kart!=kart_hit)
            continue;

        // Handle the actual explosion. The kart that fired a flyable will
        // only be affected if it's a direct hit. This allows karts to use
        // rockets on short distance.
        if( (m_owner!=kart || m_owner==kart_hit) && !kart->getKartAnimation())
        {
            // The explosion animation will register itself with the kart
            // and will free it later.
            ExplosionAnimation::create(kart, getXYZ(), kart==kart_hit);
            if(kart==kart_hit && world->getTrack()->isArena())
            {
                world->kartHit(kart->getWorldKartId());
            }
        }
    }
    world->getTrack()->handleExplosion(getXYZ(), object, secondary_hits);
}   // explode
Ejemplo n.º 4
0
/** Initialises the item. Note that m_distance_2 must be defined before calling
 *  this function, since it pre-computes some values based on this.
 *  \param type Type of the item.
 *  \param xyz Position of this item.
 *  \param normal Normal for this item.
 */
void Item::initItem(ItemType type, const Vec3 &xyz, const Vec3&normal)
{
    ItemState::initItem(type, xyz, normal);
    // Now determine in which quad this item is, and its distance
    // from the center within this quad.
    m_graph_node = Graph::UNKNOWN_SECTOR;
    m_distance_from_center = 9999.9f;
    m_avoidance_points[0] = NULL;
    m_avoidance_points[1] = NULL;

    // Check that Graph exist (it might not in battle mode without navmesh)
    if (Graph::get())
    {
        Graph::get()->findRoadSector(xyz, &m_graph_node);
    }
    if (DriveGraph::get() && m_graph_node != Graph::UNKNOWN_SECTOR)
    {
        // Item is on drive graph. Pre-compute the distance from center
        // of this item, which is used by the AI (mostly for avoiding items)
        Vec3 distances;
        DriveGraph::get()->spatialToTrack(&distances, getXYZ(), m_graph_node);
        m_distance_from_center = distances.getX();
        const DriveNode* dn = DriveGraph::get()->getNode(m_graph_node);
        const Vec3& right = dn->getRightUnitVector();
        // Give it 10% more space, since the kart will not always come
        // parallel to the drive line.
        Vec3 delta = right * sqrt(m_distance_2) * 1.3f;
        m_avoidance_points[0] = new Vec3(getXYZ() + delta);
        m_avoidance_points[1] = new Vec3(getXYZ() - delta);
    }

}   // initItem
void DynamicEntity::updateGround()
{
  // ground is not a real dynamic object so we have to update it manually
  Ground.XYZ.x = getXYZ().x;
  Ground.XYZ.y = Game::Arena->getHeight(getXYZ().x, getXYZ().z) - GROUND_COLLISION_RADIUS;
  Ground.XYZ.z = getXYZ().z;
  // it's not handled by the corpus manager
  Ground.update(0);
}
/** Returns the maximum height of the terrain at the current point. While
 *  generall the height is arbitrary (a skybox is not part of the physics and
 *  will therefore not be detected), it is important that a rubber ball does
 *  not end up on top of a tunnel.
 *  \param vertical_offset A vertical offset which is added to the current
 *         position of the kart in order to avoid tunneling effects (it could
 *         happen that the raycast down find the track since it uses the
 *         vertical offset, while the raycast up would hit under the track
 *         if the vertical offset is not used).
 *  \returns The height (Y coordinate) of the next terrain element found by
 *           a raycast up. If no terrain is found, it returns 99990
 */
float RubberBall::getMaxTerrainHeight(const Vec3 &vertical_offset) const
{
    const TriangleMesh &tm = World::getWorld()->getTrack()->getTriangleMesh();
    Vec3 to(getXYZ());
    to.setY(10000.0f);
    Vec3 hit_point;
    const Material *material;
    tm.castRay(getXYZ()+vertical_offset, to, &hit_point, &material);

    return (material) ? hit_point.getY() : 99999.f;
}   // getMaxTerrainHeight
RubberBall::RubberBall(AbstractKart *kart)
          : Flyable(kart, PowerupManager::POWERUP_RUBBERBALL, 0.0f /* mass */),
            TrackSector()
{
    // For debugging purpose: pre-fix each debugging line with the id of
    // the ball so that it's easy to collect all debug output for one
    // particular ball only.
    m_next_id++;
    m_id = m_next_id;

    // Don't let Flyable update the terrain information, since this object
    // has to do it earlier than that.
    setDoTerrainInfo(false);
    float forw_offset = 0.5f*kart->getKartLength() + m_extend.getZ()*0.5f+5.0f;

    createPhysics(forw_offset, btVector3(0.0f, 0.0f, m_speed*2),
                  new btSphereShape(0.5f*m_extend.getY()),
                  -70.0f /*gravity*/,
                  true /*rotates*/);

    // Do not adjust the up velocity
    setAdjustUpVelocity(false);
    m_max_lifespan       = 9999;
    m_target             = NULL;
    m_aiming_at_target   = false;
    m_fast_ping          = false;
    // At the start the ball aims at quads till it gets close enough to the
    // target:
    m_height_timer       = 0.0f;
    m_interval           = m_st_interval;
    m_current_max_height = m_max_height;
    m_ping_sfx           = sfx_manager->createSoundSource("ball_bounce");
    // Just init the previoux coordinates with some value that's not getXYZ()
    m_previous_xyz       = m_owner->getXYZ();
    m_previous_height    = 2.0f;  //
    // A negative value indicates that the timer is not active
    m_delete_timer       = -1.0f;
    m_tunnel_count       = 0;

    LinearWorld *world = dynamic_cast<LinearWorld*>(World::getWorld());
    // FIXME: what does the rubber ball do in case of battle mode??
    if(!world) return;

    computeTarget();

    // initialises the current graph node
    TrackSector::update(getXYZ());
    TerrainInfo::update(getXYZ());
    initializeControlPoints(m_owner->getXYZ());

}   // RubberBall
Ejemplo n.º 8
0
/** Creates the explosion physical effect, i.e. pushes the karts and ph
 *  appropriately. The corresponding visual/sfx needs to be added manually!
 *  \param kart_hit If non-NULL a kart that was directly hit.
 *  \param object If non-NULL a physical item that was hit directly.
 *  \param secondary_hits True if items that are not directly hit should
 *         also be affected.
 */
void Flyable::explode(AbstractKart *kart_hit, PhysicalObject *object,
                      bool secondary_hits)
{
    // Apply explosion effect
    // ----------------------
    World *world = World::getWorld();
    for ( unsigned int i = 0 ; i < world->getNumKarts() ; i++ )
    {
        AbstractKart *kart = world->getKart(i);
        // Don't explode teammates in team world
        if (world->hasTeam() &&
            world->getKartTeam(kart->getWorldKartId()) ==
            world->getKartTeam(m_owner->getWorldKartId()))
            continue;

        if (kart->isGhostKart()) continue;

        // If no secondary hits should be done, only hit the
        // direct hit kart.
        if(!secondary_hits && kart!=kart_hit)
            continue;

        // Handle the actual explosion. The kart that fired a flyable will
        // only be affected if it's a direct hit. This allows karts to use
        // rockets on short distance.
        if( (m_owner!=kart || m_owner==kart_hit) && !kart->getKartAnimation())
        {
            // The explosion animation will register itself with the kart
            // and will free it later.
            ExplosionAnimation::create(kart, getXYZ(), kart==kart_hit);
            if (kart == kart_hit)
            {
                world->kartHit(kart->getWorldKartId(),
                    m_owner->getWorldKartId());

                if (m_owner->getController()->canGetAchievements())
                {
                    if (m_owner->getWorldKartId() != kart->getWorldKartId())
                        PlayerManager::addKartHit(kart->getWorldKartId());
                    PlayerManager::increaseAchievement(AchievementsStatus::ALL_HITS, 1);
                    if (race_manager->isLinearRaceMode())
                        PlayerManager::increaseAchievement(AchievementsStatus::ALL_HITS_1RACE, 1);
                }
            }
        }
    }
    Track::getCurrentTrack()->handleExplosion(getXYZ(), object,secondary_hits);
}   // explode
Ejemplo n.º 9
0
Archivo: Acc.cpp Proyecto: PSam95/PFM
void Acc::debugXYZ() {
	double acc[3];
	getXYZ(&acc[0], &acc[1], &acc[2]);
	Serial.print("AcX = "); Serial.print(acc[0], 4);
	Serial.print("\t | AcY = "); Serial.print(acc[1], 4);
	Serial.print("\t | AcZ = "); Serial.println(acc[2], 4);
}
Ejemplo n.º 10
0
/** Virtual function called when the plunger hits something.
 *  The plunger is special in that it is not deleted when hitting an object.
 *  Instead it stays around (though not as a graphical or physical object)
 *  till the rubber band expires.
 *  \param kart Pointer to the kart hit (NULL if not a kart).
 *  \param obj  Pointer to PhysicalObject object if hit (NULL otherwise).
 */
void Plunger::hit(Kart *kart, PhysicalObject *obj)
{
    if(isOwnerImmunity(kart)) return;

    RaceGUIBase* gui = World::getWorld()->getRaceGUI();
    irr::core::stringw hit_message;

    // pulling back makes no sense in battle mode, since this mode is not a race.
    // so in battle mode, always hide view
    if( m_reverse_mode || race_manager->isBattleMode() )
    {
        if(kart)
        {
            kart->blockViewWithPlunger();

            hit_message += StringUtils::insertValues(getPlungerInFaceString(),
                                                     kart->getName().c_str(),
                                                     m_owner->getName().c_str()
                                                    ).c_str();
            gui->addMessage(hit_message, NULL, 3.0f, 40, video::SColor(255, 255, 255, 255), false);
        }

        m_keep_alive = 0;
        // Make this object invisible by placing it faaar down. Note that if this
        // objects is simply removed from the scene graph, it might be auto-deleted
        // because the ref count reaches zero.
        getNode()->setVisible(false);
        World::getWorld()->getPhysics()->removeBody(getBody());
    }
    else
    {
        m_keep_alive = m_owner->getKartProperties()->getRubberBandDuration();

        // Make this object invisible by placing it faaar down. Not that if this
        // objects is simply removed from the scene graph, it might be auto-deleted
        // because the ref count reaches zero.
        scene::ISceneNode *node = getNode();
        if(node)
        {
            node->setVisible(false);
        }
        World::getWorld()->getPhysics()->removeBody(getBody());

        if(kart)
        {
            m_rubber_band->hit(kart);
            return;
        }
        else if(obj)
        {
            Vec3 pos(obj->getBody()->getWorldTransform().getOrigin());
            m_rubber_band->hit(NULL, &pos);
        }
        else
        {
            m_rubber_band->hit(NULL, &(getXYZ()));
        }
    }
}   // hit
/*Function: Get the angular velocity and its unit is degree per second.*/
void ITG3200::getAngularVelocity(float *ax,float *ay,float *az)
{
	int16_t x,y,z;
	getXYZ(&x,&y,&z);
	*ax = x/14.375;
	*ay = y/14.375;
	*az = z/14.375;
}
Ejemplo n.º 12
0
void MMA7660::getAcceleration(float *ax,float *ay,float *az)
{
    int8_t x,y,z;
    getXYZ(&x,&y,&z);
    *ax = x/21.00;
    *ay = y/21.00;
    *az = z/21.00;
}
/** Updates the graphics model. Mainly set the graphical position to be the
 *  same as the physics position, but uses offsets to position and rotation
 *  for special gfx effects (e.g. skidding will turn the karts more). 
 *  \param offset_xyz Offset to be added to the position.
 *  \param rotation Additional rotation.
 */
void Moveable::updateGraphics(float dt, const Vec3& offset_xyz,  
                              const btQuaternion& rotation)
{
    Vec3 xyz=getXYZ()+offset_xyz;
    m_node->setPosition(xyz.toIrrVector());
    btQuaternion r_all = getRotation()*rotation;
    Vec3 hpr;
    hpr.setHPR(r_all);
    m_node->setRotation(hpr.toIrrHPR());
}   // updateGraphics
Ejemplo n.º 14
0
bool MMA7660::getAcceleration(float *ax,float *ay,float *az)
{
    int8_t x,y,z;
    if(!getXYZ(&x,&y,&z))return 0;
    *ax = x/21.00;
    *ay = y/21.00;
    *az = z/21.00;
    
    return 1;
}
Ejemplo n.º 15
0
/** Virtual function called when the plunger hits something.
 *  The plunger is special in that it is not deleted when hitting an object.
 *  Instead it stays around (though not as a graphical or physical object)
 *  till the rubber band expires.
 *  \param kart Pointer to the kart hit (NULL if not a kart).
 *  \param obj  Pointer to PhysicalObject object if hit (NULL otherwise).
 *  \returns True if there was actually a hit (i.e. not owner, and target is
 *           not immune), false otherwise.
 */
bool Plunger::hit(AbstractKart *kart, PhysicalObject *obj)
{
    if(isOwnerImmunity(kart)) return false;

    // pulling back makes no sense in battle mode, since this mode is not a race.
    // so in battle mode, always hide view
    if( m_reverse_mode || race_manager->isBattleMode() )
    {
        if(kart)
        {
            kart->blockViewWithPlunger();
            if (kart->getController()->isPlayerController())
                SFXManager::get()->quickSound("plunger");
        }

        m_keep_alive = 0;
        // Make this object invisible.
        getNode()->setVisible(false);
        World::getWorld()->getPhysics()->removeBody(getBody());
    }
    else
    {
        m_keep_alive = m_owner->getKartProperties()->getRubberBandDuration() *
                       m_owner->getPlayerDifficulty()->getRubberBandDuration();

        // Make this object invisible by placing it faaar down. Not that if this
        // objects is simply removed from the scene graph, it might be auto-deleted
        // because the ref count reaches zero.
        scene::ISceneNode *node = getNode();
        if(node)
        {
            node->setVisible(false);
        }
        World::getWorld()->getPhysics()->removeBody(getBody());

        if(kart)
        {
            m_rubber_band->hit(kart);
            return false;
        }
        else if(obj)
        {
            Vec3 pos(obj->getBody()->getWorldTransform().getOrigin());
            m_rubber_band->hit(NULL, &pos);
        }
        else
        {
            m_rubber_band->hit(NULL, &(getXYZ()));
        }
    }

    // Rubber band attached.
    return false;
}   // hit
Ejemplo n.º 16
0
/** Updates the graphics model. Mainly set the graphical position to be the
 *  same as the physics position, but uses offsets to position and rotation
 *  for special gfx effects (e.g. skidding will turn the karts more).
 *  \param offset_xyz Offset to be added to the position.
 *  \param rotation Additional rotation.
 */
void Moveable::updateGraphics(float dt, const Vec3& offset_xyz,
                              const btQuaternion& rotation)
{
    Vec3 xyz=getXYZ()+offset_xyz;
    m_node->setPosition(xyz.toIrrVector());
    btQuaternion r_all = getRotation()*rotation;
    if(btFuzzyZero(r_all.getX()) && btFuzzyZero(r_all.getY()-0.70710677f) &&
       btFuzzyZero(r_all.getZ()) && btFuzzyZero(r_all.getW()-0.70710677f)   )
        r_all.setX(0.000001f);
    Vec3 hpr;
    hpr.setHPR(r_all);
    m_node->setRotation(hpr.toIrrHPR());
}   // updateGraphics
/** Updates the height of the rubber ball, and if necessary also adjusts the
 *  maximum height of the ball depending on distance from the target. The
 *  height is decreased when the ball is getting closer to the target so it
 *  hops faster and faster. This function modifies m_current_max_height.
 *  \return Returns the new height of the ball.
 */
float RubberBall::updateHeight()
{
    // When the ball hits the floor, we adjust maximum height and
    // interval so that the ball bounces faster when it is getting
    // closer to the target.
    if(m_height_timer>m_interval)
    {
        m_height_timer -= m_interval;
        if(m_ping_sfx->getStatus()!=SFXManager::SFX_PLAYING)
        {
            m_ping_sfx->position(getXYZ());
            m_ping_sfx->play();
        }

        if(m_fast_ping)
        {
            // Some experimental formulas
            m_current_max_height = 0.5f*sqrt(m_distance_to_target);
            // If the ball just missed the target, m_distance_to_target
            // can be huge (close to track length) due to the order in
            // which a lost target is detected. Avoid this by clamping
            // m_current_max_height.
            if(m_current_max_height>m_max_height)
                m_current_max_height = m_max_height;
            m_interval           = m_current_max_height / 10.0f;
            // Avoid too small hops and esp. a division by zero
            if(m_interval<0.01f)
                m_interval = 0.1f;
        }
        else
        {
            // Reset the values in case that the ball was already trying
            // to get closer to the target, and then the target disappears
            // (e.g. is eliminated or finishes the race).
            m_interval           = m_st_interval;
            m_current_max_height = m_max_height;
        }
    }   // if m_height_timer > m_interval


    // Determine the height of the ball
    // ================================
    // Consider f(x) = s * x*(x-m_intervall), which is a parabolic function
    // with f(0) = 0, f(m_intervall)=0. We then scale this function to
    // fulfill: f(m_intervall/2) = max_height, or:
    // f(m_interval/2) = s*(-m_interval^2)/4 = max_height
    // --> s =  4*max_height / -m_interval^2
    float s = 4.0f * m_current_max_height / (-m_interval*m_interval);
    return m_height_timer * (m_height_timer-m_interval) * s;
}   // updateHeight
/** Checks if the line from the previous ball position to the new position
 *  hits something, which indicates that the ball is tunneling through. If
 *  this happens, the ball position is adjusted so that it is just before
 *  the hit point. If tunneling happens four frames in a row the ball is
 *  considered stuck and explodes (e.g. the ball might try to tunnel through
 *  a wall to get to a 'close' target. In this case the ball would not
 *  move much anymore and be stuck).
 *  \return True if the ball tunneled often enough to be removed.
 */
bool RubberBall::checkTunneling()
{
    const TriangleMesh &tm = World::getWorld()->getTrack()->getTriangleMesh();
    Vec3 hit_point;
    const Material *material;

    tm.castRay(m_previous_xyz, getXYZ(), &hit_point, &material);

    if(material)
    {
        // If there are three consecutive tunnelling
        m_tunnel_count++;
        if(m_tunnel_count > 3)
        {
#ifdef PRINT_BALL_REMOVE_INFO
            Log::debug("RubberBall",
                       "Ball %d nearly tunneled at %f %f %f -> %f %f %f",
                        m_id, m_previous_xyz.getX(),m_previous_xyz.getY(),
                        m_previous_xyz.getZ(),
                        getXYZ().getX(),getXYZ().getY(),getXYZ().getZ());
#endif
            hit(NULL);
            return true;
        }
        // In case of a hit, move the hit point towards the
        // previous point by the radius of the ball --> this
        // point will just allow the ball to avoid tunneling.
        Vec3 diff = m_previous_xyz - hit_point;
        hit_point += diff * (1.1f*m_extend.getY()/diff.length());
        setXYZ(hit_point);
        return false;
    }
    else
        m_tunnel_count = 0;
    return false;
}   // checkTunneling
Ejemplo n.º 19
0
double HMC5883L::getHeadingXY()
{
    int16_t raw_data[3];
    getXYZ(raw_data);
    double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X)
    
    // TODO: declenation angle compensation
    
    if(heading < 0.0) // fix sign
        heading += PI2;
        
    if(heading > PI2) // fix overflow
        heading -= PI2;
        
    return heading;
}
Ejemplo n.º 20
0
pcl::PointCloud<pcl::PointXYZ>::Ptr CloudGrabberImpl::getXYZ() {
  if (m_ss == IDLE) {
    printf("start streaming just for this point cloud\n");
    startXYZ();
    pcl::PointCloud<pcl::PointXYZ>::Ptr out = getXYZ();
    stop();
    return out;
  }
  else if (m_ss == XYZ) {
    m_rs = PENDING;
    printf("waiting...\n");
    while (m_rs == PENDING) sleep(.01);
    printf("ok\n");
    return m_xyz;
  }
  else PRINT_AND_THROW("asked for xyz but currently streaming xyzrgb");
}
Ejemplo n.º 21
0
void ADXL335::getAcceleration(float *ax,float *ay,float *az)
{
    int x,y,z;
    float xvoltage,yvoltage,zvoltage;
    getXYZ(&x,&y,&z);
    xvoltage = (float)x*ADC_REF/ADC_AMPLITUDE;
    yvoltage = (float)y*ADC_REF/ADC_AMPLITUDE;
    zvoltage = (float)z*ADC_REF/ADC_AMPLITUDE;
    Serial.println("voltage:");
    Serial.println(xvoltage);
    Serial.println(yvoltage);
    Serial.println(zvoltage);
    *ax = (xvoltage - ZERO_X)/SENSITIVITY;
    *ay = (yvoltage - ZERO_Y)/SENSITIVITY;
    *az = (zvoltage - ZERO_Z)/SENSITIVITY;
    
}
Ejemplo n.º 22
0
/** @brief calculate the real target of the weapon and the angle it should fire at
 * @todo: doesn't take into account the drop or air resistance
 */
Quaternion Unit::getBallisticAngle(const Vector3& a_position)
{
  // get the intended target
  Vector3 pointer_position = Controller->getPointerPos();
  // get the vector pointing at the inteded target
  Vector3 pointer_direction = pointer_position - a_position;
  // get the flattened vector
  pointer_direction.y = 0;

  // get the real direction confined to the torso angle
  Real distance = pointer_direction.length();
  pointer_direction = getDirection() * distance;
  // get the point the vector is pointing at from the perspective of the weapon
  pointer_position = pointer_direction + a_position;

  // we need to apply the offset so that the angles converge from the weapons mounted on the sides
  Vector3 weapon_offset = getXYZ() - a_position;
  // the angle must be limited otherwise you could fire sideways - this limits it to 5 degrees
  Real limit = distance / 11.4300523;
  if (limit < weapon_offset.length()) {
    weapon_offset.normalise();
    weapon_offset *= limit;
  }
  pointer_position += weapon_offset;

  // check if the real target is within the arena
  Game::Arena->isOutOfArena(pointer_position);
  // get the height at this point
  Real height = Game::Arena->getHeight(pointer_position.x, pointer_position.z);

  // adjust by the target mode
  if (Controller->ControlBlock.target_high) {
    pointer_position.y = height + target_high_offset;
  } else if (Controller->ControlBlock.target_air) {
    pointer_position.y = height + target_air_offset;
  } else {
    pointer_position.y = height + target_low_offset;
  }

  // the real direction to the real target
  pointer_direction = pointer_position - a_position;

  // return the quaternion for the projectile
  return Vector3::UNIT_Z.getRotationTo(pointer_direction);
}
Ejemplo n.º 23
0
void ADXL335::getPitchAndRoll(float *roll, float *pitch)
{
	int x,y,z;
	float xvoltage,yvoltage,zvoltage, ax, ay, az;
	getXYZ(&x,&y,&z);
	xvoltage = (float)x*ADC_REF/ADC_AMPLITUDE;
	yvoltage = (float)y*ADC_REF/ADC_AMPLITUDE;
	zvoltage = (float)z*ADC_REF/ADC_AMPLITUDE;
	//Serial.println("voltage:");
	//Serial.println(xvoltage);
	//Serial.println(yvoltage);
	//Serial.println(zvoltage);
	ax = (xvoltage - ZERO_X)/SENSITIVITY;
	ay = (yvoltage - ZERO_Y)/SENSITIVITY;
	az = (zvoltage - ZERO_Z)/SENSITIVITYx+1;
	*pitch=atan2(ax,az)*180/3.141592;
	*roll=atan2(ay,az)*180/3.141592;	
}
/** Sets up the control points for the interpolation. The parameter contains
 *  the coordinates of the first control points (i.e. a control point that
 *  influences the direction the ball is flying only, not the actual
 *  coordinates - see details about Catmull-Rom splines). This function will
 *  then set the 2nd control point to be the current coordinates of the ball,
 *  and find two more appropriate control points for a smooth movement.
 *  \param xyz Coordinates of the first control points.
 */
void RubberBall::initializeControlPoints(const Vec3 &xyz)
{
    m_control_points[0]     = xyz;
    m_control_points[1]     = getXYZ();
    m_last_aimed_graph_node = getSuccessorToHitTarget(getCurrentGraphNode());
    // This call defined m_control_points[3], but also sets a new
    // m_last_aimed_graph_node, which is further away from the current point,
    // which avoids the problem that the ball might go too quickly to the
    // left or right when firing the ball off track.
    getNextControlPoint();
    m_control_points[2]     =
        QuadGraph::get()->getQuadOfNode(m_last_aimed_graph_node).getCenter();

    // This updates m_last_aimed_graph_node, and sets m_control_points[3]
    getNextControlPoint();
    m_length_cp_1_2 = (m_control_points[2]-m_control_points[1]).length();
    m_t             = 0;
    m_t_increase    = m_speed/m_length_cp_1_2;
}   // initializeControlPoints
Ejemplo n.º 25
0
static void getYml(GLenum face, size_t width, size_t height,
    float *Y00,
    float *Y1minus1, float *Y10, float *Y11,
    float *Y2minus2, float *Y2minus1, float *Y20, float *Y21, float *Y22)
{
    for (unsigned i = 0; i < width; i++)
    {
        for (unsigned j = 0; j < height; j++)
        {
            float x, y, z;
            float fi = float(i), fj = float(j);
            fi /= width, fj /= height;
            fi = 2 * fi - 1, fj = 2 * fj - 1;
            getXYZ(face, fi, fj, x, y, z);

            // constant part of Ylm
            float c00 = 0.282095f;
            float c1minus1 = 0.488603f;
            float c10 = 0.488603f;
            float c11 = 0.488603f;
            float c2minus2 = 1.092548f;
            float c2minus1 = 1.092548f;
            float c21 = 1.092548f;
            float c20 = 0.315392f;
            float c22 = 0.546274f;

            size_t idx = i * height + j;

            Y00[idx] = c00;
            Y1minus1[idx] = c1minus1 * y;
            Y10[idx] = c10 * z;
            Y11[idx] = c11 * x;
            Y2minus2[idx] = c2minus2 * x * y;
            Y2minus1[idx] = c2minus1 * y * z;
            Y21[idx] = c21 * x * z;
            Y20[idx] = c20 * (3 * z * z - 1);
            Y22[idx] = c22 * (x * x - y * y);
        }
    }
}
Ejemplo n.º 26
0
/// Loads and normalizes all PNGs into a tensor memory block \p resultT in the
/// NCHW 3x224x224 format.
static void loadImagesAndPreprocess(const std::vector<std::string> &filenames,
                                    float *&resultT, size_t *resultDims) {
  assert(filenames.size() > 0 &&
         "There must be at least one filename in filenames");
  std::pair<float, float> range = std::make_pair(0., 256.0);
  unsigned numImages = filenames.size();
  // N x C x H x W
  resultDims[0] = numImages;
  resultDims[1] = 3;
  resultDims[2] = DEFAULT_HEIGHT;
  resultDims[3] = DEFAULT_WIDTH;
  size_t resultSizeInBytes =
      numImages * 3 * DEFAULT_HEIGHT * DEFAULT_WIDTH * sizeof(float);
  resultT = static_cast<float *>(malloc(resultSizeInBytes));
  // We iterate over all the png files, reading them all into our result tensor
  // for processing
  for (unsigned n = 0; n < numImages; n++) {
    float *imageT{nullptr};
    size_t dims[3];
    bool loadSuccess = !readPngImage(filenames[n].c_str(), range, imageT, dims);
    assert(loadSuccess && "Error reading input image.");

    assert((dims[0] == DEFAULT_HEIGHT && dims[1] == DEFAULT_WIDTH) &&
           "All images must have the same Height and Width");

    // Convert to BGR, as this is what NN is expecting.
    for (unsigned z = 0; z < 3; z++) {
      for (unsigned y = 0; y < dims[1]; y++) {
        for (unsigned x = 0; x < dims[0]; x++) {
          resultT[getXYZW(resultDims, n, 2 - z, x, y)] =
              imageT[getXYZ(dims, x, y, z)];
        }
      }
    }
  }
  printf("Loaded images size in bytes is: %lu\n", resultSizeInBytes);
}
/** Moves the rubber ball in a straight line towards the target. This is used
 *  once the rubber ball is close to its target. It restricts the angle by
 *  which the rubber ball can change its direction per frame.
 *  \param next_xyz The position the ball should move to.
 *  \param dt Time step size.
 */
void RubberBall::moveTowardsTarget(Vec3 *next_xyz, float dt)
{
    // If the rubber ball is already close to a target, i.e. aiming
    // at it directly, stop interpolating, instead fly straight
    // towards it.
    Vec3 diff = m_target->getXYZ()-getXYZ();
    // Avoid potential division by zero
    if(diff.length2()==0)
        *next_xyz = getXYZ();
    else
        *next_xyz = getXYZ() + (dt*m_speed/diff.length())*diff;

    Vec3 old_vec = getXYZ()-m_previous_xyz;
    Vec3 new_vec = *next_xyz - getXYZ();
    float angle  = atan2(new_vec.getZ(), new_vec.getX())
                 - atan2(old_vec.getZ(), old_vec.getX());
    // Adjust angle to be between -180 and 180 degrees
    if(angle < -M_PI)
        angle += 2*M_PI;
    else if(angle > M_PI)
        angle -= 2*M_PI;

    // If the angle is too large, adjust next xyz
    if(fabsf(angle)>m_st_target_max_angle*dt)
    {
        core::vector2df old_2d(old_vec.getX(), old_vec.getZ());
        if(old_2d.getLengthSQ()==0.0f) old_2d.Y = 1.0f;
        old_2d.normalize();
        old_2d.rotateBy(  RAD_TO_DEGREE * dt
                                       * (angle > 0 ?  m_st_target_max_angle
                                                    : -m_st_target_max_angle));
        next_xyz->setX(getXYZ().getX() + old_2d.X*dt*m_speed);
        next_xyz->setZ(getXYZ().getZ() + old_2d.Y*dt*m_speed);
    }   // if fabsf(angle) > m_st_target_angle_max*dt

    assert(!isnan((*next_xyz)[0]));
    assert(!isnan((*next_xyz)[1]));
    assert(!isnan((*next_xyz)[2]));
}   // moveTowardsTarget
void SpecificWorker::getImage(ColorSeq& color, DepthSeq& depth, PointSeq& points, RoboCompJointMotor::MotorStateMap &hState, RoboCompDifferentialRobot::TBaseState& bState)
{
	getDepth(depth,hState,bState);
	getRGB(color,hState,bState);
	getXYZ(points,hState,bState);
}
void IsoSurfaceThread::run()
{
    int numThreads = GLFunctions::idealThreadCount;

    int chunkSize = m_scalarField->size() / numThreads;

    int begin = m_id * chunkSize;
    int end = m_id * chunkSize + chunkSize;

    if ( m_id == numThreads - 1 )
    {
        end = m_scalarField->size() - ( ( m_nX + 1 ) * ( m_nY + 1 ) );
    }

    int x;
    int y;
    int z;

    // Generate isosurface.
    for ( int k = begin; k < end; ++k )
    {
        getXYZ( k, x, y, z );
        if ( x == m_nX ) continue;
        if ( y == m_nY ) continue;

        // Calculate table lookup index from those
        // vertices which are below the isolevel.
        unsigned int tableIndex = 0;
        if ( m_scalarField->at( z * m_nPointsInSlice + y * m_nPointsInXDirection + x ) < m_isoLevel )
            tableIndex |= 1;
        if ( m_scalarField->at( z * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + x ) < m_isoLevel )
            tableIndex |= 2;
        if ( m_scalarField->at( z * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
            tableIndex |= 4;
        if ( m_scalarField->at( z * m_nPointsInSlice + y * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
            tableIndex |= 8;
        if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + y * m_nPointsInXDirection + x ) < m_isoLevel )
            tableIndex |= 16;
        if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + x ) < m_isoLevel )
            tableIndex |= 32;
        if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
            tableIndex |= 64;
        if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + y * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
            tableIndex |= 128;

        // Now create a triangulation of the isosurface in this
        // cell.
        if ( edgeTable[ tableIndex ] != 0 )
        {
            if ( edgeTable[ tableIndex ] & 8 )
            {
                POINT3DID pt = calculateIntersection( x, y, z, 3 );
                unsigned int id = getEdgeID( x, y, z, 3 );
                QMutexLocker locker( m_mutex );
                m_i2pt3idVertices->insert( id, pt );
                locker.unlock();
            }
            if ( edgeTable[ tableIndex ] & 1 )
            {
                POINT3DID pt = calculateIntersection( x, y, z, 0 );
                unsigned int id = getEdgeID( x, y, z, 0 );
                QMutexLocker locker( m_mutex );
                m_i2pt3idVertices->insert( id, pt );
                locker.unlock();
            }
            if ( edgeTable[ tableIndex ] & 256 )
            {
                POINT3DID pt = calculateIntersection( x, y, z, 8 );
                unsigned int id = getEdgeID( x, y, z, 8 );
                QMutexLocker locker( m_mutex );
                m_i2pt3idVertices->insert( id, pt );
                locker.unlock();
            }

            if ( x == m_nX - 1 )
            {
                if ( edgeTable[ tableIndex ] & 4 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 2 );
                    unsigned int id = getEdgeID( x, y, z, 2 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
                if ( edgeTable[ tableIndex ] & 2048 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 11 );
                    unsigned int id = getEdgeID( x, y, z, 11 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
            }
            if ( y == m_nY - 1 )
            {
                if ( edgeTable[ tableIndex ] & 2 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 1 );
                    unsigned int id = getEdgeID( x, y, z, 1 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
                if ( edgeTable[ tableIndex ] & 512 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 9 );
                    unsigned int id = getEdgeID( x, y, z, 9 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
            }
            if ( z == m_nZ - 1 )
            {
                if ( edgeTable[ tableIndex ] & 16 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 4 );
                    unsigned int id = getEdgeID( x, y, z, 4 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
                if ( edgeTable[ tableIndex ] & 128 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 7 );
                    unsigned int id = getEdgeID( x, y, z, 7 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
            }
            if ( ( x == m_nX - 1 ) && ( y == m_nY - 1 ) )
                if ( edgeTable[ tableIndex ] & 1024 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 10 );
                    unsigned int id = getEdgeID( x, y, z, 10 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
            if ( ( x == m_nX - 1 ) && ( z == m_nZ - 1 ) )
                if ( edgeTable[ tableIndex ] & 64 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 6 );
                    unsigned int id = getEdgeID( x, y, z, 6 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }
            if ( ( y == m_nY - 1 ) && ( z == m_nZ - 1 ) )
                if ( edgeTable[ tableIndex ] & 32 )
                {
                    POINT3DID pt = calculateIntersection( x, y, z, 5 );
                    unsigned int id = getEdgeID( x, y, z, 5 );
                    QMutexLocker locker( m_mutex );
                    m_i2pt3idVertices->insert( id, pt );
                    locker.unlock();
                }

            for ( int i = 0; triTable[ tableIndex ][ i ] != -1; i += 3 )
            {
                TRIANGLE triangle;
                unsigned int pointID0, pointID1, pointID2;
                pointID0 = getEdgeID( x, y, z, triTable[ tableIndex ][ i ] );
                pointID1 = getEdgeID( x, y, z, triTable[ tableIndex ][ i + 1 ] );
                pointID2 = getEdgeID( x, y, z, triTable[ tableIndex ][ i + 2 ] );
                triangle.pointID[ 0 ] = pointID0;
                triangle.pointID[ 1 ] = pointID1;
                triangle.pointID[ 2 ] = pointID2;
                QMutexLocker locker( m_mutex );
                m_trivecTriangles->push_back( triangle );
                locker.unlock();
            }
        }
    }
}
Ejemplo n.º 30
0
/** Updates the bowling ball ineach frame. If this function returns true, the
 *  object will be removed by the projectile manager.
 *  \param dt Time step size.
 *  \returns True of this object should be removed.
 */
bool Bowling::updateAndDelete(float dt)
{
    bool can_be_deleted = Flyable::updateAndDelete(dt);
    if(can_be_deleted)
        return true;

    const AbstractKart *kart=0;
    Vec3        direction;
    float       minDistance;
    getClosestKart(&kart, &minDistance, &direction);
    if(kart && minDistance<m_st_max_distance_squared)   // move bowling towards kart
    {
        // limit angle, so that the bowling ball does not turn
        // around to hit a kart behind
        if(fabs(m_body->getLinearVelocity().angle(direction)) < 1.3)
        {
            direction*=1/direction.length()*m_st_force_to_target;
            m_body->applyCentralForce(direction);
        }
    }

    // Bowling balls lose energy (e.g. when hitting the track), so increase
    // the speed if the ball is too slow, but only if it's not too high (if
    // the ball is too high, it is 'pushed down', which can reduce the
    // speed, which causes the speed to increase, which in turn causes
    // the ball to fly higher and higher.
    //btTransform trans = getTrans();
    float hat         = getXYZ().getY()-getHoT();
    if(hat-0.5f*m_extend.getY()<0.01f)
    {
        const Material *material = getMaterial();
        if(!material || material->isDriveReset())
        {
            hit(NULL);
            return true;
        }
    }
    btVector3 v       = m_body->getLinearVelocity();
    float vlen        = v.length2();
    if (hat<= m_max_height)
    {
        if(vlen<0.8*m_speed*m_speed)
        {   // bowling lost energy (less than 80%), i.e. it's too slow - speed it up:
            if(vlen==0.0f) {
                v = btVector3(.5f, .0, 0.5f);  // avoid 0 div.
            }
 //           m_body->setLinearVelocity(v*(m_speed/sqrt(vlen)));
        }   // vlen < 0.8*m_speed*m_speed
    }   // hat< m_max_height

    if(vlen<0.1)
    {
        hit(NULL);
        return true;
    }

    if (m_roll_sfx->getStatus()==SFXBase::SFX_PLAYING)
        m_roll_sfx->setPosition(getXYZ());

    return false;
}   // updateAndDelete